switch from std::unordered_map to btHashMap

13 files changed, 68 insertions, 57 deletions

diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index ca7417f8f..144208908 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -10,7 +10,6 @@
 #include "btSoftBody.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
-#include <unordered_map>
 
 class btDeformableRigidDynamicsWorld;
 
@@ -21,6 +20,7 @@ struct DeformableContactConstraint
     btAlignedObjectArray<btScalar> m_value;
     // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve
     btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
+    btSoftBody::Node* node;
     
     DeformableContactConstraint(const btSoftBody::RContact& rcontact)
     {
@@ -58,7 +58,6 @@ struct DeformableContactConstraint
 
 struct DeformableFrictionConstraint
 {
-    
     btAlignedObjectArray<bool> m_static; // whether the friction is static
     btAlignedObjectArray<btScalar> m_impulse; // the impulse magnitude the node feels
     btAlignedObjectArray<btScalar> m_dv;      // the dv magnitude of the node
@@ -75,6 +74,7 @@ struct DeformableFrictionConstraint
     
     // the total impulse the node applied to the rb in the tangential direction in the cg solve
     btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse;
+    btSoftBody::Node* node;
     
     DeformableFrictionConstraint()
     {
@@ -103,22 +103,18 @@ struct DeformableFrictionConstraint
 class btCGProjection
 {
 public:
-//    static const int dim = 3;
     typedef btAlignedObjectArray<btVector3> TVStack;
     typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
     typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
-//    using TVStack = btAlignedObjectArray<btVector3>;
-//    using TVArrayStack = btAlignedObjectArray<btAlignedObjectArray<btVector3> >;
-//    using TArrayStack = btAlignedObjectArray<btAlignedObjectArray<btScalar> >;
     btAlignedObjectArray<btSoftBody *> m_softBodies;
     btDeformableRigidDynamicsWorld* m_world;
-    const std::unordered_map<btSoftBody::Node *, size_t>* m_indices;
+    const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
     const btScalar& m_dt;
     
-    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const std::unordered_map<btSoftBody::Node *, size_t>* indices)
+    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const btAlignedObjectArray<btSoftBody::Node*>* nodes)
     : m_softBodies(softBodies)
     , m_dt(dt)
-    , m_indices(indices)
+    , m_nodes(nodes)
     {
     }
     
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index a9b77e368..50db01ec9 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -9,7 +9,7 @@
 
 btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
 : m_softBodies(softBodies)
-, projection(m_softBodies, m_dt, &m_indices)
+, projection(m_softBodies, m_dt, &m_nodes)
 , m_backupVelocity(backup_v)
 {
     m_preconditioner = new DefaultPreconditioner();
@@ -63,10 +63,11 @@ 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
-    for (auto it : projection.m_constraints)
+//    for (auto it : projection.m_constraints)
+    for (int i = 0; i < projection.m_constraints.size(); ++i)
     {
-        int i = m_indices[it.first];
-        it.first->m_v = m_backupVelocity[i] + dv[i];
+        int index = projection.m_constraints.getKeyAtIndex(i).getUid1();
+        m_nodes[index]->m_v = m_backupVelocity[index] + dv[index];
     }
 }
 
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
index 22a685631..42d85f538 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
@@ -29,7 +29,7 @@ public:
     Preconditioner* m_preconditioner;
     btDeformableContactProjection projection;
     const TVStack& m_backupVelocity;
-    std::unordered_map<btSoftBody::Node *, size_t> m_indices;
+    btAlignedObjectArray<btSoftBody::Node* > m_nodes;
     
     btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
     
@@ -97,20 +97,23 @@ public:
     
     virtual void updateId()
     {
-        size_t index = 0;
+        size_t id = 0;
+        m_nodes.clear();
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
-                m_indices[&(psb->m_nodes[j])] = index++;
+                psb->m_nodes[j].index = id;
+                m_nodes.push_back(&psb->m_nodes[j]);
+                ++id;
             }
         }
     }
     
-    std::unordered_map<btSoftBody::Node *, size_t>* getIndices()
+    const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
     {
-        return &m_indices;
+        return &m_nodes;
     }
 };
 
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index a1616a460..4be5d4680 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -50,6 +50,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
     {
         m_dv.resize(m_numNodes);
         m_residual.resize(m_numNodes);
+        m_backupVelocity.resize(m_numNodes);
     }
     
     for (int i = 0; i < m_dv.size(); ++i)
@@ -121,7 +122,6 @@ bool btDeformableBodySolver::updateNodes()
     if (numNodes != m_numNodes)
     {
         m_numNodes = numNodes;
-        m_backupVelocity.resize(numNodes);
         return true;
     }
     return false;
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index 5528d8f84..9f1216eab 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -33,6 +33,7 @@ protected:
     btScalar m_dt;
     btConjugateGradient<btDeformableBackwardEulerObjective> cg;
     
