Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members

joint.cpp

Go to the documentation of this file.
00001 /* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
00002    this file is part of rcssserver3D
00003    Fri May 9 2003
00004    Copyright (C) 2003 Koblenz University
00005    $Id: joint.cpp,v 1.6 2005/12/18 18:06:18 rollmark Exp $
00006 
00007    This program is free software; you can redistribute it and/or modify
00008    it under the terms of the GNU General Public License as published by
00009    the Free Software Foundation; version 2 of the License.
00010 
00011    This program is distributed in the hope that it will be useful,
00012    but WITHOUT ANY WARRANTY; without even the implied warranty of
00013    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014    GNU General Public License for more details.
00015 
00016    You should have received a copy of the GNU General Public License
00017    along with this program; if not, write to the Free Software
00018    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
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             // we cannot use the logserver here
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             // we cannot use the logserver here
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 

Generated on Thu Apr 6 15:25:38 2006 for rcssserver3d by  doxygen 1.4.4