more vehicle preparation and some bullet collision detection bugfixes (related to collision margin)

7 files changed, 576 insertions, 80 deletions

diff --git a/source/gameengine/Ketsji/KX_KetsjiEngine.cpp b/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
index 8be153ecb60..e9d10f785b0 100644
--- a/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
+++ b/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
@@ -333,6 +333,13 @@ void KX_KetsjiEngine::NextFrame()
 		m_clockTime = m_kxsystem->GetTimeInSeconds();
 	
 	double deltatime = m_clockTime - m_frameTime;
+	if (deltatime<0.f)
+	{
+		printf("problem with clock\n");
+		deltatime = 0.f;
+		m_clockTime = 0.f;
+		m_frameTime = 0.f;
+	}
 
 	// Compute the number of logic frames to do each update (fixed tic bricks)
 	int frames =int(deltatime*m_ticrate);
@@ -341,9 +348,21 @@ void KX_KetsjiEngine::NextFrame()
 //		PIL_sleep_ms(4);
 	
 	KX_SceneList::iterator sceneit;
+	int frameOut = 5;
 	
+	if (frames>frameOut)
+	{
+		printf("framedOut: %d\n",frames);
+		m_frameTime+=(frames-frameOut)*(1.0/m_ticrate);
+		frames = frameOut;
+	}
+
 	while (frames)
 	{
+		if (frames > frameOut)
+		{
+			printf ("what happened\n");
+		}
 
 		m_frameTime += 1.0/m_ticrate;
 		
diff --git a/source/gameengine/Ketsji/KX_VehicleWrapper.cpp b/source/gameengine/Ketsji/KX_VehicleWrapper.cpp
index 299a2d1f180..9745af028d5 100644
--- a/source/gameengine/Ketsji/KX_VehicleWrapper.cpp
+++ b/source/gameengine/Ketsji/KX_VehicleWrapper.cpp
@@ -4,6 +4,9 @@
 #include "KX_VehicleWrapper.h"
 #include "PHY_IPhysicsEnvironment.h"
 #include "PHY_IVehicle.h"
+#include "KX_PyMath.h"
+#include "KX_GameObject.h"
+#include "KX_MotionState.h"
 
 #ifdef HAVE_CONFIG_H
 #include <config.h>
@@ -20,6 +23,13 @@ KX_VehicleWrapper::KX_VehicleWrapper(
 
 KX_VehicleWrapper::~KX_VehicleWrapper()
 {
+	int numMotion = m_motionStates.size();
+	for (int i=0;i<numMotion;i++)
+	{
+		PHY_IMotionState* motionState = m_motionStates[i];
+		delete motionState;
+	}
+	m_motionStates.clear();
 }
 
 
@@ -27,18 +37,92 @@ PyObject* KX_VehicleWrapper::PyAddWheel(PyObject* self,
 											PyObject* args, 
 											PyObject* kwds)
 {
+	
+	PyObject* pylistPos,*pylistDir,*pylistAxleDir;
+	PyObject* wheelGameObject;
+	float suspensionRestLength,wheelRadius;
+	int hasSteering;
+
+	
+	if (PyArg_ParseTuple(args,"OOOOffi",&wheelGameObject,&pylistPos,&pylistDir,&pylistAxleDir,&suspensionRestLength,&wheelRadius,&hasSteering))
+	{
+		KX_GameObject* gameOb = (KX_GameObject*) wheelGameObject;
+		
+		PHY_IMotionState* motionState = new KX_MotionState(gameOb->GetSGNode());
+
+		MT_Vector3 attachPos,attachDir,attachAxle;
+		PyVecTo(pylistPos,attachPos);
+		PyVecTo(pylistDir,attachDir);
+		PyVecTo(pylistAxleDir,attachAxle);
+		PHY__Vector3 aPos,aDir,aAxle;
+		aPos[0] = attachPos[0];
+		aPos[1] = attachPos[1];
+		aPos[2] = attachPos[2];
+		aDir[0] = attachDir[0];
+		aDir[1] = attachDir[1];
+		aDir[2] = attachDir[2];
+		aAxle[0] = attachAxle[0];
+		aAxle[1] = attachAxle[1];
+		aAxle[2] = attachAxle[2];
+		
+		printf("attempt for addWheel: suspensionRestLength%f wheelRadius %f, hasSteering:%d\n",suspensionRestLength,wheelRadius,hasSteering);
+		m_vehicle->AddWheel(motionState,aPos,aDir,aAxle,suspensionRestLength,wheelRadius,hasSteering);
+		
+	}
 	Py_INCREF(Py_None);
 	return Py_None;
 }
 
 
 
-PyObject* KX_VehicleWrapper::PyGetWheelsTransform(PyObject* self, 
+
+PyObject* KX_VehicleWrapper::PyGetWheelPosition(PyObject* self, 
 											PyObject* args, 
 											PyObject* kwds)
 {
-	assert(0);
-	return PyInt_FromLong(m_vehicle->GetNumWheels());
+	
+	int wheelIndex;
+
+	if (PyArg_ParseTuple(args,"i",&wheelIndex))
+	{
+		float position[3];
+		m_vehicle->GetWheelPosition(wheelIndex,position[0],position[1],position[2]);
+		MT_Vector3 pos(position[0],position[1],position[2]);
+		return PyObjectFrom(pos);
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+PyObject* KX_VehicleWrapper::PyGetWheelRotation(PyObject* self, 
+											PyObject* args, 
+											PyObject* kwds)
+{
+	int wheelIndex;
+	if (PyArg_ParseTuple(args,"i",&wheelIndex))
+	{
+		return PyFloat_FromDouble(m_vehicle->GetWheelRotation(wheelIndex));
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+PyObject* KX_VehicleWrapper::PyGetWheelOrientationQuaternion(PyObject* self, 
+											PyObject* args, 
+											PyObject* kwds)
+{
+	int wheelIndex;
+	if (PyArg_ParseTuple(args,"i",&wheelIndex))
+	{
+		float orn[4];
+		m_vehicle->GetWheelOrientationQuaternion(wheelIndex,orn[0],orn[1],orn[2],orn[3]);
+		MT_Quaternion	quatorn(orn[0],orn[1],orn[2],orn[3]);
+		MT_Matrix3x3 ornmat(quatorn);
+		return PyObjectFrom(ornmat);
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+
 }
 
 
@@ -57,6 +141,58 @@ PyObject* KX_VehicleWrapper::PyGetConstraintId(PyObject* self,
 	return PyInt_FromLong(m_vehicle->GetUserConstraintId());
 }
 
+
+
+PyObject* KX_VehicleWrapper::PyApplyEngineForce(PyObject* self, 
+											PyObject* args, 
+											PyObject* kwds)
+{
+	float force;
+	int wheelIndex;
+
+	if (PyArg_ParseTuple(args,"fi",&force,&wheelIndex))
+	{
+		m_vehicle->ApplyEngineForce(force,wheelIndex);
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+
+PyObject* KX_VehicleWrapper::PyApplyBraking(PyObject* self, 
+											PyObject* args, 
+											PyObject* kwds)
+{
+	float braking;
+	int wheelIndex;
+
+	if (PyArg_ParseTuple(args,"fi",&braking,&wheelIndex))
+	{
+		m_vehicle->ApplyBraking(braking,wheelIndex);
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+
+
+
+PyObject* KX_VehicleWrapper::PySetSteeringValue(PyObject* self, 
+											PyObject* args, 
+											PyObject* kwds)
+{
+	float steeringValue;
+	int wheelIndex;
+
+	if (PyArg_ParseTuple(args,"fi",&steeringValue,&wheelIndex))
+	{
+		m_vehicle->SetSteeringValue(steeringValue,wheelIndex);
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+
 PyObject* KX_VehicleWrapper::PyGetConstraintType(PyObject* self, 
 											PyObject* args, 
 											PyObject* kwds)
@@ -67,6 +203,7 @@ PyObject* KX_VehicleWrapper::PyGetConstraintType(PyObject* self,
 
 
 
+
 //python specific stuff
 PyTypeObject KX_VehicleWrapper::Type = {
 	PyObject_HEAD_INIT(&PyType_Type)
@@ -130,9 +267,14 @@ int	KX_VehicleWrapper::_setattr(const STR_String& attr,PyObject* pyobj)
 PyMethodDef KX_VehicleWrapper::Methods[] = {
 	{"addWheel",(PyCFunction) KX_VehicleWrapper::sPyAddWheel, METH_VARARGS},
 	{"getNumWheels",(PyCFunction) KX_VehicleWrapper::sPyGetNumWheels, METH_VARARGS},
-	{"getWheelsTransform",(PyCFunction) KX_VehicleWrapper::sPyGetWheelsTransform, METH_VARARGS},
+	{"getWheelOrientationQuaternion",(PyCFunction) KX_VehicleWrapper::sPyGetWheelOrientationQuaternion, METH_VARARGS},
+	{"getWheelRotation",(PyCFunction) KX_VehicleWrapper::sPyGetWheelRotation, METH_VARARGS},
+	{"getWheelPosition",(PyCFunction) KX_VehicleWrapper::sPyGetWheelPosition, METH_VARARGS},
 	{"getConstraintId",(PyCFunction) KX_VehicleWrapper::sPyGetConstraintId, METH_VARARGS},
 	{"getConstraintType",(PyCFunction) KX_VehicleWrapper::sPyGetConstraintType, METH_VARARGS},
+	{"setSteeringValue",(PyCFunction) KX_VehicleWrapper::sPySetSteeringValue, METH_VARARGS},
+	{"applyEngineForce",(PyCFunction) KX_VehicleWrapper::sPyApplyEngineForce, METH_VARARGS},
+	{"applyBraking",(PyCFunction) KX_VehicleWrapper::sPyApplyBraking, METH_VARARGS},
 	{NULL,NULL} //Sentinel
 };
 
diff --git a/source/gameengine/Ketsji/KX_VehicleWrapper.h b/source/gameengine/Ketsji/KX_VehicleWrapper.h
index ebdf1b0fa2f..22b8f186f4a 100644
--- a/source/gameengine/Ketsji/KX_VehicleWrapper.h
+++ b/source/gameengine/Ketsji/KX_VehicleWrapper.h
@@ -4,6 +4,9 @@
 #include "Value.h"
 #include "PHY_DynamicTypes.h"
 class PHY_IVehicle;
+class PHY_IMotionState;
+
+#include <vector>
 
 ///Python interface to physics vehicles (primarily 4-wheel cars and 2wheel bikes)
 class	KX_VehicleWrapper : public PyObjectPlus
@@ -11,6 +14,9 @@ class	KX_VehicleWrapper : public PyObjectPlus
 	Py_Header;
 	virtual PyObject*		_getattr(const STR_String& attr);
 	virtual int 			_setattr(const STR_String& attr, PyObject *value);
+
+	std::vector<PHY_IMotionState*> m_motionStates;
+
 public:
 	KX_VehicleWrapper(PHY_IVehicle* vehicle,class PHY_IPhysicsEnvironment* physenv,PyTypeObject *T = &Type);
 	virtual ~KX_VehicleWrapper ();
@@ -19,11 +25,21 @@ public:
 	
 	KX_PYMETHOD(KX_VehicleWrapper,AddWheel);
 	KX_PYMETHOD(KX_VehicleWrapper,GetNumWheels);
-	KX_PYMETHOD(KX_VehicleWrapper,GetWheelsTransform);
+	KX_PYMETHOD(KX_VehicleWrapper,GetWheelOrientationQuaternion);
+	KX_PYMETHOD(KX_VehicleWrapper,GetWheelRotation);
+	
+	KX_PYMETHOD(KX_VehicleWrapper,GetWheelPosition);
 	
 	KX_PYMETHOD(KX_VehicleWrapper,GetConstraintId);
 	KX_PYMETHOD(KX_VehicleWrapper,GetConstraintType);
 
+	KX_PYMETHOD(KX_VehicleWrapper,SetSteeringValue);
+
+	KX_PYMETHOD(KX_VehicleWrapper,ApplyEngineForce);
+
+	KX_PYMETHOD(KX_VehicleWrapper,ApplyBraking);
+
+
 
 private:
 	PHY_IVehicle*			 m_vehicle;
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
index cdf617f1722..afc2aa2c70f 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
@@ -20,7 +20,7 @@
 #include "NarrowPhaseCollision/SubSimplexConvexCast.h"
 
 #include "CollisionDispatch/ToiContactDispatcher.h"
-
+#include "PHY_IMotionState.h"
 
 #include "CollisionDispatch/EmptyCollisionAlgorithm.h"
 #include "CollisionDispatch/UnionFind.h"
@@ -30,6 +30,16 @@
 
 bool useIslands = true;
 
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
+#include "Vehicle/RaycastVehicle.h"
+#include "Vehicle/VehicleRaycaster.h"
+#include "Vehicle/VehicleTuning.h"
+#include "PHY_IVehicle.h"
+VehicleTuning	gTuning;
+
+#endif //NEW_BULLET_VEHICLE_SUPPORT
+#include "AabbUtil2.h"
+
 #include "ConstraintSolver/ConstraintSolver.h"
 #include "ConstraintSolver/Point2PointConstraint.h"
 //#include "BroadphaseCollision/QueryDispatcher.h"
@@ -44,9 +54,146 @@ void DrawRasterizerLine(const float* from,const float* to,int color);
 #include "ConstraintSolver/ContactConstraint.h"
 
 
-
 #include <stdio.h>
 
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
+class WrapperVehicle : public PHY_IVehicle
+{
+
+	RaycastVehicle*	m_vehicle;
+	PHY_IPhysicsController*	m_chassis;
+		
+public:
+
+	WrapperVehicle(RaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
+		:m_vehicle(vehicle),
+		m_chassis(chassis)
+	{
+	}
+
+	RaycastVehicle*	GetVehicle()
+	{
+		return m_vehicle;
+	}
+
+	PHY_IPhysicsController*	GetChassis()
+	{
+		return m_chassis;
+	}
+
+	virtual void	AddWheel(
+			PHY_IMotionState*	motionState,
+			PHY__Vector3	connectionPoint,
+			PHY__Vector3	downDirection,
+			PHY__Vector3	axleDirection,
+			float	suspensionRestLength,
+			float wheelRadius,
+			bool hasSteering
+		)
+	{
+		SimdVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
+		SimdVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
+		SimdVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
+		
+
+		WheelInfo& info = m_vehicle->AddWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
+			suspensionRestLength,wheelRadius,gTuning,hasSteering);
+		info.m_clientInfo = motionState;
+		
+	}
+
+	void	SyncWheels()
+	{
+		int numWheels = GetNumWheels();
+		int i;
+		for (i=0;i<numWheels;i++)
+		{
+			WheelInfo& info = m_vehicle->GetWheelInfo(i);
+			PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
+			SimdTransform trans = m_vehicle->GetWheelTransformWS(i);
+			SimdQuaternion orn = trans.getRotation();
+			const SimdVector3& pos = trans.getOrigin();
+			motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
+			motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
+
+		}
+	}
+
+	virtual	int		GetNumWheels() const
+	{
+		return m_vehicle->GetNumWheels();
+	}
+	
+	virtual void	GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
+	{
+		SimdTransform	trans = m_vehicle->GetWheelTransformWS(wheelIndex);
+		posX = trans.getOrigin().x();
+		posY = trans.getOrigin().y();
+		posZ = trans.getOrigin().z();
+	}
+	virtual void	GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
+	{
+		SimdTransform	trans = m_vehicle->GetWheelTransformWS(wheelIndex);
+		SimdQuaternion quat = trans.getRotation();
+		SimdMatrix3x3 orn2(quat);
+
+		quatX = trans.getRotation().x();
+		quatY = trans.getRotation().y();
+		quatZ = trans.getRotation().z();
+		quatW = trans.getRotation()[3];
+		
+
+		//printf("test");
+
+
+	}
+
+	virtual float	GetWheelRotation(int wheelIndex) const
+	{
+		float rotation = 0.f;
+
+		if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
+		{
+			WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
+			rotation = info.m_rotation;
+		}
+		return rotation;
+
+	}
+
+
+	
+	virtual int	GetUserConstraintId() const
+	{
+		return m_vehicle->GetUserConstraintId();
+	}
+
+	virtual int	GetUserConstraintType() const
+	{
+		return m_vehicle->GetUserConstraintType();
+	}
+
+	virtual	void	SetSteeringValue(float steering,int wheelIndex)
+	{
+		m_vehicle->SetSteeringValue(steering,wheelIndex);
+	}
+
+	virtual	void	ApplyEngineForce(float force,int wheelIndex)
+	{
+		m_vehicle->ApplyEngineForce(force,wheelIndex);
+	}
+
+	virtual	void	ApplyBraking(float braking,int wheelIndex)
+	{
+		if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
+		{
+			WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
+			info.m_brake = braking;
+		}
+	}
+
+};
+#endif //NEW_BULLET_VEHICLE_SUPPORT
 
 
 
@@ -192,7 +339,7 @@ void	CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr
 			if  ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() ||
 				(&p2p->GetRigidBodyB() == ctrl->GetRigidBody())))
 			{
-				 removeConstraint(p2p->GetConstraintId());
+				 removeConstraint(p2p->GetUserConstraintId());
 				//only 1 constraint per constroller
 				break;
 			}
@@ -209,7 +356,7 @@ void	CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr
 			if  ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() ||
 				(&p2p->GetRigidBodyB() == ctrl->GetRigidBody())))
 			{
-				removeConstraint(p2p->GetConstraintId());
+				removeConstraint(p2p->GetUserConstraintId());
 				//only 1 constraint per constroller
 				break;
 			}
@@ -414,20 +561,25 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 			p2p->SolveConstraint( timeStep );
 			
 		}
-		/*
+
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
 		//vehicles
-		int numVehicles = m_vehicles.size();
-		for (i=0;i<numVehicles;i++)
+		int numVehicles = m_wrapperVehicles.size();
+		for (int i=0;i<numVehicles;i++)
 		{
-			Vehicle* vehicle = m_vehicles[i];
-			vehicle->UpdateVehicle( timeStep );
+			WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
+			RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
+			vehicle->UpdateVehicle( timeStep);
 		}
-		*/
+#endif //NEW_BULLET_VEHICLE_SUPPORT
+
 		
 		
 		
 	}
 	
+
+
 	{
 		
 		
@@ -593,8 +745,26 @@ bool	CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
 			
 	}
 	
+
+
 	SyncMotionStates(timeStep);
 
+	
+#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];
+			
+			for (int j=0;j<wrapperVehicle->GetVehicle()->GetNumWheels();j++)
+			{
+				wrapperVehicle->GetVehicle()->UpdateWheelTransformsWS(wrapperVehicle->GetVehicle()->GetWheelInfo(j));
+			}
+			
+			wrapperVehicle->SyncWheels();
+		}
+#endif //NEW_BULLET_VEHICLE_SUPPORT
 	}
 	return true;
 }
@@ -734,6 +904,78 @@ void		CcdPhysicsEnvironment::setGravity(float x,float y,float z)
 }
 
 
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
+
+class BlenderVehicleRaycaster : public VehicleRaycaster
+{
+	CcdPhysicsEnvironment* m_physEnv;
+	PHY_IPhysicsController*	m_chassis;
+
+public:
+	BlenderVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis):
+	m_physEnv(physEnv),
+		m_chassis(chassis)
+	{
+	}
+
+	/*	struct VehicleRaycasterResult
+	{
+		VehicleRaycasterResult() :m_distFraction(-1.f){};
+		SimdVector3	m_hitPointInWorld;
+		SimdVector3	m_hitNormalInWorld;
+		SimdScalar	m_distFraction;
+	};
+*/
+	virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result)
+	{
+		
+		
+		float hit[3];
+		float normal[3];
+
+		PHY_IPhysicsController*	ignore = m_chassis;
+		void* hitObject = m_physEnv->rayTest(ignore,from.x(),from.y(),from.z(),to.x(),to.y(),to.z(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
+		if (hitObject)
+		{
+			result.m_hitPointInWorld[0] = hit[0];
+			result.m_hitPointInWorld[1] = hit[1];
+			result.m_hitPointInWorld[2] = hit[2];
+			result.m_hitNormalInWorld[0] = normal[0];
+			result.m_hitNormalInWorld[1] = normal[1];
+			result.m_hitNormalInWorld[2] = normal[2];
+			result.m_hitNormalInWorld.normalize();
+			//calc fraction? or put it in the interface?
+			//calc for now
+			
+			result.m_distFraction = (result.m_hitPointInWorld-from).length() / (to-from).length();
+			//some safety for 'explosion' due to sudden penetration of the full 'ray'
+/*			if (result.m_distFraction<0.1)
+			{
+				printf("Vehicle Raycast: avoided instability due to penetration. Consider moving the connection points deeper inside vehicle chassis");
+				result.m_distFraction = 1.f;
+				hitObject = 0;
+			}
+			*/
+
+/*			if (result.m_distFraction>1.)
+			{
+				printf("Vehicle Raycast: avoided instability 1Consider moving the connection points deeper inside vehicle chassis");				
+				result.m_distFraction = 1.f;
+				hitObject = 0;
+			}
+			*/
+
+				
+			
+		}
+		//?
+		return hitObject;
+	}
+};
+#endif //NEW_BULLET_VEHICLE_SUPPORT
+
+static int gConstraintUid = 1;
+
 int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
 														float pivotX,float pivotY,float pivotZ,
 														float axisX,float axisY,float axisZ)
@@ -769,12 +1011,32 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 			}
 			
 			m_p2pConstraints.push_back(p2p);
-			
+			p2p->SetUserConstraintId(gConstraintUid++);
+			p2p->SetUserConstraintType(type);
 			//64 bit systems can't cast pointer to int. could use size_t instead.
-			return p2p->GetConstraintId();
+			return p2p->GetUserConstraintId();
 			
 			break;
 		}
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
+
+	case PHY_VEHICLE_CONSTRAINT:
+		{
+			VehicleTuning* tuning = new VehicleTuning();
+			RigidBody* chassis = rb0;
+			BlenderVehicleRaycaster* raycaster = new BlenderVehicleRaycaster(this,ctrl0);
+			RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
+			vehicle->SetBalance(false);
+			WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
+			m_wrapperVehicles.push_back(wrapperVehicle);
+			vehicle->SetUserConstraintId(gConstraintUid++);
+			vehicle->SetUserConstraintType(type);
+			return vehicle->GetUserConstraintId();
+
+			break;
+		};
+#endif //NEW_BULLET_VEHICLE_SUPPORT
+
 	default:
 		{
 		}
@@ -797,7 +1059,7 @@ void		CcdPhysicsEnvironment::removeConstraint(int	constraintId)
 		!(i==m_p2pConstraints.end()); i++)
 		{
 			Point2PointConstraint* p2p = (*i);
-			if (p2p->GetConstraintId() == constraintId)
+			if (p2p->GetUserConstraintId() == constraintId)
 			{
 				std::swap(*i, m_p2pConstraints.back());
 				m_p2pConstraints.pop_back();
@@ -814,11 +1076,21 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 
 	SimdTransform	rayFromTrans,rayToTrans;
 	rayFromTrans.setIdentity();
-	rayFromTrans.setOrigin(SimdVector3(fromX,fromY,fromZ));
+
+	SimdVector3 rayFrom(fromX,fromY,fromZ);
+
+	rayFromTrans.setOrigin(rayFrom);
 	rayToTrans.setIdentity();
-	rayToTrans.setOrigin(SimdVector3(toX,toY,toZ));
+	SimdVector3 rayTo(toX,toY,toZ);
+	rayToTrans.setOrigin(rayTo);
 
+	//do culling based on aabb (rayFrom/rayTo)
+	SimdVector3 rayAabbMin = rayFrom;
+	SimdVector3 rayAabbMax = rayFrom;
+	rayAabbMin.setMin(rayTo);
+	rayAabbMax.setMax(rayTo);
 
+	
 	CcdPhysicsController* nearestHit = 0;
 	
 	std::vector<CcdPhysicsController*>::iterator i;
@@ -830,71 +1102,81 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 	!(i==m_controllers.end()); i++)
 	{
 		CcdPhysicsController* ctrl = (*i);
+		if (ctrl == ignoreClient)
+			continue;
 		RigidBody* body = ctrl->GetRigidBody();
+		SimdVector3 bodyAabbMin,bodyAabbMax;
+		body->getAabb(bodyAabbMin,bodyAabbMax);
 
-		if (body->GetCollisionShape()->IsConvex())
-		{
-			ConvexCast::CastResult rayResult;
-			rayResult.m_fraction = 1.f;
+		//check aabb overlap
 
-			ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape();
-			VoronoiSimplexSolver	simplexSolver;
-			SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
-			
-			if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult))
+		if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,bodyAabbMin,bodyAabbMax))
+		{
+			if (body->GetCollisionShape()->IsConvex())
 			{
-				//add hit
-				rayResult.m_normal.normalize();
-				if (rayResult.m_fraction < minFraction)
+				ConvexCast::CastResult rayResult;
+				rayResult.m_fraction = 1.f;
+
+				ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape();
+				VoronoiSimplexSolver	simplexSolver;
+				SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
+				
+				if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult))
 				{
+					//add hit
+					if (rayResult.m_normal.length2() > 0.0001f)
+					{
+						rayResult.m_normal.normalize();
+						if (rayResult.m_fraction < minFraction)
+						{
 
 
-					minFraction = rayResult.m_fraction;
-					nearestHit = ctrl;
-					normalX = rayResult.m_normal.getX();
-					normalY = rayResult.m_normal.getY();
-					normalZ = rayResult.m_normal.getZ();
-					hitX = rayResult.m_hitTransformA.getOrigin().getX();
-					hitY = rayResult.m_hitTransformA.getOrigin().getY();
-					hitZ = rayResult.m_hitTransformA.getOrigin().getZ();
+							minFraction = rayResult.m_fraction;
+							nearestHit = ctrl;
+							normalX = rayResult.m_normal.getX();
+							normalY = rayResult.m_normal.getY();
+							normalZ = rayResult.m_normal.getZ();
+							hitX = rayResult.m_hitTransformA.getOrigin().getX();
+							hitY = rayResult.m_hitTransformA.getOrigin().getY();
+							hitZ = rayResult.m_hitTransformA.getOrigin().getZ();
+						}
+					}
 				}
 			}
