source/SoCollisionShape.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2004, University of Braunschweig (for members see AUTHORS file)
 * Copyright (c) 2007, 2008, 2009, University of Karlsruhe (for members see AUTHORS file)
 *
 * ISC License
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include "../include/IPSA/SoCollisionShape.h"
#include "../include/IPSA/SoWorldPhysics.h"
#include <Inventor/SbBox.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoDrawStyle.h>

SO_KIT_SOURCE(SoCollisionShape);


void SoCollisionShape::initClass()
{
                SO_KIT_INIT_CLASS(SoCollisionShape, SoBaseKit, "BaseKit");
}


SoCallbackAction::Response SoCollisionShape::InitCollisionShapesCB(void* userdata, SoCallbackAction* action, const SoNode* node)
{
    SoWorldPhysics* world = static_cast<SoWorldPhysics*> (userdata);
    SoCollisionShape* collisionShape = dynamic_cast<SoCollisionShape*> (const_cast<SoNode*> (node));
    //printf("InitCollisionShape Callback (%s)\n\r",collisionShape->getName().getString());
    if (NULL != world && NULL != collisionShape)
    {
        collisionShape->setUpConnections(TRUE, TRUE);
        collisionShape->setSpace(world->ownspace);
        collisionShape->createGeom(action->getModelMatrix());
        collisionShape->updateGeomPosition(action->getModelMatrix());
        if (collisionShape->autoinitialise.getValue())
            collisionShape->updateVisualisationAndOdeProperties();
    }
    return SoCallbackAction::CONTINUE;
}


SoCollisionShape::SoCollisionShape(const SbString& nodeName /*= ""*/)
: CoinRefTransform(SbMatrix::identity()),
space(NULL),
geometry(NULL),
COG(SbVec3f(0.0f, 0.0f, 0.0f))
{
                SO_KIT_CONSTRUCTOR(SoCollisionShape);

                SO_KIT_ADD_CATALOG_ENTRY(separator,SoSeparator,FALSE,this,"",TRUE);
                SO_KIT_ADD_CATALOG_ENTRY(visDrawStyle,SoDrawStyle,TRUE,separator,ODETransform,TRUE);
                SO_KIT_ADD_CATALOG_ENTRY(ODETransform,SoMatrixTransform,FALSE,separator,visualisation,TRUE);
                SO_KIT_ADD_CATALOG_ABSTRACT_ENTRY(visualisation,SoNode,SoCube,TRUE,separator,"",TRUE);

                SO_KIT_ADD_FIELD(alignment,(SoCollisionShape::STANDARD));  // centered alignment
                SO_KIT_DEFINE_ENUM_VALUE(eAlignment, STANDARD);
                SO_KIT_DEFINE_ENUM_VALUE(eAlignment, CENTERED);
                SO_KIT_SET_SF_ENUM_TYPE(alignment, eAlignment);

                SO_KIT_ADD_FIELD(surfaceMaterial, (SoCollisionShape::GREASY));
                SO_KIT_DEFINE_ENUM_VALUE(eSurfaceMaterial, CLEAN);
                SO_KIT_DEFINE_ENUM_VALUE(eSurfaceMaterial, GREASY);
                SO_KIT_DEFINE_ENUM_VALUE(eSurfaceMaterial, WOOD);
                SO_KIT_DEFINE_ENUM_VALUE(eSurfaceMaterial, GLASS);
                SO_KIT_DEFINE_ENUM_VALUE(eSurfaceMaterial, STEEL);
                SO_KIT_DEFINE_ENUM_VALUE(eSurfaceMaterial, RUBBER);
                SO_KIT_SET_SF_ENUM_TYPE(surfaceMaterial, eSurfaceMaterial);

    SO_KIT_ADD_FIELD(category,(1));
    SO_KIT_ADD_FIELD(collisionmask,(1));
    SO_KIT_ADD_FIELD(autoinitialise,(TRUE));

    SO_KIT_INIT_INSTANCE();

                // change drawstyle as the default is set to visible
                SoDrawStyle* dstyle = dynamic_cast<SoDrawStyle*> (getPart("visDrawStyle", TRUE));
                dstyle->style = SoDrawStyle::INVISIBLE;

                // assign transform
                transform = dynamic_cast<SoMatrixTransform*> (getPart("ODETransform", FALSE));

                this->setName(nodeName);

                updateOdePropertiesSensor.setFunction(SoCollisionShape::UpdateVisualisationAndOdePropertiesCB);
                updateOdePropertiesSensor.setData(this);

    // Do NOT call setUpConnections() here as it is a virtual method subclasses override.
    // It is not defined what happens when a virtual method is called inside a
    // constructor.
    // Therefore it is called in SoCollisionShape::InitCollisionShapesCB() when the SoCollisionShape
    // subclass instance gets initialised.
}


