documentation and optimization

examples/DeformableDemo/DeformableSelfCollision.cpp

@@ -153,7 +153,7 @@ void DeformableSelfCollision::initPhysics()
                                                          btVector3(-s, h, +4*s),
                                                          btVector3(+s, h, +4*s),
                                                          10,40,
-                                                         0, true, 0.1);
+                                                         0, true, 0.01);
 
         
         psb->getCollisionShape()->setMargin(0.2);
@@ -161,7 +161,7 @@ void DeformableSelfCollision::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 = 0.02;
+        psb->m_cfg.kDF = 0.2;
         psb->rotate(btQuaternion(0,SIMD_PI / 2, 0));
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;

examples/DeformableDemo/VolumetricDeformable.cpp

@@ -91,7 +91,7 @@ public:
     
     void Ctor_RbUpStack(int count)
     {
-        float mass = 0.02;
+        float mass = 1;
         
         btCompoundShape* cylinderCompound = new btCompoundShape;
         btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
@@ -166,7 +166,7 @@ void VolumetricDeformable::initPhysics()
 	m_solver = sol;
 
 	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
-    btVector3 gravity = btVector3(0, -10, 0);
+    btVector3 gravity = btVector3(0, -100, 0);
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -196,7 +196,6 @@ void VolumetricDeformable::initPhysics()
         //add the ground to the dynamics world
         m_dynamicsWorld->addRigidBody(body);
     }
-    getDeformableDynamicsWorld()->setLineSearch(false);
     
     createStaticBox(btVector3(1, 5, 5), btVector3(-5,0,0));
     createStaticBox(btVector3(1, 5, 5), btVector3(5,0,0));
@@ -220,19 +219,17 @@ void VolumetricDeformable::initPhysics()
         psb->m_cfg.kDF = 0.5;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         
-        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(0,0.03);
-        getDeformableDynamicsWorld()->addForce(psb, mass_spring);
-        m_forces.push_back(mass_spring);
-        
         btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
         getDeformableDynamicsWorld()->addForce(psb, gravity_force);
         m_forces.push_back(gravity_force);
         
-        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.5,2.5);
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.02);
         getDeformableDynamicsWorld()->addForce(psb, neohookean);
         m_forces.push_back(neohookean);
         
     }
+    getDeformableDynamicsWorld()->setImplicit(true);
+    getDeformableDynamicsWorld()->setLineSearch(false);
     // add a few rigid bodies
     Ctor_RbUpStack(4); 
     

src/BulletSoftBody/btConjugateGradient.h