+    
 public:
     btDeformableBackwardEulerObjective* m_objective;
     
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index 4ae07290c..5a3f9e555 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -8,7 +8,7 @@
 #include "btDeformableContactProjection.h"
 #include "btDeformableRigidDynamicsWorld.h"
 #include <algorithm>
-#include <math.h>
+#include <cmath>
 static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
                          btMultiBodyJacobianData& jacobianData,
                          const btVector3& contact_point,
@@ -49,10 +49,10 @@ void btDeformableContactProjection::update()
     m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo());
 
     // loop through constraints to set constrained values
-    for (auto& it : m_constraints)
+    for (int index = 0; index < m_constraints.size(); ++index)
     {
-        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
-        btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
+        btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
         for (int i = 0; i < constraints.size(); ++i)
         {
             DeformableContactConstraint& constraint = constraints[i];
@@ -230,13 +230,13 @@ void btDeformableContactProjection::setConstraints()
                 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[&(psb->m_nodes[j])] = c;
+                m_constraints.insert(psb->m_nodes[j].index, c);
                 
                 btAlignedObjectArray<DeformableFrictionConstraint> f;
                 f.push_back(DeformableFrictionConstraint());
                 f.push_back(DeformableFrictionConstraint());
                 f.push_back(DeformableFrictionConstraint());
-                m_frictions[&(psb->m_nodes[j])] = f;
+                m_frictions.insert(psb->m_nodes[j].index, f);
             }
         }
     }
