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

#include <Inventor/nodes/SoTransform.h>


SO_KIT_SOURCE(SoJoint);

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


SoJoint::SoJoint(const SbString& BodyOne, const SbString& BodyTwo, const SbString& nodeName)
: INIT_CFM(1e-8f),
INIT_ERP(0.2f),
isInitialised(FALSE)
{
                SO_KIT_CONSTRUCTOR(SoJoint);
                isBuiltIn = TRUE;

                SO_KIT_ADD_CATALOG_ENTRY(visualisationShape, SoSeparator, FALSE, this, "", TRUE);

                SO_KIT_ADD_FIELD(Bodyname1, (""));
                SO_KIT_ADD_FIELD(Bodyname2, (""));
                SO_KIT_ADD_FIELD(VisualisationScale, (1.0f));
                SO_KIT_ADD_FIELD(drawVisualisationShape, (FALSE));

                SO_KIT_INIT_INSTANCE();

                soJointFeedback = new dJointFeedback;
                soJointGroupId = dJointGroupCreate(0);

                drawVisualisationShapeSensor.setFunction(SoJoint::IPSAJointDrawVisualisationChangedCB);
                drawVisualisationShapeSensor.setData(this);
                updateOdeParametersSensor.setFunction(SoJoint::IPSAUpdateOdeParametersCB);
                updateOdeParametersSensor.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 SoJoint::InitOdeJointsCB() when the SoJoint
                // subclass instance gets initialised.

                Bodyname1.setValue(BodyOne);
                Bodyname2.setValue(BodyTwo);

                this->setName(nodeName);
}


SoBaseKit* SoJoint::copy(SbBool copyConnections) const
{
                SoJoint* newJoint = static_cast<SoJoint*> (SoBaseKit::copy(copyConnections));

                newJoint->soJointFeedback = soJointFeedback;
                newJoint->soJointGroupId = soJointGroupId;
                newJoint->soJointId = soJointId;
                newJoint->isInitialised = isInitialised;
                newJoint->setUpConnections(TRUE, TRUE);

                return newJoint;
}


SbBool SoJoint::setUpConnections(SbBool onOff, SbBool doItAlways)
{
                if (!doItAlways && connectionsSetUp == onOff)
                                return onOff;
                if (onOff)
                {
                                SoBaseKit::setUpConnections(onOff, doItAlways);
                                // update before attach
                                SoJoint::IPSAJointDrawVisualisationChangedCB(this, NULL);
                                // attach field to sensor
                                if (&drawVisualisationShape != drawVisualisationShapeSensor.getAttachedField())
                                                drawVisualisationShapeSensor.attach(&drawVisualisationShape);
                                if (&updateOdeParametersTrigger != updateOdeParametersSensor.getAttachedField())
                                    updateOdeParametersSensor.attach(&updateOdeParametersTrigger);
                }
                else
                {
                                if (NULL != drawVisualisationShapeSensor.getAttachedField())
                                                drawVisualisationShapeSensor.detach();
                                if (NULL != updateOdeParametersSensor.getAttachedField())
                                    updateOdeParametersSensor.detach();
                                SoBaseKit::setUpConnections(onOff, doItAlways);
                }

                connectionsSetUp = onOff;
                return !connectionsSetUp;
}


SoJoint::~SoJoint()
{
                delete soJointFeedback;
                soJointFeedback = NULL;
                destroyJoint();
}


void  SoJoint::destroyJoint()
{
                if (NULL != soJointGroupId)
                {
                                dJointGroupDestroy(soJointGroupId);
                                soJointGroupId = NULL;
                }
}


dJointID SoJoint::getJointID()
{
                return this->soJointId;
}


SoCallbackAction::Response SoJoint::InitOdeJointsCB(void* userdata, SoCallbackAction* action, const SoNode* node)
{
    SoJoint* joint = dynamic_cast<SoJoint*> (const_cast<SoNode*> (node));
    if (NULL == joint)
        return SoCallbackAction::CONTINUE;

    if (!joint->isInitialised)
    {
        joint->setUpConnections(TRUE, TRUE);
        printf("creating %s (%s) ", joint->getTypeId().getName().getString(), joint->getName().getString());

        //get connected physic bodies
        SoPhysics* p1 = joint->getPhysicsBody1();
        SoPhysics* p2 = joint->getPhysicsBody2();
        assert (p1 != NULL);

        printf("linking (%s)<->(%s)\n\r", p1->getName().getString(), (p2 != NULL ? p2->getName().getString():("")) );

        dWorldID soWorldId = p1->getWorld();
        assert (soWorldId != NULL && "SoJoint::InitOdeJointsCB() soWorldID must not be NULL");

        // this is needed for the fixed joint which gets connected to the
        // world if the second body ID is NULL.
        dBodyID body2 = NULL;
        if (p2 != NULL)
        {
            assert (soWorldId == p2->getWorld() && "SoJoint::InitOdeJointsCB() bodies connected to the joint don't exist in the same world");
            body2 = p2->getBody();
        }

        joint->createOdeJoint(soWorldId);
        dJointID odeJointId = joint->soJointId;

        if (NULL != odeJointId)
        {
            // attach bodies to the joint and set the feedback
            dJointAttach(odeJointId, p1->getBody(), body2);
            dJointSetFeedback(odeJointId, joint->soJointFeedback);
        }
        else
            printf("SoJoint::InitOdeJointsCB() no joint created");

        joint->isInitialised = TRUE;
    }

    joint->updateOdeJointProperties(action->getModelMatrix());
    // set the joint parameters
    joint->setParams();

    return SoCallbackAction::CONTINUE;
}


