add ode

1 files changed, 434 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/demo/demo_jointPR.cpp b/libs/ode-0.16.1/ode/demo/demo_jointPR.cpp
new file mode 100644
index 0000000..b760af1
--- /dev/null
+++ b/libs/ode-0.16.1/ode/demo/demo_jointPR.cpp
@@ -0,0 +1,434 @@
+/*************************************************************************
+ *                                                                       *
+ * 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>
+#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
+#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 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 *, 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()
+{
+    dAllocateODEDataForThread(dAllocateMaskAll);
+
+    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");
+
+    printf("Press 'v' to give a defined velocity and add a FMax to the rotoide axis\n");
+    printf("Press 'c' to set the velocity to zero and remove the FMax\n");
+
+    printf("Press 'l' to add limits (-0.5 to 0.5rad) on the rotoide axis\n");
+    printf("Press 'k' to remove the limits on the rotoide axis\n");
+
+    printf("Press 'i' to get joint info\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;
+
+    case 'v':
+    case 'V':
+        dJointSetPRParam(joint[0], dParamVel2, 2);
+        dJointSetPRParam(joint[0], dParamFMax2, 500);
+        break;
+
+    case 'c':
+    case 'C':
+        dJointSetPRParam(joint[0], dParamVel2, 0);
+        dJointSetPRParam(joint[0], dParamFMax2, 0);
+        break;
+
+    case 'l':
+    case 'L':
+        dJointSetPRParam(joint[0], dParamLoStop2, -0.5);
+        dJointSetPRParam(joint[0], dParamHiStop2,  0.5);
+        break;
+
+    case 'k':
+    case 'K':
+        dJointSetPRParam(joint[0], dParamLoStop2, -dInfinity);
+        dJointSetPRParam(joint[0], dParamHiStop2,  dInfinity);
+        break;
+
+    case 'i':
+    case 'I':
+        dVector3 anchor;
+        dJointGetPRAnchor(joint[0], anchor);
+        dReal angle = dJointGetPRAngle(joint[0]);
+        dReal w = dJointGetPRAngleRate(joint[0]);
+
+        dReal l = dJointGetPRPosition(joint[0]);
+        dReal v = dJointGetPRPositionRate(joint[0]);
+
+        printf("Anchor: [%6.4f, %6.4f, %6.4f]\n", anchor[0], anchor[1], anchor[2]);
+        printf("Position: %7.4f, Rate: %7.4f\n", l, v);
+        printf("Angle: %7.4f, Rate: %7.4f\n", angle, w);
+        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 >= 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);
+
+    // 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);
+    dCloseODE();
+    return 0;
+}
+