more work on parallel dispatching of simulation islands

Bullet/BroadphaseCollision/Dispatcher.h

@@ -87,7 +87,7 @@ public:
 
 	virtual	void	ReleaseManifoldResult(ManifoldResult*)=0;
 
-	virtual void	DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo)=0;
+	virtual void	DispatchAllCollisionPairs(class BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo)=0;
 
 	virtual int GetNumManifolds() const = 0;
 

Bullet/CollisionDispatch/CollisionDispatcher.cpp

@@ -179,7 +179,7 @@ void	CollisionDispatcher::ReleaseManifoldResult(ManifoldResult*)
 }
 
 
-void	CollisionDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo)
+void	CollisionDispatcher::DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo)
 {
 	//m_blockedForChanges = true;
 
@@ -187,12 +187,12 @@ void	CollisionDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* pairCa
 
 	int dispatcherId = GetUniqueId();
 
-	pairCache->RefreshOverlappingPairs();
+	
 
-	for (i=0;i<pairCache->GetNumOverlappingPairs();i++)
+	for (i=0;i<numPairs;i++)
 	{
 
-		BroadphasePair& pair = pairCache->GetOverlappingPair(i);
+		BroadphasePair& pair = pairs[i];
 
 		if (dispatcherId>= 0)
 		{

Bullet/CollisionDispatch/CollisionDispatcher.h

@@ -103,7 +103,7 @@ public:
 
 	virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;}
 	
-	virtual void	DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo);
+	virtual void	DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo);
 
 	
 

Bullet/CollisionDispatch/CollisionWorld.cpp

@@ -104,7 +104,7 @@ void	CollisionWorld::PerformDiscreteCollisionDetection()
 
 	Dispatcher* dispatcher = GetDispatcher();
 	if (dispatcher)
-		dispatcher->DispatchAllCollisionPairs(m_pairCache,dispatchInfo);
+		dispatcher->DispatchAllCollisionPairs(&m_pairCache->GetOverlappingPair(0),m_pairCache->GetNumOverlappingPairs(),dispatchInfo);
 
 }
 

Bullet/CollisionDispatch/CollisionWorld.h

@@ -84,14 +84,14 @@ class CollisionWorld
 
 	std::vector<CollisionObject*>	m_collisionObjects;
 	
-	CollisionDispatcher*	m_dispatcher1;
+	Dispatcher*	m_dispatcher1;
 
 	OverlappingPairCache*	m_pairCache;
 	
 
 public:
 
-	CollisionWorld(CollisionDispatcher* dispatcher,OverlappingPairCache* pairCache)
+	CollisionWorld(Dispatcher* dispatcher,OverlappingPairCache* pairCache)
 		:m_dispatcher1(dispatcher),
 		m_pairCache(pairCache)
 	{

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -145,8 +145,8 @@ int main(int argc,char** argv)
 	SimdVector3 worldAabbMin(-30000,-30000,-30000);
 	SimdVector3 worldAabbMax(30000,30000,30000);
 
-	//BroadphaseInterface* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies,maxOverlap);
+	OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies,maxOverlap);
-	OverlappingPairCache* broadphase = new SimpleBroadphase(maxProxies,maxOverlap);
+	//OverlappingPairCache* broadphase = new SimpleBroadphase(maxProxies,maxOverlap);
 
 #ifdef USE_PARALLEL_DISPATCHER
 	physicsEnvironmentPtr = new ParallelPhysicsEnvironment(dispatcher2,broadphase);

Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp

@@ -321,7 +321,7 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV
 
 
 
-CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,OverlappingPairCache* pairCache)
+CcdPhysicsEnvironment::CcdPhysicsEnvironment(Dispatcher* dispatcher,OverlappingPairCache* pairCache)
 :m_scalingPropagated(false),
 m_numIterations(4),
 m_numTimeSubSteps(1),
@@ -676,7 +676,8 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 	dispatchInfo.m_stepCount = 0;
 	dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection;
 
-	GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo);
+	scene->RefreshOverlappingPairs();
+	GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
 
 
 #ifdef USE_QUICKPROF
@@ -844,7 +845,8 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 				dispatchInfo.m_stepCount = 0;
 				dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
 
-				GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo);
+				//pairCache->RefreshOverlappingPairs();//??
+				GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
 				
 				toi = dispatchInfo.m_timeOfImpact;
 

Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h

@@ -54,6 +54,8 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 	
 	//timestep subdivisions
 	int	m_numTimeSubSteps;
