add profiling and code clean up

8 files changed, 27 insertions, 20 deletions

diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
index dfec3dde5..b8dac7184 100644
--- a/src/BulletSoftBody/btConjugateGradient.h
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -17,15 +17,13 @@
 #include <cmath>
 #include <LinearMath/btAlignedObjectArray.h>
 #include <LinearMath/btVector3.h>
-
+#include "LinearMath/btQuickprof.h"
 template <class MatrixX>
 class btConjugateGradient
 {
-//    using TVStack = btAlignedObjectArray<btVector3>;
     typedef btAlignedObjectArray<btVector3> TVStack;
     TVStack r,p,z,temp;
     int max_iterations;
-    
 public:
     btConjugateGradient(const int max_it_in)
     : max_iterations(max_it_in)
@@ -37,6 +35,7 @@ public:
     // return the number of iterations taken
     int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance)
     {
+        BT_PROFILE("CGSolve");
         btAssert(x.size() == b.size());
         reinitialize(b);
         
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index 618648b47..44783fb3d 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -12,6 +12,7 @@
  */
 
 #include "btDeformableBackwardEulerObjective.h"
+#include "LinearMath/btQuickprof.h"
 
 btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
 : m_softBodies(softBodies)
@@ -23,6 +24,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned
 
 void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated)
 {
+    BT_PROFILE("reinitialize");
     if(nodeUpdated)
     {
         updateId();
@@ -43,9 +45,7 @@ void btDeformableBackwardEulerObjective::setDt(btScalar dt)
 
 void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
 {
-    for (int i = 0; i < b.size(); ++i)
-        b[i].setZero();
-    
+    BT_PROFILE("multiply");
     // add in the mass term
     size_t counter = 0;
     for (int i = 0; i < m_softBodies.size(); ++i)
@@ -54,11 +54,11 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             const btSoftBody::Node& node = psb->m_nodes[j];
-            b[counter] += (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
+            b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
             ++counter;
         }
     }
-    
+
     for (int i = 0; i < m_lf.size(); ++i)
     {
         // add damping matrix
@@ -97,6 +97,7 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
 
 void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
 {
+    BT_PROFILE("computeResidual");
     // add implicit force
     for (int i = 0; i < m_lf.size(); ++i)
     {
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
index db448ebe9..82af57f6d 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
@@ -13,7 +13,6 @@
 
 #ifndef BT_BACKWARD_EULER_OBJECTIVE_H
 #define BT_BACKWARD_EULER_OBJECTIVE_H
-#include <functional>
 #include "btConjugateGradient.h"
 #include "btDeformableLagrangianForce.h"
 #include "btDeformableMassSpringForce.h"
@@ -21,6 +20,7 @@
 #include "btDeformableContactProjection.h"
 #include "btPreconditioner.h"
 #include "btDeformableRigidDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
 
 class btDeformableRigidDynamicsWorld;
 class btDeformableBackwardEulerObjective
@@ -36,7 +36,6 @@ public:
     btDeformableContactProjection projection;
     const TVStack& m_backupVelocity;
     btAlignedObjectArray<btSoftBody::Node* > m_nodes;
-    
     btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
     
     virtual ~btDeformableBackwardEulerObjective() {}
@@ -72,6 +71,7 @@ public:
     // enforce constraints in CG solve
     void enforceConstraint(TVStack& x)
     {
+        BT_PROFILE("enforceConstraint");
         projection.enforceConstraint(x);
         updateVelocity(x);
     }
@@ -85,6 +85,7 @@ public:
     // update the projections and project the residual
     void project(TVStack& r)
     {
+        BT_PROFILE("project");
         projection.update();
         projection.project(r);
     }
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 27cb8e467..9382106e9 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -14,6 +14,7 @@
 #include <stdio.h>
 #include <limits>
 #include "btDeformableBodySolver.h"
+#include "LinearMath/btQuickprof.h"
 
 btDeformableBodySolver::btDeformableBodySolver()
 : m_numNodes(0)
@@ -29,6 +30,7 @@ btDeformableBodySolver::~btDeformableBodySolver()
 
 void btDeformableBodySolver::solveConstraints(float solverdt)
 {
+    BT_PROFILE("solveConstraints");
     m_objective->setDt(solverdt);
     
     // add constraints to the solver
@@ -70,6 +72,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
 
 void btDeformableBodySolver::setConstraints()
 {
+    BT_PROFILE("setConstraint");
     m_objective->setConstraints();
 }
 
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index dff5398c9..7470c7a93 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -224,6 +224,7 @@ void btDeformableContactProjection::update()
 
 void btDeformableContactProjection::setConstraints()
 {
+    BT_PROFILE("setConstraints");
     // set Dirichlet constraint
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
@@ -233,12 +234,14 @@ void btDeformableContactProjection::setConstraints()
             if (psb->m_nodes[j].m_im == 0)
             {
                 btAlignedObjectArray<DeformableContactConstraint> c;
+                c.reserve(3);
                 c.push_back(DeformableContactConstraint(btVector3(1,0,0)));
                 c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
                 c.push_back(DeformableContactConstraint(btVector3(0,0,1)));
                 m_constraints.insert(psb->m_nodes[j].index, c);
                 
                 btAlignedObjectArray<DeformableFrictionConstraint> f;
+                f.reserve(3);
                 f.push_back(DeformableFrictionConstraint());
                 f.push_back(DeformableFrictionConstraint());
                 f.push_back(DeformableFrictionConstraint());
@@ -246,13 +249,12 @@ void btDeformableContactProjection::setConstraints()
             }
         }
     }
-
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
         btMultiBodyJacobianData jacobianData_normal;
         btMultiBodyJacobianData jacobianData_complementary;
-        
+        std::cout <<psb->m_rcontacts.size() << std::endl;
         for (int j = 0; j < psb->m_rcontacts.size(); ++j)
         {
             const btSoftBody::RContact& c = psb->m_rcontacts[j];
@@ -296,6 +298,7 @@ void btDeformableContactProjection::setConstraints()
                 const btScalar dn = btDot(vr, cti.m_normal);
                 if (dn < SIMD_EPSILON)
                 {
+                    
                     if (m_constraints.find(c.m_node->index) == NULL)
                     {
                         btAlignedObjectArray<DeformableContactConstraint> constraints;
@@ -369,7 +372,6 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
                     x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
                 }
             }
-            
         }
         else
         {
diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h
index 2d70ef0ae..ca8ddf983 100644
--- a/src/BulletSoftBody/btDeformableMassSpringForce.h
+++ b/src/BulletSoftBody/btDeformableMassSpringForce.h
@@ -94,15 +94,15 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
+            btScalar scaled_k_damp = psb->m_dampingCoefficient * scale;
             for (int j = 0; j < psb->m_links.size(); ++j)
             {
                 const btSoftBody::Link& link = psb->m_links[j];
                 btSoftBody::Node* node1 = link.m_n[0];
                 btSoftBody::Node* node2 = link.m_n[1];
-                btScalar k_damp = psb->m_dampingCoefficient;
                 size_t id1 = node1->index;
                 size_t id2 = node2->index;
-                btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]);
+                btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
                 df[id1] += local_scaled_df;
                 df[id2] -= local_scaled_df;
             }
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index ef48f97b1..116d4e0bc 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -14,10 +14,11 @@
 #include <stdio.h>
 #include "btDeformableRigidDynamicsWorld.h"
 #include "btDeformableBodySolver.h"
-
+#include "LinearMath/btQuickprof.h"
 
 void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
+    BT_PROFILE("internalSingleStepSimulation");
     reinitialize(timeStep);
     // add gravity to velocity of rigid and multi bodys
     applyRigidBodyGravity(timeStep);
@@ -50,7 +51,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
 void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
 {
     // perform position correction for all constraints 
-//    for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints)
+    BT_PROFILE("positionCorrection");
     for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index)
     {
         btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)];
@@ -134,6 +135,7 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
 
 void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt)
 {
+    BT_PROFILE("integrateTransforms");
     m_deformableBodySolver->backupVelocity();
     positionCorrection(dt);
     btMultiBodyDynamicsWorld::integrateTransforms(dt);
@@ -169,6 +171,7 @@ void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collision
 
 void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
+    BT_PROFILE("predictUnconstraintMotion");
     btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
     m_deformableBodySolver->predictMotion(timeStep);
 }
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
index 7d771d455..cc4fdae05 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
@@ -78,9 +78,7 @@ public:
         m_sbi.m_sparsesdf.Initialize();
         m_internalTime = 0.0;
     }
-//    btAlignedObjectArray<std::function<void(btScalar, btDeformableRigidDynamicsWorld*)> > m_beforeSolverCallbacks;
-    
-    
+
     void setSolverCallback(btSolverCallback cb)
     {
         m_solverCallback = cb;