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

#include <ode/ode.h>

#include <cassert>

SO_NODE_SOURCE(SoWorldPhysics);

bool SoWorldPhysics::bOdeColliderInitialised = false;
bool SoWorldPhysics::bClassesInitialised = false;


void SoWorldPhysics::initClass()
{
                // Initialise type id variables
                SO_NODE_INIT_CLASS(SoWorldPhysics, SoSeparator, "Separator");
                SoWorldPhysics::Init();
}


void SoWorldPhysics::Init()
{
    if (!bOdeColliderInitialised)
    {
        dInitODE(); // initialise ODE collider
        bOdeColliderInitialised = true;
    }
}


void SoWorldPhysics::DeInit()
{
                if (bOdeColliderInitialised)
                {
                                dCloseODE();    // clean up ODE collider stuff
                                bOdeColliderInitialised = false;
                }
}


SoWorldPhysics::SoWorldPhysics(const SbString& nodeName /*= ""*/, int numChildren /*= 5*/)
: SoSeparator(numChildren),
MAX_BODY_CONTACTS(64),
DEFAULT_ODE_STEPSIZE(0.004f),
connectionsSetUp(false)
{
                SO_NODE_CONSTRUCTOR(SoWorldPhysics);
                isBuiltIn = TRUE;

                SO_NODE_ADD_FIELD(gravity,(0.0f, -9.81f, 0.0f));
                SO_NODE_ADD_FIELD(ERP,(0.2f));
                SO_NODE_ADD_FIELD(CFM,(1e-8f));
                SO_NODE_ADD_FIELD(SoftCFM,(0.001f));
                SO_NODE_ADD_FIELD(SoftERP,(-1.0f));
                SO_NODE_ADD_FIELD(stepsize,(DEFAULT_ODE_STEPSIZE));

                SO_NODE_ADD_FIELD(ContactApprox,(SoWorldPhysics::BOXFRICTION));  // global Frictionmodel at Contactpoints, def. Boxfriction
                SO_NODE_DEFINE_ENUM_VALUE(eContactApprox, BOXFRICTION);
                SO_NODE_DEFINE_ENUM_VALUE(eContactApprox, PYRAMIDFRICTION);
                SO_NODE_SET_SF_ENUM_TYPE(ContactApprox, eContactApprox);

                SO_NODE_ADD_FIELD(stepping, (SoWorldPhysics::QUICKSTEP));
                SO_NODE_DEFINE_ENUM_VALUE(eStepping, WORLDSTEP);
                SO_NODE_DEFINE_ENUM_VALUE(eStepping, QUICKSTEP);
                SO_NODE_DEFINE_ENUM_VALUE(eStepping, STEPFAST1);
                SO_NODE_SET_SF_ENUM_TYPE(stepping, eStepping);
                SO_NODE_ADD_FIELD(sf1maxiterations, (5));

                ownworld = dWorldCreate();
                ownspace = dHashSpaceCreate (0);
                iContactFeedback.reserve(1000);  // reserve for 1000 contact feedback structures

                printf("World (%p) created\n\r", ownworld);

                contactgroup= dJointGroupCreate (0);
                dWorldSetERP(ownworld, ERP.getValue());
                dWorldSetCFM(ownworld, CFM.getValue());

                updateOdeParamtersSensor.setFunction(updateOdeParamtersCB);
                updateOdeParamtersSensor.setData(this);

    updateOdeParamtersTrigger.appendConnection(&gravity);
    updateOdeParamtersTrigger.appendConnection(&ERP);
    updateOdeParamtersTrigger.appendConnection(&CFM);
    updateOdeParamtersTrigger.appendConnection(&stepsize);

    setUpConnections(TRUE, TRUE);

    sens.setFunction(SoWorldPhysics::StepOde);
    sens.setData(this);

                this->setName(nodeName);
}


SoWorldPhysics::~SoWorldPhysics()
{
//              dJointGroupEmpty(this->contactgroup);
//              dWorldDestroy(this->ownworld);
//              dSpaceDestroy(this->ownspace);
                printf("World (%p) deleted\n\r", ownworld);
}