+protected:
+
 	int	m_ccdMode;
 	int	m_solverType;
 	int	m_profileTimings;
@@ -64,7 +66,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 	SimulationIslandManager*	m_islandManager;
 
 	public:
-		CcdPhysicsEnvironment(CollisionDispatcher* dispatcher=0, OverlappingPairCache* pairCache=0);
+		CcdPhysicsEnvironment(Dispatcher* dispatcher=0, OverlappingPairCache* pairCache=0);
 
 		virtual		~CcdPhysicsEnvironment();
 
@@ -200,7 +202,17 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 			return m_collisionWorld;
 		}
 
-	private:
+		SimulationIslandManager*	GetSimulationIslandManager()
+		{
+			return m_islandManager;
+		}
+
+		const SimulationIslandManager*	GetSimulationIslandManager() const 
+		{
+			return m_islandManager;
+		}
+
+	protected:
 		
 		
 
@@ -220,6 +232,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 
 		bool	m_scalingPropagated;
 
+		
 
 };
 

Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp

@@ -267,8 +267,69 @@ void	ParallelIslandDispatcher::ReleaseManifoldResult(ManifoldResult*)
 
 }
 
-void	ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo)
+
+void	ParallelIslandDispatcher::DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo)
 {
+	//m_blockedForChanges = true;
+
+	int i;
+
+	int dispatcherId = GetUniqueId();
+
+	
+
+	for (i=0;i<numPairs;i++)
+	{
+
+		BroadphasePair& pair = pairs[i];
+
+		if (dispatcherId>= 0)
+		{
+			//dispatcher will keep algorithms persistent in the collision pair
+			if (!pair.m_algorithms[dispatcherId])
+			{
+				pair.m_algorithms[dispatcherId] = FindAlgorithm(
+					*pair.m_pProxy0,
+					*pair.m_pProxy1);
+			}
+
+			if (pair.m_algorithms[dispatcherId])
+			{
+				if (dispatchInfo.m_dispatchFunc == 		DispatcherInfo::DISPATCH_DISCRETE)
+				{
+					pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+				} else
+				{
+					float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+					if (dispatchInfo.m_timeOfImpact > toi)
+						dispatchInfo.m_timeOfImpact = toi;
+
+				}
+			}
+		} else
+		{
+			//non-persistent algorithm dispatcher
+			CollisionAlgorithm* algo = FindAlgorithm(
+				*pair.m_pProxy0,
+				*pair.m_pProxy1);
+
+			if (algo)
+			{
+				if (dispatchInfo.m_dispatchFunc == 		DispatcherInfo::DISPATCH_DISCRETE)
+				{
+					algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+				} else
+				{
+					float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+					if (dispatchInfo.m_timeOfImpact > toi)
+						dispatchInfo.m_timeOfImpact = toi;
+				}
+			}
+		}
+
+	}
+
+	//m_blockedForChanges = false;
 
 }
 

Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h

@@ -118,8 +118,7 @@ public:
 
 	virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;}
 
-	virtual void	DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo);
+	virtual void	DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo);
-
 	
 	
 

Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp

@@ -20,13 +20,14 @@ subject to the following restrictions:
 #include "ParallelIslandDispatcher.h"
 #include "CollisionDispatch/CollisionWorld.h"
 #include "ConstraintSolver/TypedConstraint.h"
-
+#include "CollisionDispatch/SimulationIslandManager.h"
+#include "SimulationIsland.h"
 
 
 ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, OverlappingPairCache* pairCache):
-CcdPhysicsEnvironment(0,pairCache)
+CcdPhysicsEnvironment(dispatcher,pairCache)
 {
-
+	
 }
 
 ParallelPhysicsEnvironment::~ParallelPhysicsEnvironment()