SoBaseKit* SoCollisionShape::copy(SbBool copyConnections) const
{
                // Use the standard version of the copy method to create a
                // copy of this instance, including its field data
                SoCollisionShape* newSoCollisionShape = dynamic_cast<SoCollisionShape*> (SoBaseKit::copy(copyConnections));
                assert(geometry == NULL && "Can not copy initialised SoCollisionshapes!\n"); // Can not copy initialised SoCollisionshapes, this is untested yet!

                newSoCollisionShape->space = space;
                newSoCollisionShape->geometry = geometry;
                newSoCollisionShape->COG = COG;
                newSoCollisionShape->transform = transform;
                newSoCollisionShape->CoinRefTransform = CoinRefTransform;
                newSoCollisionShape->setUpConnections(TRUE, TRUE);

                return newSoCollisionShape;
}


SbBool SoCollisionShape::setUpConnections(SbBool onOff, SbBool doItAlways /*=FALSE*/)
{
    if (!doItAlways && onOff == connectionsSetUp)
        return onOff;
    if (onOff)
    {
        SoBaseKit::setUpConnections(onOff, doItAlways);
        // update before attach
        SoCollisionShape::UpdateVisualisationAndOdePropertiesCB(this, NULL);
        // attach field to sensor
        if (&updateOdePropertiesTrigger != updateOdePropertiesSensor.getAttachedField())
            updateOdePropertiesSensor.attach(&updateOdePropertiesTrigger);
    }
    else
    {
        if (NULL != updateOdePropertiesSensor.getAttachedField())
            updateOdePropertiesSensor.detach();
        SoBaseKit::setUpConnections(onOff, doItAlways);
    }

    connectionsSetUp = onOff;
    return !connectionsSetUp;
}


SoCollisionShape::~SoCollisionShape()
{
    if (geometry != NULL)
    {
        dGeomDestroy(geometry);
        geometry = NULL;
    }
}


void SoCollisionShape::updateVisualisationAndOdeProperties()
{
                SoCube* vis = dynamic_cast<SoCube*> (getPart("visualisation", TRUE));
                if(NULL != vis)
                {
                                vis->width.setValue(1);
                                vis->height.setValue(1);
                                vis->depth.setValue(1);
                }
}


dGeomID SoCollisionShape::getGeom()
{
                return geometry;
}


dGeomID SoCollisionShape::createGeom(const SbMatrix& mm)
{
    if(geometry == NULL)
    {
        geometry = this->cGeom(mm);
        dGeomSetBody(geometry, NULL); // make geom placeable
        dGeomSetData(geometry, this); // Set Geom Userdata-Pointer to the current SoCollisionshape instance
    }

    updateVisualisationAndOdeProperties();

    // refresh category and collisionmask
    dGeomSetCategoryBits(geometry, category.getValue());
    dGeomSetCollideBits(geometry, collisionmask.getValue());
    return geometry;
}


