vrsode/src/CarSimple.cpp

Go to the documentation of this file.

#include "CarSimple.h"
#include "PhysicsManager.h"
#include "CylinderShape.h"
#include "random_utils/LogManager.h"

#include <vrs/box.h>
#include <vrs/color.h>
#include <vrs/torus.h>
#include <vrs/vector.h>
#include <vrs/opengl/shapematerialgl.h>
#include <vrs/translation.h>


namespace vrsode
{

CarSimple::CarSimple(VRS::SO<VRS::SceneThing> p_parent)
{
    m_springLengthFront = 0.2;
    m_springLengthRear = 0.2;
    m_springPowerFront = 31;
    m_springPowerRear = 27;
    m_springSpeedFront = 1.4;
    m_springSpeedRear = 1.4;
    m_wheelSizeFront = 0.3;
    m_wheelSizeRear = 0.4;
    m_wheelMass = 0.01;
    m_bodyMass = 8.0;
    m_debugNodes = true;
    m_wheelMu = dInfinity;

    m_carMaxSpeed = 25;
    m_carPower = 50;

    m_chassisOffset         = VRS::Vector( 0.0, 1.6,  0.0);

    m_wheelOffsetFL         = VRS::Vector( 2.1, 1.0, -1.8);
    m_wheelOffsetFR         = VRS::Vector( 2.1, 1.0,  1.8);
    m_wheelOffsetRL         = VRS::Vector(-2.1, 1.0, -1.8);
    m_wheelOffsetRR         = VRS::Vector(-2.1, 1.0,  1.8);

    m_suspensionOffsetFL    = VRS::Vector( 2.1, 1.1, -0.5);
    m_suspensionOffsetFR    = VRS::Vector( 2.1, 1.1,  0.5);
    m_suspensionOffsetRL    = VRS::Vector(-2.1, 1.1, -0.5);
    m_suspensionOffsetRR    = VRS::Vector(-2.1, 1.1,  0.5);

    m_steerOffsetFL         = VRS::Vector( 2.3, 1.0, -0.5);
    m_steerOffsetFR         = VRS::Vector( 2.3, 1.0,  0.5);


    m_rootNode = new VRS::SceneThing(p_parent);
    m_rootNode ->append(new VRS::ShapeMaterialGL(
            VRS::Color(0.1, 0.1, 0.1), VRS::Color(0.1, 0.1, 0.1)));

    VRS::SO<VRS::SceneThing> st;

    // chassis body
    st = new VRS::SceneThing(m_rootNode);
    st->append(new VRS::ShapeMaterialGL(
            VRS::Color(1.0, 0.3, 0.3), VRS::Color(0.15, 0.05, 0.05)));
    st->append(new VRS::Box(VRS::Vector(-2.0, -0.2, -0.8),
                            VRS::Vector(2.0, 0.2, 0.8)));
    m_chassis = new PhysicsBody(st, CS_Box);
    m_chassis->setDebugNode(m_debugNodes);

    // wheel body front left
    st = new VRS::SceneThing(m_rootNode);
    st->append(new VRS::Torus(m_wheelSizeFront));

    VRS::SO<Shape> wheelShapeFL = 
        new CylinderShape(VRS::Vector(0,1,0),1,m_wheelSizeFront*2.0);
    m_wheelFL = new PhysicsBody(st, wheelShapeFL);
//     m_wheelFL = new PhysicsBody(st, CS_Sphere);
    m_wheelFL->setDebugNode(m_debugNodes);
    m_wheelFL->surface()->setMu(m_wheelMu);

    // suspension body front left
    st = new VRS::SceneThing(m_rootNode);
    m_suspensionFL = new PhysicsBody(st, CS_None);
    m_suspensionFL->setDebugNode(m_debugNodes);

    // steering body front left
    st = new VRS::SceneThing(m_rootNode);
    m_steerFL = new PhysicsBody(st, CS_None);
    m_steerFL->setDebugNode(m_debugNodes);

    // wheel body front right
    st = new VRS::SceneThing(m_rootNode);
    st->append(new VRS::Torus(m_wheelSizeFront));
    VRS::SO<Shape> wheelShapeFR = 
        new CylinderShape(VRS::Vector(0,1,0),1,m_wheelSizeFront*2.0);
    m_wheelFR = new PhysicsBody(st, wheelShapeFR);
//     m_wheelFR = new PhysicsBody(st, CS_Sphere);
    m_wheelFR->setDebugNode(m_debugNodes);
    m_wheelFR->surface()->setMu(m_wheelMu);

    // suspension body front right
    st = new VRS::SceneThing(m_rootNode);
    m_suspensionFR = new PhysicsBody(st, CS_None);
    m_suspensionFR->setDebugNode(m_debugNodes);

    // steering body front right
    st = new VRS::SceneThing(m_rootNode);
    m_steerFR = new PhysicsBody(st, CS_None);
    m_steerFR->setDebugNode(m_debugNodes);

    // wheel body rear left
    st = new VRS::SceneThing(m_rootNode);
    st->append(new VRS::Torus(m_wheelSizeRear));
    VRS::SO<Shape> wheelShapeRL = 
        new CylinderShape(VRS::Vector(0,1,0),1,m_wheelSizeRear*2.0);
    m_wheelRL = new PhysicsBody(st, wheelShapeRL);
//     m_wheelRL = new PhysicsBody(st, CS_Sphere);
    m_wheelRL->setDebugNode(m_debugNodes);
    m_wheelRL->surface()->setMu(m_wheelMu);

    // suspension body rear left
    st = new VRS::SceneThing(m_rootNode);
    m_suspensionRL = new PhysicsBody(st, CS_None);
    m_suspensionRL->setDebugNode(m_debugNodes);

    // wheel body rear right
    st = new VRS::SceneThing(m_rootNode);
    st->append(new VRS::Torus(m_wheelSizeRear));
    VRS::SO<Shape> wheelShapeRR = 
        new CylinderShape(VRS::Vector(0,1,0),1,m_wheelSizeRear*2.0);
    m_wheelRR = new PhysicsBody(st, wheelShapeRR);
//     m_wheelRR = new PhysicsBody(st, CS_Sphere);
    m_wheelRR->setDebugNode(m_debugNodes);
    m_wheelRR->surface()->setMu(m_wheelMu);

    // suspension body rear right
    st = new VRS::SceneThing(m_rootNode);
    m_suspensionRR = new PhysicsBody(st, CS_None);
    m_suspensionRR->setDebugNode(m_debugNodes);

    // // rotate wheels to position
    m_wheelFL->setRotation(1,0,0,90);
    m_wheelFR->setRotation(1,0,0,90);
    m_wheelRL->setRotation(1,0,0,90);
    m_wheelRR->setRotation(1,0,0,90);

    // position chassis bodies
    m_chassis->setPosition(m_chassisOffset);

    // position wheel bodies
    m_wheelFL->setPosition(m_wheelOffsetFL);
    m_wheelFR->setPosition(m_wheelOffsetFR);
    m_wheelRL->setPosition(m_wheelOffsetRL);
    m_wheelRR->setPosition(m_wheelOffsetRR);

    // position steer bodies
    m_steerFL->setPosition(m_steerOffsetFL);
    m_steerFR->setPosition(m_steerOffsetFR);

    // position suspension bodies
    m_suspensionFL->setPosition(m_suspensionOffsetFL);
    m_suspensionFR->setPosition(m_suspensionOffsetFR);
    m_suspensionRL->setPosition(m_suspensionOffsetRL);
    m_suspensionRR->setPosition(m_suspensionOffsetRR);

    // joints
    // front left
    // connect wheel and steering
    m_axisFL = new PhysicsJointHinge(m_wheelFL, m_steerFL,
                             VRS::Vector( 2.1, 1.0,-1.8),
                             VRS::Vector(0,0,1));

    // connect steering and suspension
    m_steerControlFL =new PhysicsJointHinge(m_steerFL, m_suspensionFL,
                             VRS::Vector( 2.1, 0.7, -1.8),
                             VRS::Vector(0,1,0));

    // connect suspension and chassis
    m_springFL = new PhysicsJointSlider(m_chassis, m_suspensionFL,
                             VRS::Vector(0,1,0));

    // front right
    // connect wheel and steering
    m_axisFR = new PhysicsJointHinge(m_wheelFR, m_steerFR,
                             VRS::Vector( 2.1, 1.0, 1.8),
                             VRS::Vector(0,0,1));


    // connect steering and suspension
    m_steerControlFR = new PhysicsJointHinge(m_steerFR, m_suspensionFR,
                             VRS::Vector( 2.1, 0.7, 1.8),
                             VRS::Vector(0,1,0));


    // connect suspension and chassis
    m_springFR = new PhysicsJointSlider(m_chassis, m_suspensionFR,
                             VRS::Vector(0,1,0));

    // rear left
    // connect wheel and suspension
    m_axisRL = new PhysicsJointHinge(m_suspensionRL, m_wheelRL,
                             VRS::Vector(-2.1, 1.0,-1.8),
                             VRS::Vector(0,0,1));

    // connect suspension and chassis
    m_springRL = new PhysicsJointSlider(m_chassis, m_suspensionRL,
                             VRS::Vector(0,1,0));

    // rear right
    // connect wheel and suspension
    m_axisRR = new PhysicsJointHinge(m_suspensionRR, m_wheelRR,
                             VRS::Vector(-2.1, 1.0, 1.8),
                             VRS::Vector(0,0,1));

    // connect suspension and chassis
    m_springRR = new PhysicsJointSlider(m_chassis, m_suspensionRR,
                             VRS::Vector(0,1,0));

    // high speed rotation mode
    m_wheelFL->setHighSpeedRotationAxis(VRS::Vector(0,0,1));
    m_wheelFR->setHighSpeedRotationAxis(VRS::Vector(0,0,1));
    m_wheelRL->setHighSpeedRotationAxis(VRS::Vector(0,0,1));
    m_wheelRR->setHighSpeedRotationAxis(VRS::Vector(0,0,1));

    m_wheelFL->setHighSpeedRotationMode(true);
    m_wheelFR->setHighSpeedRotationMode(true);
    m_wheelRL->setHighSpeedRotationMode(true);
    m_wheelRR->setHighSpeedRotationMode(true);

    // set masses
    m_chassis->setMass(m_bodyMass, VRS::Vector(0,-30,0));
    m_wheelFL->setMass(m_wheelMass);
    m_wheelFR->setMass(m_wheelMass);
    m_wheelRL->setMass(m_wheelMass);
    m_wheelRR->setMass(m_wheelMass);

    // configure suspension springs
    m_springFL->setHighStop(0.0);
    m_springFL->setLowStop(-m_springLengthFront);
    m_springFL->setMotorVelocity(m_springSpeedFront);
    m_springFL->setMotorMaxForce(m_springPowerFront);

    m_springFR->setHighStop(0.0);
    m_springFR->setLowStop(-m_springLengthFront);
    m_springFR->setMotorVelocity(m_springSpeedFront);
    m_springFR->setMotorMaxForce(m_springPowerFront);

    m_springRL->setHighStop(0.0);
    m_springRL->setLowStop(-m_springLengthRear);
    m_springRL->setMotorVelocity(m_springSpeedRear);
    m_springRL->setMotorMaxForce(m_springPowerRear);

    m_springRR->setHighStop(0.0);
    m_springRR->setLowStop(-m_springLengthRear);
    m_springRR->setMotorVelocity(m_springSpeedRear);
    m_springRR->setMotorMaxForce(m_springPowerRear);

    // configure steering
    m_steerControlFL->setHighStop(0.0);
    m_steerControlFL->setLowStop(0.0);
    m_steerControlFR->setHighStop(0.0);
    m_steerControlFR->setLowStop(0.0);

//     // cfm
//     double cfm = 0.000001;
//
//     m_axisFL->setCFM(cfm);
//     m_axisFR->setCFM(cfm);
//     m_axisRL->setCFM(cfm);
//     m_axisRR->setCFM(cfm);
//
//     m_steerControlFL->setCFM(cfm);
//     m_steerControlFR->setCFM(cfm);
//
//     m_springFL->setCFM(cfm);
//     m_springFR->setCFM(cfm);
//     m_springRL->setCFM(cfm);
//     m_springRR->setCFM(cfm);

    // done :)
}

CarSimple::~CarSimple()
{
    // TODO: cleanup
}

void
CarSimple::accelerate(double p_amount)
{
    enableAllBodies();

    if(p_amount)
    {
        m_axisRL->setMotorVelocity(m_carMaxSpeed * p_amount);
        m_axisRR->setMotorVelocity(m_carMaxSpeed * p_amount);
        m_axisRL->setMotorMaxForce(m_carPower);
        m_axisRR->setMotorMaxForce(m_carPower);
    }
    else
    {
        m_axisRL->setMotorVelocity(m_carMaxSpeed);
        m_axisRR->setMotorVelocity(m_carMaxSpeed);
        m_axisRL->setMotorMaxForce(0);
        m_axisRR->setMotorMaxForce(0);
    }
}

void
CarSimple::steerLeft(double p_amount)
{
    enableAllBodies();
    m_steerControlFL->setHighStop(-0.55 * p_amount);
    m_steerControlFL->setLowStop(-0.55 * p_amount);
    m_steerControlFR->setHighStop(-0.55 * p_amount);
    m_steerControlFR->setLowStop(-0.55 * p_amount);
}

void
CarSimple::steerRight(double p_amount)
{
    enableAllBodies();
    m_steerControlFL->setHighStop(0.55 * p_amount);
    m_steerControlFL->setLowStop(0.55 * p_amount);
    m_steerControlFR->setHighStop(0.55 * p_amount);
    m_steerControlFR->setLowStop(0.55 * p_amount);
}

void
CarSimple::nos(double p_amount)
{
    enableAllBodies();
    m_chassis->addRelativeForceAtRelativePosition(
            VRS::Vector(350 * p_amount,-40,0),
            VRS::Vector(-2,0,0));
}

void
CarSimple::handBreak(bool p_enable)
{
    enableAllBodies();
    if(p_enable)
    {
        m_axisFL->setMotorVelocity(0);
        m_axisFR->setMotorVelocity(0);
        m_axisFL->setMotorMaxForce(15);
        m_axisFR->setMotorMaxForce(15);
        m_axisRL->setMotorVelocity(0);
        m_axisRR->setMotorVelocity(0);
        m_axisRL->setMotorMaxForce(200);
        m_axisRR->setMotorMaxForce(200);
    }
    else
    {
        m_axisFL->setMotorVelocity(100);
        m_axisFR->setMotorVelocity(100);
        m_axisFL->setMotorMaxForce(0);
        m_axisFR->setMotorMaxForce(0);
        m_axisRL->setMotorVelocity(100);
        m_axisRR->setMotorVelocity(100);
        m_axisRL->setMotorMaxForce(0);
        m_axisRR->setMotorMaxForce(0);
    }
}

VRS::SO<VRS::SceneThing>
CarSimple::sceneThing()
{
    return m_chassis->sceneThing();
}

void
CarSimple::attachCamera(VRS::SO<VRS::Camera> p_camera)
{

}

void
CarSimple::enableAllBodies()
{
     m_chassis->setEnabled(true);

     m_wheelFL->setEnabled(true);
     m_wheelFR->setEnabled(true);
     m_wheelRL->setEnabled(true);
     m_wheelRR->setEnabled(true);

     m_suspensionFL->setEnabled(true);
     m_suspensionFR->setEnabled(true);
     m_suspensionRL->setEnabled(true);
     m_suspensionRR->setEnabled(true);

     m_steerFL->setEnabled(true);
     m_steerFR->setEnabled(true);
}

void
CarSimple::setPosition(VRS::Vector p_position)
{
    // position chassis bodies
    m_chassis->setPosition(m_chassisOffset + p_position);
    m_chassis->setLinearVelocity(VRS::Vector(0,0,0));
    m_chassis->setAngularVelocity(VRS::Vector(0,0,0));

    // position wheel bodies
    m_wheelFL->setPosition(m_wheelOffsetFL + p_position);
    m_wheelFR->setPosition(m_wheelOffsetFR + p_position);
    m_wheelRL->setPosition(m_wheelOffsetRL + p_position);
    m_wheelRR->setPosition(m_wheelOffsetRR + p_position);
    m_wheelFL->setLinearVelocity(VRS::Vector(0,0,0));
    m_wheelFR->setLinearVelocity(VRS::Vector(0,0,0));
    m_wheelRL->setLinearVelocity(VRS::Vector(0,0,0));
    m_wheelRR->setLinearVelocity(VRS::Vector(0,0,0));
    m_wheelFL->setAngularVelocity(VRS::Vector(0,0,0));
    m_wheelFR->setAngularVelocity(VRS::Vector(0,0,0));
    m_wheelRL->setAngularVelocity(VRS::Vector(0,0,0));
    m_wheelRR->setAngularVelocity(VRS::Vector(0,0,0));

    // position steer bodies
    m_steerFL->setPosition(m_steerOffsetFL + p_position);
    m_steerFR->setPosition(m_steerOffsetFR + p_position);
    m_steerFL->setLinearVelocity(VRS::Vector(0,0,0));
    m_steerFR->setLinearVelocity(VRS::Vector(0,0,0));
    m_steerFL->setAngularVelocity(VRS::Vector(0,0,0));
    m_steerFR->setAngularVelocity(VRS::Vector(0,0,0));

    // position suspension bodies
    m_suspensionFL->setPosition(m_suspensionOffsetFL + p_position);
    m_suspensionFR->setPosition(m_suspensionOffsetFR + p_position);
    m_suspensionRL->setPosition(m_suspensionOffsetRL + p_position);
    m_suspensionRR->setPosition(m_suspensionOffsetRR + p_position);
    m_suspensionFL->setLinearVelocity(VRS::Vector(0,0,0));
    m_suspensionFR->setLinearVelocity(VRS::Vector(0,0,0));
    m_suspensionRL->setLinearVelocity(VRS::Vector(0,0,0));
    m_suspensionRR->setLinearVelocity(VRS::Vector(0,0,0));
    m_suspensionFL->setAngularVelocity(VRS::Vector(0,0,0));
    m_suspensionFR->setAngularVelocity(VRS::Vector(0,0,0));
    m_suspensionRL->setAngularVelocity(VRS::Vector(0,0,0));
    m_suspensionRR->setAngularVelocity(VRS::Vector(0,0,0));

    enableAllBodies();
}

void
CarSimple::setPosition(double p_x, double p_y, double p_z)
{
    setPosition(VRS::Vector(p_x, p_y, p_z));
}

VRS::Vector
CarSimple::position()
{
//     return m_chassis->position() - m_chassisOffset;
    return m_chassis->translationNode()->getTranslate();
}

} // namespace vrsode

---