@@ -39,13 +40,15 @@ ParallelPhysicsEnvironment::~ParallelPhysicsEnvironment()
 /// Perform an integration step of duration 'timeStep'.
 bool	ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 {
+	OverlappingPairCache*	scene = m_collisionWorld->GetPairCache();
+	scene->RefreshOverlappingPairs();
+
 
 #ifdef USE_QUICKPROF
-	Profiler::beginBlock("CalcSimulationIslands");
+	Profiler::beginBlock("IslandUnionFind");
 #endif //USE_QUICKPROF
 
-	/*
+	GetSimulationIslandManager()->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher());
-	GetCollisionWorld()->UpdateActivationState();
 
 	{
 		int i;
@@ -56,367 +59,100 @@ bool	ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 
 			const RigidBody* colObj0 = &constraint->GetRigidBodyA();
 			const RigidBody* colObj1 = &constraint->GetRigidBodyB();
-			
+
 			if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
-						((colObj1) && ((colObj1)->mergesSimulationIslands())))
+				((colObj1) && ((colObj1)->mergesSimulationIslands())))
 			{
 				if (colObj0->IsActive() || colObj1->IsActive())
 				{
-					GetDispatcher()->GetUnionFind().unite((colObj0)->m_islandTag1,
+
+					GetSimulationIslandManager()->GetUnionFind().unite((colObj0)->m_islandTag1,
 						(colObj1)->m_islandTag1);
 				}
 			}
 		}
 	}
 
-	GetCollisionWorld()->StoreIslandActivationState();
+	GetSimulationIslandManager()->StoreIslandActivationState(GetCollisionWorld());
-*/
 
 #ifdef USE_QUICKPROF
-	Profiler::endBlock("CalcSimulationIslands");
+	Profiler::endBlock("IslandUnionFind");
 #endif //USE_QUICKPROF
 
-
+	
-
+	///calculate simulation islands
-/*
+	
-	//printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
-
-	if (SimdFuzzyZero(timeStep))
-		return true;
-
-	if (m_debugDrawer)
-	{
-		gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation);
-	}
-
-
 #ifdef USE_QUICKPROF
-	Profiler::beginBlock("SyncMotionStates");
+	Profiler::beginBlock("BuildIslands");
 #endif //USE_QUICKPROF
 
+	std::vector<SimulationIsland> simulationIslands;
+	simulationIslands.resize(GetNumControllers());
 
-	//this is needed because scaling is not known in advance, and scaling has to propagate to the shape
+	int k;
-	if (!m_scalingPropagated)
+	for (k=0;k<GetNumControllers();k++)
 	{
-		SyncMotionStates(timeStep);
-		m_scalingPropagated = true;
-	}
-
-
-#ifdef USE_QUICKPROF
-	Profiler::endBlock("SyncMotionStates");
-
-	Profiler::beginBlock("predictIntegratedTransform");
-#endif //USE_QUICKPROF
-
-	{
-		//		std::vector<CcdPhysicsController*>::iterator i;
-
-
-
-		int k;
-		for (k=0;k<GetNumControllers();k++)
-		{
 			CcdPhysicsController* ctrl = m_controllers[k];
-			//		SimdTransform predictedTrans;
+			int tag = ctrl->GetRigidBody()->m_islandTag1;
-			RigidBody* body = ctrl->GetRigidBody();
+			if (tag>=0)
-			if (body->IsActive())
 			{
-				if (!body->IsStatic())
+				simulationIslands[tag].m_controllers.push_back(ctrl);
-				{
-					body->applyForces( timeStep);
-					body->integrateVelocities( timeStep);
-					body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
-				}
 			}
+	}
 
+	Dispatcher* dispatcher = GetCollisionWorld()->GetDispatcher();
+
+	
+	
+	int i;
+	for (int i=0;i<	scene->GetNumOverlappingPairs();i++)
+	{
+		BroadphasePair* pair = &scene->GetOverlappingPair(i);
+
+		CollisionObject*	col0 = static_cast<CollisionObject*>(pair->m_pProxy0->m_clientObject);
+		CollisionObject*	col1 = static_cast<CollisionObject*>(pair->m_pProxy1->m_clientObject);
+		
+		if (col0->m_islandTag1 > col1->m_islandTag1)
+		{
+			simulationIslands[col0->m_islandTag1].m_overlappingPairs.push_back(*pair);
+		} else
+		{
+			simulationIslands[col1->m_islandTag1].m_overlappingPairs.push_back(*pair);
 		}
 	}
 
-#ifdef USE_QUICKPROF
+	//add all overlapping pairs for each island
-	Profiler::endBlock("predictIntegratedTransform");
-#endif //USE_QUICKPROF
 
-	BroadphaseInterface*	scene = GetBroadphase();
+	for (i=0;i<dispatcher->GetNumManifolds();i++)
+	{
+		 PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
+		 
+		 //filtering for response
 
+		 CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
+		 CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
+		 {
+			 int islandTag = colObj0->m_islandTag1;
+			 if (colObj1->m_islandTag1 > islandTag)
+				 islandTag = colObj1->m_islandTag1;
 
-	//
+				if (dispatcher->NeedsResponse(*colObj0,*colObj1))
-	// collision detection (?)
+					simulationIslands[islandTag].m_manifolds.push_back(manifold);
-	//
+			
-
+		 }
-
+	}
-#ifdef USE_QUICKPROF
-	Profiler::beginBlock("DispatchAllCollisionPairs");
-#endif //USE_QUICKPROF
-
-
-	int numsubstep = m_numIterations;
-
-
-	DispatcherInfo dispatchInfo;
-	dispatchInfo.m_timeStep = timeStep;
-	dispatchInfo.m_stepCount = 0;
-	dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection;
-
-	scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g);
-
-
-#ifdef USE_QUICKPROF
-	Profiler::endBlock("DispatchAllCollisionPairs");
-#endif //USE_QUICKPROF
-
-
-	int numRigidBodies = m_controllers.size();
-
 	
 
 
-	//contacts
+	
-#ifdef USE_QUICKPROF
+	//Each simulation island can be processed in parallel
-	Profiler::beginBlock("SolveConstraint");
+	for (k=0;k<simulationIslands.size();k++)
-#endif //USE_QUICKPROF
-
-
-	//solve the regular constraints (point 2 point, hinge, etc)
-
-	for (int g=0;g<numsubstep;g++)
 	{
-		//
+		if (simulationIslands[k].m_controllers.size())
-		// constraint solving
-		//
-
-
-		int i;
-		int numConstraints = m_constraints.size();
-
-		//point to point constraints
-		for (i=0;i< numConstraints ; i++ )
 		{
-			TypedConstraint* constraint = m_constraints[i];
+			simulationIslands[k].Simulate(dispatcher,GetBroadphase(),m_solver,timeStep);
-
-			constraint->BuildJacobian();
-			constraint->SolveConstraint( timeStep );
-
 		}
-
-
 	}
-
-#ifdef USE_QUICKPROF
-	Profiler::endBlock("SolveConstraint");
-#endif //USE_QUICKPROF
-
-	//solve the vehicles
-
-#ifdef NEW_BULLET_VEHICLE_SUPPORT
-	//vehicles
-	int numVehicles = m_wrapperVehicles.size();
-	for (int i=0;i<numVehicles;i++)
-	{
-		WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
-		RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
-		vehicle->UpdateVehicle( timeStep);
-	}
-#endif //NEW_BULLET_VEHICLE_SUPPORT
-
-
-	struct InplaceSolverIslandCallback : public ParallelIslandDispatcher::IslandCallback
-	{
-
-		ContactSolverInfo& m_solverInfo;
-		ConstraintSolver*	m_solver;
-		IDebugDraw*	m_debugDrawer;
-
-		InplaceSolverIslandCallback(
-			ContactSolverInfo& solverInfo,
-			ConstraintSolver*	solver,
-			IDebugDraw*	debugDrawer)
-			:m_solverInfo(solverInfo),
-			m_solver(solver),
-			m_debugDrawer(debugDrawer)
-		{
-
-		}
-
-		virtual	void	ProcessIsland(PersistentManifold**	manifolds,int numManifolds)
-		{
-			m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
-		}
-
-	};
-
-
-	m_solverInfo.m_friction = 0.9f;
-	m_solverInfo.m_numIterations = m_numIterations;
-	m_solverInfo.m_timeStep = timeStep;
-	m_solverInfo.m_restitution = 0.f;//m_restitution;
-
-	InplaceSolverIslandCallback	solverCallback(
-		m_solverInfo,
-		m_solver,
-		m_debugDrawer);
-
-#ifdef USE_QUICKPROF
-	Profiler::beginBlock("BuildAndProcessIslands");
-#endif //USE_QUICKPROF
-
-	/// solve all the contact points and contact friction
-	GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
-
-#ifdef USE_QUICKPROF
-	Profiler::endBlock("BuildAndProcessIslands");
-
-	Profiler::beginBlock("CallbackTriggers");
-#endif //USE_QUICKPROF
-
-	CallbackTriggers();
-
-#ifdef USE_QUICKPROF
-	Profiler::endBlock("CallbackTriggers");
-
-
-	Profiler::beginBlock("proceedToTransform");
-
-#endif //USE_QUICKPROF
-	{
-
-
-
-		{
-
-			
-			
-			UpdateAabbs(timeStep);
-
-
-			float toi = 1.f;
-
-
-
-			if (m_ccdMode == 3)
-			{
-				DispatcherInfo dispatchInfo;
-				dispatchInfo.m_timeStep = timeStep;
-				dispatchInfo.m_stepCount = 0;
-				dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
-
-				scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
-				toi = dispatchInfo.m_timeOfImpact;
-
-			}
-
-			
-
-			//
-			// integrating solution
-			//
-
-			{
-				
-				std::vector<CcdPhysicsController*>::iterator i;
-
-				for (i=m_controllers.begin();
-					!(i==m_controllers.end()); i++)
-				{
-
-					CcdPhysicsController* ctrl = *i;
-
-					SimdTransform predictedTrans;
-					RigidBody* body = ctrl->GetRigidBody();
-					
-					if (body->IsActive())
-					{
-
-						if (!body->IsStatic())
-						{
-							body->predictIntegratedTransform(timeStep*	toi, predictedTrans);
-							body->proceedToTransform( predictedTrans);
-						}
-
-					}
-				}
-
-			}
-
-
-
-
-
-			//
-			// disable sleeping physics objects
-			//
-
-			std::vector<CcdPhysicsController*> m_sleepingControllers;
-
-			std::vector<CcdPhysicsController*>::iterator i;
-
-			for (i=m_controllers.begin();
-				!(i==m_controllers.end()); i++)
-			{
-				CcdPhysicsController* ctrl = (*i);
-				RigidBody* body = ctrl->GetRigidBody();
-
-				ctrl->UpdateDeactivation(timeStep);
-
-
-				if (ctrl->wantsSleeping())
-				{
-					if (body->GetActivationState() == ACTIVE_TAG)
-						body->SetActivationState( WANTS_DEACTIVATION );
-				} else
-				{
-					if (body->GetActivationState() != DISABLE_DEACTIVATION)
-						body->SetActivationState( ACTIVE_TAG );
-				}
-
-				if (useIslands)
-				{
-					if (body->GetActivationState() == ISLAND_SLEEPING)
-					{
-						m_sleepingControllers.push_back(ctrl);
-					}
-				} else
-				{
-					if (ctrl->wantsSleeping())
-					{
-						m_sleepingControllers.push_back(ctrl);
-					}
-				}
-			}
-
-
-
-
-		}
-
-
-#ifdef USE_QUICKPROF
-		Profiler::endBlock("proceedToTransform");
-
-		Profiler::beginBlock("SyncMotionStates");
-#endif //USE_QUICKPROF
-
-		SyncMotionStates(timeStep);
-
-#ifdef USE_QUICKPROF
-		Profiler::endBlock("SyncMotionStates");
-
-		Profiler::endProfilingCycle();
-#endif //USE_QUICKPROF
-
-
-#ifdef NEW_BULLET_VEHICLE_SUPPORT
-		//sync wheels for vehicles
-		int numVehicles = m_wrapperVehicles.size();
-		for (int i=0;i<numVehicles;i++)
-		{
-			WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
-
-			wrapperVehicle->SyncWheels();
-		}
-#endif //NEW_BULLET_VEHICLE_SUPPORT
-	}
-*/
-
 	return true;
-}
+
+}

Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp

