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

#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoRotation.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/actions/SoCallbackAction.h>


SO_KIT_SOURCE(SoJointHinge);

void
SoJointHinge::initClass()
{
                SO_KIT_INIT_CLASS(SoJointHinge, SoJoint, "Joint");
}


SoJointHinge::SoJointHinge(const SbString& BodyOne, const SbString& BodyTwo, const SbString& nodeName)
: SoJoint(BodyOne,BodyTwo,nodeName)
{
                SO_KIT_CONSTRUCTOR(SoJointHinge);
                isBuiltIn = TRUE;

                SO_KIT_ADD_FIELD(soAnchor,(0.0f, 0.0f, 0.0f));
                SO_KIT_ADD_FIELD(soAxis,(1.0f, 0.0f, 0.0f));

                SO_KIT_ADD_FIELD(AngleOffset,(0.0));
                SO_KIT_ADD_FIELD(LoStop,(-dInfinity));
                SO_KIT_ADD_FIELD(HiStop,(dInfinity));
                SO_KIT_ADD_FIELD(Velocity,(0.0));
                SO_KIT_ADD_FIELD(MaxForce,(0.0));
                SO_KIT_ADD_FIELD(FudgeFactor,(1.0));
                SO_KIT_ADD_FIELD(Bounce,(0.0));
                SO_KIT_ADD_FIELD(CFM,(INIT_CFM));
                SO_KIT_ADD_FIELD(StopERP,(INIT_ERP));
                SO_KIT_ADD_FIELD(StopCFM,(INIT_CFM));
                SO_KIT_ADD_FIELD(SuspensionERP,(INIT_ERP));
                SO_KIT_ADD_FIELD(SuspensionCFM,(INIT_CFM));

                SO_KIT_INIT_INSTANCE();

    updateOdeParametersTrigger.appendConnection(&AngleOffset);
    updateOdeParametersTrigger.appendConnection(&LoStop);
    updateOdeParametersTrigger.appendConnection(&HiStop);
    updateOdeParametersTrigger.appendConnection(&Velocity);
    updateOdeParametersTrigger.appendConnection(&MaxForce);
    updateOdeParametersTrigger.appendConnection(&FudgeFactor);
    updateOdeParametersTrigger.appendConnection(&Bounce);
    updateOdeParametersTrigger.appendConnection(&CFM);
    updateOdeParametersTrigger.appendConnection(&StopERP);
    updateOdeParametersTrigger.appendConnection(&StopCFM);
    updateOdeParametersTrigger.appendConnection(&SuspensionERP);
    updateOdeParametersTrigger.appendConnection(&SuspensionCFM);
}


void SoJointHinge::setTorque(dReal torque1 /*= 0.0f*/, dReal torque2 /*= 0.0f*/, dReal torque3 /*= 0.0f*/)
{
                dJointAddHingeTorque(this->soJointId, torque1);
}


void SoJointHinge::createOdeJoint(const dWorldID soWorldId)
{
    // save initial values
    initVelocity = Velocity.getValue();
    initMaxForce = MaxForce.getValue();
    // create the ode joint
    soJointId = dJointCreateHinge(soWorldId, soJointGroupId);
}


void SoJointHinge::updateOdeJointProperties(const SbMatrix& modelMatrix)
{
    Velocity.setValue(initVelocity);
    MaxForce.setValue(initMaxForce);

    // get translation and rotation from coin
    // to translate anchor and rotate axis
    SbVec3f anchorbase = soAnchor.getValue();
    SbVec3f axisbase = soAxis.getValue();
    SbVec3f raxbase,ranchorbase;

    SbMatrix mm = modelMatrix;
    mm.multDirMatrix(axisbase,raxbase);         // rotate axis: direction vector only
    mm.multVecMatrix(anchorbase,ranchorbase);   // Transformation: rotate first and translate the anchor

    dJointSetHingeAnchor(this->soJointId, ranchorbase[0], ranchorbase[1], ranchorbase[2]);
    dJointSetHingeAxis(this->soJointId, raxbase[0], raxbase[1], raxbase[2]);
}


SoNode* SoJointHinge::createVisualisation()
{
                SoSeparator* result = new SoSeparator;

                SoSphere* anchorPoint = new SoSphere;
                anchorPoint->radius.setValue(30 * VisualisationScale.getValue());

                SoCylinder* axis = new SoCylinder;
                axis->radius.setValue(anchorPoint->radius.getValue() / 5.0f);
                axis->height.setValue(anchorPoint->radius.getValue() * 6.0f);

                SoTransform* axisTransform = new SoTransform;
                SoJoint::GenerateCylinderAlignmentRotation(soAxis.getValue(), *axisTransform);
                axisTransform->translation.setValue(this->soAnchor.getValue());

                result->addChild(axisTransform);
                result->addChild(anchorPoint);
                result->addChild(axis);

                return result;
}


void SoJointHinge::setParams()
{
                dJointSetHingeParam(soJointId, dParamLoStop, LoStop.getValue() - AngleOffset.getValue());
                dJointSetHingeParam(soJointId, dParamHiStop ,HiStop.getValue() - AngleOffset.getValue());
                dJointSetHingeParam(soJointId, dParamVel, Velocity.getValue());
                dJointSetHingeParam(soJointId, dParamFMax, MaxForce.getValue());
                dJointSetHingeParam(soJointId, dParamFudgeFactor, FudgeFactor.getValue());
                dJointSetHingeParam(soJointId, dParamBounce, Bounce.getValue());
                dJointSetHingeParam(soJointId, dParamCFM, CFM.getValue());
                dJointSetHingeParam(soJointId, dParamStopERP, StopERP.getValue());
                dJointSetHingeParam(soJointId, dParamStopCFM, StopCFM.getValue());
                dJointSetHingeParam(soJointId, dParamSuspensionERP, SuspensionERP.getValue());
                dJointSetHingeParam(soJointId, dParamSuspensionCFM, SuspensionCFM.getValue());
}


SoSFFloat SoJointHinge::getAngle()
{
                SoSFFloat ret;
                ret.setValue(dJointGetHingeAngle(this->soJointId) + AngleOffset.getValue());
                return ret;
}


SoSFFloat SoJointHinge::getAngleRate()
{
                SoSFFloat ret;
                ret.setValue(dJointGetHingeAngleRate(this->soJointId));
                return ret;
}

---