00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include <zeitgeist/logserver/logserver.h>
00021 #include "joint.h"
00022 #include "body.h"
00023
00024 using namespace oxygen;
00025 using namespace boost;
00026 using namespace std;
00027 using namespace salt;
00028
00029 Joint::Joint() : ODEObject(), mODEJoint(0)
00030 {
00031 }
00032
00033 Joint::~Joint()
00034 {
00035 EnableFeedback(false);
00036 if (mODEJoint)
00037 {
00038 dJointDestroy(mODEJoint);
00039 mODEJoint = 0;
00040 }
00041 }
00042
00043 void Joint::OnLink()
00044 {
00045 if (mODEJoint == 0)
00046 {
00047 return;
00048 }
00049
00050 dJointSetData(mODEJoint, this);
00051 }
00052
00053 shared_ptr<Joint> Joint::GetJoint(dJointID id)
00054 {
00055 if (id == 0)
00056 {
00057 return shared_ptr<Joint>();
00058 }
00059
00060 Joint* jointPtr =
00061 static_cast<Joint*>(dJointGetData(id));
00062
00063 if (jointPtr == 0)
00064 {
00065
00066 cerr << "ERROR: (Joint) no joint found for dJointID "
00067 << id << "\n";
00068 return shared_ptr<Joint>();
00069 }
00070
00071 shared_ptr<Joint> joint = shared_static_cast<Joint>
00072 (make_shared(jointPtr->GetSelf()));
00073
00074 if (joint.get() == 0)
00075 {
00076
00077 cerr << "ERROR: (Joint) got no shared_ptr for dJointID "
00078 << id << "\n";
00079 }
00080
00081 return joint;
00082 }
00083
00084 void Joint::Attach(shared_ptr<Body> body1, shared_ptr<Body> body2)
00085 {
00086 if (mODEJoint == 0)
00087 {
00088 GetLog()->Error()
00089 << "(Joint) ERROR: Attach called with uninitialized ODE joint\n";
00090 return;
00091 }
00092
00093 string path1,path2;
00094 dBodyID id1,id2;
00095
00096 static const char strStaticEnv[] = "<static environment>";
00097
00098 if (body1.get() == 0)
00099 {
00100 id1 = 0;
00101 path1 = strStaticEnv;
00102 } else
00103 {
00104 id1 = body1->GetODEBody();
00105 path1 = body1->GetFullPath();
00106 }
00107
00108 if (body2.get() == 0)
00109 {
00110 id2 = 0;
00111 path2 = strStaticEnv;
00112 } else
00113 {
00114 id2 = body2->GetODEBody();
00115 path2 = body2->GetFullPath();
00116 }
00117
00118 GetLog()->Debug() << "(Joint) Attaching '" << path1 << "' to '"
00119 << path2 << '\n';
00120
00121 dJointAttach(mODEJoint, id1, id2);
00122 }
00123
00124 shared_ptr<Body> Joint::GetBody(const std::string& path)
00125 {
00126 if (path.empty())
00127 {
00128 return shared_ptr<Body>();
00129 }
00130
00131 shared_ptr<Leaf> mySelf = shared_static_cast<Leaf>
00132 (make_shared(GetSelf()));
00133
00134 shared_ptr<Leaf> leaf = GetCore()->Get(path,mySelf);
00135
00136 if (leaf.get() == 0)
00137 {
00138 GetLog()->Error()
00139 << "(Joint) ERROR: cannot find node '"
00140 << path << "'\n";
00141 return shared_ptr<Body>();
00142 }
00143
00144 shared_ptr<Body> body = shared_dynamic_cast<Body>(leaf);
00145
00146 if (body.get() == 0)
00147 {
00148 GetLog()->Error()
00149 << "(Joint) ERROR: node '"
00150 << path << "' is not a Body node \n";
00151 }
00152
00153 return body;
00154 }
00155
00156 void Joint::Attach(const std::string& path1, const std::string& path2)
00157 {
00158 shared_ptr<Body> body1 = GetBody(path1);
00159 shared_ptr<Body> body2 = GetBody(path2);
00160
00161 Attach(body1,body2);
00162 }
00163
00164 int Joint::GetType()
00165 {
00166 return dJointGetType(mODEJoint);
00167 }
00168
00169 boost::shared_ptr<Body> Joint::GetBody(EBodyIndex idx)
00170 {
00171 return Body::GetBody(dJointGetBody(mODEJoint, idx));
00172 }
00173
00174 bool Joint::AreConnected (shared_ptr<Body> body1, shared_ptr<Body> body2)
00175 {
00176 if (
00177 (body1.get() == 0) ||
00178 (body2.get() == 0)
00179 )
00180 {
00181 return false;
00182 }
00183
00184 return dAreConnected(body1->GetODEBody(),body2->GetODEBody());
00185 }
00186
00187 bool Joint::AreConnectedExcluding (shared_ptr<Body> body1,
00188 shared_ptr<Body> body2,
00189 int joint_type)
00190 {
00191 if (
00192 (body1.get() == 0) ||
00193 (body2.get() == 0)
00194 )
00195 {
00196 return false;
00197 }
00198
00199 return dAreConnectedExcluding(body1->GetODEBody(),body2->GetODEBody(),
00200 joint_type);
00201 }
00202
00203 void Joint::EnableFeedback(bool enable)
00204 {
00205 if (enable)
00206 {
00207 if (mFeedback.get() == 0)
00208 {
00209 mFeedback = shared_ptr<dJointFeedback>(new dJointFeedback());
00210 memset(mFeedback.get(),0,sizeof(dJointFeedback));
00211 }
00212 } else
00213 {
00214 if (mFeedback.get() != 0)
00215 {
00216 mFeedback.reset();
00217 }
00218 }
00219
00220 dJointSetFeedback(mODEJoint,mFeedback.get());
00221 }
00222
00223 bool Joint::FeedBackEnabled()
00224 {
00225 return (dJointGetFeedback(mODEJoint) != 0);
00226 }
00227
00228 Vector3f Joint::GetFeedbackForce(EBodyIndex idx)
00229 {
00230 dJointFeedback* fb = mFeedback.get();
00231 if (fb == 0)
00232 {
00233 return Vector3f(0,0,0);
00234 }
00235
00236 switch (idx)
00237 {
00238 case BI_FIRST :
00239 return Vector3f(
00240 fb->f1[0],
00241 fb->f1[1],
00242 fb->f1[2]
00243 );
00244
00245 case BI_SECOND :
00246 return Vector3f(
00247 fb->f2[0],
00248 fb->f2[1],
00249 fb->f2[2]
00250 );
00251
00252 default:
00253 return Vector3f(0,0,0);
00254 }
00255 }
00256
00257 Vector3f Joint::GetFeedbackTorque(EBodyIndex idx)
00258 {
00259 dJointFeedback* fb = mFeedback.get();
00260 if (fb == 0)
00261 {
00262 return Vector3f(0,0,0);
00263 }
00264
00265 switch (idx)
00266 {
00267 case BI_FIRST :
00268 return Vector3f(
00269 fb->t1[0],
00270 fb->t1[1],
00271 fb->t1[2]
00272 );
00273
00274 case BI_SECOND :
00275 return Vector3f(
00276 fb->t2[0],
00277 fb->t2[1],
00278 fb->t2[2]
00279 );
00280
00281 default:
00282 return Vector3f(0,0,0);
00283 }
00284 }
00285
00286 void Joint::SetBounce(EAxisIndex idx, float bounce)
00287 {
00288 SetParameter(dParamBounce + (idx * dParamGroup),bounce);
00289 }
00290
00291 float Joint::GetBounce(EAxisIndex idx)
00292 {
00293 return GetParameter(dParamBounce + (idx * dParamGroup));
00294 }
00295
00296 void Joint::SetLowStopPos(EAxisIndex idx, float pos)
00297 {
00298 SetParameter(dParamLoStop + (idx * dParamGroup), pos);
00299 }
00300
00301 float Joint::GetLowStopPos(EAxisIndex idx)
00302 {
00303 return GetParameter(dParamLoStop + (idx * dParamGroup));
00304 }
00305
00306 void Joint::SetHighStopPos(EAxisIndex idx, float pos)
00307 {
00308 SetParameter(dParamHiStop + (idx * dParamGroup), pos);
00309 }
00310
00311 float Joint::GetHighStopPos(EAxisIndex idx)
00312 {
00313 return GetParameter(dParamHiStop + (idx * dParamGroup));
00314 }
00315
00316 void Joint::SetLowStopDeg(EAxisIndex idx, float deg)
00317 {
00318 SetParameter(dParamLoStop + (idx * dParamGroup), gDegToRad(deg));
00319 }
00320
00321 float Joint::GetLowStopDeg(EAxisIndex idx)
00322 {
00323 return gRadToDeg(GetParameter(dParamLoStop + (idx * dParamGroup)));
00324 }
00325
00326 void Joint::SetHighStopDeg(EAxisIndex idx, float deg)
00327 {
00328 SetParameter(dParamHiStop + (idx * dParamGroup), gDegToRad(deg));
00329 }
00330
00331 float Joint::GetHighStopDeg(EAxisIndex idx)
00332 {
00333 return gRadToDeg(GetParameter(dParamHiStop + (idx * dParamGroup)));
00334 }
00335
00336 void Joint::SetCFM(EAxisIndex idx, float cfm)
00337 {
00338 SetParameter(dParamCFM + (idx * dParamGroup), cfm);
00339 }
00340
00341 float Joint::GetCFM(EAxisIndex idx)
00342 {
00343 return GetParameter(dParamCFM + (idx * dParamGroup));
00344 }
00345
00346 void Joint::SetStopCFM(EAxisIndex idx, float cfm)
00347 {
00348 SetParameter(dParamStopCFM + (idx * dParamGroup), cfm);
00349 }
00350
00351 float Joint::GetStopCFM(EAxisIndex idx)
00352 {
00353 return GetParameter(dParamStopCFM + (idx * dParamGroup));
00354 }
00355
00356 void Joint::SetStopERP(EAxisIndex idx, float erp)
00357 {
00358 SetParameter(dParamStopERP + (idx * dParamGroup), erp);
00359 }
00360
00361 float Joint::GetStopERP(EAxisIndex idx)
00362 {
00363 return GetParameter(dParamStopERP + (idx * dParamGroup));
00364 }
00365
00366 void Joint::SetSuspensionERP(EAxisIndex idx, float erp)
00367 {
00368 SetParameter(dParamSuspensionERP + (idx * dParamGroup), erp);
00369 }
00370
00371 float Joint::GetSuspensionERP(EAxisIndex idx)
00372 {
00373 return GetParameter(dParamSuspensionERP + (idx * dParamGroup));
00374 }
00375
00376 void Joint::SetSuspensionCFM(EAxisIndex idx, float cfm)
00377 {
00378 SetParameter(dParamSuspensionCFM + (idx * dParamGroup), cfm);
00379 }
00380
00381 float Joint::GetSuspensionCFM(EAxisIndex idx)
00382 {
00383 return GetParameter(dParamSuspensionCFM + (idx * dParamGroup));
00384 }
00385
00386 void Joint::SetLinearMotorVelocity(EAxisIndex idx, float vel)
00387 {
00388 SetParameter(dParamVel + (idx * dParamGroup), vel);
00389 }
00390
00391 float Joint::GetLinearMotorVelocity(EAxisIndex idx)
00392 {
00393 return GetParameter(dParamVel + (idx * dParamGroup));
00394 }
00395
00396 void Joint::SetAngularMotorVelocity(EAxisIndex idx, float deg)
00397 {
00398 SetParameter(dParamVel + (idx * dParamGroup), gDegToRad(deg));
00399 }
00400
00401 float Joint::GetAngularMotorVelocity(EAxisIndex idx)
00402 {
00403 return gRadToDeg(GetParameter(dParamVel + (idx * dParamGroup)));
00404 }
00405
00406 void Joint::SetMaxMotorForce(EAxisIndex idx, float f)
00407 {
00408 SetParameter(dParamFMax + (idx * dParamGroup), f);
00409 }
00410
00411 float Joint::GetMaxMotorForce(EAxisIndex idx)
00412 {
00413 return GetParameter(dParamFMax + (idx * dParamGroup));
00414 }
00415
00416
00417
00418
00419