Do not std::map to store mapping from rigid body to index in array

src/body/RigidBody.cpp

@@ -42,7 +42,7 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde
           : CollisionBody(transform, world, id), mInitMass(decimal(1.0)),
             mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
             mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
-            mJointsList(nullptr) {
+            mJointsList(nullptr), mArrayIndex(0) {
 
     // Compute the inverse mass
     mMassInverse = decimal(1.0) / mInitMass;

src/body/RigidBody.h

@@ -50,6 +50,11 @@ class DynamicsWorld;
   */
 class RigidBody : public CollisionBody {
 
+    private :
+
+        /// Index of the body in arrays for contact/constraint solver
+        uint mArrayIndex;
+
     protected :
 
         // -------------------- Attributes -------------------- //

src/constraint/BallAndSocketJoint.cpp

@@ -45,8 +45,8 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
 void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
     // Initialize the bodies index in the velocity array
-    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
+    mIndexBody1 = mBody1->mArrayIndex;
-    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+    mIndexBody2 = mBody2->mArrayIndex;
 
     // Get the bodies center of mass and orientations
     const Vector3& x1 = mBody1->mCenterOfMassWorld;

src/constraint/FixedJoint.cpp

@@ -53,8 +53,8 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
 void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
     // Initialize the bodies index in the velocity array
-    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
+    mIndexBody1 = mBody1->mArrayIndex;
-    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+    mIndexBody2 = mBody2->mArrayIndex;
 
     // Get the bodies positions and orientations
     const Vector3& x1 = mBody1->mCenterOfMassWorld;

src/constraint/HingeJoint.cpp

@@ -68,8 +68,8 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
 void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
     // Initialize the bodies index in the velocity array
-    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
+    mIndexBody1 = mBody1->mArrayIndex;
-    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+    mIndexBody2 = mBody2->mArrayIndex;
 
     // Get the bodies positions and orientations
     const Vector3& x1 = mBody1->mCenterOfMassWorld;

src/constraint/SliderJoint.cpp

@@ -67,8 +67,8 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
 void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
 
     // Initialize the bodies index in the veloc ity array
-    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
+    mIndexBody1 = mBody1->mArrayIndex;
-    mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+    mIndexBody2 = mBody2->mArrayIndex;
 
     // Get the bodies positions and orientations
     const Vector3& x1 = mBody1->mCenterOfMassWorld;

src/engine/ConstraintSolver.cpp

@@ -30,9 +30,7 @@
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
+ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) {
-                 : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
-                   mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
 
 #ifdef IS_PROFILING_ACTIVE
 

src/engine/ConstraintSolver.h

@@ -60,18 +60,12 @@ struct ConstraintSolverData {
         /// Reference to the bodies orientations
         Quaternion* orientations;
 
-        /// Reference to the map that associates rigid body to their index
-        /// in the constrained velocities array
-        const std::map<RigidBody*, uint>& mapBodyToConstrainedVelocityIndex;
-
         /// True if warm starting of the solver is active
         bool isWarmStartingActive;
 
         /// Constructor
-        ConstraintSolverData(const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
+        ConstraintSolverData() :linearVelocities(nullptr), angularVelocities(nullptr),
-                           :linearVelocities(nullptr), angularVelocities(nullptr),
+                                positions(nullptr), orientations(nullptr) {
-                            positions(nullptr), orientations(nullptr),
-                            mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
 
         }
 
@@ -152,10 +146,6 @@ class ConstraintSolver {
 
         // -------------------- Attributes -------------------- //
 
-        /// Reference to the map that associates rigid body to their index in
-        /// the constrained velocities array
-        const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
-
         /// Current time step
         decimal mTimeStep;
 
@@ -176,7 +166,7 @@ class ConstraintSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
+        ConstraintSolver();
 
         /// Destructor
         ~ConstraintSolver() = default;

src/engine/ContactSolver.cpp

@@ -39,12 +39,10 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
 const decimal ContactSolver::SLOP = decimal(0.01);
 
 // Constructor
-ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
+ContactSolver::ContactSolver(SingleFrameAllocator& allocator)
-                             SingleFrameAllocator& allocator)
               :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr),
                mContactConstraints(nullptr), mSingleFrameAllocator(allocator),
                mLinearVelocities(nullptr), mAngularVelocities(nullptr),
-               mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
                mIsSplitImpulseActive(true) {
 
 }
@@ -131,8 +129,8 @@ void ContactSolver::initializeForIsland(Island* island) {
         // Initialize the internal contact manifold structure using the external
         // contact manifold
         new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
-        mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
+        mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex;
-        mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
+        mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex;
         mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
         mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
         mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;

src/engine/ContactSolver.h

@@ -302,9 +302,6 @@ class ContactSolver {
         /// Array of angular velocities
         Vector3* mAngularVelocities;
 
-        /// Reference to the map of rigid body to their index in the constrained velocities array
-        const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
-
         /// True if the split impulse position correction is active
         bool mIsSplitImpulseActive;
 
@@ -342,8 +339,7 @@ class ContactSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
+        ContactSolver(SingleFrameAllocator& allocator);
-                      SingleFrameAllocator& allocator);
 
         /// Destructor
         ~ContactSolver() = default;

src/engine/DynamicsWorld.cpp

@@ -41,8 +41,7 @@ using namespace std;
  */
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
               : CollisionWorld(),
-                mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator),
+                mContactSolver(mSingleFrameAllocator),
-                mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
                 mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
                 mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
                 mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
@@ -167,7 +166,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
         for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
 
             // Get the constrained velocity
-            uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
+            uint indexArray = bodies[b]->mArrayIndex;
             Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
             Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
 
@@ -205,7 +204,7 @@ void DynamicsWorld::updateBodiesState() {
 
         for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
 
-            uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
+            uint index = bodies[b]->mArrayIndex;
 
             // Update the linear and angular velocity of the body
             bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
@@ -250,21 +249,14 @@ void DynamicsWorld::initVelocityArrays() {
     assert(mConstrainedPositions != nullptr);
     assert(mConstrainedOrientations != nullptr);
 
-    // Reset the velocities arrays
+    // Initialize the map of body indexes in the velocity arrays
-    for (uint i=0; i<nbBodies; i++) {
+    uint i = 0;
+    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+
         mSplitLinearVelocities[i].setToZero();
         mSplitAngularVelocities[i].setToZero();
-    }
 
-    // Initialize the map of body indexes in the velocity arrays
+        (*it)->mArrayIndex = i++;
-    mMapBodyToConstrainedVelocityIndex.clear();
-    std::set<RigidBody*>::const_iterator it;
-    uint indexBody = 0;
-    for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-
-        // Add the body into the map
-        mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody));
-        indexBody++;
     }
 }
 
@@ -289,7 +281,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
         for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
 
             // Insert the body into the map of constrained velocities
-            uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
+            uint indexBody = bodies[b]->mArrayIndex;
 
             assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
             assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));

src/engine/DynamicsWorld.h

@@ -100,9 +100,6 @@ class DynamicsWorld : public CollisionWorld {
         /// Array of constrained rigid bodies orientation (for position error correction)
         Quaternion* mConstrainedOrientations;
 
-        /// Map body to their index in the constrained velocities array
-        std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
-
         /// Number of islands in the world
         uint mNbIslands;