[ODE] Joint forces
Erwin de Vries
erwin at vo.com
Tue Aug 27 12:17:02 2002
This is a multi-part message in MIME format.
------=_NextPart_000_0091_01C24E0E.F03D40D0
Content-Type: text/plain;
charset="iso-8859-1"
Content-Transfer-Encoding: 7bit
Attached are the patches for the src and include dirs.
Have fun with them.
----- Original Message -----
From: "Erwin de Vries" <erwin@vo.com>
To: <ode@q12.org>
Sent: Tuesday, August 27, 2002 10:37
Subject: Re: [ODE] Joint forces
> I dont think i posted the actual code for this. I will do that when i get
> home tonight (CET).
>
> Erwin
>
> ----- Original Message -----
> From: "Nate W" <coding@natew.com>
> To: <ode@q12.org>
> Sent: Tuesday, August 27, 2002 1:44 AM
> Subject: RE: [ODE] Joint forces
>
>
> > On Mon, 26 Aug 2002, Gary R. Van Sickle wrote:
> >
> > > typedef void dJointCallback (dJointID j, dReal stepsize, dVector3 f1,
> > > dVector3 t1, dVector3 f2, dVector3 t2);
> >
> > Ah, cool. That will give me the information I'm looking for.
> >
> > Thanks!
> >
> > --
> >
> > Nate Waddoups
> > Redmond WA USA
> > http://www.natew.com
> >
> >
> > _______________________________________________
> > ODE mailing list
> > ODE@q12.org
> > http://q12.org/mailman/listinfo/ode
> >
> >
>
> _______________________________________________
> ODE mailing list
> ODE@q12.org
> http://q12.org/mailman/listinfo/ode
>
>
------=_NextPart_000_0091_01C24E0E.F03D40D0
Content-Type: application/octet-stream;
name="J.patch"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="J.patch"
Index: ode/src/joint.cpp
=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
RCS file: /cvsroot/ode/ode/src/joint.cpp,v
retrieving revision 1.40
diff -u -r1.40 joint.cpp
--- ode/src/joint.cpp 2002/06/25 23:46:18 1.40
+++ ode/src/joint.cpp 2002/08/27 19:13:33
@@ -2023,6 +2023,9 @@
return joint->mode;
}
=20
+extern "C" const dContact* dJointGetContact (dxJointContact *joint){
+ return &joint->contact;
+}
=20
dxJoint::Vtable __damotor_vtable =3D {
sizeof(dxJointAMotor),
Index: ode/src/joint.h
=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
RCS file: /cvsroot/ode/ode/src/joint.h,v
retrieving revision 1.33
diff -u -r1.33 joint.h
--- ode/src/joint.h 2002/06/25 23:46:18 1.33
+++ ode/src/joint.h 2002/08/27 19:13:34
@@ -28,6 +28,8 @@
#include <ode/contact.h>
#include "obstack.h"
=20
+// copied from include/ode
+typedef int dJointCallback (dJointID j, dReal stepsize, dVector3 f1, =
dVector3 t1, dVector3 f2, dVector3 t2);
=20
// joint flags
enum {
@@ -121,6 +123,7 @@
Vtable *vtable; // virtual function table
int flags; // dJOINT_xxx flags
dxJointNode node[2]; // connections to bodies. node[1].body can be 0
+ dJointCallback *cb; // user provided callback. can be 0
};
=20
=20
Index: ode/src/step.cpp
=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
RCS file: /cvsroot/ode/ode/src/step.cpp,v
retrieving revision 1.24
diff -u -r1.24 step.cpp
--- ode/src/step.cpp 2002/06/25 23:46:19 1.24
+++ ode/src/step.cpp 2002/08/27 19:13:36
@@ -173,6 +173,31 @@
A[6] +=3D sum;
}
=20
+static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q)
+{
+ int k;
+ dReal sum;
+ dIASSERT (q>0 && A && B && C);
+ sum =3D 0;
+ for (k=3D0; k<q; k++) sum +=3D B[k*8] * C[k];
+ A[0] =3D sum;
+ sum =3D 0;
+ for (k=3D0; k<q; k++) sum +=3D B[1+k*8] * C[k];
+ A[1] =3D sum;
+ sum =3D 0;
+ for (k=3D0; k<q; k++) sum +=3D B[2+k*8] * C[k];
+ A[2] =3D sum;
+ sum =3D 0;
+ for (k=3D0; k<q; k++) sum +=3D B[4+k*8] * C[k];
+ A[4] =3D sum;
+ sum =3D 0;
+ for (k=3D0; k<q; k++) sum +=3D B[5+k*8] * C[k];
+ A[5] =3D sum;
+ sum =3D 0;
+ for (k=3D0; k<q; k++) sum +=3D B[6+k*8] * C[k];
+ A[6] =3D sum;
+}
+
=
//***********************************************************************=
*****
// body rotation
=20
@@ -923,13 +948,44 @@
# endif
// compute cforce =3D J'*lambda
for (i=3D0; i<nj; i++) {
- dReal *JJ =3D J + 2*8*ofs[i];
- MultiplyAdd1_8q1 (cforce + 8*joint[i]->node[0].body->tag,JJ,
- lambda+ofs[i], info[i].m);
- if (joint[i]->node[1].body) {
- MultiplyAdd1_8q1 (cforce + 8*joint[i]->node[1].body->tag,
- JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
- }
+ dReal *JJ =3D J + 2*8*ofs[i];
+
+ dxBody* b1 =3D joint[i]->node[0].body;
+ dxBody* b2 =3D joint[i]->node[1].body;
+
+ if (joint[i]->cb){ // We have a callback so we need to take the slow =
path
+ dReal Data1[8];
+ Multiply1_8q1 (Data1, JJ, lambda+ofs[i], info[i].m);
+
+ dReal Data2[8];
+ if (b2){
+ Multiply1_8q1 (Data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
+ }
+ else dSetZero(Data2, 8);
+ if (joint[i]->cb(joint[i], stepsize, Data1, Data1 + 4, Data2, Data2 =
+ 4)){
+ (cforce + 8*b1->tag)[0] +=3D Data1[0];
+ (cforce + 8*b1->tag)[1] +=3D Data1[1];
+ (cforce + 8*b1->tag)[2] +=3D Data1[2];
+ (cforce + 8*b1->tag)[4] +=3D Data1[4];
+ (cforce + 8*b1->tag)[5] +=3D Data1[5];
+ (cforce + 8*b1->tag)[6] +=3D Data1[6];
+
+ if (b2){
+ (cforce + 8*b2->tag)[0] +=3D Data2[0];
+ (cforce + 8*b2->tag)[1] +=3D Data2[1];
+ (cforce + 8*b2->tag)[2] +=3D Data2[2];
+ (cforce + 8*b2->tag)[4] +=3D Data2[4];
+ (cforce + 8*b2->tag)[5] +=3D Data2[5];
+ (cforce + 8*b2->tag)[6] +=3D Data2[6];
+ }
+ }
+ }
+ else{
+ MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m);
+ if (b2) {
+ MultiplyAdd1_8q1 (cforce + 8*b2->tag, JJ + 8*info[i].m, =
lambda+ofs[i], info[i].m);
+ }
+ }
}
}
=20
------=_NextPart_000_0091_01C24E0E.F03D40D0
Content-Type: application/octet-stream;
name="J_include.patch"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="J_include.patch"
Index: include/ode/objects.h
=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
RCS file: /cvsroot/ode/include/ode/objects.h,v
retrieving revision 1.32
diff -u -r1.32 objects.h
--- include/ode/objects.h 2002/07/13 23:35:07 1.32
+++ include/ode/objects.h 2002/08/27 19:14:57
@@ -129,6 +129,10 @@
=20
void dJointDestroy (dJointID);
=20
+typedef int dJointCallback (dJointID j, dReal stepsize, dVector3 f1, =
dVector3 t1, dVector3 f2, dVector3 t2);
+void dJointSetCallback (dJointID, dJointCallback* cb);
+dJointCallback* dJointGetCallback (dJointID);
+
dJointGroupID dJointGroupCreate (int max_size);
void dJointGroupDestroy (dJointGroupID);
void dJointGroupEmpty (dJointGroupID);
@@ -187,6 +191,8 @@
dReal dJointGetAMotorAngleRate (dJointID, int anum);
dReal dJointGetAMotorParam (dJointID, int parameter);
int dJointGetAMotorMode (dJointID);
+
+const dContact* dJointGetContact (dJointID);
=20
int dAreConnected (dBodyID, dBodyID);
=20
------=_NextPart_000_0091_01C24E0E.F03D40D0--