update contact projection

src/BulletSoftBody/btBackwardEulerObjective.h

@@ -101,6 +101,7 @@ public:
         {
             // damping force is implicit and elastic force is explicit
             m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
+//            m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
         }
     }
     

src/BulletSoftBody/btContactProjection.cpp

@@ -76,7 +76,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
                 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                 {
                     rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * dt : btVector3(0, 0, 0);
+                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1) + btVector3(0,-10,0)*dt) * dt : btVector3(0, 0, 0);
                 }
                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                 {
@@ -117,7 +117,8 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
                     //c.m_node->m_v -= impulse * c.m_c2 / dt;
                     // TODO: only contact is considered here, add friction later
                     btVector3 normal = cti.m_normal.normalized();
-                    btVector3 dv = -impulse * c.m_c2;
+                    btVector3 diff = c.m_node->m_v - m_backupVelocity[m_indices[c.m_node]];
+                    btVector3 dv = -impulse * c.m_c2/dt + diff;
                     btScalar dvn = dv.dot(normal);
                     m_constrainedDirections[m_indices[c.m_node]].push_back(normal);
                     m_constrainedValues[m_indices[c.m_node]].push_back(dvn);

src/BulletSoftBody/btDeformableBodySolver.h

@@ -8,6 +8,7 @@
 #ifndef BT_DEFORMABLE_BODY_SOLVERS_H
 #define BT_DEFORMABLE_BODY_SOLVERS_H
 
+#include <iostream>
 #include "btSoftBodySolvers.h"
 #include "btBackwardEulerObjective.h"
 #include "btDeformableRigidDynamicsWorld.h"
@@ -87,6 +88,7 @@ public:
             // only need to advect x here if elastic force is implicit
 //            prepareSolve(solverdt);
             m_objective.computeResidual(solverdt, m_residual);
+            moveTempVelocity(solverdt, m_residual);
             m_objective.computeStep(m_dv, m_residual, solverdt);
             
             updateVelocity();
@@ -94,6 +96,20 @@ public:
         advect(solverdt);
     }
     
+    void moveTempVelocity(btScalar dt, const TVStack& f)
+    {
+        size_t 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)
+            {
+                auto& node = psb->m_nodes[j];
+                node.m_v += node.m_im * dt * f[counter++];
+            }
+        }
+    }
+    
     void reinitialize(bool nodeUpdated)
     {
         if (nodeUpdated)
@@ -119,7 +135,7 @@ public:
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
                 auto& node = psb->m_nodes[j];
-                node.m_x = node.m_q + dt * node.m_v * psb->m_dampingCoefficient;
+                node.m_x = node.m_q + dt * node.m_v;
             }
         }
     }
@@ -132,7 +148,13 @@ public:
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
                 auto& node = psb->m_nodes[j];
-                node.m_x +=  dt * m_dv[counter++];
+//                node.m_x +=  dt * m_dv[counter++];
+                node.m_x +=  dt * node.m_v;
+                if (j == 4)
+                {
+                    std::cout << "x  " << psb->m_nodes[j].m_x.getY() << std::endl;
+                    std::cout << "v  " << psb->m_nodes[j].m_v.getY() << std::endl;
+                }
             }
         }
     }
@@ -147,6 +169,7 @@ public:
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
                 psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
+                
                 ++counter;
             }
         }

src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp

@@ -26,7 +26,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     }
     
     ///apply gravity, predict motion
-    btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+    predictUnconstraintMotion(timeStep);
     
     
     btDispatcherInfo& dispatchInfo = btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getDispatchInfo();
@@ -56,7 +56,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     ///solve deformable bodies constraints
     solveDeformableBodiesConstraints(timeStep);
 
-    predictUnconstraintMotion(timeStep);
+//    predictUnconstraintMotion(timeStep);
     //integrate transforms
     btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::integrateTransforms(timeStep);
     
@@ -68,6 +68,11 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     ///update soft bodies
     m_deformableBodySolver->updateSoftBodies();
     
+    for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
+    {
+        btRigidBody* body = m_nonStaticRigidBodies[i];
+        std::cout << "rb v = " << body->getLinearVelocity().getY() << std::endl;
+    }
     // End solver-wise simulation step
     // ///////////////////////////////
 }

src/BulletSoftBody/btDeformableRigidDynamicsWorld.h

@@ -64,7 +64,7 @@ public:
     
     virtual void predictUnconstraintMotion(btScalar timeStep)
     {
-//        btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+        btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
         m_deformableBodySolver->predictMotion(float(timeStep));
     }
     // virtual void internalStepSingleStepSimulation(btScalar timeStep);