vrsode/src/PhysicsBody.cpp

Go to the documentation of this file.

#include "PhysicsBody.h"
#include "PhysicsManager.h"
#include "random_utils/LogManager.h"

#include <ode/ode.h>

#include <vrs/container/iterator.h>
#include <vrs/sg/scenething.h>

#include <vrs/rotation.h>
#include <vrs/translation.h>

namespace vrsode
{

PhysicsBody::PhysicsBody(VRS::SO<VRS::SceneThing> p_sceneThing,
                         CollisionShape p_collisionShape, bool p_forceSync)
    : CollisionBody(p_sceneThing, p_collisionShape, false)
{
    m_forceSync = p_forceSync;
    m_highSpeedRotationMode = false;
    m_highSpeedRotationAxis = VRS::Vector();

    dWorldID worldId = PhysicsManager::get()->worldId();

    m_bodyId = dBodyCreate(worldId);

    if(geomId())
        dGeomSetBody(geomId(), m_bodyId);

    PhysicsManager::get()->registerPhysicsBody(this);

    assert(!geomId() || dGeomGetBody(geomId()) == m_bodyId);

    setMass(1);
}

PhysicsBody::PhysicsBody(VRS::SO<VRS::SceneThing> p_sceneThing,
                         VRS::SO<Shape> p_shape, bool p_forceSync)
    : CollisionBody(p_sceneThing, p_shape, false)
{
    m_forceSync = p_forceSync;
    m_highSpeedRotationMode = false;
    m_highSpeedRotationAxis = VRS::Vector();

    dWorldID worldId = PhysicsManager::get()->worldId();

    m_bodyId = dBodyCreate(worldId);

    if(geomId())
        dGeomSetBody(geomId(), m_bodyId);

    PhysicsManager::get()->registerPhysicsBody(this);

    assert(!geomId() || dGeomGetBody(geomId()) == m_bodyId);
    
    setMass(1);
}

PhysicsBody::~PhysicsBody()
{
}

void
PhysicsBody::destroy()
{
    PhysicsManager::get()->unregisterCollisionBody(this);
    dBodyDestroy(bodyId());
    dGeomDestroy(geomId());
}

dBodyID
PhysicsBody::bodyId()
{
    return m_bodyId;
}

double
PhysicsBody::mass()
{
    dMass mass;
    dBodyGetMass(m_bodyId, &mass);
    return mass.mass;
}

void
PhysicsBody::setMass(double p_massValue, VRS::Vector p_center)
{
    dMass mass;
    dMassSetZero(&mass);

    dMassSetParameters(&mass, p_massValue,
        p_center[0], p_center[1], p_center[2],
        0,0,0,0,0,0);
    
    dMassCheck(&mass);
    dBodySetMass(m_bodyId, &mass);
}

void
PhysicsBody::setHighSpeedRotationMode(bool p_mode)
{
    m_highSpeedRotationMode = p_mode;
    dBodySetFiniteRotationMode(m_bodyId, !p_mode);

    if(p_mode)
        dBodySetFiniteRotationAxis(m_bodyId, 
                                m_highSpeedRotationAxis[0], 
                                m_highSpeedRotationAxis[1], 
                                m_highSpeedRotationAxis[2]);
}

void
PhysicsBody::setHighSpeedRotationAxis(double p_x, double p_y, double p_z)
{
    m_highSpeedRotationAxis[0] = p_x;
    m_highSpeedRotationAxis[1] = p_y;
    m_highSpeedRotationAxis[2] = p_z;
}

void
PhysicsBody::setHighSpeedRotationAxis(VRS::Vector p_vector)
{
    setHighSpeedRotationAxis(p_vector[0], p_vector[1], p_vector[2]);
}

void
PhysicsBody::setIgnoreGravity(bool p_ignore)
{
    dBodySetGravityMode(m_bodyId, !p_ignore);
}

void
PhysicsBody::update()
{
    CollisionBody::update();
    
    if(m_highSpeedRotationMode)
        setHighSpeedRotationMode(true);
}

void
PhysicsBody::syncTransformationsToODE()
{
    dMatrix3 rot;
    VRS::Matrix matrix;
    
    if(odeShape())
        matrix = rotation() * odeShape()->offsetRotation()->matrix();
    else
        matrix = rotation();

    rot[ 0] = matrix.element(0, 0);
    rot[ 1] = matrix.element(0, 1);
    rot[ 2] = matrix.element(0, 2);
    rot[ 3] = matrix.element(3, 0);
    rot[ 4] = matrix.element(1, 0);
    rot[ 5] = matrix.element(1, 1);
    rot[ 6] = matrix.element(1, 2);
    rot[ 7] = matrix.element(3, 1);
    rot[ 8] = matrix.element(2, 0);
    rot[ 9] = matrix.element(2, 1);
    rot[10] = matrix.element(2, 2);
    rot[11] = matrix.element(3, 2);
    
    dBodySetRotation(m_bodyId, rot);
    
    VRS::Vector offset;
    if(odeShape())
        offset = odeShape()->offsetCenter();

    VRS::Vector pos = position() + offset;
    dBodySetPosition(m_bodyId, pos[0], pos[1], pos[2]);
}

void
PhysicsBody::syncTransformationsToVRS()
{
    // no sync if there is nothing to see, except for force sync
    if(!m_forceSync)
        if((!vrsShape() && !debugNode()))
            return;

    const dReal* pos = dBodyGetPosition(m_bodyId);
    translationNode()->setTranslate(VRS::Vector(pos[0], pos[1], pos[2]));

    // TODO: maybe there is a way without violating const
    const dReal* rot = dBodyGetRotation(m_bodyId);
    VRS::Matrix* matrix = const_cast<VRS::Matrix*>(&(rotation()));

    VRS::SO<VRS::Rotation> undoOffsetRotation = new VRS::Rotation();
    undoOffsetRotation->setAngle(odeShape()->offsetRotation()->getAngle() * -1);
    undoOffsetRotation->setAxis(odeShape()->offsetRotation()->getAxis());
    undoOffsetRotation->setCenter(odeShape()->offsetRotation()->getCenter());

    VRS::Matrix odeMatrix;
    odeMatrix.setElement(0, 0, rot[ 0]);
    odeMatrix.setElement(0, 1, rot[ 1]);
    odeMatrix.setElement(0, 2, rot[ 2]);
    odeMatrix.setElement(1, 0, rot[ 4]);
    odeMatrix.setElement(1, 1, rot[ 5]);
    odeMatrix.setElement(1, 2, rot[ 6]);
    odeMatrix.setElement(2, 0, rot[ 8]);
    odeMatrix.setElement(2, 1, rot[ 9]);
    odeMatrix.setElement(2, 2, rot[10]);

    VRS::Matrix vrsMatrix;
    vrsMatrix = odeMatrix * undoOffsetRotation->matrix();
    
    matrix->setElement(0, 0, vrsMatrix.element(0, 0));
    matrix->setElement(0, 1, vrsMatrix.element(0, 1));
    matrix->setElement(0, 2, vrsMatrix.element(0, 2));
    matrix->setElement(1, 0, vrsMatrix.element(1, 0));
    matrix->setElement(1, 1, vrsMatrix.element(1, 1));
    matrix->setElement(1, 2, vrsMatrix.element(1, 2));
    matrix->setElement(2, 0, vrsMatrix.element(2, 0));
    matrix->setElement(2, 1, vrsMatrix.element(2, 1));
    matrix->setElement(2, 2, vrsMatrix.element(2, 2));
}

bool
PhysicsBody::isEnabled() const
{
    return dBodyIsEnabled(m_bodyId);
}

void
PhysicsBody::setEnabled(bool p_enable)
{
    p_enable ? dBodyEnable(m_bodyId) : dBodyDisable(m_bodyId);
}

void
PhysicsBody::setAutoDisableMode(bool p_autoDisable)
{
    dBodySetAutoDisableFlag(m_bodyId, p_autoDisable);
}

void
PhysicsBody::setAutoDisableLinearThreshold(double p_threshold)
{
    dBodySetAutoDisableLinearThreshold(m_bodyId, p_threshold);
}

void
PhysicsBody::setAutoDisableAngularThreshold(double p_threshold)
{
    dBodySetAutoDisableAngularThreshold(m_bodyId, p_threshold);
}

void
PhysicsBody::setAutoDisableTime(double p_time)
{
    dBodySetAutoDisableTime(m_bodyId, p_time);
}

void
PhysicsBody::setAutoDisableSteps(unsigned int p_steps)
{
    dBodySetAutoDisableSteps(m_bodyId, p_steps);
}

void
PhysicsBody::setAngularVelocity(VRS::Vector p_velocity)
{
    dBodySetAngularVel(m_bodyId,
                       p_velocity[0], p_velocity[0], p_velocity[0]);
}

void
PhysicsBody::setLinearVelocity(VRS::Vector p_velocity)
{
    dBodySetLinearVel(m_bodyId,
                      p_velocity[0], p_velocity[0], p_velocity[0]);
}

VRS::Vector
PhysicsBody::angularVelocity()
{
    const dReal* velocity = dBodyGetAngularVel(m_bodyId);
    return VRS::Vector(velocity[0], velocity[1], velocity[2]);
}

VRS::Vector
PhysicsBody::linearVelocity()
{
    const dReal* velocity = dBodyGetLinearVel(m_bodyId);
    return VRS::Vector(velocity[0], velocity[1], velocity[2]);
}

void
PhysicsBody::addForce(VRS::Vector p_force)
{
    dBodyAddForce(m_bodyId, p_force[0], p_force[1], p_force[2]);
}

void
PhysicsBody::addTorque(VRS::Vector p_torque)
{
    dBodyAddTorque(m_bodyId, p_torque[0], p_torque[1], p_torque[2]);
}

void
PhysicsBody::addRelativeForce(VRS::Vector p_force)
{
    dBodyAddRelForce(m_bodyId, p_force[0], p_force[1], p_force[2]);
}

void
PhysicsBody::addRelativeTorque(VRS::Vector p_torque)
{
    dBodyAddRelTorque(m_bodyId, p_torque[0], p_torque[1], p_torque[2]);
}

void
PhysicsBody::addForceAtPosition(VRS::Vector p_force, VRS::Vector p_position)
{
    dBodyAddForceAtPos(m_bodyId, p_force[0], p_force[1], p_force[2],
                       p_position[0], p_position[1], p_position[2]);
}

void
PhysicsBody::addRelativeForceAtPosition(VRS::Vector p_force,
                                        VRS::Vector p_position)
{
    dBodyAddRelForceAtPos(m_bodyId, p_force[0], p_force[1], p_force[2],
                          p_position[0], p_position[1], p_position[2]);
}

void
PhysicsBody::addForceAtRelativePosition(VRS::Vector p_force,
                                        VRS::Vector p_position)
{
    dBodyAddForceAtRelPos(m_bodyId, p_force[0], p_force[1], p_force[2],
                          p_position[0], p_position[1], p_position[2]);
}

void
PhysicsBody::addRelativeForceAtRelativePosition(VRS::Vector p_force,
                                                VRS::Vector p_position)
{
    dBodyAddRelForceAtRelPos(m_bodyId, p_force[0], p_force[1], p_force[2],
                             p_position[0], p_position[1], p_position[2]);
}

}

---