OpenSim: physics/ODEFixedConstraint.cpp Source File

00001 /****************************************************************************
00002   Copyright (C)2002 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: ODEFixedConstraint.cpp 1031 2004-02-11 20:46:36Z jungd $
00019   $Revision: 1.5 $
00020   $Date: 2004-02-11 15:46:36 -0500 (Wed, 11 Feb 2004) $
00021   $Author: jungd $
00022  
00023 ****************************************************************************/
00024 
00025 #include <physics/ODEFixedConstraint>
00026 #include <physics/ODEConstraintGroup>
00027 #include <physics/ODESolid>
00028 
00029 using physics::ODEFixedConstraint;
00030 using physics::FixedConstraint;
00031 using physics::Body;
00032 using physics::ODESolid;
00033 
00034 
00035 ODEFixedConstraint::ODEFixedConstraint()
00036 {
00037 }
00038 
00039 ODEFixedConstraint::~ODEFixedConstraint()
00040 {
00041 }
00042 
00043 
00044 void ODEFixedConstraint::attach(ref<Body> body1, ref<Body> body2)
00045 {
00046   Assert(body1 != 0);
00047   Assert(body2 != 0);
00048   Assertm(body1 != body2, "body1 is not body2");
00049   
00050   if (jointID == 0)
00051     throw std::runtime_error(Exception("Constraint must be added to a ConstraintGroup before being attached to bodies"));
00052   
00053   this->body1 = body1;
00054   this->body2 = body2;
00055 
00056   ref<ODESolid> solid1 = narrow_ref<ODESolid>(body1);
00057   ref<ODESolid> solid2 = narrow_ref<ODESolid>(body2);
00058 
00059   dJointAttach(jointID, solid1->getBodyID(), solid2->getBodyID());
00060   dJointSetHingeAnchor(jointID, 0,0,0);
00061   dJointSetHingeAxis(jointID, 0,0,1 );
00062   dJointSetHingeParam(jointID, dParamLoStop, -consts::epsilon);
00063   dJointSetHingeParam(jointID, dParamHiStop, consts::epsilon);
00064   dJointSetHingeParam(jointID, dParamBounce, 0);
00065 }
00066 
00067 ref<Body> ODEFixedConstraint::getBody(Int index)
00068 {
00069   Assert(index < 2);
00070   return (index==0)?body1:body2;
00071 }
00072 
00073 ref<const Body> ODEFixedConstraint::getBody(Int index) const
00074 {
00075   Assert(index < 2);
00076   return (index==0)?body1:body2;
00077 }
00078 
00079 
00080 void ODEFixedConstraint::onConstraintGroupAdd(ref<ConstraintGroup> g)
00081 {
00082   Assert(g != 0);
00083   Assert(instanceof(*g,ODEConstraintGroup));
00084   group = g;
00085 
00086   ref<ODEConstraintGroup> ogroup = narrow_ref<ODEConstraintGroup>(group);
00087   Assert(ogroup->getWorldID());
00088 
00089   // the ODE fixed joint type isn't implemented to maintain a fixed orientation
00090   //  between the two bodies yet.  So, we hack a fixed joint by using a Hinge joint
00091   //  and setting its lo-hi stops to the same angle
00092   setJointID ( dJointCreateHinge(ogroup->getWorldID(), ogroup->getJointGroupID()) );
00093 }
00094