Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/kicklib/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp @ 8501

Last change on this file since 8501 was 7983, checked in by rgrieder, 14 years ago

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