Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/archive/tutorialHS09/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp @ 9098

Last change on this file since 9098 was 5738, checked in by landauf, 15 years ago

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