/************************************************************************* * * * 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; }