Index: code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
===================================================================
--- code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp	(revision 2662)
+++ code/trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp	(revision 2882)
@@ -30,4 +30,9 @@
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 
 //for debug rendering
@@ -47,13 +52,5 @@
 
 
-
-//vehicle
-#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
-#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
-#include "BulletDynamics/Vehicle/btWheelInfo.h"
-//character
-#include "BulletDynamics/Character/btCharacterControllerInterface.h"
-
-#include "LinearMath/btIDebugDraw.h"
+#include "BulletDynamics/Dynamics/btActionInterface.h"
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btMotionState.h"
@@ -114,5 +111,4 @@
 		if (body)
 		{
-				btTransform predictedTrans;
 				if (body->getActivationState() != ISLAND_SLEEPING)
 				{
@@ -149,4 +145,22 @@
 		}
 	}
+	bool drawConstraints = false;
+	if (getDebugDrawer())
+	{
+		int mode = getDebugDrawer()->getDebugMode();
+		if(mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		{
+			drawConstraints = true;
+		}
+	}
+	if(drawConstraints)
+	{
+		for(int i = getNumConstraints()-1; i>=0 ;i--)
+		{
+			btTypedConstraint* constraint = getConstraint(i);
+			debugDrawConstraint(constraint);
+		}
+	}
+
 
 
@@ -191,30 +205,9 @@
 		}
 	
-		for (  i=0;i<this->m_vehicles.size();i++)
-		{
-			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
-			{
-				btVector3 wheelColor(0,255,255);
-				if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
-				{
-					wheelColor.setValue(0,0,255);
-				} else
-				{
-					wheelColor.setValue(255,0,255);
-				}
-		
-				btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
-
-				btVector3 axle = btVector3(	
-					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
-					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
-					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
-
-
-				//m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
-				//debug wheels (cylinders)
-				m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
-				m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
-
+		if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
+		{
+			for (i=0;i<m_actions.size();i++)
+			{
+				m_actions[i]->debugDraw(m_debugDrawer);
 			}
 		}
@@ -288,5 +281,5 @@
 		}
 	}
-
+/*
 	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
 	{
@@ -300,4 +293,6 @@
 		}
 	}
+	*/
+
 
 }
@@ -340,5 +335,6 @@
 	if (getDebugDrawer())
 	{
-		gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
+		btIDebugDraw* debugDrawer = getDebugDrawer ();
+		gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
 	}
 	if (numSimulationSubSteps)
@@ -404,8 +400,6 @@
 
 	///update vehicle simulation
-	updateVehicles(timeStep);
-	
-	updateCharacters(timeStep);
-
+	updateActions(timeStep);
+	
 	updateActivationState( timeStep );
 
@@ -471,27 +465,13 @@
 
 
-void	btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
-{
-	BT_PROFILE("updateVehicles");
-	
-	for ( int i=0;i<m_vehicles.size();i++)
-	{
-		btRaycastVehicle* vehicle = m_vehicles[i];
-		vehicle->updateVehicle( timeStep);
-	}
-}
-
-void	btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep)
-{
-	BT_PROFILE("updateCharacters");
-	
-	for ( int i=0;i<m_characters.size();i++)
-	{
-		btCharacterControllerInterface* character = m_characters[i];
-		character->preStep (this);
-		character->playerStep (this,timeStep);
-	}
-}
-
+void	btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
+{
+	BT_PROFILE("updateActions");
+	
+	for ( int i=0;i<m_actions.size();i++)
+	{
+		m_actions[i]->updateAction( this, timeStep);
+	}
+}
 	
 	
@@ -550,22 +530,33 @@
 }
 
