summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/demo/demo_piston.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/ode/demo/demo_piston.cpp')
-rw-r--r--libs/ode-0.16.1/ode/demo/demo_piston.cpp813
1 files changed, 813 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/demo/demo_piston.cpp b/libs/ode-0.16.1/ode/demo/demo_piston.cpp
new file mode 100644
index 0000000..8a0453a
--- /dev/null
+++ b/libs/ode-0.16.1/ode/demo/demo_piston.cpp
@@ -0,0 +1,813 @@
+/*************************************************************************
+ * *
+ * 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. *
+ * *
+ * Created by: Remi Ricard *
+ * (remi.ricard@simlog.com or papaDoc@videotron.ca) *
+ * Creation date: 2007/05/04 *
+ *************************************************************************/
+
+/*
+ This program demonstrates how the Piston joint works.
+
+ A Piston joint enables the sliding of a body with respect to another body
+ and the 2 bodies are free to rotate about the sliding axis.
+
+ - The yellow body is fixed to the world.
+ - The yellow body and the blue body are attached by a Piston joint with
+ the axis along the x direction.
+ - The purple object is a geometry obstacle.
+ - The red line is the representation of the prismatic axis
+ - The orange line is the representation of the rotoide axis
+ - The light blue ball is the anchor position
+
+ N.B. Many command options are available type -h to print them.
+*/
+
+#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
+#define dsDrawSphere dsDrawSphereD
+#endif
+
+
+const dReal VEL_INC = 0.01; // Velocity increment
+
+// physics parameters
+const dReal PI = 3.14159265358979323846264338327950288419716939937510;
+const dReal BODY1_LENGTH = 1.5; // Size along the X axis
+
+const dReal RADIUS = 0.2;
+const dReal AXIS_RADIUS = 0.01;
+
+
+#define X 0
+#define Y 1
+#define Z 2
+
+enum INDEX
+{
+ BODY1 = 0,
+ BODY2,
+ RECT,
+ BOX,
+ OBS,
+ GROUND,
+ NUM_PARTS,
+ ALL = NUM_PARTS
+};
+
+const int catBits[NUM_PARTS+1] =
+{
+ 0x0001, ///< Ext Cylinder category
+ 0x0002, ///< Int Cylinder category
+ 0x0004, ///< Int_Rect Cylinder category
+ 0x0008, ///< Box category
+ 0x0010, ///< Obstacle category
+ 0x0020, ///< Ground category
+ ~0L ///< All categories
+};
+
+#define Mass1 10
+#define Mass2 8
+
+
+//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 dJointGroupID contactgroup;
+static dBodyID body[NUM_PARTS];
+static dGeomID geom[NUM_PARTS];
+
+// Default Positions and anchor of the 2 bodies
+dVector3 pos1;
+dVector3 pos2;
+dVector3 anchor;
+
+static dJoint *joint;
+
+
+const dReal BODY2_SIDES[3] = {0.4, 0.4, 0.4};
+const dReal OBS_SIDES[3] = {1,1,1};
+const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2};
+
+
+int type = dJointTypePiston;
+
+//#pragma message("tc to be changed to 0")
+
+int tc = 0; // The test case choice;
+
+
+//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) );
+ }
+ }
+}
+
+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 't' to add force on prismatic joint in the positive axis direction\n");
+ printf ("Press 'y' to add force on prismatic joint in the negative axis direction\n");
+
+ printf ("Press 'i' to add limits on the prismatic joint (0 to 0) \n");
+ printf ("Press 'o' to add limits on the rotoide joint (0 to 0)\n");
+ printf ("Press 'k' to add limits on the rotoide joint (-45 to 45deg) \n");
+ printf ("Press 'l' to remove limits on the rotoide joint \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 'p' to print the Position of the joint.\n");
+
+ printf ("Press '+' Go to the next test case.\n");
+ printf ("Press '-' Go to the previous test case.\n");
+
+ printf ("Press '8' To remove one of the body. The blue body and the world will be\n");
+ printf (" attached to the joint (blue body at position 1)\n");
+ printf ("Press '9' To remove one of the body. The blue body and the world will be\n");
+ printf (" attached to the joint (body body at position 2)\n");
+
+
+}
+
+
+// start simulation - set viewpoint
+static void start()
+{
+ dAllocateODEDataForThread(dAllocateMaskAll);
+
+ dsSetViewpoint (xyz,hpr);
+ printf ("This program demonstrates how the Piston joint works.\n");
+ printf ("A Piston joint enables the sliding of a body with respect to another body\n");
+ printf ("and the 2 bodies are free to rotate about the sliding axis.\n\n");
+ printf ("The yellow body is fixed to the world\n");
+ printf ("The yellow body and the blue body are attached by a Piston joint with\n");
+ printf ("the axis along the x direction.\n");
+ printf ("The purple object is a geometry obstacle.\n");
+
+ printKeyBoardShortCut();
+}
+
+
+void setPositionBodies (int val)
+{
+ const dVector3 POS1 = {0,0,1.5,0};
+ const dVector3 POS2 = {0,0,1.5,0};
+ const dVector3 ANCHOR = {0,0,1.5,0};
+
+ for (int i=0; i<3; ++i)
+ {
+ pos1[i] = POS1[i];
+ pos2[i] = POS2[i];
+ anchor[i] = ANCHOR[i];
+ }
+
+ if (body[BODY1])
+ {
+ dBodySetLinearVel (body[BODY1], 0,0,0);
+ dBodySetAngularVel (body[BODY1], 0,0,0);
+ }
+
+ if (body[BODY2])
+ {
+ dBodySetLinearVel (body[BODY2], 0,0,0);
+ dBodySetAngularVel (body[BODY2], 0,0,0);
+ }
+
+ switch (val)
+ {
+ case 3:
+ pos1[Z] += -0.5;
+ anchor[Z] -= 0.25;
+ break;
+ case 2:
+ pos1[Z] -= 0.5;
+ anchor[Z] -= 0.5;
+ break;
+ case 1:
+ pos1[Z] += -0.5;
+ break;
+ default: // This is also case 0
+ // Nothing to be done
+ break;
+ }
+
+ const dMatrix3 R =
+ {
+ 1,0,0,0,
+ 0,1,0,0,
+ 0,0,1,0
+ };
+
+ if (body[BODY1])
+ {
+ dBodySetPosition (body[BODY1], pos1[X], pos1[Y], pos1[Z]);
+ dBodySetRotation (body[BODY1], R);
+ }
+
+ if (body[BODY2])
+ {
+ dBodySetPosition (body[BODY2], pos2[X], pos2[Y], pos2[Z]);
+ dBodySetRotation (body[BODY2], R);
+ }
+
+
+
+ if (joint)
+ {
+ joint->attach (body[BODY1], body[BODY2]);
+ if (joint->getType() == dJointTypePiston)
+ dJointSetPistonAnchor(joint->id(), anchor[X], anchor[Y], anchor[Z]);
+ }
+
+}
+
+
+// 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[BODY1],4,0,0);
+ break;
+ case 'w' :
+ case 'W' :
+ dBodyAddForce (body[BODY1],-4,0,0);
+ break;
+
+ case 'a' :
+ case 'A' :
+ dBodyAddForce (body[BODY1],0,40,0);
+ break;
+ case 's' :
+ case 'S' :
+ dBodyAddForce (body[BODY1],0,-40,0);
+ break;
+
+ case 'z' :
+ case 'Z' :
+ dBodyAddForce (body[BODY1],0,0,4);
+ break;
+ case 'x' :
+ case 'X' :
+ dBodyAddForce (body[BODY1],0,0,-4);
+ break;
+
+ // Torque
+ case 'e':
+ case 'E':
+ dBodyAddTorque (body[BODY1],0.1,0,0);
+ break;
+ case 'r':
+ case 'R':
+ dBodyAddTorque (body[BODY1],-0.1,0,0);
+ break;
+
+ case 'd':
+ case 'D':
+ dBodyAddTorque (body[BODY1],0, 0.1,0);
+ break;
+ case 'f':
+ case 'F':
+ dBodyAddTorque (body[BODY1],0,-0.1,0);
+ break;
+
+ case 'c':
+ case 'C':
+ dBodyAddTorque (body[BODY1],0.1,0,0);
+ break;
+ case 'v':
+ case 'V':
+ dBodyAddTorque (body[BODY1],-0.1,0,0);
+ break;
+
+ case 't':
+ case 'T':
+ if (joint->getType() == dJointTypePiston)
+ dJointAddPistonForce (joint->id(),1);
+ else
+ dJointAddSliderForce (joint->id(),1);
+ break;
+ case 'y':
+ case 'Y':
+ if (joint->getType() == dJointTypePiston)
+ dJointAddPistonForce (joint->id(),-1);
+ else
+ dJointAddSliderForce (joint->id(),-1);
+ break;
+
+
+ case '8' :
+ dJointAttach(joint->id(), body[0], 0);
+ break;
+ case '9' :
+ dJointAttach(joint->id(), 0, body[0]);
+ break;
+
+ case 'i':
+ case 'I' :
+ joint->setParam (dParamLoStop, 0);
+ joint->setParam (dParamHiStop, 0);
+ break;
+
+ case 'o':
+ case 'O' :
+ joint->setParam (dParamLoStop2, 0);
+ joint->setParam (dParamHiStop2, 0);
+ break;
+
+ case 'k':
+ case 'K':
+ joint->setParam (dParamLoStop2, -45.0*3.14159267/180.0);
+ joint->setParam (dParamHiStop2, 45.0*3.14159267/180.0);
+ break;
+ case 'l':
+ case 'L':
+ joint->setParam (dParamLoStop2, -dInfinity);
+ joint->setParam (dParamHiStop2, dInfinity);
+ break;
+
+ // Velocity of joint
+ case ',':
+ case '<' :
+ {
+ dReal vel = joint->getParam (dParamVel) - VEL_INC;
+ joint->setParam (dParamVel, vel);
+ std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
+ }
+ break;
+
+ case '.':
+ case '>' :
+ {
+ dReal vel = joint->getParam (dParamVel) + VEL_INC;
+ joint->setParam (dParamVel, vel);
+ std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
+ }
+ break;
+
+ case 'p' :
+ case 'P' :
+ {
+ switch (joint->getType() )
+ {
+ case dJointTypeSlider :
+ {
+ dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
+ std::cout<<"Position ="<<sj->getPosition() <<"\n";
+ }
+ break;
+ case dJointTypePiston :
+ {
+ dPistonJoint *rj = reinterpret_cast<dPistonJoint *> (joint);
+ std::cout<<"Position ="<<rj->getPosition() <<"\n";
+ }
+ break;
+ default:
+ {} // keep the compiler happy
+ }
+ }
+ break;
+
+ case '+' :
+ (++tc) %= 4;
+ setPositionBodies (tc);
+ break;
+ case '-' :
+ (--tc) %= 4;
+ setPositionBodies (tc);
+ 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)
+{
+ const dReal *rot;
+ dVector3 ax;
+ dReal l=0;
+
+ switch (joint->getType() )
+ {
+ case dJointTypeSlider :
+ ( (dSliderJoint *) joint)->getAxis (ax);
+ l = ( (dSliderJoint *) joint)->getPosition();
+ break;
+ case dJointTypePiston :
+ ( (dPistonJoint *) joint)->getAxis (ax);
+ l = ( (dPistonJoint *) joint)->getPosition();
+ break;
+ default:
+ {} // keep the compiler happy
+ }
+
+
+ if (!pause)
+ {
+ double simstep = 0.01; // 1ms 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[BODY2], 1,1,0);
+
+ drawBox (geom[RECT], 0,0,1);
+
+ if ( geom[BODY1] )
+ {
+ const dReal *pos = dGeomGetPosition (geom[BODY1]);
+ rot = dGeomGetRotation (geom[BODY1]);
+ dsSetColor (0,0,1);
+
+ dGeomCapsuleGetParams (geom[BODY1], &radius, &length);
+ dsDrawCapsule (pos, rot, length, radius);
+ }
+
+
+ drawBox (geom[OBS], 1,0,1);
+
+
+ // Draw the prismatic axis
+ if ( geom[BODY1] )
+ {
+ const dReal *pos = dGeomGetPosition (geom[BODY1]);
+ rot = dGeomGetRotation (geom[BODY2]);
+ dVector3 p;
+ p[X] = pos[X] - l*ax[X];
+ p[Y] = pos[Y] - l*ax[Y];
+ p[Z] = pos[Z] - l*ax[Z];
+ dsSetColor (1,0,0);
+ dsDrawCylinder (p, rot, 3.75, 1.05*AXIS_RADIUS);
+ }
+
+
+ if (joint->getType() == dJointTypePiston )
+ {
+ dVector3 anchor;
+ dJointGetPistonAnchor(joint->id(), anchor);
+
+ // Draw the rotoide axis
+ rot = dGeomGetRotation (geom[BODY2]);
+ dsSetColor (1,0.5,0);
+ dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS);
+
+
+ dsSetColor (0,1,1);
+ rot = dGeomGetRotation (geom[BODY1]);
+ dsDrawSphere (anchor, rot, 1.5*RADIUS);
+ }
+
+ }
+}
+
+
+void Help (char **argv)
+{
+ printf ("%s ", argv[0]);
+ printf (" -h | --help : print this help\n");
+ printf (" -s | --slider : Set the joint as a slider\n");
+ printf (" -p | --piston : Set the joint as a Piston. (Default joint)\n");
+ printf (" -1 | --offset1 : Create an offset between the 2 bodies\n");
+ printf (" Offset one of the body by z=-0.5 and keep the anchor\n");
+ printf (" point in the middle of the fixed body\n");
+ printf (" -2 | --offset2 : Create an offset between the 2 bodies\n");
+ printf (" Offset one of the body by z=-0.5 and set the anchor\n");
+ printf (" point in the middle of the movable body\n");
+ printf (" -3 | --offset3 : Create an offset between the 2 bodies\n");
+ printf (" Offset one of the body by z=-0.5 and set the anchor\n");
+ printf (" point in the middle of the 2 bodies\n");
+ printf (" -t | --texture-path path : Path to the texture.\n");
+ printf (" Default = %s\n", DRAWSTUFF_TEXTURE_PATH);
+ printf (" -n | --notFixed : In free space with no gravity mode");
+ printf ("-notex : Don't use texture\n");
+ printf ("-noshadow : No shadow\n");
+ printf ("-noshadows : No shadows\n");
+ printf ("-pause : Initial pause\n");
+ printf ("--------------------------------------------------\n");
+ printf ("Hit any key to continue:");
+ getchar();
+
+ exit (0);
+}
+
+int main (int argc, char **argv)
+{
+ dInitODE2(0);
+ bool fixed = true;
+
+ // 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;
+
+ dVector3 offset;
+ dSetZero (offset, 4);
+
+ // Default test case
+
+ if (argc >= 2 )
+ {
+ for (int i=1; i < argc; ++i)
+ {
+ //static int tata = 0;
+
+ if (1)
+ {
+ if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
+ Help (argv);
+
+ if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) )
+ type = dJointTypeSlider;
+
+ 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
+ }
+ }
+
+
+ if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) )
+ tc = 1;
+
+ if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) )
+ tc = 2;
+
+ if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) )
+ tc = 3;
+
+ if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) )
+ fixed = false;
+ }
+ }
+
+ 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;
+ dMatrix3 R;
+
+
+ // Create the Obstacle
+ geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]);
+ dGeomSetCategoryBits (geom[OBS], catBits[OBS]);
+ dGeomSetCollideBits (geom[OBS], catBits[ALL]);
+ //Rotation of 45deg around y
+ dRFromAxisAndAngle (R, 1,1,0, -0.25*PI);
+ dGeomSetRotation (geom[OBS], R);
+ dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5);
+
+
+ //Rotation of 90deg around y
+ // Will orient the Z axis along X
+ dRFromAxisAndAngle (R, 0,1,0, -0.5*PI);
+
+
+ // Create Body2 (Wiil be attached to the world)
+ body[BODY2] = dBodyCreate (world);
+ // Main axis of cylinder is along X=1
+ dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
+ dMassAdjust (&m, Mass1);
+ geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
+ dGeomSetBody (geom[BODY2], body[BODY2]);
+ dGeomSetOffsetRotation (geom[BODY2], R);
+ dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]);
+ dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) );
+ dBodySetMass (body[BODY2], &m);
+
+
+ // Create Body 1 (Slider on the prismatic axis)
+ body[BODY1] = dBodyCreate (world);
+ // Main axis of capsule is along X=1
+ dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH);
+ dMassAdjust (&m, Mass1);
+ geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH);
+ dGeomSetBody (geom[BODY1], body[BODY1]);
+ dGeomSetOffsetRotation (geom[BODY1], R);
+ dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]);
+ dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]);
+
+ dMass mRect;
+ dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
+ dMassAdd (&m, &mRect);
+ // TODO: translate m?
+ geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
+ dGeomSetBody (geom[RECT], body[BODY1]);
+ dGeomSetOffsetPosition (geom[RECT],
+ (BODY1_LENGTH-RECT_SIDES[0]) /2.0,
+ 0.0,
+ -RADIUS -RECT_SIDES[2]/2.0);
+ dGeomSetCategoryBits (geom[RECT], catBits[RECT]);
+ dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) );
+
+ dBodySetMass (body[BODY1], &m);
+
+
+
+ setPositionBodies (tc);
+
+
+ if ( fixed )
+ {
+ // Attache external cylinder to the world
+ dJointID fixed = dJointCreateFixed (world,0);
+ dJointAttach (fixed , NULL, body[BODY2]);
+ dJointSetFixed (fixed );
+ dWorldSetGravity (world,0,0,-0.8);
+ }
+ else
+ {
+ dWorldSetGravity (world,0,0,0);
+ }
+
+
+
+
+ // The static is here only to help debugging
+ switch (type)
+ {
+ case dJointTypeSlider :
+ {
+ dSliderJoint *sj = new dSliderJoint (world, 0);
+ sj->attach (body[BODY1], body[BODY2]);
+ sj->setAxis (1, 0, 0);
+ joint = sj;
+ }
+ break;
+
+ case dJointTypePiston : // fall through default
+ default:
+ {
+ dPistonJoint *pj = new dPistonJoint (world, 0);
+ pj->attach (body[BODY1], body[BODY2]);
+ pj->setAxis (1, 0, 0);
+
+ dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]);
+
+ joint = pj;
+ }
+ break;
+ };
+
+
+ // run simulation
+ dsSimulationLoop (argc,argv,400,300,&fn);
+
+ delete joint;
+ dJointGroupDestroy (contactgroup);
+ dSpaceDestroy (space);
+ dWorldDestroy (world);
+ dCloseODE();
+ return 0;
+}
+
+
+
+
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 01:25:32 +0000