vrsode/src/CollisionBody.cpp

Go to the documentation of this file.

#include "CollisionBody.h"
#include "PhysicsManager.h"
#include "BoxShape.h"
#include "CylinderShape.h"
#include "SphereShape.h"
#include "NoShape.h"
#include "PlaneShape.h"

#include "random_utils/LogManager.h"
// #include "random_utils/AutoLocker.h"

#include <ode/ode.h>

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

#include <vrs/box.h>
#include <vrs/colorattribute.h>
#include <vrs/constantdetail.h>
#include <vrs/facestyle.h>
#include <vrs/plane.h>
#include <vrs/rotation.h>
#include <vrs/shape.h>
#include <vrs/sphere.h>
#include <vrs/translation.h>

namespace vrsode
{

CollisionBody::CollisionBody(VRS::SO<VRS::SceneThing> p_sceneThing,
                             CollisionShape p_collisionShape, bool p_register)
{
    m_cleanedUp         = false;
    m_reportCollision   = false;
    m_wantToDestroy     = false;
    m_sceneThing        = p_sceneThing;
    m_collisionShape    = p_collisionShape;

    setupTransformationNodes();

    VRS::Bounds boundingBox = m_sceneThing->boundingBox(0);

    switch(m_collisionShape)
    {
        case CS_Sphere:
            m_odeShape = new SphereShape(boundingBox);
            m_geomId = m_odeShape->geomId();
        break;
        case CS_Box:
            m_odeShape = new BoxShape(boundingBox);
            m_geomId = m_odeShape->geomId();
        break;
        case CS_None:
            m_odeShape = new NoShape();
            m_geomId = m_odeShape->geomId();
        break;
        default:
            rErr("NOT IMPLEMENTED: Provided CollisionShape cannot be "
                 "generated, please provide a pre created shape.");
            m_odeShape = 0;
            m_geomId = 0;
        break;
    }

    if(m_geomId)
    {
        dSpaceAdd(PhysicsManager::get()->spaceId(), m_geomId);
        dGeomSetData(m_geomId, this);
        syncTransformationsToODE();
        setSurface(new Surface());
        if(p_register)
            PhysicsManager::get()->registerCollisionBody(this);
    }
}

CollisionBody::CollisionBody(VRS::SO<VRS::SceneThing> p_sceneThing,
              VRS::SO<Shape> p_shape, bool p_register)
{
    m_cleanedUp         = false;
    m_reportCollision   = false;
    m_wantToDestroy     = false;
    m_sceneThing        = p_sceneThing;
    m_collisionShape    = p_shape->collisionShape();
    m_sceneNodeLock     = new SceneNodeLock;

    setupTransformationNodes();

    m_odeShape          = p_shape;
    m_geomId            = p_shape->geomId();

    if(m_geomId)
    {
        dSpaceAdd(PhysicsManager::get()->spaceId(), m_geomId);
        dGeomSetData(m_geomId, this);
        syncTransformationsToODE();
        setSurface(new Surface());
        if(p_register)
            PhysicsManager::get()->registerCollisionBody(this);
    }
}

CollisionBody::~CollisionBody()
{
    // if(!m_cleanedUp)
    //     destroy();
}

void
CollisionBody::destroy()
{
    PhysicsManager::get()->unregisterCollisionBody(this);
    dGeomDestroy(geomId());
    m_cleanedUp = true;
}

bool
CollisionBody::cleanedUp()
{
    return m_cleanedUp;
}

void
CollisionBody::setCleanedUp(bool p_cleanedUp)
{
    m_cleanedUp = p_cleanedUp;
}

VRS::SO<VRS::SceneThing>
CollisionBody::sceneThing() const
{
    return m_sceneThing;
}

const dGeomID
CollisionBody::geomId() const
{
    return m_geomId;
}

const CollisionShape
CollisionBody::collisionShape() const
{
    return m_collisionShape;
}

void
CollisionBody::setupTransformationNodes()
{
    if(!m_sceneThing)
    {
        rErr("Tried to setup a PhysicsBody which does not have a SceneThing "
             "associated. Nothing done!");
        return;
    }

    // find the shape object
    m_vrsShape = 0;
    for(int i = 0; i < m_sceneThing->objects(); i++)
        if(m_vrsShape = dynamic_cast<VRS::Shape*>(m_sceneThing->object(i)))
           break;

    // check for rotation object
    VRS::Iterator<VRS::SO<VRS::SharedObj> >* rotationItr =
            m_sceneThing->find(VRS::Rotation::ClassNameVRS());

    // if no rotation node is present, add one.
    if(rotationItr->size() == 0)
    {
        m_rotation = new VRS::Rotation();
        m_sceneThing->prepend(m_rotation);
    }
    else
    {
        // TODO: handle already existing rotations.
        rErr("Handling already existing rotations is not yet implemented!");
        return;
    }

    // check for translation object
    VRS::Iterator<VRS::SO<VRS::SharedObj> >* translationItr =
            m_sceneThing->find(VRS::Translation::ClassNameVRS());

    // if no translation node was found, add one.
    if(translationItr->size() == 0)
    {
        m_translation = new VRS::Translation(0, 0, 0);
        m_sceneThing->prepend(m_translation);
    }
    else
    {
        // TODO: handle already existing translations.
        rErr("Handling already existing translations is not yet implemented!");
        return;
    }

    // locker object not needed anymore since we have "ThreadSafeCanvas"
    // insertLocker();
}

void
CollisionBody::insertLocker()
{
    while(m_sceneThing->objects())
    {
        m_sceneNodeLock->append(m_sceneThing->object(0));
        m_sceneThing->remove(0);
    }

    m_sceneThing->append(m_sceneNodeLock);
}

const VRS::Vector
CollisionBody::position() const
{
    return m_translation->getTranslate();
}

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

void
CollisionBody::setPosition(VRS::Vector p_position)
{
    // RU_SYNCHRONIZE(PhysicsManager::get());
    m_translation->setTranslate(p_position);

    syncTransformationsToODE();
}

const VRS::Matrix&
CollisionBody::rotation() const
{
    return m_rotation->matrix();
}


void
CollisionBody::setRotation(double p_axisX, double p_axisY, double p_axisZ,
                           double p_angle)
{
    setRotation(VRS::Vector(p_axisX, p_axisY, p_axisZ), p_angle);
}

void
CollisionBody::setRotation(VRS::Vector p_axis, double p_angle)
{
    // RU_SYNCHRONIZE(PhysicsManager::get());

    VRS::Vector offset;
    if(m_odeShape)
        offset = m_odeShape->offsetCenter();

    m_rotation->setAxis(p_axis);
    m_rotation->setCenter(offset);
    m_rotation->setAngle(p_angle);

    syncTransformationsToODE();
}

void
CollisionBody::syncTransformationsToODE()
{
    if(dGeomGetClass(m_geomId) == dPlaneClass)
        return;

    dMatrix3 rot;
    VRS::Matrix matrix;
    
    if(m_odeShape)
        matrix = rotation() * m_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);

