Back to index

supertuxkart  0.5+dfsg1
btRaycastVehicle.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
00003  *
00004  * Permission to use, copy, modify, distribute and sell this software
00005  * and its documentation for any purpose is hereby granted without fee,
00006  * provided that the above copyright notice appear in all copies.
00007  * Erwin Coumans makes no representations about the suitability 
00008  * of this software for any purpose.  
00009  * It is provided "as is" without express or implied warranty.
00010 */
00011 
00012 #include "LinearMath/btVector3.h"
00013 #include "btRaycastVehicle.h"
00014 
00015 #include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
00016 #include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
00017 #include "LinearMath/btQuaternion.h"
00018 #include "BulletDynamics/Dynamics/btDynamicsWorld.h"
00019 #include "btVehicleRaycaster.h"
00020 #include "btWheelInfo.h"
00021 #include "LinearMath/btMinMax.h"
00022 
00023 
00024 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
00025 
00026 static btRigidBody s_fixedObject( 0,0,0);
00027 
00028 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,     btVehicleRaycaster* raycaster )
00029 : btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
00030 m_vehicleRaycaster(raycaster),
00031 m_pitchControl(btScalar(0.))
00032 {
00033        m_chassisBody = chassis;
00034        m_indexRightAxis = 0;
00035        m_indexUpAxis = 2;
00036        m_indexForwardAxis = 1;
00037        defaultInit(tuning);
00038 }
00039 
00040 
00041 void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
00042 {
00043        (void)tuning;
00044        m_currentVehicleSpeedKmHour = btScalar(0.);
00045        m_steeringValue = btScalar(0.);
00046        
00047 }
00048 
00049        
00050 
00051 btRaycastVehicle::~btRaycastVehicle()
00052 {
00053 }
00054 
00055 
00056 //
00057 // basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
00058 //
00059 btWheelInfo&  btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
00060 {
00061 
00062        btWheelInfoConstructionInfo ci;
00063 
00064        ci.m_chassisConnectionCS = connectionPointCS;
00065        ci.m_wheelDirectionCS = wheelDirectionCS0;
00066        ci.m_wheelAxleCS = wheelAxleCS;
00067        ci.m_suspensionRestLength = suspensionRestLength;
00068        ci.m_wheelRadius = wheelRadius;
00069        ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
00070        ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
00071        ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
00072        ci.m_frictionSlip = tuning.m_frictionSlip;
00073        ci.m_bIsFrontWheel = isFrontWheel;
00074        ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
00075 
00076        m_wheelInfo.push_back( btWheelInfo(ci));
00077        
00078        btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];
00079        
00080        updateWheelTransformsWS( wheel , false );
00081        updateWheelTransform(getNumWheels()-1,false);
00082        return wheel;
00083 }
00084 
00085 
00086 
00087 
00088 const btTransform&   btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
00089 {
00090        assert(wheelIndex < getNumWheels());
00091        const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
00092        return wheel.m_worldTransform;
00093 
00094 }
00095 
00096 void   btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
00097 {
00098        
00099        btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
00100        updateWheelTransformsWS(wheel,interpolatedTransform);
00101        btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
00102        const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
00103        btVector3 fwd = up.cross(right);
00104        fwd = fwd.normalize();
00105 //     up = right.cross(fwd);
00106 //     up.normalize();
00107 
00108        //rotate around steering over de wheelAxleWS
00109        btScalar steering = wheel.m_steering;
00110        
00111        btQuaternion steeringOrn(up,steering);//wheel.m_steering);
00112        btMatrix3x3 steeringMat(steeringOrn);
00113 
00114        btQuaternion rotatingOrn(right,-wheel.m_rotation);
00115        btMatrix3x3 rotatingMat(rotatingOrn);
00116 
00117        btMatrix3x3 basis2(
00118               right[0],fwd[0],up[0],
00119               right[1],fwd[1],up[1],
00120               right[2],fwd[2],up[2]
00121        );
00122        
00123        wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
00124        wheel.m_worldTransform.setOrigin(
00125               wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
00126        );
00127 }
00128 
00129 void btRaycastVehicle::resetSuspension()
00130 {
00131 
00132        int i;
00133        for (i=0;i<m_wheelInfo.size();     i++)
00134        {
00135                      btWheelInfo& wheel = m_wheelInfo[i];
00136                      wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
00137                      wheel.m_suspensionRelativeVelocity = btScalar(0.0);
00138                      
00139                      wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
00140                      //wheel_info.setContactFriction(btScalar(0.0));
00141                      wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
00142        }
00143 }
00144 
00145 void   btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
00146 {
00147        wheel.m_raycastInfo.m_isInContact = false;
00148 
00149        btTransform chassisTrans = getChassisWorldTransform();
00150        if (interpolatedTransform && (getRigidBody()->getMotionState()))
00151        {
00152               getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
00153        }
00154 
00155        wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
00156        wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() *  wheel.m_wheelDirectionCS ;
00157        wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
00158 }
00159 
00160 btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
00161 {
00162        updateWheelTransformsWS( wheel,false);
00163 
00164        
00165        btScalar depth = -1;
00166        
00167        btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;
00168 
00169        btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
00170        const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
00171        wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
00172        const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
00173 
00174        btScalar param = btScalar(0.);
00175        
00176        btVehicleRaycaster::btVehicleRaycasterResult     rayResults;
00177 
00178        assert(m_vehicleRaycaster);
00179 
00180        void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
00181 
00182        wheel.m_raycastInfo.m_groundObject = 0;
00183 
00184        if (object)
00185        {
00186               param = rayResults.m_distFraction;
00187               depth = raylen * rayResults.m_distFraction;
00188               wheel.m_raycastInfo.m_contactNormalWS  = rayResults.m_hitNormalInWorld;
00189               wheel.m_raycastInfo.m_isInContact = true;
00190               
00191               wheel.m_raycastInfo.m_groundObject = &s_fixedObject;//todo for driving on dynamic/movable objects!;
00192               //wheel.m_raycastInfo.m_groundObject = object;
00193 
00194 
00195               btScalar hitDistance = param*raylen;
00196               wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
00197               //clamp on max suspension travel
00198 
00199               btScalar  minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
00200               btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);
00201               if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
00202               {
00203                      wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
00204               }
00205               if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
00206               {
00207                      wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
00208               }
00209 
00210               wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
00211 
00212               btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );
00213 
00214               btVector3 chassis_velocity_at_contactPoint;
00215               btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();
00216 
00217               chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
00218 
00219               btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
00220 
00221               if ( denominator >= btScalar(-0.1))
00222               {
00223                      wheel.m_suspensionRelativeVelocity = btScalar(0.0);
00224                      wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
00225               }
00226               else
00227               {
00228                      btScalar inv = btScalar(-1.) / denominator;
00229                      wheel.m_suspensionRelativeVelocity = projVel * inv;
00230                      wheel.m_clippedInvContactDotSuspension = inv;
00231               }
00232                      
00233        } else
00234        {
00235               //put wheel info as in rest position
00236               wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
00237               wheel.m_suspensionRelativeVelocity = btScalar(0.0);
00238               wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
00239               wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
00240        }
00241 
00242        return depth;
00243 }
00244 
00245 
00246 const btTransform& btRaycastVehicle::getChassisWorldTransform() const
00247 {
00248        /*if (getRigidBody()->getMotionState())
00249        {
00250               btTransform chassisWorldTrans;
00251               getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
00252               return chassisWorldTrans;
00253        }
00254        */
00255 
00256        
00257        return getRigidBody()->getCenterOfMassTransform();
00258 }
00259 
00260 
00261 void btRaycastVehicle::updateVehicle( btScalar step )
00262 {
00263        {
00264               for (int i=0;i<getNumWheels();i++)
00265               {
00266                      updateWheelTransform(i,false);
00267               }
00268        }
00269 
00270 
00271        m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
00272        
00273        const btTransform& chassisTrans = getChassisWorldTransform();
00274 
00275        btVector3 forwardW (
00276               chassisTrans.getBasis()[0][m_indexForwardAxis],
00277               chassisTrans.getBasis()[1][m_indexForwardAxis],
00278               chassisTrans.getBasis()[2][m_indexForwardAxis]);
00279 
00280        if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
00281        {
00282               m_currentVehicleSpeedKmHour *= btScalar(-1.);
00283        }
00284 
00285        //
00286        // simulate suspension
00287        //
00288        
00289        int i=0;
00290        for (i=0;i<m_wheelInfo.size();i++)
00291        {
00292               btScalar depth; 
00293               depth = rayCast( m_wheelInfo[i]);
00294        }
00295 
00296        updateSuspension(step);
00297 
00298        
00299        for (i=0;i<m_wheelInfo.size();i++)
00300        {
00301               //apply suspension force
00302               btWheelInfo& wheel = m_wheelInfo[i];
00303               
00304               btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
00305               
00306               btScalar gMaxSuspensionForce = btScalar(6000.);
00307               if (suspensionForce > gMaxSuspensionForce)
00308               {
00309                      suspensionForce = gMaxSuspensionForce;
00310               }
00311               btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
00312               btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
00313               
00314               getRigidBody()->applyImpulse(impulse, relpos);
00315        
00316        }
00317        
00318 
00319        
00320        updateFriction( step);
00321 
00322        
00323        for (i=0;i<m_wheelInfo.size();i++)
00324        {
00325               btWheelInfo& wheel = m_wheelInfo[i];
00326               btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
00327               btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
00328 
00329               if (wheel.m_raycastInfo.m_isInContact)
00330               {
00331                      const btTransform&   chassisWorldTransform = getChassisWorldTransform();
00332 
00333                      btVector3 fwd (
00334                             chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
00335                             chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
00336                             chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
00337 
00338                      btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
00339                      fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
00340 
00341                      btScalar proj2 = fwd.dot(vel);
00342                      
00343                      wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
00344                      wheel.m_rotation += wheel.m_deltaRotation;
00345 
00346               } else
00347               {
00348                      wheel.m_rotation += wheel.m_deltaRotation;
00349               }
00350               
00351               wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
00352 
00353        }
00354 
00355 
00356 
00357 }
00358 
00359 
00360 void   btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
00361 {
00362        assert(wheel>=0 && wheel < getNumWheels());
00363 
00364        btWheelInfo& wheelInfo = getWheelInfo(wheel);
00365        wheelInfo.m_steering = steering;
00366 }
00367 
00368 
00369 
00370 btScalar      btRaycastVehicle::getSteeringValue(int wheel) const
00371 {
00372        return getWheelInfo(wheel).m_steering;
00373 }
00374 
00375 
00376 void   btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
00377 {
00378        assert(wheel>=0 && wheel < getNumWheels());
00379        btWheelInfo& wheelInfo = getWheelInfo(wheel);
00380        wheelInfo.m_engineForce = force;
00381 }
00382 
00383 
00384 const btWheelInfo&   btRaycastVehicle::getWheelInfo(int index) const
00385 {
00386        btAssert((index >= 0) && (index <  getNumWheels()));
00387        
00388        return m_wheelInfo[index];
00389 }
00390 
00391 btWheelInfo&  btRaycastVehicle::getWheelInfo(int index) 
00392 {
00393        btAssert((index >= 0) && (index <  getNumWheels()));
00394        
00395        return m_wheelInfo[index];
00396 }
00397 
00398 void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
00399 {
00400        btAssert((wheelIndex >= 0) && (wheelIndex <      getNumWheels()));
00401        getWheelInfo(wheelIndex).m_brake = brake;
00402 }
00403 
00404 
00405 void   btRaycastVehicle::updateSuspension(btScalar deltaTime)
00406 {
00407        (void)deltaTime;
00408 
00409        btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
00410        
00411        for (int w_it=0; w_it<getNumWheels(); w_it++)
00412        {
00413               btWheelInfo &wheel_info = m_wheelInfo[w_it];
00414               
00415               if ( wheel_info.m_raycastInfo.m_isInContact )
00416               {
00417                      btScalar force;
00418                      //     Spring
00419                      {
00420                             btScalar      susp_length                 = wheel_info.getSuspensionRestLength();
00421                             btScalar      current_length = wheel_info.m_raycastInfo.m_suspensionLength;
00422 
00423                             btScalar length_diff = (susp_length - current_length);
00424 
00425                             force = wheel_info.m_suspensionStiffness
00426                                    * length_diff * wheel_info.m_clippedInvContactDotSuspension;
00427                      }
00428               
00429                      // Damper
00430                      {
00431                             btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
00432                             {
00433                                    btScalar      susp_damping;
00434                                    if ( projected_rel_vel < btScalar(0.0) )
00435                                    {
00436                                           susp_damping = wheel_info.m_wheelsDampingCompression;
00437                                    }
00438                                    else
00439                                    {
00440                                           susp_damping = wheel_info.m_wheelsDampingRelaxation;
00441                                    }
00442                                    force -= susp_damping * projected_rel_vel;
00443                             }
00444                      }
00445 
00446                      // RESULT
00447                      wheel_info.m_wheelsSuspensionForce = force * chassisMass;
00448                      if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
00449                      {
00450                             wheel_info.m_wheelsSuspensionForce = btScalar(0.);
00451                      }
00452               }
00453               else
00454               {
00455                      wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
00456               }
00457        }
00458 
00459 }
00460 
00461 
00462 struct btWheelContactPoint
00463 {
00464        btRigidBody* m_body0;
00465        btRigidBody* m_body1;
00466        btVector3     m_frictionPositionWorld;
00467        btVector3     m_frictionDirectionWorld;
00468        btScalar      m_jacDiagABInv;
00469        btScalar      m_maxImpulse;
00470 
00471 
00472        btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
00473               :m_body0(body0),
00474               m_body1(body1),
00475               m_frictionPositionWorld(frictionPosWorld),
00476               m_frictionDirectionWorld(frictionDirectionWorld),
00477               m_maxImpulse(maxImpulse)
00478        {
00479               btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
00480               btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
00481               btScalar      relaxation = 1.f;
00482               m_jacDiagABInv = relaxation/(denom0+denom1);
00483        }
00484 
00485 
00486 
00487 };
00488 
00489 btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
00490 {
00491 
00492        btScalar j1=0.f;
00493 
00494        const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
00495 
00496        btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); 
00497        btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
00498        
00499        btScalar maxImpulse  = contactPoint.m_maxImpulse;
00500        
00501        btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
00502        btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
00503        btVector3 vel = vel1 - vel2;
00504 
00505        btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
00506 
00507        // calculate j that moves us to zero relative velocity
00508        j1 = -vrel * contactPoint.m_jacDiagABInv;
00509        btSetMin(j1, maxImpulse);
00510        btSetMax(j1, -maxImpulse);
00511 
00512        return j1;
00513 }
00514 
00515 
00516 
00517 
00518 btScalar sideFrictionStiffness2 = btScalar(1.0);
00519 void   btRaycastVehicle::updateFriction(btScalar timeStep)
00520 {
00521 
00522               //calculate the impulse, so that the wheels don't move sidewards
00523               int numWheel = getNumWheels();
00524               if (!numWheel)
00525                      return;
00526 
00527               m_forwardWS.resize(numWheel);
00528               m_axle.resize(numWheel);
00529               m_forwardImpulse.resize(numWheel);
00530               m_sideImpulse.resize(numWheel);
00531               
00532               int numWheelsOnGround = 0;
00533        
00534 
00535               //collapse all those loops into one!
00536               for (int i=0;i<getNumWheels();i++)
00537               {
00538                      btWheelInfo& wheelInfo = m_wheelInfo[i];
00539                      class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
00540                      if (groundObject)
00541                             numWheelsOnGround++;
00542                      m_sideImpulse[i] = btScalar(0.);
00543                      m_forwardImpulse[i] = btScalar(0.);
00544 
00545               }
00546        
00547               {
00548        
00549                      for (int i=0;i<getNumWheels();i++)
00550                      {
00551 
00552                             btWheelInfo& wheelInfo = m_wheelInfo[i];
00553                                    
00554                             class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
00555 
00556                             if (groundObject)
00557                             {
00558 
00559                                    const btTransform& wheelTrans = getWheelTransformWS( i );
00560 
00561                                    btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
00562                                    m_axle[i] = btVector3(      
00563                                           wheelBasis0[0][m_indexRightAxis],
00564                                           wheelBasis0[1][m_indexRightAxis],
00565                                           wheelBasis0[2][m_indexRightAxis]);
00566                                    
00567                                    const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
00568                                    btScalar proj = m_axle[i].dot(surfNormalWS);
00569                                    m_axle[i] -= surfNormalWS * proj;
00570                                    m_axle[i] = m_axle[i].normalize();
00571                                    
00572                                    m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
00573                                    m_forwardWS[i].normalize();
00574 
00575                             
00576                                    resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
00577                                                    *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
00578                                                    btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
00579 
00580                                    m_sideImpulse[i] *= sideFrictionStiffness2;
00581                                           
00582                             }
00583                             
00584 
00585                      }
00586               }
00587 
00588        btScalar sideFactor = btScalar(1.);
00589        btScalar fwdFactor = 0.5;
00590 
00591        bool sliding = false;
00592        {
00593               for (int wheel =0;wheel <getNumWheels();wheel++)
00594               {
00595                      btWheelInfo& wheelInfo = m_wheelInfo[wheel];
00596                      class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
00597 
00598                      btScalar      rollingFriction = 0.f;
00599 
00600                      if (groundObject)
00601                      {
00602                             if (wheelInfo.m_engineForce != 0.f)
00603                             {
00604                                    rollingFriction = wheelInfo.m_engineForce* timeStep;
00605                             } else
00606                             {
00607                                    btScalar defaultRollingFrictionImpulse = 0.f;
00608                                    btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
00609                                    btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
00610                                    rollingFriction = calcRollingFriction(contactPt);
00611                             }
00612                      }
00613 
00614                      //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
00615                      
00616 
00617 
00618 
00619                      m_forwardImpulse[wheel] = btScalar(0.);
00620                      m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
00621 
00622                      if (groundObject)
00623                      {
00624                             m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
00625                             
00626                             btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
00627                             btScalar maximpSide = maximp;
00628 
00629                             btScalar maximpSquared = maximp * maximpSide;
00630                      
00631 
00632                             m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
00633 
00634                             btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
00635                             btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
00636                             
00637                             btScalar impulseSquared = (x*x + y*y);
00638 
00639                             if (impulseSquared > maximpSquared)
00640                             {
00641                                    sliding = true;
00642                                    
00643                                    btScalar factor = maximp / btSqrt(impulseSquared);
00644                                    
00645                                    m_wheelInfo[wheel].m_skidInfo *= factor;
00646                             }
00647                      } 
00648 
00649               }
00650        }
00651 
00652        
00653 
00654 
00655               if (sliding)
00656               {
00657                      for (int wheel = 0;wheel < getNumWheels(); wheel++)
00658                      {
00659                             if (m_sideImpulse[wheel] != btScalar(0.))
00660                             {
00661                                    if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
00662                                    {
00663                                           m_forwardImpulse[wheel] *=  m_wheelInfo[wheel].m_skidInfo;
00664                                           m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
00665                                    }
00666                             }
00667                      }
00668               }
00669 
00670               // apply the impulses
00671               {
00672                      for (int wheel = 0;wheel<getNumWheels() ; wheel++)
00673                      {
00674                             btWheelInfo& wheelInfo = m_wheelInfo[wheel];
00675 
00676                             btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - 
00677                                           m_chassisBody->getCenterOfMassPosition();
00678 
00679                             if (m_forwardImpulse[wheel] != btScalar(0.))
00680                             {
00681                                    m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
00682                             }
00683                             if (m_sideImpulse[wheel] != btScalar(0.))
00684                             {
00685                                    class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
00686 
00687                                    btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - 
00688                                           groundObject->getCenterOfMassPosition();
00689 
00690                                    
00691                                    btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
00692 
00693                                    rel_pos[2] *= wheelInfo.m_rollInfluence;
00694                                    m_chassisBody->applyImpulse(sideImp,rel_pos);
00695 
00696                                    //apply friction impulse on the ground
00697                                    groundObject->applyImpulse(-sideImp,rel_pos2);
00698                             }
00699                      }
00700               }
00701 
00702        
00703 }
00704 
00705 
00706 void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
00707 {
00708 //     RayResultCallback& resultCallback;
00709 
00710        btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);
00711 
00712        m_dynamicsWorld->rayTest(from, to, rayCallback);
00713 
00714        if (rayCallback.HasHit())
00715        {
00716               
00717               btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
00718               if (body)
00719               {
00720                      result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
00721                      result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
00722                      result.m_hitNormalInWorld.normalize();
00723                      result.m_distFraction = rayCallback.m_closestHitFraction;
00724                      return body;
00725               }
00726        }
00727        return 0;
00728 }