ruby-bullet 0.0.2-x86-mingw32

data/deps/include/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp

@@ -0,0 +1,771 @@
+/*
+ * 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 "btRaycastVehicle.h"
+
+#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
+#include "LinearMath/btQuaternion.h"
+#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
+#include "btVehicleRaycaster.h"
+#include "btWheelInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+
+#define ROLLING_INFLUENCE_FIX
+
+
+btRigidBody& btActionInterface::getFixedBody()
+{
+	static btRigidBody s_fixed(0, 0,0);
+	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+	return s_fixed;
+}
+
+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;
+	ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
+
+	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 = &getFixedBody();///@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;
+		
+		if (suspensionForce > wheel.m_maxSuspensionForce)
+		{
+			suspensionForce = wheel.m_maxSuspensionForce;
+		}
+		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];
+
+#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
+					btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
+					rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
+#else
+					rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
+#endif
+					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,1,1);
+		if (getWheelInfo(v).m_raycastInfo.m_isInContact)
+		{
+			wheelColor.setValue(0,0,1);
+		} else
+		{
+			wheelColor.setValue(1,0,1);
+		}
+
+		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;
+}
+