upgraded Bullet rigidbody physics to latest version 1.9

6 files changed, 295 insertions, 159 deletions

diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
index 60b8dbbf862..83bddc8ee1e 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
@@ -34,7 +34,7 @@ class BP_Proxy;
 float	gDeactivationTime = 2.f;
 bool	gDisableDeactivation = false;
 
-float gLinearSleepingTreshold = 0.4f;
+float gLinearSleepingTreshold = 0.8f;
 float gAngularSleepingTreshold = 1.0f;
 
 #include "Dynamics/MassProps.h"
@@ -210,28 +210,6 @@ void		CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionsta
 
 }
 
-void CcdPhysicsController::SetMargin(float margin)
-{
-	if (m_body && m_body->GetCollisionShape())
-	{
-		m_body->GetCollisionShape()->SetMargin(margin);
-	}
-
-
-}
-
-float CcdPhysicsController::GetMargin() const
-{
-	if (m_body && m_body->GetCollisionShape())
-	{
-		return m_body->GetCollisionShape()->GetMargin();
-	}
-
-	return 0.f;
-
-}
-
-
 		// kinematic methods
 void		CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
 {
@@ -557,6 +535,7 @@ PHY_IPhysicsController*	CcdPhysicsController::GetReplica()
 DefaultMotionState::DefaultMotionState()
 {
 	m_worldTransform.setIdentity();
+	m_localScaling.setValue(1.f,1.f,1.f);
 }
 
 
@@ -574,9 +553,9 @@ void	DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
 
 void	DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
 {
-	scaleX = 1.;
-	scaleY = 1.;
-	scaleZ = 1.;
+	scaleX = m_localScaling.getX();
+	scaleY = m_localScaling.getY();
+	scaleZ = m_localScaling.getZ();
 }
 
 void	DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.h b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
index 7488e74db01..632d5d776d2 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
@@ -39,20 +39,38 @@ extern bool gDisableDeactivation;
 class CcdPhysicsEnvironment;
 
 
+
+
 struct CcdConstructionInfo
 {
+
+	///CollisionFilterGroups provides some optional usage of basic collision filtering
+	///this is done during broadphase, so very early in the pipeline
+	///more advanced collision filtering should be done in CollisionDispatcher::NeedsCollision
+	enum CollisionFilterGroups
+	{
+	        DefaultFilter = 1,
+	        StaticFilter = 2,
+	        KinematicFilter = 4,
+	        DebrisFilter = 8,
+	        AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter,
+	};
+
+
 	CcdConstructionInfo()
 		: m_gravity(0,0,0),
+		m_scaling(1.f,1.f,1.f),
 		m_mass(0.f),
 		m_restitution(0.1f),
 		m_friction(0.5f),
 		m_linearDamping(0.1f),
 		m_angularDamping(0.1f),
+		m_collisionFlags(0),
+		m_collisionFilterGroup(DefaultFilter),
+		m_collisionFilterMask(AllFilter),
 		m_MotionState(0),
 		m_physicsEnv(0),
-		m_inertiaFactor(1.f),
-		m_scaling(1.f,1.f,1.f),
-		m_collisionFlags(0)
+		m_inertiaFactor(1.f)
 	{
 	}
 
@@ -66,6 +84,15 @@ struct CcdConstructionInfo
 	SimdScalar	m_angularDamping;
 	int			m_collisionFlags;
 
+	///optional use of collision group/mask:
+	///only collision with object goups that match the collision mask.
+	///this is very basic early out. advanced collision filtering should be
+	///done in the CollisionDispatcher::NeedsCollision and NeedsResponse
+	///both values default to 1
+	short int	m_collisionFilterGroup;
+	short int	m_collisionFilterMask;
+
+
 	CollisionShape*			m_collisionShape;
 	class	PHY_IMotionState*			m_MotionState;
 
@@ -158,10 +185,21 @@ class CcdPhysicsController : public PHY_IPhysicsController
 		virtual	void				setNewClientInfo(void* clientinfo);
 		virtual PHY_IPhysicsController*	GetReplica();
 		
+		///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors
+		short int	GetCollisionFilterGroup() const
+		{
+			return m_cci.m_collisionFilterGroup;
+		}
+		///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors
+		short int	GetCollisionFilterMask() const
+		{
+			return m_cci.m_collisionFilterMask;
+		}
+
 
 		virtual void	calcXform() {} ;
-		virtual void SetMargin(float margin);
-		virtual float GetMargin() const;
+		virtual void SetMargin(float margin) {};
+		virtual float GetMargin() const {return 0.f;};
 
 
 		bool	wantsSleeping();
@@ -208,6 +246,7 @@ class	DefaultMotionState : public PHY_IMotionState
 		virtual	void	calculateWorldTransformations();
 		
 		SimdTransform	m_worldTransform;
+		SimdVector3		m_localScaling;
 
 };
 
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
index 2fee6d06963..813462fdf5e 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
@@ -30,14 +30,12 @@ subject to the following restrictions:
 
 #include "CollisionShapes/ConvexShape.h"
 #include "CollisionShapes/ConeShape.h"
-
-
+#include "CollisionDispatch/SimulationIslandManager.h"
 
 #include "BroadphaseCollision/Dispatcher.h"
 #include "NarrowPhaseCollision/PersistentManifold.h"
 #include "CollisionShapes/TriangleMeshShape.h"
-#include "ConstraintSolver/OdeConstraintSolver.h"
-#include "ConstraintSolver/SimpleConstraintSolver.h"
+#include "ConstraintSolver/SequentialImpulseConstraintSolver.h"
 
 
 //profiling/timings
@@ -56,7 +54,7 @@ subject to the following restrictions:
 #include "PHY_IMotionState.h"
 
 #include "CollisionDispatch/EmptyCollisionAlgorithm.h"
-#include "CollisionDispatch/UnionFind.h"
+
 
 
 #include "CollisionShapes/SphereShape.h"
@@ -77,6 +75,7 @@ RaycastVehicle::VehicleTuning	gTuning;
 #include "ConstraintSolver/ConstraintSolver.h"
 #include "ConstraintSolver/Point2PointConstraint.h"
 #include "ConstraintSolver/HingeConstraint.h"
+#include "ConstraintSolver/Generic6DofConstraint.h"
 
 
 //#include "BroadphaseCollision/QueryDispatcher.h"
@@ -322,7 +321,7 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV
 
 
 
-CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
+CcdPhysicsEnvironment::CcdPhysicsEnvironment(Dispatcher* dispatcher,OverlappingPairCache* pairCache)
 :m_scalingPropagated(false),
 m_numIterations(10),
 m_numTimeSubSteps(1),
@@ -336,18 +335,19 @@ m_enableSatCollisionDetection(false)
 	{
 		m_triggerCallbacks[i] = 0;
 	}
+
 	if (!dispatcher)
 		dispatcher = new CollisionDispatcher();
 
 
-	if(!broadphase)
+	if(!pairCache)
 	{
 
 		//todo: calculate/let user specify this world sizes
 		SimdVector3 worldMin(-10000,-10000,-10000);
 		SimdVector3 worldMax(10000,10000,10000);
 
-		broadphase = new AxisSweep3(worldMin,worldMax);
+		pairCache = new AxisSweep3(worldMin,worldMax);
 
 		//broadphase = new SimpleBroadphase();
 	}
@@ -355,11 +355,13 @@ m_enableSatCollisionDetection(false)
 
 	setSolverType(1);//issues with quickstep and memory allocations
 
-	m_collisionWorld = new CollisionWorld(dispatcher,broadphase);
+	m_collisionWorld = new CollisionWorld(dispatcher,pairCache);
 
 	m_debugDrawer = 0;
 	m_gravity = SimdVector3(0.f,-10.f,0.f);
 
+	m_islandManager = new SimulationIslandManager();
+
 
 }
 
