source/SoPhysics.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/SoPhysics.h"
#include "../include/IPSA/SoWorldPhysics.h"
#include "../include/IPSA/SoCollisionShape.h"

#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoResetTransform.h>
#include <Inventor/nodes/SoNode.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/SbLinear.h>
#include <cstdio>


SO_KIT_SOURCE(SoPhysics);

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


SoPhysics::SoPhysics(const SbString& nodeName /*=""*/)
: transform(NULL),
CoinRefTransform(SbMatrix::identity()),
isInitialised(FALSE),
myWorld(NULL),
body(NULL),
geom(NULL)
{
                SO_KIT_CONSTRUCTOR(SoPhysics);
                isBuiltIn = TRUE;

                SO_KIT_ADD_CATALOG_ENTRY(separator,SoSeparator,FALSE,this,"",TRUE);
                SO_KIT_ADD_CATALOG_ENTRY(reset,SoResetTransform,FALSE,separator,collisionShape,TRUE);
                SO_KIT_ADD_CATALOG_ENTRY(collisionShape,SoCollisionShape,TRUE,separator,ODETransform,TRUE);
                SO_KIT_ADD_CATALOG_ENTRY(ODETransform,SoMatrixTransform,FALSE,separator,visualisationShape,TRUE);
                SO_KIT_ADD_CATALOG_ABSTRACT_ENTRY(visualisationShape,SoNode,SoSeparator,TRUE,separator,"",TRUE);

                SO_KIT_ADD_FIELD(AUTO_UPDATE_COLLISION_SHAPE, (TRUE));
                SO_KIT_ADD_FIELD(mass,(1.0f));
                SO_KIT_ADD_FIELD(linVelocity,(0.0f ,0.0f, 0.0f));
                SO_KIT_ADD_FIELD(angVelocity,(0.0f, 0.0f, 0.0f));
                SO_KIT_ADD_FIELD(jointList, (NULL));
                SO_KIT_ADD_FIELD(torqueField,(0.0f, 0.0f, 0.0f));
                SO_KIT_ADD_FIELD(forceField,(0.0f, 0.0f, 0.0f));
                SO_KIT_ADD_FIELD(forcePos,(0.0f, 0.0f, 0.0f));
                SO_KIT_ADD_FIELD(inertia,(0.0f, 0.0f, 0.0f));
                SO_KIT_ADD_FIELD(scale,(1.0f, 1.0f, 1.0f));
                SO_KIT_ADD_FIELD(measureContactForces,(FALSE));
                SO_KIT_ADD_FIELD(drawCollisionShape,(FALSE));

                SO_KIT_INIT_INSTANCE();

                setName(nodeName);

                // default mass is a sphere with m=1 and radius=1
                inertia.set1Value(0, 0.4f, 0.0f, 0.0f);
                inertia.set1Value(1, 0.0f, 0.4f, 0.0f);
                inertia.set1Value(2, 0.0f, 0.0f, 0.4f);

                updateDependentObjectsSensor.setFunction(SoPhysics::UpdateDependentObjectsCB);
                updateDependentObjectsSensor.setData(this);

                fieldUpdateSensor.setFunction(SoPhysics::FieldChangedCB);
                fieldUpdateSensor.setData(this);
                fieldUpdateTrigger.appendConnection(&mass);
    fieldUpdateTrigger.appendConnection(&linVelocity);
    fieldUpdateTrigger.appendConnection(&angVelocity);
    fieldUpdateTrigger.appendConnection(&drawCollisionShape);
    fieldUpdateTrigger.appendConnection(&inertia);
    fieldUpdateTrigger.appendConnection(&scale);

    // 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 SoPhysics::InitialiseOdeBodiesCB() when the SoPhysics
    // subclass instance gets initialised.
}


SoPhysics::~SoPhysics()
{
    /* There may be a problem that the member  body appears not to
       be initialised to zero sometimes. Then deletion leads to
       memory corruption.
    */
    if (body != NULL)
    {
                                dBodyDestroy(body);
        body = NULL;
    }
}


