Plane2d.patch
From ODE Wiki
Below is a patch to ODE 0.6 to add the dJointPlane2D functionality described in HOWTO constrain objects to 2d. To apply, save the text below to a file called "plane2d.patch", and move the patch file to the directory just below the ODE-0.6 directory. From there, run the following command:
patch -Np0 <plane2d.patch
Then make and re-install ODE as before. Note that this places the test code in a strange place: drawstuff/test.pserocka/.
plane2d.patch:
diff -Naur ode-0.6.old/drawstuff/test.pserocka/GNUmakefile ode-0.6/drawstuff/test.pserocka/GNUmakefile
--- ode-0.6.old/drawstuff/test.pserocka/GNUmakefile 1969-12-31 19:00:00.000000000 -0500
+++ ode-0.6/drawstuff/test.pserocka/GNUmakefile 2006-07-31 19:24:01.859329000 -0400
@@ -0,0 +1,21 @@
+CXX := g++
+CXXFLAGS := -O
+
+CPPFLAGS :=
+LDFLAGS :=
+LDLIBS :=
+
+CPPFLAGS += -I ../include
+LDFLAGS += -L ../lib -L ../drawstuff/src
+LDLIBS += -lode -ldrawstuff
+
+CPPFLAGS += -I /usr/X11R6/include
+LDFLAGS += -L /usr/X11R6/lib
+LDLIBS += -lGLU -lGL -lX11
+LDLIBS += -lm
+
+all: test_plane2d
+
+test_plane2d: test_plane2d.o
+ $(CXX) -o $@ $< $(CXXFLAGS) $(LDFLAGS) $(LDLIBS)
+
diff -Naur ode-0.6.old/drawstuff/test.pserocka/test_plane2d.cc ode-0.6/drawstuff/test.pserocka/test_plane2d.cc
--- ode-0.6.old/drawstuff/test.pserocka/test_plane2d.cc 1969-12-31 19:00:00.000000000 -0500
+++ ode-0.6/drawstuff/test.pserocka/test_plane2d.cc 2006-07-31 13:53:29.211866000 -0400
@@ -0,0 +1,273 @@
+// Test my Plane2D constraint.
+// Uses ode-0.35 collision API.
+
+# include <stdio.h>
+# include <stdlib.h>
+# include <math.h>
+# include <ode/ode.h>
+# include <drawstuff/drawstuff.h>
+
+
+# define N_BODIES 40
+# define STAGE_SIZE 8.0 // in m
+
+# define TIME_STEP 0.01
+# define K_SPRING 10.0
+# define K_DAMP 10.0
+
+// select correct drawing functions
+
+#ifdef dDOUBLE
+#define dsDrawBox dsDrawBoxD
+#define dsDrawSphere dsDrawSphereD
+#define dsDrawCylinder dsDrawCylinderD
+#define dsDrawCapsule dsDrawCapsuleD
+#endif
+
+static dWorld dyn_world;
+static dBody dyn_bodies[N_BODIES];
+static dReal bodies_sides[N_BODIES][3];
+
+static dSpaceID coll_space_id;
+static dJointID plane2d_joint_ids[N_BODIES];
+static dJointGroup
+ coll_contacts;
+
+
+
+static void cb_start ()
+/*************************/
+{
+ static float xyz[3] = { 0.5*STAGE_SIZE, 0.5*STAGE_SIZE, 0.65*STAGE_SIZE};
+ static float hpr[3] = { 90, -90, 0 };
+
+ dsSetViewpoint (xyz, hpr);
+}
+
+
+
+static void cb_near_collision (void *data, dGeomID o1, dGeomID o2)
+/********************************************************************/
+{
+ dBodyID b1 = dGeomGetBody (o1);
+ dBodyID b2 = dGeomGetBody (o2);
+ dContact contact;
+
+
+ // exit without doing anything if the two bodies are static
+ if (b1 == 0 && b2 == 0)
+ return;
+
+ // exit without doing anything if the two bodies are connected by a joint
+ if (b1 && b2 && dAreConnected (b1, b2))
+ {
+ /* MTRAP; */
+ return;
+ }
+
+ contact.surface.mode = 0;
+ contact.surface.mu = 0; // frictionless
+
+ if (dCollide (o1, o2, 0, &contact.geom, sizeof (dContactGeom)))
+ {
+ dJointID c = dJointCreateContact (dyn_world.id(),
+ coll_contacts.id (), &contact);
+ dJointAttach (c, b1, b2);
+ }
+}
+
+
+static void track_to_pos (dBody &body, dJointID joint_id,
+ dReal target_x, dReal target_y)
+/************************************************************************/
+{
+ double curr_x = body.getPosition()[0];
+ double curr_y = body.getPosition()[1];
+
+ dJointSetPlane2DXParam (joint_id, dParamVel, 1 * (target_x - curr_x));
+ dJointSetPlane2DYParam (joint_id, dParamVel, 1 * (target_y - curr_y));
+}
+
+
+
+static void cb_sim_step (int pause)
+/*************************************/
+{
+ if (! pause)
+ {
+ static double
+ angle = 0;
+
+ angle += 0.01;
+
+ track_to_pos (dyn_bodies[0], plane2d_joint_ids[0],
+ STAGE_SIZE/2 + STAGE_SIZE/2.0 * cos (angle),
+ STAGE_SIZE/2 + STAGE_SIZE/2.0 * sin (angle));
+
+ /* double f0 = 0.001; */
+ /* for (int b = 0; b < N_BODIES; b ++) */
+ /* { */
+ /* double p = 1 + b / (double) N_BODIES; */
+ /* double q = 2 - b / (double) N_BODIES; */
+ /* dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0); */
+ /* } */
+ /* dyn_bodies[0].addTorque (0, 0, 0.1); */
+
+ const int n = 10;
+ for (int i = 0; i < n; i ++)
+ {
+ dSpaceCollide (coll_space_id, 0, &cb_near_collision);
+ dyn_world.step (TIME_STEP/n);
+ coll_contacts.empty ();
+ }
+ }
+
+# if 1 /* [ */
+ {
+ // @@@ hack Plane2D constraint error reduction here:
+ for (int b = 0; b < N_BODIES; b ++)
+ {
+ const dReal *rot = dBodyGetAngularVel (dyn_bodies[b].id());
+ const dReal *quat_ptr;
+ dReal quat[4],
+ quat_len;
+
+
+ quat_ptr = dBodyGetQuaternion (dyn_bodies[b].id());
+ quat[0] = quat_ptr[0];
+ quat[1] = 0;
+ quat[2] = 0;
+ quat[3] = quat_ptr[3];
+ quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]);
+ quat[0] /= quat_len;
+ quat[3] /= quat_len;
+ dBodySetQuaternion (dyn_bodies[b].id(), quat);
+ dBodySetAngularVel (dyn_bodies[b].id(), 0, 0, rot[2]);
+ }
+ }
+# endif /* ] */
+
+
+# if 0 /* [ */
+ {
+ // @@@ friction
+ for (int b = 0; b < N_BODIES; b ++)
+ {
+ const dReal *vel = dBodyGetLinearVel (dyn_bodies[b].id()),
+ *rot = dBodyGetAngularVel (dyn_bodies[b].id());
+ dReal s = 1.00;
+ dReal t = 0.99;
+
+ dBodySetLinearVel (dyn_bodies[b].id(), s*vel[0],s*vel[1],s*vel[2]);
+ dBodySetAngularVel (dyn_bodies[b].id(),t*rot[0],t*rot[1],t*rot[2]);
+ }
+ }
+# endif /* ] */
+
+
+ {
+ // ode drawstuff
+
+ dsSetTexture (DS_WOOD);
+ for (int b = 0; b < N_BODIES; b ++)
+ {
+ if (b == 0)
+ dsSetColor (1.0, 0.5, 1.0);
+ else
+ dsSetColor (0, 0.5, 1.0);
+ /* if (b == 0) */
+ /* dsDrawSphere (dyn_bodies[b].getPosition(), */
+ /* dyn_bodies[b].getRotation(), bodies_sides[b][0]); */
+ /* else */
+ dsDrawBox (dyn_bodies[b].getPosition(),
+ dyn_bodies[b].getRotation(), bodies_sides[b]);
+ }
+ }
+}
+
+
+
+extern int main
+/******************/
+(
+ int argc,
+ char **argv
+)
+{
+ int b;
+ dsFunctions drawstuff_functions;
+
+
+ // dynamic world
+
+ double cf_mixing = 1 / (TIME_STEP * K_SPRING + K_DAMP);
+ double err_reduct = TIME_STEP * K_SPRING * cf_mixing;
+ err_reduct = 0.5;
+ cf_mixing = 0.001;
+ dWorldSetERP (dyn_world.id (), err_reduct);
+ dWorldSetCFM (dyn_world.id (), cf_mixing);
+ dyn_world.setGravity (0, 0.0, -1.0);
+
+ coll_space_id = dSimpleSpaceCreate (0);
+
+ // dynamic bodies
+ for (b = 0; b < N_BODIES; b ++)
+ {
+ int l = (int) (1 + sqrt ((double) N_BODIES));
+ double x = (0.5 + (b / l)) / l * STAGE_SIZE;
+ double y = (0.5 + (b % l)) / l * STAGE_SIZE;
+ double z = 1.0 + 0.1 * drand48 ();
+
+ bodies_sides[b][0] = 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES);
+ bodies_sides[b][1] = 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES);
+ bodies_sides[b][2] = z;
+
+ dyn_bodies[b].create (dyn_world);
+ dyn_bodies[b].setPosition (x, y, z/2);
+ dyn_bodies[b].setData ((void*) b);
+ dBodySetLinearVel (dyn_bodies[b].id (),
+ 3 * (drand48 () - 0.5), 3 * (drand48 () - 0.5), 0);
+
+ dMass m;
+ m.setBox (1, bodies_sides[b][0],bodies_sides[b][1],bodies_sides[b][2]);
+ m.adjust (0.1 * bodies_sides[b][0] * bodies_sides[b][1]);
+ dyn_bodies[b].setMass (&m);
+
+ plane2d_joint_ids[b] = dJointCreatePlane2D (dyn_world.id (), 0);
+ dJointAttach (plane2d_joint_ids[b], dyn_bodies[b].id (), 0);
+ }
+
+ dJointSetPlane2DXParam (plane2d_joint_ids[0], dParamFMax, 10);
+ dJointSetPlane2DYParam (plane2d_joint_ids[0], dParamFMax, 10);
+
+
+ // collision geoms and joints
+ dCreatePlane (coll_space_id, 1, 0, 0, 0);
+ dCreatePlane (coll_space_id, -1, 0, 0, -STAGE_SIZE);
+ dCreatePlane (coll_space_id, 0, 1, 0, 0);
+ dCreatePlane (coll_space_id, 0, -1, 0, -STAGE_SIZE);
+
+ for (b = 0; b < N_BODIES; b ++)
+ {
+ dGeomID coll_box_id;
+ coll_box_id = dCreateBox (coll_space_id,
+ bodies_sides[b][0], bodies_sides[b][1], bodies_sides[b][2]);
+ dGeomSetBody (coll_box_id, dyn_bodies[b].id ());
+ }
+
+ coll_contacts.create (0);
+
+ {
+ // simulation loop (by drawstuff lib)
+ drawstuff_functions.version = DS_VERSION;
+ drawstuff_functions.start = &cb_start;
+ drawstuff_functions.step = &cb_sim_step;
+ drawstuff_functions.command = 0;
+ drawstuff_functions.stop = 0;
+ drawstuff_functions.path_to_textures = getenv ("ODE_DRAWSTUFF_TEXPATH");
+
+ dsSimulationLoop (argc, argv, 800, 800, &drawstuff_functions);
+ }
+
+ return 0;
+}
diff -Naur ode-0.6.old/include/ode/common.h ode-0.6/include/ode/common.h
--- ode-0.6.old/include/ode/common.h 2006-05-31 15:08:15.000000000 -0400
+++ ode-0.6/include/ode/common.h 2006-07-31 19:44:15.307164750 -0400
@@ -254,7 +254,8 @@
dJointTypeFixed,
dJointTypeNull,
dJointTypeAMotor,
- dJointTypeLMotor
+ dJointTypeLMotor,
+ dJointTypePlane2D
};
diff -Naur ode-0.6.old/include/ode/objects.h ode-0.6/include/ode/objects.h
--- ode-0.6.old/include/ode/objects.h 2006-05-08 14:52:52.000000000 -0400
+++ ode-0.6/include/ode/objects.h 2006-08-01 14:45:37.135979000 -0400
@@ -1093,15 +1093,16 @@
* @brief Get the type of the joint
* @ingroup joints
* @return the type, being one of these:
- * \li JointTypeBall
- * \li JointTypeHinge
- * \li JointTypeSlider
- * \li JointTypeContact
- * \li JointTypeUniversal
- * \li JointTypeHinge2
- * \li JointTypeFixed
- * \li JointTypeAMotor
- * \li JointTypeLMotor
+ * \li dJointTypeBall
+ * \li dJointTypeHinge
+ * \li dJointTypeSlider
+ * \li dJointTypeContact
+ * \li dJointTypeUniversal
+ * \li dJointTypeHinge2
+ * \li dJointTypeFixed
+ * \li dJointTypeAMotor
+ * \li dJointTypeLMotor
+ * \li dJointTypePlane2D
*/
ODE_API int dJointGetType (dJointID);
@@ -1694,6 +1695,32 @@
*/
ODE_API int dAreConnectedExcluding (dBodyID, dBodyID, int joint_type);
+/**
+ * @brief Create a new planar constraint joint.
+ * @ingroup joints
+ * @param dJointGroupID set to 0 to allocate the joint normally.
+ * If it is nonzero the joint is allocated in the given joint group.
+ */
+ODE_API dJointID dJointCreatePlane2D (dWorldID, dJointGroupID);
+
+/**
+ * @brief Set a motor parameter for the X-axis motor.
+ * @ingroup joints
+ */
+ODE_API void dJointSetPlane2DXParam (dJointID, int parameter, dReal value);
+
+/**
+ * @brief Set a motor parameter for the Y-axis motor.
+ * @ingroup joints
+ */
+ODE_API void dJointSetPlane2DYParam (dJointID, int parameter, dReal value);
+
+/**
+ * @brief Set a motor parameter for the rotational motor.
+ * @ingroup joints
+ */
+ODE_API void dJointSetPlane2DAngleParam (dJointID, int parameter, dReal value);
+
#ifdef __cplusplus
}
diff -Naur ode-0.6.old/ode/src/joint.cpp ode-0.6/ode/src/joint.cpp
--- ode-0.6.old/ode/src/joint.cpp 2006-03-24 14:36:06.000000000 -0500
+++ ode-0.6/ode/src/joint.cpp 2006-08-01 14:50:38.942840750 -0400
@@ -2996,3 +2996,154 @@
(dxJoint::getInfo1_fn*) nullGetInfo1,
(dxJoint::getInfo2_fn*) nullGetInfo2,
dJointTypeNull};
+
+
+# define VoXYZ(v1, o1, x, y, z) \
+ ( \
+ (v1)[0] o1 (x), \
+ (v1)[1] o1 (y), \
+ (v1)[2] o1 (z) \
+ )
+
+static dReal Midentity[3][3] =
+ {
+ { 1, 0, 0 },
+ { 0, 1, 0 },
+ { 0, 0, 1, }
+ };
+
+
+
+static void plane2dInit (dxJointPlane2D *j)
+/*********************************************/
+{
+ /* MINFO ("plane2dInit ()"); */
+ j->motor_x.init (j->world);
+ j->motor_y.init (j->world);
+ j->motor_angle.init (j->world);
+}
+
+
+
+static void plane2dGetInfo1 (dxJointPlane2D *j, dxJoint::Info1 *info)
+/***********************************************************************/
+{
+ /* MINFO ("plane2dGetInfo1 ()"); */
+
+ info->nub = 3;
+ info->m = 3;
+
+ if (j->motor_x.fmax > 0)
+ j->row_motor_x = info->m ++;
+ if (j->motor_y.fmax > 0)
+ j->row_motor_y = info->m ++;
+ if (j->motor_angle.fmax > 0)
+ j->row_motor_angle = info->m ++;
+}
+
+
+
+static void plane2dGetInfo2 (dxJointPlane2D *joint, dxJoint::Info2 *info)
+/***************************************************************************/
+{
+ int r0 = 0,
+ r1 = info->rowskip,
+ r2 = 2 * r1;
+ dReal eps = info->fps * info->erp;
+
+ /* MINFO ("plane2dGetInfo2 ()"); */
+
+/*
+ v = v1, w = omega1
+ (v2, omega2 not important (== static environment))
+
+ constraint equations:
+ xz = 0
+ wx = 0
+ wy = 0
+
+ <=> ( 0 0 1 ) (vx) ( 0 0 0 ) (wx) ( 0 )
+ ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 )
+ ( 0 0 0 ) (vz) ( 0 1 0 ) (wz) ( 0 )
+ J1/J1l Omega1/J1a
+*/
+
+ // fill in linear and angular coeff. for left hand side:
+
+ VoXYZ (&info->J1l[r0], =, 0, 0, 1);
+ VoXYZ (&info->J1l[r1], =, 0, 0, 0);
+ VoXYZ (&info->J1l[r2], =, 0, 0, 0);
+
+ VoXYZ (&info->J1a[r0], =, 0, 0, 0);
+ VoXYZ (&info->J1a[r1], =, 1, 0, 0);
+ VoXYZ (&info->J1a[r2], =, 0, 1, 0);
+
+ // error correction (against drift):
+
+ // a) linear vz, so that z (== pos[2]) == 0
+ //info->c[0] = eps * -joint->node[0].body->pos[2];
+ dBodyID body = joint->node[0].body;
+ info->c[0] = eps * -(dBodyGetPosition(body)[2]);
+
+# if 0
+ // b) angular correction? -> left to application !!!
+ // dReal *body_z_axis = &joint->node[0].body->R[8];
+ dReal *body_z_axis = &(dBodyGetRotation(body)[8]);
+ info->c[1] = eps * +atan2 (body_z_axis[1], body_z_axis[2]); // wx error
+ info->c[2] = eps * -atan2 (body_z_axis[0], body_z_axis[2]); // wy error
+# endif
+
+ // if the slider is powered, or has joint limits, add in the extra row:
+
+ if (joint->row_motor_x > 0)
+ joint->motor_x.addLimot (
+ (dxJoint*)joint, info, joint->row_motor_x, Midentity[0], 0);
+
+ if (joint->row_motor_y > 0)
+ joint->motor_y.addLimot (
+ (dxJoint*)joint, info, joint->row_motor_y, Midentity[1], 0);
+
+ if (joint->row_motor_angle > 0)
+ joint->motor_angle.addLimot (
+ (dxJoint*)joint, info, joint->row_motor_angle, Midentity[2], 1);
+}
+
+
+
+dxJoint::Vtable __dplane2d_vtable =
+{
+ sizeof (dxJointPlane2D),
+ (dxJoint::init_fn*) plane2dInit,
+ (dxJoint::getInfo1_fn*) plane2dGetInfo1,
+ (dxJoint::getInfo2_fn*) plane2dGetInfo2,
+ dJointTypePlane2D
+};
+
+
+void dJointSetPlane2DXParam (dJointID joint,
+ int parameter, dReal value)
+{
+ dUASSERT (joint, "bad joint argument");
+ dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d");
+ ((dxJointPlane2D*)joint)->motor_x.set (parameter, value);
+}
+
+
+void dJointSetPlane2DYParam ( dJointID joint,
+ int parameter, dReal value)
+{
+ dUASSERT (joint, "bad joint argument");
+ dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d");
+ ((dxJointPlane2D*)joint)->motor_y.set (parameter, value);
+}
+
+
+void dJointSetPlane2DAngleParam ( dJointID joint,
+ int parameter, dReal value)
+{
+ dUASSERT (joint, "bad joint argument");
+ dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d");
+ ((dxJointPlane2D*)joint)->motor_angle.set (parameter, value);
+}
+
+
diff -Naur ode-0.6.old/ode/src/joint.h ode-0.6/ode/src/joint.h
--- ode-0.6.old/ode/src/joint.h 2006-03-14 14:03:58.000000000 -0500
+++ ode-0.6/ode/src/joint.h 2006-07-31 19:57:49.750064250 -0400
@@ -265,6 +265,21 @@
extern struct dxJoint::Vtable __dfixed_vtable;
+// plane2D
+
+struct dxJointPlane2D : public dxJoint
+{
+ int row_motor_x;
+ int row_motor_y;
+ int row_motor_angle;
+ dxJointLimitMotor motor_x;
+ dxJointLimitMotor motor_y;
+ dxJointLimitMotor motor_angle;
+};
+
+extern struct dxJoint::Vtable __dplane2d_vtable;
+
+
// null joint, for testing only
struct dxJointNull : public dxJoint {
diff -Naur ode-0.6.old/ode/src/ode.cpp ode-0.6/ode/src/ode.cpp
--- ode-0.6.old/ode/src/ode.cpp 2006-05-05 11:29:49.000000000 -0400
+++ ode-0.6/ode/src/ode.cpp 2006-07-31 19:56:57.270784500 -0400
@@ -979,6 +979,13 @@
return createJoint (w,group,&__dlmotor_vtable);
}
+
+dxJoint * dJointCreatePlane2D (dWorldID w, dJointGroupID group)
+{
+ dAASSERT (w);
+ return createJoint (w,group,&__dplane2d_vtable);
+}
+
void dJointDestroy (dxJoint *j)
{
dAASSERT (j);
