Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp @ 1963

Last change on this file since 1963 was 1963, checked in by rgrieder, 16 years ago

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