contact solve for newton

src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -83,7 +83,7 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
 
 void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
 {
-    // only the velocity of the constrained nodes needs to be updated during CG solve
+    // only the velocity of the constrained nodes needs to be updated during contact solve
     for (int i = 0; i < projection.m_constraints.size(); ++i)
     {
         int index = projection.m_constraints.getKeyAtIndex(i).getUid1();

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -67,7 +67,7 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
             {
                 break;
             }
-//            m_objective->applyDynamicFriction(m_residual);
+            m_objective->applyDynamicFriction(m_residual);
             computeStep(m_ddv, m_residual);
             updateDv();
             for (int j = 0; j < m_numNodes; ++j)
@@ -189,6 +189,20 @@ void btDeformableBodySolver::backupVelocity()
     }
 }
 
+void btDeformableBodySolver::backupVn()
+{
+    int counter = 0;
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodySet[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn;
+            m_backupVelocity[counter++] = psb->m_nodes[j].m_vn;
+        }
+    }
+}
+
 void btDeformableBodySolver::revertVelocity()
 {
     int counter = 0;
@@ -246,8 +260,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
     for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
     {
         btSoftBody::Node& n = psb->m_nodes[i];
-        n.m_q = n.m_x;
-        n.m_q += n.m_v * dt;
+        n.m_q = n.m_x + n.m_v * dt;
     }
     /* Bounds                */
     psb->updateBounds();

src/BulletSoftBody/btDeformableBodySolver.h

@@ -75,6 +75,7 @@ public:
     void predictDeformableMotion(btSoftBody* psb, btScalar dt);
     
     void backupVelocity();
+    void backupVn();
     void revertVelocity();
     void updateVelocity();
     

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -168,6 +168,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
             }
             node.m_x  =  node.m_x + timeStep * node.m_v;
             node.m_q = node.m_x;
+            node.m_vn = node.m_v;
         }
     }
     m_deformableBodySolver->revertVelocity();
@@ -175,11 +176,8 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 
 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 {
-    if (!m_implicit)
-    {
-        // save v_{n+1}^* velocity after explicit forces
-        m_deformableBodySolver->backupVelocity();
-    }
+    // save v_{n+1}^* velocity after explicit forces
+    m_deformableBodySolver->backupVelocity();
     
     // set up constraints among multibodies and between multibodies and deformable bodies
     setupConstraints();
@@ -187,11 +185,13 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
     
     if (m_implicit)
     {
-    // revert to v_n after collisions are registered
-        m_deformableBodySolver->revertVelocity();
-        // todo @xuchenhan : think about whether contact solve should be done with v_n velocity or v_{n+1}^* velocity. It's using v_n for implicit and v_{n+1}^* for non-implicit now.
+        // at this point dv = v_{n+1} - v_{n+1}^*
+        // modify dv such that dv = v_{n+1} - v_n
+        // modify m_backupVelocity so that it stores v_n instead of v_{n+1}^* as needed by Newton
+        m_deformableBodySolver->backupVn();
     }
-    // At this point, dv is golden for nodes in contact
+    
+    // At this point, dv should be golden for nodes in contact
     m_deformableBodySolver->solveDeformableConstraints(timeStep);
 }
 
@@ -211,7 +211,6 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints()
         
         // build islands
         m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
-        // write the constraint information of each island to the callback, and also setup the constraints in solver
     }
 }
 
@@ -317,11 +316,11 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
     m_internalTime += timeStep;
     m_deformableBodySolver->setImplicit(m_implicit);
     m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
-    if (m_implicit)
-    {
-        // backup v_n velocity
-        m_deformableBodySolver->backupVelocity();
-    }
+//    if (m_implicit)
+//    {
+//        // todo: backup v_n velocity somewhere else
+//        m_deformableBodySolver->backupVelocity();
+//    }
     btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
     dispatchInfo.m_timeStep = timeStep;
     dispatchInfo.m_stepCount = 0;

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -73,7 +73,7 @@ public:
         m_sbi.m_broadphase = pairCache;
         m_sbi.m_dispatcher = dispatcher;
         m_sbi.m_sparsesdf.Initialize();
-        m_sbi.m_sparsesdf.setDefaultVoxelsz(0.0025);
+        m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025);
         m_sbi.m_sparsesdf.Reset();
         
         m_sbi.air_density = (btScalar)1.2;
@@ -82,7 +82,7 @@ public:
         m_sbi.water_normal = btVector3(0, 0, 0);
         m_sbi.m_gravity.setValue(0, -10, 0);
         m_internalTime = 0.0;
-        m_implicit = false;
+        m_implicit = true;
     }
 
     void setSolverCallback(btSolverCallback cb)

src/BulletSoftBody/btSoftBody.h

@@ -251,8 +251,9 @@ public:
 	struct Node : Feature
 	{
 		btVector3 m_x;       // Position
-		btVector3 m_q;       // Previous step position
+		btVector3 m_q;       // Previous step position/Test position
 		btVector3 m_v;       // Velocity
+        btVector3 m_vn;      // Previous step velocity
 		btVector3 m_f;       // Force accumulator
 		btVector3 m_n;       // Normal
 		btScalar m_im;       // 1/mass