Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/environment2/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp @ 8355

Last change on this file since 8355 was 8351, checked in by rgrieder, 14 years ago

Merged kicklib2 branch back to trunk (includes former branches ois_update, mac_osx and kicklib).

Notes for updating

Linux:
You don't need an extra package for CEGUILua and Tolua, it's already shipped with CEGUI.
However you do need to make sure that the OgreRenderer is installed too with CEGUI 0.7 (may be a separate package).
Also, Orxonox now recognises if you install the CgProgramManager (a separate package available on newer Ubuntu on Debian systems).

Windows:
Download the new dependency packages versioned 6.0 and use these. If you have problems with that or if you don't like the in game console problem mentioned below, you can download the new 4.3 version of the packages (only available for Visual Studio 2005/2008).

Key new features:

  • *Support for Mac OS X*
  • Visual Studio 2010 support
  • Bullet library update to 2.77
  • OIS library update to 1.3
  • Support for CEGUI 0.7 —> Support for Arch Linux and even SuSE
  • Improved install target
  • Compiles now with GCC 4.6
  • Ogre Cg Shader plugin activated for Linux if available
  • And of course lots of bug fixes

There are also some regressions:

  • No support for CEGUI 0.5, Ogre 1.4 and boost 1.35 - 1.39 any more
  • In game console is not working in main menu for CEGUI 0.7
  • Tolua (just the C lib, not the application) and CEGUILua libraries are no longer in our repository. —> You will need to get these as well when compiling Orxonox
  • And of course lots of new bugs we don't yet know about
  • 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.