summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/demo/demo_jointPR.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/ode/demo/demo_jointPR.cpp')
-rw-r--r--libs/ode-0.16.1/ode/demo/demo_jointPR.cpp434
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;
+}
+
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 19:44:49 +0000