@@ -373,7 +375,7 @@ void	CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
 	body->setGravity( m_gravity );
 	m_controllers.push_back(ctrl);
 
-	m_collisionWorld->AddCollisionObject(body);
+	m_collisionWorld->AddCollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
 
 	assert(body->m_broadphaseHandle);
 
@@ -385,7 +387,8 @@ void	CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
 	assert(shapeinterface);
 
 	const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
-
+	
+	body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
 
 	SimdPoint3 minAabb,maxAabb;
 
@@ -637,6 +640,9 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 			CcdPhysicsController* ctrl = m_controllers[k];
 			//		SimdTransform predictedTrans;
 			RigidBody* body = ctrl->GetRigidBody();
+			
+			body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
+
 			if (body->IsActive())
 			{
 				if (!body->IsStatic())
@@ -654,7 +660,7 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 	Profiler::endBlock("predictIntegratedTransform");
 #endif //USE_QUICKPROF
 
-	BroadphaseInterface*	scene = GetBroadphase();
+	OverlappingPairCache*	scene = m_collisionWorld->GetPairCache();
 
 
 	//
@@ -674,8 +680,10 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 	dispatchInfo.m_timeStep = timeStep;
 	dispatchInfo.m_stepCount = 0;
 	dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection;
+	dispatchInfo.m_debugDraw = this->m_debugDrawer;
 
-	scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g);
+	scene->RefreshOverlappingPairs();
+	GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
 
 
 #ifdef USE_QUICKPROF
