IPSA: source/SoJointHinge2.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 
00191 #include "../include/IPSA/SoJointHinge2.h"
00192 #include "../include/IPSA/SoPhysics.h"
00193 
00194 #include <Inventor/nodes/SoSphere.h>
00195 #include <Inventor/nodes/SoCylinder.h>
00196 #include <Inventor/nodes/SoRotation.h>
00197 #include <Inventor/nodes/SoTransform.h>
00198 #include <Inventor/nodes/SoNode.h>
00199 #include <Inventor/actions/SoCallbackAction.h>
00200 
00201 SO_KIT_SOURCE(SoJointHinge2);
00202 
00205 void
00206 SoJointHinge2::initClass()
00207 {
00208                 SO_KIT_INIT_CLASS(SoJointHinge2, SoJoint, "Joint");
00209 }
00210 
00211 
00218 SoJointHinge2::SoJointHinge2(const SbString& BodyOne, const SbString& BodyTwo, const SbString& nodeName)
00219 : SoJoint(BodyOne,BodyTwo,nodeName)
00220 {
00221                 SO_KIT_CONSTRUCTOR(SoJointHinge2);
00222                 isBuiltIn = TRUE;
00223 
00224                 SO_KIT_ADD_FIELD(soAnchor,(0.0f,0.0f,0.0f));
00225                 SO_KIT_ADD_FIELD(soAxis1,(0.0f,0.0f,0.0f));
00226                 SO_KIT_ADD_FIELD(soAxis2,(0.0f,0.0f,0.0f));
00227                 SO_KIT_ADD_FIELD(soTorque, (0.0f,0.0f));
00228 
00229                 SO_KIT_ADD_FIELD(LoStop,(-dInfinity));
00230                 SO_KIT_ADD_FIELD(HiStop,(dInfinity));
00231                 SO_KIT_ADD_FIELD(Velocity,(0.0));
00232                 SO_KIT_ADD_FIELD(MaxForce,(0.0));
00233                 SO_KIT_ADD_FIELD(FudgeFactor,(1.0));
00234                 SO_KIT_ADD_FIELD(Bounce,(0.0));
00235                 SO_KIT_ADD_FIELD(CFM,(INIT_CFM));
00236                 SO_KIT_ADD_FIELD(StopERP,(INIT_ERP));
00237                 SO_KIT_ADD_FIELD(StopCFM,(INIT_CFM));
00238                 SO_KIT_ADD_FIELD(SuspensionERP,(INIT_ERP));
00239                 SO_KIT_ADD_FIELD(SuspensionCFM,(INIT_CFM));
00240 
00241                 SO_KIT_ADD_FIELD(LoStop2,(-dInfinity));
00242                 SO_KIT_ADD_FIELD(HiStop2,(dInfinity));
00243                 SO_KIT_ADD_FIELD(Velocity2,(0.0));
00244                 SO_KIT_ADD_FIELD(MaxForce2,(0.0));
00245                 SO_KIT_ADD_FIELD(FudgeFactor2,(1.0));
00246                 SO_KIT_ADD_FIELD(Bounce2,(0.0));
00247                 SO_KIT_ADD_FIELD(CFM2,(INIT_CFM));
00248                 SO_KIT_ADD_FIELD(StopERP2,(INIT_ERP));
00249                 SO_KIT_ADD_FIELD(StopCFM2,(INIT_CFM));
00250                 SO_KIT_ADD_FIELD(SuspensionERP2,(INIT_ERP));
00251                 SO_KIT_ADD_FIELD(SuspensionCFM2,(INIT_CFM));
00252 
00253                 SO_KIT_INIT_INSTANCE();
00254 
00255     updateOdeParametersTrigger.appendConnection(&LoStop);
00256     updateOdeParametersTrigger.appendConnection(&HiStop);
00257     updateOdeParametersTrigger.appendConnection(&Velocity);
00258     updateOdeParametersTrigger.appendConnection(&MaxForce);
00259     updateOdeParametersTrigger.appendConnection(&FudgeFactor);
00260     updateOdeParametersTrigger.appendConnection(&Bounce);
00261     updateOdeParametersTrigger.appendConnection(&CFM);
00262     updateOdeParametersTrigger.appendConnection(&StopERP);
00263     updateOdeParametersTrigger.appendConnection(&StopCFM);
00264     updateOdeParametersTrigger.appendConnection(&SuspensionERP);
00265     updateOdeParametersTrigger.appendConnection(&SuspensionCFM);
00266 
00267     updateOdeParametersTrigger.appendConnection(&LoStop2);
00268     updateOdeParametersTrigger.appendConnection(&HiStop2);
00269     updateOdeParametersTrigger.appendConnection(&Velocity2);
00270     updateOdeParametersTrigger.appendConnection(&MaxForce2);
00271     updateOdeParametersTrigger.appendConnection(&FudgeFactor2);
00272     updateOdeParametersTrigger.appendConnection(&Bounce2);
00273     updateOdeParametersTrigger.appendConnection(&CFM2);
00274     updateOdeParametersTrigger.appendConnection(&StopERP2);
00275     updateOdeParametersTrigger.appendConnection(&StopCFM2);
00276     updateOdeParametersTrigger.appendConnection(&SuspensionERP2);
00277     updateOdeParametersTrigger.appendConnection(&SuspensionCFM2);
00278 }
00279 
00280 
00289 void SoJointHinge2::setTorque(dReal torque1 /*= 0.0f*/, dReal torque2 /*= 0.0f*/, dReal torque3 /*= 0.0f*/)
00290 {
00291                 dJointAddHinge2Torques(this->soJointId, torque1, torque2);
00292 }
00293 
00294 
00300 void SoJointHinge2::createOdeJoint(const dWorldID soWorldId)
00301 {
00302     // save initial values
00303     initVelocity[0] = Velocity.getValue();
00304     initMaxForce[0] = MaxForce.getValue();
00305     initVelocity[1] = Velocity2.getValue();
00306     initMaxForce[1] = MaxForce2.getValue();
00307 
00308     soJointId = dJointCreateHinge2(soWorldId, soJointGroupId);
00309 }
00310 
00311 
00318 void SoJointHinge2::updateOdeJointProperties(const SbMatrix& modelMatrix)
00319 {
00320     Velocity.setValue(initVelocity[0]);
00321     MaxForce.setValue(initMaxForce[0]);
00322     Velocity2.setValue(initVelocity[1]);
00323     MaxForce2.setValue(initMaxForce[1]);
00324 
00325     // get translation and rotation from coin
00326     // to translate anchor and rotate axes
00327     SbVec3f anchorbase = soAnchor.getValue();
00328     SbVec3f axisbase1 = soAxis1.getValue();
00329     SbVec3f axisbase2 = soAxis2.getValue();
00330     SbVec3f raxbase1,raxbase2,ranchorbase;
00331 
00332     SbMatrix mm = modelMatrix;
00333     mm.multDirMatrix(axisbase1,raxbase1);       // rotate axis: direction vector only
00334     mm.multDirMatrix(axisbase2,raxbase2);       // rotate axis: direction vector only
00335     mm.multVecMatrix(anchorbase,ranchorbase);   // Transformation: rotate first and translate the anchor
00336 
00337     dJointSetHinge2Anchor(soJointId, ranchorbase[0], ranchorbase[1], ranchorbase[2]);
00338     dJointSetHinge2Axis1(soJointId,  raxbase1[0], raxbase1[1], raxbase1[2]);
00339     dJointSetHinge2Axis2(soJointId,  raxbase2[0], raxbase2[1], raxbase2[2]);
00340 }
00341 
00342 
00343 SoNode* SoJointHinge2::createVisualisation()
00344 {
00345                 SoSeparator* result = new SoSeparator;
00346 
00347                 // Anchor Point
00348                 SoSphere* anchorPoint = new SoSphere;
00349                 anchorPoint->radius.setValue(30);
00350 
00351                 // First Axis
00352                 SoCylinder* axis1 = new SoCylinder;
00353                 axis1->radius.setValue(anchorPoint->radius.getValue() / 5.0f);
00354                 axis1->height.setValue(anchorPoint->radius.getValue() * 6.0f);
00355 
00356                 SoTransform* axisTransform1 = new SoTransform;
00357                 SoJoint::GenerateCylinderAlignmentRotation(soAxis1.getValue(), *axisTransform1);
00358                 axisTransform1->translation.setValue(this->soAnchor.getValue());
00359 
00360                 // Second Axis
00361                 SoCylinder* axis2 = new SoCylinder;
00362                 axis2->radius.setValue(anchorPoint->radius.getValue() / 5.0f);
00363                 axis2->height.setValue(anchorPoint->radius.getValue() * 6.0f);
00364 
00365                 SoTransform* axisTransform2 = new SoTransform;
00366                 SoJoint::GenerateCylinderAlignmentRotation(soAxis2.getValue(), *axisTransform2);
00367                 axisTransform2->translation.setValue(this->soAnchor.getValue());
00368 
00369                 SoSeparator* sepAnchorAndAxis1 = new SoSeparator;
00370                 sepAnchorAndAxis1->addChild(axisTransform1);
00371                 sepAnchorAndAxis1->addChild(anchorPoint);
00372                 sepAnchorAndAxis1->addChild(axis1);
00373 
00374                 SoSeparator* sepAxis2 = new SoSeparator;
00375                 sepAxis2->addChild(axisTransform2);
00376                 sepAxis2->addChild(axis2);
00377 
00378                 // The rotations for both axis are calculated the same way.
00379                 // Therefore the two separators are needed, because otherwise
00380                 // one rotation would affect the other rotation.
00381                 result->addChild(sepAnchorAndAxis1);
00382                 result->addChild(sepAxis2);
00383 
00384                 return result;
00385 }
00386 
00387 
00388 void SoJointHinge2::setParams()
00389 {
00390                 dJointSetHinge2Param(soJointId,dParamLoStop,LoStop.getValue());
00391                 dJointSetHinge2Param(soJointId,dParamHiStop,HiStop.getValue());
00392                 dJointSetHinge2Param(soJointId,dParamVel,Velocity.getValue());
00393                 dJointSetHinge2Param(soJointId,dParamFMax,MaxForce.getValue());
00394                 dJointSetHinge2Param(soJointId,dParamFudgeFactor,FudgeFactor.getValue());
00395                 dJointSetHinge2Param(soJointId,dParamBounce,Bounce.getValue());
00396                 dJointSetHinge2Param(soJointId,dParamCFM,CFM.getValue());
00397                 dJointSetHinge2Param(soJointId,dParamStopERP,StopERP.getValue());
00398                 dJointSetHinge2Param(soJointId,dParamStopCFM,StopCFM.getValue());
00399                 dJointSetHingeParam(soJointId, dParamSuspensionERP, SuspensionERP.getValue());
00400                 dJointSetHingeParam(soJointId, dParamSuspensionCFM, SuspensionCFM.getValue());
00401 
00402                 dJointSetHinge2Param(soJointId,dParamLoStop2,LoStop2.getValue());
00403                 dJointSetHinge2Param(soJointId,dParamHiStop2,HiStop2.getValue());
00404                 dJointSetHinge2Param(soJointId,dParamVel2,Velocity2.getValue());
00405                 dJointSetHinge2Param(soJointId,dParamFMax2,MaxForce2.getValue());
00406                 dJointSetHinge2Param(soJointId,dParamFudgeFactor2,FudgeFactor2.getValue());
00407                 dJointSetHinge2Param(soJointId,dParamBounce2,Bounce2.getValue());
00408                 dJointSetHinge2Param(soJointId,dParamCFM2,CFM2.getValue());
00409                 dJointSetHinge2Param(soJointId,dParamStopERP2,StopERP2.getValue());
00410                 dJointSetHinge2Param(soJointId,dParamStopCFM2,StopCFM2.getValue());
00411     dJointSetHingeParam(soJointId, dParamSuspensionERP2, SuspensionERP2.getValue());
00412     dJointSetHingeParam(soJointId, dParamSuspensionCFM2, SuspensionCFM2.getValue());
00413 }
00414 
00415 SoSFFloat SoJointHinge2::getAngle()
00416 {
00417                 SoSFFloat ret;
00418                 ret.setValue(dJointGetHinge2Angle1(this->soJointId));
00419                 return ret;
00420 }
00421 
00422 SoSFFloat SoJointHinge2::getAngleRate()
00423 {
00424                 SoSFFloat ret;
00425                 ret.setValue(dJointGetHinge2Angle1Rate(this->soJointId));
00426                 return ret;
00427 }
00428