SoBaseKit* SoPhysics::copy(SbBool copyConnections) const
{
                // Use the standard version of the copy method to create a
                // copy of this instance, including its field data
                SoPhysics* newSoPhysics = dynamic_cast<SoPhysics*> (SoBaseKit::copy(copyConnections));

                assert(isInitialised == false && "Can not copy initialised SoPhysics!\n"); // Can not copy initialised SoPhysics, this is untested yet!

                newSoPhysics->myWorld = myWorld;
                newSoPhysics->CoinRefTransform = CoinRefTransform;
                newSoPhysics->body = body;
                newSoPhysics->geom = geom;
                newSoPhysics->transform = transform;
                newSoPhysics->isInitialised = isInitialised;

                return newSoPhysics;
}


void SoPhysics::FieldChangedCB(void* data, SoSensor*)
{
                SoPhysics* physicObj = static_cast<SoPhysics*> (data);

                if (NULL == physicObj)
                                return;

                if (!physicObj->isInitialised)
                                return;

                //mass has changed
                // TODO ( Inertia change currently not supported due to TriMesh! )
                dMass cm;
                dBodyGetMass(physicObj->body,&cm);
                cm.mass = physicObj->mass.getValue();
                dBodySetMass(physicObj->body,&cm);

                //linear velocity
                physicObj->setLinearVel(physicObj->linVelocity.getValue());

                //angVel
                SbVec3f ang = physicObj->angVelocity.getValue();
                dBodySetAngularVel(physicObj->body,ang[0],ang[1],ang[2]);

                //drawCollisionShape
                SoCollisionShape* cshape = dynamic_cast<SoCollisionShape*> (physicObj->getPart("collisionShape", FALSE));
                cshape->setVisualisationStatus(physicObj->drawCollisionShape.getValue());
}


SbBool SoPhysics::setUpConnections(SbBool onOff, SbBool doItAlways)
{
                if (!doItAlways && connectionsSetUp == onOff)
                                return onOff;
                if (onOff)
                {
                                SoBaseKit::setUpConnections(onOff, doItAlways);
                                // update before attach
                                SoPhysics::FieldChangedCB(this, NULL);
                                SoPhysics::UpdateDependentObjectsCB(this, NULL);
                                // attach fields to sensors

        if (&fieldUpdateTrigger != fieldUpdateSensor.getAttachedField())
            fieldUpdateSensor.attach(&fieldUpdateTrigger);
        if (&updateDependentObjectsTrigger != updateDependentObjectsSensor.getAttachedField())
            updateDependentObjectsSensor.attach(&updateDependentObjectsTrigger);
                }
                else
                {
        if (NULL != fieldUpdateSensor.getAttachedField())
            fieldUpdateSensor.detach();
        if (NULL != updateDependentObjectsSensor.getAttachedField())
            updateDependentObjectsSensor.detach();
                                SoBaseKit::setUpConnections(onOff, doItAlways);
                }

                connectionsSetUp = onOff;
                return !connectionsSetUp;
}


dMass SoPhysics::getInitialMass() {
                dMass mass;
                SbVec3f i0 = this->inertia[0];
                SbVec3f i1 = this->inertia[1];
                SbVec3f i2 = this->inertia[2];

                float m = this->mass.getValue();
                dMassSetParameters(&mass,m,0,0,0,i0[0],i1[1],i2[2],i0[1],i0[2],i1[2]);

                return mass;
}


void SoPhysics::setWorld(dWorldID w)
{
                myWorld = w;
}


dWorldID SoPhysics::getWorld()
{
                return myWorld;
}