-		}
-		else
-		{
-				if (body->GetCollisionShape()->IsConcave())
-				{
-
-					TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape();
-					
-					SimdTransform worldToBody = body->getCenterOfMassTransform().inverse();
+			else
+			{
+					if (body->GetCollisionShape()->IsConcave())
+					{
 
-					SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin();
-					SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin();
+						TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape();
+						
+						SimdTransform worldToBody = body->getCenterOfMassTransform().inverse();
 
-					RaycastCallback	rcb(rayFromLocal,rayToLocal);
-					rcb.m_hitFraction = minFraction;
+						SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin();
+						SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin();
 
-					SimdVector3 aabbMax(1e30f,1e30f,1e30f);
+						RaycastCallback	rcb(rayFromLocal,rayToLocal);
+						rcb.m_hitFraction = minFraction;
 
-					triangleMesh->ProcessAllTriangles(&rcb,-aabbMax,aabbMax);
-					if (rcb.m_hitFound)// && (rcb.m_hitFraction < minFraction))
-					{
-						nearestHit = ctrl;
-						minFraction = rcb.m_hitFraction;
-						SimdVector3 hitNormalWorld = body->getCenterOfMassTransform()(rcb.m_hitNormalLocal);
-
-						normalX = hitNormalWorld.getX();
-						normalY = hitNormalWorld.getY();
-						normalZ = hitNormalWorld.getZ();
-						SimdVector3 hitWorld;
-						hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rcb.m_hitFraction);
-						hitX = hitWorld.getX();
-						hitY = hitWorld.getY();
-						hitZ = hitWorld.getZ();
-						
+						triangleMesh->ProcessAllTriangles(&rcb,rayAabbMin,rayAabbMax);
+						if (rcb.m_hitFound)// && (rcb.m_hitFraction < minFraction))
+						{
+							nearestHit = ctrl;
+							minFraction = rcb.m_hitFraction;
+							SimdVector3 hitNormalWorld = body->getCenterOfMassTransform()(rcb.m_hitNormalLocal);
+
+							normalX = hitNormalWorld.getX();
+							normalY = hitNormalWorld.getY();
+							normalZ = hitNormalWorld.getZ();
+							SimdVector3 hitWorld;
+							hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rcb.m_hitFraction);
+							hitX = hitWorld.getX();
+							hitY = hitWorld.getY();
+							hitZ = hitWorld.getZ();
+							
+						}
 					}
-				}
+			}
 		}
 	}
 
