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