@@ -69,7 +69,7 @@ public:
             if (dot(p,temp) < SIMD_EPSILON)
             {
                 if (verbose)
-                    std::cout << "Encountered negative direction in CG!"<<std::endl;
+                    std::cout << "Encountered negative direction in CG!" << std::endl;
                 if (k == 1)
                 {
                     x = b;

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -22,7 +22,7 @@
 btDeformableBodySolver::btDeformableBodySolver()
 : m_numNodes(0)
 , m_cg(20)
-, m_maxNewtonIterations(10)
+, m_maxNewtonIterations(5)
 , m_newtonTolerance(1e-4)
 , m_lineSearch(true)
 {
@@ -412,6 +412,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
     psb->m_fdbvt.optimizeIncremental(1);
 }
 
+
 void btDeformableBodySolver::updateSoftBodies()
 {
     for (int i = 0; i < m_softBodies.size(); i++)
@@ -419,7 +420,7 @@ void btDeformableBodySolver::updateSoftBodies()
         btSoftBody *psb = (btSoftBody *)m_softBodies[i];
         if (psb->isActive())
         {
-            psb->updateNormals(); // normal is updated here
+            psb->updateNormals();
         }
     }
 }

src/BulletSoftBody/btDeformableBodySolver.h

@@ -29,26 +29,24 @@ class btDeformableMultiBodyDynamicsWorld;
 
 class btDeformableBodySolver : public btSoftBodySolver
 {
-//    using TVStack = btAlignedObjectArray<btVector3>;
     typedef btAlignedObjectArray<btVector3> TVStack;
 protected:
-    int m_numNodes;
+    int m_numNodes;                 // total number of deformable body nodes
-    TVStack m_dv;
+    TVStack m_dv;                   // v_{n+1} - v_n
-    TVStack m_backup_dv;
+    TVStack m_backup_dv;            // backed up dv
-    TVStack m_ddv;
+    TVStack m_ddv;                  // incremental dv
-    TVStack m_residual;
+    TVStack m_residual;             // rhs of the linear solve
-    btAlignedObjectArray<btSoftBody *> m_softBodies;
+    btAlignedObjectArray<btSoftBody *> m_softBodies;  // all deformable bodies
-   
+    TVStack m_backupVelocity;       // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
-    btAlignedObjectArray<btVector3> m_backupVelocity;
+    btScalar m_dt;                  // dt
-    btScalar m_dt;
+    btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;  // CG solver
-    btScalar m_contact_iterations;
+    bool m_implicit;                // use implicit scheme if true, explicit scheme if false
-    btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
+    int m_maxNewtonIterations;      // max number of newton iterations
-    bool m_implicit;
+    btScalar m_newtonTolerance;     // stop newton iterations if f(x) < m_newtonTolerance
-    int m_maxNewtonIterations;
+    bool m_lineSearch;              // If true, use newton's method with line search under implicit scheme
-    btScalar m_newtonTolerance;
-    bool m_lineSearch;
     
 public:
+    // handles data related to objective function
     btDeformableBackwardEulerObjective* m_objective;
     
     btDeformableBodySolver();
@@ -60,49 +58,62 @@ public:
         return DEFORMABLE_SOLVER;
     }
 
+    // update soft body normals
     virtual void updateSoftBodies();
-
-    virtual void copyBackToSoftBodies(bool bMove = true) {}
     
+    // solve the momentum equation
     virtual void solveDeformableConstraints(btScalar solverdt);
     
+    // solve the contact between deformable and rigid as well as among deformables
     btScalar solveContactConstraints();
-    
+
-    virtual void solveConstraints(btScalar dt){}
+    // resize/clear data structures
-    
     void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
     
+    // set up contact constraints
     void setConstraints();
     
+    // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
+    virtual void predictMotion(btScalar solverdt);
+    
+    // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^*
+    // x_{n+1}^* is stored in m_q
     void predictDeformableMotion(btSoftBody* psb, btScalar dt);
     
+    // save the current velocity to m_backupVelocity
     void backupVelocity();
+    
+    // set m_dv and m_backupVelocity to desired value to prepare for momentum solve
     void setupDeformableSolve(bool implicit);
+    
+    // set the current velocity to that backed up in m_backupVelocity
     void revertVelocity();
+    
+    // set velocity to m_dv + m_backupVelocity
     void updateVelocity();
     
+    // update the node count
     bool updateNodes();
     
-    void computeStep(TVStack& dv, const TVStack& residual);
+    // calculate the change in dv resulting from the momentum solve
+    void computeStep(TVStack& ddv, const TVStack& residual);
     
+    // calculate the change in dv resulting from the momentum solve when line search is turned on
     btScalar computeDescentStep(TVStack& ddv, const TVStack& residual);
-                     
-    virtual void predictMotion(btScalar solverdt);
 
     virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
 
+    // process collision between deformable and rigid
     virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap)
     {
         softBody->defaultCollisionHandler(collisionObjectWrap);
     }
-
+    
+    // process collision between deformable and deformable
     virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
         softBody->defaultCollisionHandler(otherSoftBody);
     }
-    virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
+
-    
-    virtual bool checkInitialized(){return true;}
-    
     // If true, implicit time stepping scheme is used.
     // Otherwise, explicit time stepping scheme is used
     void setImplicit(bool implicit);
@@ -136,6 +147,12 @@ public:
     // 1/2 * dv^T * M * dv
     // used in line search
     btScalar kineticEnergy();
+    
+    // unused functions
+    virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
+    virtual void solveConstraints(btScalar dt){}
+    virtual bool checkInitialized(){return true;}
+    virtual void copyBackToSoftBodies(bool bMove = true) {}
 };
 
 #endif /* btDeformableBodySolver_h */

src/BulletSoftBody/btDeformableContactProjection.h