@@ -925,8 +1207,9 @@ Dispatcher* CcdPhysicsEnvironment::GetDispatcher()
 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
 {
 	
-	
-	m_vehicles.clear();
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
+	m_wrapperVehicles.clear();
+#endif //NEW_BULLET_VEHICLE_SUPPORT
 	
 	//m_broadphase->DestroyScene();
 	//delete broadphase ? release reference on broadphase ?
@@ -958,3 +1241,25 @@ const PersistentManifold*	CcdPhysicsEnvironment::GetManifold(int index) const
 {
 	return m_dispatcher->GetManifoldByIndexInternal(index);
 }
+
+
+
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
+
+//complex constraint for vehicles
+PHY_IVehicle*	CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
+{
+	int i;
+
+	int numVehicles = m_wrapperVehicles.size();
+	for (i=0;i<numVehicles;i++)
+	{
+		WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
+		if (wrapperVehicle->GetVehicle()->GetUserConstraintId() == constraintId)
+			return wrapperVehicle;
+	}
+
+	return 0;
+}
+
+#endif //NEW_BULLET_VEHICLE_SUPPORT
\ No newline at end of file
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
index a599968a305..9664e9b6233 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
@@ -13,7 +13,10 @@ class ToiContactDispatcher;
 class Dispatcher;
 //#include "BroadphaseInterface.h"
 
-class Vehicle;
+//switch on/off new vehicle support
+//#define NEW_BULLET_VEHICLE_SUPPORT 1
+
+class WrapperVehicle;
 class PersistentManifold;
 class BroadphaseInterface;
 class IDebugDraw;
@@ -79,11 +82,15 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 			float axisX,float axisY,float axisZ);
 	    virtual void		removeConstraint(int	constraintid);
 