void SoCollisionShape::setSpace(dSpaceID spaceID)
{
                if (NULL == space)
                                space = spaceID;
                else if (spaceID == space)
                {
                                //printf("SoCollisionShape::setSpace() -> the spaceID has already been set to the same value!");
                }
                else
                {
                                printf("SoCollisionShape::setSpace() -> the space has already been set!");
                                printf(" Changing the collision space is not allowed/supported currently.\n");
                }
}


dGeomID SoCollisionShape::cGeom(const SbMatrix& mm)
{
                assert(space != NULL);

                return dCreateBox(space, 1, 1, 1);
}


SbBool SoCollisionShape::getVisualisationStatus()
{
                SoDrawStyle* dstyle = dynamic_cast<SoDrawStyle*> (getPart("visDrawStyle", FALSE));
                if(dstyle->style.getValue() == SoDrawStyle::INVISIBLE)
                                return FALSE;
                else
                                return TRUE;
}


void SoCollisionShape::setVisualisationStatus(SbBool onOff)
{
                SoDrawStyle* dstyle = dynamic_cast<SoDrawStyle*> (getPart("visDrawStyle", FALSE));
                if(onOff)
                                dstyle->style = SoDrawStyle::FILLED;
                else
                                dstyle->style = SoDrawStyle::INVISIBLE;
}

void SoCollisionShape::getCOG(SbVec3f& COM)
{
                COM = COG;
}

void SoCollisionShape::UpdateVisualisationAndOdePropertiesCB(void* data, SoSensor* sensor)
{
                SoCollisionShape* collisionShape = static_cast<SoCollisionShape*> (data);
                if (NULL == collisionShape)
                                return;

                collisionShape->updateVisualisationAndOdeProperties();
}

void SoCollisionShape::updateGeomPosition(const SbMatrix& mm)
{
                SbVec3f translation;
                SbRotation rotation,so;
                SbVec3f scaling;
                CoinRefTransform.getTransform(translation, rotation, scaling, so);
                CoinRefTransform.setTransform(-COG, rotation, scaling); // transform visualisation with COG, keep rotation and scaling

                SbMatrix modelMatrix;
                if (alignment.getValue() == SoCollisionShape::STANDARD)
                                modelMatrix.setTranslate(COG);
                else if (alignment.getValue() == SoCollisionShape::CENTERED)
                                modelMatrix.setTranslate(SbVec3f(0.0,0.0,0.0));

                modelMatrix = modelMatrix.multRight(mm);
                modelMatrix.getTransform(translation, rotation, scaling, so);

                // extract rotation
                SbVec3f axis;
                float radians;
                rotation.getValue(axis, radians);
                const float* AXIS_VALUES = axis.getValue();
                dMatrix3 rot;

                dRFromAxisAndAngle(rot, AXIS_VALUES[0], AXIS_VALUES[1], AXIS_VALUES[2], radians);

                //set position and rotation of geom
                dGeomSetPosition(geometry, translation[0], translation[1], translation[2]);
                dGeomSetRotation(geometry, rot);
                //printf("cshape at:\n"); translation.print(stdout);
                //updateOIPosition();
}

void SoCollisionShape::updateOIPosition()
{
                const dReal* TRANSLATION = dGeomGetPosition (geometry);
                dQuaternion rot;
                dGeomGetQuaternion (geometry, rot);

                SbMatrix ftrans = CoinRefTransform; // use base reference transformation
                SbMatrix odet = SbMatrix::identity();
                odet.setTransform(SbVec3f(TRANSLATION[0],TRANSLATION[1],TRANSLATION[2]),
                                                  SbRotation(rot[1],rot[2],rot[3],rot[0]),
                                                  SbVec3f(1,1,1));
                ftrans.multRight(odet);
                printf("cshape: oi/ode at: [%f %f %f]\n", TRANSLATION[0], TRANSLATION[1], TRANSLATION[2]);

                assert(transform != NULL);
                transform->matrix.setValue(ftrans);
}

---