bug fixes in constraints projections; cpplized various functions

src/BulletSoftBody/btDeformableBodySolver.h

@@ -16,7 +16,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 
 struct btCollisionObjectWrapper;
-
+class btBackwardEulerObjective;
 class btDeformableRigidDynamicsWorld;
 
 class btDeformableBodySolver : public btSoftBodySolver
@@ -29,25 +29,17 @@ protected:
     TVStack m_dv;
     TVStack m_residual;
     btAlignedObjectArray<btSoftBody *> m_softBodySet;
-    btBackwardEulerObjective m_objective;
+    btBackwardEulerObjective* m_objective;
     int m_solveIterations;
     int m_impulseIterations;
     btDeformableRigidDynamicsWorld* m_world;
     btAlignedObjectArray<btVector3> m_backupVelocity;
+    btScalar m_dt;
     
 public:
-    btDeformableBodySolver()
-    : m_numNodes(0)
-    , m_objective(m_softBodySet, m_backupVelocity)
-    , m_solveIterations(1)
-    , m_impulseIterations(1)
-    , m_world(nullptr)
-    {
-    }
+    btDeformableBodySolver();
     
-    virtual ~btDeformableBodySolver()
-    {
-    }
+    virtual ~btDeformableBodySolver();
     
     virtual SolverTypes getSolverType() const
     {
@@ -78,21 +70,9 @@ public:
 
     virtual void copyBackToSoftBodies(bool bMove = true) {}
 
-    virtual void solveConstraints(float solverdt)
-    {
-        bool nodeUpdated = updateNodes();
-        reinitialize(nodeUpdated);
-        for (int i = 0; i < m_solveIterations; ++i)
-        {
-            // only need to advect x here if elastic force is implicit
-//            prepareSolve(solverdt);
-            m_objective.computeResidual(solverdt, m_residual);
-            m_objective.computeStep(m_dv, m_residual, solverdt);
-            
-            updateVelocity();
-        }
-        advect(solverdt);
-    }
+    virtual void solveConstraints(float solverdt);
+    
+    void postStabilize();
     
     void moveTempVelocity(btScalar dt, const TVStack& f)
     {
@@ -108,34 +88,10 @@ public:
         }
     }
     
-    void reinitialize(bool nodeUpdated)
-    {
-        if (nodeUpdated)
-        {
-            m_dv.resize(m_numNodes);
-            m_residual.resize(m_numNodes);
-        }
-        
-        for (int i = 0; i < m_dv.size(); ++i)
-        {
-            m_dv[i].setZero();
-            m_residual[i].setZero();
-        }
-        m_objective.reinitialize(nodeUpdated);
-    }
+    void reinitialize(bool nodeUpdated);
+    
+    void setConstraintDirections();
     
-    void prepareSolve(btScalar dt)
-    {
-        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_x = node.m_q + dt * node.m_v;
-            }
-        }
-    }
     void advect(btScalar dt)
     {
         size_t counter = 0;
@@ -145,13 +101,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 * node.m_v;
+                node.m_x +=  dt * m_dv[counter++];
+//                node.m_x  =  node.m_q + dt * node.m_v;
             }
         }
     }
     
-    void updateVelocity()
+    void backupVelocity()
     {
         // serial implementation
         int counter = 0;
@@ -160,13 +116,13 @@ public:
             btSoftBody* psb = m_softBodySet[i];
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
-                psb->m_nodes[j].m_v += m_dv[counter];
-                
-                ++counter;
+                m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
             }
         }
     }
     
+    void updateVelocity();
+    
     bool updateNodes()
     {
         int numNodes = 0;
@@ -175,6 +131,7 @@ public:
         if (numNodes != m_numNodes)
         {
             m_numNodes = numNodes;
+            m_backupVelocity.resize(numNodes);
             return true;
         }
         return false;
@@ -200,15 +157,11 @@ public:
         softBody->defaultCollisionHandler(collisionObjectWrap);
     }
 
-    virtual void processCollision(btSoftBody *, btSoftBody *) {
-        // TODO
+    virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
+        softBody->defaultCollisionHandler(otherSoftBody);
     }
     
-    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
-    {
-        m_world = world;
-        m_objective.setWorld(world);
-    }
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world);
 };
 
 #endif /* btDeformableBodySolver_h */