void SoPhysics::createOdeBody(SoCallbackAction *action)
{
                if (isInitialised)
                                return;

                //check if WorldID was initialised
                assert(myWorld != NULL);

                printf("creating ODE-Body <%s> (%s)\n", getTypeId().getName().getString(), getName().getString());

                //create body
                body = dBodyCreate(myWorld);
                dBodySetData(body, this);

                // connect body and geom
                SoCollisionShape *cshape = dynamic_cast<SoCollisionShape *> (getPart("collisionShape", TRUE));
    geom = cshape->getGeom();   // Geoms have been created before the SoPhysics-Nodes, see SoWorldPhysics!
    assert(geom != NULL);
    cshape->setVisualisationStatus(false); // hide collision shape
                dGeomSetBody (geom, body);   // attach Geom to body

                SbVec3f lCOG;
                cshape->getCOG(lCOG);

                SbVec3f ct,cs;
                SbRotation cr,cso;
                CoinRefTransform.getTransform(ct, cr, cs, cso);
                CoinRefTransform.setTransform(-lCOG, cr, cs); // transform visualisation, keep rotation and scaling

                init_linVelocity = this->linVelocity.getValue();
                init_angVelocity = this->angVelocity.getValue();

                // transform is initially set to the offset the CenterOfGravity has
                transform = dynamic_cast<SoMatrixTransform*> (getPart("ODETransform", FALSE));

                updateOdeBodyParameters(action->getModelMatrix());

                //set mass after all the parameters have been updated
                dMass bmass = getInitialMass();
                dBodySetMass(body, &bmass);

                isInitialised = TRUE;
}


