Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/release2012/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp @ 10609

Last change on this file since 10609 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 21.0 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
25#define ROLLING_INFLUENCE_FIX
26
27
28btRigidBody& 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
35btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,  btVehicleRaycaster* raycaster )
36:m_vehicleRaycaster(raycaster),
37m_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
47void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
48{
49        (void)tuning;
50        m_currentVehicleSpeedKmHour = btScalar(0.);
51        m_steeringValue = btScalar(0.);
52       
53}
54
55       
56
57btRaycastVehicle::~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//
65btWheelInfo&    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
95const 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
103void    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
136void 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
152void    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
167btScalar 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
253const 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
268void 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 = rayCast( m_wheelInfo[i]);
301        }
302
303        updateSuspension(step);
304
305       
306        for (i=0;i<m_wheelInfo.size();i++)
307        {
308                //apply suspension force
309                btWheelInfo& wheel = m_wheelInfo[i];
310               
311                btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
312               
313                if (suspensionForce > wheel.m_maxSuspensionForce)
314                {
315                        suspensionForce = wheel.m_maxSuspensionForce;
316                }
317                btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
318                btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
319               
320                getRigidBody()->applyImpulse(impulse, relpos);
321       
322        }
323       
324
325       
326        updateFriction( step);
327
328       
329        for (i=0;i<m_wheelInfo.size();i++)
330        {
331                btWheelInfo& wheel = m_wheelInfo[i];
332                btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
333                btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
334
335                if (wheel.m_raycastInfo.m_isInContact)
336                {
337                        const btTransform&      chassisWorldTransform = getChassisWorldTransform();
338
339                        btVector3 fwd (
340                                chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
341                                chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
342                                chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
343
344                        btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
345                        fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
346
347                        btScalar proj2 = fwd.dot(vel);
348                       
349                        wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
350                        wheel.m_rotation += wheel.m_deltaRotation;
351
352                } else
353                {
354                        wheel.m_rotation += wheel.m_deltaRotation;
355                }
356               
357                wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
358
359        }
360
361
362
363}
364
365
366void    btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
367{
368        btAssert(wheel>=0 && wheel < getNumWheels());
369
370        btWheelInfo& wheelInfo = getWheelInfo(wheel);
371        wheelInfo.m_steering = steering;
372}
373
374
375
376btScalar        btRaycastVehicle::getSteeringValue(int wheel) const
377{
378        return getWheelInfo(wheel).m_steering;
379}
380
381
382void    btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
383{
384        btAssert(wheel>=0 && wheel < getNumWheels());
385        btWheelInfo& wheelInfo = getWheelInfo(wheel);
386        wheelInfo.m_engineForce = force;
387}
388
389
390const btWheelInfo&      btRaycastVehicle::getWheelInfo(int index) const
391{
392        btAssert((index >= 0) && (index <       getNumWheels()));
393       
394        return m_wheelInfo[index];
395}
396
397btWheelInfo&    btRaycastVehicle::getWheelInfo(int index) 
398{
399        btAssert((index >= 0) && (index <       getNumWheels()));
400       
401        return m_wheelInfo[index];
402}
403
404void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
405{
406        btAssert((wheelIndex >= 0) && (wheelIndex <     getNumWheels()));
407        getWheelInfo(wheelIndex).m_brake = brake;
408}
409
410
411void    btRaycastVehicle::updateSuspension(btScalar deltaTime)
412{
413        (void)deltaTime;
414
415        btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
416       
417        for (int w_it=0; w_it<getNumWheels(); w_it++)
418        {
419                btWheelInfo &wheel_info = m_wheelInfo[w_it];
420               
421                if ( wheel_info.m_raycastInfo.m_isInContact )
422                {
423                        btScalar force;
424                        //      Spring
425                        {
426                                btScalar        susp_length                     = wheel_info.getSuspensionRestLength();
427                                btScalar        current_length = wheel_info.m_raycastInfo.m_suspensionLength;
428
429                                btScalar length_diff = (susp_length - current_length);
430
431                                force = wheel_info.m_suspensionStiffness
432                                        * length_diff * wheel_info.m_clippedInvContactDotSuspension;
433                        }
434               
435                        // Damper
436                        {
437                                btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
438                                {
439                                        btScalar        susp_damping;
440                                        if ( projected_rel_vel < btScalar(0.0) )
441                                        {
442                                                susp_damping = wheel_info.m_wheelsDampingCompression;
443                                        }
444                                        else
445                                        {
446                                                susp_damping = wheel_info.m_wheelsDampingRelaxation;
447                                        }
448                                        force -= susp_damping * projected_rel_vel;
449                                }
450                        }
451
452                        // RESULT
453                        wheel_info.m_wheelsSuspensionForce = force * chassisMass;
454                        if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
455                        {
456                                wheel_info.m_wheelsSuspensionForce = btScalar(0.);
457                        }
458                }
459                else
460                {
461                        wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
462                }
463        }
464
465}
466
467
468struct btWheelContactPoint
469{
470        btRigidBody* m_body0;
471        btRigidBody* m_body1;
472        btVector3       m_frictionPositionWorld;
473        btVector3       m_frictionDirectionWorld;
474        btScalar        m_jacDiagABInv;
475        btScalar        m_maxImpulse;
476
477
478        btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
479                :m_body0(body0),
480                m_body1(body1),
481                m_frictionPositionWorld(frictionPosWorld),
482                m_frictionDirectionWorld(frictionDirectionWorld),
483                m_maxImpulse(maxImpulse)
484        {
485                btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
486                btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
487                btScalar        relaxation = 1.f;
488                m_jacDiagABInv = relaxation/(denom0+denom1);
489        }
490
491
492
493};
494
495btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
496btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
497{
498
499        btScalar j1=0.f;
500
501        const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
502
503        btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); 
504        btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
505       
506        btScalar maxImpulse  = contactPoint.m_maxImpulse;
507       
508        btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
509        btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
510        btVector3 vel = vel1 - vel2;
511
512        btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
513
514        // calculate j that moves us to zero relative velocity
515        j1 = -vrel * contactPoint.m_jacDiagABInv;
516        btSetMin(j1, maxImpulse);
517        btSetMax(j1, -maxImpulse);
518
519        return j1;
520}
521
522
523
524
525btScalar sideFrictionStiffness2 = btScalar(1.0);
526void    btRaycastVehicle::updateFriction(btScalar       timeStep)
527{
528
529                //calculate the impulse, so that the wheels don't move sidewards
530                int numWheel = getNumWheels();
531                if (!numWheel)
532                        return;
533
534                m_forwardWS.resize(numWheel);
535                m_axle.resize(numWheel);
536                m_forwardImpulse.resize(numWheel);
537                m_sideImpulse.resize(numWheel);
538               
539                int numWheelsOnGround = 0;
540       
541
542                //collapse all those loops into one!
543                for (int i=0;i<getNumWheels();i++)
544                {
545                        btWheelInfo& wheelInfo = m_wheelInfo[i];
546                        class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
547                        if (groundObject)
548                                numWheelsOnGround++;
549                        m_sideImpulse[i] = btScalar(0.);
550                        m_forwardImpulse[i] = btScalar(0.);
551
552                }
553       
554                {
555       
556                        for (int i=0;i<getNumWheels();i++)
557                        {
558
559                                btWheelInfo& wheelInfo = m_wheelInfo[i];
560                                       
561                                class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
562
563                                if (groundObject)
564                                {
565
566                                        const btTransform& wheelTrans = getWheelTransformWS( i );
567
568                                        btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
569                                        m_axle[i] = btVector3( 
570                                                wheelBasis0[0][m_indexRightAxis],
571                                                wheelBasis0[1][m_indexRightAxis],
572                                                wheelBasis0[2][m_indexRightAxis]);
573                                       
574                                        const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
575                                        btScalar proj = m_axle[i].dot(surfNormalWS);
576                                        m_axle[i] -= surfNormalWS * proj;
577                                        m_axle[i] = m_axle[i].normalize();
578                                       
579                                        m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
580                                        m_forwardWS[i].normalize();
581
582                               
583                                        resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
584                                                          *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
585                                                          btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
586
587                                        m_sideImpulse[i] *= sideFrictionStiffness2;
588                                               
589                                }
590                               
591
592                        }
593                }
594
595        btScalar sideFactor = btScalar(1.);
596        btScalar fwdFactor = 0.5;
597
598        bool sliding = false;
599        {
600                for (int wheel =0;wheel <getNumWheels();wheel++)
601                {
602                        btWheelInfo& wheelInfo = m_wheelInfo[wheel];
603                        class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
604
605                        btScalar        rollingFriction = 0.f;
606
607                        if (groundObject)
608                        {
609                                if (wheelInfo.m_engineForce != 0.f)
610                                {
611                                        rollingFriction = wheelInfo.m_engineForce* timeStep;
612                                } else
613                                {
614                                        btScalar defaultRollingFrictionImpulse = 0.f;
615                                        btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
616                                        btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
617                                        rollingFriction = calcRollingFriction(contactPt);
618                                }
619                        }
620
621                        //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
622                       
623
624
625
626                        m_forwardImpulse[wheel] = btScalar(0.);
627                        m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
628
629                        if (groundObject)
630                        {
631                                m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
632                               
633                                btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
634                                btScalar maximpSide = maximp;
635
636                                btScalar maximpSquared = maximp * maximpSide;
637                       
638
639                                m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
640
641                                btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
642                                btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
643                               
644                                btScalar impulseSquared = (x*x + y*y);
645
646                                if (impulseSquared > maximpSquared)
647                                {
648                                        sliding = true;
649                                       
650                                        btScalar factor = maximp / btSqrt(impulseSquared);
651                                       
652                                        m_wheelInfo[wheel].m_skidInfo *= factor;
653                                }
654                        } 
655
656                }
657        }
658
659       
660
661
662                if (sliding)
663                {
664                        for (int wheel = 0;wheel < getNumWheels(); wheel++)
665                        {
666                                if (m_sideImpulse[wheel] != btScalar(0.))
667                                {
668                                        if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
669                                        {
670                                                m_forwardImpulse[wheel] *=      m_wheelInfo[wheel].m_skidInfo;
671                                                m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
672                                        }
673                                }
674                        }
675                }
676
677                // apply the impulses
678                {
679                        for (int wheel = 0;wheel<getNumWheels() ; wheel++)
680                        {
681                                btWheelInfo& wheelInfo = m_wheelInfo[wheel];
682
683                                btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - 
684                                                m_chassisBody->getCenterOfMassPosition();
685
686                                if (m_forwardImpulse[wheel] != btScalar(0.))
687                                {
688                                        m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
689                                }
690                                if (m_sideImpulse[wheel] != btScalar(0.))
691                                {
692                                        class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
693
694                                        btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - 
695                                                groundObject->getCenterOfMassPosition();
696
697                                       
698                                        btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
699
700#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
701                                        btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
702                                        rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
703#else
704                                        rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
705#endif
706                                        m_chassisBody->applyImpulse(sideImp,rel_pos);
707
708                                        //apply friction impulse on the ground
709                                        groundObject->applyImpulse(-sideImp,rel_pos2);
710                                }
711                        }
712                }
713
714       
715}
716
717
718
719void    btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
720{
721
722        for (int v=0;v<this->getNumWheels();v++)
723        {
724                btVector3 wheelColor(0,1,1);
725                if (getWheelInfo(v).m_raycastInfo.m_isInContact)
726                {
727                        wheelColor.setValue(0,0,1);
728                } else
729                {
730                        wheelColor.setValue(1,0,1);
731                }
732
733                btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
734
735                btVector3 axle = btVector3(     
736                        getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
737                        getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
738                        getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
739
740                //debug wheels (cylinders)
741                debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
742                debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
743
744        }
745}
746
747
748void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
749{
750//      RayResultCallback& resultCallback;
751
752        btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);
753
754        m_dynamicsWorld->rayTest(from, to, rayCallback);
755
756        if (rayCallback.hasHit())
757        {
758               
759                btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
760        if (body && body->hasContactResponse())
761                {
762                        result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
763                        result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
764                        result.m_hitNormalInWorld.normalize();
765                        result.m_distFraction = rayCallback.m_closestHitFraction;
766                        return body;
767                }
768        }
769        return 0;
770}
771
Note: See TracBrowser for help on using the repository browser.