+#ifdef NEW_BULLET_VEHICLE_SUPPORT
 		//complex constraint for vehicles
+		virtual PHY_IVehicle*	getVehicleConstraint(int constraintId);
+#else
 		virtual PHY_IVehicle*	getVehicleConstraint(int constraintId)
 		{
 			return 0;
 		}
+#endif //NEW_BULLET_VEHICLE_SUPPORT
 
 		virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
 										float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ);
@@ -132,7 +139,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 
 		std::vector<Point2PointConstraint*> m_p2pConstraints;
 
-		std::vector<Vehicle*>	m_vehicles;
+		std::vector<WrapperVehicle*>	m_wrapperVehicles;
 
 		class ToiContactDispatcher* m_dispatcher;
 
diff --git a/source/gameengine/Physics/common/PHY_DynamicTypes.h b/source/gameengine/Physics/common/PHY_DynamicTypes.h
index e247a8c5f2a..54aaf240660 100644
--- a/source/gameengine/Physics/common/PHY_DynamicTypes.h
+++ b/source/gameengine/Physics/common/PHY_DynamicTypes.h
@@ -103,7 +103,6 @@ typedef enum PHY_ConstraintType {
 
 } PHY_ConstraintType;
 
-typedef float	PHY_Vector3[3];
 
 #endif //__PHY_DYNAMIC_TYPES
 