    dGeomSetRotation(m_geomId, rot);

    VRS::Vector offset;
    if(m_odeShape)
        offset = m_odeShape->offsetCenter();

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

void
CollisionBody::setDebugNode(bool p_debugNode)
{
    if(!m_odeShape)
        return;

    if(m_odeShape->debugNodeActive() == p_debugNode)
        return;
        
    m_odeShape->activateDebugNode(p_debugNode);
    
    if(p_debugNode)
        m_sceneThing->append(m_odeShape->debugNode());
    else
        m_sceneThing->remove(m_odeShape->debugNode());
}

VRS::SO<VRS::Translation>
CollisionBody::translationNode()
{
    return m_translation;
}

VRS::SO<VRS::Rotation>
CollisionBody::rotationNode()
{
    return m_rotation;
}

VRS::SO<VRS::Shape>
CollisionBody::vrsShape()
{
    return m_vrsShape;
}

void
CollisionBody::handleCollision(VRS::SO<CollisionBody> p_partner)
{
    if(m_reportCollision)
        std::cout << this << " collided with " << p_partner << std::endl;
}

void
CollisionBody::setSurface(VRS::SO<Surface> p_surface)
{
    m_surface = p_surface;
}

VRS::SO<Surface>
CollisionBody::surface()
{
    return m_surface;
}

VRS::SO<Shape>
CollisionBody::odeShape()
{
    return m_odeShape;
}

bool
CollisionBody::debugNode()
{
    if(m_odeShape)
        return m_odeShape->debugNodeActive();
    return false;
}

void
CollisionBody::setCollisionCategory(unsigned int p_value)
{
//     // RU_SYNCHRONIZE(PhysicsManager::get());
    if(m_geomId)
        dGeomSetCategoryBits(m_geomId, p_value);
}

unsigned int
CollisionBody::collisionCategory()
{
    if(!m_geomId)
        return 0;

//     // RU_SYNCHRONIZE(PhysicsManager::get());
    return dGeomGetCategoryBits(m_geomId);
}

void
CollisionBody::setCollisionPartnerCategories(unsigned int p_value)
{
//     // RU_SYNCHRONIZE(PhysicsManager::get());
    if(m_geomId)
        dGeomSetCollideBits(m_geomId, p_value);
}

unsigned int
CollisionBody::collisionPartnerCategories()
{
    if(!m_geomId)
        return 0;

//     // RU_SYNCHRONIZE(PhysicsManager::get());
    return dGeomGetCollideBits(m_geomId);
}

VRS::SO<SceneNodeLock>
CollisionBody::sceneNodeLock()
{
    return m_sceneNodeLock;
}

void
CollisionBody::setReportCollision(bool p_report)
{
    m_reportCollision = p_report;
}

void
CollisionBody::wantToDestroy()
{
    m_wantToDestroy = true;
}

void
CollisionBody::update()
{
    if(m_wantToDestroy)
        destroy();
}

}

---