00001
00002
00003
00004
00005
00006
00007
00008
00009
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
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
00113
00114
00115
00116 btScalar steering = wheel.m_steering;
00117
00118 btQuaternion steeringOrn(up,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
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
00200
00201
00202 btScalar hitDistance = param*raylen;
00203 wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
00204
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
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
00256
00257
00258
00259
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
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
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);
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
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
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
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
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
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
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
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;
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
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
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
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
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