@@ -685,7 +693,8 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 
 	int numRigidBodies = m_controllers.size();
 
-	m_collisionWorld->UpdateActivationState();
+	
+	m_islandManager->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher());
 
 	{
 		int i;
@@ -702,15 +711,15 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 			{
 				if (colObj0->IsActive() || colObj1->IsActive())
 				{
-					GetDispatcher()->GetUnionFind().unite((colObj0)->m_islandTag1,
+
+					m_islandManager->GetUnionFind().unite((colObj0)->m_islandTag1,
 						(colObj1)->m_islandTag1);
 				}
 			}
 		}
 	}
 
-	m_collisionWorld->StoreIslandActivationState();
-
+	m_islandManager->StoreIslandActivationState(GetCollisionWorld());
 
 
 	//contacts
@@ -762,7 +771,7 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 #endif //NEW_BULLET_VEHICLE_SUPPORT
 
 
-	struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
+	struct InplaceSolverIslandCallback : public SimulationIslandManager::IslandCallback
 	{
 
 		ContactSolverInfo& m_solverInfo;
@@ -803,7 +812,7 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 #endif //USE_QUICKPROF
 
 	/// solve all the contact points and contact friction
-	GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
+	m_islandManager->BuildAndProcessIslands(GetCollisionWorld()->GetDispatcher(),m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
 
 #ifdef USE_QUICKPROF
 	Profiler::endBlock("BuildAndProcessIslands");
@@ -842,7 +851,9 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 				dispatchInfo.m_stepCount = 0;
 				dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
 
-				scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
+				//pairCache->RefreshOverlappingPairs();//??
+				GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
+				
 				toi = dispatchInfo.m_timeOfImpact;
 
 			}
@@ -871,6 +882,11 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 
 						if (!body->IsStatic())
 						{
+							if (body->m_hitFraction < 1.f)
+							{
+								//set velocity to zero... until we have proper CCD integrated
+								body->setLinearVelocity(body->getLinearVelocity()*0.5f);
+							}
 							body->predictIntegratedTransform(timeStep*	toi, predictedTrans);
 							body->proceedToTransform( predictedTrans);
 						}
@@ -1033,7 +1049,7 @@ void		CcdPhysicsEnvironment::setSolverType(int solverType)
 			if (m_solverType != solverType)
 			{
 
-				m_solver = new SimpleConstraintSolver();
+				m_solver = new SequentialImpulseConstraintSolver();
 
 				break;
 			}
@@ -1043,7 +1059,7 @@ void		CcdPhysicsEnvironment::setSolverType(int solverType)
 	default:
 		if (m_solverType != solverType)
 		{
-			m_solver = new OdeConstraintSolver();
+//			m_solver = new OdeConstraintSolver();
 
 			break;
 		}
@@ -1182,8 +1198,8 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 	SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
 	SimdVector3 axisInA(axisX,axisY,axisZ);
 	SimdVector3 axisInB = rb1 ? 
-		(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : 
-	rb0->getCenterOfMassTransform().getBasis() * -axisInA;
+		(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
+	rb0->getCenterOfMassTransform().getBasis() * axisInA;
 
 	bool angularOnly = false;
 
@@ -1213,9 +1229,55 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 			break;
 		}
 
+	case PHY_GENERIC_6DOF_CONSTRAINT:
+		{
+			Generic6DofConstraint* genericConstraint = 0;
+
+			if (rb1)
+			{
+				SimdTransform frameInA;
+				SimdTransform frameInB;
+				
+				SimdVector3 axis1, axis2;
+				SimdPlaneSpace1( axisInA, axis1, axis2 );
+
+				frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
+					                          axisInA.y(), axis1.y(), axis2.y(),
+											  axisInA.z(), axis1.z(), axis2.z() );
+
+	
+				SimdPlaneSpace1( axisInB, axis1, axis2 );
+				frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
+					                          axisInB.y(), axis1.y(), axis2.y(),
+											  axisInB.z(), axis1.z(), axis2.z() );
+
+				frameInA.setOrigin( pivotInA );
+				frameInB.setOrigin( pivotInB );
+
+				genericConstraint = new Generic6DofConstraint(
+					*rb0,*rb1,
+					frameInA,frameInB);
+
+
+			} else
+			{
+				// TODO: Implement single body case...
+
+			}
+			
+
+			m_constraints.push_back(genericConstraint);
+			genericConstraint->SetUserConstraintId(gConstraintUid++);
+			genericConstraint->SetUserConstraintType(type);
+			//64 bit systems can't cast pointer to int. could use size_t instead.
+			return genericConstraint->GetUserConstraintId();
+
+			break;
+		}
 	case PHY_ANGULAR_CONSTRAINT:
 		angularOnly = true;
 
