OpenSim: physics/ODEJoint.cpp Source File

00001 /****************************************************************************
00002   Copyright (C)1996 David Jung <opensim@pobox.com>
00003 
00004   This program/file is free software; you can redistribute it and/or modify
00005   it under the terms of the GNU General Public License as published by
00006   the Free Software Foundation; either version 2 of the License, or
00007   (at your option) any later version.
00008   
00009   This program is distributed in the hope that it will be useful,
00010   but WITHOUT ANY WARRANTY; without even the implied warranty of
00011   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012   GNU General Public License for more details. (http://www.gnu.org)
00013   
00014   You should have received a copy of the GNU General Public License
00015   along with this program; if not, write to the Free Software
00016   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017   
00018   $Id: ODEJoint.cpp 1031 2004-02-11 20:46:36Z jungd $
00019   $Revision: 1.4 $
00020   $Date: 2004-02-11 15:46:36 -0500 (Wed, 11 Feb 2004) $
00021   $Author: jungd $
00022  
00023 ****************************************************************************/
00024 
00025 #include <physics/ODEJoint>
00026 #include <physics/ODEMotor>
00027 #include <physics/ODEConstraintGroup>
00028 #include <physics/ODESolid>
00029 
00030 using physics::ODEJoint;
00031 using physics::ODEMotor;
00032 using physics::Joint;
00033 using physics::Motor;
00034 using physics::Body;
00035 using physics::ODESolid;
00036 
00037 
00038 ODEJoint::ODEJoint()
00039 {
00040 }
00041 
00042 ODEJoint::~ODEJoint()
00043 {
00044 }
00045 
00046 
00047 void ODEJoint::attach(ref<Body> body1, ref<Body> body2)
00048 {
00049   Assert(body1 != 0);
00050   Assert(body2 != 0);
00051   Assertm(body1 != body2, "body1 is not body2");
00052 
00053   if (jointID == 0)
00054     throw std::runtime_error(Exception("Constraint must be added to a ConstraintGroup before being attached to bodies"));
00055   
00056   this->body1 = body1;
00057   this->body2 = body2;
00058 
00059   ref<ODESolid> solid1 = narrow_ref<ODESolid>(body1);
00060   ref<ODESolid> solid2 = narrow_ref<ODESolid>(body2);
00061 
00062   dJointAttach(jointID, solid1->bodyID, solid2->bodyID);
00063 }
00064 
00065 ref<Body> ODEJoint::getBody(Int index)
00066 {
00067   Assert(index < 2);
00068   return (index==0)?body1:body2;
00069 }
00070 
00071 ref<const Body> ODEJoint::getBody(Int index) const
00072 {
00073   Assert(index < 2);
00074   return (index==0)?body1:body2;
00075 }
00076 
00077 
00078 void ODEJoint::attachMotor(Int dof, ref<Motor> motor)
00079 {
00080   if (!hasMotor(dof))
00081     throw std::invalid_argument(Exception("Invalid motor dof attachment for this Joint"));
00082 
00083   if (motor) {
00084     
00085     Assertm( instanceof(*motor, ODEMotor), "Motor created by same SolidSystem as Joint");
00086     
00087     ref<ODEMotor> omotor = narrow_ref<ODEMotor>(motor);
00088     motors[dof] = omotor;
00089     if (motor != 0) {
00090       ref<Joint> self(this);
00091       omotor->setJoint(self, dof);
00092     }
00093   } else { // remove
00094     motors[dof]->setJoint(ref<Joint>(0),dof);
00095   }
00096 
00097 }
00098 
00099 
00100 ref<Motor> ODEJoint::getMotor(Int dof)
00101 {
00102   if (!hasMotor(dof))
00103     throw std::invalid_argument(Exception("Invalid motor dof attachment for this Joint"));
00104 
00105   return motors[dof];
00106 }
00107 
00108 ref<const Motor> ODEJoint::getMotor(Int dof) const
00109 {
00110   if (!hasMotor(dof))
00111     throw std::invalid_argument(Exception("Invalid motor dof attachment for this Joint"));
00112 
00113   return motors[dof];
00114 }
00115 
00116 
00117