void SoWorldPhysics::NearCallback (void* data, dGeomID o1, dGeomID o2)
{
                // exit without doing anything if the two bodies are connected by a joint
                dBodyID body1 = dGeomGetBody(o1);
                dBodyID body2 = dGeomGetBody(o2);
                if (body1 && body2 && dAreConnectedExcluding (body1,body2,dJointTypeContact))
                                return;
                //
                // *** Determine friction coefficient between geoms
                //

                // fetch SoCollisionShapes from geom user data
                SoCollisionShape* colShape1 = static_cast<SoCollisionShape*>(dGeomGetData(o1));
                SoCollisionShape* colShape2 = static_cast<SoCollisionShape*>(dGeomGetData(o2));

                int material1 = colShape1->surfaceMaterial.getValue();
                int material2 = colShape2->surfaceMaterial.getValue();

                assert(SoCollisionShape::NO_OF_SURFACEMATERIALS > material1 && "SoWorldPhysics::NearCallback() invalid surfacematerial>max for geom1.");
                assert(0 <= material1 && "SoWorldPhysics::NearCallback() invalid surfacematerial<0 for geom1.");
                assert(SoCollisionShape::NO_OF_SURFACEMATERIALS > material2 && "SoWorldPhysics::NearCallback() invalid surfacematerial for geom2. ");
                assert(0 <= material2 && "SoWorldPhysics::NearCallback() invalid surfacematerial<0 for geom2.");

                double mu = frictioncoeff[material1][material2];
                dReal ode_mu = dInfinity;
                if (mu < 1.0)
                                ode_mu = static_cast<dReal>(mu/(1.0-mu)); // map real world mu from 0 to inf

                // !!! Here we perform the actual collision check:
                SoWorldPhysics* world = static_cast<SoWorldPhysics*> (data);

                dContact* contact = new dContact[world->MAX_BODY_CONTACTS];   // up to MAX_BODY_CONTACTS contacts per box-box
                int no_of_contacts = dCollide (o1, o2, world->MAX_BODY_CONTACTS, &contact[0].geom, sizeof(dContact));
                if (0 == no_of_contacts)
                                return;

                bool useFB = false;
                if (NULL != body1) // if physics body assigned to geom
                {
                                SoPhysics* pb1 = static_cast<SoPhysics*>(dBodyGetData(body1));
                                useFB |= pb1->measureContactForces.getValue() == TRUE;
                }
                if (NULL != body2) // if physics body assigned to geom
                {
                                SoPhysics* pb2 = static_cast<SoPhysics*>(dBodyGetData(body2));
                                useFB |= pb2->measureContactForces.getValue() == TRUE;
                }

                for (int i = 0; i < no_of_contacts; i++)
                {
                                // create contact object
                                contact[i].surface.mode = dContactBounce | dContactSoftCFM | world->ContactApprox.getValue();
                                if (world->SoftERP.getValue() >= 0.0)
                                                contact[i].surface.mode |= dContactSoftERP;

                                contact[i].surface.mu = ode_mu;
                                contact[i].surface.mu2 = 0;
                                contact[i].surface.bounce = 0.2f;
                                contact[i].surface.bounce_vel = 0.1f;
                                contact[i].surface.soft_cfm = world->SoftCFM.getValue();
                                contact[i].surface.soft_erp = world->SoftERP.getValue();

                                dJointID c = dJointCreateContact (world->ownworld, world->contactgroup, contact + i);
                                dJointAttach (c, body1, body2); // contact joints get deleted in UpdateSingleWorldCB()

                                // The Contact Joint Forces become recorded in iContactFeedback,
                                // thus AFTER a call of a world step (in UpdateSingleWorldCB)
                                // the feedback structures are filled with the contact forces.
                                if (useFB)
                                {
                                                dJointFeedback dummy;
                                                world->iContactFeedback.push_back(dummy);
                                                dJointSetFeedback(c, &(world->iContactFeedback.back()) );

                                                // TODO: Move section for creation of friction cones
                                                // (if useFB) to a code section AFTER the world step.
                                                // TODO: don't create cones which are completely identical

                                                if (0.0f >= ode_mu || dInfinity <= ode_mu )
                                                                continue;

                                                dJointFeedback& jointFeedback = world->iContactFeedback.back();
                                                dVector3& forceVector = jointFeedback.f1;
                                                dVector3& normalVector = contact[i].geom.normal;

                                                double normalForce = (  normalVector[0] * forceVector[0]
                                                                                      + normalVector[2] * forceVector[2]
                                                                                      + normalVector[2] * forceVector[2]);
                                                double radius = normalForce * ode_mu;

                                                if(0 == normalForce || SoWorldPhysics::IsNaN(normalForce))
                                                                continue;
/*
                                                SoFrictionCone* fCone = new SoFrictionCone;
                                                fCone->ref();
                                                fCone->scaling.setValue(0.1f); // was: 0.1 for demo
                                                fCone->height.setValue(fabs((float)normalForce));
                                                fCone->radius.setValue(fabs((float)radius));
                                                dVector3& contactPosition = contact[i].geom.pos;
                                                fCone->contactPoint.setValue(contactPosition[0], contactPosition[1], contactPosition[2]);
                                                fCone->contactNormal.setValue(normalVector[0], normalVector[1], normalVector[2]);
                                                fCone->addToGraph(world);
                                                fCone->unref();
*/
                                }

                                // trace contacts here:
                                //printf("contact found at [%2.3f,%2.3f,%2.3f]\n",contact[i].geom.pos[0],contact[i].geom.pos[1],contact[i].geom.pos[2]);
                }

                delete[] contact;
}