@@ -14,4 +14,513 @@ subject to the following restrictions:
 */
 
 #include "SimulationIsland.h"
+#include "SimdTransform.h"
+#include "CcdPhysicsController.h"
+#include "BroadphaseCollision/OverlappingPairCache.h"
+#include "CollisionShapes/CollisionShape.h"
+#include "BroadphaseCollision/Dispatcher.h"
+#include "ConstraintSolver/ContactSolverInfo.h"
+#include "ConstraintSolver/ConstraintSolver.h"
 
+extern float gContactBreakingTreshold;
+
+bool	SimulationIsland::Simulate(Dispatcher* dispatcher,BroadphaseInterface* broadphase,class ConstraintSolver*	solver,float timeStep)
+{
+	//then execute all stuff below for each simulation island
+
+
+#ifdef USE_QUICKPROF
+	Profiler::endBlock("BuildIslands");
+#endif //USE_QUICKPROF
+
+
+
+
+	///build simulation islands, and add them to a job queue, which can be processed in parallel
+	///or on the GPU
+
+
+	//printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
+
+	if (SimdFuzzyZero(timeStep))
+		return true;
+
+	//	if (m_debugDrawer)
+	//	{
+	//		gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation);
+	//	}
+
+
+#ifdef USE_QUICKPROF
+	Profiler::beginBlock("SyncMotionStates");
+#endif //USE_QUICKPROF
+
+
+	//this is needed because scaling is not known in advance, and scaling has to propagate to the shape
+	//if (!m_scalingPropagated)
+	//{
+	//	SyncMotionStates(timeStep);
+	//	m_scalingPropagated = true;
+	//}
+
+
+#ifdef USE_QUICKPROF
+	Profiler::endBlock("SyncMotionStates");
+
+	Profiler::beginBlock("predictIntegratedTransform");
+#endif //USE_QUICKPROF
+
+	{
+		//		std::vector<CcdPhysicsController*>::iterator i;
+
+
+
+		int k;
+		for (k=0;k<GetNumControllers();k++)
+		{
+			CcdPhysicsController* ctrl = m_controllers[k];
+			//		SimdTransform predictedTrans;
+			RigidBody* body = ctrl->GetRigidBody();
+			if (body->IsActive())
+			{
+				if (!body->IsStatic())
+				{
+					body->applyForces( timeStep);
+					body->integrateVelocities( timeStep);
+					body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
+				}
+			}
+
+		}
+	}
+
+#ifdef USE_QUICKPROF
+	Profiler::endBlock("predictIntegratedTransform");
+#endif //USE_QUICKPROF
+	
+	//BroadphaseInterface*	scene = GetBroadphase();
+
+
+	//
+	// collision detection (?)
+	//
+
+
+	#ifdef USE_QUICKPROF
+	Profiler::beginBlock("DispatchAllCollisionPairs");
+	#endif //USE_QUICKPROF
+
+
+//	int numsubstep = m_numIterations;
+
+
+	DispatcherInfo dispatchInfo;
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_enableSatConvex = false;//m_enableSatCollisionDetection;
+
+	//pairCache->RefreshOverlappingPairs();
+	if (m_overlappingPairs.size())
+	{
+		dispatcher->DispatchAllCollisionPairs(&m_overlappingPairs[0],m_overlappingPairs.size(),dispatchInfo);///numsubstep,g);
+	}
+
+
+	#ifdef USE_QUICKPROF
+	Profiler::endBlock("DispatchAllCollisionPairs");
+	#endif //USE_QUICKPROF
+
+/*
+
+
+	int numRigidBodies = m_controllers.size();
+
+
+
+
+	//contacts
+	#ifdef USE_QUICKPROF
+	Profiler::beginBlock("SolveConstraint");
+	#endif //USE_QUICKPROF
+
+
+	//solve the regular constraints (point 2 point, hinge, etc)
+
+	for (int g=0;g<numsubstep;g++)
+	{
+	//
+	// constraint solving
+	//
+
+
+	int i;
+	int numConstraints = m_constraints.size();
+
+	//point to point constraints
+	for (i=0;i< numConstraints ; i++ )
+	{
+	TypedConstraint* constraint = m_constraints[i];
+
+	constraint->BuildJacobian();
+	constraint->SolveConstraint( timeStep );
+
+	}
+
+
+	}
+
+	#ifdef USE_QUICKPROF
+	Profiler::endBlock("SolveConstraint");
+	#endif //USE_QUICKPROF
+
+	//solve the vehicles
+
+	#ifdef NEW_BULLET_VEHICLE_SUPPORT
+	//vehicles
+	int numVehicles = m_wrapperVehicles.size();
+	for (int i=0;i<numVehicles;i++)
+	{
+	WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
+	RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
+	vehicle->UpdateVehicle( timeStep);
+	}
+	#endif //NEW_BULLET_VEHICLE_SUPPORT
+
+
+	struct InplaceSolverIslandCallback : public ParallelIslandDispatcher::IslandCallback
+	{
+
+	ContactSolverInfo& m_solverInfo;
+	ConstraintSolver*	m_solver;
+	IDebugDraw*	m_debugDrawer;
+
+	InplaceSolverIslandCallback(
+	ContactSolverInfo& solverInfo,
+	ConstraintSolver*	solver,
+	IDebugDraw*	debugDrawer)
+	:m_solverInfo(solverInfo),
+	m_solver(solver),
+	m_debugDrawer(debugDrawer)
+	{
+
+	}
+
+	virtual	void	ProcessIsland(PersistentManifold**	manifolds,int numManifolds)
+	{
+	m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
+	}
+
+	};
+
+
+	m_solverInfo.m_friction = 0.9f;
+	m_solverInfo.m_numIterations = m_numIterations;
+	m_solverInfo.m_timeStep = timeStep;
+	m_solverInfo.m_restitution = 0.f;//m_restitution;
+
+	InplaceSolverIslandCallback	solverCallback(
+	m_solverInfo,
+	m_solver,
+	m_debugDrawer);
+
+	#ifdef USE_QUICKPROF
+	Profiler::beginBlock("BuildAndProcessIslands");
+	#endif //USE_QUICKPROF
+
+	/// solve all the contact points and contact friction
+	GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
+
+	#ifdef USE_QUICKPROF
+	Profiler::endBlock("BuildAndProcessIslands");
+
+	Profiler::beginBlock("CallbackTriggers");
+	#endif //USE_QUICKPROF
+
+	CallbackTriggers();
+
+	#ifdef USE_QUICKPROF
+	Profiler::endBlock("CallbackTriggers");
+
+	}
+	*/
+
+	//OverlappingPairCache*	scene = GetCollisionWorld()->GetPairCache();
+	
+	ContactSolverInfo	solverInfo;
+
+	solverInfo.m_friction = 0.9f;
+	solverInfo.m_numIterations = 10;//m_numIterations;
+	solverInfo.m_timeStep = timeStep;
+	solverInfo.m_restitution = 0.f;//m_restitution;
+
+
+	if (m_manifolds.size())
+	{
+		solver->SolveGroup( &m_manifolds[0],m_manifolds.size(),solverInfo,0);
+	}
+
+
+#ifdef USE_QUICKPROF
+	Profiler::beginBlock("proceedToTransform");
+#endif //USE_QUICKPROF
+	{
+
+
+
+		{
+
+
+			UpdateAabbs(broadphase,timeStep);
+
+
+			float toi = 1.f;
+
+
+
+			/*		if (m_ccdMode == 3)
+			{
+			DispatcherInfo dispatchInfo;
+			dispatchInfo.m_timeStep = timeStep;
+			dispatchInfo.m_stepCount = 0;
+			dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
+
+			//			GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo);
+			toi = dispatchInfo.m_timeOfImpact;
+
+			}
+			*/
+
+
+
+			//
+			// integrating solution
+			//
+
+			{
+
+				std::vector<CcdPhysicsController*>::iterator i;
+
+				for (i=m_controllers.begin();
+					!(i==m_controllers.end()); i++)
+				{
+
+					CcdPhysicsController* ctrl = *i;
+
+					SimdTransform predictedTrans;
+					RigidBody* body = ctrl->GetRigidBody();
+
+					if (body->IsActive())
+					{
+
+						if (!body->IsStatic())
+						{
+							body->predictIntegratedTransform(timeStep*	toi, predictedTrans);
+							body->proceedToTransform( predictedTrans);
+						}
+
+					}
+				}
+
+			}
+
+
+
+
+
+			//
+			// disable sleeping physics objects
+			//
+
+			std::vector<CcdPhysicsController*> m_sleepingControllers;
+
+			std::vector<CcdPhysicsController*>::iterator i;
+
+			for (i=m_controllers.begin();
+				!(i==m_controllers.end()); i++)
+			{
+				CcdPhysicsController* ctrl = (*i);
+				RigidBody* body = ctrl->GetRigidBody();
+
+				ctrl->UpdateDeactivation(timeStep);
+
+
+				if (ctrl->wantsSleeping())
+				{
+					if (body->GetActivationState() == ACTIVE_TAG)
+						body->SetActivationState( WANTS_DEACTIVATION );
+				} else
+				{
+					if (body->GetActivationState() != DISABLE_DEACTIVATION)
+						body->SetActivationState( ACTIVE_TAG );
+				}
+
+				if (true)
+				{
+					if (body->GetActivationState() == ISLAND_SLEEPING)
+					{
+						m_sleepingControllers.push_back(ctrl);
+					}
+				} else
+				{
+					if (ctrl->wantsSleeping())
+					{
+						m_sleepingControllers.push_back(ctrl);
+					}
+				}
+			}
+
+
+
+
+		}
+
+
+#ifdef USE_QUICKPROF
+		Profiler::endBlock("proceedToTransform");
+
+		Profiler::beginBlock("SyncMotionStates");
+#endif //USE_QUICKPROF
+
+		SyncMotionStates(timeStep);
+
+#ifdef USE_QUICKPROF
+		Profiler::endBlock("SyncMotionStates");
+
+		Profiler::endProfilingCycle();
+#endif //USE_QUICKPROF
+
+
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
+		//sync wheels for vehicles
+		int numVehicles = m_wrapperVehicles.size();
+		for (int i=0;i<numVehicles;i++)
+		{
+			WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
+
+			wrapperVehicle->SyncWheels();
+		}
+#endif //NEW_BULLET_VEHICLE_SUPPORT
+
+		return true;
+	}
+}
+
+
+
+void	SimulationIsland::SyncMotionStates(float timeStep)
+{
+	std::vector<CcdPhysicsController*>::iterator i;
+
+	//
+	// synchronize the physics and graphics transformations
+	//
+
+	for (i=m_controllers.begin();
+		!(i==m_controllers.end()); i++)
+	{
+		CcdPhysicsController* ctrl = (*i);
+		ctrl->SynchronizeMotionStates(timeStep);
+
+	}
+
+}
+
+
+
+void	SimulationIsland::UpdateAabbs(BroadphaseInterface* scene,float	timeStep)
+{
+	std::vector<CcdPhysicsController*>::iterator i;
+
+
+	//
+			// update aabbs, only for moving objects (!)
+			//
+			for (i=m_controllers.begin();
+				!(i==m_controllers.end()); i++)
+			{
+				CcdPhysicsController* ctrl = (*i);
+				RigidBody* body = ctrl->GetRigidBody();
+
+
+				SimdPoint3 minAabb,maxAabb;
+				CollisionShape* shapeinterface = ctrl->GetCollisionShape();
+
+
+
+				shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
+					body->getLinearVelocity(),
+					//body->getAngularVelocity(),
+					SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
+					timeStep,minAabb,maxAabb);
+
+
+				SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
+				minAabb -= manifoldExtraExtents;
+				maxAabb += manifoldExtraExtents;
+
+				BroadphaseProxy* bp = body->m_broadphaseHandle;
+				if (bp)
+				{
+
+					SimdVector3 color (1,1,0);
+
+					class IDebugDraw*	m_debugDrawer = 0;
+/*
+					if (m_debugDrawer)
+					{	
+						//draw aabb
+						switch (body->GetActivationState())
+						{
+						case ISLAND_SLEEPING:
+							{
+								color.setValue(1,1,1);
+								break;
+							}
+						case WANTS_DEACTIVATION:
+							{
+								color.setValue(0,0,1);
+								break;
+							}
+						case ACTIVE_TAG:
+							{
+								break;
+							}
+						case DISABLE_DEACTIVATION:
+							{
+								color.setValue(1,0,1);
+							};
+
+						};
+
+						if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
+						{
+							DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
+						}
+					}
+*/
+
+			
+					if ( (maxAabb-minAabb).length2() < 1e12f)
+					{
+						scene->SetAabb(bp,minAabb,maxAabb);
+					} else
+					{
+						//something went wrong, investigate
+						//removeCcdPhysicsController(ctrl);
+						body->SetActivationState(DISABLE_SIMULATION);
+
+						static bool reportMe = true;
+						if (reportMe)
+						{
+							reportMe = false;
+							printf("Overflow in AABB, object removed from simulation \n");
+							printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
+							printf("Please include above information, your Platform, version of OS.\n");
+							printf("Thanks.\n");
+						}
+						
+					}
+
+				}
+			}
+}

Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h

