00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include <PVLE/Physics/Joint.h>
00028 #include <PVLE/Physics/Body.h>
00029 #include <PVLE/Physics/World.h>
00030 #include <PVLE/Physics/Visitor.h>
00031
00032 #include <PVLE/3D/Utility3D.h>
00033
00034
00035 namespace Physics {
00036
00037 Joint::Joint(World * pWorld, EJointType type) : id(0), worldId(pWorld->id) { init(type, NULL); }
00038 Joint::Joint(World * pWorld, Contact * pContact) : id(0), worldId(pWorld->id) { init(CONTACT, pContact); }
00039
00040 Joint::Joint(World * pWorld, EJointType type, dJointGroupID joinGroup) : id(0), worldId(pWorld->id) { init(type, NULL, joinGroup); }
00041 Joint::Joint(World * pWorld, Contact * pContact, dJointGroupID joinGroup) : id(0), worldId(pWorld->id) { init(CONTACT, pContact, joinGroup); }
00042
00043 Joint::Joint(const Joint & v) : id(0), worldId(v.worldId) {
00044 EJointType type = v.getType();
00045 if (type == CONTACT) THROW_TRACED_EXCEPTION("Unable to copy a contact joint");
00046 init(type, NULL);
00047 copyParameters(v);
00048 }
00049
00050 void Joint::copyParameters(const Joint & v) {
00051 DBG_TRY_BLOCK_START
00052
00053 EJointType type = getType();
00054 ASSERT(type == v.getType());
00055
00056 UCHAR nbAxes;
00057 switch(type) {
00058 case BALL:
00059 nbAxes = 0;
00060 setPos(v.getPosBody1());
00061 break;
00062 case HINGE:
00063 nbAxes = 1;
00064 setPos(v.getPosBody1());
00065 setAxis1(v.getAxis1());
00066 break;
00067 case SLIDER:
00068 nbAxes = 1;
00069 setAxis1(v.getAxis1());
00070 break;
00071 case CONTACT:
00072 nbAxes = 0; break;
00073 case UNIVERSAL:
00074 nbAxes = 2;
00075 setPos(v.getPosBody1());
00076 setAxis1(v.getAxis1());
00077 setAxis2(v.getAxis2());
00078 break;
00079 case HINGE2:
00080 nbAxes = 2;
00081 setPos(v.getPosBody1());
00082 setAxis1(v.getAxis1());
00083 setAxis2(v.getAxis2());
00084 break;
00085 case FIXED:
00086 nbAxes = 0; break;
00087 case ANGULAR_MOTOR:
00088 setAMotorMode(v.getAMotorMode());
00089 nbAxes = v.getNumAxes();
00090 setNumAxes(nbAxes);
00091 for(UCHAR i=0; i<nbAxes; ++i) {
00092 setMotorAxis(i, v.getAxisFrameAMotor(i), v.getMotorAxis(i));
00093 setAxisAngleAMotor(i, v.getAxisAngleAMotor(i));
00094 }
00095 break;
00096 case LINEAR_MOTOR:
00097 nbAxes = v.getNumAxes();
00098 setNumAxes(nbAxes);
00099
00100 THROW_TRACED_EXCEPTION("LINEAR_MOTOR joint COPY is not supported yet");
00101 break;
00102 case PLANE2D:
00103 nbAxes = 0;
00104 THROW_TRACED_EXCEPTION("PLANE2D joint is not supported yet");
00105
00106 break;
00107 case PRISMATIC_ROTOIDE:
00108 nbAxes = 2;
00109 setPos(v.getPosBody1());
00110 setAxis1(v.getAxis1());
00111 setAxis2(v.getAxis2());
00112 break;
00113 default:
00114 THROW_TRACED_EXCEPTION("Unimplemented joint type used for copy");
00115 }
00116
00117
00118 for(UCHAR i=0; i<nbAxes; ++i) {
00119 setCFM(v.getCFM(i), i);
00120 setLoStop(v.getLoStop(i), i);
00121 setHiStop(v.getHiStop(i), i);
00122 setStopBouncyness(v.getStopBouncyness(i), i);
00123 setStopERP(v.getStopERP(i), i);
00124 setStopCFM(v.getStopCFM(i), i);
00125
00126 setMotorVel(v.getMotorVel(i), i);
00127 setMotorMaxForce(v.getMotorMaxForce(i), i);
00128 setMotorFudgeFactor(v.getMotorFudgeFactor(i), i);
00129 }
00130
00131 DBG_TRY_BLOCK_END
00132 }
00133
00134 void Joint::attach(Body * pBody1, Body * pBody2) {
00135 dJointAttach(id, pBody1 ? pBody1->id : NULL, pBody2 ? pBody2->id : NULL);
00136 if (getType()==FIXED) dJointSetFixed(id);
00137 }
00138
00139 Body * Joint::getBody1() { dBodyID odeBody = dJointGetBody(id, 0); return odeBody ? static_cast<Body *>(dBodyGetData(odeBody)) : NULL; }
00140 Body * Joint::getBody2() { dBodyID odeBody = dJointGetBody(id, 1); return odeBody ? static_cast<Body *>(dBodyGetData(odeBody)) : NULL; }
00141 const Body * Joint::getBody1() const { dBodyID odeBody = dJointGetBody(id, 0); return odeBody ? static_cast<const Body *>(dBodyGetData(odeBody)) : NULL; }
00142 const Body * Joint::getBody2() const { dBodyID odeBody = dJointGetBody(id, 1); return odeBody ? static_cast<const Body *>(dBodyGetData(odeBody)) : NULL; }
00143
00144
00145 void Joint::init(EJointType type, dContact * pContact, dJointGroupID joinGroup) {
00146 switch(type) {
00147 case BALL:
00148 id = dJointCreateBall(worldId, joinGroup); break;
00149 case HINGE:
00150 id = dJointCreateHinge(worldId, joinGroup); break;
00151 case SLIDER:
00152 id = dJointCreateSlider(worldId, joinGroup); break;
00153 case CONTACT:
00154 id = dJointCreateContact(worldId, joinGroup, pContact); break;
00155 case UNIVERSAL:
00156 id = dJointCreateUniversal(worldId, joinGroup); break;
00157 case HINGE2:
00158 id = dJointCreateHinge2(worldId, joinGroup); break;
00159 case FIXED:
00160 id = dJointCreateFixed(worldId, joinGroup); break;
00161 case ANGULAR_MOTOR:
00162 id = dJointCreateAMotor(worldId, joinGroup); break;
00163 case LINEAR_MOTOR:
00164 id = dJointCreateLMotor(worldId, joinGroup); break;
00165 case PLANE2D:
00166 THROW_TRACED_EXCEPTION("PLANE2D joint is not supported yet");
00167 id = dJointCreatePlane2D(worldId, joinGroup); break;
00168 case PRISMATIC_ROTOIDE:
00169 id = dJointCreatePR(worldId, joinGroup); break;
00170 default:
00171 THROW_TRACED_EXCEPTION("Unimplemented joint type used for creation");
00172 }
00173 dJointSetData(id, this);
00174 }
00175
00176
00177
00178 void Joint::setPos(const osg::Vec3 & pos) {
00179 switch(getType()) {
00180 case BALL:
00181 dJointSetBallAnchor(id, pos.x(), pos.y(), pos.z()); return;
00182 case HINGE:
00183 dJointSetHingeAnchor(id, pos.x(), pos.y(), pos.z()); return;
00184 case UNIVERSAL:
00185 dJointSetUniversalAnchor(id, pos.x(), pos.y(), pos.z()); return;
00186 case HINGE2:
00187 dJointSetHinge2Anchor(id, pos.x(), pos.y(), pos.z()); return;
00188 case PRISMATIC_ROTOIDE:
00189 dJointSetPRAnchor(id, pos.x(), pos.y(), pos.z()); return;
00190 default:
00191 ASSERT(false && "Cannot set position for this joint type");
00192 }
00193 }
00194
00195 osg::Vec3 Joint::getPosBody1() const {
00196 dVector3 res;
00197 switch(getType()) {
00198 case BALL:
00199 dJointGetBallAnchor(id, res); break;
00200 case HINGE:
00201 dJointGetHingeAnchor(id, res); break;
00202 case UNIVERSAL:
00203 dJointGetUniversalAnchor(id, res); break;
00204 case HINGE2:
00205 dJointGetHinge2Anchor(id, res); break;
00206 case PRISMATIC_ROTOIDE:
00207 dJointGetPRAnchor(id, res); break;
00208 default:
00209 ASSERT(false && "Cannot get position (Body 1) for this joint type");
00210 }
00211 return toGraphVec3(res);
00212 }
00213
00214 osg::Vec3 Joint::getPosBody2() const {
00215 dVector3 res;
00216 switch(getType()) {
00217 case BALL:
00218 dJointGetBallAnchor2(id, res); break;
00219 case HINGE:
00220 dJointGetHingeAnchor2(id, res); break;
00221 case UNIVERSAL:
00222 dJointGetUniversalAnchor2(id, res); break;
00223 case HINGE2:
00224 dJointGetHinge2Anchor2(id, res); break;
00225 default:
00226 ASSERT(false && "Cannot get position (Body 2) for this joint type");
00227 }
00228 return toGraphVec3(res);
00229 }
00230
00231 void Joint::setAxis1(const osg::Vec3 & axis) {
00232 switch(getType()) {
00233 case HINGE:
00234 dJointSetHingeAxis(id, axis.x(), axis.y(), axis.z()); return;
00235 case SLIDER:
00236 dJointSetSliderAxis(id, axis.x(), axis.y(), axis.z()); return;
00237 case UNIVERSAL:
00238 dJointSetUniversalAxis1(id, axis.x(), axis.y(), axis.z()); return;
00239 case HINGE2:
00240 dJointSetHinge2Axis1(id, axis.x(), axis.y(), axis.z()); return;
00241 case PRISMATIC_ROTOIDE:
00242 dJointSetPRAxis1(id, axis.x(), axis.y(), axis.z()); break;
00243 default:
00244 ASSERT(false && "Cannot set axis 1 for this joint type");
00245 }
00246 }
00247
00248 osg::Vec3 Joint::getAxis1() const {
00249 dVector3 res;
00250 switch(getType()) {
00251 case HINGE:
00252 dJointGetHingeAxis(id, res); break;
00253 case SLIDER:
00254 dJointGetSliderAxis(id, res); break;
00255 case UNIVERSAL:
00256 dJointGetUniversalAxis1(id, res); break;
00257 case HINGE2:
00258 dJointGetHinge2Axis1(id, res); break;
00259 case PRISMATIC_ROTOIDE:
00260 dJointGetPRAxis1(id, res); break;
00261 default:
00262 ASSERT(false && "Cannot get axis 1 for this joint type");
00263 }
00264 return toGraphVec3(res);
00265 }
00266
00267 void Joint::setAxis2(const osg::Vec3 & axis) {
00268 switch(getType()) {
00269 case UNIVERSAL:
00270 dJointSetUniversalAxis2(id, axis.x(), axis.y(), axis.z()); return;
00271 case HINGE2:
00272 dJointSetHinge2Axis2(id, axis.x(), axis.y(), axis.z()); return;
00273 case PRISMATIC_ROTOIDE:
00274 dJointSetPRAxis2(id, axis.x(), axis.y(), axis.z()); break;
00275 default:
00276 ASSERT(false && "Cannot set axis 2 for this joint type");
00277 }
00278 }
00279
00280 osg::Vec3 Joint::getAxis2() const {
00281 dVector3 res;
00282 if (getType() == UNIVERSAL) dJointGetUniversalAxis2(id, res);
00283 else dJointGetHinge2Axis2(id, res);
00284
00285 switch(getType()) {
00286 case UNIVERSAL:
00287 dJointGetUniversalAxis2(id, res); break;
00288 case HINGE2:
00289 dJointGetHinge2Axis2(id, res); break;
00290 case PRISMATIC_ROTOIDE:
00291 dJointGetPRAxis2(id, res); break;
00292 default:
00293 ASSERT(false && "Cannot get axis 2 for this joint type");
00294 }
00295 return toGraphVec3(res);
00296 }
00297
00298 void Joint::setAMotorMode(Joint::EAMotorMode mode) {
00299 ASSERT(getType()==ANGULAR_MOTOR && "Cannot set the mode for this joint type");
00300 dJointSetAMotorMode(id, mode);
00301 }
00302
00303 Joint::EAMotorMode Joint::getAMotorMode() const {
00304 ASSERT(getType()==ANGULAR_MOTOR && "Cannot get the mode for this joint type");
00305 return static_cast<EAMotorMode>(dJointGetAMotorMode(id));
00306 }
00307
00308 void Joint::setNumAxes(UCHAR num) {
00309 ASSERT((getType()==ANGULAR_MOTOR || getType()==LINEAR_MOTOR) && "Cannot set the number of axis for this joint type");
00310 ASSERT(num>=0 && num<=2);
00311 if (getType()==ANGULAR_MOTOR) dJointSetAMotorNumAxes(id, num);
00312 else dJointSetLMotorNumAxes(id, num);
00313 }
00314
00315 UCHAR Joint::getNumAxes() const {
00316 ASSERT((getType()==ANGULAR_MOTOR || getType()==LINEAR_MOTOR) && "Cannot set the number of axis for this joint type");
00317 return (getType()==ANGULAR_MOTOR) ? dJointGetAMotorNumAxes(id) : dJointGetLMotorNumAxes(id);
00318 }
00319
00320
00321 void Joint::setMotorAxis(UCHAR num, Joint::EAnchoredTo frame, const osg::Vec3 & axis) {
00322 ASSERT((getType()==ANGULAR_MOTOR || getType()==LINEAR_MOTOR) && "Cannot set the axis for this joint type");
00323 ASSERT(num>=0 && num<=2);
00324 if (getType()==ANGULAR_MOTOR) dJointSetAMotorAxis(id, num, frame, axis.x(), axis.y(), axis.z());
00325 else dJointSetLMotorAxis(id, num, frame, axis.x(), axis.y(), axis.z());
00326 }
00327
00328 osg::Vec3 Joint::getMotorAxis(UCHAR num) const {
00329 ASSERT(num>=0 && num<=2);
00330 ASSERT((getType()==ANGULAR_MOTOR || getType()==LINEAR_MOTOR) && "Cannot get the axis for this joint type");
00331 dVector3 res;
00332 if (getType()==ANGULAR_MOTOR) dJointGetAMotorAxis(id, num, res);
00333 else dJointGetLMotorAxis(id, num, res);
00334 return toGraphVec3(res);
00335 }
00336
00337 Joint::EAnchoredTo Joint::getAxisFrameAMotor(UCHAR num) const {
00338
00339 ASSERT(getType()==ANGULAR_MOTOR && "Cannot get the frame for this joint type");
00340 ASSERT(num>=0 && num<=2);
00341 return static_cast<EAnchoredTo>(dJointGetAMotorAxisRel(id, num));
00342 }
00343
00344 void Joint::setAxisAngleAMotor(UCHAR num, dReal angle) {
00345 ASSERT(getType()==ANGULAR_MOTOR && "Cannot set the angle for this joint type");
00346 ASSERT(num>=0 && num<=2);
00347 dJointSetAMotorAngle(id, num, angle);
00348 }
00349
00350 dReal Joint::getAxisAngleAMotor(UCHAR num) const {
00351 ASSERT(getType()==ANGULAR_MOTOR && "Cannot set the angle for this joint type");
00352 ASSERT(num>=0 && num<=2);
00353 return dJointGetAMotorAngle(id, num);
00354 }
00355
00356 dReal Joint::getAxisAngleRateAMotor(UCHAR num) const {
00357 ASSERT(getType()==ANGULAR_MOTOR && "Cannot set the angle for this joint type");
00358 ASSERT(num>=0 && num<=2);
00359 return dJointGetAMotorAngleRate(id, num);
00360 }
00361
00362
00363 dReal Joint::getRelAngleAxis1() const {
00364 ASSERT((getType()==HINGE || getType()==HINGE2) && "Cannot get relative angle on axis 1 for this joint type");
00365 return (getType() == HINGE) ? dJointGetHingeAngle(id) : dJointGetHinge2Angle1(id);
00366 }
00367
00368 dReal Joint::getRelAngleRateAxis1() const {
00369 ASSERT((getType()==HINGE || getType()==HINGE2) && "Cannot get relative angle rate on axis 1 for this joint type");
00370 return (getType() == HINGE) ? dJointGetHingeAngleRate(id) : dJointGetHinge2Angle1Rate(id);
00371 }
00372
00373 dReal Joint::getRelAngleRateAxis2() const {
00374 ASSERT(getType()==HINGE2 && "Cannot get relative angle rate on axis 2 for this joint type");
00375 return dJointGetHinge2Angle2Rate(id);
00376 }
00377
00378 dReal Joint::getRelPosAxis1() const {
00379 ASSERT((getType()==SLIDER || getType()==PRISMATIC_ROTOIDE) && "Cannot get relative position on axis 1 for this joint type");
00380 if (getType()==SLIDER) return dJointGetSliderPosition(id);
00381 return dJointGetPRPosition(id);
00382 }
00383
00384 dReal Joint::getRelPosRateAxis1() const {
00385 ASSERT((getType()==SLIDER || getType()==PRISMATIC_ROTOIDE) && "Cannot get relative position rate on axis 1 for this joint type");
00386 if (getType()==SLIDER) return dJointGetSliderPosition(id);
00387 return dJointGetPRPositionRate(id);
00388 }
00389
00390 Joint::TFuncODESetParam Joint::getODESetParamFuncPt() const {
00391 switch(getType()) {
00392 case HINGE:
00393 return &dJointSetHingeParam;
00394 case SLIDER:
00395 return &dJointSetSliderParam;
00396 case UNIVERSAL:
00397 return &dJointSetUniversalParam;
00398 case HINGE2:
00399 return &dJointSetHinge2Param;
00400 case ANGULAR_MOTOR:
00401 return &dJointSetAMotorParam;
00402 case LINEAR_MOTOR:
00403 return &dJointSetLMotorParam;
00404
00405
00406 case PRISMATIC_ROTOIDE:
00407 return &dJointSetPRParam;
00408 default:
00409
00410 THROW_TRACED_EXCEPTION("Unsupported joint type for setting parameters.");
00411 }
00412 }
00413
00414 Joint::TFuncODEGetParam Joint::getODEGetParamFuncPt() const {
00415 switch(getType()) {
00416 case HINGE:
00417 return &dJointGetHingeParam;
00418 case SLIDER:
00419 return &dJointGetSliderParam;
00420 case UNIVERSAL:
00421 return &dJointGetUniversalParam;
00422 case HINGE2:
00423 return &dJointGetHinge2Param;
00424 case ANGULAR_MOTOR:
00425 return &dJointGetAMotorParam;
00426 case LINEAR_MOTOR:
00427 return &dJointGetLMotorParam;
00428
00429
00430 case PRISMATIC_ROTOIDE:
00431 return &dJointGetPRParam;
00432 default:
00433
00434 THROW_TRACED_EXCEPTION("Unsupported joint type for getting parameters.");
00435 }
00436 }
00437
00438
00439 void Joint::translate(const osg::Vec3 & vec) {
00440 if (getBody1() && getBody2()) return;
00441 EJointType type = getType();
00442 if (type==PLANE2D) THROW_TRACED_EXCEPTION("PLANE2D joint is not supported yet");
00443 if (type==BALL || type==HINGE || type==UNIVERSAL || type==HINGE2 || type==PRISMATIC_ROTOIDE) setPos(getPosBody1() + vec);
00444 }
00445
00446
00447 void Joint::rotate(const osg::Quat & quat) {
00448 if (getBody1() && getBody2()) return;
00449
00450
00451
00452 UCHAR nbAxes;
00453 EJointType type = getType();
00454 switch(type) {
00455 case HINGE:
00456 setAxis1(getAxis1() * quat);
00457 break;
00458 case SLIDER:
00459 setAxis1(getAxis1() * quat);
00460 break;
00461 case UNIVERSAL:
00462 setAxis1(getAxis1() * quat);
00463 setAxis2(getAxis2() * quat);
00464 break;
00465 case HINGE2:
00466 setAxis1(getAxis1() * quat);
00467 setAxis2(getAxis2() * quat);
00468 break;
00469 case ANGULAR_MOTOR:
00470 nbAxes = getNumAxes();
00471 for(UCHAR i=0; i<nbAxes; ++i) {
00472 setMotorAxis(i, getAxisFrameAMotor(i), getMotorAxis(i) * quat);
00473 }
00474 break;
00475 case LINEAR_MOTOR:
00476 nbAxes = getNumAxes();
00477
00478 THROW_TRACED_EXCEPTION("LINEAR_MOTOR joint ROTATE is not supported yet");
00479 break;
00480 case PLANE2D:
00481 THROW_TRACED_EXCEPTION("PLANE2D joint is not supported yet");
00482 break;
00483 case PRISMATIC_ROTOIDE:
00484 setAxis1(getAxis1() * quat);
00485 setAxis2(getAxis2() * quat);
00486 break;
00487 }
00488 }
00489
00490
00491 void Joint::rotate(const osg::Quat & quat, const osg::Vec3 & center) {
00492 osg::Matrix mat = osg::Matrix::translate(-center) * osg::Matrix::rotate(quat) * osg::Matrix::translate(center);
00493
00494
00495 rotate(mat.getRotate());
00496 }
00497
00498 void Joint::accept(Visitor & v) { v.apply(*this); }
00499
00500
00501
00502 }