void SoWorldPhysics::updateOdeParamtersCB(void* data, SoSensor*)
{
                SoWorldPhysics* world = static_cast<SoWorldPhysics*> (data);

                if (NULL == world)
                                return;

                printf("updating world parameters\n");
                SbVec3f grav = world->gravity.getValue();
                dWorldSetGravity (world->ownworld, grav[0], grav[1], grav[2]);

                float erp = world->ERP.getValue();
                dWorldSetERP(world->ownworld, erp);

                float cfm = world->CFM.getValue();
                dWorldSetCFM(world->ownworld, cfm);

                world->sens.setInterval(SbTime(world->getStepsize()));
}


SbBool SoWorldPhysics::setUpConnections(SbBool onOff, SbBool doItAlways)
{
                if (!doItAlways && connectionsSetUp == onOff)
                                return onOff;
                if (onOff)
                {
                                // do an update before attaching
                                SoWorldPhysics::updateOdeParamtersCB(this, NULL);
                                // attach fields to sensors
                                if (&updateOdeParamtersTrigger != updateOdeParamtersSensor.getAttachedField())
                                    updateOdeParamtersSensor.attach(&updateOdeParamtersTrigger);
                }
                else
                {
                    if (NULL != updateOdeParamtersSensor.getAttachedField())
                        updateOdeParamtersSensor.detach();
                }

                connectionsSetUp = onOff;
                return !connectionsSetUp;
}


SbBool SoWorldPhysics::readInstance(SoInput* in, unsigned short flags)
{
                this->setUpConnections(FALSE, TRUE);
                SbBool resultValue = SoSeparator::readInstance(in, flags);
                this->setUpConnections(TRUE, TRUE);
                return resultValue;
}


