IPSA: source/SoJointAMotor.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 
00278 #include "../include/IPSA/SoJointAMotor.h"
00279 #include "../include/IPSA/SoPhysics.h"
00280 
00281 #include <Inventor/nodes/SoNode.h>
00282 #include <Inventor/actions/SoCallbackAction.h>
00283 #include <cassert>
00284 
00285 SO_KIT_SOURCE(SoJointAMotor);
00286 
00289 void
00290 SoJointAMotor::initClass()
00291 {
00292     SO_KIT_INIT_CLASS(SoJointAMotor, SoJoint, "Joint");
00293 }
00294 
00295 
00302 SoJointAMotor::SoJointAMotor(const SbString& BodyOne, const SbString& BodyTwo, const SbString& nodeName)
00303 : SoJoint(BodyOne,BodyTwo,nodeName)
00304 {
00305     SO_KIT_CONSTRUCTOR(SoJointAMotor);
00306     isBuiltIn = TRUE;
00307 
00308     SO_KIT_ADD_FIELD(soNumberAxes,(3));
00309     SO_KIT_ADD_FIELD(soMotorMode,(eMMEulerMode));
00310     SO_KIT_ADD_FIELD(soAxis,(0.0f,0.0f,0.0f));
00311     SO_KIT_ADD_FIELD(soAngle,(0.0f,0.0f,0.0f));
00312     SO_KIT_ADD_FIELD(soRelativeOrientation,(0));
00313     SO_KIT_ADD_FIELD(soTorque, (0.0f, 0.0f, 0.0f));
00314 
00315     SO_KIT_ADD_FIELD(LoStop,(-dInfinity));
00316     SO_KIT_ADD_FIELD(HiStop,(dInfinity));
00317     SO_KIT_ADD_FIELD(Velocity,(0.0));
00318     SO_KIT_ADD_FIELD(MaxForce,(0.0));
00319     SO_KIT_ADD_FIELD(FudgeFactor,(1.0));
00320     SO_KIT_ADD_FIELD(Bounce,(0.0));
00321     SO_KIT_ADD_FIELD(CFM,(INIT_CFM));
00322     SO_KIT_ADD_FIELD(StopERP,(INIT_ERP));
00323     SO_KIT_ADD_FIELD(StopCFM,(INIT_CFM));
00324     SO_KIT_ADD_FIELD(SuspensionERP,(INIT_ERP));
00325     SO_KIT_ADD_FIELD(SuspensionCFM,(INIT_CFM));
00326 
00327     SO_KIT_ADD_FIELD(LoStop2,(-dInfinity));
00328     SO_KIT_ADD_FIELD(HiStop2,(dInfinity));
00329     SO_KIT_ADD_FIELD(Velocity2,(0.0));
00330     SO_KIT_ADD_FIELD(MaxForce2,(0.0));
00331     SO_KIT_ADD_FIELD(FudgeFactor2,(1.0));
00332     SO_KIT_ADD_FIELD(Bounce2,(0.0));
00333     SO_KIT_ADD_FIELD(CFM2,(INIT_CFM));
00334     SO_KIT_ADD_FIELD(StopERP2,(INIT_ERP));
00335     SO_KIT_ADD_FIELD(StopCFM2,(INIT_CFM));
00336     SO_KIT_ADD_FIELD(SuspensionERP2,(INIT_ERP));
00337     SO_KIT_ADD_FIELD(SuspensionCFM2,(INIT_CFM));
00338 
00339     SO_KIT_ADD_FIELD(LoStop3,(-dInfinity));
00340     SO_KIT_ADD_FIELD(HiStop3,(dInfinity));
00341     SO_KIT_ADD_FIELD(Velocity3,(0.0));
00342     SO_KIT_ADD_FIELD(MaxForce3,(0.0));
00343     SO_KIT_ADD_FIELD(FudgeFactor3,(1.0));
00344     SO_KIT_ADD_FIELD(Bounce3,(0.0));
00345     SO_KIT_ADD_FIELD(CFM3,(INIT_CFM));
00346     SO_KIT_ADD_FIELD(StopERP3,(INIT_ERP));
00347     SO_KIT_ADD_FIELD(StopCFM3,(INIT_CFM));
00348     SO_KIT_ADD_FIELD(SuspensionERP3,(INIT_ERP));
00349     SO_KIT_ADD_FIELD(SuspensionCFM3,(INIT_CFM));
00350 
00351     SO_KIT_INIT_INSTANCE();
00352 
00353     updateOdeParametersTrigger.appendConnection(&LoStop);
00354     updateOdeParametersTrigger.appendConnection(&HiStop);
00355     updateOdeParametersTrigger.appendConnection(&Velocity);
00356     updateOdeParametersTrigger.appendConnection(&MaxForce);
00357     updateOdeParametersTrigger.appendConnection(&FudgeFactor);
00358     updateOdeParametersTrigger.appendConnection(&Bounce);
00359     updateOdeParametersTrigger.appendConnection(&CFM);
00360     updateOdeParametersTrigger.appendConnection(&StopERP);
00361     updateOdeParametersTrigger.appendConnection(&StopCFM);
00362 
00363     updateOdeParametersTrigger.appendConnection(&LoStop2);
00364     updateOdeParametersTrigger.appendConnection(&HiStop2);
00365     updateOdeParametersTrigger.appendConnection(&Velocity2);
00366     updateOdeParametersTrigger.appendConnection(&MaxForce2);
00367     updateOdeParametersTrigger.appendConnection(&FudgeFactor2);
00368     updateOdeParametersTrigger.appendConnection(&Bounce2);
00369     updateOdeParametersTrigger.appendConnection(&CFM2);
00370     updateOdeParametersTrigger.appendConnection(&StopERP2);
00371     updateOdeParametersTrigger.appendConnection(&StopCFM2);
00372 
00373     updateOdeParametersTrigger.appendConnection(&LoStop3);
00374     updateOdeParametersTrigger.appendConnection(&HiStop3);
00375     updateOdeParametersTrigger.appendConnection(&Velocity3);
00376     updateOdeParametersTrigger.appendConnection(&MaxForce3);
00377     updateOdeParametersTrigger.appendConnection(&FudgeFactor3);
00378     updateOdeParametersTrigger.appendConnection(&Bounce3);
00379     updateOdeParametersTrigger.appendConnection(&CFM3);
00380     updateOdeParametersTrigger.appendConnection(&StopERP3);
00381     updateOdeParametersTrigger.appendConnection(&StopCFM3);
00382 }
00383 
00384 
00393 void SoJointAMotor::setTorque(dReal torque1 /*= 0.0f*/, dReal torque2 /*= 0.0f*/, dReal torque3 /*= 0.0f*/)
00394 {
00395     dJointAddAMotorTorques(this->soJointId, torque1, torque2, torque3);
00396 }
00397 
00398 
00399 void SoJointAMotor::setParams()
00400 {
00401     dJointSetAMotorParam(soJointId,dParamLoStop,LoStop.getValue());
00402     dJointSetAMotorParam(soJointId,dParamHiStop,HiStop.getValue());
00403     dJointSetAMotorParam(soJointId,dParamVel,Velocity.getValue());
00404     dJointSetAMotorParam(soJointId,dParamFMax,MaxForce.getValue());
00405     dJointSetAMotorParam(soJointId,dParamFudgeFactor,FudgeFactor.getValue());
00406     dJointSetAMotorParam(soJointId,dParamBounce,Bounce.getValue());
00407     dJointSetAMotorParam(soJointId,dParamCFM,CFM.getValue());
00408     dJointSetAMotorParam(soJointId,dParamStopERP,StopERP.getValue());
00409     dJointSetAMotorParam(soJointId,dParamStopCFM,StopCFM.getValue());
00410 
00411     dJointSetAMotorParam(soJointId,dParamLoStop2,LoStop2.getValue());
00412     dJointSetAMotorParam(soJointId,dParamHiStop2,HiStop2.getValue());
00413     dJointSetAMotorParam(soJointId,dParamVel2,Velocity2.getValue());
00414     dJointSetAMotorParam(soJointId,dParamFMax2,MaxForce2.getValue());
00415     dJointSetAMotorParam(soJointId,dParamFudgeFactor2,FudgeFactor2.getValue());
00416     dJointSetAMotorParam(soJointId,dParamBounce2,Bounce2.getValue());
00417     dJointSetAMotorParam(soJointId,dParamCFM2,CFM2.getValue());
00418     dJointSetAMotorParam(soJointId,dParamStopERP2,StopERP2.getValue());
00419     dJointSetAMotorParam(soJointId,dParamStopCFM2,StopCFM2.getValue());
00420 
00421     dJointSetAMotorParam(soJointId,dParamLoStop3,LoStop3.getValue());
00422     dJointSetAMotorParam(soJointId,dParamHiStop3,HiStop3.getValue());
00423     dJointSetAMotorParam(soJointId,dParamVel3,Velocity3.getValue());
00424     dJointSetAMotorParam(soJointId,dParamFMax3,MaxForce3.getValue());
00425     dJointSetAMotorParam(soJointId,dParamFudgeFactor3,FudgeFactor3.getValue());
00426     dJointSetAMotorParam(soJointId,dParamBounce3,Bounce3.getValue());
00427     dJointSetAMotorParam(soJointId,dParamCFM3,CFM3.getValue());
00428     dJointSetAMotorParam(soJointId,dParamStopERP3,StopERP3.getValue());
00429     dJointSetAMotorParam(soJointId,dParamStopCFM3,StopCFM3.getValue());
00430 }
00431 
00432 
00438 void SoJointAMotor::createOdeJoint(const dWorldID soWorldId)
00439 {
00440     // save initial values
00441     initVelocity[0] = Velocity.getValue();
00442     initMaxForce[0] = MaxForce.getValue();
00443     initVelocity[1] = Velocity2.getValue();
00444     initMaxForce[1] = MaxForce2.getValue();
00445     initVelocity[2] = Velocity3.getValue();
00446     initMaxForce[2] = MaxForce3.getValue();
00447     initTorque = soTorque.getValue();
00448 
00449     soJointId = dJointCreateAMotor(soWorldId, soJointGroupId);
00450     dJointSetAMotorNumAxes (soJointId, soNumberAxes.getValue());   // set no. of axes
00451 }
00452 
00453 
00460 void SoJointAMotor::updateOdeJointProperties(const SbMatrix& modelMatrix)
00461 {
00462     Velocity.setValue(initVelocity[0]);
00463     MaxForce.setValue(initMaxForce[0]);
00464     Velocity2.setValue(initVelocity[1]);
00465     MaxForce2.setValue(initMaxForce[1]);
00466     Velocity3.setValue(initVelocity[2]);
00467     MaxForce3.setValue(initMaxForce[2]);
00468     soTorque.setValue(initTorque);
00469 
00470     // get translation and rotation from coin
00471     // to translate anchor and rotate axis
00472     int relOrientation[3];
00473     relOrientation[0] = soRelativeOrientation[0];
00474     relOrientation[1] = soRelativeOrientation[1];
00475     relOrientation[2] = soRelativeOrientation[2];
00476 
00477     SbVec3f axisbase1, axisbase2, axisbase3;
00478     SbMatrix mm = modelMatrix;
00479     mm.multDirMatrix(soAxis[0], axisbase1); // rotate axis1: direction vector only
00480     mm.multDirMatrix(soAxis[0], axisbase1); // rotate axis2: direction vector only
00481     mm.multDirMatrix(soAxis[0], axisbase1); // rotate axis3: direction vector only
00482     /*
00483      \todo For dAMotorEuler mode:
00484      - Only axes 0 and 2 need to be set. Axis 1 will be determined automatically at each time step.
00485      - Axes 0 and 2 must be perpendicular to each other.
00486      - Axis 0 must be anchored to the first body, axis 2 must be anchored to the second body.
00487      */
00488     dJointSetAMotorAxis(this->soJointId, 0, relOrientation[0], axisbase1[0], axisbase1[1], axisbase1[2]);
00489     dJointSetAMotorAxis(this->soJointId, 1, relOrientation[1], axisbase2[0], axisbase2[1], axisbase2[2]);
00490     dJointSetAMotorAxis(this->soJointId, 2, relOrientation[2], axisbase3[0], axisbase3[1], axisbase3[2]);
00491 
00492     //add torque
00493     this->setTorque(initTorque[0], initTorque[1], initTorque[2]);
00494 }
00495 
00496 
00497 SoNode* SoJointAMotor::createVisualisation()
00498 {
00499     return new SoSeparator;
00500 }