void SoPhysics::updateOIPosition()
{
    // use the position the body currently has in the ODE engine
    SbMatrix odet = this->getMatrixTransform();
    // and multiply it with the COG offset matrix
    SbMatrix ftrans = CoinRefTransform; // use base reference transformation
    ftrans.multRight(odet);
//    printf("physics: oi/ode at: [%f %f %f]\n",ftrans[0],ftrans[1],ftrans[2]);

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


void SoPhysics::updateOdeBody(SoCallbackAction *action){
                if (!isInitialised)
                                return;

                //force
                int numValues = forceField.getNum();
                for (int i = 0; i < numValues; i++)
                {
                                this->setForceAtPosition(forceField[i], forcePos[i]);
                }
                //torque
                numValues = torqueField.getNum();
                for (int j = 0; j < numValues; j++)
                {
                                this->setTorque(torqueField[j]);
                }

                // inventor position
                updateOIPosition();
}

void SoPhysics::resetOdeBody(SoCallbackAction* action)
{
    if (!isInitialised)
                                return;

    this->linVelocity.setValue(init_linVelocity);
    this->angVelocity.setValue(init_angVelocity);

    updateOdeBodyParameters(action->getModelMatrix());
}


void SoPhysics::updateOdeBodyParameters(const SbMatrix& actionModelMatrix)
{
    // consider alignment of collisionshape and
    // get initial position and rotation from coin, correct for  COG-Trafo
    SoCollisionShape *cshape = dynamic_cast<SoCollisionShape *> (getPart("collisionShape", FALSE));
    int alignment = cshape->alignment.getValue();
    SbMatrix mm;
    if (alignment == SoCollisionShape::STANDARD)
    {
        SbVec3f lCOG;
        cshape->getCOG(lCOG);
        mm.setTranslate(lCOG);
    }
    else if (alignment == SoCollisionShape::CENTERED)
        mm.setTranslate(SbVec3f(0.0, 0.0, 0.0));

    mm = mm.multRight(actionModelMatrix);
    SbVec3f translation, scaling;
    SbRotation rotation, sr;
    mm.getTransform(translation, rotation, scaling, sr);

    //set Scale
    scale.setValue(scaling);

    // 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 the body
    dBodySetRotation(body, rot);

    dBodySetPosition(body, translation[0], translation[1], translation[2]);

    // reset velocity
    this->setLinearVel(init_linVelocity);
    dBodySetAngularVel(body, init_angVelocity[0], init_angVelocity[1], init_angVelocity[2]);

    // update inventor position
    updateOIPosition();
}


dBodyID SoPhysics::getBody()
{
                return this->body;
}


SoSFVec3f SoPhysics::getCurrentForce()
{
                SoSFVec3f ret;
                const dReal* FORCE = dBodyGetForce(this->body);
                ret.setValue(FORCE[0],FORCE[1],FORCE[2]);
                return ret;
}


SoSFVec3f SoPhysics::getCurrentTorque()
{
                SoSFVec3f ret;
                const dReal* TORQUE;
                TORQUE = dBodyGetTorque(this->body);
                ret.setValue(TORQUE[0],TORQUE[1],TORQUE[2]);
                return ret;
}


SoSFVec3f SoPhysics::getCurrentLinearVel()
{
                SoSFVec3f ret;
                const dReal* VELOCITY = dBodyGetLinearVel(this->body);
                ret.setValue(VELOCITY[0], VELOCITY[1], VELOCITY[2]);
                return ret;
}


SoSFVec3f SoPhysics::getCurrentPosition()
{
                SoSFVec3f ret;
                dVector3 cppos;
                dBodyGetRelPointPos(body,0,0,0,cppos);

                ret.setValue(cppos[0], cppos[1], cppos[2]);
                return ret;
}


SbMatrix SoPhysics::getMatrixTransform()
{
                const dReal* TRANSLATION = dBodyGetPosition (body);
                const dReal* ROTATION = dBodyGetQuaternion (body);
                SbMatrix ret = SbMatrix::identity();
                ret.setTransform(SbVec3f(TRANSLATION[0],TRANSLATION[1],TRANSLATION[2]),
                                                 SbRotation(ROTATION[1],ROTATION[2],ROTATION[3],ROTATION[0]),
                                                 SbVec3f(1,1,1));
                return ret;
}


SbMatrix SoPhysics::getOIMatrixTransform()
{
    return transform->matrix.getValue();
}


void SoPhysics::setForce(const SbVec3f& force)
{
                dBodyAddForce(this->body, force[0], force[1], force[2]);
}


void SoPhysics::setForceAtPosition(const SbVec3f& force, const SbVec3f& position)
{
                dBodyAddForceAtPos(this->body,
                                                   force[0], force[1], force[2],
                                                   position[0], position[1], position[2]);
}


void SoPhysics::setRelForceAtPosition(const SbVec3f& force, const SbVec3f& position)
{
                dBodyAddRelForceAtRelPos(this->body,
                                                   force[0], force[1], force[2],
                                                   position[0], position[1], position[2]);
}


void SoPhysics::setRelForce(const SbVec3f& relForce)
{
                dBodyAddRelForce(this->body, relForce[0], relForce[1], relForce[2]);
}


void SoPhysics::setLinearVel(const SbVec3f& vel)
{
                dBodySetLinearVel(this->body, vel[0], vel[1], vel[2]);
}

void SoPhysics::setTorque(const SbVec3f& torque)
{
                dBodyAddTorque(this->body, torque[0], torque[1], torque[2]);
}


void SoPhysics::setCollisionShapeVisibility(SbBool visible)
{
                this->drawCollisionShape.setValue(visible);
}


void SoPhysics::UpdateDependentObjectsCB(void* data, SoSensor* sensor)
{
                SoPhysics* physicsBody = static_cast<SoPhysics*> (data);
                if (NULL == physicsBody)
                                return;

                physicsBody->updateDependentObjects();
}


SoCallbackAction::Response SoPhysics::InitialiseOdeBodiesCB(void* userdata, SoCallbackAction* action, const SoNode* node)
{
    SoWorldPhysics* world = static_cast<SoWorldPhysics*> (userdata);
    SoPhysics* physicsBody = dynamic_cast<SoPhysics*> (const_cast<SoNode*> (node));
    if (NULL != world && NULL != physicsBody)
    {
        physicsBody->setUpConnections(TRUE, TRUE);
        physicsBody->setWorld(world->ownworld);
        physicsBody->createOdeBody(action);
    }
    return SoCallbackAction::CONTINUE;
}


SoCallbackAction::Response SoPhysics::UpdateOdeBodiesCB(void* userdata, SoCallbackAction* action, const SoNode* node)
{
    //printf("Updating bodies\n");
    SoPhysics* physicsBody = dynamic_cast<SoPhysics*> (const_cast<SoNode*> (node));
    if (NULL != physicsBody)
        physicsBody->updateOdeBody(action);
    return SoCallbackAction::CONTINUE;
}


SoCallbackAction::Response SoPhysics::ResetOdeBodiesCB(void *userdata, SoCallbackAction* action, const SoNode* node)
{
    SoPhysics* physicsBody = dynamic_cast<SoPhysics*> (const_cast<SoNode*> (node));
    if (NULL != physicsBody)
        physicsBody->resetOdeBody(action);
    return SoCallbackAction::CONTINUE;
}

---