[ODE] [PATCH] dBodyAddImpulse() etc functionality
Gary R. Van Sickle
g.r.vansickle at worldnet.att.net
Thu May 23 23:39:01 2002
This is a multi-part message in MIME format.
------=_NextPart_000_0016_01C202C3.AD7F13E0
Content-Type: text/plain;
charset="iso-8859-1"
Content-Transfer-Encoding: 7bit
The attached patch adds impulse accumulators and the following functions to ODE:
dBodyAddImpulse
dBodyAddRelImpulse
dBodyAddImpulseAtPos
dBodyAddImpulseAtRelPos
dBodyAddRelImpulseAtPos
dBodyAddRelImpulseAtRelPos
Diffed against current cvs.
--
Gary R. Van Sickle
Brewer. Patriot.
------=_NextPart_000_0016_01C202C3.AD7F13E0
Content-Type: application/octet-stream;
name="ode.diff"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="ode.diff"
Index: include/ode/objects.h=0A=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=0A=
RCS file: /cvsroot/ode/include/ode/objects.h,v=0A=
retrieving revision 1.30=0A=
diff -p -u -b -r1.30 objects.h=0A=
--- include/ode/objects.h 2002/04/05 07:52:35 1.30=0A=
+++ include/ode/objects.h 2002/05/24 06:31:17=0A=
@@ -67,15 +67,25 @@ void dBodyGetMass (dBodyID, dMass *mass)=0A=
=0A=
void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);=0A=
void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);=0A=
+void dBodyAddImpulse (dBodyID, dReal fx, dReal fy, dReal fz);=0A=
void dBodyAddRelForce (dBodyID, dReal fx, dReal fy, dReal fz);=0A=
void dBodyAddRelTorque (dBodyID, dReal fx, dReal fy, dReal fz);=0A=
+void dBodyAddRelImpulse (dBodyID, dReal fx, dReal fy, dReal fz);=0A=
void dBodyAddForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz,=0A=
dReal px, dReal py, dReal pz);=0A=
+void dBodyAddImpulseAtPos (dBodyID, dReal fx, dReal fy, dReal fz,=0A=
+ dReal px, dReal py, dReal pz);=0A=
void dBodyAddForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,=0A=
dReal px, dReal py, dReal pz);=0A=
+void dBodyAddImpulseAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,=0A=
+ dReal px, dReal py, dReal pz);=0A=
void dBodyAddRelForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz,=0A=
dReal px, dReal py, dReal pz);=0A=
+void dBodyAddRelImpulseAtPos (dBodyID, dReal fx, dReal fy, dReal fz,=0A=
+ dReal px, dReal py, dReal pz);=0A=
void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,=0A=
+ dReal px, dReal py, dReal pz);=0A=
+void dBodyAddRelImpulseAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,=0A=
dReal px, dReal py, dReal pz);=0A=
=0A=
const dReal * dBodyGetForce (dBodyID);=0A=
Index: include/ode/odecpp.h=0A=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=0A=
RCS file: /cvsroot/ode/include/ode/odecpp.h,v=0A=
retrieving revision 1.9=0A=
diff -p -u -b -r1.9 odecpp.h=0A=
--- include/ode/odecpp.h 2001/12/25 06:22:50 1.9=0A=
+++ include/ode/odecpp.h 2002/05/24 06:31:18=0A=
@@ -64,7 +64,7 @@ public:=0A=
dBody (dWorld &world)=0A=
{ _id =3D dBodyCreate (world.id()); }=0A=
~dBody()=0A=
- { dBodyDestroy (_id); }=0A=
+ { if(_id!=3D0) dBodyDestroy (_id); }=0A=
void create (dWorld &world)=0A=
{ if (_id) dBodyDestroy (_id); _id =3D dBodyCreate (world.id()); }=0A=
dBodyID id()=0A=
@@ -163,7 +163,7 @@ public:=0A=
dJoint()=0A=
{ _id =3D 0; }=0A=
~dJoint()=0A=
- { dJointDestroy (_id); }=0A=
+ { if(_id !=3D 0) dJointDestroy (_id); }=0A=
dJointID id()=0A=
{ return _id; }=0A=
=0A=
Index: ode/src/objects.h=0A=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=0A=
RCS file: /cvsroot/ode/ode/src/objects.h,v=0A=
retrieving revision 1.8=0A=
diff -p -u -b -r1.8 objects.h=0A=
--- ode/src/objects.h 2002/01/06 19:22:24 1.8=0A=
+++ ode/src/objects.h 2002/05/24 06:31:19=0A=
@@ -74,6 +74,7 @@ struct dxBody : public dObject {=0A=
dMatrix3 R; // rotation matrix, always corresponds to q=0A=
dVector3 lvel,avel; // linear and angular velocity of POR=0A=
dVector3 facc,tacc; // force and torque accululators=0A=
+ dVector3 iacc,itacc; // impulse and impulsive torque accululators=0A=
dVector3 finite_rot_axis; // finite rotation axis, unit length or =
0=3Dnone=0A=
};=0A=
=0A=
Index: ode/src/ode.cpp=0A=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=0A=
RCS file: /cvsroot/ode/ode/src/ode.cpp,v=0A=
retrieving revision 1.29=0A=
diff -p -u -b -r1.29 ode.cpp=0A=
--- ode/src/ode.cpp 2002/04/05 07:52:36 1.29=0A=
+++ ode/src/ode.cpp 2002/05/24 06:31:20=0A=
@@ -347,6 +347,8 @@ dxBody *dBodyCreate (dxWorld *w)=0A=
dSetZero (b->avel,4);=0A=
dSetZero (b->facc,4);=0A=
dSetZero (b->tacc,4);=0A=
+ dSetZero (b->iacc,4);=0A=
+ dSetZero (b->itacc,4);=0A=
dSetZero (b->finite_rot_axis,4);=0A=
addObjectToList (b,(dObject **) &w->firstbody);=0A=
w->nb++;=0A=
@@ -514,6 +516,15 @@ void dBodyAddTorque (dBodyID b, dReal fx=0A=
}=0A=
=0A=
=0A=
+void dBodyAddImpulse (dBodyID b, dReal fx, dReal fy, dReal fz)=0A=
+{=0A=
+ dAASSERT (b);=0A=
+ b->iacc[0] +=3D fx;=0A=
+ b->iacc[1] +=3D fy;=0A=
+ b->iacc[2] +=3D fz;=0A=
+}=0A=
+=0A=
+=0A=
void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)=0A=
{=0A=
dAASSERT (b);=0A=
@@ -544,6 +555,21 @@ void dBodyAddRelTorque (dBodyID b, dReal=0A=
}=0A=
=0A=
=0A=
+void dBodyAddRelImpulse (dBodyID b, dReal fx, dReal fy, dReal fz)=0A=
+{=0A=
+ dAASSERT (b);=0A=
+ dVector3 t1,t2;=0A=
+ t1[0] =3D fx;=0A=
+ t1[1] =3D fy;=0A=
+ t1[2] =3D fz;=0A=
+ t1[3] =3D 0;=0A=
+ dMULTIPLY0_331 (t2,b->R,t1);=0A=
+ b->iacc[0] +=3D t2[0];=0A=
+ b->iacc[1] +=3D t2[1];=0A=
+ b->iacc[2] +=3D t2[2];=0A=
+}=0A=
+=0A=
+=0A=
void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,=0A=
dReal px, dReal py, dReal pz)=0A=
{=0A=
@@ -562,6 +588,24 @@ void dBodyAddForceAtPos (dBodyID b, dRea=0A=
}=0A=
=0A=
=0A=
+void dBodyAddImpulseAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,=0A=
+ dReal px, dReal py, dReal pz)=0A=
+{=0A=
+ dAASSERT (b);=0A=
+ b->iacc[0] +=3D fx;=0A=
+ b->iacc[1] +=3D fy;=0A=
+ b->iacc[2] +=3D fz;=0A=
+ dVector3 f,q;=0A=
+ f[0] =3D fx;=0A=
+ f[1] =3D fy;=0A=
+ f[2] =3D fz;=0A=
+ q[0] =3D px - b->pos[0];=0A=
+ q[1] =3D py - b->pos[1];=0A=
+ q[2] =3D pz - b->pos[2];=0A=
+ dCROSS (b->itacc,+=3D,q,f);=0A=
+}=0A=
+=0A=
+=0A=
void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,=0A=
dReal px, dReal py, dReal pz)=0A=
{=0A=
@@ -583,6 +627,27 @@ void dBodyAddForceAtRelPos (dBodyID b, d=0A=
}=0A=
=0A=
=0A=
+void dBodyAddImpulseAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,=0A=
+ dReal px, dReal py, dReal pz)=0A=
+{=0A=
+ dAASSERT (b);=0A=
+ dVector3 prel,f,p;=0A=
+ f[0] =3D fx;=0A=
+ f[1] =3D fy;=0A=
+ f[2] =3D fz;=0A=
+ f[3] =3D 0;=0A=
+ prel[0] =3D px;=0A=
+ prel[1] =3D py;=0A=
+ prel[2] =3D pz;=0A=
+ prel[3] =3D 0;=0A=
+ dMULTIPLY0_331 (p,b->R,prel);=0A=
+ b->iacc[0] +=3D f[0];=0A=
+ b->iacc[1] +=3D f[1];=0A=
+ b->iacc[2] +=3D f[2];=0A=
+ dCROSS (b->itacc,+=3D,p,f);=0A=
+}=0A=
+=0A=
+=0A=
void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,=0A=
dReal px, dReal py, dReal pz)=0A=
{=0A=
@@ -604,6 +669,27 @@ void dBodyAddRelForceAtPos (dBodyID b, d=0A=
}=0A=
=0A=
=0A=
+void dBodyAddRelImpulseAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,=0A=
+ dReal px, dReal py, dReal pz)=0A=
+{=0A=
+ dAASSERT (b);=0A=
+ dVector3 frel,f;=0A=
+ frel[0] =3D fx;=0A=
+ frel[1] =3D fy;=0A=
+ frel[2] =3D fz;=0A=
+ frel[3] =3D 0;=0A=
+ dMULTIPLY0_331 (f,b->R,frel);=0A=
+ b->iacc[0] +=3D f[0];=0A=
+ b->iacc[1] +=3D f[1];=0A=
+ b->iacc[2] +=3D f[2];=0A=
+ dVector3 q;=0A=
+ q[0] =3D px - b->pos[0];=0A=
+ q[1] =3D py - b->pos[1];=0A=
+ q[2] =3D pz - b->pos[2];=0A=
+ dCROSS (b->itacc,+=3D,q,f);=0A=
+}=0A=
+=0A=
+=0A=
void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,=0A=
dReal px, dReal py, dReal pz)=0A=
{=0A=
@@ -623,6 +709,28 @@ void dBodyAddRelForceAtRelPos (dBodyID b=0A=
b->facc[1] +=3D f[1];=0A=
b->facc[2] +=3D f[2];=0A=
dCROSS (b->tacc,+=3D,p,f);=0A=
+}=0A=
+=0A=
+=0A=
+void dBodyAddRelImpulseAtRelPos (dBodyID b, dReal fx, dReal fy, dReal =
fz,=0A=
+ dReal px, dReal py, dReal pz)=0A=
+{=0A=
+ dAASSERT (b);=0A=
+ dVector3 frel,prel,f,p;=0A=
+ frel[0] =3D fx;=0A=
+ frel[1] =3D fy;=0A=
+ frel[2] =3D fz;=0A=
+ frel[3] =3D 0;=0A=
+ prel[0] =3D px;=0A=
+ prel[1] =3D py;=0A=
+ prel[2] =3D pz;=0A=
+ prel[3] =3D 0;=0A=
+ dMULTIPLY0_331 (f,b->R,frel);=0A=
+ dMULTIPLY0_331 (p,b->R,prel);=0A=
+ b->iacc[0] +=3D f[0];=0A=
+ b->iacc[1] +=3D f[1];=0A=
+ b->iacc[2] +=3D f[2];=0A=
+ dCROSS (b->itacc,+=3D,p,f);=0A=
}=0A=
=0A=
=0A=
Index: ode/src/step.cpp=0A=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=
=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=0A=
RCS file: /cvsroot/ode/ode/src/step.cpp,v=0A=
retrieving revision 1.23=0A=
diff -p -u -b -r1.23 step.cpp=0A=
--- ode/src/step.cpp 2002/01/06 19:22:24 1.23=0A=
+++ ode/src/step.cpp 2002/05/24 06:31:22=0A=
@@ -262,6 +262,30 @@ static inline void moveAndRotateBody (dx=0A=
dQtoR (b->q,b->R);=0A=
}=0A=
=0A=
+// Adds the impulse accumulators to the force and torque accumulators =
once=0A=
+// we know the stepsize.=0A=
+static ConvertImpulsesToForces(dxBody * const *body, int nb, dReal =
stepsize)=0A=
+{ =0A=
+ int i;=0A=
+ for (i=3D0; i<nb; i++)=0A=
+ {=0A=
+ body[i]->facc[0] +=3D body[i]->iacc[0]/stepsize;=0A=
+ body[i]->facc[1] +=3D body[i]->iacc[1]/stepsize;=0A=
+ body[i]->facc[2] +=3D body[i]->iacc[2]/stepsize;=0A=
+ body[i]->tacc[0] +=3D body[i]->itacc[0]/stepsize;=0A=
+ body[i]->tacc[1] +=3D body[i]->itacc[1]/stepsize;=0A=
+ body[i]->tacc[2] +=3D body[i]->itacc[2]/stepsize;=0A=
+ =0A=
+ // Zero out the impulse accumulators.=0A=
+ body[i]->iacc[0] =3D 0;=0A=
+ body[i]->iacc[1] =3D 0;=0A=
+ body[i]->iacc[2] =3D 0;=0A=
+ body[i]->itacc[0] =3D 0;=0A=
+ body[i]->itacc[1] =3D 0;=0A=
+ body[i]->itacc[2] =3D 0;=0A=
+ }=0A=
+}=0A=
+=0A=
=
//***********************************************************************=
*****=0A=
// the slow, but sure way=0A=
=0A=
@@ -284,6 +308,9 @@ void dInternalStepIsland_x1 (dxWorld *wo=0A=
// number all bodies in the body list - set their tag values=0A=
for (i=3D0; i<nb; i++) body[i]->tag =3D i;=0A=
=0A=
+ // Convert the accumulated impulses to forces and torques for each =
body.=0A=
+ ConvertImpulsesToForces(body, nb, stepsize);=0A=
+ =0A=
// make a local copy of the joint array, because we might want to =
modify it.=0A=
// (the "dxJoint *const*" declaration says we're allowed to modify =
the joints=0A=
// but not the joint array, because the caller might need it =
unchanged).=0A=
@@ -595,6 +622,9 @@ void dInternalStepIsland_x2 (dxWorld *wo=0A=
// number all bodies in the body list - set their tag values=0A=
for (i=3D0; i<nb; i++) body[i]->tag =3D i;=0A=
=0A=
+ // Convert the accumulated impulses to forces and torques for each =
body.=0A=
+ ConvertImpulsesToForces(body, nb, stepsize);=0A=
+ =0A=
// make a local copy of the joint array, because we might want to =
modify it.=0A=
// (the "dxJoint *const*" declaration says we're allowed to modify =
the joints=0A=
// but not the joint array, because the caller might need it =
unchanged).=0A=
------=_NextPart_000_0016_01C202C3.AD7F13E0--