contact solve for newton

data/single_tet.vtk

@@ -0,0 +1,15 @@
+# vtk DataFile Version 2.0
+ball_, Created by Gmsh
+ASCII
+DATASET UNSTRUCTURED_GRID
+POINTS 4 double
+0 0 0
+1 0 0
+0 1 0
+0 0 1
+
+CELLS 1 1
+4 0 1 2 3
+
+CELL_TYPES 1
+10

examples/DeformableDemo/DeformableRigid.cpp

@@ -234,11 +234,11 @@ void DeformableRigid::initPhysics()
         psb->setTotalMass(1);
         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 = 2;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         
-        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.01, false);
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.1, true);
         getDeformableDynamicsWorld()->addForce(psb, mass_spring);
         m_forces.push_back(mass_spring);
         

examples/DeformableDemo/GraspDeformable.cpp

@@ -325,7 +325,8 @@ void GraspDeformable::initPhysics()
     {
         char relative_path[1024];
 //        b3FileUtils::findFile("banana.vtk", relative_path, 1024);
-         b3FileUtils::findFile("ball.vtk", relative_path, 1024);
+        b3FileUtils::findFile("ball.vtk", relative_path, 1024);
+//        b3FileUtils::findFile("single_tet.vtk", relative_path, 1024);
 //         b3FileUtils::findFile("tube.vtk", relative_path, 1024);
 //         b3FileUtils::findFile("torus.vtk", relative_path, 1024);
 //         b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024);
@@ -343,21 +344,16 @@ void GraspDeformable::initPhysics()
         psb->scale(btVector3(.25, .25, .25));
 //        psb->scale(btVector3(.3, .3, .3));  // for tube, torus, boot
 //        psb->scale(btVector3(1, 1, 1));  // for ditto
-//        psb->translate(btVector3(0, 0, 2)); for boot
+        psb->translate(btVector3(.25, 0, 0.4)); 
-        psb->getCollisionShape()->setMargin(0.01);
+        psb->getCollisionShape()->setMargin(0.02);
         psb->setTotalMass(.1);
         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.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
-        psb->getWorldInfo()->m_maxDisplacement = .1f;
-        // nonlinear damping 
-//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.04, true));
-//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
-//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(0,6));
         
-        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,0.04, true);
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,.04, true);
         getDeformableDynamicsWorld()->addForce(psb, mass_spring);
         m_forces.push_back(mass_spring);
         
@@ -365,7 +361,7 @@ void GraspDeformable::initPhysics()
         getDeformableDynamicsWorld()->addForce(psb, gravity_force);
         m_forces.push_back(gravity_force);
         
-        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,10);
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(5,10);
         getDeformableDynamicsWorld()->addForce(psb, neohookean);
         m_forces.push_back(neohookean);
     }
@@ -415,8 +411,8 @@ void GraspDeformable::initPhysics()
     
     {
         SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
-        slider.m_minVal = -.1;
+        slider.m_minVal = -.3;
-        slider.m_maxVal = .1;
+        slider.m_maxVal = .3;
         m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
     }
     
@@ -470,8 +466,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor
 {
     //init the base
     btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
-    float baseMass = 1.f;
+    float baseMass = 100.f;
-    float linkMass = 1.f;
+    float linkMass = 100.f;
     int numLinks = 2;
     
     if (baseMass)

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_x + n.m_v * dt;
-        n.m_q += 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
+        // at this point dv = v_{n+1} - v_{n+1}^*
-        m_deformableBodySolver->revertVelocity();
+        // modify dv such that dv = v_{n+1} - v_n
-        // 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.
+        // 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)
+//    if (m_implicit)
-    {
+//    {
-        // backup v_n velocity
+//        // todo: backup v_n velocity somewhere else
-        m_deformableBodySolver->backupVelocity();
+//        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