collisionObject: renamed uniqueId to worldArrayIndex; removed linear search in removeCollisionObject

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -28,6 +28,7 @@ btCollisionObject::btCollisionObject()
 		m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
 		m_islandTag1(-1),
 		m_companionId(-1),
+        m_worldArrayIndex(-1),
 		m_activationState1(1),
 		m_deactivationTime(btScalar(0.)),
 		m_friction(btScalar(0.5)),

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -79,7 +79,7 @@ protected:
 
 	int				m_islandTag1;
 	int				m_companionId;
-    int             m_uniqueId;
+    int             m_worldArrayIndex;  // index of object in world's collisionObjects array
 
 	mutable int				m_activationState1;
 	mutable btScalar			m_deactivationTime;
@@ -456,14 +456,15 @@ public:
 		m_companionId = id;
 	}
 
-    SIMD_FORCE_INLINE int getUniqueId() const
+    SIMD_FORCE_INLINE int getWorldArrayIndex() const
     {
-        return	m_uniqueId;
+        return	m_worldArrayIndex;
     }
 
-    void	setUniqueId( int id )
+    // only should be called by CollisionWorld
+    void setWorldArrayIndex(int ix)
     {
-        m_uniqueId = id;
+        m_worldArrayIndex = ix;
     }
 
     SIMD_FORCE_INLINE btScalar			getHitFraction() const

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -115,7 +115,9 @@ void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
 
 	//check that the object isn't already added
 	btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
+    btAssert(collisionObject->getWorldArrayIndex() == -1);  // do not add the same object to more than one collision world
 
+    collisionObject->setWorldArrayIndex(m_collisionObjects.size());
 	m_collisionObjects.push_back(collisionObject);
 
 	//calculate new AABB
@@ -195,6 +197,7 @@ void	btCollisionWorld::updateAabbs()
 	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
+        btAssert(colObj->getWorldArrayIndex() == i);
 
 		//only update aabb of active objects
 		if (m_forceUpdateAllAabbs || colObj->isActive())
@@ -253,9 +256,25 @@ void	btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
 	}
 
 
-	//swapremove
+    int iObj = collisionObject->getWorldArrayIndex();
-	m_collisionObjects.remove(collisionObject);
+    btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously?
-
+    if (iObj >= 0 && iObj < m_collisionObjects.size())
+    {
+        btAssert(collisionObject == m_collisionObjects[iObj]);
+        m_collisionObjects.swap(iObj, m_collisionObjects.size()-1);
+        m_collisionObjects.pop_back();
+        if (iObj < m_collisionObjects.size())
+        {
+            m_collisionObjects[iObj]->setWorldArrayIndex(iObj);
+        }
+    }
+    else
+    {
+        // slow linear search
+        //swapremove
+        m_collisionObjects.remove(collisionObject);
+    }
+    collisionObject->setWorldArrayIndex(-1);
 }
 
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -751,7 +751,7 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
         // Kinematic bodies can be in multiple islands at once, so it is a
         // race condition to write to them, so we use an alternate method
         // to record the solverBodyId
-        int uniqueId = body.getUniqueId();
+        int uniqueId = body.getWorldArrayIndex();
         const int INVALID_SOLVER_BODY_ID = -1;
         if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
         {

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp

@@ -149,7 +149,6 @@ void	btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
 {
 	BT_PROFILE("solveConstraints");
 
-
 	m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 
@@ -161,8 +160,3 @@ void	btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
 }
 
 
-void btDiscreteDynamicsWorldMt::addCollisionObject(btCollisionObject* collisionObject, short int collisionFilterGroup, short int collisionFilterMask)
-{
-    collisionObject->setUniqueId(m_collisionObjects.size());
-    btDiscreteDynamicsWorld::addCollisionObject(collisionObject, collisionFilterGroup, collisionFilterMask);
-}

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h

@@ -35,8 +35,6 @@ protected:
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
-
 	btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
 	virtual ~btDiscreteDynamicsWorldMt();
 };