@@ -26,8 +26,6 @@ class btDeformableContactProjection
 {
 public:
     typedef btAlignedObjectArray<btVector3> TVStack;
-    typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
-    typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
     
     // map from node index to static constraint
@@ -54,10 +52,10 @@ public:
     {
     }
     
-    // apply the constraints to the rhs
+    // apply the constraints to the rhs of the linear solve
     virtual void project(TVStack& x);
     
-    // add to friction
+    // add friction force to the rhs of the linear solve
     virtual void applyDynamicFriction(TVStack& f);
     
     // update the constraints

src/BulletSoftBody/btDeformableLagrangianForce.h

@@ -67,6 +67,7 @@ public:
     {
     }
     
+    // get number of nodes that have the force
     virtual int getNumNodes()
     {
         int numNodes = 0;
@@ -94,10 +95,7 @@ public:
         btVector3 c1 = dx[id1] - dx[id0];
         btVector3 c2 = dx[id2] - dx[id0];
         btVector3 c3 = dx[id3] - dx[id0];
-        btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
+        return btMatrix3x3(c1,c2,c3).transpose();
-                       c1.getY(), c2.getY(), c3.getY(),
-                       c1.getZ(), c2.getZ(), c3.getZ());
-        return dF;
     }
     
     // Calculate the incremental deformable generated from the current velocity
@@ -106,12 +104,10 @@ public:
         btVector3 c1 = n1->m_v - n0->m_v;
         btVector3 c2 = n2->m_v - n0->m_v;
         btVector3 c3 = n3->m_v - n0->m_v;
-        btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
+        return btMatrix3x3(c1,c2,c3).transpose();
-                       c1.getY(), c2.getY(), c3.getY(),
-                       c1.getZ(), c2.getZ(), c3.getZ());
-        return dF;
     }
     
+    // test for addScaledElasticForce function
     virtual void testDerivative()
     {
         for (int i = 0; i<m_softBodies.size();++i)
@@ -222,6 +218,7 @@ public:
         }
     }
     
+    // test for addScaledElasticForce function
     virtual void testHessian()
     {
         for (int i = 0; i<m_softBodies.size();++i)

src/BulletSoftBody/btDeformableMassSpringForce.h

@@ -20,6 +20,8 @@
 
 class btDeformableMassSpringForce : public btDeformableLagrangianForce
 {
+    // If true, the damping force will be in the direction of the spring
+    // If false, the damping force will be in the direction of the velocity
     bool m_momentum_conserving;
     btScalar m_elasticStiffness, m_dampingStiffness;
 public:

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -59,3 +59,67 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
     }
     return 0.f;
 }
+
+void btDeformableMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+{
+    m_tmpMultiBodyConstraints = multiBodyConstraints;
+    m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
+    
+    // inherited from MultiBodyConstraintSolver
+    solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+    
+    // overriden
+    solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+    
+    // inherited from MultiBodyConstraintSolver
+    solveGroupCacheFriendlyFinish(bodies, numBodies, info);
+    
+    m_tmpMultiBodyConstraints = 0;
+    m_tmpNumMultiBodyConstraints = 0;
+}
+
+void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+    btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
+                                         m_tmpSolverContactConstraintPool,
+                                         m_tmpSolverNonContactConstraintPool,
+                                         m_tmpSolverContactFrictionConstraintPool,
+                                         m_tmpSolverContactRollingFrictionConstraintPool,
+                                         m_orderTmpConstraintPool,
+                                         m_orderNonContactConstraintPool,
+                                         m_orderFrictionConstraintPool,
+                                         m_tmpConstraintSizesPool,
+                                         m_resolveSingleConstraintRowGeneric,
+                                         m_resolveSingleConstraintRowLowerLimit,
+                                         m_resolveSplitPenetrationImpulse,
+                                         m_kinematicBodyUniqueIdToSolverBodyTable,
+                                         m_btSeed2,
+                                         m_fixedBodyId,
+                                         m_maxOverrideNumSolverIterations);
+    
+    for (int i = 0; i < numBodies; i++)
+    {
+        int bodyId = siData.getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
+        
+        btRigidBody* body = btRigidBody::upcast(bodies[i]);
+        if (body && body->getInvMass())
+        {
+            btSolverBody& solverBody = siData.m_tmpSolverBodyPool[bodyId];
+            solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
+            solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
+        }
+    }
+}
+
+void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
+{
+    for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
+    {
+        btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
+        if (body)
+        {
+            m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
+            m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
+        }
+    }
+}

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h

@@ -22,6 +22,13 @@
 
 class btDeformableBodySolver;
 
