MultiSource/Benchmarks/Bullet/btRaycastVehicle.cpp - third_party/github.com/llvm/llvm-test-suite - Git at Google

 /*
  * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
  *
  * Permission to use, copy, modify, distribute and sell this software
  * and its documentation for any purpose is hereby granted without fee,
  * provided that the above copyright notice appear in all copies.
  * Erwin Coumans makes no representations about the suitability
  * of this software for any purpose.
  * It is provided "as is" without express or implied warranty.
 */

 #include "LinearMath/btVector3.h"
 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"

 #include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
 #include "LinearMath/btQuaternion.h"
 #include "BulletDynamics/Dynamics/btDynamicsWorld.h"
 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
 #include "BulletDynamics/Vehicle/btWheelInfo.h"
 #include "LinearMath/btMinMax.h"
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"

 static btRigidBody s_fixedObject( 0,0,0);

 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster )
 :m_vehicleRaycaster(raycaster),
 m_pitchControl(btScalar(0.))
 {
 	m_chassisBody = chassis;
 	m_indexRightAxis = 0;
 	m_indexUpAxis = 2;
 	m_indexForwardAxis = 1;
 	defaultInit(tuning);
 }


 void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
 {
 	(void)tuning;
 	m_currentVehicleSpeedKmHour = btScalar(0.);
 	m_steeringValue = btScalar(0.);

 }


 btRaycastVehicle::~btRaycastVehicle()
 {
 }


 //
 // basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
 //
 btWheelInfo&	btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
 {

 	btWheelInfoConstructionInfo ci;

 	ci.m_chassisConnectionCS = connectionPointCS;
 	ci.m_wheelDirectionCS = wheelDirectionCS0;
 	ci.m_wheelAxleCS = wheelAxleCS;
 	ci.m_suspensionRestLength = suspensionRestLength;
 	ci.m_wheelRadius = wheelRadius;
 	ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
 	ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
 	ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
 	ci.m_frictionSlip = tuning.m_frictionSlip;
 	ci.m_bIsFrontWheel = isFrontWheel;
 	ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;

 	m_wheelInfo.push_back( btWheelInfo(ci));

 	btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];

 	updateWheelTransformsWS( wheel , false );
 	updateWheelTransform(getNumWheels()-1,false);
 	return wheel;
 }


 const btTransform&	btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
 {
 	btAssert(wheelIndex < getNumWheels());
 	const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
 	return wheel.m_worldTransform;

 }

 void	btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
 {

 	btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
 	updateWheelTransformsWS(wheel,interpolatedTransform);
 	btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
 	const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
 	btVector3 fwd = up.cross(right);
 	fwd = fwd.normalize();
 //	up = right.cross(fwd);
 //	up.normalize();

 	//rotate around steering over de wheelAxleWS
 	btScalar steering = wheel.m_steering;

 	btQuaternion steeringOrn(up,steering);//wheel.m_steering);
 	btMatrix3x3 steeringMat(steeringOrn);

 	btQuaternion rotatingOrn(right,-wheel.m_rotation);
 	btMatrix3x3 rotatingMat(rotatingOrn);

 	btMatrix3x3 basis2(
 		right[0],fwd[0],up[0],
 		right[1],fwd[1],up[1],
 		right[2],fwd[2],up[2]
 	);

 	wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
 	wheel.m_worldTransform.setOrigin(
 		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
 	);
 }

 void btRaycastVehicle::resetSuspension()
 {

 	int i;
 	for (i=0;i<m_wheelInfo.size();	i++)
 	{
 			btWheelInfo& wheel = m_wheelInfo[i];
 			wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
 			wheel.m_suspensionRelativeVelocity = btScalar(0.0);

 			wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
 			//wheel_info.setContactFriction(btScalar(0.0));
 			wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
 	}
 }

 void	btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
 {
 	wheel.m_raycastInfo.m_isInContact = false;

 	btTransform chassisTrans = getChassisWorldTransform();
 	if (interpolatedTransform && (getRigidBody()->getMotionState()))
 	{
 		getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
 	}

 	wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
 	wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() *  wheel.m_wheelDirectionCS ;
 	wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
 }

 btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 {
 	updateWheelTransformsWS( wheel,false);


 	btScalar depth = -1;

 	btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;

 	btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
 	const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
 	wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
 	const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;

 	btScalar param = btScalar(0.);

 	btVehicleRaycaster::btVehicleRaycasterResult	rayResults;

 	btAssert(m_vehicleRaycaster);

 	void* object = m_vehicleRaycaster->castRay(source,target,rayResults);

 	wheel.m_raycastInfo.m_groundObject = 0;

 	if (object)
 	{
 		param = rayResults.m_distFraction;
 		depth = raylen * rayResults.m_distFraction;
 		wheel.m_raycastInfo.m_contactNormalWS  = rayResults.m_hitNormalInWorld;
 		wheel.m_raycastInfo.m_isInContact = true;

 		wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!;
 		//wheel.m_raycastInfo.m_groundObject = object;


 		btScalar hitDistance = param*raylen;
 		wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
 		//clamp on max suspension travel

 		btScalar  minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
 		btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);
 		if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
 		{
 			wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
 		}
 		if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
 		{
 			wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
 		}

 		wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;

 		btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );

 		btVector3 chassis_velocity_at_contactPoint;
 		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();

 		chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);

 		btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );

 		if ( denominator >= btScalar(-0.1))
 		{
 			wheel.m_suspensionRelativeVelocity = btScalar(0.0);
 			wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
 		}
 		else
 		{
 			btScalar inv = btScalar(-1.) / denominator;
 			wheel.m_suspensionRelativeVelocity = projVel * inv;
 			wheel.m_clippedInvContactDotSuspension = inv;
 		}

 	} else
 	{
 		//put wheel info as in rest position
 		wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
 		wheel.m_suspensionRelativeVelocity = btScalar(0.0);
 		wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
 		wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
 	}

 	return depth;
 }


 const btTransform& btRaycastVehicle::getChassisWorldTransform() const
 {
 	/*if (getRigidBody()->getMotionState())
 	{
 		btTransform chassisWorldTrans;
 		getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
 		return chassisWorldTrans;
 	}
 	*/


 	return getRigidBody()->getCenterOfMassTransform();
 }


 void btRaycastVehicle::updateVehicle( btScalar step )
 {
 	{
 		for (int i=0;i<getNumWheels();i++)
 		{
 			updateWheelTransform(i,false);
 		}
 	}


 	m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();

 	const btTransform& chassisTrans = getChassisWorldTransform();

 	btVector3 forwardW (
 		chassisTrans.getBasis()[0][m_indexForwardAxis],
 		chassisTrans.getBasis()[1][m_indexForwardAxis],
 		chassisTrans.getBasis()[2][m_indexForwardAxis]);

 	if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
 	{
 		m_currentVehicleSpeedKmHour *= btScalar(-1.);
 	}

 	//
 	// simulate suspension
 	//

 	int i=0;
 	for (i=0;i<m_wheelInfo.size();i++)
 	{
 		btScalar depth;
 		depth = rayCast( m_wheelInfo[i]);
 	}

 	updateSuspension(step);


 	for (i=0;i<m_wheelInfo.size();i++)
 	{
 		//apply suspension force
 		btWheelInfo& wheel = m_wheelInfo[i];

 		btScalar suspensionForce = wheel.m_wheelsSuspensionForce;

 		btScalar gMaxSuspensionForce = btScalar(6000.);
 		if (suspensionForce > gMaxSuspensionForce)
 		{
 			suspensionForce = gMaxSuspensionForce;
 		}
 		btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
 		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();

 		getRigidBody()->applyImpulse(impulse, relpos);

 	}


 	updateFriction( step);


 	for (i=0;i<m_wheelInfo.size();i++)
 	{
 		btWheelInfo& wheel = m_wheelInfo[i];
 		btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
 		btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );

 		if (wheel.m_raycastInfo.m_isInContact)
 		{
 			const btTransform&	chassisWorldTransform = getChassisWorldTransform();

 			btVector3 fwd (
 				chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
 				chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
 				chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);

 			btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
 			fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;

 			btScalar proj2 = fwd.dot(vel);

 			wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
 			wheel.m_rotation += wheel.m_deltaRotation;

 		} else
 		{
 			wheel.m_rotation += wheel.m_deltaRotation;
 		}

 		wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact

 	}


 }


 void	btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
 {
 	btAssert(wheel>=0 && wheel < getNumWheels());

 	btWheelInfo& wheelInfo = getWheelInfo(wheel);
 	wheelInfo.m_steering = steering;
 }


 btScalar	btRaycastVehicle::getSteeringValue(int wheel) const
 {
 	return getWheelInfo(wheel).m_steering;
 }


 void	btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
 {
 	btAssert(wheel>=0 && wheel < getNumWheels());
 	btWheelInfo& wheelInfo = getWheelInfo(wheel);
 	wheelInfo.m_engineForce = force;
 }


 const btWheelInfo&	btRaycastVehicle::getWheelInfo(int index) const
 {
 	btAssert((index >= 0) && (index < 	getNumWheels()));

 	return m_wheelInfo[index];
 }

 btWheelInfo&	btRaycastVehicle::getWheelInfo(int index)
 {
 	btAssert((index >= 0) && (index < 	getNumWheels()));

 	return m_wheelInfo[index];
 }

 void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
 {
 	btAssert((wheelIndex >= 0) && (wheelIndex < 	getNumWheels()));
 	getWheelInfo(wheelIndex).m_brake = brake;
 }


 void	btRaycastVehicle::updateSuspension(btScalar deltaTime)
 {
 	(void)deltaTime;

 	btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();

 	for (int w_it=0; w_it<getNumWheels(); w_it++)
 	{
 		btWheelInfo &wheel_info = m_wheelInfo[w_it];

 		if ( wheel_info.m_raycastInfo.m_isInContact )
 		{
 			btScalar force;
 			//	Spring
 			{
 				btScalar	susp_length			= wheel_info.getSuspensionRestLength();
 				btScalar	current_length = wheel_info.m_raycastInfo.m_suspensionLength;

 				btScalar length_diff = (susp_length - current_length);

 				force = wheel_info.m_suspensionStiffness
 					* length_diff * wheel_info.m_clippedInvContactDotSuspension;
 			}

 			// Damper
 			{
 				btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
 				{
 					btScalar	susp_damping;
 					if ( projected_rel_vel < btScalar(0.0) )
 					{
 						susp_damping = wheel_info.m_wheelsDampingCompression;
 					}
 					else
 					{
 						susp_damping = wheel_info.m_wheelsDampingRelaxation;
 					}
 					force -= susp_damping * projected_rel_vel;
 				}
 			}

 			// RESULT
 			wheel_info.m_wheelsSuspensionForce = force * chassisMass;
 			if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
 			{
 				wheel_info.m_wheelsSuspensionForce = btScalar(0.);
 			}
 		}
 		else
 		{
 			wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
 		}
 	}

 }


 struct btWheelContactPoint
 {
 	btRigidBody* m_body0;
 	btRigidBody* m_body1;
 	btVector3	m_frictionPositionWorld;
 	btVector3	m_frictionDirectionWorld;
 	btScalar	m_jacDiagABInv;
 	btScalar	m_maxImpulse;


 	btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
 		:m_body0(body0),
 		m_body1(body1),
 		m_frictionPositionWorld(frictionPosWorld),
 		m_frictionDirectionWorld(frictionDirectionWorld),
 		m_maxImpulse(maxImpulse)
 	{
 		btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
 		btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
 		btScalar	relaxation = 1.f;
 		m_jacDiagABInv = relaxation/(denom0+denom1);
 	}


 };

 btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
 btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
 {

 	btScalar j1=0.f;

 	const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;

 	btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition();
 	btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();

 	btScalar maxImpulse  = contactPoint.m_maxImpulse;

 	btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
 	btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
 	btVector3 vel = vel1 - vel2;

 	btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);

 	// calculate j that moves us to zero relative velocity
 	j1 = -vrel * contactPoint.m_jacDiagABInv;
 	btSetMin(j1, maxImpulse);
 	btSetMax(j1, -maxImpulse);

 	return j1;
 }


 btScalar sideFrictionStiffness2 = btScalar(1.0);
 void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 {

 		//calculate the impulse, so that the wheels don't move sidewards
 		int numWheel = getNumWheels();
 		if (!numWheel)
 			return;

 		m_forwardWS.resize(numWheel);
 		m_axle.resize(numWheel);
 		m_forwardImpulse.resize(numWheel);
 		m_sideImpulse.resize(numWheel);

 		int numWheelsOnGround = 0;


 		//collapse all those loops into one!
 		for (int i=0;i<getNumWheels();i++)
 		{
 			btWheelInfo& wheelInfo = m_wheelInfo[i];
 			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
 			if (groundObject)
 				numWheelsOnGround++;
 			m_sideImpulse[i] = btScalar(0.);
 			m_forwardImpulse[i] = btScalar(0.);

 		}

 		{

 			for (int i=0;i<getNumWheels();i++)
 			{

 				btWheelInfo& wheelInfo = m_wheelInfo[i];

 				class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;

 				if (groundObject)
 				{

 					const btTransform& wheelTrans = getWheelTransformWS( i );

 					btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
 					m_axle[i] = btVector3(
 						wheelBasis0[0][m_indexRightAxis],
 						wheelBasis0[1][m_indexRightAxis],
 						wheelBasis0[2][m_indexRightAxis]);

 					const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
 					btScalar proj = m_axle[i].dot(surfNormalWS);
 					m_axle[i] -= surfNormalWS * proj;
 					m_axle[i] = m_axle[i].normalize();

 					m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
 					m_forwardWS[i].normalize();


 					resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
 							  *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
 							  btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);

 					m_sideImpulse[i] *= sideFrictionStiffness2;

 				}


 			}
 		}

 	btScalar sideFactor = btScalar(1.);
 	btScalar fwdFactor = 0.5;

 	bool sliding = false;
 	{
 		for (int wheel =0;wheel <getNumWheels();wheel++)
 		{
 			btWheelInfo& wheelInfo = m_wheelInfo[wheel];
 			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;

 			btScalar	rollingFriction = 0.f;

 			if (groundObject)
 			{
 				if (wheelInfo.m_engineForce != 0.f)
 				{
 					rollingFriction = wheelInfo.m_engineForce* timeStep;
 				} else
 				{
 					btScalar defaultRollingFrictionImpulse = 0.f;
 					btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
 					btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
 					rollingFriction = calcRollingFriction(contactPt);
 				}
 			}

 			//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)


 			m_forwardImpulse[wheel] = btScalar(0.);
 			m_wheelInfo[wheel].m_skidInfo= btScalar(1.);

 			if (groundObject)
 			{
 				m_wheelInfo[wheel].m_skidInfo= btScalar(1.);

 				btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
 				btScalar maximpSide = maximp;

 				btScalar maximpSquared = maximp * maximpSide;


 				m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;

 				btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
 				btScalar y = (m_sideImpulse[wheel] ) * sideFactor;

 				btScalar impulseSquared = (x*x + y*y);

 				if (impulseSquared > maximpSquared)
 				{
 					sliding = true;

 					btScalar factor = maximp / btSqrt(impulseSquared);

 					m_wheelInfo[wheel].m_skidInfo *= factor;
 				}
 			}

 		}
 	}


 		if (sliding)
 		{
 			for (int wheel = 0;wheel < getNumWheels(); wheel++)
 			{
 				if (m_sideImpulse[wheel] != btScalar(0.))
 				{
 					if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
 					{
 						m_forwardImpulse[wheel] *=	m_wheelInfo[wheel].m_skidInfo;
 						m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
 					}
 				}
 			}
 		}

 		// apply the impulses
 		{
 			for (int wheel = 0;wheel<getNumWheels() ; wheel++)
 			{
 				btWheelInfo& wheelInfo = m_wheelInfo[wheel];

 				btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
 						m_chassisBody->getCenterOfMassPosition();

 				if (m_forwardImpulse[wheel] != btScalar(0.))
 				{
 					m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
 				}
 				if (m_sideImpulse[wheel] != btScalar(0.))
 				{
 					class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;

 					btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
 						groundObject->getCenterOfMassPosition();


 					btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];

 					rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
 					m_chassisBody->applyImpulse(sideImp,rel_pos);

 					//apply friction impulse on the ground
 					groundObject->applyImpulse(-sideImp,rel_pos2);
 				}
 			}
 		}


 }


 void	btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
 {

 	for (int v=0;v<this->getNumWheels();v++)
 	{
 		btVector3 wheelColor(0,255,255);
 		if (getWheelInfo(v).m_raycastInfo.m_isInContact)
 		{
 			wheelColor.setValue(0,0,255);
 		} else
 		{
 			wheelColor.setValue(255,0,255);
 		}

 		btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();

 		btVector3 axle = btVector3(
 			getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
 			getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
 			getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);

 		//debug wheels (cylinders)
 		debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
 		debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);

 	}
 }


 void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
 {
 //	RayResultCallback& resultCallback;

 	btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);

 	m_dynamicsWorld->rayTest(from, to, rayCallback);

 	if (rayCallback.hasHit())
 	{

 		btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
         if (body && body->hasContactResponse())
 		{
 			result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
 			result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
 			result.m_hitNormalInWorld.normalize();
 			result.m_distFraction = rayCallback.m_closestHitFraction;
 			return body;
 		}
 	}
 	return 0;
 }
	/*
	* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
	*
	* Permission to use, copy, modify, distribute and sell this software
	* and its documentation for any purpose is hereby granted without fee,
	* provided that the above copyright notice appear in all copies.
	* Erwin Coumans makes no representations about the suitability
	* of this software for any purpose.
	* It is provided "as is" without express or implied warranty.
	*/

	#include "LinearMath/btVector3.h"
	#include "BulletDynamics/Vehicle/btRaycastVehicle.h"

	#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
	#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
	#include "LinearMath/btQuaternion.h"
	#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
	#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
	#include "BulletDynamics/Vehicle/btWheelInfo.h"
	#include "LinearMath/btMinMax.h"
	#include "LinearMath/btIDebugDraw.h"
	#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"

	static btRigidBody s_fixedObject( 0,0,0);

	btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
	:m_vehicleRaycaster(raycaster),
	m_pitchControl(btScalar(0.))
	{
	m_chassisBody = chassis;
	m_indexRightAxis = 0;
	m_indexUpAxis = 2;
	m_indexForwardAxis = 1;
	defaultInit(tuning);
	}


	void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
	{
	(void)tuning;
	m_currentVehicleSpeedKmHour = btScalar(0.);
	m_steeringValue = btScalar(0.);

	}



	btRaycastVehicle::~btRaycastVehicle()
	{
	}


	//
	// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
	//
	btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
	{

	btWheelInfoConstructionInfo ci;

	ci.m_chassisConnectionCS = connectionPointCS;
	ci.m_wheelDirectionCS = wheelDirectionCS0;
	ci.m_wheelAxleCS = wheelAxleCS;
	ci.m_suspensionRestLength = suspensionRestLength;
	ci.m_wheelRadius = wheelRadius;
	ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
	ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
	ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
	ci.m_frictionSlip = tuning.m_frictionSlip;
	ci.m_bIsFrontWheel = isFrontWheel;
	ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;

	m_wheelInfo.push_back( btWheelInfo(ci));

	btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];

	updateWheelTransformsWS( wheel , false );
	updateWheelTransform(getNumWheels()-1,false);
	return wheel;
	}




	const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
	{
	btAssert(wheelIndex < getNumWheels());
	const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
	return wheel.m_worldTransform;

	}

	void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
	{

	btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
	updateWheelTransformsWS(wheel,interpolatedTransform);
	btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
	const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
	btVector3 fwd = up.cross(right);
	fwd = fwd.normalize();
	// up = right.cross(fwd);
	// up.normalize();

	//rotate around steering over de wheelAxleWS
	btScalar steering = wheel.m_steering;

	btQuaternion steeringOrn(up,steering);//wheel.m_steering);
	btMatrix3x3 steeringMat(steeringOrn);

	btQuaternion rotatingOrn(right,-wheel.m_rotation);
	btMatrix3x3 rotatingMat(rotatingOrn);

	btMatrix3x3 basis2(
	right[0],fwd[0],up[0],
	right[1],fwd[1],up[1],
	right[2],fwd[2],up[2]
	);

	wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
	wheel.m_worldTransform.setOrigin(
	wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
	);
	}

	void btRaycastVehicle::resetSuspension()
	{

	int i;
	for (i=0;i<m_wheelInfo.size(); i++)
	{
	btWheelInfo& wheel = m_wheelInfo[i];
	wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
	wheel.m_suspensionRelativeVelocity = btScalar(0.0);

	wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
	//wheel_info.setContactFriction(btScalar(0.0));
	wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
	}
	}

	void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
	{
	wheel.m_raycastInfo.m_isInContact = false;

	btTransform chassisTrans = getChassisWorldTransform();
	if (interpolatedTransform && (getRigidBody()->getMotionState()))
	{
	getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
	}

	wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
	wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ;
	wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
	}

	btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
	{
	updateWheelTransformsWS( wheel,false);


	btScalar depth = -1;

	btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;

	btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
	const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
	wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
	const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;

	btScalar param = btScalar(0.);

	btVehicleRaycaster::btVehicleRaycasterResult rayResults;

	btAssert(m_vehicleRaycaster);

	void* object = m_vehicleRaycaster->castRay(source,target,rayResults);

	wheel.m_raycastInfo.m_groundObject = 0;

	if (object)
	{
	param = rayResults.m_distFraction;
	depth = raylen * rayResults.m_distFraction;
	wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
	wheel.m_raycastInfo.m_isInContact = true;

	wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!;
	//wheel.m_raycastInfo.m_groundObject = object;


	btScalar hitDistance = param*raylen;
	wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
	//clamp on max suspension travel

	btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
	btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);
	if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
	{
	wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
	}
	if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
	{
	wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
	}

	wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;

	btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );

	btVector3 chassis_velocity_at_contactPoint;
	btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();

	chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);

	btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );

	if ( denominator >= btScalar(-0.1))
	{
	wheel.m_suspensionRelativeVelocity = btScalar(0.0);
	wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
	}
	else
	{
	btScalar inv = btScalar(-1.) / denominator;
	wheel.m_suspensionRelativeVelocity = projVel * inv;
	wheel.m_clippedInvContactDotSuspension = inv;
	}

	} else
	{
	//put wheel info as in rest position
	wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
	wheel.m_suspensionRelativeVelocity = btScalar(0.0);
	wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
	wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
	}

	return depth;
	}


	const btTransform& btRaycastVehicle::getChassisWorldTransform() const
	{
	/*if (getRigidBody()->getMotionState())
	{
	btTransform chassisWorldTrans;
	getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
	return chassisWorldTrans;
	}
	*/


	return getRigidBody()->getCenterOfMassTransform();
	}


	void btRaycastVehicle::updateVehicle( btScalar step )
	{
	{
	for (int i=0;i<getNumWheels();i++)
	{
	updateWheelTransform(i,false);
	}
	}


	m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();

	const btTransform& chassisTrans = getChassisWorldTransform();

	btVector3 forwardW (
	chassisTrans.getBasis()[0][m_indexForwardAxis],
	chassisTrans.getBasis()[1][m_indexForwardAxis],
	chassisTrans.getBasis()[2][m_indexForwardAxis]);

	if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
	{
	m_currentVehicleSpeedKmHour *= btScalar(-1.);
	}

	//
	// simulate suspension
	//

	int i=0;
	for (i=0;i<m_wheelInfo.size();i++)
	{
	btScalar depth;
	depth = rayCast( m_wheelInfo[i]);
	}

	updateSuspension(step);


	for (i=0;i<m_wheelInfo.size();i++)
	{
	//apply suspension force
	btWheelInfo& wheel = m_wheelInfo[i];

	btScalar suspensionForce = wheel.m_wheelsSuspensionForce;

	btScalar gMaxSuspensionForce = btScalar(6000.);
	if (suspensionForce > gMaxSuspensionForce)
	{
	suspensionForce = gMaxSuspensionForce;
	}
	btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
	btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();

	getRigidBody()->applyImpulse(impulse, relpos);

	}



	updateFriction( step);


	for (i=0;i<m_wheelInfo.size();i++)
	{
	btWheelInfo& wheel = m_wheelInfo[i];
	btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
	btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );

	if (wheel.m_raycastInfo.m_isInContact)
	{
	const btTransform& chassisWorldTransform = getChassisWorldTransform();

	btVector3 fwd (
	chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
	chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
	chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);

	btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
	fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;

	btScalar proj2 = fwd.dot(vel);

	wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
	wheel.m_rotation += wheel.m_deltaRotation;

	} else
	{
	wheel.m_rotation += wheel.m_deltaRotation;
	}

	wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact

	}



	}


	void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
	{
	btAssert(wheel>=0 && wheel < getNumWheels());

	btWheelInfo& wheelInfo = getWheelInfo(wheel);
	wheelInfo.m_steering = steering;
	}



	btScalar btRaycastVehicle::getSteeringValue(int wheel) const
	{
	return getWheelInfo(wheel).m_steering;
	}


	void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
	{
	btAssert(wheel>=0 && wheel < getNumWheels());
	btWheelInfo& wheelInfo = getWheelInfo(wheel);
	wheelInfo.m_engineForce = force;
	}


	const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const
	{
	btAssert((index >= 0) && (index < getNumWheels()));

	return m_wheelInfo[index];
	}

	btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
	{
	btAssert((index >= 0) && (index < getNumWheels()));

	return m_wheelInfo[index];
	}

	void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
	{
	btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
	getWheelInfo(wheelIndex).m_brake = brake;
	}


	void btRaycastVehicle::updateSuspension(btScalar deltaTime)
	{
	(void)deltaTime;

	btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();

	for (int w_it=0; w_it<getNumWheels(); w_it++)
	{
	btWheelInfo &wheel_info = m_wheelInfo[w_it];

	if ( wheel_info.m_raycastInfo.m_isInContact )
	{
	btScalar force;
	// Spring
	{
	btScalar susp_length = wheel_info.getSuspensionRestLength();
	btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;

	btScalar length_diff = (susp_length - current_length);

	force = wheel_info.m_suspensionStiffness
	* length_diff * wheel_info.m_clippedInvContactDotSuspension;
	}

	// Damper
	{
	btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
	{
	btScalar susp_damping;
	if ( projected_rel_vel < btScalar(0.0) )
	{
	susp_damping = wheel_info.m_wheelsDampingCompression;
	}
	else
	{
	susp_damping = wheel_info.m_wheelsDampingRelaxation;
	}
	force -= susp_damping * projected_rel_vel;
	}
	}

	// RESULT
	wheel_info.m_wheelsSuspensionForce = force * chassisMass;
	if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
	{
	wheel_info.m_wheelsSuspensionForce = btScalar(0.);
	}
	}
	else
	{
	wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
	}
	}

	}


	struct btWheelContactPoint
	{
	btRigidBody* m_body0;
	btRigidBody* m_body1;
	btVector3 m_frictionPositionWorld;
	btVector3 m_frictionDirectionWorld;
	btScalar m_jacDiagABInv;
	btScalar m_maxImpulse;


	btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
	:m_body0(body0),
	m_body1(body1),
	m_frictionPositionWorld(frictionPosWorld),
	m_frictionDirectionWorld(frictionDirectionWorld),
	m_maxImpulse(maxImpulse)
	{
	btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
	btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
	btScalar relaxation = 1.f;
	m_jacDiagABInv = relaxation/(denom0+denom1);
	}



	};

	btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
	btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
	{

	btScalar j1=0.f;

	const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;

	btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition();
	btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();

	btScalar maxImpulse = contactPoint.m_maxImpulse;

	btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
	btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
	btVector3 vel = vel1 - vel2;

	btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);

	// calculate j that moves us to zero relative velocity
	j1 = -vrel * contactPoint.m_jacDiagABInv;
	btSetMin(j1, maxImpulse);
	btSetMax(j1, -maxImpulse);

	return j1;
	}




	btScalar sideFrictionStiffness2 = btScalar(1.0);
	void btRaycastVehicle::updateFriction(btScalar timeStep)
	{

	//calculate the impulse, so that the wheels don't move sidewards
	int numWheel = getNumWheels();
	if (!numWheel)
	return;

	m_forwardWS.resize(numWheel);
	m_axle.resize(numWheel);
	m_forwardImpulse.resize(numWheel);
	m_sideImpulse.resize(numWheel);

	int numWheelsOnGround = 0;


	//collapse all those loops into one!
	for (int i=0;i<getNumWheels();i++)
	{
	btWheelInfo& wheelInfo = m_wheelInfo[i];
	class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
	if (groundObject)
	numWheelsOnGround++;
	m_sideImpulse[i] = btScalar(0.);
	m_forwardImpulse[i] = btScalar(0.);

	}

	{

	for (int i=0;i<getNumWheels();i++)
	{

	btWheelInfo& wheelInfo = m_wheelInfo[i];

	class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;

	if (groundObject)
	{

	const btTransform& wheelTrans = getWheelTransformWS( i );

	btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
	m_axle[i] = btVector3(
	wheelBasis0[0][m_indexRightAxis],
	wheelBasis0[1][m_indexRightAxis],
	wheelBasis0[2][m_indexRightAxis]);

	const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
	btScalar proj = m_axle[i].dot(surfNormalWS);
	m_axle[i] -= surfNormalWS * proj;
	m_axle[i] = m_axle[i].normalize();

	m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
	m_forwardWS[i].normalize();


	resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
	*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
	btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);

	m_sideImpulse[i] *= sideFrictionStiffness2;

	}


	}
	}

	btScalar sideFactor = btScalar(1.);
	btScalar fwdFactor = 0.5;

	bool sliding = false;
	{
	for (int wheel =0;wheel <getNumWheels();wheel++)
	{
	btWheelInfo& wheelInfo = m_wheelInfo[wheel];
	class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;

	btScalar rollingFriction = 0.f;

	if (groundObject)
	{
	if (wheelInfo.m_engineForce != 0.f)
	{
	rollingFriction = wheelInfo.m_engineForce* timeStep;
	} else
	{
	btScalar defaultRollingFrictionImpulse = 0.f;
	btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
	btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
	rollingFriction = calcRollingFriction(contactPt);
	}
	}

	//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)




	m_forwardImpulse[wheel] = btScalar(0.);
	m_wheelInfo[wheel].m_skidInfo= btScalar(1.);

	if (groundObject)
	{
	m_wheelInfo[wheel].m_skidInfo= btScalar(1.);

	btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
	btScalar maximpSide = maximp;

	btScalar maximpSquared = maximp * maximpSide;


	m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;

	btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
	btScalar y = (m_sideImpulse[wheel] ) * sideFactor;

	btScalar impulseSquared = (xx + yy);

	if (impulseSquared > maximpSquared)
	{
	sliding = true;

	btScalar factor = maximp / btSqrt(impulseSquared);

	m_wheelInfo[wheel].m_skidInfo *= factor;
	}
	}

	}
	}




	if (sliding)
	{
	for (int wheel = 0;wheel < getNumWheels(); wheel++)
	{
	if (m_sideImpulse[wheel] != btScalar(0.))
	{
	if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
	{
	m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
	m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
	}
	}
	}
	}

	// apply the impulses
	{
	for (int wheel = 0;wheel<getNumWheels() ; wheel++)
	{
	btWheelInfo& wheelInfo = m_wheelInfo[wheel];

	btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
	m_chassisBody->getCenterOfMassPosition();

	if (m_forwardImpulse[wheel] != btScalar(0.))
	{
	m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
	}
	if (m_sideImpulse[wheel] != btScalar(0.))
	{
	class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;

	btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
	groundObject->getCenterOfMassPosition();


	btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];

	rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
	m_chassisBody->applyImpulse(sideImp,rel_pos);

	//apply friction impulse on the ground
	groundObject->applyImpulse(-sideImp,rel_pos2);
	}
	}
	}


	}



	void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
	{

	for (int v=0;v<this->getNumWheels();v++)
	{
	btVector3 wheelColor(0,255,255);
	if (getWheelInfo(v).m_raycastInfo.m_isInContact)
	{
	wheelColor.setValue(0,0,255);
	} else
	{
	wheelColor.setValue(255,0,255);
	}

	btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();

	btVector3 axle = btVector3(
	getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
	getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
	getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);

	//debug wheels (cylinders)
	debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
	debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);

	}
	}


	void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
	{
	// RayResultCallback& resultCallback;

	btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);

	m_dynamicsWorld->rayTest(from, to, rayCallback);

	if (rayCallback.hasHit())
	{

	btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
	if (body && body->hasContactResponse())
	{
	result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
	result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
	result.m_hitNormalInWorld.normalize();
	result.m_distFraction = rayCallback.m_closestHitFraction;
	return body;
	}
	}
	return 0;
	}