constraints between bodies merge constraint simulations (this fixes problems with deactivation/sleeping)

Bullet/CollisionDispatch/CollisionDispatcher.cpp

@@ -122,8 +122,10 @@ void CollisionDispatcher::ReleaseManifold(PersistentManifold* manifold)
 //
 // todo: this is random access, it can be walked 'cache friendly'!
 //
-void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback* callback)
+void CollisionDispatcher::BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback)
 {
+	int numBodies  = collisionObjects.size();
+
 	for (int islandId=0;islandId<numBodies;islandId++)
 	{
 
@@ -133,7 +135,25 @@ void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback*
 
 		bool allSleeping = true;
 
-		for (int i=0;i<GetNumManifolds();i++)
+		int i;
+		for (i=0;i<numBodies;i++)
+		{
+			CollisionObject* colObj0 = collisionObjects[i];
+			if (colObj0->m_islandTag1 == islandId)
+			{
+				if (colObj0->GetActivationState()== ACTIVE_TAG)
+				{
+					allSleeping = false;
+				}
+				if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
+				{
+					allSleeping = false;
+				}
+			}
+		}
+
+		
+		for (i=0;i<GetNumManifolds();i++)
 		{
 			 PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
 			 
@@ -141,25 +161,11 @@ void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback*
 
 			 CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
 			 CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
-
-
-			 
 			 {
 				if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
 					((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
 				{
 
-					if (((colObj0) && (colObj0)->GetActivationState()== ACTIVE_TAG) ||
-						((colObj1) && (colObj1)->GetActivationState() == ACTIVE_TAG))
-					{
-						allSleeping = false;
-					}
-					if (((colObj0) && (colObj0)->GetActivationState()== DISABLE_DEACTIVATION) ||
-						((colObj1) && (colObj1)->GetActivationState() == DISABLE_DEACTIVATION))
-					{
-						allSleeping = false;
-					}
-
 					if (NeedsResponse(*colObj0,*colObj1))
 						islandmanifold.push_back(manifold);
 				}
@@ -167,51 +173,34 @@ void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback*
 		}
 		if (allSleeping)
 		{
-			//tag all as 'ISLAND_SLEEPING'
+			int i;
-			for (size_t i=0;i<islandmanifold.size();i++)
+			for (i=0;i<numBodies;i++)
 			{
-				PersistentManifold* manifold = islandmanifold[i];
+				CollisionObject* colObj0 = collisionObjects[i];
-
+				if (colObj0->m_islandTag1 == islandId)
-				CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
-				CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
-
-				if ((colObj0))	
 				{
-					(colObj0)->SetActivationState( ISLAND_SLEEPING );
+					colObj0->SetActivationState( ISLAND_SLEEPING );
 				}
-				if ((colObj1))	
-				{
-					(colObj1)->SetActivationState( ISLAND_SLEEPING);
 			}
 
-			}
+			
 		} else
 		{
 
-			//tag all as 'ISLAND_SLEEPING'
+			int i;
-			for (size_t i=0;i<islandmanifold.size();i++)
+			for (i=0;i<numBodies;i++)
 			{
-				 PersistentManifold* manifold = islandmanifold[i];
+				CollisionObject* colObj0 = collisionObjects[i];
-				 CollisionObject* body0 = (CollisionObject*)manifold->GetBody0();
+				if (colObj0->m_islandTag1 == islandId)
-				 CollisionObject* body1 = (CollisionObject*)manifold->GetBody1();
-
-				if (body0)	
 				{
-					if ( body0->GetActivationState() == ISLAND_SLEEPING)
+					if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
 					{
-						body0->SetActivationState( WANTS_DEACTIVATION);
+						colObj0->SetActivationState( WANTS_DEACTIVATION);
 					}
 				}
-				if (body1)	
-				{
-					if ( body1->GetActivationState() == ISLAND_SLEEPING)
-					{
-						body1->SetActivationState(WANTS_DEACTIVATION);
-					}
-				}
-
 			}
 
+			/// Process the actual simulation, only if not sleeping/deactivated
 			if (islandmanifold.size())
 			{
 				callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());

Bullet/CollisionDispatch/CollisionDispatcher.h

@@ -26,7 +26,7 @@ subject to the following restrictions:
 
 class IDebugDraw;
 
-
+typedef std::vector<struct CollisionObject*> CollisionObjectArray;
 
 
 struct CollisionAlgorithmCreateFunc
@@ -106,7 +106,7 @@ public:
 	virtual void ReleaseManifold(PersistentManifold* manifold);
 
 	
-	virtual void BuildAndProcessIslands(int numBodies, IslandCallback* callback);
+	virtual void BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback);
 
 	///allows the user to get contact point callbacks 
 	virtual	ManifoldResult*	GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold);

Bullet/CollisionDispatch/CollisionWorld.cpp

@@ -76,6 +76,14 @@ void	CollisionWorld::UpdateActivationState()
 	
 	m_dispatcher->FindUnions();
 	
+
+	
+}
+
+
+
+void	CollisionWorld::StoreIslandActivationState()
+{
 	// put the islandId ('find' value) into m_tag	
 	{
 		UnionFind& unionFind = m_dispatcher->GetUnionFind();
@@ -98,7 +106,6 @@ void	CollisionWorld::UpdateActivationState()
 			index++;
 		}
 	}
-	
 }
 
 

Bullet/CollisionDispatch/CollisionWorld.h

@@ -13,6 +13,7 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+
 /**
  * @mainpage Bullet Documentation
  *
@@ -57,19 +58,23 @@ subject to the following restrictions:
  * 
  */
  
+ 
+
 #ifndef COLLISION_WORLD_H
 #define COLLISION_WORLD_H
 
 
 class CollisionShape;
-class CollisionDispatcher;
 class BroadphaseInterface;
 #include "SimdVector3.h"
 #include "SimdTransform.h"
 #include "CollisionObject.h"
+#include "CollisionDispatcher.h" //for definition of CollisionObjectArray
 
 #include <vector>
 
+
+
 ///CollisionWorld is interface and container for the collision detection
 class CollisionWorld
 {
@@ -93,6 +98,7 @@ public:
 	virtual ~CollisionWorld();
 
 	virtual	void	UpdateActivationState();
+	virtual	void	StoreIslandActivationState();
 
 	BroadphaseInterface*	GetBroadphase()
 	{
@@ -197,6 +203,17 @@ public:
 
 	void	AddCollisionObject(CollisionObject* collisionObject);
 
+	CollisionObjectArray& GetCollisionObjectArray()
+	{
+		return m_collisionObjects;
+	}
+
+	const CollisionObjectArray& GetCollisionObjectArray() const
+	{
+		return m_collisionObjects;
+	}
+
+
 	void	RemoveCollisionObject(CollisionObject* collisionObject);
 
 	virtual void	PerformDiscreteCollisionDetection();

Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp

@@ -80,8 +80,6 @@ RaycastVehicle::VehicleTuning	gTuning;
 #include "ConstraintSolver/Generic6DofConstraint.h"
 
 
-
-
 //#include "BroadphaseCollision/QueryDispatcher.h"
 //#include "BroadphaseCollision/QueryBox.h"
 //todo: change this to allow dynamic registration of types!
@@ -506,6 +504,10 @@ void	CcdPhysicsEnvironment::beginFrame()
 
 bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 {
+	//don't simulate without timesubsteps
+	if (m_numTimeSubSteps<1)
+		return true;
+
 	//printf("proceedDeltaTime\n");
 	
 
@@ -686,6 +688,27 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 
 	m_collisionWorld->UpdateActivationState();
 
+	{
+		int i;
+		int numConstraints = m_constraints.size();
+		for (i=0;i< numConstraints ; i++ )
+		{
+			TypedConstraint* constraint = m_constraints[i];
+
+			const CollisionObject* colObj0 = &constraint->GetRigidBodyA();
+			const CollisionObject* colObj1 = &constraint->GetRigidBodyB();
+			
+			if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
+						((colObj1) && ((colObj1)->mergesSimulationIslands())))
+			{
+				GetDispatcher()->GetUnionFind().unite((colObj0)->m_islandTag1,
+					(colObj1)->m_islandTag1);
+			}
+		}
+	}
+
+	m_collisionWorld->StoreIslandActivationState();
+
 
 
 	//contacts
@@ -778,7 +801,7 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 #endif //USE_QUICKPROF
 
 	/// solve all the contact points and contact friction
-	GetDispatcher()->BuildAndProcessIslands(numRigidBodies,&solverCallback);
+	GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
 
 #ifdef USE_QUICKPROF
 	Profiler::endBlock("BuildAndProcessIslands");