+// btDeformableMultiBodyConstraintSolver extendsn btMultiBodyConstraintSolver to solve for the contact among rigid/multibody and deformable bodies. Notice that the following constraints
+// 1. rigid/multibody against rigid/multibody
+// 2. rigid/multibody against deforamble
+// 3. deformable against deformable
+// 4. deformable self collision
+// 5. joint constraints
+// are all coupled in this solve.
 ATTRIBUTE_ALIGNED16(class)
 btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
 {
@@ -32,34 +39,10 @@ protected:
     virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
     
     // write the velocity of the the solver body to the underlying rigid body
-    void solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
+    void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
-    {
+
-        for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
+    // write the velocity of the underlying rigid body to the the the solver body
-        {
+    void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
-            btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
-            if (body)
-            {
-                m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
-                m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
-            }
-        }
-    }
-    
-    void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
-    {
-        for (int i = 0; i < numBodies; i++)
-        {
-            int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
-            
-            btRigidBody* body = btRigidBody::upcast(bodies[i]);
-            if (body && body->getInvMass())
-            {
-                btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
-                solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
-                solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
-            }
-        }
-    }
     
 public:
     BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -69,23 +52,7 @@ public:
         m_deformableSolver = deformableSolver;
     }
     
-    virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+    virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
-    {
-        m_tmpMultiBodyConstraints = multiBodyConstraints;
-        m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
-        
-        // inherited from MultiBodyConstraintSolver
-        solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
-        
-        // overriden
-        solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
-        
-        // inherited from MultiBodyConstraintSolver
-        solveGroupCacheFriendlyFinish(bodies, numBodies, info);
-        
-        m_tmpMultiBodyConstraints = 0;
-        m_tmpNumMultiBodyConstraints = 0;
-    }
 };
 
 #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */

src/BulletSoftBody/btDeformableNeoHookeanForce.h

@@ -17,7 +17,7 @@ subject to the following restrictions:
 #define BT_NEOHOOKEAN_H
 
 #include "btDeformableLagrangianForce.h"
-
+#include "LinearMath/btQuickprof.h"
 // This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
 class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
 {
@@ -155,8 +155,9 @@ public:
                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
                 btMatrix3x3 P;
                 firstPiola(psb->m_tetraScratches[j],P);
-                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+//                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
                 btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
+                btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
                 
                 btSoftBody::Node* node0 = tetra.m_n[0];
                 btSoftBody::Node* node1 = tetra.m_n[1];
@@ -202,8 +203,9 @@ public:
                 btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
                 btMatrix3x3 dP;
                 firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
-                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+//                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
                 btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+                btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
 
                 // damping force differential
                 btScalar scale1 = scale * tetra.m_element_measure;
@@ -237,8 +239,9 @@ public:
                 btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
                 btMatrix3x3 dP;
                 firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
-                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+//                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
                 btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
+                btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
                 
                 // elastic force differential
                 btScalar scale1 = scale * tetra.m_element_measure;
@@ -252,27 +255,33 @@ public:
     
     void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
     {
-        P = s.m_F * m_mu * ( 1. - 1. / (s.m_trace + 1.)) + s.m_cofF * (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
+        btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.)));
+        btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
+        P = s.m_F * c1 + s.m_cofF * c2;
     }
     
     // Let P be the first piola stress.
     // This function calculates the dP = dP/dF * dF
     void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF,  btMatrix3x3& dP)
     {
-        dP = dF * m_mu * ( 1. - 1. / (s.m_trace + 1.)) + s.m_F * (2*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace)));
+        btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.));
-        
+        btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace)));
+        btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
+        dP = dF * c1 + s.m_F * c2;
         addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP);
-        dP += s.m_cofF * m_lambda * DotProduct(s.m_cofF, dF);
+        dP += s.m_cofF * c3;
     }
     
     // Let Q be the damping stress.
     // This function calculates the dP = dQ/dF * dF
     void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF,  btMatrix3x3& dP)
     {
-        dP = dF * m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)) + s.m_F * (2*m_mu_damp) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace)));
+        btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)));
-
+        btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace))));
+        btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
+        dP = dF * c1 + s.m_F * c2;
         addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP);
-        dP += s.m_cofF * m_lambda_damp * DotProduct(s.m_cofF, dF);
+        dP += s.m_cofF * c3;
     }
     
     btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)