@@ -290,22 +290,22 @@ void btDeformableContactProjection::setConstraints()
                 const btScalar dn = btDot(vr, cti.m_normal);
                 if (dn < SIMD_EPSILON)
                 {
-                    if (m_constraints.find(c.m_node) == m_constraints.end())
+                    if (m_constraints.find(c.m_node->index) == NULL)
                     {
                         btAlignedObjectArray<DeformableContactConstraint> constraints;
                         constraints.push_back(DeformableContactConstraint(c));
-                        m_constraints[c.m_node] = constraints;
+                        m_constraints.insert(c.m_node->index,constraints);
                         btAlignedObjectArray<DeformableFrictionConstraint> frictions;
                         frictions.push_back(DeformableFrictionConstraint());
-                        m_frictions[c.m_node] = frictions;
+                        m_frictions.insert(c.m_node->index,frictions);
                     }
                     else
                     {
                         // group colinear constraints into one
                         const btScalar angle_epsilon = 0.015192247; // less than 10 degree
                         bool merged = false;
-                        btAlignedObjectArray<DeformableContactConstraint>& constraints = m_constraints[c.m_node];
-                        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[c.m_node];
+                        btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints[c.m_node->index];
+                        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[c.m_node->index];
                         for (int j = 0; j < constraints.size(); ++j)
                         {
                             const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
@@ -337,11 +337,11 @@ void btDeformableContactProjection::setConstraints()
 void btDeformableContactProjection::enforceConstraint(TVStack& x)
 {
     const int dim = 3;
-    for (auto& it : m_constraints)
+    for (int index = 0; index < m_constraints.size(); ++index)
     {
-        const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
-        size_t i = m_indices->at(it.first);
-        const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
+        const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
+        size_t i = m_constraints.getKeyAtIndex(index).getUid1();
+        const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
         btAssert(constraints.size() <= dim);
         btAssert(constraints.size() > 0);
         if (constraints.size() == 1)
@@ -400,11 +400,11 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
 void btDeformableContactProjection::project(TVStack& x)
 {
     const int dim = 3;
-    for (auto& it : m_constraints)
+    for (int index = 0; index < m_constraints.size(); ++index)
     {
-        const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
-        size_t i = m_indices->at(it.first);
-        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
+        const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
+        size_t i = m_constraints.getKeyAtIndex(index).getUid1();
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
         btAssert(constraints.size() <= dim);
         btAssert(constraints.size() > 0);
         if (constraints.size() == 1)
diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h
index 2ca571195..9e61fed34 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.h
+++ b/src/BulletSoftBody/btDeformableContactProjection.h
@@ -11,14 +11,19 @@
 #include "btSoftBody.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "LinearMath/btHashMap.h"
 class btDeformableContactProjection : public btCGProjection
 {
 public:
-    std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
-    std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableFrictionConstraint> > m_frictions;
+//    std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
+//    std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableFrictionConstraint> > m_frictions;
     
-    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const std::unordered_map<btSoftBody::Node *, size_t>* indices)
-    : btCGProjection(softBodies, dt, indices)
+    // map from node index to constraints
+    btHashMap<btHashInt, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
+    btHashMap<btHashInt, btAlignedObjectArray<DeformableFrictionConstraint> >m_frictions;
+    
+    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const btAlignedObjectArray<btSoftBody::Node*>* nodes)
+    : btCGProjection(softBodies, dt, nodes)
     {
     }
     
diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h
index 88d9107e1..0c2ae535d 100644
--- a/src/BulletSoftBody/btDeformableGravityForce.h
+++ b/src/BulletSoftBody/btDeformableGravityForce.h
@@ -47,7 +47,7 @@ public:
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
                 btSoftBody::Node& n = psb->m_nodes[j];
-                size_t id = m_indices->at(&n);
+                size_t id = n.index;
                 btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
                 btVector3 scaled_force = scale * m_gravity * mass;
                 force[id] += scaled_force;
diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h
index ff75eeb1c..6e1c54352 100644
--- a/src/BulletSoftBody/btDeformableLagrangianForce.h
+++ b/src/BulletSoftBody/btDeformableLagrangianForce.h
@@ -9,7 +9,8 @@
 #define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
 
 #include "btSoftBody.h"
-#include <unordered_map>
+#include <LinearMath/btHashMap.h>
+
 enum btDeformableLagrangianForceType
 {
     BT_GRAVITY_FORCE = 1,
@@ -22,7 +23,7 @@ public:
 //    using TVStack = btAlignedObjectArray<btVector3>;
     typedef btAlignedObjectArray<btVector3> TVStack;
     btAlignedObjectArray<btSoftBody *> m_softBodies;
-    const std::unordered_map<btSoftBody::Node *, size_t>* m_indices;
+    const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
     
     btDeformableLagrangianForce()
     {
@@ -57,9 +58,9 @@ public:
         m_softBodies.push_back(psb);
     }
     
-    virtual void setIndices(const std::unordered_map<btSoftBody::Node *, size_t>* indices)
+    virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
     {
-        m_indices = indices;
+        m_nodes = nodes;
     }
 };
 #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */
diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h
index d3ccd70dc..841101c8f 100644
--- a/src/BulletSoftBody/btDeformableMassSpringForce.h
+++ b/src/BulletSoftBody/btDeformableMassSpringForce.h
@@ -41,8 +41,8 @@ public:
                 const btSoftBody::Link& link = psb->m_links[j];
                 btSoftBody::Node* node1 = link.m_n[0];
                 btSoftBody::Node* node2 = link.m_n[1];
-                size_t id1 = m_indices->at(node1);
-                size_t id2 = m_indices->at(node2);
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
                 
                 // damping force
                 btVector3 v_diff = (node2->m_v - node1->m_v);
@@ -68,8 +68,8 @@ public:
                 btSoftBody::Node* node2 = link.m_n[1];
                 btScalar kLST = link.Feature::m_material->m_kLST;
                 btScalar r = link.m_rl;
-                size_t id1 = m_indices->at(node1);
-                size_t id2 = m_indices->at(node2);
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
                 
                 // elastic force
                 // explicit elastic force
@@ -94,8 +94,8 @@ public:
                 btSoftBody::Node* node1 = link.m_n[0];
                 btSoftBody::Node* node2 = link.m_n[1];
                 btScalar k_damp = psb->m_dampingCoefficient;
-                size_t id1 = m_indices->at(node1);
-                size_t id2 = m_indices->at(node2);
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
                 btVector3 local_scaled_df = scale * 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 78c5f24a9..6e1ed75bd 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -45,10 +45,11 @@ 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)
+//    for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints)
+    for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index)
     {
-        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_deformableBodySolver->m_objective->projection.m_frictions[it.first];
-        btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)];
+        btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
         for (int i = 0; i < constraints.size(); ++i)
         {
             DeformableContactConstraint& constraint = constraints[i];
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 5e33d74d0..87633a83c 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -258,6 +258,7 @@ public:
 		btScalar m_area;     // Area
 		btDbvtNode* m_leaf;  // Leaf data
 		int m_battach : 1;   // Attached
+        int index;
 	};
 	/* Link			*/
 	ATTRIBUTE_ALIGNED16(struct)
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 77bcdc5d8..eb487a56f 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -995,7 +995,9 @@ struct btSoftColliders
                                 btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
                                 btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
                                 
-                                btMatrix3x3 rot(normal, t1, t2); // world frame to local frame
+                                btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+                                                t1.getX(), t1.getY(), t1.getZ(),
+                                                t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
                                 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
                                 btScalar dt = psb->m_sst.sdt;
                                 btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();