fixed Bullet bug: constraints didn't merge simulation islands.

4 files changed, 61 insertions, 51 deletions

diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp
index 0c746781758..d50624cc5fb 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp
+++ b/extern/bullet/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'
-			for (size_t i=0;i<islandmanifold.size();i++)
+			int i;
+			for (i=0;i<numBodies;i++)
 			{
-				PersistentManifold* manifold = islandmanifold[i];
-
-				CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
-				CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
-
-				if ((colObj0))	
-				{
-					(colObj0)->SetActivationState( ISLAND_SLEEPING );
-				}
-				if ((colObj1))	
+				CollisionObject* colObj0 = collisionObjects[i];
+				if (colObj0->m_islandTag1 = islandId)
 				{
-					(colObj1)->SetActivationState( ISLAND_SLEEPING);
+					colObj0->SetActivationState( ISLAND_SLEEPING );
 				}
-
 			}
+
+			
 		} else
 		{
 
-			//tag all as 'ISLAND_SLEEPING'
-			for (size_t i=0;i<islandmanifold.size();i++)
+			int i;
+			for (i=0;i<numBodies;i++)
 			{
-				 PersistentManifold* manifold = islandmanifold[i];
-				 CollisionObject* body0 = (CollisionObject*)manifold->GetBody0();
-				 CollisionObject* body1 = (CollisionObject*)manifold->GetBody1();
-
-				if (body0)	
-				{
-					if ( body0->GetActivationState() == ISLAND_SLEEPING)
-					{
-						body0->SetActivationState( WANTS_DEACTIVATION);
-					}
-				}
-				if (body1)	
+				CollisionObject* colObj0 = collisionObjects[i];
+				if (colObj0->m_islandTag1 = islandId)
 				{
-					if ( body1->GetActivationState() == ISLAND_SLEEPING)
+					if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
 					{
-						body1->SetActivationState(WANTS_DEACTIVATION);
+						colObj0->SetActivationState( WANTS_DEACTIVATION);
 					}
 				}
-
 			}
 
+			/// Process the actual simulation, only if not sleeping/deactivated
 			if (islandmanifold.size())
 			{
 				callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());
diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h
index 3896c19e161..d559f77bebe 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h
+++ b/extern/bullet/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);
diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp
index 1c057412962..fb905c31eb4 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp
+++ b/extern/bullet/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++;
 		}
 	}
-	
 }
 
 
diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h
index 611ee9616fc..dd0b4e779e7 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h
+++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h
@@ -18,14 +18,16 @@ subject to the following restrictions:
 
 
 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
 {
@@ -49,6 +51,7 @@ public:
 	virtual ~CollisionWorld();
 
 	virtual	void	UpdateActivationState();
+	virtual	void	StoreIslandActivationState();
 
 	BroadphaseInterface*	GetBroadphase()
 	{
@@ -153,6 +156,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();