SoCallbackAction::Response SoWorldPhysics::UpdateSingleWorldCB(void* userdata, SoCallbackAction* action, const SoNode* node)
{
                SoWorldPhysics* world = dynamic_cast<SoWorldPhysics*> (const_cast<SoNode*> (node));
                assert(NULL != world && "SoWorldPhysics::UpdateSingleWorldCB(): no world present to operate on.");

                dJointGroupEmpty (world->contactgroup);        // delete all contact joints
                world->iContactFeedback.clear();                      // reset contact joint feedback structures
                SoFrictionCone::RemoveAllFrictionConesFromGraph(world);

                //printf("Do ODE collide & worldstep...\n");
                // collision
                dSpaceCollide (world->ownspace, world, &NearCallback);

                // use stepping method as selected in stepping field
                switch (world->stepping.getValue())
                {
                                case (SoWorldPhysics::WORLDSTEP):
                                                dWorldStep (world->ownworld, world->stepsize.getValue());
                                                break;
                                case (SoWorldPhysics::QUICKSTEP):
                                                dWorldQuickStep (world->ownworld, world->stepsize.getValue());
                                                break;
                                //case (SoWorldPhysics::STEPFAST1):
                                //              dWorldStepFast1 (world->ownworld, world->stepsize.getValue(), world->sf1maxiterations.getValue());
                                //              break;
                                default:
                                                assert(0 && "Unknown stepping method, aborting.");
                                                break;
                };

                return SoCallbackAction::CONTINUE;
}

void SoWorldPhysics::StepOde(void* data, SoSensor*)
{
                SoNode* n = static_cast<SoNode*> (data);
    if (NULL == n)
    {
            printf("No World available for stepping\n");
            //return;
    }
                SoCallbackAction updateODEAction;
                updateODEAction.addPostCallback(SoWorldPhysics::getClassTypeId(), UpdateSingleWorldCB,NULL);
                updateODEAction.addPostCallback(SoPhysics::getClassTypeId(), SoPhysics::UpdateOdeBodiesCB,NULL);
                updateODEAction.apply(n);
}


void SoWorldPhysics::resetWorld()
{
    // reset bodies
    SoCallbackAction resetBodyAction;
    resetBodyAction.addPreCallback(SoPhysics::getClassTypeId(), SoPhysics::ResetOdeBodiesCB, NULL);
                resetBodyAction.apply(this);

                //reset joints
                SoCallbackAction initJoint;
                initJoint.addPreCallback(SoJoint::getClassTypeId(), SoJoint::InitOdeJointsCB,NULL);
                initJoint.apply(this);

    // reset this world
                dJointGroupEmpty (this->contactgroup);       // delete all contact joints
                this->iContactFeedback.clear();              // reset contact joint feedback structures
                SoFrictionCone::RemoveAllFrictionConesFromGraph(this);

}

void SoWorldPhysics::initialiseWorld()
{
    //initialise collision shapes
                SoCallbackAction initCollisionShapes;
                initCollisionShapes.addPostCallback(SoCollisionShape::getClassTypeId(), SoCollisionShape::InitCollisionShapesCB, this);
                initCollisionShapes.apply(this);

                //initialise physics bodies
                SoCallbackAction initPhysicsBodies;
                initPhysicsBodies.addPostCallback(SoPhysics::getClassTypeId(), SoPhysics::InitialiseOdeBodiesCB, this);
                initPhysicsBodies.apply(this);

                //initialise joints
                SoCallbackAction initJoint;
                initJoint.addPreCallback(SoJoint::getClassTypeId(), SoJoint::InitOdeJointsCB,NULL);
                initJoint.apply(this);
                printf("World & bodies initialised\n\r");
}


SoCallbackAction::Response SoWorldPhysics::InitialiseWorldCB(void* userdata, SoCallbackAction* action, const SoNode* node)
{
                SoWorldPhysics* world = dynamic_cast<SoWorldPhysics*> (const_cast<SoNode*> (node));
                if (NULL != world)
                                world->initialiseWorld();
                return SoCallbackAction::CONTINUE;
}


SoCallbackAction::Response SoWorldPhysics::ResetWorldCB(void* userdata, SoCallbackAction* action, const SoNode* node)
{
                SoWorldPhysics* world = dynamic_cast<SoWorldPhysics*> (const_cast<SoNode*> (node));
                if (NULL != world)
                                world->resetWorld();
                return SoCallbackAction::CONTINUE;
}


SoCallbackAction::Response SoWorldPhysics::ScheduleWorldCB(void* userdata, SoCallbackAction* action, const SoNode* node)
{
                SoWorldPhysics* world = dynamic_cast<SoWorldPhysics*> (const_cast<SoNode*> (node));
                if (NULL != world)
                                world->schedule();
                return SoCallbackAction::CONTINUE;
}


