1 files changed, 377 insertions, 0 deletions

diff --git a/libraries/ode-0.9/ode/demo/demo_jointPR.cpp b/libraries/ode-0.9/ode/demo/demo_jointPR.cpp
new file mode 100644
index 0000000..e0d002d
--- /dev/null
+++ b/libraries/ode-0.9/ode/demo/demo_jointPR.cpp
@@ -0,0 +1,377 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 file try to demonstrate how the PR joint is working.
+
+The axisP is draw in red and the axisR is in green
+
+*/
+
+
+#include <ode/ode.h>
+#include <drawstuff/drawstuff.h>
+#include <iostream>
+#include <math.h>
+
+
+#define DRAWSTUFF_TEXTURE_PATH "../../drawstuff/textures"
+
+
+#ifdef _MSC_VER
+#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
+#endif
+// select correct drawing functions
+#ifdef dDOUBLE
+#define dsDrawBox dsDrawBoxD
+#endif
+
+// physics parameters
+#define BOX1_LENGTH 2    // Size along the X axis 
+#define BOX1_WIDTH 1     // Size along the Y axis
+#define BOX1_HEIGHT 0.4  // Size along the Z axis (up) since gravity is (0,0,-10)
+#define BOX2_LENGTH 0.2
+#define BOX2_WIDTH 0.1
+#define BOX2_HEIGHT 0.4
+#define Mass1 10
+#define Mass2 0.1
+
+
+#define PRISMATIC_ONLY 1
+#define ROTOIDE_ONLY   2
+int flag = 0;
+
+
+//camera view
+static float xyz[3] = {2.0f,-3.5f,2.0000f};
+static float hpr[3] = {90.000f,-25.5000f,0.0000f};
+//world,space,body & geom
+static dWorldID world;
+static dSpaceID space;
+static dSpaceID box1_space;
+static dSpaceID box2_space;
+static dBodyID box1_body[1];
+static dBodyID box2_body[1];
+static dJointID joint[1];
+static dJointGroupID contactgroup;
+static dGeomID ground;
+static dGeomID box1[1];
+static dGeomID box2[1];
+
+
+//collision detection
+static void nearCallback (void *data, dGeomID o1, dGeomID o2)
+{
+          int i,n;
+
+                  dBodyID b1 = dGeomGetBody(o1);
+                  dBodyID b2 = dGeomGetBody(o2);
+                  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
+                  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));
+                        }
+                  }
+}
+
+
+// start simulation - set viewpoint
+static void start()
+{
+                dsSetViewpoint (xyz,hpr);
+                printf ("Press 'd' to add force along positive x direction.\nPress 'a' to add force along negative x direction.\n");
+                printf ("Press 'w' to add force along positive y direction.\nPress 's' to add force along negative y direction.\n");
+                printf ("Press 'e' to add torque around positive z direction.\nPress 'q' to add torque around negative z direction.\n");
+                printf ("Press 'o' to add force around positive x direction \n");
+}
+
+// function to update camera position at each step.
+void update()
+{
+//              const dReal *a =(dBodyGetPosition (box1_body[0]));
+//              float dx=a[0];
+//              float dy=a[1];
+//              float dz=a[2];
+//              xyz[0]=dx;
+//              xyz[1]=dy-5;
+//              xyz[2]=dz+2;
+//              hpr[1]=-22.5000f;
+//              dsSetViewpoint (xyz,hpr);
+}
+
+
+// called when a key pressed
+static void command (int cmd)
+{
+    switch(cmd)
+        {
+                case 'w': case 'W':
+                        dBodyAddForce(box2_body[0],0,500,0);
+                        std::cout<<(dBodyGetPosition(box2_body[0])[1]-dBodyGetPosition(box1_body[0])[1])<<'\n';
+                        break;
+                case 's': case 'S':
+                        dBodyAddForce(box2_body[0],0,-500,0);
+                        std::cout<<(dBodyGetPosition(box2_body[0])[1]-dBodyGetPosition(box1_body[0])[1])<<'\n';
+                        break;
+                case 'd': case 'D':
+                        dBodyAddForce(box2_body[0],500,0,0);
+                        std::cout<<(dBodyGetPosition(box2_body[0])[0]-dBodyGetPosition(box1_body[0])[0])<<'\n';
+                        break;
+                case 'a': case 'A':
+                        dBodyAddForce(box2_body[0],-500,0,0);
+                        std::cout<<(dBodyGetPosition(box2_body[0])[0]-dBodyGetPosition(box1_body[0])[0])<<'\n';
+                        break;
+                case 'e': case 'E':
+                        dBodyAddRelTorque(box2_body[0],0,0,200);
+                        break;
+                case 'q': case 'Q':
+                        dBodyAddRelTorque(box2_body[0],0,0,-200);
+                        break;
+                case 'o': case 'O':
+                        dBodyAddForce(box1_body[0],10000,0,0);
+                        break;
+        }
+}
+
+
+// simulation loop
+static void simLoop (int pause)
+{
+        if (!pause)
+        {
+                //draw 2 boxes
+                dVector3 ss;
+                dsSetTexture (DS_WOOD);
+
+                const dReal *posBox2 = dGeomGetPosition(box2[0]);
+                const dReal *rotBox2 = dGeomGetRotation(box2[0]);
+                dsSetColor (1,1,0);
+                dGeomBoxGetLengths (box2[0],ss);
+                dsDrawBox (posBox2, rotBox2, ss);
+
+                const dReal *posBox1 = dGeomGetPosition(box1[0]);
+                const dReal *rotBox1 = dGeomGetRotation(box1[0]);
+                dsSetColor (1,1,2);
+                dGeomBoxGetLengths (box1[0], ss);
+                dsDrawBox (posBox1, rotBox1, ss);
+
+                dVector3 anchorPos;
+                dJointGetPRAnchor (joint[0], anchorPos);
+
+                // Draw the axisP 
+                if (ROTOIDE_ONLY != flag )
+                {
+                  dsSetColor (1,0,0);
+                  dVector3 sizeP = {0, 0.1, 0.1};
+                  for (int i=0; i<3; ++i)
+                    sizeP[0] += (anchorPos[i] - posBox1[i])*(anchorPos[i] - posBox1[i]);
+                  sizeP[0] = sqrt(sizeP[0]);
+                  dVector3 posAxisP;
+                  for (int i=0; i<3; ++i)
+                    posAxisP[i] = posBox1[i] + (anchorPos[i] - posBox1[i])/2.0;
+                  dsDrawBox (posAxisP, rotBox1, sizeP);
+                }
+                
+
+                // Draw the axisR 
+                if (PRISMATIC_ONLY != flag )
+                {
+                  dsSetColor (0,1,0);
+                  dVector3 sizeR = {0, 0.1, 0.1};
+                  for (int i=0; i<3; ++i)
+                    sizeR[0] += (anchorPos[i] - posBox2[i])*(anchorPos[i] - posBox2[i]);
+                  sizeR[0] = sqrt(sizeR[0]);
+                  dVector3 posAxisR;
+                  for (int i=0; i<3; ++i)
+                    posAxisR[i] = posBox2[i] + (anchorPos[i] - posBox2[i])/2.0;
+                  dsDrawBox (posAxisR, rotBox2, sizeR);
+                }
+
+                dSpaceCollide (space,0,&nearCallback);
+                dWorldQuickStep (world,0.0001);
+                update();
+                dJointGroupEmpty (contactgroup);
+          }
+}
+
+
+void Help(char **argv)
+{
+  printf("%s ", argv[0]);
+  printf(" -h | --help : print this help\n");
+  printf(" -b | --both : Display how the complete joint works\n");
+  printf("               Default behavior\n");
+  printf(" -p | --prismatic-only : Display how the prismatic part works\n");
+  printf("                         The anchor pts is set at the center of body 2\n");
+  printf(" -r | --rotoide-only   : Display how the rotoide part works\n");
+  printf("                         The anchor pts is set at the center of body 1\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(!flag && (0 == strcmp("-p", argv[i]) ||0 == strcmp("--prismatic-only", argv[i])) )
+        flag = PRISMATIC_ONLY;
+      
+      if(!flag && (0 == strcmp("-r", argv[i]) || 0 == strcmp("--rotoide-only", argv[i])) )
+        flag = ROTOIDE_ONLY;
+      
+      if(0 == strcmp("-t", argv[i]) || 0 == strcmp("--texture-path", argv[i]))
+      {
+        int j = i+1;
+        if ( j+1 > argc      ||  // Check if we have enough arguments
+             argv[j] == '\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
+      }
+    }
+  }
+  
+  // create world
+  world = dWorldCreate();
+  space = dHashSpaceCreate (0);
+  contactgroup = dJointGroupCreate (0);
+  dWorldSetGravity (world,0,0,-10);
+  ground = dCreatePlane (space,0,0,1,0);
+  
+  //create two boxes
+  dMass m;
+  box1_body[0] = dBodyCreate (world);
+  dMassSetBox (&m,1,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT);
+  dMassAdjust (&m,Mass1);
+  dBodySetMass (box1_body[0],&m);
+  box1[0] = dCreateBox (0,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT);
+  dGeomSetBody (box1[0],box1_body[0]);
+  
+  box2_body[0] = dBodyCreate (world);
+  dMassSetBox (&m,10,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT);
+  dMassAdjust (&m,Mass2);
+  dBodySetMass (box2_body[0],&m);
+  box2[0] = dCreateBox (0,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT);
+  dGeomSetBody (box2[0],box2_body[0]);
+  
+  //set the initial positions of body1 and body2
+  dMatrix3 R;
+  dRSetIdentity(R);
+  dBodySetPosition (box1_body[0],0,0,BOX1_HEIGHT/2.0);
+  dBodySetRotation (box1_body[0], R);
+  
+  dBodySetPosition (box2_body[0],
+                    2.1, 
+                    0.0,
+                    BOX2_HEIGHT/2.0);
+  dBodySetRotation (box2_body[0], R);
+  
+  
+  //set PR joint
+  joint[0] = dJointCreatePR(world,0);
+  dJointAttach (joint[0],box1_body[0],box2_body[0]);  
+  switch (flag)
+  {
+    case PRISMATIC_ONLY:
+      dJointSetPRAnchor (joint[0], 
+                         2.1, 
+                         0.0,
+                         BOX2_HEIGHT/2.0);
+      dJointSetPRParam (joint[0],dParamLoStop, -0.5);
+      dJointSetPRParam (joint[0],dParamHiStop, 1.5);
+      break;
+      
+    case ROTOIDE_ONLY:
+      dJointSetPRAnchor (joint[0], 
+                         0.0, 
+                         0.0,
+                         BOX2_HEIGHT/2.0);
+      dJointSetPRParam (joint[0],dParamLoStop, 0.0);
+      dJointSetPRParam (joint[0],dParamHiStop, 0.0);
+      break;
+      
+    default:
+      dJointSetPRAnchor (joint[0], 
+                         1.1, 
+                         0.0,
+                         BOX2_HEIGHT/2.0);
+      dJointSetPRParam (joint[0],dParamLoStop, -0.5);
+      dJointSetPRParam (joint[0],dParamHiStop, 1.5);
+      break;
+  }
+  
+  dJointSetPRAxis1(joint[0],1,0,0);
+  dJointSetPRAxis2(joint[0],0,0,1);
+// We position  the 2 body
+// The position of the rotoide joint is on the second body so it can rotate on itself
+// and move along the X axis.
+// With this anchor 
+// - A force in X will move only the body 2 inside the low and hi limit
+//   of the prismatic
+// - A force in Y will make the 2 bodies to rotate around on the plane
+  
+  box1_space = dSimpleSpaceCreate (space);
+  dSpaceSetCleanup (box1_space,0);
+  dSpaceAdd(box1_space,box1[0]);
+  
+  // run simulation
+  dsSimulationLoop (argc,argv,400,300,&fn);
+  dJointGroupDestroy (contactgroup);
+  dSpaceDestroy (space);
+  dWorldDestroy (world);
+  return 0;
+}
+