Logo Search packages:      
Sourcecode: blender version File versions  Download package

btRaycastVehicle.cpp

/*
 * 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 "BulletDynamics/ConstraintSolver/btContactConstraint.h"

static btRigidBody s_fixedObject( 0,0,0);

btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,    btVehicleRaycaster* raycaster )
: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
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
{
      assert(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;

      assert(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)
{
      assert(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)
{
      assert(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[2] *= wheelInfo.m_rollInfluence;
                              m_chassisBody->applyImpulse(sideImp,rel_pos);

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

      
}


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

Generated by  Doxygen 1.6.0   Back to index