SoCallbackAction::Response SoWorldPhysics::UnscheduleWorldCB(void* userdata, SoCallbackAction* action, const SoNode* node)
{
                SoWorldPhysics* world = dynamic_cast<SoWorldPhysics*> (const_cast<SoNode*> (node));
                if (NULL != world)
                                world->unschedule();
                return SoCallbackAction::CONTINUE;
}


void SoWorldPhysics::ApplyActionToWorldPhysicNodesInGraph(SoCallbackAction::SoCallbackActionCB &cb, SoNode* root, void* userdata)
{
                SoCallbackAction callbackAction;
                callbackAction.addPostCallback(SoWorldPhysics::getClassTypeId(), cb, userdata);
                callbackAction.apply(root);
}


void SoWorldPhysics::setStepsize(float stepSizeInSeconds)
{
                // apply stepsize
                if (stepSizeInSeconds <= 0.0)
                {
                                printf("Invalid Stepsize: %f [sec]! Keeping old value: %f\n", stepSizeInSeconds, this->stepsize.getValue());
                }
                else
                {
                                this->stepsize.setValue(stepSizeInSeconds);
                                printf("Set Stepsize to: %f [sec].\n", stepSizeInSeconds);
                }
}


float SoWorldPhysics::getStepsize()
{
                return this->stepsize.getValue();
}


void SoWorldPhysics::schedule()
{
                if (!this->sens.isScheduled())
                                this->sens.schedule();
                printf("ODE scheduled.\n");
}


void SoWorldPhysics::unschedule()
{
                if (this->sens.isScheduled())
                                this->sens.unschedule();
                printf("ODE unscheduled.\n");
}


bool SoWorldPhysics::IsNaN(double value)
{
                return (value != value);
}


// TODO: move into separate IPSA class
void SoWorldPhysics::initClasses()
{
                if (bClassesInitialised)
                                return;

                SoWorldPhysics::initClass();

                SoCollisionShape::initClass();
                SoCollisionShapeBox::initClass();
                SoCollisionShapeCylinder::initClass();
                SoCollisionShapeSphere::initClass();
                SoCollisionShapePlane::initClass();
                SoCollisionShapeTriMesh::initClass();

                SoJoint::initClass();
                SoJointAMotor::initClass();
                SoJointBall::initClass();
                SoJointFixed::initClass();
                SoJointHinge::initClass();
                SoJointHinge2::initClass();
                SoJointSlider::initClass();
                SoJointUniversal::initClass();

                SoPhysics::initClass();
                SoPhysicsBox::initClass();
                SoPhysicsCylinder::initClass(); // This must be done before SoPhysicsCapsule as the later is a subclass of SoPhysicsCylinder
                SoPhysicsCapsule::initClass();
                SoPhysicsSphere::initClass();
                SoPhysicsTorus::initClass();
                SoPhysicsTriMesh::initClass();

                SoFrictionCone::initClass();
                SoPrefixNode::initClass();
                bClassesInitialised = true;
}


// The frictiontable:
double SoWorldPhysics::frictioncoeff[SoCollisionShape::NO_OF_SURFACEMATERIALS][SoCollisionShape::NO_OF_SURFACEMATERIALS] =
{
//    CL   GR   W    G    S    R
    { 0.0, 1.0, 0.0, 0.0, 0.0 ,0.0}, // CLEAN
    { 1.0, 1.0, 1.0, 1.0, 1.0 ,1.0}, // GREASY
    { 0.0, 1.0, 0.4, 0.3, 0.3 ,0.5}, // WOOD
    { 0.0, 1.0, 0.3, 0.4, 0.6 ,0.8}, // GLASS
    { 0.0, 1.0, 0.2, 0.3, 0.7 ,0.8}, // STEEL
    { 0.0, 1.0, 0.5, 0.8, 0.8 ,1.0}  // RUBBER
};

---