+
 	case PHY_LINEHINGE_CONSTRAINT:
 		{
 			HingeConstraint* hinge = 0;
@@ -1271,21 +1333,62 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 
 }
 
-float		CcdPhysicsEnvironment::getAppliedImpulse(int	constraintid)
+
+
+
+//Following the COLLADA physics specification for constraints
+int			CcdPhysicsEnvironment::createUniversalD6Constraint(
+						class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
+						SimdTransform& frameInA,
+						SimdTransform& frameInB,
+						const SimdVector3& linearMinLimits,
+						const SimdVector3& linearMaxLimits,
+						const SimdVector3& angularMinLimits,
+						const SimdVector3& angularMaxLimits
+)
 {
-	std::vector<TypedConstraint*>::iterator i;
 
-	for (i=m_constraints.begin();
-		!(i==m_constraints.end()); i++)
+	//we could either add some logic to recognize ball-socket and hinge, or let that up to the user
+	//perhaps some warning or hint that hinge/ball-socket is more efficient?
+	
+	Generic6DofConstraint* genericConstraint = 0;
+	CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
+	CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
+	
+	RigidBody* rb0 = ctrl0->GetRigidBody();
+	RigidBody* rb1 = ctrl1->GetRigidBody();
+
+	if (rb1)
 	{
-		TypedConstraint* constraint = (*i);
-		if (constraint->GetUserConstraintId() == constraintid)
-		{
-			return constraint->GetAppliedImpulse();
-		}
+		
+
+		genericConstraint = new Generic6DofConstraint(
+			*rb0,*rb1,
+			frameInA,frameInB);
+		genericConstraint->setLinearLowerLimit(linearMinLimits);
+		genericConstraint->setLinearUpperLimit(linearMaxLimits);
+		genericConstraint->setAngularLowerLimit(angularMinLimits);
+		genericConstraint->setAngularUpperLimit(angularMaxLimits);
+	} else
+	{
+		// TODO: Implement single body case...
+		//No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
+
 	}
-	return 0.f;
+	
+	if (genericConstraint)
+	{
+		m_constraints.push_back(genericConstraint);
+		genericConstraint->SetUserConstraintId(gConstraintUid++);
+		genericConstraint->SetUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
+		//64 bit systems can't cast pointer to int. could use size_t instead.
+		return genericConstraint->GetUserConstraintId();
+	}
+	return 0;
 }
