dali-physics/third-party/bullet3/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp

   1 /*
   2  * Copyright (c) 2005 Erwin Coumans https://bulletphysics.org
   3  *
   4  * Permission to use, copy, modify, distribute and sell this software
   5  * and its documentation for any purpose is hereby granted without fee,
   6  * provided that the above copyright notice appear in all copies.
   7  * Erwin Coumans makes no representations about the suitability
   8  * of this software for any purpose.
   9  * It is provided "as is" without express or implied warranty.
  10 */
  11
  12 #include "LinearMath/btVector3.h"
  13 #include "btRaycastVehicle.h"
  14
  15 #include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
  16 #include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
  17 #include "LinearMath/btQuaternion.h"
  18 #include "BulletDynamics/Dynamics/btDynamicsWorld.h"
  19 #include "btVehicleRaycaster.h"
  20 #include "btWheelInfo.h"
  21 #include "LinearMath/btMinMax.h"
  22 #include "LinearMath/btIDebugDraw.h"
  23 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
  24
  25 #define ROLLING_INFLUENCE_FIX
  26
  27 btRigidBody& btActionInterface::getFixedBody()
  28 {
  29         static btRigidBody s_fixed(0, 0, 0);
  30         s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
  31         return s_fixed;
  32 }
  33
  34 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster)
  35         : m_vehicleRaycaster(raycaster),
  36           m_pitchControl(btScalar(0.))
  37 {
  38         m_chassisBody = chassis;
  39         m_indexRightAxis = 0;
  40         m_indexUpAxis = 2;
  41         m_indexForwardAxis = 1;
  42         defaultInit(tuning);
  43 }
  44
  45 void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
  46 {
  47         (void)tuning;
  48         m_currentVehicleSpeedKmHour = btScalar(0.);
  49         m_steeringValue = btScalar(0.);
  50 }
  51
  52 btRaycastVehicle::~btRaycastVehicle()
  53 {
  54 }
  55
  56 //
  57 // basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
  58 //
  59 btWheelInfo& btRaycastVehicle::addWheel(const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0, const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius, const btVehicleTuning& tuning, bool isFrontWheel)
  60 {
  61         btWheelInfoConstructionInfo ci;
  62
  63         ci.m_chassisConnectionCS = connectionPointCS;
  64         ci.m_wheelDirectionCS = wheelDirectionCS0;
  65         ci.m_wheelAxleCS = wheelAxleCS;
  66         ci.m_suspensionRestLength = suspensionRestLength;
  67         ci.m_wheelRadius = wheelRadius;
  68         ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
  69         ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
  70         ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
  71         ci.m_frictionSlip = tuning.m_frictionSlip;
  72         ci.m_bIsFrontWheel = isFrontWheel;
  73         ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
  74         ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
  75
  76         m_wheelInfo.push_back(btWheelInfo(ci));
  77
  78         btWheelInfo& wheel = m_wheelInfo[getNumWheels() - 1];
  79
  80         updateWheelTransformsWS(wheel, false);
  81         updateWheelTransform(getNumWheels() - 1, false);
  82         return wheel;
  83 }
  84
  85 const btTransform& btRaycastVehicle::getWheelTransformWS(int wheelIndex) const
  86 {
  87         btAssert(wheelIndex < getNumWheels());
  88         const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
  89         return wheel.m_worldTransform;
  90 }
  91
  92 void btRaycastVehicle::updateWheelTransform(int wheelIndex, bool interpolatedTransform)
  93 {
  94         btWheelInfo& wheel = m_wheelInfo[wheelIndex];
  95         updateWheelTransformsWS(wheel, interpolatedTransform);
  96         btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
  97         const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
  98         btVector3 fwd = up.cross(right);
  99         fwd = fwd.normalize();
 100         //      up = right.cross(fwd);
 101         //      up.normalize();
 102
 103         //rotate around steering over de wheelAxleWS
 104         btScalar steering = wheel.m_steering;
 105
 106         btQuaternion steeringOrn(up, steering);  //wheel.m_steering);
 107         btMatrix3x3 steeringMat(steeringOrn);
 108
 109         btQuaternion rotatingOrn(right, -wheel.m_rotation);
 110         btMatrix3x3 rotatingMat(rotatingOrn);
 111
 112         btMatrix3x3 basis2;
 113         basis2[0][m_indexRightAxis] = -right[0];
 114         basis2[1][m_indexRightAxis] = -right[1];
 115         basis2[2][m_indexRightAxis] = -right[2];
 116
 117         basis2[0][m_indexUpAxis] = up[0];
 118         basis2[1][m_indexUpAxis] = up[1];
 119         basis2[2][m_indexUpAxis] = up[2];
 120
 121         basis2[0][m_indexForwardAxis] = fwd[0];
 122         basis2[1][m_indexForwardAxis] = fwd[1];
 123         basis2[2][m_indexForwardAxis] = fwd[2];
 124
 125         wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
 126         wheel.m_worldTransform.setOrigin(
 127                 wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength);
 128 }
 129
 130 void btRaycastVehicle::resetSuspension()
 131 {
 132         int i;
 133         for (i = 0; i < m_wheelInfo.size(); i++)
 134         {
 135                 btWheelInfo& wheel = m_wheelInfo[i];
 136                 wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
 137                 wheel.m_suspensionRelativeVelocity = btScalar(0.0);
 138
 139                 wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
 140                 //wheel_info.setContactFriction(btScalar(0.0));
 141                 wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
 142         }
 143 }
 144
 145 void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel, bool interpolatedTransform)
 146 {
 147         wheel.m_raycastInfo.m_isInContact = false;
 148
 149         btTransform chassisTrans = getChassisWorldTransform();
 150         if (interpolatedTransform && (getRigidBody()->getMotionState()))
 151         {
 152                 getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
 153         }
 154
 155         wheel.m_raycastInfo.m_hardPointWS = chassisTrans(wheel.m_chassisConnectionPointCS);
 156         wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS;
 157         wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
 158 }
 159
 160 btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 161 {
 162         updateWheelTransformsWS(wheel, false);
 163
 164         btScalar depth = -1;
 165
 166         btScalar raylen = wheel.getSuspensionRestLength() + wheel.m_wheelsRadius;
 167
 168         btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
 169         const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
 170         wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
 171         const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
 172
 173         btScalar param = btScalar(0.);
 174
 175         btVehicleRaycaster::btVehicleRaycasterResult rayResults;
 176
 177         btAssert(m_vehicleRaycaster);
 178
 179         void* object = m_vehicleRaycaster->castRay(source, target, rayResults);
 180
 181         wheel.m_raycastInfo.m_groundObject = 0;
 182
 183         if (object)
 184         {
 185                 param = rayResults.m_distFraction;
 186                 depth = raylen * rayResults.m_distFraction;
 187                 wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
 188                 wheel.m_raycastInfo.m_isInContact = true;
 189
 190                 wheel.m_raycastInfo.m_groundObject = &getFixedBody();  ///@todo for driving on dynamic/movable objects!;
 191                 //wheel.m_raycastInfo.m_groundObject = object;
 192
 193                 btScalar hitDistance = param * raylen;
 194                 wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
 195                 //clamp on max suspension travel
 196
 197                 btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm * btScalar(0.01);
 198                 btScalar maxSuspensionLength = wheel.getSuspensionRestLength() + wheel.m_maxSuspensionTravelCm * btScalar(0.01);
 199                 if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
 200                 {
 201                         wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
 202                 }
 203                 if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
 204                 {
 205                         wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
 206                 }
 207
 208                 wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
 209
 210                 btScalar denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(wheel.m_raycastInfo.m_wheelDirectionWS);
 211
 212                 btVector3 chassis_velocity_at_contactPoint;
 213                 btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
 214
 215                 chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
 216
 217                 btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
 218
 219                 if (denominator >= btScalar(-0.1))
 220                 {
 221                         wheel.m_suspensionRelativeVelocity = btScalar(0.0);
 222                         wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
 223                 }
 224                 else
 225                 {
 226                         btScalar inv = btScalar(-1.) / denominator;
 227                         wheel.m_suspensionRelativeVelocity = projVel * inv;
 228                         wheel.m_clippedInvContactDotSuspension = inv;
 229                 }
 230         }
 231         else
 232         {
 233                 //put wheel info as in rest position
 234                 wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
 235                 wheel.m_suspensionRelativeVelocity = btScalar(0.0);
 236                 wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
 237                 wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
 238         }
 239
 240         return depth;
 241 }
 242
 243 const btTransform& btRaycastVehicle::getChassisWorldTransform() const
 244 {
 245         /*if (getRigidBody()->getMotionState())
 246         {
 247                 btTransform chassisWorldTrans;
 248                 getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
 249                 return chassisWorldTrans;
 250         }
 251         */
 252
 253         return getRigidBody()->getCenterOfMassTransform();
 254 }
 255
 256 void btRaycastVehicle::updateVehicle(btScalar step)
 257 {
 258         {
 259                 for (int i = 0; i < getNumWheels(); i++)
 260                 {
 261                         updateWheelTransform(i, false);
 262                 }
 263         }
 264
 265         m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
 266
 267         const btTransform& chassisTrans = getChassisWorldTransform();
 268
 269         btVector3 forwardW(
 270                 chassisTrans.getBasis()[0][m_indexForwardAxis],
 271                 chassisTrans.getBasis()[1][m_indexForwardAxis],
 272                 chassisTrans.getBasis()[2][m_indexForwardAxis]);
 273
 274         if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
 275         {
 276                 m_currentVehicleSpeedKmHour *= btScalar(-1.);
 277         }
 278
 279         //
 280         // simulate suspension
 281         //
 282
 283         int i = 0;
 284         for (i = 0; i < m_wheelInfo.size(); i++)
 285         {
 286                 //btScalar depth;
 287                 //depth =
 288                 rayCast(m_wheelInfo[i]);
 289         }
 290
 291         updateSuspension(step);
 292
 293         for (i = 0; i < m_wheelInfo.size(); i++)
 294         {
 295                 //apply suspension force
 296                 btWheelInfo& wheel = m_wheelInfo[i];
 297
 298                 btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
 299
 300                 if (suspensionForce > wheel.m_maxSuspensionForce)
 301                 {
 302                         suspensionForce = wheel.m_maxSuspensionForce;
 303                 }
 304                 btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
 305                 btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
 306
 307                 getRigidBody()->applyImpulse(impulse, relpos);
 308         }
 309
 310         updateFriction(step);
 311
 312         for (i = 0; i < m_wheelInfo.size(); i++)
 313         {
 314                 btWheelInfo& wheel = m_wheelInfo[i];
 315                 btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
 316                 btVector3 vel = getRigidBody()->getVelocityInLocalPoint(relpos);
 317
 318                 if (wheel.m_raycastInfo.m_isInContact)
 319                 {
 320                         const btTransform& chassisWorldTransform = getChassisWorldTransform();
 321
 322                         btVector3 fwd(
 323                                 chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
 324                                 chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
 325                                 chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
 326
 327                         btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
 328                         fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
 329
 330                         btScalar proj2 = fwd.dot(vel);
 331
 332                         wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
 333                         wheel.m_rotation += wheel.m_deltaRotation;
 334                 }
 335                 else
 336                 {
 337                         wheel.m_rotation += wheel.m_deltaRotation;
 338                 }
 339
 340                 wheel.m_deltaRotation *= btScalar(0.99);  //damping of rotation when not in contact
 341         }
 342 }
 343
 344 void btRaycastVehicle::setSteeringValue(btScalar steering, int wheel)
 345 {
 346         btAssert(wheel >= 0 && wheel < getNumWheels());
 347
 348         btWheelInfo& wheelInfo = getWheelInfo(wheel);
 349         wheelInfo.m_steering = steering;
 350 }
 351
 352 btScalar btRaycastVehicle::getSteeringValue(int wheel) const
 353 {
 354         return getWheelInfo(wheel).m_steering;
 355 }
 356
 357 void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
 358 {
 359         btAssert(wheel >= 0 && wheel < getNumWheels());
 360         btWheelInfo& wheelInfo = getWheelInfo(wheel);
 361         wheelInfo.m_engineForce = force;
 362 }
 363
 364 const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const
 365 {
 366         btAssert((index >= 0) && (index < getNumWheels()));
 367
 368         return m_wheelInfo[index];
 369 }
 370
 371 btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
 372 {
 373         btAssert((index >= 0) && (index < getNumWheels()));
 374
 375         return m_wheelInfo[index];
 376 }
 377
 378 void btRaycastVehicle::setBrake(btScalar brake, int wheelIndex)
 379 {
 380         btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
 381         getWheelInfo(wheelIndex).m_brake = brake;
 382 }
 383
 384 void btRaycastVehicle::updateSuspension(btScalar deltaTime)
 385 {
 386         (void)deltaTime;
 387
 388         btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
 389
 390         for (int w_it = 0; w_it < getNumWheels(); w_it++)
 391         {
 392                 btWheelInfo& wheel_info = m_wheelInfo[w_it];
 393
 394                 if (wheel_info.m_raycastInfo.m_isInContact)
 395                 {
 396                         btScalar force;
 397                         //      Spring
 398                         {
 399                                 btScalar susp_length = wheel_info.getSuspensionRestLength();
 400                                 btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
 401
 402                                 btScalar length_diff = (susp_length - current_length);
 403
 404                                 force = wheel_info.m_suspensionStiffness * length_diff * wheel_info.m_clippedInvContactDotSuspension;
 405                         }
 406
 407                         // Damper
 408                         {
 409                                 btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
 410                                 {
 411                                         btScalar susp_damping;
 412                                         if (projected_rel_vel < btScalar(0.0))
 413                                         {
 414                                                 susp_damping = wheel_info.m_wheelsDampingCompression;
 415                                         }
 416                                         else
 417                                         {
 418                                                 susp_damping = wheel_info.m_wheelsDampingRelaxation;
 419                                         }
 420                                         force -= susp_damping * projected_rel_vel;
 421                                 }
 422                         }
 423
 424                         // RESULT
 425                         wheel_info.m_wheelsSuspensionForce = force * chassisMass;
 426                         if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
 427                         {
 428                                 wheel_info.m_wheelsSuspensionForce = btScalar(0.);
 429                         }
 430                 }
 431                 else
 432                 {
 433                         wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
 434                 }
 435         }
 436 }
 437
 438 struct btWheelContactPoint
 439 {
 440         btRigidBody* m_body0;
 441         btRigidBody* m_body1;
 442         btVector3 m_frictionPositionWorld;
 443         btVector3 m_frictionDirectionWorld;
 444         btScalar m_jacDiagABInv;
 445         btScalar m_maxImpulse;
 446
 447         btWheelContactPoint(btRigidBody* body0, btRigidBody* body1, const btVector3& frictionPosWorld, const btVector3& frictionDirectionWorld, btScalar maxImpulse)
 448                 : m_body0(body0),
 449                   m_body1(body1),
 450                   m_frictionPositionWorld(frictionPosWorld),
 451                   m_frictionDirectionWorld(frictionDirectionWorld),
 452                   m_maxImpulse(maxImpulse)
 453         {
 454                 btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
 455                 btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
 456                 btScalar relaxation = 1.f;
 457                 m_jacDiagABInv = relaxation / (denom0 + denom1);
 458         }
 459 };
 460
 461 btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround);
 462 btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround)
 463 {
 464         btScalar j1 = 0.f;
 465
 466         const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
 467
 468         btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition();
 469         btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
 470
 471         btScalar maxImpulse = contactPoint.m_maxImpulse;
 472
 473         btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
 474         btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
 475         btVector3 vel = vel1 - vel2;
 476
 477         btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
 478
 479         // calculate j that moves us to zero relative velocity
 480         j1 = -vrel * contactPoint.m_jacDiagABInv / btScalar(numWheelsOnGround);
 481         btSetMin(j1, maxImpulse);
 482         btSetMax(j1, -maxImpulse);
 483
 484         return j1;
 485 }
 486
 487 btScalar sideFrictionStiffness2 = btScalar(1.0);
 488 void btRaycastVehicle::updateFriction(btScalar timeStep)
 489 {
 490         //calculate the impulse, so that the wheels don't move sidewards
 491         int numWheel = getNumWheels();
 492         if (!numWheel)
 493                 return;
 494
 495         m_forwardWS.resize(numWheel);
 496         m_axle.resize(numWheel);
 497         m_forwardImpulse.resize(numWheel);
 498         m_sideImpulse.resize(numWheel);
 499
 500         int numWheelsOnGround = 0;
 501
 502         //collapse all those loops into one!
 503         for (int i = 0; i < getNumWheels(); i++)
 504         {
 505                 btWheelInfo& wheelInfo = m_wheelInfo[i];
 506                 class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
 507                 if (groundObject)
 508                         numWheelsOnGround++;
 509                 m_sideImpulse[i] = btScalar(0.);
 510                 m_forwardImpulse[i] = btScalar(0.);
 511         }
 512
 513         {
 514                 for (int i = 0; i < getNumWheels(); i++)
 515                 {
 516                         btWheelInfo& wheelInfo = m_wheelInfo[i];
 517
 518                         class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
 519
 520                         if (groundObject)
 521                         {
 522                                 const btTransform& wheelTrans = getWheelTransformWS(i);
 523
 524                                 btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
 525                                 m_axle[i] = -btVector3(
 526                                         wheelBasis0[0][m_indexRightAxis],
 527                                         wheelBasis0[1][m_indexRightAxis],
 528                                         wheelBasis0[2][m_indexRightAxis]);
 529
 530                                 const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
 531                                 btScalar proj = m_axle[i].dot(surfNormalWS);
 532                                 m_axle[i] -= surfNormalWS * proj;
 533                                 m_axle[i] = m_axle[i].normalize();
 534
 535                                 m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
 536                                 m_forwardWS[i].normalize();
 537
 538                                 resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
 539                                                                            *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
 540                                                                            btScalar(0.), m_axle[i], m_sideImpulse[i], timeStep);
 541
 542                                 m_sideImpulse[i] *= sideFrictionStiffness2;
 543                         }
 544                 }
 545         }
 546
 547         btScalar sideFactor = btScalar(1.);
 548         btScalar fwdFactor = 0.5;
 549
 550         bool sliding = false;
 551         {
 552                 for (int wheel = 0; wheel < getNumWheels(); wheel++)
 553                 {
 554                         btWheelInfo& wheelInfo = m_wheelInfo[wheel];
 555                         class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
 556
 557                         btScalar rollingFriction = 0.f;
 558
 559                         if (groundObject)
 560                         {
 561                                 if (wheelInfo.m_engineForce != 0.f)
 562                                 {
 563                                         rollingFriction = wheelInfo.m_engineForce * timeStep;
 564                                 }
 565                                 else
 566                                 {
 567                                         btScalar defaultRollingFrictionImpulse = 0.f;
 568                                         btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
 569                                         btWheelContactPoint contactPt(m_chassisBody, groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, m_forwardWS[wheel], maxImpulse);
 570                                         btAssert(numWheelsOnGround > 0);
 571                                         rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround);
 572                                 }
 573                         }
 574
 575                         //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
 576
 577                         m_forwardImpulse[wheel] = btScalar(0.);
 578                         m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
 579
 580                         if (groundObject)
 581                         {
 582                                 m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
 583
 584                                 btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
 585                                 btScalar maximpSide = maximp;
 586
 587                                 btScalar maximpSquared = maximp * maximpSide;
 588
 589                                 m_forwardImpulse[wheel] = rollingFriction;  //wheelInfo.m_engineForce* timeStep;
 590
 591                                 btScalar x = (m_forwardImpulse[wheel]) * fwdFactor;
 592                                 btScalar y = (m_sideImpulse[wheel]) * sideFactor;
 593
 594                                 btScalar impulseSquared = (x * x + y * y);
 595
 596                                 if (impulseSquared > maximpSquared)
 597                                 {
 598                                         sliding = true;
 599
 600                                         btScalar factor = maximp / btSqrt(impulseSquared);
 601
 602                                         m_wheelInfo[wheel].m_skidInfo *= factor;
 603                                 }
 604                         }
 605                 }
 606         }
 607
 608         if (sliding)
 609         {
 610                 for (int wheel = 0; wheel < getNumWheels(); wheel++)
 611                 {
 612                         if (m_sideImpulse[wheel] != btScalar(0.))
 613                         {
 614                                 if (m_wheelInfo[wheel].m_skidInfo < btScalar(1.))
 615                                 {
 616                                         m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
 617                                         m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
 618                                 }
 619                         }
 620                 }
 621         }
 622
 623         // apply the impulses
 624         {
 625                 for (int wheel = 0; wheel < getNumWheels(); wheel++)
 626                 {
 627                         btWheelInfo& wheelInfo = m_wheelInfo[wheel];
 628
 629                         btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
 630                                                                 m_chassisBody->getCenterOfMassPosition();
 631
 632                         if (m_forwardImpulse[wheel] != btScalar(0.))
 633                         {
 634                                 m_chassisBody->applyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
 635                         }
 636                         if (m_sideImpulse[wheel] != btScalar(0.))
 637                         {
 638                                 class btRigidBody* groundObject = (class btRigidBody*)m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
 639
 640                                 btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
 641                                                                          groundObject->getCenterOfMassPosition();
 642
 643                                 btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
 644
 645 #if defined ROLLING_INFLUENCE_FIX  // fix. It only worked if car's up was along Y - VT.
 646                                 btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
 647                                 rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f - wheelInfo.m_rollInfluence));
 648 #else
 649                                 rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
 650 #endif
 651                                 m_chassisBody->applyImpulse(sideImp, rel_pos);
 652
 653                                 //apply friction impulse on the ground
 654                                 groundObject->applyImpulse(-sideImp, rel_pos2);
 655                         }
 656                 }
 657         }
 658 }
 659
 660 void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
 661 {
 662         for (int v = 0; v < this->getNumWheels(); v++)
 663         {
 664                 btVector3 wheelColor(0, 1, 1);
 665                 if (getWheelInfo(v).m_raycastInfo.m_isInContact)
 666                 {
 667                         wheelColor.setValue(0, 0, 1);
 668                 }
 669                 else
 670                 {
 671                         wheelColor.setValue(1, 0, 1);
 672                 }
 673
 674                 btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
 675
 676                 btVector3 axle = btVector3(
 677                         getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
 678                         getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
 679                         getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
 680
 681                 //debug wheels (cylinders)
 682                 debugDrawer->drawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
 683                 debugDrawer->drawLine(wheelPosWS, getWheelInfo(v).m_raycastInfo.m_contactPointWS, wheelColor);
 684         }
 685 }
 686
 687 void* btDefaultVehicleRaycaster::castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result)
 688 {
 689         //      RayResultCallback& resultCallback;
 690
 691         btCollisionWorld::ClosestRayResultCallback rayCallback(from, to);
 692
 693         m_dynamicsWorld->rayTest(from, to, rayCallback);
 694
 695         if (rayCallback.hasHit())
 696         {
 697                 const btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
 698                 if (body && body->hasContactResponse())
 699                 {
 700                         result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
 701                         result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
 702                         result.m_hitNormalInWorld.normalize();
 703                         result.m_distFraction = rayCallback.m_closestHitFraction;
 704                         return (void*)body;
 705                 }
 706         }
 707         return 0;
 708 }