00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <physics/ODEHingeJoint>
00026 #include <physics/ODEConstraintGroup>
00027
00028 using physics::ODEHingeJoint;
00029 using physics::ODEConstraintGroup;
00030
00031
00032 ODEHingeJoint::ODEHingeJoint()
00033 {
00034 }
00035
00036 ODEHingeJoint::~ODEHingeJoint()
00037 {
00038 }
00039
00040 void ODEHingeJoint::onConstraintGroupAdd(ref<ConstraintGroup> g)
00041 {
00042 Assert(g != 0);
00043 Assert(instanceof(*g,ODEConstraintGroup));
00044 group = g;
00045
00046 ref<ODEConstraintGroup> ogroup = narrow_ref<ODEConstraintGroup>(group);
00047 setJointID( dJointCreateHinge(ogroup->getWorldID(), ogroup->getJointGroupID()) );
00048 }
00049
00050 void ODEHingeJoint::setAnchor(const Point3& p)
00051 {
00052 checkAddedAndAttached();
00053 Point3 gp( body1->getRelPointPos(p) );
00054 dJointSetHingeAnchor(jointID, gp.x, gp.y, gp.z);
00055 }
00056
00057 base::Point3 ODEHingeJoint::getAnchor() const
00058 {
00059 checkAddedAndAttached();
00060 dVector3 ogp;
00061 dJointGetHingeAnchor(jointID, ogp);
00062 Point3 gp(ogp[0], ogp[1], ogp[2]);
00063 return body1->getGlobalPointRelPos(gp);
00064 }
00065
00066 void ODEHingeJoint::setAxis(const Vector3& v)
00067 {
00068 checkAddedAndAttached();
00069
00070 Vector3 g(v);
00071 body1->getOrientation().rotatePoint(g);
00072 dJointSetHingeAxis(jointID, g.x, g.y, g.z);
00073 }
00074
00075 base::Vector3 ODEHingeJoint::getAxis() const
00076 {
00077 checkAddedAndAttached();
00078 dVector3 gaxis;
00079 dJointGetHingeAxis(jointID, gaxis);
00080
00081 Vector3 l(gaxis[0], gaxis[1], gaxis[2]);
00082 body1->getOrientation().invert().rotatePoint(l);
00083 return l;
00084 }
00085
00086 Real ODEHingeJoint::getAngle() const
00087 {
00088 return dJointGetHingeAngle(jointID);
00089 }
00090
00091 Real ODEHingeJoint::getAngleRate() const
00092 {
00093 return dJointGetHingeAngleRate(jointID);
00094 }
00095
00096 void ODEHingeJoint::setHighStop(Real angle)
00097 {
00098 checkAddedAndAttached();
00099 Assertm( -consts::Pi <= angle && angle <= consts::Pi, "angle within range");
00100 dJointSetHingeParam(jointID, dParamHiStop, angle);
00101 }
00102
00103 void ODEHingeJoint::setLowStop(Real angle)
00104 {
00105 checkAddedAndAttached();
00106 Assertm( -consts::Pi <= angle && angle <= consts::Pi, "angle within range");
00107 dJointSetHingeParam(jointID, dParamLoStop, angle);
00108 }
00109
00110
00111 void ODEHingeJoint::setStopRestitution(Real r)
00112 {
00113 checkAddedAndAttached();
00114 Assertm( 0 <= r && r <= 1, "restitution within range");
00115 dJointSetHingeParam(jointID, dParamBounce, r);
00116 }
00117
00118
00119 void ODEHingeJoint::setParameter(const String& name, Real value, Int dof)
00120 {
00121 Assert(dof==1);
00122 checkAddedAndAttached();
00123 if (name == "CFM")
00124 dJointSetHingeParam(jointID, dParamCFM, value);
00125 else if (name == "StopERP")
00126 dJointSetHingeParam(jointID, dParamStopERP, value);
00127 else if (name == "StopCFM")
00128 dJointSetHingeParam(jointID, dParamStopCFM, value);
00129 else if (name == "FudgeFactor")
00130 dJointSetHingeParam(jointID, dParamFudgeFactor, value);
00131 else
00132 HingeJoint::setParameter(name,value, dof);
00133 }
00134
00135
00136
00137
00138
00139
00140 bool ODEHingeJoint::hasMotor(Int dof) const
00141 {
00142 return (dof==1);
00143 }
00144
00145 void ODEHingeJoint::setMotorTargetVel(Int dof, Real vel)
00146 {
00147 Assert(dof==1);
00148 dJointSetHingeParam(jointID, dParamVel, vel);
00149 }
00150
00151 void ODEHingeJoint::setMotorMaxForce(Int dof, Real force)
00152 {
00153 Assert(dof==1);
00154 dJointSetHingeParam(jointID, dParamFMax, force);
00155 }
00156