Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / Vehicle / btRaycastVehicle.cpp
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
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
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
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
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 //
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
95 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
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
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
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
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
253 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
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
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
377 btScalar        btRaycastVehicle::getSteeringValue(int wheel) const
378 {
379         return getWheelInfo(wheel).m_steering;
380 }
381
382
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
391 const btWheelInfo&      btRaycastVehicle::getWheelInfo(int index) const
392 {
393         btAssert((index >= 0) && (index <       getNumWheels()));
394         
395         return m_wheelInfo[index];
396 }
397
398 btWheelInfo&    btRaycastVehicle::getWheelInfo(int index) 
399 {
400         btAssert((index >= 0) && (index <       getNumWheels()));
401         
402         return m_wheelInfo[index];
403 }
404
405 void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
406 {
407         btAssert((wheelIndex >= 0) && (wheelIndex <     getNumWheels()));
408         getWheelInfo(wheelIndex).m_brake = brake;
409 }
410
411
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
479         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);
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);
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
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
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