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