OpenSim: physics/ODEContactConstraint.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: ODEContactConstraint.cpp 1031 2004-02-11 20:46:36Z jungd $
00019   $Revision: 1.1 $
00020   $Date: 2004-02-11 15:46:36 -0500 (Wed, 11 Feb 2004) $
00021   $Author: jungd $
00022  
00023 ****************************************************************************/
00024 
00025 #include <physics/ODEContactConstraint>
00026 #include <physics/ODEConstraintGroup>
00027 #include <physics/ODESolid>
00028 
00029 using physics::ODEContactConstraint;
00030 using physics::ContactConstraint;
00031 using physics::Body;
00032 using physics::ODESolid;
00033 
00034 
00035 ODEContactConstraint::ODEContactConstraint()
00036  : addedToGroup(false)
00037 {
00038 }
00039 
00040 ODEContactConstraint::~ODEContactConstraint()
00041 {
00042 }
00043 
00044 
00045 void ODEContactConstraint::attach(ref<Body> body1, ref<Body> body2)
00046 {
00047   Assert(body1 != 0);
00048   Assert(body2 != 0);
00049   Assertm(body1 != body2, "body1 is not body2");
00050   
00051   if (!addedToGroup)
00052     throw std::runtime_error(Exception("Constraint must be added to a ConstraintGroup before being attached to bodies"));
00053   
00054   ref<ODEConstraintGroup> ogroup = narrow_ref<ODEConstraintGroup>(group);
00055   Assert(ogroup->getWorldID());
00056 
00057   this->body1 = body1;
00058   this->body2 = body2;
00059 
00060   // create an ODE contact joint
00061   base::clearMemory(&oc,1);
00062   oc.surface.mode = dContactApprox1 | dContactSoftERP | dContactSoftCFM | dContactBounce;
00063   //dContactSlip1 | dContactSlip2 ;
00064   //| dContactSoftERP | dContactSoftCFM; | dContactBounce;
00065   oc.surface.mu = 8; //!!! arbitrary - should be calc'd from the two materials
00066   //oc.surface.slip1 = 0.1;
00067   //oc.surface.slip2 = 0.1;
00068   oc.surface.soft_erp = 0.5;
00069   oc.surface.soft_cfm = 1e-5;
00070   oc.surface.bounce = 0.3;
00071   //oc.surface.bounce_vel = 0.1;
00072   
00073   // transform contact point and normal to global coordinates
00074   Point3 gpos(body1->getOrientation().rotate(position) + body1->getPosition() );
00075   oc.geom.pos[0] = gpos.x;
00076   oc.geom.pos[1] = gpos.y;
00077   oc.geom.pos[2] = gpos.z;
00078 
00079   Vector3 gnormal(body1->getOrientation().rotate(normal));
00080   oc.geom.normal[0] = gnormal.x;
00081   oc.geom.normal[1] = gnormal.y;
00082   oc.geom.normal[2] = gnormal.z;
00083 //!!!Debugln(Tmp,"gpos=" << gpos << " normal=" << gnormal);
00084 
00085   oc.geom.depth = depth;
00086 
00087   dJointID cj = dJointCreateContact(ogroup->getWorldID(), ogroup->getJointGroupID(), &oc);
00088   
00089   setJointID(cj);
00090 
00091   ref<ODESolid> solid1 = narrow_ref<ODESolid>(body1);
00092   ref<ODESolid> solid2 = narrow_ref<ODESolid>(body2);
00093 
00094   dJointAttach(jointID, solid1->getBodyID(), solid2->getBodyID());
00095 
00096 }
00097 
00098 
00099 ref<Body> ODEContactConstraint::getBody(Int index)
00100 {
00101   Assert(index < 2);
00102   return (index==0)?body1:body2;
00103 }
00104 
00105 ref<const Body> ODEContactConstraint::getBody(Int index) const
00106 {
00107   Assert(index < 2);
00108   return (index==0)?body1:body2;
00109 }
00110 
00111 
00112 void ODEContactConstraint::onConstraintGroupAdd(ref<ConstraintGroup> g)
00113 {
00114   Assert(g != 0);
00115   Assert(instanceof(*g,ODEConstraintGroup));
00116   group = g;
00117   
00118   if (Math::equals(normal.norm(),0) && Math::equals(position.norm(),0))
00119     throw std::runtime_error(Exception("must set ContactConstraint position,normal & depth before adding it to a ConstraintGroup"));
00120     
00121   addedToGroup = true;
00122     
00123   // joint will be created on attach()  
00124 }
00125 
00126 
00127 void ODEContactConstraint::setContactPosition(const Point3& pos)
00128 {
00129   Assertm(group==0, "contact position set before ContactConstraint added to ConstraintGroup");
00130   position = pos;
00131 }
00132 
00133 
00134 void ODEContactConstraint::setContactNormal(const Vector3& normal)
00135 {
00136   Assertm(group==0, "contact normal set before ContactConstraint added to ConstraintGroup");
00137   this->normal = normal;
00138 }
00139 
00140 
00141 void ODEContactConstraint::setContactDepth(Real depth)
00142 {
00143   Assertm(group==0, "contact depth set before ContactConstraint added to ConstraintGroup");
00144   this->depth = depth;
00145 }
00146 
00147