Random Racer: random_racer/src/TestCar.cpp Source File

00001 
00002 #include "TestCar.h"
00003 #include "random_utils/LogManager.h"
00004 #include "vrsode/PhysicsManager.h"
00005 #include "vrsode/CylinderShape.h"
00006 #include "vrsode/SphereShape.h"
00007 
00008 #include <vrs/box.h>
00009 #include <vrs/color.h>
00010 #include <vrs/torus.h>
00011 #include <vrs/vector.h>
00012 #include <vrs/opengl/shapematerialgl.h>
00013 #include <vrs/translation.h>
00014 #include <vrs/io/threedsreader.h>
00015 
00016 
00017 using namespace vrsode;
00018 
00019 namespace random_racer
00020 {
00021 
00022 TestCar::TestCar(VRS::SO<VRS::SceneThing> p_parent) : Car(p_parent)
00023 {
00024     m_springLengthFront = 0.2;
00025     m_springLengthRear = 0.2;
00026     m_springPowerFront = 31;
00027     m_springPowerRear = 27;
00028     m_springSpeedFront = 1.4;
00029     m_springSpeedRear = 1.4;
00030     m_wheelSizeFront = 0.3;
00031     m_wheelSizeRear = 0.4;
00032     m_wheelMass = 0.01;
00033     m_bodyMass = 8.0;
00034     m_debugNodes = true;
00035     m_wheelMu = dInfinity;
00036 
00037     m_carMaxSpeed = 25;
00038     m_carPower = 30;
00039 
00040     m_chassisOffset         = VRS::Vector( 0.0, 1.6,  0.0);
00041 
00042     m_wheelOffsetFL         = VRS::Vector( 2.1, 1.0, -1.8);
00043     m_wheelOffsetFR         = VRS::Vector( 2.1, 1.0,  1.8);
00044     m_wheelOffsetRL         = VRS::Vector(-2.1, 1.0, -1.8);
00045     m_wheelOffsetRR         = VRS::Vector(-2.1, 1.0,  1.8);
00046 
00047     m_suspensionOffsetFL    = VRS::Vector( 2.1, 1.1, -0.5);
00048     m_suspensionOffsetFR    = VRS::Vector( 2.1, 1.1,  0.5);
00049     m_suspensionOffsetRL    = VRS::Vector(-2.1, 1.1, -0.5);
00050     m_suspensionOffsetRR    = VRS::Vector(-2.1, 1.1,  0.5);
00051 
00052     m_steerOffsetFL         = VRS::Vector( 2.3, 1.0, -0.5);
00053     m_steerOffsetFR         = VRS::Vector( 2.3, 1.0,  0.5);
00054 
00055 
00056     VRS::SO<VRS::ThreeDSReader> reader = new VRS::ThreeDSReader();
00057 
00058     // st->append(((VRS::SceneThing*)reader->readObject(
00059     //         "car01chassis.3ds", "Cube.002")->object(1)));
00060 
00061     m_rootNode = new VRS::SceneThing(p_parent);
00062     m_rootNode ->append(new VRS::ShapeMaterialGL(
00063             VRS::Color(0.1, 0.1, 0.1), VRS::Color(0.1, 0.1, 0.1)));
00064 
00065     VRS::SO<VRS::SceneThing> st;
00066 
00067     // chassis body
00068     st = new VRS::SceneThing(m_rootNode);
00069     st->append(new VRS::ShapeMaterialGL(
00070             VRS::Color(1.0, 0.3, 0.3), VRS::Color(0.15, 0.05, 0.05)));
00071     st->append(new VRS::Box(VRS::Vector(-2.0, -0.2, -0.8),
00072                             VRS::Vector(2.0, 0.2, 0.8)));
00073     m_chassis = new PhysicsBody(st, new SphereShape(VRS::Bounds(
00074             VRS::Vector(-0.2, -0.2, -0.2), VRS::Vector(0.2, 0.2, 0.2))));
00075     // m_chassis = new PhysicsBody(st, CS_Box);
00076     m_chassis->setDebugNode(m_debugNodes);
00077 
00078     // wheel body front left
00079     st = new VRS::SceneThing(m_rootNode);
00080     st->append(new VRS::Torus(m_wheelSizeFront));
00081 
00082     VRS::SO<Shape> wheelShapeFL = 
00083         new CylinderShape(VRS::Vector(0,1,0),1,m_wheelSizeFront*2.0);
00084     m_wheelFL = new PhysicsBody(st, wheelShapeFL);
00085 //     m_wheelFL = new PhysicsBody(st, CS_Sphere);
00086     m_wheelFL->setDebugNode(m_debugNodes);
00087     m_wheelFL->surface()->setMu(m_wheelMu);
00088 
00089     // suspension body front left
00090     st = new VRS::SceneThing(m_rootNode);
00091     m_suspensionFL = new PhysicsBody(st, CS_None);
00092     m_suspensionFL->setDebugNode(m_debugNodes);
00093 
00094     // steering body front left
00095     st = new VRS::SceneThing(m_rootNode);
00096     m_steerFL = new PhysicsBody(st, CS_None);
00097     m_steerFL->setDebugNode(m_debugNodes);
00098 
00099     // wheel body front right
00100     st = new VRS::SceneThing(m_rootNode);
00101     st->append(new VRS::Torus(m_wheelSizeFront));
00102     VRS::SO<Shape> wheelShapeFR = 
00103         new CylinderShape(VRS::Vector(0,1,0),1,m_wheelSizeFront*2.0);
00104     m_wheelFR = new PhysicsBody(st, wheelShapeFR);
00105 //     m_wheelFR = new PhysicsBody(st, CS_Sphere);
00106     m_wheelFR->setDebugNode(m_debugNodes);
00107     m_wheelFR->surface()->setMu(m_wheelMu);
00108 
00109     // suspension body front right
00110     st = new VRS::SceneThing(m_rootNode);
00111     m_suspensionFR = new PhysicsBody(st, CS_None);
00112     m_suspensionFR->setDebugNode(m_debugNodes);
00113 
00114     // steering body front right
00115     st = new VRS::SceneThing(m_rootNode);
00116     m_steerFR = new PhysicsBody(st, CS_None);
00117     m_steerFR->setDebugNode(m_debugNodes);
00118 
00119     // wheel body rear left
00120     st = new VRS::SceneThing(m_rootNode);
00121     st->append(new VRS::Torus(m_wheelSizeRear));
00122     VRS::SO<Shape> wheelShapeRL = 
00123         new CylinderShape(VRS::Vector(0,1,0),1,m_wheelSizeRear*2.0);
00124     m_wheelRL = new PhysicsBody(st, wheelShapeRL);
00125 //     m_wheelRL = new PhysicsBody(st, CS_Sphere);
00126     m_wheelRL->setDebugNode(m_debugNodes);
00127     m_wheelRL->surface()->setMu(m_wheelMu);
00128 
00129     // suspension body rear left
00130     st = new VRS::SceneThing(m_rootNode);
00131     m_suspensionRL = new PhysicsBody(st, CS_None);
00132     m_suspensionRL->setDebugNode(m_debugNodes);
00133 
00134     // wheel body rear right
00135     st = new VRS::SceneThing(m_rootNode);
00136     st->append(new VRS::Torus(m_wheelSizeRear));
00137     VRS::SO<Shape> wheelShapeRR = 
00138         new CylinderShape(VRS::Vector(0,1,0),1,m_wheelSizeRear*2.0);
00139     m_wheelRR = new PhysicsBody(st, wheelShapeRR);
00140 //     m_wheelRR = new PhysicsBody(st, CS_Sphere);
00141     m_wheelRR->setDebugNode(m_debugNodes);
00142     m_wheelRR->surface()->setMu(m_wheelMu);
00143 
00144     // suspension body rear right
00145     st = new VRS::SceneThing(m_rootNode);
00146     m_suspensionRR = new PhysicsBody(st, CS_None);
00147     m_suspensionRR->setDebugNode(m_debugNodes);
00148 
00149     // // rotate wheels to position
00150     m_wheelFL->setRotation(1,0,0,90);
00151     m_wheelFR->setRotation(1,0,0,90);
00152     m_wheelRL->setRotation(1,0,0,90);
00153     m_wheelRR->setRotation(1,0,0,90);
00154 
00155     // position chassis bodies
00156     m_chassis->setPosition(m_chassisOffset);
00157 
00158     // position wheel bodies
00159     m_wheelFL->setPosition(m_wheelOffsetFL);
00160     m_wheelFR->setPosition(m_wheelOffsetFR);
00161     m_wheelRL->setPosition(m_wheelOffsetRL);
00162     m_wheelRR->setPosition(m_wheelOffsetRR);
00163 
00164     // position steer bodies
00165     m_steerFL->setPosition(m_steerOffsetFL);
00166     m_steerFR->setPosition(m_steerOffsetFR);
00167 
00168     // position suspension bodies
00169     m_suspensionFL->setPosition(m_suspensionOffsetFL);
00170     m_suspensionFR->setPosition(m_suspensionOffsetFR);
00171     m_suspensionRL->setPosition(m_suspensionOffsetRL);
00172     m_suspensionRR->setPosition(m_suspensionOffsetRR);
00173 
00174     // joints
00175     // front left
00176     // connect wheel and steering
00177     m_axisFL = new PhysicsJointHinge(m_wheelFL, m_steerFL,
00178                              VRS::Vector( 2.1, 1.0,-1.8),
00179                              VRS::Vector(0,0,1));
00180 
00181     // connect steering and suspension
00182     m_steerControlFL =new PhysicsJointHinge(m_steerFL, m_suspensionFL,
00183                              VRS::Vector( 2.1, 0.7, -1.8),
00184                              VRS::Vector(0,1,0));
00185 
00186     // connect suspension and chassis
00187     m_springFL = new PhysicsJointSlider(m_chassis, m_suspensionFL,
00188                              VRS::Vector(0,1,0));
00189 
00190     // front right
00191     // connect wheel and steering
00192     m_axisFR = new PhysicsJointHinge(m_wheelFR, m_steerFR,
00193                              VRS::Vector( 2.1, 1.0, 1.8),
00194                              VRS::Vector(0,0,1));
00195 
00196 
00197     // connect steering and suspension
00198     m_steerControlFR = new PhysicsJointHinge(m_steerFR, m_suspensionFR,
00199                              VRS::Vector( 2.1, 0.7, 1.8),
00200                              VRS::Vector(0,1,0));
00201 
00202 
00203     // connect suspension and chassis
00204     m_springFR = new PhysicsJointSlider(m_chassis, m_suspensionFR,
00205                              VRS::Vector(0,1,0));
00206 
00207     // rear left
00208     // connect wheel and suspension
00209     m_axisRL = new PhysicsJointHinge(m_suspensionRL, m_wheelRL,
00210                              VRS::Vector(-2.1, 1.0,-1.8),
00211                              VRS::Vector(0,0,1));
00212 
00213     // connect suspension and chassis
00214     m_springRL = new PhysicsJointSlider(m_chassis, m_suspensionRL,
00215                              VRS::Vector(0,1,0));
00216 
00217     // rear right
00218     // connect wheel and suspension
00219     m_axisRR = new PhysicsJointHinge(m_suspensionRR, m_wheelRR,
00220                              VRS::Vector(-2.1, 1.0, 1.8),
00221                              VRS::Vector(0,0,1));
00222 
00223     // connect suspension and chassis
00224     m_springRR = new PhysicsJointSlider(m_chassis, m_suspensionRR,
00225                              VRS::Vector(0,1,0));
00226 
00227     // high speed rotation mode
00228     m_wheelFL->setHighSpeedRotationAxis(VRS::Vector(0,0,1));
00229     m_wheelFR->setHighSpeedRotationAxis(VRS::Vector(0,0,1));
00230     m_wheelRL->setHighSpeedRotationAxis(VRS::Vector(0,0,1));
00231     m_wheelRR->setHighSpeedRotationAxis(VRS::Vector(0,0,1));
00232 
00233     m_wheelFL->setHighSpeedRotationMode(true);
00234     m_wheelFR->setHighSpeedRotationMode(true);
00235     m_wheelRL->setHighSpeedRotationMode(true);
00236     m_wheelRR->setHighSpeedRotationMode(true);
00237 
00238     // set masses
00239     m_chassis->setMass(m_bodyMass, VRS::Vector(0,-30,0));
00240     m_wheelFL->setMass(m_wheelMass);
00241     m_wheelFR->setMass(m_wheelMass);
00242     m_wheelRL->setMass(m_wheelMass);
00243     m_wheelRR->setMass(m_wheelMass);
00244 
00245     // configure suspension springs
00246     m_springFL->setHighStop(0.0);
00247     m_springFL->setLowStop(-m_springLengthFront);
00248     m_springFL->setMotorVelocity(m_springSpeedFront);
00249     m_springFL->setMotorMaxForce(m_springPowerFront);
00250 
00251     m_springFR->setHighStop(0.0);
00252     m_springFR->setLowStop(-m_springLengthFront);
00253     m_springFR->setMotorVelocity(m_springSpeedFront);
00254     m_springFR->setMotorMaxForce(m_springPowerFront);
00255 
00256     m_springRL->setHighStop(0.0);
00257     m_springRL->setLowStop(-m_springLengthRear);
00258     m_springRL->setMotorVelocity(m_springSpeedRear);
00259     m_springRL->setMotorMaxForce(m_springPowerRear);
00260 
00261     m_springRR->setHighStop(0.0);
00262     m_springRR->setLowStop(-m_springLengthRear);
00263     m_springRR->setMotorVelocity(m_springSpeedRear);
00264     m_springRR->setMotorMaxForce(m_springPowerRear);
00265 
00266     // configure steering
00267     m_steerControlFL->setHighStop(0.0);
00268     m_steerControlFL->setLowStop(0.0);
00269     m_steerControlFR->setHighStop(0.0);
00270     m_steerControlFR->setLowStop(0.0);
00271 
00272 //     // cfm
00273 //     double cfm = 0.000001;
00274 //
00275 //     m_axisFL->setCFM(cfm);
00276 //     m_axisFR->setCFM(cfm);
00277 //     m_axisRL->setCFM(cfm);
00278 //     m_axisRR->setCFM(cfm);
00279 //
00280 //     m_steerControlFL->setCFM(cfm);
00281 //     m_steerControlFR->setCFM(cfm);
00282 //
00283 //     m_springFL->setCFM(cfm);
00284 //     m_springFR->setCFM(cfm);
00285 //     m_springRL->setCFM(cfm);
00286 //     m_springRR->setCFM(cfm);
00287 
00288     // done :)
00289 
00290     // m_chassis->setAutoDisableMode(true);
00291     // 
00292     // m_wheelFL->setAutoDisableMode(true);
00293     // m_wheelFR->setAutoDisableMode(true);
00294     // m_wheelRL->setAutoDisableMode(true);
00295     // m_wheelRR->setAutoDisableMode(true);
00296     // 
00297     // m_suspensionFL->setAutoDisableMode(true);
00298     // m_suspensionFR->setAutoDisableMode(true);
00299     // m_suspensionRL->setAutoDisableMode(true);
00300     // m_suspensionRR->setAutoDisableMode(true);
00301     // 
00302     // m_steerFL->setAutoDisableMode(true);
00303     // m_steerFR->setAutoDisableMode(true);
00304 
00305 }
00306 
00307 TestCar::~TestCar()
00308 {
00309     // TODO: cleanup
00310 }
00311 
00312 void
00313 TestCar::accelerate(double p_amount)
00314 {
00315     enableAllBodies();
00316 
00317     if(p_amount)
00318     {
00319         m_axisRL->setMotorVelocity(m_carMaxSpeed * p_amount);
00320         m_axisRR->setMotorVelocity(m_carMaxSpeed * p_amount);
00321         m_axisRL->setMotorMaxForce(m_carPower);
00322         m_axisRR->setMotorMaxForce(m_carPower);
00323     }
00324     else
00325     {
00326         m_axisRL->setMotorVelocity(m_carMaxSpeed);
00327         m_axisRR->setMotorVelocity(m_carMaxSpeed);
00328         m_axisRL->setMotorMaxForce(0);
00329         m_axisRR->setMotorMaxForce(0);
00330     }
00331 }
00332 
00333 void
00334 TestCar::steer(double p_amount)
00335 {
00336     enableAllBodies();
00337     m_steerControlFL->setHighStop(0.55 * p_amount);
00338     m_steerControlFL->setLowStop(0.55 * p_amount);
00339     m_steerControlFR->setHighStop(0.55 * p_amount);
00340     m_steerControlFR->setLowStop(0.55 * p_amount);
00341 }
00342 
00343 void
00344 TestCar::nitro(double p_amount)
00345 {
00346     enableAllBodies();
00347     m_chassis->addRelativeForceAtRelativePosition(
00348             VRS::Vector(350 * p_amount,-40,0),
00349             VRS::Vector(-2,0,0));
00350 }
00351 
00352 void
00353 TestCar::handBreak(double p_enable)
00354 {
00355     enableAllBodies();
00356     if(p_enable == 1.0)
00357     {
00358         m_axisFL->setMotorVelocity(0);
00359         m_axisFR->setMotorVelocity(0);
00360         m_axisFL->setMotorMaxForce(15);
00361         m_axisFR->setMotorMaxForce(15);
00362         m_axisRL->setMotorVelocity(0);
00363         m_axisRR->setMotorVelocity(0);
00364         m_axisRL->setMotorMaxForce(200);
00365         m_axisRR->setMotorMaxForce(200);
00366     }
00367     else
00368     {
00369         m_axisFL->setMotorVelocity(100);
00370         m_axisFR->setMotorVelocity(100);
00371         m_axisFL->setMotorMaxForce(0);
00372         m_axisFR->setMotorMaxForce(0);
00373         m_axisRL->setMotorVelocity(100);
00374         m_axisRR->setMotorVelocity(100);
00375         m_axisRL->setMotorMaxForce(0);
00376         m_axisRR->setMotorMaxForce(0);
00377     }
00378 }
00379 
00380 VRS::SO<VRS::SceneThing>
00381 TestCar::sceneThing()
00382 {
00383     return m_chassis->sceneThing();
00384 }
00385 
00386 void
00387 TestCar::attachCamera(VRS::SO<VRS::Camera> p_camera)
00388 {
00389 
00390 }
00391 
00392 void
00393 TestCar::enableAllBodies()
00394 {
00395      m_chassis->setEnabled(true);
00396 
00397      m_wheelFL->setEnabled(true);
00398      m_wheelFR->setEnabled(true);
00399      m_wheelRL->setEnabled(true);
00400      m_wheelRR->setEnabled(true);
00401 
00402      m_suspensionFL->setEnabled(true);
00403      m_suspensionFR->setEnabled(true);
00404      m_suspensionRL->setEnabled(true);
00405      m_suspensionRR->setEnabled(true);
00406 
00407      m_steerFL->setEnabled(true);
00408      m_steerFR->setEnabled(true);
00409 }
00410 
00411 void
00412 TestCar::setPosition(VRS::Vector p_position)
00413 {
00414     // position chassis bodies
00415     m_chassis->setPosition(m_chassisOffset + p_position);
00416     m_chassis->setLinearVelocity(VRS::Vector(0,0,0));
00417     m_chassis->setAngularVelocity(VRS::Vector(0,0,0));
00418 
00419     // position wheel bodies
00420     m_wheelFL->setPosition(m_wheelOffsetFL + p_position);
00421     m_wheelFR->setPosition(m_wheelOffsetFR + p_position);
00422     m_wheelRL->setPosition(m_wheelOffsetRL + p_position);
00423     m_wheelRR->setPosition(m_wheelOffsetRR + p_position);
00424     m_wheelFL->setLinearVelocity(VRS::Vector(0,0,0));
00425     m_wheelFR->setLinearVelocity(VRS::Vector(0,0,0));
00426     m_wheelRL->setLinearVelocity(VRS::Vector(0,0,0));
00427     m_wheelRR->setLinearVelocity(VRS::Vector(0,0,0));
00428     m_wheelFL->setAngularVelocity(VRS::Vector(0,0,0));
00429     m_wheelFR->setAngularVelocity(VRS::Vector(0,0,0));
00430     m_wheelRL->setAngularVelocity(VRS::Vector(0,0,0));
00431     m_wheelRR->setAngularVelocity(VRS::Vector(0,0,0));
00432 
00433     // position steer bodies
00434     m_steerFL->setPosition(m_steerOffsetFL + p_position);
00435     m_steerFR->setPosition(m_steerOffsetFR + p_position);
00436     m_steerFL->setLinearVelocity(VRS::Vector(0,0,0));
00437     m_steerFR->setLinearVelocity(VRS::Vector(0,0,0));
00438     m_steerFL->setAngularVelocity(VRS::Vector(0,0,0));
00439     m_steerFR->setAngularVelocity(VRS::Vector(0,0,0));
00440 
00441     // position suspension bodies
00442     m_suspensionFL->setPosition(m_suspensionOffsetFL + p_position);
00443     m_suspensionFR->setPosition(m_suspensionOffsetFR + p_position);
00444     m_suspensionRL->setPosition(m_suspensionOffsetRL + p_position);
00445     m_suspensionRR->setPosition(m_suspensionOffsetRR + p_position);
00446     m_suspensionFL->setLinearVelocity(VRS::Vector(0,0,0));
00447     m_suspensionFR->setLinearVelocity(VRS::Vector(0,0,0));
00448     m_suspensionRL->setLinearVelocity(VRS::Vector(0,0,0));
00449     m_suspensionRR->setLinearVelocity(VRS::Vector(0,0,0));
00450     m_suspensionFL->setAngularVelocity(VRS::Vector(0,0,0));
00451     m_suspensionFR->setAngularVelocity(VRS::Vector(0,0,0));
00452     m_suspensionRL->setAngularVelocity(VRS::Vector(0,0,0));
00453     m_suspensionRR->setAngularVelocity(VRS::Vector(0,0,0));
00454 }
00455 
00456 void
00457 TestCar::setPosition(double p_x, double p_y, double p_z)
00458 {
00459     setPosition(VRS::Vector(p_x, p_y, p_z));
00460 }
00461 
00462 VRS::Vector
00463 TestCar::position()
00464 {
00465 //     return m_chassis->position() - m_chassisOffset;
00466     return m_chassis->translationNode()->getTranslate();
00467 }
00468 
00469 } // namespace vrsode