+
+
+
 void		CcdPhysicsEnvironment::removeConstraint(int	constraintId)
 {
 	std::vector<TypedConstraint*>::iterator i;
@@ -1296,15 +1399,7 @@ void		CcdPhysicsEnvironment::removeConstraint(int	constraintId)
 		TypedConstraint* constraint = (*i);
 		if (constraint->GetUserConstraintId() == constraintId)
 		{
-			//activate objects
-			if (constraint->GetRigidBodyA().mergesSimulationIslands())
-				constraint->GetRigidBodyA().activate();
-			if (constraint->GetRigidBodyB().mergesSimulationIslands())
-				constraint->GetRigidBodyB().activate();
-
 			std::swap(*i, m_constraints.back());
-
-			
 			m_constraints.pop_back();
 			break;
 		}
@@ -1352,13 +1447,6 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 	SimdVector3 rayFrom(fromX,fromY,fromZ);
 	SimdVector3 rayTo(toX,toY,toZ);
 
-	
-	if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)	
-	{
-		SimdVector3 color (1,0,0);
-		m_debugDrawer->DrawLine(rayFrom,rayTo,color);
-	}
-
 	SimdVector3	hitPointWorld,normalWorld;
 
 	//Either Ray Cast with or without filtering
@@ -1376,26 +1464,10 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 		hitX = 	rayCallback.m_hitPointWorld.getX();
 		hitY = 	rayCallback.m_hitPointWorld.getY();
 		hitZ = 	rayCallback.m_hitPointWorld.getZ();
-		if (rayCallback.m_hitNormalWorld.length2() > SIMD_EPSILON)
-		{
-			rayCallback.m_hitNormalWorld.normalize();
-		}
 
 		normalX = rayCallback.m_hitNormalWorld.getX();
 		normalY = rayCallback.m_hitNormalWorld.getY();
 		normalZ = rayCallback.m_hitNormalWorld.getZ();
-		
-		if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)	
-		{
-			SimdVector3 colorNormal(0,0,1);
-			m_debugDrawer->DrawLine(rayCallback.m_hitPointWorld,rayCallback.m_hitPointWorld+rayCallback.m_hitNormalWorld,colorNormal);
-		
-			SimdVector3 color (0,1,0);
-			m_debugDrawer->DrawLine(rayFrom,rayCallback.m_hitPointWorld,color);
-			
-		
-		}
-
 
 	}	
 
@@ -1425,15 +1497,6 @@ BroadphaseInterface*	CcdPhysicsEnvironment::GetBroadphase()
 
 
 
-const CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() const
-{
-	return m_collisionWorld->GetDispatcher();
-}
-
-CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher()
-{
-	return m_collisionWorld->GetDispatcher();
-}
 
 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
 {
@@ -1448,6 +1511,8 @@ CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
 	//first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
 	//delete m_dispatcher;
 	delete m_collisionWorld;
+	
+	delete m_islandManager;
 
 }
 
@@ -1464,15 +1529,9 @@ CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
 }
 
 
-int	CcdPhysicsEnvironment::GetNumManifolds() const
-{
-	return GetDispatcher()->GetNumManifolds();
-}
 
-const PersistentManifold*	CcdPhysicsEnvironment::GetManifold(int index) const
-{
-	return GetDispatcher()->GetManifoldByIndexInternal(index);
-}
+
+
 
 TypedConstraint*	CcdPhysicsEnvironment::getConstraintById(int constraintId)
 {
@@ -1565,9 +1624,10 @@ void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctr
 
 void	CcdPhysicsEnvironment::CallbackTriggers()
 {
+	
 	CcdPhysicsController* ctrl0=0,*ctrl1=0;
 
-	if (m_triggerCallbacks[PHY_OBJECT_RESPONSE])
+	if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints)))
 	{
 		//walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
 		int numManifolds = m_collisionWorld->GetDispatcher()->GetNumManifolds();
@@ -1577,6 +1637,16 @@ void	CcdPhysicsEnvironment::CallbackTriggers()
 			int numContacts = manifold->GetNumContacts();
 			if (numContacts)
 			{
+				if (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints))
+				{
+					for (int j=0;j<numContacts;j++)
+					{
+						SimdVector3 color(1,0,0);
+						const ManifoldPoint& cp = manifold->GetContactPoint(j);
+						if (m_debugDrawer)
+							m_debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color);
+					}
+				}
 				RigidBody* obj0 = static_cast<RigidBody* >(manifold->GetBody0());
 				RigidBody* obj1 = static_cast<RigidBody* >(manifold->GetBody1());
 