void SoJoint::createOdeJoint(const dWorldID soWorldId)
{
    soJointId = NULL;
}


void SoJoint::updateOdeJointProperties(const SbMatrix& modelMatrix)
{
    // do nothing
}


SoPhysics* SoJoint::getPhysicsBody(const SbName& bodyName)
{
                if (0 >= bodyName.getLength())
                                return NULL;

                SoNode* bodyNode = SoNode::getByName(bodyName.getString());

                SoPhysics* physics = dynamic_cast<SoPhysics*>(bodyNode);

                return physics;
}


SoPhysics* SoJoint::getPhysicsBody1()
{
                return getPhysicsBody(Bodyname1.getValue());
}


SoPhysics* SoJoint::getPhysicsBody2()
{
                return getPhysicsBody(Bodyname2.getValue());
}


void SoJoint::setTorque(dReal torque1 /*= 0.0f*/, dReal torque2 /*= 0.0f*/, dReal torque3 /*= 0.0f*/)
{
                printf("This method is not implemented by this joint\n");
}


void SoJoint::setForce(dReal force1 /*= 0.0f*/)
{
                printf("This method is not implemented by this joint\n");
}


SoSFVec3f SoJoint::getTorque(int idx)
{
                SoSFVec3f ret;

                assert(soJointId != NULL);
                soJointFeedback = dJointGetFeedback(soJointId);

                switch (idx)
                {
                                case 1:
                                                ret.setValue(soJointFeedback->t1[0],soJointFeedback->t1[1],soJointFeedback->t1[2]);
                                                break;
                                case 2:
                                                ret.setValue(soJointFeedback->t2[0],soJointFeedback->t2[1],soJointFeedback->t2[2]);
                                                break;
                                default:
                                                ret.setValue(soJointFeedback->t1[0],soJointFeedback->t1[1],soJointFeedback->t1[2]);
                                                break;
                }
                return ret;
}


SoSFVec3f SoJoint::getForce(int idx)
{
                SoSFVec3f ret;

                assert(soJointId != NULL);
                soJointFeedback = dJointGetFeedback(soJointId);

                switch (idx)
                {
                                case 1:
                                                ret.setValue(soJointFeedback->f1[0],soJointFeedback->f1[1],soJointFeedback->f1[2]);
                                                break;
                                case 2:
                                                ret.setValue(soJointFeedback->f2[0],soJointFeedback->f2[1],soJointFeedback->f2[2]);
                                                break;
                                default:
                                                ret.setValue(soJointFeedback->f1[0],soJointFeedback->f1[1],soJointFeedback->f1[2]);
                                                break;
                }
                return ret;
}


void SoJoint::IPSAJointDrawVisualisationChangedCB(void* data, SoSensor*)
{
                SoJoint* currentJoint = static_cast<SoJoint*> (data);
                if (NULL == currentJoint)
                                return;

                SoSeparator* currentVisualisation = dynamic_cast<SoSeparator*> (currentJoint->visualisationShape.getValue());
                if (FALSE == currentJoint->drawVisualisationShape.getValue())
                                currentVisualisation->removeAllChildren();
                else
                {
                                SoNode* visualisation = currentJoint->createVisualisation();
                                if (NULL != visualisation)
                                                currentVisualisation->addChild(visualisation);
                }
}


void SoJoint::IPSAUpdateOdeParametersCB(void* data, SoSensor*)
{
    SoJoint* currentJoint = static_cast<SoJoint*>(data);
    if(!currentJoint)
        return;

    currentJoint->setParams();
}


void SoJoint::GenerateCylinderAlignmentRotation(const SbVec3f& axis, SoTransform& transform)
{
                SbVec3f rotation;
                rotation[0] = axis.dot(SbVec3f(0, 0, 1));
                rotation[1] = axis.dot(SbVec3f(0, 1, 0));
                rotation[2] = axis.dot(SbVec3f(1, 0, 0));

                transform.rotation.setValue(rotation, -1.57f);
}

---