Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: downloads/ogreode/prefab/src/OgreOdeVehicle.cpp @ 21

Last change on this file since 21 was 21, checked in by nicolasc, 16 years ago

added ogreode and Colladaplugin
File size: 21.5 KB
Line 
1#include "OgreOdeWorld.h"
2#include "OgreOdeBody.h"
3#include "OgreOdeJoint.h"
4#include "OgreOdeGeometry.h"
5#include "OgreOdeSpace.h"
6#include "OgreOdeVehicle.h"
7#include "OgreOdeEntityInformer.h"
8#include "OgreOdeMass.h"
9#include "OgreOdeUtility.h"
10
11#include "OgreOdeVehicle.h"
12
13using namespace Ogre;
14using namespace OgreOde;
15using namespace OgreOde_Prefab;
16
17//------------------------------------------------------------------------------------------------
18const Ogre::Real Vehicle::Wheel::FudgeFactor = 0.1;
19
20//------------------------------------------------------------------------------------------------
21Vehicle::Wheel::Wheel(Vehicle* vehicle,const String& name,const String& mesh,const Ogre::Vector3& position,Real mass, World *world, Space* space):
22        Object(OgreOde_Prefab::ObjectType_Wheel, world) 
23{
24        _owner = vehicle;
25        _name = name;
26
27        _entity = _world->getSceneManager()->createEntity(name + "_Entity",mesh);
28        _entity->setCastShadows(true); 
29
30        EntityInformer ei(_entity);
31        _radius = ei.getRadius();
32
33        _node = _world->getSceneManager()->getRootSceneNode()->createChildSceneNode(name + "_Node");
34        _node->setPosition(position);
35        _body = new Body(_world, name + "_Body");
36
37
38        _node->attachObject(_entity);
39        _node->attachObject(_body);
40
41        _body->setMass(SphereMass(mass,_radius));
42
43        _geometry = new SphereGeometry(_radius, _world, space);
44        _geometry->setBody(_body);
45        _geometry->setUserObject(this);
46
47        _joint = new SuspensionJoint(_world);
48        _joint->attach(vehicle->getBody(),_body);
49        _joint->setAnchor(_body->getPosition());
50        _joint->setAxis(Vector3::UNIT_Y);
51        _joint->setAdditionalAxis(Vector3::NEGATIVE_UNIT_X);
52        _joint->setParameter(Joint::Parameter_FudgeFactor,Vehicle::Wheel::FudgeFactor);
53       
54        _bouncyness = _friction = _fds = 0.0;
55
56        setSteerFactor(0.0);
57        setSteerLimit(0.0);
58        setSteerForce(0.0);
59        setSteerSpeed(0.0);
60                       
61        setPowerFactor(0.0);
62                       
63        setBrakeFactor(0.0);
64
65        setSteer(0.0);
66}
67
68
69//------------------------------------------------------------------------------------------------
70void Vehicle::Wheel::setFactors(Real steer,Real power,Real brake)
71{
72        setSteerFactor(steer);
73        setPowerFactor(power);
74        setBrakeFactor(brake);
75}
76
77
78//------------------------------------------------------------------------------------------------
79Real Vehicle::Wheel::getRPM()
80{
81        return ((_joint)&&(_power_factor > 0.0))?_joint->getParameter(Joint::Parameter_MotorVelocity,2):0.0;
82}
83
84
85//------------------------------------------------------------------------------------------------
86Real Vehicle::Wheel::getSteer()
87{
88        return ((_joint)&&(_steer_factor != 0.0))?_joint->getAngle():0.0;
89}
90
91
92//------------------------------------------------------------------------------------------------
93void Vehicle::Wheel::setSteerFactor(Real factor)
94{
95        _steer_factor = factor;
96        adjustJointParameters();
97}
98
99
100//------------------------------------------------------------------------------------------------
101void Vehicle::Wheel::setSteerLimit(Real limit)
102{
103        _steer_limit = limit;
104        adjustJointParameters();
105}
106
107
108//------------------------------------------------------------------------------------------------
109void Vehicle::Wheel::adjustJointParameters()
110{
111        if (_joint)
112        {
113                _joint->setParameter(Joint::Parameter_HighStop,_steer_limit * fabs(_steer_factor));
114                _joint->setParameter(Joint::Parameter_LowStop,-(_steer_limit * fabs(_steer_factor)));
115        }
116}
117
118
119//------------------------------------------------------------------------------------------------
120/*
121According to the ODE docs;
122
123By adjusting the values of ERP and CFM, you can achieve various effects.
124For example you can simulate springy constraints, where the two bodies oscillate
125as though connected by springs. Or you can simulate more spongy constraints, without
126the oscillation. In fact, ERP and CFM can be selected to have the same effect as any
127desired spring and damper constants. If you have a spring constant kp and damping constant kd,
128then the corresponding ODE constants are:
129
130ERP = h kp / (h kp + kd)
131CFM = 1 / (h kp + kd)
132
133where h is the stepsize. These values will give the same effect as a spring-and-damper
134system simulated with implicit first order integration.
135*/
136void Vehicle::Wheel::setSuspension(Real spring_constant,Real damping_constant,Real step_size)
137{
138        _spring = spring_constant;
139        _damping = damping_constant;
140        _step = step_size;
141
142        if (_joint)
143        {
144                _joint->setParameter(Joint::Parameter_SuspensionERP,_step * _spring / (_step * _spring + _damping));
145                _joint->setParameter(Joint::Parameter_SuspensionCFM,1.0 / (_step * _spring + _damping));
146        }
147}
148//------------------------------------------------------------------------------------------------
149const Ogre::Vector3& Vehicle::Wheel::getPosition()
150{
151        return _body->getPosition();
152}
153//------------------------------------------------------------------------------------------------
154const Ogre::Quaternion& Vehicle::Wheel::getOrientation()
155{
156        return _body->getOrientation();
157}
158void Vehicle::Wheel::setPosition(const Ogre::Vector3& position)
159{
160        _body->setPosition(position);
161}
162//------------------------------------------------------------------------------------------------
163void Vehicle::Wheel::update(Real power_force,Real desired_rpm,Real brake_force)
164{
165        if (!_joint) return;
166
167        Real force = (power_force * _power_factor) - (brake_force * _brake_factor);
168
169        if (force > 0.0)
170        {
171                _joint->setParameter(Joint::Parameter_MotorVelocity,desired_rpm,2);
172                _joint->setParameter(Joint::Parameter_MaximumForce,force,2);
173        }
174        else
175        {
176                _joint->setParameter(Joint::Parameter_MotorVelocity,0.01,2);
177                _joint->setParameter(Joint::Parameter_MaximumForce,-force,2);
178        }
179}
180//------------------------------------------------------------------------------------------------
181void Vehicle::Wheel::setSteer(Real position)
182{
183        Real steer_angle = position * _steer_limit;
184
185        // Steer
186        if (_steer_limit * _steer_factor != 0.0)
187        {
188                Real vel = (steer_angle * _steer_factor) - _joint->getAngle();
189                vel *= (_steer_speed * fabs(_steer_factor));
190
191                _joint->setParameter(Joint::Parameter_MotorVelocity,vel);
192                _joint->setParameter(Joint::Parameter_MaximumForce,_steer_force);
193        }
194}
195//------------------------------------------------------------------------------------------------
196void Vehicle::Wheel::snapOff()
197{
198        delete _joint;
199        _joint = 0;
200}
201//------------------------------------------------------------------------------------------------
202Vehicle::Wheel::~Wheel()
203{
204        delete _joint;
205        delete _body;
206        delete _geometry;
207
208        _world->getSceneManager()->getRootSceneNode()->removeAndDestroyChild(_node->getName());
209        _world->getSceneManager()->destroyEntity(_entity->getName());
210}
211//------------------------------------------------------------------------------------------------
212Vehicle::Vehicle(const Ogre::String& name,const Ogre::String& mesh, Ogre::Real mass, 
213                 OgreOde::World *world, 
214                 OgreOde::Space* space,
215                 const Ogre::Vector3& offset,
216                 OgreOde::Geometry* geometry):
217        Object(OgreOde_Prefab::ObjectType_Sphere_Wheeled_Vehicle, world),
218    _name ( name),
219    _antisway (false),
220    _swayRate (Ogre::Real(0.0)),
221    _swayLastUpdate (Ogre::Real(0.0)),
222    _swayForce (Ogre::Real(0.0)),
223    _swayForceLimit (0)
224
225{
226       
227        _entity = _world->getSceneManager()->createEntity(name + "_Entity",mesh);
228        _entity->setCastShadows(true);
229        _node = _world->getSceneManager()->getRootSceneNode()->createChildSceneNode(name + "_Node");
230    _body = new Body(_world, name + "_Body");
231
232        _trans_node = _node->createChildSceneNode("Trans_" + _node->getName());
233        _trans_node->attachObject(_entity);
234        _node->attachObject(_body);
235
236        EntityInformer ei(_entity);
237        Vector3 box = ei.getSize();
238       
239        BoxMass box_mass(mass,Vector3(box.x,box.y,box.z));
240        _body->setMass(box_mass);
241
242    _space = new SimpleSpace(_world, space ? space : _world->getDefaultSpace());
243        _space->setInternalCollisions(false);
244        _space->setAutoCleanup(false);
245
246        _offset = -offset;
247
248        if (!geometry) 
249                _geometry = new BoxGeometry(box, _world, space ? space : _world->getDefaultSpace());
250        else 
251                _geometry = geometry;
252        _geometry->setUserObject(this);
253
254        _transform = new TransformGeometry(_world, _space);
255        _transform->setEncapsulatedGeometry(_geometry);
256        _transform->setBody(_body);
257        _geometry->setPosition(_offset);
258
259        _trans_node->setPosition(_offset);
260
261        _engine = new Vehicle::Engine();
262}
263//------------------------------------------------------------------------------------------------
264    Vehicle::Vehicle(const Ogre::String& name, 
265                    OgreOde::World *world, 
266                    OgreOde::Space* space,
267                    OgreOde::Geometry* geometry) : 
268        Object(OgreOde_Prefab::ObjectType_Sphere_Wheeled_Vehicle, world),
269        _name (name),
270        _antisway(false),
271        _swayRate (Ogre::Real(0.0)),
272        _swayLastUpdate (Ogre::Real(0.0)),
273        _swayForce (Ogre::Real(0.0)),
274        _swayForceLimit (0),
275        _entity (0),
276        _geometry (geometry),
277        _transform (0)
278{
279
280        _space = new SimpleSpace(_world, space?space:_world->getDefaultSpace());
281        _space->setInternalCollisions(false);
282        _space->setAutoCleanup(false);
283
284        _engine = new Vehicle::Engine();
285
286        _node = _world->getSceneManager()->getRootSceneNode()->createChildSceneNode(_name  + Ogre::StringConverter::toString (instanceNumber) + "_Node");
287        _body = new Body(_world, _name  + Ogre::StringConverter::toString (instanceNumber) + "_Body");
288
289        _trans_node = _node->createChildSceneNode("Trans_" + _node->getName());
290        _node->attachObject(_body);
291
292        if (_geometry) 
293                _geometry->setUserObject(this);
294       
295}
296//------------------------------------------------------------------------------------------------
297Vehicle::Wheel* Vehicle::addWheel(const String& mesh,const Ogre::Vector3& position,Real mass)
298{
299        const String wheelName ( _name + 
300                Ogre::StringConverter::toString (instanceNumber) + 
301                "_Wheel" + 
302                StringConverter::toString((unsigned int)_wheels.size() + 1));
303
304        Vehicle::Wheel* wheel = new Vehicle::Wheel(this, 
305                                                                                                wheelName, 
306                                                                                                mesh, 
307                                                                                                position + _body->getPosition() + _offset,
308                                                                                                mass,
309                                                _world,
310                                                                                                _space);
311        _wheels.push_back(wheel);
312        return wheel;
313}
314//------------------------------------------------------------------------------------------------
315const Ogre::Vector3& Vehicle::getPosition()
316{
317    _lastPosition = _body->getPosition() + _offset;
318        return _lastPosition;
319}
320//------------------------------------------------------------------------------------------------
321const Ogre::Quaternion& Vehicle::getOrientation()
322{
323        return _body->getOrientation();
324}
325//------------------------------------------------------------------------------------------------
326void Vehicle::setPosition(const Ogre::Vector3& position)
327{
328        for (std::vector<Vehicle::Wheel*>::iterator i = _wheels.begin();i != _wheels.end();i++)
329        {
330                Vector3 diff = (*i)->getPosition() - _body->getPosition();
331                (*i)->setPosition(position + diff);
332        }
333        _body->setPosition(position);
334}
335//------------------------------------------------------------------------------------------------
336void Vehicle::setSuspension(Real spring_constant,Real damping_constant,Real step_size)
337{
338        for (std::vector<Vehicle::Wheel*>::iterator i = _wheels.begin();i != _wheels.end();i++)
339        {
340                (*i)->setSuspension(spring_constant,damping_constant,step_size);
341        }
342}
343//------------------------------------------------------------------------------------------------
344void Vehicle::setInputs(bool left,bool right,bool throttle,bool brake)
345{
346    if (left || right || throttle)
347    {
348        for (std::vector<Vehicle::Wheel*>::iterator i = _wheels.begin();i != _wheels.end();i++)
349        {
350            (*i)->getBody()->wake();
351        }
352        _body->wake();
353    }
354        _engine->setInputs(throttle,brake);
355        setSteering(0.0 + ((right)?1.0:0.0) - ((left)?1.0:0.0));
356}
357//------------------------------------------------------------------------------------------------
358void Vehicle::setInputs(Real steering_position,Real throttle_brake)
359{
360        _engine->setInputs(throttle_brake);
361        setSteering(steering_position);
362}
363//------------------------------------------------------------------------------------------------
364void Vehicle::setInputs(Real steering_position,Real throttle_position,Real brake_position)
365{
366        _engine->setInputs(throttle_position,brake_position);
367        setSteering(steering_position);
368}
369//------------------------------------------------------------------------------------------------
370void Vehicle::setSteering(Real steering_position)
371{
372        for (std::vector<Vehicle::Wheel*>::iterator i = _wheels.begin();i != _wheels.end();i++)
373        {
374                (*i)->setSteer(steering_position);
375        }
376}
377//------------------------------------------------------------------------------------------------
378Real Vehicle::getVelocity()
379{
380        return _body->getLinearVelocity().length();
381}
382//------------------------------------------------------------------------------------------------
383void Vehicle::applyAntiSwayBarForces (  )
384{
385        Real amt;
386        Body * wheelBody;
387        for( int i = 0; i < 4; i++)
388        {
389                SuspensionJoint * const wheelJoint = static_cast<SuspensionJoint*>( _wheels.at( i )->getBody()->getJoint(0) );
390                const Ogre::Vector3 anchor2 (wheelJoint->getAdditionalAnchor());
391                const Ogre::Vector3 anchor1 (wheelJoint->getAnchor());
392                const Ogre::Vector3 axis (wheelJoint->getAxis());
393                const Ogre::Real displacement = ( anchor1 - anchor2 ).dotProduct( axis );
394                if( displacement > 0 )
395                {
396
397                        amt = displacement * _swayForce;
398                        if( amt > _swayForceLimit ) 
399                                amt = _swayForceLimit;
400
401                        // force down
402                        wheelBody = _wheels.at( i )->getBody();
403                        wheelBody->addForce( -axis * amt );
404
405                        // force up
406                        wheelBody = _wheels.at( i^1 )->getBody();
407                        wheelBody->addForce( axis * amt );
408                }
409        }
410}
411//------------------------------------------------------------------------------------------------
412void Vehicle::update(Real time)
413{
414        std::vector<Vehicle::Wheel*>::iterator b = _wheels.begin();
415        std::vector<Vehicle::Wheel*>::iterator e = _wheels.end();
416
417        Real rpm = FLT_MIN;
418
419        for (std::vector<Vehicle::Wheel*>::iterator i = b;i != e;i++)
420        {
421                rpm = std::max(rpm,(*i)->getRPM());
422        }
423
424        _engine->update(time);
425        Real power = _engine->getPowerAtRPM(rpm);
426        Real desired_rpm = _engine->getDesiredRPM();
427        Real brake = _engine->getBrakeForce();
428
429        for (std::vector<Vehicle::Wheel*>::iterator i = b;i != e;i++)
430        {
431                (*i)->update(power,desired_rpm,brake);
432        }
433       
434        if (_antisway)
435        {
436                _swayLastUpdate += time;
437                if  (_swayLastUpdate > _swayRate)
438                {
439                        applyAntiSwayBarForces ();
440                        _swayLastUpdate =Ogre::Real(0.0);
441                }
442        }
443}
444//------------------------------------------------------------------------------------------------
445Vehicle::Wheel* Vehicle::findWheel(Geometry* geometry)
446{
447        for (std::vector<Vehicle::Wheel*>::iterator i = _wheels.begin();i != _wheels.end();i++)
448        {
449                if ((*i)->getGeometry() == geometry)
450                        return (*i);
451        }
452        return 0;
453}
454//------------------------------------------------------------------------------------------------
455Vehicle::~Vehicle()
456{
457        // _world->unregisterVehicle(_name);
458
459        for (std::vector<Vehicle::Wheel*>::iterator i = _wheels.begin();i != _wheels.end();i++) 
460                delete (*i);
461
462        delete _body;
463        delete _geometry;
464        delete _transform;
465        delete _space;
466        delete _engine;
467
468        _node->removeAndDestroyChild(_trans_node->getName());
469        _world->getSceneManager()->getRootSceneNode()->removeAndDestroyChild(_node->getName());
470        _world->getSceneManager()->destroyEntity(_entity->getName());
471}
472//------------------------------------------------------------------------------------------------
473Vehicle::Engine::Engine()  :
474        _torque_curve(0),
475        _curve_length (0)
476{
477        setInputs(false,false);
478        setRevLimit(40.0);
479        setTorque(0.5,5.0);
480        setBrakeForce(500.0);
481}
482//------------------------------------------------------------------------------------------------
483Real Vehicle::Engine::getPowerAtRPM(Real rpm)
484{
485        _wheel_rpm = rpm;
486
487        const Ogre::Real pos = getRevFactor() * (_curve_length - 1);
488        const unsigned int i = (unsigned int)pos;
489        const Ogre::Real dx = pos - ((Real)i);
490
491        assert (i < _curve_length);
492        return _torque_curve[i] + (dx * (_torque_curve[i+1] - _torque_curve[i]));
493}
494//------------------------------------------------------------------------------------------------
495Real Vehicle::Engine::getBrakeForce()
496{
497        return _brake_force * _brake_position;
498}
499//------------------------------------------------------------------------------------------------
500void Vehicle::Engine::setInputs(Real throttle_position,Real brake_position)
501{
502        _throttle_position = throttle_position;
503        _brake_position = _brake_position;
504}
505//------------------------------------------------------------------------------------------------
506void Vehicle::Engine::setInputs(Real throttle_brake)
507{
508        _throttle_position = std::max((Real)0.0,throttle_brake);
509        _brake_position = std::max((Real)0.0,-throttle_brake);
510}
511//------------------------------------------------------------------------------------------------
512void Vehicle::Engine::setInputs(bool throttle,bool brake)
513{
514        _throttle_position = (throttle)?1.0:0.0;
515        _brake_position = (brake)?1.0:0.0;
516}
517//------------------------------------------------------------------------------------------------
518// Manual gearbox
519void Vehicle::Engine::changeUp()
520{
521}
522//------------------------------------------------------------------------------------------------
523void Vehicle::Engine::changeDown()
524{
525}
526//------------------------------------------------------------------------------------------------
527void Vehicle::Engine::setGear(char code)
528{
529}
530//------------------------------------------------------------------------------------------------
531void Vehicle::Engine::setGear(unsigned int i)
532{
533}
534//------------------------------------------------------------------------------------------------
535// Information functions
536Real Vehicle::Engine::getDesiredRPM()
537{
538        return _rev_limit * _throttle_position;
539}
540//------------------------------------------------------------------------------------------------
541Real Vehicle::Engine::getRevFactor()
542{
543        return _wheel_rpm * _inv_rev_limit;
544}
545//------------------------------------------------------------------------------------------------
546char Vehicle::Engine::getGearCode()
547{
548        return '\0';
549}
550//------------------------------------------------------------------------------------------------
551// Engine parameters
552void Vehicle::Engine::setRevLimit(Real rpm)
553{
554        _rev_limit = rpm;
555        _inv_rev_limit = 1.0 / _rev_limit;
556}
557//------------------------------------------------------------------------------------------------
558// Set a custom torque curve
559void Vehicle::Engine::setTorque(Real* torque,unsigned int samples)
560{
561        assert((samples > 1)&&("Need more than one sample for setTorque"));
562
563        delete[] _torque_curve;
564        _curve_length = samples;
565        _torque_curve = new Ogre::Real[_curve_length + 1];
566
567        for (unsigned int i = 0;i < samples;i++) 
568                _torque_curve[i] = torque[i];
569        _torque_curve[_curve_length] = torque[samples - 1];
570}
571//------------------------------------------------------------------------------------------------
572// Set a linear torque curve, minimum should be greater than zero,
573// or the vehicle won't go anywhere!
574void Vehicle::Engine::setTorque(Real minimum,Ogre::Real maximum)
575{
576        Real curve[] = {minimum,maximum};
577        setTorque(curve, 2);
578}
579//------------------------------------------------------------------------------------------------
580// Set a constant torque "curve"
581void Vehicle::Engine::setTorque(Real constant)
582{
583        setTorque(constant, constant);
584}
585//------------------------------------------------------------------------------------------------
586// Braking parameters
587void Vehicle::Engine::setBrakeForce(Real force)
588{
589        _brake_force = force;
590}
591//------------------------------------------------------------------------------------------------
592void Vehicle::Engine::update(Real time)
593{
594}
595
596//------------------------------------------------------------------------------------------------
597// Drivetrain parameters
598void Vehicle::Engine::addGear(Real ratio,char code)
599{
600}
601//------------------------------------------------------------------------------------------------
602void Vehicle::Engine::setFinalDrive(Real ratio)
603{
604}
605//------------------------------------------------------------------------------------------------
606void Vehicle::Engine::setAutoShiftRevs(Real up,Real down)
607{
608}
609//------------------------------------------------------------------------------------------------
610void Vehicle::Engine::setAutoBox(bool automatic)
611{
612}
613//------------------------------------------------------------------------------------------------
614Vehicle::Engine::~Engine()
615{
616        delete[] _torque_curve;
617}
618//------------------------------------------------------------------------------------------------
619void Vehicle::Wheel::setupTyreContact(OgreOde::Contact* contact)
620{
621        Quaternion vehicle_orient = _owner->getBody()->getOrientation();
622
623        Vector3 wheel_up = vehicle_orient * Ogre::Vector3::UNIT_Y;
624        Quaternion wheel_rot(Radian(getSteer()), (Vector3) wheel_up);
625        Vector3 wheel_forward = wheel_rot * (vehicle_orient * Ogre::Vector3::UNIT_Z);
626        Vector3 wheel_velocity = _body->getLinearVelocity();
627
628        contact->setFrictionMode(Contact::Flag_BothFrictionPyramids);
629        contact->setBouncyness(_bouncyness);
630        contact->setCoulombFriction(_friction);
631        contact->setFirstFrictionDirection(wheel_forward);
632        contact->setAdditionalFDS(wheel_velocity.length() * _fds);
633}
634//------------------------------------------------------------------------------------------------
635bool Vehicle::handleTyreCollision(OgreOde::Contact* contact)
636{
637        Geometry *geom = contact->getFirstGeometry();
638
639        Object* pObject = (Object*) geom->getUserObject();
640        if (pObject && 
641                (pObject->getObjectType() == OgreOde_Prefab::ObjectType_Wheel))
642        {
643                ((OgreOde_Prefab::Vehicle::Wheel*)(pObject))->setupTyreContact(contact);
644                return true;
645        }
646        else
647        {
648                geom = contact->getSecondGeometry();
649                pObject = (Object*) geom->getUserObject();
650                if (pObject && (pObject->getObjectType() == OgreOde_Prefab::ObjectType_Wheel))
651                {
652                        ((OgreOde_Prefab::Vehicle::Wheel*)pObject)->setupTyreContact(contact);
653                        return true;
654                }
655        }
656        return false;
657}
Note: See TracBrowser for help on using the repository browser.