@@ -16,14 +16,35 @@ subject to the following restrictions:
 #ifndef SIMULATION_ISLAND_H
 #define SIMULATION_ISLAND_H
 
+#include <vector>
+class BroadphaseInterface;
+class Dispatcher;
+
 ///SimulationIsland groups all computations and data (for collision detection and dynamics) that can execute in parallel with other SimulationIsland's
 ///The ParallelPhysicsEnvironment and ParallelIslandDispatcher will dispatch SimulationIsland's
 ///At the start of the simulation timestep the simulation islands are re-calculated
 ///During one timestep there is no merging or splitting of Simulation Islands
 class SimulationIsland
 {
+	
 	public:
-		
+	std::vector<class CcdPhysicsController*> m_controllers;
+	std::vector<class PersistentManifold*> m_manifolds;
+	std::vector<class BroadphasePair> m_overlappingPairs;
+	
+	bool	Simulate(Dispatcher* dispatcher,BroadphaseInterface* broadphase,	class ConstraintSolver*	solver, float timeStep);
+	
+	
+	int	GetNumControllers()
+	{
+		return m_controllers.size();
+	}
+
+	
+	
+
+	void	SyncMotionStates(float timeStep);
+	void	UpdateAabbs(BroadphaseInterface* broadphase,float timeStep);
 };
 
 #endif //SIMULATION_ISLAND_H