source: code/branches/plehmannFS16/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp @ 11137

/*
 * 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;
}