add ode

1 files changed, 735 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp b/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp
new file mode 100644
index 0000000..6ec3093
--- /dev/null
+++ b/libs/ode-0.16.1/ode/demo/demo_jointPU.cpp
@@ -0,0 +1,735 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+  This program demonstrates how the PU joint works.
+  A PU joint is a combination of a Universal joint and a Slider joint.
+  It is a universal joint with a slider between the anchor point and
+  body 1.
+
+
+  The upper yellow body is fixed to the world
+  The lower yellow body is attached to the upper body by a PU joint
+  The green object is one aprt of the slider.
+  The purple object is the second part of the slider.
+  The red object represent the axis1 of the universal part.
+  The blue object represent the axis2 of the universal part.
+  The gray object represent the anchor2 of the PU joint.
+*/
+
+
+#include <ode/ode.h>
+#include <drawstuff/drawstuff.h>
+#include <iostream>
+#include <math.h>
+#include "texturepath.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
+#endif
+// select correct drawing functions
+#ifdef dDOUBLE
+#define dsDrawBox dsDrawBoxD
+#define dsDrawCylinder dsDrawCylinderD
+#define dsDrawCapsule dsDrawCapsuleD
+#endif
+
+enum IDX_CYL_DIM
+{
+  RADIUS,
+  LENGTH,
+  NUM_CYL_DIM
+};
+
+
+const dVector3 boxDim = {1,1,1};
+const dVector3 extDim = {0.2,0.2,1.2};
+const dVector3 ancDim = {0.2,0.2,0.5};
+const dReal    axDim[NUM_CYL_DIM] = {0.1,1.0};
+
+
+int type = dJointTypePU;
+
+
+const dReal VEL_INC = 0.01; // Velocity increment
+
+// physics parameters
+const dReal PI = 3.14159265358979323846264338327950288419716939937510;
+
+
+const dReal INT_EXT_RATIO = 0.8;
+
+#define X 0
+#define Y 1
+#define Z 2
+
+enum INDEX
+{
+  W = 0,
+  D,
+  EXT,
+  INT,
+  AXIS1,
+  AXIS2,
+  ANCHOR,
+  GROUND,
+  NUM_PARTS,
+  ALL = NUM_PARTS,
+  // INDEX for catBits
+  JOINT,
+  LAST_INDEX_CNT
+};
+
+const int catBits[LAST_INDEX_CNT] =
+  {
+    0x0001, ///< W Cylinder category
+    0x0002, ///< D Cylinder category
+    0x0004, ///< EXT sliderr category
+    0x0008, ///< INT slider category
+    0x0010, ///< AXIS1 universal category
+    0x0020, ///< AXIS2 universal category
+    0x0040, ///< ANCHOR category
+    0x0080, ///< Ground category
+    ~0L,    ///< All categories
+    0x0004 | 0x0008 | 0x0010 | 0x0020 ///< JOINT category
+  };
+
+#define Mass1 10
+#define Mass2 8
+
+
+//camera view
+static float xyz[3] = {6.0f,0.0f,6.0000f};
+static float hpr[3] = {-180.000f,-25.5000f,0.0000f};
+
+
+//world,space,body & geom
+static dWorldID         world;
+static dSpaceID         space;
+static dJointGroupID    contactgroup;
+static dBodyID          body[NUM_PARTS];
+static dGeomID          geom[NUM_PARTS];
+
+static dJoint *joint;
+
+
+
+const dReal BOX_SIDES[3]  = {1.0,1.0,1.0};
+const dReal OBS_SIDES[3]  = {0.4,0.4,0.4};
+const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2};
+
+
+//collision detection
+static void nearCallback (void *, dGeomID o1, dGeomID o2)
+{
+  int i,n;
+
+  const int N = 10;
+  dContact contact[N];
+  n = dCollide (o1,o2,N,&contact[0].geom,sizeof (dContact) );
+  if (n > 0) {
+    for  (i=0; i<n; i++) {
+      contact[i].surface.mode = (dContactSlip1 | dContactSlip2 |
+                                 dContactSoftERP | dContactSoftCFM |
+                                 dContactApprox1);
+      contact[i].surface.mu = 0.1;
+      contact[i].surface.slip1 = 0.02;
+      contact[i].surface.slip2 = 0.02;
+      contact[i].surface.soft_erp = 0.1;
+      contact[i].surface.soft_cfm = 0.0001;
+      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
+      dJointAttach (c,dGeomGetBody (contact[i].geom.g1),dGeomGetBody (contact[i].geom.g2) );
+    }
+  }
+}
+
+static void printKeyBoardShortCut()
+{
+  printf ("Press 'h' for this help.\n");
+  printf ("Press 'q' to add force on BLUE body along positive x direction.\n");
+  printf ("Press 'w' to add force on BLUE body along negative x direction.\n");
+
+  printf ("Press 'a' to add force on BLUE body along positive y direction.\n");
+  printf ("Press 's' to add force on BLUE body along negative y direction.\n");
+
+  printf ("Press 'z' to add force on BLUE body along positive z direction.\n");
+  printf ("Press 'x' to add force on BLUE body along negative z direction.\n");
+
+  printf ("Press 'e' to add torque on BLUE body around positive x direction \n");
+  printf ("Press 'r' to add torque on BLUE body around negative x direction \n");
+
+  printf ("Press 'd' to add torque on BLUE body around positive y direction \n");
+  printf ("Press 'f' to add torque on BLUE body around negative y direction \n");
+
+  printf ("Press 'c' to add torque on BLUE body around positive z direction \n");
+  printf ("Press 'v' to add torque on BLUE body around negative z direction \n");
+
+  printf ("Press '.' to increase joint velocity along the prismatic direction.\n");
+  printf ("Press ',' to decrease joint velocity along the prismatic direction.\n");
+
+  printf ("Press 'l' Toggle ON/OFF the limits on all the axis\n");
+  printf ("Press 'g' Toggle ON/OFF the gravity\n");
+
+
+  printf ("Press 'p' to print the position, angle and rates of the joint.\n");
+}
+
+
+// start simulation - set viewpoint
+static void start()
+{
+  dAllocateODEDataForThread(dAllocateMaskAll);
+
+  dsSetViewpoint (xyz,hpr);
+  printf ("This program demonstrates how the PU joint works.\n");
+  printf ("A PU joint is a combination of a Universal joint and a Slider joint.\n");
+  printf ("It is a universal joint with a slider between the anchor point and \n");
+  printf ("body 1.\n\n");
+  printf ("The upper yellow body is fixed to the world\n");
+  printf ("The lower yellow body is attached to the upper body by a PU joint\n");
+  printf ("The green object is one aprt of the slider.\n");
+  printf ("The purple object is the second part of the slider.\n");
+  printf ("The red object represent the axis1 of the universal part. \n");
+  printf ("The blue object represent the axis2 of the universal part. \n");
+  printf ("The gray object represent the anchor2 of the PU joint. \n");
+  printKeyBoardShortCut();
+}
+
+// function to update camera position at each step.
+void update()
+{
+//   static FILE *file = fopen("x:/sim/src/libode/tstsrcSF/export.dat", "w");
+
+//   static int cnt = 0;
+//   char str[24];
+//   sprintf(str, "%06d",cnt++);
+
+//   dWorldExportDIF(world, file, str);
+}
+
+
+// called when a key pressed
+static void command (int cmd)
+{
+  switch (cmd) {
+case 'h' : case 'H' : case '?' :
+      printKeyBoardShortCut();
+      break;
+
+      // Force
+  case 'q' : case 'Q' :
+      dBodyAddForce(body[D],40,0,0);
+      break;
+  case 'w' : case 'W' :
+      dBodyAddForce(body[D],-40,0,0);
+      break;
+
+  case 'a' : case 'A' :
+      dBodyAddForce(body[D],0,40,0);
+      break;
+  case 's' : case 'S' :
+      dBodyAddForce(body[D],0,-40,0);
+      break;
+
+  case 'z' : case 'Z' :
+      dBodyAddForce(body[D],0,0,40);
+      break;
+  case 'x' : case 'X' :
+      dBodyAddForce(body[D],0,0,-40);
+      break;
+
+      // Torque
+  case 'e': case 'E':
+      dBodyAddTorque(body[D],0.1,0,0);
+      break;
+  case 'r': case 'R':
+      dBodyAddTorque(body[D],-0.1,0,0);
+      break;
+
+  case 'd': case 'D':
+      dBodyAddTorque(body[D],0, 0.1,0);
+      break;
+  case 'f': case 'F':
+      dBodyAddTorque(body[D],0,-0.1,0);
+      break;
+
+  case 'c': case 'C':
+      dBodyAddTorque(body[D],0,0,0.1);
+      break;
+  case 'v': case 'V':
+      dBodyAddTorque(body[D],0,0,0.1);
+      break;
+
+      // Velocity of joint
+  case ',': case '<' : {
+      dReal vel = joint->getParam (dParamVel3) - VEL_INC;
+      joint->setParam (dParamVel3, vel);
+	  joint->setParam (dParamFMax3, 2);
+      std::cout<<"Velocity = "<<vel<<"  FMax = 2"<<'\n';
+    }
+    break;
+
+  case '.': case '>' : {
+      dReal vel = joint->getParam (dParamVel3) + VEL_INC;
+      joint->setParam (dParamVel3, vel);
+	  joint->setParam (dParamFMax3, 2);
+      std::cout<<"Velocity = "<<vel<<"  FMax = 2"<<'\n';
+    }
+    break;
+
+  case 'l': case 'L' : {
+      dReal aLimit, lLimit, fmax;
+      if (  joint->getParam (dParamFMax) ) {
+        aLimit = dInfinity;
+        lLimit = dInfinity;
+        fmax = 0;
+      }
+      else {
+        aLimit = 0.25*PI;
+        lLimit = 0.5*axDim[LENGTH];
+        fmax = 0.02;
+      }
+
+      joint->setParam (dParamFMax1, fmax);
+      joint->setParam (dParamFMax2, fmax);
+      joint->setParam (dParamFMax3, fmax);
+
+      switch (joint->getType() ) {
+        case dJointTypePR : {
+          dPRJoint *pr = reinterpret_cast<dPRJoint *> (joint);
+          pr->setParam (dParamLoStop, -lLimit);
+          pr->setParam (dParamHiStop, -lLimit);
+          pr->setParam (dParamLoStop2, aLimit);
+          pr->setParam (dParamHiStop2, -aLimit);
+        }
+        break;
+        case dJointTypePU : {
+          dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
+          pu->setParam (dParamLoStop1, -aLimit);
+          pu->setParam (dParamHiStop1, aLimit);
+          pu->setParam (dParamLoStop2, -aLimit);
+          pu->setParam (dParamHiStop2, aLimit);
+          pu->setParam (dParamLoStop3, -lLimit);
+          pu->setParam (dParamHiStop3, lLimit);
+        }
+        break;
+        default: {} // keep the compiler happy
+      }
+    }
+
+    break;
+
+  case 'g': case 'G' : {
+      dVector3 g;
+      dWorldGetGravity(world, g);
+      if (  g[2]< -0.1 )
+        dWorldSetGravity(world, 0, 0, 0);
+      else
+        dWorldSetGravity(world, 0, 0, -0.5);
+
+    }
+
+case 'p' :case 'P' : {
+      switch (joint->getType() ) {
+        case dJointTypeSlider : {
+          dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
+          std::cout<<"Position ="<<sj->getPosition() <<"\n";
+        }
+        break;
+        case dJointTypePU : {
+          dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
+          std::cout<<"Position ="<<pu->getPosition() <<"\n";
+          std::cout<<"Position Rate="<<pu->getPositionRate() <<"\n";
+          std::cout<<"Angle1 ="<<pu->getAngle1() <<"\n";
+          std::cout<<"Angle1 Rate="<<pu->getAngle1Rate() <<"\n";
+          std::cout<<"Angle2 ="<<pu->getAngle2() <<"\n";
+          std::cout<<"Angle2 Rate="<<pu->getAngle2Rate() <<"\n";
+        }
+        break;
+        default: {} // keep the compiler happy
+      }
+    }
+    break;
+  }
+}
+
+static void drawBox (dGeomID id, int R, int G, int B)
+{
+  if (!id)
+    return;
+
+  const dReal *pos = dGeomGetPosition (id);
+  const dReal *rot = dGeomGetRotation (id);
+  dsSetColor (R,G,B);
+
+  dVector3 l;
+  dGeomBoxGetLengths (id, l);
+  dsDrawBox (pos, rot, l);
+}
+
+
+// simulation loop
+static void simLoop (int pause)
+{
+  static bool todo = false;
+  if ( todo ) { // DEBUG
+    static int cnt = 0;
+    ++cnt;
+
+    if (cnt == 5)
+      command ( 'q' );
+    if (cnt == 10)
+      dsStop();
+  }
+
+
+
+
+  if (!pause) {
+    double simstep = 0.01; // 10ms simulation steps
+    double dt = dsElapsedTime();
+
+    int nrofsteps = (int) ceilf (dt/simstep);
+    if (!nrofsteps)
+      nrofsteps = 1;
+
+    for (int i=0; i<nrofsteps && !pause; i++) {
+      dSpaceCollide (space,0,&nearCallback);
+      dWorldStep (world, simstep);
+
+      dJointGroupEmpty (contactgroup);
+    }
+
+    update();
+
+
+    dReal radius, length;
+
+    dsSetTexture (DS_WOOD);
+
+    drawBox (geom[W], 1,1,0);
+
+
+    drawBox (geom[EXT], 0,1,0);
+
+    dVector3 anchorPos;
+
+
+
+    dReal ang1 = 0;
+    dReal ang2 = 0;
+    dVector3 axisP, axisR1, axisR2;
+
+    if ( dJointTypePU == type ) {
+      dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
+      ang1 = pu->getAngle1();
+      ang2 = pu->getAngle2();
+      pu->getAxis1 (axisR1);
+      pu->getAxis2 (axisR2);
+      pu->getAxisP (axisP);
+
+      dJointGetPUAnchor (pu->id(), anchorPos);
+    }
+    else if ( dJointTypePR == type ) {
+      dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
+      pr->getAxis1 (axisP);
+      pr->getAxis2 (axisR1);
+
+      dJointGetPRAnchor (pr->id(), anchorPos);
+    }
+
+
+    // Draw the axisR
+    if ( geom[INT] ) {
+      dsSetColor (1,0,1);
+      dVector3 l;
+      dGeomBoxGetLengths (geom[INT], l);
+
+      const dReal *rotBox = dGeomGetRotation (geom[W]);
+
+      dVector3 pos;
+      for (int i=0; i<3; ++i)
+        pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
+      dsDrawBox (pos, rotBox, l);
+    }
+
+    dsSetTexture (DS_CHECKERED);
+    if ( geom[AXIS1] ) {
+      dQuaternion q, qAng;
+      dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
+      dGeomGetQuaternion (geom[AXIS1], q);
+
+      dQuaternion qq;
+      dQMultiply1 (qq, qAng, q);
+      dMatrix3 R;
+      dQtoR (qq,R);
+
+
+      dGeomCylinderGetParams (geom[AXIS1], &radius, &length);
+      dsSetColor (1,0,0);
+      dsDrawCylinder (anchorPos, R, length, radius);
+    }
+
+    if ( dJointTypePU == type && geom[AXIS2] ) {
+      //dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
+
+      dQuaternion q, qAng, qq, qq1;
+      dGeomGetQuaternion (geom[AXIS2], q);
+
+      dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
+      dQMultiply1 (qq, qAng, q);
+
+
+      dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
+
+      dQMultiply1 (qq1, qAng, qq);
+
+
+      dMatrix3 R;
+      dQtoR (qq1,R);
+
+
+      dGeomCylinderGetParams (geom[AXIS2], &radius, &length);
+      dsSetColor (0,0,1);
+      dsDrawCylinder (anchorPos, R, length, radius);
+    }
+
+    dsSetTexture (DS_WOOD);
+
+    // Draw the anchor
+    if ( geom[ANCHOR] ) {
+      dsSetColor (1,1,1);
+      dVector3 l;
+      dGeomBoxGetLengths (geom[ANCHOR], l);
+
+      const dReal *rotBox = dGeomGetRotation (geom[D]);
+      const dReal *posBox = dGeomGetPosition (geom[D]);
+
+      dVector3 e;
+      for (int i=0; i<3; ++i)
+        e[i] = posBox[i] - anchorPos[i];
+      dNormalize3 (e);
+
+      dVector3 pos;
+      for (int i=0; i<3; ++i)
+        pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
+      dsDrawBox (pos, rotBox, l);
+    }
+
+    drawBox (geom[D], 1,1,0);
+  }
+}
+
+
+void Help (char **argv)
+{
+  printf ("%s ", argv[0]);
+  printf (" -h | --help   : print this help\n");
+  printf (" -p | --PRJoint : Use a PR joint instead of PU joint\n");
+  printf (" -t | --texture-path path  : Path to the texture.\n");
+  printf ("                             Default = %s\n", DRAWSTUFF_TEXTURE_PATH);
+  printf ("--------------------------------------------------\n");
+  printf ("Hit any key to continue:");
+  getchar();
+
+  exit (0);
+}
+
+int main (int argc, char **argv)
+{
+  // setup pointers to drawstuff callback functions
+  dsFunctions fn;
+  fn.version = DS_VERSION;
+  fn.start = &start;
+  fn.step = &simLoop;
+  fn.command = &command;
+  fn.stop = 0;
+  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
+
+  if (argc >= 2 ) {
+    for (int i=1; i < argc; ++i) {
+      if (  0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
+        Help (argv);
+
+      if (  0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) )
+        type = dJointTypePR;
+
+      if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) {
+        int j = i+1;
+        if ( j >= argc      ||  // Check if we have enough arguments
+             argv[j][0] == '\0' ||  // We should have a path here
+             argv[j][0] == '-' ) // We should have a path not a command line
+          Help (argv);
+        else
+          fn.path_to_textures = argv[++i]; // Increase i since we use this argument
+      }
+    }
+  }
+
+  dInitODE2(0);
+
+  world = dWorldCreate();
+  dWorldSetERP (world, 0.8);
+
+  space = dSimpleSpaceCreate (0);
+  contactgroup = dJointGroupCreate (0);
+  geom[GROUND] = dCreatePlane (space, 0,0,1,0);
+  dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
+  dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
+
+  dMass m;
+
+  // Create the body attached to the World
+  body[W] = dBodyCreate (world);
+  // Main axis of cylinder is along X=1
+  m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
+  m.adjust (Mass1);
+  geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
+  dGeomSetBody (geom[W], body[W]);
+  dGeomSetCategoryBits (geom[W], catBits[W]);
+  dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) );
+  dBodySetMass (body[W], &m);
+
+
+
+
+
+  // Create the dandling body
+  body[D] = dBodyCreate (world);
+  // Main axis of capsule is along X=1
+  m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
+  m.adjust (Mass1);
+  geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
+  dGeomSetBody (geom[D], body[D]);
+  dGeomSetCategoryBits (geom[D], catBits[D]);
+  dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) );
+  dBodySetMass (body[D], &m);
+
+
+  // Create the external part of the slider joint
+  geom[EXT] = dCreateBox (0, extDim[X], extDim[Y], extDim[Z]);
+  dGeomSetCategoryBits (geom[EXT], catBits[EXT]);
+  dGeomSetCollideBits (geom[EXT],
+                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
+
+  // Create the internal part of the slider joint
+  geom[INT] = dCreateBox (0, INT_EXT_RATIO*extDim[X],
+                          INT_EXT_RATIO*extDim[Y],
+                          INT_EXT_RATIO*extDim[Z]);
+  dGeomSetCategoryBits (geom[INT], catBits[INT]);
+  dGeomSetCollideBits (geom[INT],
+                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
+
+
+  dMatrix3 R;
+  // Create the first axis of the universal joi9nt
+  //Rotation of 90deg around y
+  geom[AXIS1] = dCreateCylinder(0, axDim[RADIUS], axDim[LENGTH]);
+  dRFromAxisAndAngle(R, 0,1,0, 0.5*PI);
+  dGeomSetRotation(geom[AXIS1], R);
+  dGeomSetCategoryBits(geom[AXIS1], catBits[AXIS1]);
+  dGeomSetCollideBits(geom[AXIS1],
+                      catBits[ALL]  & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
+
+
+  // Create the second axis of the universal joint
+  geom[AXIS2] = dCreateCylinder(0, axDim[RADIUS], axDim[LENGTH]);
+  //Rotation of 90deg around y
+  dRFromAxisAndAngle(R, 1,0,0, 0.5*PI);
+  dGeomSetRotation(geom[AXIS2], R);
+  dGeomSetCategoryBits(geom[AXIS2], catBits[AXIS2]);
+  dGeomSetCollideBits(geom[AXIS2],
+                      catBits[ALL]  & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
+
+  // Create the anchor
+  geom[ANCHOR] = dCreateBox (0, ancDim[X], ancDim[Y], ancDim[Z]);
+  dGeomSetCategoryBits(geom[ANCHOR], catBits[ANCHOR]);
+  dGeomSetCollideBits(geom[ANCHOR],
+                      catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
+
+
+
+  if (body[W]) {
+    dBodySetPosition(body[W], 0, 0, 5);
+  }
+
+
+  if (geom[EXT]) {
+    dGeomSetPosition(geom[EXT], 0,0,3.8);
+  }
+  if (geom[INT]) {
+    dGeomSetPosition(geom[INT], 0,0,2.6);
+  }
+  if (geom[AXIS1]) {
+    dGeomSetPosition(geom[AXIS1], 0,0,2.5);
+  }
+  if (geom[AXIS2]) {
+    dGeomSetPosition(geom[AXIS2], 0,0,2.5);
+  }
+
+  if (geom[ANCHOR]) {
+    dGeomSetPosition(geom[ANCHOR], 0,0,2.25);
+  }
+
+  if (body[D]) {
+    dBodySetPosition(body[D], 0,0,1.5);
+  }
+
+
+
+  // Attache the upper box to the world
+  dJointID fixed = dJointCreateFixed (world,0);
+  dJointAttach (fixed , NULL, body[W]);
+  dJointSetFixed (fixed );
+
+  if (type == dJointTypePR) {
+    dPRJoint *pr = new dPRJoint (world, 0);
+    pr->attach (body[W], body[D]);
+    pr->setAxis1 (0, 0, -1);
+    pr->setAxis2 (1, 0, 0);
+    joint = pr;
+
+    dJointSetPRAnchor (pr->id(), 0, 0, 2.5);
+  }
+  else {
+    dPUJoint *pu = new dPUJoint (world, 0);
+    pu->attach (body[W], body[D]);
+    pu->setAxis1 (1, 0, 0);
+    pu->setAxis2 (0, 1, 0);
+    pu->setAxisP (0, 0, -1);
+    joint = pu;
+
+    dJointSetPUAnchor (pu->id(), 0, 0, 2.5);
+  }
+
+
+  // run simulation
+  dsSimulationLoop (argc,argv,400,300,&fn);
+
+  delete joint;
+  dJointGroupDestroy (contactgroup);
+  dSpaceDestroy (space);
+  dWorldDestroy (world);
+  dCloseODE();
+  return 0;
+}
+