switch to Baraff style constraint

examples/DeformableDemo/DeformableRigid.cpp

@@ -228,11 +228,11 @@ void DeformableRigid::initPhysics()
         psb->getCollisionShape()->setMargin(0.1);
         psb->generateBendingConstraints(2);
         psb->setTotalMass(1);
-        psb->setSpringStiffness(1);
+        psb->setSpringStiffness(2);
-        psb->setDampingCoefficient(0.01);
+        psb->setDampingCoefficient(0.05);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb->m_cfg.kDF = 1;
+        psb->m_cfg.kDF = 0;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());

examples/DeformableDemo/GraspDeformable.cpp

@@ -331,14 +331,15 @@ void GraspDeformable::initPhysics()
         psb->getCollisionShape()->setMargin(0.1);
         psb->setTotalMass(1);
         psb->setSpringStiffness(0);
-        psb->setDampingCoefficient(0.04);
+        psb->setDampingCoefficient(0.02);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb->m_cfg.kDF = 2;
+        psb->m_cfg.kDF = 0;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(2,2));
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(1,1));
     }
     
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

src/BulletSoftBody/btConjugateGradient.h

@@ -32,62 +32,108 @@ public:
     
     virtual ~btConjugateGradient(){}
     
+//    // return the number of iterations taken
+//    int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance)
+//    {
+//        BT_PROFILE("CGSolve");
+//        btAssert(x.size() == b.size());
+//        reinitialize(b);
+//        
+//        // r = b - A * x --with assigned dof zeroed out
+//        A.multiply(x, temp);
+//        r = sub(b, temp);
+//        A.project(r);
+//
+//        btScalar r_norm = std::sqrt(squaredNorm(r));
+//        if (r_norm < tolerance) {
+//            std::cout << "Iteration = 0" << std::endl;
+//            std::cout << "Two norm of the residual = " << r_norm << std::endl;
+//            return 0;
+//        }
+//        
+//        // z = M^(-1) * r
+//        A.precondition(r, z);
+//        p = z;
+//        // temp = A*p
+//        A.multiply(p, temp);
+//        A.project(temp);
+//        btScalar r_dot_z = dot(z,r), r_dot_z_new;
+//        // alpha = r^T * z / (p^T * A * p)
+//        btScalar alpha = r_dot_z / dot(p, temp), beta;
+//        
+//        for (int k = 1; k < max_iterations; k++) {
+//            //  x += alpha * p;
+//            //  r -= alpha * temp;
+//            multAndAddTo(alpha, p, x);
+//            multAndAddTo(-alpha, temp, r);
+//            // zero out the dofs of r
+//            A.project(r);
+////            A.enforceConstraint(x);
+//            r_norm = std::sqrt(squaredNorm(r));
+//            
+//            if (r_norm < tolerance) {
+//                std::cout << "ConjugateGradient iterations " << k << std::endl;
+//                return k;
+//            }
+//            
+//            // z = M^(-1) * r
+//            A.precondition(r, z);
+//            r_dot_z_new = dot(r,z);
+//            beta = r_dot_z_new/ r_dot_z;
+//            r_dot_z = r_dot_z_new;
+//            // p = z + beta * p;
+//            p = multAndAdd(beta, p, z);
+//            // temp = A * p;
+//            A.multiply(p, temp);
+//            A.project(temp);
+//            // alpha = r^T * z / (p^T * A * p)
+//            alpha = r_dot_z / dot(p, temp);
+//        }
+//        std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
+//        return max_iterations;
+//    }
+    
     // return the number of iterations taken
     int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance)
     {
         BT_PROFILE("CGSolve");
         btAssert(x.size() == b.size());
         reinitialize(b);
-        
         // r = b - A * x --with assigned dof zeroed out
         A.multiply(x, temp);
         r = sub(b, temp);
         A.project(r);
-        A.enforceConstraint(x);
-        
-        btScalar r_norm = std::sqrt(squaredNorm(r));
-        if (r_norm < tolerance) {
-            std::cout << "Iteration = 0" << std::endl;
-            std::cout << "Two norm of the residual = " << r_norm << std::endl;
-            return 0;
-        }
-        
         // z = M^(-1) * r
         A.precondition(r, z);
+        A.project(z);
+        btScalar r_dot_z = dot(z,r);
+        if (r_dot_z < tolerance) {
+            std::cout << "Iteration = 0" << std::endl;
+            std::cout << "Two norm of the residual = " << r_dot_z << std::endl;
+            return 0;
+        }
         p = z;
-        // temp = A*p
+        btScalar r_dot_z_new = r_dot_z;
-        A.multiply(p, temp);
-        
-        btScalar r_dot_z = dot(z,r), r_dot_z_new;
-        // alpha = r^T * z / (p^T * A * p)
-        btScalar alpha = r_dot_z / dot(p, temp), beta;
-        
         for (int k = 1; k < max_iterations; k++) {
+            // temp = A*p
+            A.multiply(p, temp);
+            A.project(temp);
+            // alpha = r^T * z / (p^T * A * p)
+            btScalar alpha = r_dot_z_new / dot(p, temp);
             //  x += alpha * p;
-            //  r -= alpha * temp;
             multAndAddTo(alpha, p, x);
+            //  r -= alpha * temp;
             multAndAddTo(-alpha, temp, r);
-            // zero out the dofs of r
+            // z = M^(-1) * r
-            A.project(r);
+            A.precondition(r, z);
-            A.enforceConstraint(x);
+            r_dot_z = r_dot_z_new;
-            r_norm = std::sqrt(squaredNorm(r));
+            r_dot_z_new = dot(r,z);
-            
+            if (r_dot_z_new < tolerance) {
-            if (r_norm < tolerance) {
                 std::cout << "ConjugateGradient iterations " << k << std::endl;
                 return k;
             }
-            
+            btScalar beta = r_dot_z_new/ r_dot_z;
-            // z = M^(-1) * r
-            A.precondition(r, z);
-            r_dot_z_new = dot(r,z);
-            beta = r_dot_z_new/ r_dot_z;
-            r_dot_z = r_dot_z_new;
-            // p = z + beta * p;
             p = multAndAdd(beta, p, z);
-            // temp = A * p;
-            A.multiply(p, temp);
-            // alpha = r^T * z / (p^T * A * p)
-            alpha = r_dot_z / dot(p, temp);
         }
         std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
         return max_iterations;

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -73,7 +73,6 @@ public:
     {
         BT_PROFILE("enforceConstraint");
         projection.enforceConstraint(x);
-        updateVelocity(x);
     }
     
     // add dv to velocity
@@ -86,7 +85,6 @@ public:
     void project(TVStack& r)
     {
         BT_PROFILE("project");
-        projection.update();
         projection.project(r);
     }
     

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -31,20 +31,20 @@ btDeformableBodySolver::~btDeformableBodySolver()
 void btDeformableBodySolver::solveConstraints(float solverdt)
 {
     BT_PROFILE("solveConstraints");
-    // add constraints to the solver
-    setConstraints();
-    
     // save v_{n+1}^* velocity after explicit forces
     backupVelocity();
     
-    m_objective->computeResidual(solverdt, m_residual);
+    // add constraints to the solver
+    setConstraints();
 
+    m_objective->computeResidual(solverdt, m_residual);
     computeStep(m_dv, m_residual);
     updateVelocity();
 }
 
 void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual)
 {
+    
     btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * m_objective->computeNorm(residual);
     m_cg.solve(*m_objective, dv, residual, tolerance);
 }
@@ -75,6 +75,12 @@ void btDeformableBodySolver::setConstraints()
 {
     BT_PROFILE("setConstraint");
     m_objective->setConstraints();
+    for (int i = 0; i < 10; ++i)
+    {
+        m_objective->projection.update();
+        m_objective->enforceConstraint(m_dv);
+        m_objective->updateVelocity(m_dv);
+    }
 }
 
 void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)