@@ -1603,6 +1673,7 @@ void	CcdPhysicsEnvironment::CallbackTriggers()
 
 	}
 
+
 }
 
 
@@ -1764,3 +1835,18 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float conera
 	return sphereController;
 }
 	
+float		CcdPhysicsEnvironment::getAppliedImpulse(int	constraintid)
+{
+	std::vector<TypedConstraint*>::iterator i;
+
+	for (i=m_constraints.begin();
+		!(i==m_constraints.end()); i++)
+	{
+		TypedConstraint* constraint = (*i);
+		if (constraint->GetUserConstraintId() == constraintid)
+		{
+			return constraint->GetAppliedImpulse();
+		}
+	}
+	return 0.f;
+}
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
index 73bd9f8c95e..a1f075e3882 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
@@ -20,11 +20,13 @@ subject to the following restrictions:
 #include <vector>
 class CcdPhysicsController;
 #include "SimdVector3.h"
+#include "SimdTransform.h"
 
 
 
-class TypedConstraint;
 
+class TypedConstraint;
+class SimulationIslandManager;
 class CollisionDispatcher;
 class Dispatcher;
 //#include "BroadphaseInterface.h"
@@ -37,6 +39,7 @@ class Dispatcher;
 class WrapperVehicle;
 class PersistentManifold;
 class BroadphaseInterface;
+class OverlappingPairCache;
 class IDebugDraw;
 
 /// CcdPhysicsEnvironment is experimental mainloop for physics simulation using optional continuous collision detection.
@@ -47,12 +50,17 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 {
 	SimdVector3 m_gravity;
 	
+	
+
+protected:
 	IDebugDraw*	m_debugDrawer;
 	//solver iterations
 	int	m_numIterations;
 	
 	//timestep subdivisions
 	int	m_numTimeSubSteps;
+
+
 	int	m_ccdMode;
 	int	m_solverType;
 	int	m_profileTimings;
@@ -60,9 +68,10 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 
 	ContactSolverInfo	m_solverInfo;
 	
+	SimulationIslandManager*	m_islandManager;
 
 	public:
-		CcdPhysicsEnvironment(CollisionDispatcher* dispatcher=0, BroadphaseInterface* broadphase=0);
+		CcdPhysicsEnvironment(Dispatcher* dispatcher=0, OverlappingPairCache* pairCache=0);
 
 		virtual		~CcdPhysicsEnvironment();
 
@@ -98,7 +107,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 		virtual void		endFrame() {};
 		/// Perform an integration step of duration 'timeStep'.
 		virtual	bool		proceedDeltaTime(double curTime,float timeStep);
-		bool				proceedDeltaTimeOneStep(float timeStep);
+		virtual bool		proceedDeltaTimeOneStep(float timeStep);
 
 		virtual	void		setFixedTimeStep(bool useFixedTimeStep,float fixedTimeStep){};
 		//returns 0.f if no fixed timestep is used
@@ -112,9 +121,24 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 		virtual int			createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
 			float pivotX,float pivotY,float pivotZ,
 			float axisX,float axisY,float axisZ);
+
+
+		//Following the COLLADA physics specification for constraints
+		virtual int			createUniversalD6Constraint(
+		class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
+			SimdTransform& localAttachmentFrameRef,
+			SimdTransform& localAttachmentOther,
+			const SimdVector3& linearMinLimits,
+			const SimdVector3& linearMaxLimits,
+			const SimdVector3& angularMinLimits,
+			const SimdVector3& angularMaxLimits
+			);
+
+
 	    virtual void		removeConstraint(int	constraintid);
-		virtual float		getAppliedImpulse(int	constraintid);
 
+		
+		virtual float		getAppliedImpulse(int	constraintid);
 
 		virtual void	CallbackTriggers();
 