-void	btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
-{
-	m_vehicles.push_back(vehicle);
-}
-
-void	btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
-{
-	m_vehicles.remove(vehicle);
-}
-
-void	btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character)
-{
-	m_characters.push_back(character);
-}
-
-void	btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character)
-{
-	m_characters.remove(character);
+void	btDiscreteDynamicsWorld::addAction(btActionInterface* action)
+{
+	m_actions.push_back(action);
+}
+
+void	btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
+{
+	m_actions.remove(action);
+}
+
+
+void	btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
+{
+	addAction(vehicle);
+}
+
+void	btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
+{
+	removeAction(vehicle);
+}
+
+void	btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
+{
+	addAction(character);
+}
+
+void	btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
+{
+	removeAction(character);
 }
 
@@ -643,6 +634,9 @@
 			if (islandId<0)
 			{
-				///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
-				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+				if (numManifolds + m_numConstraints)
+				{
+					///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+					m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+				}
 			} else
 			{
@@ -690,5 +684,5 @@
 	}
 
-//	assert(0);
+//	btAssert(0);
 		
 	
@@ -754,12 +748,14 @@
 	btScalar m_allowedPenetration;
 	btOverlappingPairCache* m_pairCache;
+	btDispatcher* m_dispatcher;
 
 
 public:
-	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : 
+	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
 	  btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
 		m_allowedPenetration(0.0f),
 		m_me(me),
-		m_pairCache(pairCache)
+		m_pairCache(pairCache),
+		m_dispatcher(dispatcher)
 	{
 	}
@@ -796,18 +792,24 @@
 			return false;
 
-		///don't do CCD when there are already contact points (touching contact/penetration)
-		btAlignedObjectArray<btPersistentManifold*> manifoldArray;
-		btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
-		if (collisionPair)
-		{
-			if (collisionPair->m_algorithm)
-			{
-				manifoldArray.resize(0);
-				collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
-				for (int j=0;j<manifoldArray.size();j++)
-				{
-					btPersistentManifold* manifold = manifoldArray[j];
-					if (manifold->getNumContacts()>0)
-						return false;
+		btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
+
+		//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
+		if (m_dispatcher->needsResponse(m_me,otherObj))
+		{
+			///don't do CCD when there are already contact points (touching contact/penetration)
+			btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+			btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
+			if (collisionPair)
+			{
+				if (collisionPair->m_algorithm)
+				{
+					manifoldArray.resize(0);
+					collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
+					for (int j=0;j<manifoldArray.size();j++)
+					{
+						btPersistentManifold* manifold = manifoldArray[j];
+						if (manifold->getNumContacts()>0)
+							return false;
+					}
 				}
 			}
@@ -847,5 +849,5 @@
 						gNumClampedCcdMotions++;
 						
-						btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
+						btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
 						btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
 						btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
@@ -1188,4 +1190,187 @@
 
 
+void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
+{
+	bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
+	bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
+	btScalar dbgDrawSize = constraint->getDbgDrawSize();
+	if(dbgDrawSize <= btScalar(0.f))
+	{
+		return;
+	}
+
+	switch(constraint->getConstraintType())
+	{
+		case POINT2POINT_CONSTRAINT_TYPE:
+			{
+				btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
+				btTransform tr;
+				tr.setIdentity();
+				btVector3 pivot = p2pC->getPivotInA();
+				pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; 
+				tr.setOrigin(pivot);
+				getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				// that ideally should draw the same frame	
+				pivot = p2pC->getPivotInB();
+				pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; 
+				tr.setOrigin(pivot);
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			}
+			break;
+		case HINGE_CONSTRAINT_TYPE:
+			{
+				btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
+				btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				btScalar minAng = pHinge->getLowerLimit();
+				btScalar maxAng = pHinge->getUpperLimit();
+				if(minAng == maxAng)
+				{
+					break;
+				}
+				bool drawSect = true;
+				if(minAng > maxAng)
+				{
+					minAng = btScalar(0.f);
+					maxAng = SIMD_2_PI;
+					drawSect = false;
+				}
+				if(drawLimits) 
+				{
+					btVector3& center = tr.getOrigin();
+					btVector3 normal = tr.getBasis().getColumn(2);
+					btVector3 axis = tr.getBasis().getColumn(0);
+					getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
+				}
+			}
+			break;
+		case CONETWIST_CONSTRAINT_TYPE:
+			{
+				btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
+				btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				if(drawLimits)
+				{
+					//const btScalar length = btScalar(5);
+					const btScalar length = dbgDrawSize;
+					static int nSegments = 8*4;
+					btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
+					btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
+					pPrev = tr * pPrev;
+					for (int i=0; i<nSegments; i++)
+					{
+						fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
+						btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
+						pCur = tr * pCur;
+						getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
+
+						if (i%(nSegments/8) == 0)
+							getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
+
+						pPrev = pCur;
+					}						
+					btScalar tws = pCT->getTwistSpan();
+					btScalar twa = pCT->getTwistAngle();
+					bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
+					if(useFrameB)
+					{
+						tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
+					}
+					else
+					{
+						tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
+					}
+					btVector3 pivot = tr.getOrigin();
+					btVector3 normal = tr.getBasis().getColumn(0);
+					btVector3 axis1 = tr.getBasis().getColumn(1);
+					getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
+
+				}
+			}
+			break;
+		case D6_CONSTRAINT_TYPE:
+			{
+				btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
+				btTransform tr = p6DOF->getCalculatedTransformA();
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				tr = p6DOF->getCalculatedTransformB();
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				if(drawLimits) 
+				{
+					tr = p6DOF->getCalculatedTransformA();
+					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
+					btVector3 up = tr.getBasis().getColumn(2);
+					btVector3 axis = tr.getBasis().getColumn(0);
+					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
+					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
+					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
+					axis = tr.getBasis().getColumn(1);
+					btScalar ay = p6DOF->getAngle(1);
+					btScalar az = p6DOF->getAngle(2);
+					btScalar cy = btCos(ay);
+					btScalar sy = btSin(ay);
+					btScalar cz = btCos(az);
+					btScalar sz = btSin(az);
+					btVector3 ref;
+					ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
+					ref[1] = -sz*axis[0] + cz*axis[1];
+					ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
+					tr = p6DOF->getCalculatedTransformB();
+					btVector3 normal = -tr.getBasis().getColumn(0);
+					btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
+					btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
+					if(minFi > maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
+					}
+					else if(minFi < maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
+					}
+					tr = p6DOF->getCalculatedTransformA();
+					btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
+					btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
+					getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
+				}
+			}
+			break;
+		case SLIDER_CONSTRAINT_TYPE:
+			{
+				btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
+				btTransform tr = pSlider->getCalculatedTransformA();
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				tr = pSlider->getCalculatedTransformB();
+				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				if(drawLimits)
+				{
+					btTransform tr = pSlider->getCalculatedTransformA();
+					btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
+					btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
+					getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
+					btVector3 normal = tr.getBasis().getColumn(0);
+					btVector3 axis = tr.getBasis().getColumn(1);
+					btScalar a_min = pSlider->getLowerAngLimit();
+					btScalar a_max = pSlider->getUpperAngLimit();
+					const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
+					getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
+				}
+			}
+			break;
+		default : 
+			break;
+	}
+	return;
+} // btDiscreteDynamicsWorld::debugDrawConstraint()
+
+
+
+
+
 void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
@@ -1216,2 +1401,4 @@
 	return m_constraints[index];
 }
+
+
