IPSA: source/SoJointUniversal.cpp Source File

00001 /*
00002  * Copyright (c) 2004, University of Braunschweig (for members see AUTHORS file)
00003  * Copyright (c) 2007, 2008, 2009, University of Karlsruhe (for members see AUTHORS file)
00004  *
00005  * ISC License
00006  *
00007  * Permission to use, copy, modify, and/or distribute this software for any
00008  * purpose with or without fee is hereby granted, provided that the above
00009  * copyright notice and this permission notice appear in all copies.
00010  *
00011  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
00012  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
00013  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
00014  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
00015  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
00016  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
00017  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
00018  */
00019 
00177 #include "../include/IPSA/SoJointUniversal.h"
00178 #include "../include/IPSA/SoPhysics.h"
00179 
00180 #include <Inventor/nodes/SoSphere.h>
00181 #include <Inventor/nodes/SoCylinder.h>
00182 #include <Inventor/nodes/SoRotation.h>
00183 #include <Inventor/nodes/SoTransform.h>
00184 #include <Inventor/nodes/SoNode.h>
00185 #include <Inventor/actions/SoCallbackAction.h>
00186 
00187 SO_KIT_SOURCE(SoJointUniversal);
00188 
00191 void
00192 SoJointUniversal::initClass()
00193 {
00194                 SO_KIT_INIT_CLASS(SoJointUniversal, SoJoint, "Joint");
00195 }
00196 
00197 
00204 SoJointUniversal::SoJointUniversal(const SbString& BodyOne, const SbString& BodyTwo, const SbString& nodeName)
00205 : SoJoint(BodyOne,BodyTwo,nodeName)
00206 {
00207                 SO_KIT_CONSTRUCTOR(SoJointUniversal);
00208                 isBuiltIn = TRUE;
00209 
00210                 SO_KIT_ADD_FIELD(soAnchor,(0.0f,0.0f,0.0f));
00211                 SO_KIT_ADD_FIELD(soAxis1,(0.0f,0.0f,0.0f));
00212                 SO_KIT_ADD_FIELD(soAxis2,(0.0f,0.0f,0.0f));
00213                 SO_KIT_ADD_FIELD(soTorque, (0.0f,0.0f));
00214 
00215                 SO_KIT_ADD_FIELD(LoStop,(-dInfinity));
00216                 SO_KIT_ADD_FIELD(HiStop,(dInfinity));
00217                 SO_KIT_ADD_FIELD(Velocity,(0.0));
00218                 SO_KIT_ADD_FIELD(MaxForce,(0.0));
00219                 SO_KIT_ADD_FIELD(FudgeFactor,(1.0));
00220                 SO_KIT_ADD_FIELD(Bounce,(0.0));
00221                 SO_KIT_ADD_FIELD(CFM,(INIT_CFM));
00222                 SO_KIT_ADD_FIELD(StopERP,(INIT_ERP));
00223                 SO_KIT_ADD_FIELD(StopCFM,(INIT_CFM));
00224 
00225                 SO_KIT_ADD_FIELD(LoStop2,(-dInfinity));
00226                 SO_KIT_ADD_FIELD(HiStop2,(dInfinity));
00227                 SO_KIT_ADD_FIELD(Velocity2,(0.0));
00228                 SO_KIT_ADD_FIELD(MaxForce2,(0.0));
00229                 SO_KIT_ADD_FIELD(FudgeFactor2,(1.0));
00230                 SO_KIT_ADD_FIELD(Bounce2,(0.0));
00231                 SO_KIT_ADD_FIELD(CFM2,(INIT_CFM));
00232                 SO_KIT_ADD_FIELD(StopERP2,(INIT_ERP));
00233                 SO_KIT_ADD_FIELD(StopCFM2,(INIT_CFM));
00234 
00235                 SO_KIT_INIT_INSTANCE();
00236 
00237     updateOdeParametersTrigger.appendConnection(&LoStop);
00238     updateOdeParametersTrigger.appendConnection(&HiStop);
00239     updateOdeParametersTrigger.appendConnection(&Velocity);
00240     updateOdeParametersTrigger.appendConnection(&MaxForce);
00241     updateOdeParametersTrigger.appendConnection(&FudgeFactor);
00242     updateOdeParametersTrigger.appendConnection(&Bounce);
00243     updateOdeParametersTrigger.appendConnection(&CFM);
00244     updateOdeParametersTrigger.appendConnection(&StopERP);
00245     updateOdeParametersTrigger.appendConnection(&StopCFM);
00246 
00247     updateOdeParametersTrigger.appendConnection(&LoStop2);
00248     updateOdeParametersTrigger.appendConnection(&HiStop2);
00249     updateOdeParametersTrigger.appendConnection(&Velocity2);
00250     updateOdeParametersTrigger.appendConnection(&MaxForce2);
00251     updateOdeParametersTrigger.appendConnection(&FudgeFactor2);
00252     updateOdeParametersTrigger.appendConnection(&Bounce2);
00253     updateOdeParametersTrigger.appendConnection(&CFM2);
00254     updateOdeParametersTrigger.appendConnection(&StopERP2);
00255     updateOdeParametersTrigger.appendConnection(&StopCFM2);
00256 }
00257 
00258 
00267 void SoJointUniversal::setTorque(dReal torque1 /*= 0.0f*/, dReal torque2 /*= 0.0f*/, dReal torque3 /*= 0.0f*/)
00268 {
00269                 dJointAddUniversalTorques(this->soJointId, torque1, torque2);
00270 }
00271 
00272 
00278 void SoJointUniversal::createOdeJoint(const dWorldID soWorldId)
00279 {
00280     // save initial values
00281     initVelocity[0] = Velocity.getValue();
00282     initMaxForce[0] = MaxForce.getValue();
00283     initVelocity[1] = Velocity2.getValue();
00284     initMaxForce[1] = MaxForce2.getValue();
00285 
00286     soJointId = dJointCreateUniversal(soWorldId, soJointGroupId);
00287 }
00288 
00289 
00296 void SoJointUniversal::updateOdeJointProperties(const SbMatrix& modelMatrix)
00297 {
00298     Velocity.setValue(initVelocity[0]);
00299     MaxForce.setValue(initMaxForce[0]);
00300     Velocity2.setValue(initVelocity[1]);
00301     MaxForce2.setValue(initMaxForce[1]);
00302 
00303     // get translation and rotation from coin
00304     // to translate anchor and rotate axis
00305     SbVec3f anchorbase = soAnchor.getValue();
00306     SbVec3f axisbase1 = soAxis1.getValue();
00307     SbVec3f axisbase2 = soAxis2.getValue();
00308     SbVec3f raxbase1,raxbase2,ranchorbase;
00309 
00310     SbMatrix mm = modelMatrix;
00311     mm.multDirMatrix(axisbase1,raxbase1);           // rotate axis1: direction vector only
00312     mm.multDirMatrix(axisbase2,raxbase2);           // rotate axis2: direction vector only
00313     mm.multVecMatrix(anchorbase,ranchorbase);       // Transformation: rotate first and translate the anchor
00314 
00315     dJointSetUniversalAnchor(this->soJointId, ranchorbase[0], ranchorbase[1], ranchorbase[2]);
00316     dJointSetUniversalAxis1(this->soJointId,  raxbase1[0],  raxbase1[1],  raxbase1[2]);
00317     dJointSetUniversalAxis2(this->soJointId,  raxbase2[0],  raxbase2[1],  raxbase2[2]);
00318 }
00319 
00320 
00321 SoNode* SoJointUniversal::createVisualisation()
00322 {
00323                 SoSeparator* result = new SoSeparator;
00324 
00325                 // Anchor Point
00326                 SoSphere* anchorPoint = new SoSphere;
00327                 anchorPoint->radius.setValue(30);
00328 
00329                 // First Axis
00330                 SoCylinder* axis1 = new SoCylinder;
00331                 axis1->radius.setValue(anchorPoint->radius.getValue() / 5.0f);
00332                 axis1->height.setValue(anchorPoint->radius.getValue() * 6.0f);
00333 
00334                 SoTransform* axisTransform1 = new SoTransform;
00335                 SoJoint::GenerateCylinderAlignmentRotation(soAxis1.getValue(), *axisTransform1);
00336                 axisTransform1->translation.setValue(this->soAnchor.getValue());
00337 
00338                 // Second Axis
00339                 SoCylinder* axis2 = new SoCylinder;
00340                 axis2->radius.setValue(anchorPoint->radius.getValue() / 5.0f);
00341                 axis2->height.setValue(anchorPoint->radius.getValue() * 6.0f);
00342 
00343                 SoTransform* axisTransform2 = new SoTransform;
00344                 SoJoint::GenerateCylinderAlignmentRotation(soAxis2.getValue(), *axisTransform2);
00345                 axisTransform2->translation.setValue(this->soAnchor.getValue());
00346 
00347                 SoSeparator* sepAnchorAndAxis1 = new SoSeparator;
00348                 sepAnchorAndAxis1->addChild(axisTransform1);
00349                 sepAnchorAndAxis1->addChild(anchorPoint);
00350                 sepAnchorAndAxis1->addChild(axis1);
00351 
00352                 SoSeparator* sepAxis2 = new SoSeparator;
00353                 sepAxis2->addChild(axisTransform2);
00354                 sepAxis2->addChild(axis2);
00355 
00356                 // The rotations for both axis are calculated the same way.
00357                 // Therefore the two separators are needed, because otherwise
00358                 // one rotation would affect the other rotation.
00359                 result->addChild(sepAnchorAndAxis1);
00360                 result->addChild(sepAxis2);
00361 
00362                 return result;
00363 }
00364 
00365 
00366 void SoJointUniversal::setParams()
00367 {
00368                 dJointSetUniversalParam(soJointId,dParamLoStop,LoStop.getValue());
00369                 dJointSetUniversalParam(soJointId,dParamHiStop,HiStop.getValue());
00370                 dJointSetUniversalParam(soJointId,dParamVel,Velocity.getValue());
00371                 dJointSetUniversalParam(soJointId,dParamFMax,MaxForce.getValue());
00372                 dJointSetUniversalParam(soJointId,dParamFudgeFactor,FudgeFactor.getValue());
00373                 dJointSetUniversalParam(soJointId,dParamBounce,Bounce.getValue());
00374                 dJointSetUniversalParam(soJointId,dParamCFM,CFM.getValue());
00375                 dJointSetUniversalParam(soJointId,dParamStopERP,StopERP.getValue());
00376                 dJointSetUniversalParam(soJointId,dParamStopCFM,StopCFM.getValue());
00377 
00378                 dJointSetUniversalParam(soJointId,dParamLoStop2,LoStop2.getValue());
00379                 dJointSetUniversalParam(soJointId,dParamHiStop2,HiStop2.getValue());
00380                 dJointSetUniversalParam(soJointId,dParamVel2,Velocity2.getValue());
00381                 dJointSetUniversalParam(soJointId,dParamFMax2,MaxForce2.getValue());
00382                 dJointSetUniversalParam(soJointId,dParamFudgeFactor2,FudgeFactor2.getValue());
00383                 dJointSetUniversalParam(soJointId,dParamBounce2,Bounce2.getValue());
00384                 dJointSetUniversalParam(soJointId,dParamCFM2,CFM2.getValue());
00385                 dJointSetUniversalParam(soJointId,dParamStopERP2,StopERP2.getValue());
00386                 dJointSetUniversalParam(soJointId,dParamStopCFM2,StopCFM2.getValue());
00387 }