@@ -160,9 +184,9 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 
 		BroadphaseInterface*	GetBroadphase();
 
-		CollisionDispatcher* GetDispatcher();
 		
-		const CollisionDispatcher* GetDispatcher() const;
+		
+		
 
 		bool	IsSatCollisionDetectionEnabled() const
 		{
@@ -180,16 +204,39 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 
 		CcdPhysicsController* GetPhysicsController( int index);
 
-		int	GetNumManifolds() const;
+		
 
 		const PersistentManifold*	GetManifold(int index) const;
 
 		std::vector<TypedConstraint*> m_constraints;
 
-	private:
+		void	SyncMotionStates(float timeStep);
+
 		
+		class CollisionWorld*	GetCollisionWorld()
+		{
+			return m_collisionWorld;
+		}
+
+		const class CollisionWorld*	GetCollisionWorld() const
+		{
+			return m_collisionWorld;
+		}
+
+		SimulationIslandManager*	GetSimulationIslandManager()
+		{
+			return m_islandManager;
+		}
+
+		const SimulationIslandManager*	GetSimulationIslandManager() const 
+		{
+			return m_islandManager;
+		}
+
+	protected:
 		
-		void	SyncMotionStates(float timeStep);
+		
+
 		
 		std::vector<CcdPhysicsController*> m_controllers;
 		
@@ -206,6 +253,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 
 		bool	m_scalingPropagated;
 
+		
 
 };
 
diff --git a/source/gameengine/Physics/common/PHY_DynamicTypes.h b/source/gameengine/Physics/common/PHY_DynamicTypes.h
index c9ca3221355..3952377181a 100644
--- a/source/gameengine/Physics/common/PHY_DynamicTypes.h
+++ b/source/gameengine/Physics/common/PHY_DynamicTypes.h
@@ -1,38 +1,24 @@
-/**
- * $Id$
- *
- * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * as published by the Free Software Foundation; either version 2
- * of the License, or (at your option) any later version. The Blender
- * Foundation also sells licenses for use in proprietary software under
- * the Blender License.  See http://www.blender.org/BL/ for information
- * about this.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
- *
- * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
- * All rights reserved.
- *
- * The Original Code is: all of this file.
- *
- * Contributor(s): none yet.
- *
- * ***** END GPL/BL DUAL LICENSE BLOCK *****
- */
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
 #ifndef __PHY_DYNAMIC_TYPES
 #define __PHY_DYNAMIC_TYPES
 
 
+
 class	PHY_ResponseTable;
 
 class PHY_Shape;
@@ -68,11 +54,6 @@ typedef enum
 		PHY__Vector3 m_normal;               /* point2 - point1 */ 
 	} PHY_CollData;
 
-/* A response callback is called by SOLID for each pair of collding objects. 'client-data'
-   is a pointer to an arbitrary structure in the client application. The client objects are
-   pointers to structures in the client application associated with the coliding objects.
-   'coll_data' is the collision data computed by SOLID.
-*/
 
 	typedef bool (*PHY_ResponseCallback)(void *client_data,
 										   void *client_object1,
@@ -101,9 +82,11 @@ typedef enum PHY_ConstraintType {
 	PHY_LINEHINGE_CONSTRAINT=2,
 	PHY_ANGULAR_CONSTRAINT = 3,//hinge without ball socket
 	PHY_VEHICLE_CONSTRAINT=11,//complex 'constraint' that turns a rigidbody into a vehicle
+	PHY_GENERIC_6DOF_CONSTRAINT=12,//can leave any of the 6 degree of freedom 'free' or 'locked'
 
 } PHY_ConstraintType;
 
+typedef float	PHY_Vector3[3];
 
 #endif //__PHY_DYNAMIC_TYPES
 
diff --git a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
index fe4370fa725..2f3d8283d10 100644
--- a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
+++ b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
@@ -90,6 +90,7 @@ class PHY_IPhysicsEnvironment
 		virtual void		removeConstraint(int	constraintid)=0;
 		virtual float		getAppliedImpulse(int	constraintid){ return 0.f;}
 
+
 		//complex constraint for vehicles
 		virtual PHY_IVehicle*	getVehicleConstraint(int constraintId) =0;