diff --git a/source/gameengine/Physics/common/PHY_IVehicle.h b/source/gameengine/Physics/common/PHY_IVehicle.h
index 655c1154bb9..0e6fe3379a2 100644
--- a/source/gameengine/Physics/common/PHY_IVehicle.h
+++ b/source/gameengine/Physics/common/PHY_IVehicle.h
@@ -13,9 +13,10 @@ public:
 	virtual ~PHY_IVehicle();
 	
 	virtual void	AddWheel(
-			PHY_Vector3	connectionPoint,
-			PHY_Vector3	downDirection,
-			PHY_Vector3	axleDirection,
+			PHY_IMotionState* motionState,
+			PHY__Vector3	connectionPoint,
+			PHY__Vector3	downDirection,
+			PHY__Vector3	axleDirection,
 			float	suspensionRestLength,
 			float wheelRadius,
 			bool hasSteering
@@ -24,13 +25,20 @@ public:
 
 	virtual	int		GetNumWheels() const = 0;
 	
-	virtual const PHY_IMotionState*		GetWheelMotionState(int wheelIndex) const = 0;
+	virtual void	GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const = 0;
+	virtual void	GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const = 0;
+	virtual float	GetWheelRotation(int wheelIndex) const = 0;
 
 	virtual int	GetUserConstraintId() const =0;
 	virtual int	GetUserConstraintType() const =0;
 
 	//some basic steering/braking/tuning/balancing (bikes)
 
+	virtual	void	SetSteeringValue(float steering,int wheelIndex) = 0;
+
+	virtual	void	ApplyEngineForce(float force,int wheelIndex) = 0;
+
+	virtual	void	ApplyBraking(float braking,int wheelIndex) = 0;
 
 };