ode/demo/demo_piston.cpp

   1 /*************************************************************************
   2  *                                                                       *
   3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
   4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   5  *                                                                       *
   6  * This library is free software; you can redistribute it and/or         *
   7  * modify it under the terms of EITHER:                                  *
   8  *   (1) The GNU Lesser General Public License as published by the Free  *
   9  *       Software Foundation; either version 2.1 of the License, or (at  *
  10  *       your option) any later version. The text of the GNU Lesser      *
  11  *       General Public License is included with this library in the     *
  12  *       file LICENSE.TXT.                                               *
  13  *   (2) The BSD-style license that is included with this library in     *
  14  *       the file LICENSE-BSD.TXT.                                       *
  15  *                                                                       *
  16  * This library is distributed in the hope that it will be useful,       *
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  20  *                                                                       *
  21  * Created by:    Remi Ricard                                            *
  22  *                (remi.ricard@simlog.com or papaDoc@videotron.ca)       *
  23  * Creation date: 2007/05/04                                             *
  24  *************************************************************************/
  25
  26 /*
  27   This program demonstrates how the Piston joint works.
  28
  29   A Piston joint enables the sliding of a body with respect to another body
  30   and the 2 bodies are free to rotate about the sliding axis.
  31
  32   - The yellow body is fixed to the world.
  33   - The yellow body and the blue body are attached by a Piston joint with
  34     the axis along the x direction.
  35   - The purple object is a geometry obstacle.
  36   - The red line is the representation of the prismatic axis
  37   - The orange line is the representation of the rotoide axis
  38   - The light blue ball is the anchor position
  39
  40   N.B. Many command options are available type -h to print them.
  41 */
  42
  43 #include <ode/ode.h>
  44 #include <drawstuff/drawstuff.h>
  45 #include <iostream>
  46 #include <math.h>
  47 #include "texturepath.h"
  48
  49 #ifdef _MSC_VER
  50 #pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
  51 #endif
  52 // select correct drawing functions
  53 #ifdef dDOUBLE
  54 #define dsDrawBox dsDrawBoxD
  55 #define dsDrawCylinder dsDrawCylinderD
  56 #define dsDrawCapsule dsDrawCapsuleD
  57 #define dsDrawSphere dsDrawSphereD
  58 #endif
  59
  60
  61 const dReal VEL_INC = 0.01; // Velocity increment
  62
  63 // physics parameters
  64 const dReal PI = 3.14159265358979323846264338327950288419716939937510;
  65 const dReal BODY1_LENGTH = 1.5;    // Size along the X axis
  66
  67 const dReal RADIUS = 0.2;
  68 const dReal AXIS_RADIUS = 0.01;
  69
  70
  71 #define X 0
  72 #define Y 1
  73 #define Z 2
  74
  75 enum INDEX
  76 {
  77     BODY1 = 0,
  78     BODY2,
  79     RECT,
  80     BOX,
  81     OBS,
  82     GROUND,
  83     NUM_PARTS,
  84     ALL = NUM_PARTS
  85 };
  86
  87 const int catBits[NUM_PARTS+1] =
  88 {
  89     0x0001, ///< Ext Cylinder category
  90     0x0002, ///< Int Cylinder category
  91     0x0004, ///< Int_Rect Cylinder category
  92     0x0008, ///< Box category
  93     0x0010, ///< Obstacle category
  94     0x0020, ///< Ground category
  95     ~0L    ///< All categories
  96 };
  97
  98 #define Mass1 10
  99 #define Mass2 8
 100
 101
 102 //camera view
 103 static float xyz[3] = {2.0f,-3.5f,2.0000f};
 104 static float hpr[3] = {90.000f,-25.5000f,0.0000f};
 105
 106
 107 //world,space,body & geom
 108 static dWorldID         world;
 109 static dSpaceID         space;
 110 static dJointGroupID    contactgroup;
 111 static dBodyID          body[NUM_PARTS];
 112 static dGeomID          geom[NUM_PARTS];
 113
 114 // Default Positions and anchor of the 2 bodies
 115 dVector3 pos1;
 116 dVector3 pos2;
 117 dVector3 anchor;
 118
 119 static dJoint *joint;
 120
 121
 122 const dReal BODY2_SIDES[3] = {0.4, 0.4, 0.4};
 123 const dReal OBS_SIDES[3]  = {1,1,1};
 124 const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2};
 125
 126
 127 int type = dJointTypePiston;
 128
 129 //#pragma message("tc to be changed to 0")
 130
 131 int tc = 0; // The test case choice;
 132
 133
 134 //collision detection
 135 static void nearCallback (void *, dGeomID o1, dGeomID o2)
 136 {
 137     int i,n;
 138
 139     dBodyID b1 = dGeomGetBody (o1);
 140     dBodyID b2 = dGeomGetBody (o2);
 141     if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact) ) return;
 142     const int N = 10;
 143     dContact contact[N];
 144     n = dCollide (o1,o2,N,&contact[0].geom,sizeof (dContact) );
 145     if (n > 0)
 146     {
 147         for  (i=0; i<n; i++)
 148         {
 149             contact[i].surface.mode = (dContactSlip1 | dContactSlip2 |
 150                                        dContactSoftERP | dContactSoftCFM |
 151                                        dContactApprox1);
 152             contact[i].surface.mu = 0.1;
 153             contact[i].surface.slip1 = 0.02;
 154             contact[i].surface.slip2 = 0.02;
 155             contact[i].surface.soft_erp = 0.1;
 156             contact[i].surface.soft_cfm = 0.0001;
 157             dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
 158             dJointAttach (c,dGeomGetBody (contact[i].geom.g1),dGeomGetBody (contact[i].geom.g2) );
 159         }
 160     }
 161 }
 162
 163 static void printKeyBoardShortCut()
 164 {
 165     printf ("Press 'h' for this help.\n");
 166     printf ("Press 'q' to add force on BLUE body along positive x direction.\n");
 167     printf ("Press 'w' to add force on BLUE body along negative x direction.\n");
 168
 169     printf ("Press 'a' to add force on BLUE body along positive y direction.\n");
 170     printf ("Press 's' to add force on BLUE body along negative y direction.\n");
 171
 172     printf ("Press 'z' to add force on BLUE body along positive z direction.\n");
 173     printf ("Press 'x' to add force on BLUE body along negative z direction.\n");
 174
 175     printf ("Press 'e' to add torque on BLUE body around positive x direction \n");
 176     printf ("Press 'r' to add torque on BLUE body around negative x direction \n");
 177
 178     printf ("Press 'd' to add torque on BLUE body around positive y direction \n");
 179     printf ("Press 'f' to add torque on BLUE body around negative y direction \n");
 180
 181     printf ("Press 'c' to add torque on BLUE body around positive z direction \n");
 182     printf ("Press 'v' to add torque on BLUE body around negative z direction \n");
 183
 184     printf ("Press 't' to add force on prismatic joint in the positive axis direction\n");
 185     printf ("Press 'y' to add force on prismatic joint in the negative axis direction\n");
 186
 187     printf ("Press 'i' to add limits on the prismatic joint (0 to 0) \n");
 188     printf ("Press 'o' to add limits on the rotoide joint (0 to 0)\n");
 189     printf ("Press 'k' to add limits on the rotoide joint (-45 to 45deg) \n");
 190     printf ("Press 'l' to remove limits on the rotoide joint \n");
 191
 192
 193     printf ("Press '.' to increase joint velocity along the prismatic direction.\n");
 194     printf ("Press ',' to decrease joint velocity along the prismatic direction.\n");
 195
 196     printf ("Press 'p' to print the Position of the joint.\n");
 197
 198     printf ("Press '+' Go to the next test case.\n");
 199     printf ("Press '-' Go to the previous test case.\n");
 200
 201     printf ("Press '8' To remove one of the body. The blue body and the world will be\n");
 202     printf ("          attached to the joint (blue body at position 1)\n");
 203     printf ("Press '9' To remove one of the body. The blue body and the world will be\n");
 204     printf ("          attached to the joint (body body at position 2)\n");
 205
 206
 207 }
 208
 209
 210 // start simulation - set viewpoint
 211 static void start()
 212 {
 213     dAllocateODEDataForThread(dAllocateMaskAll);
 214
 215     dsSetViewpoint (xyz,hpr);
 216     printf ("This program demonstrates how the Piston joint works.\n");
 217     printf ("A Piston joint enables the sliding of a body with respect to another body\n");
 218     printf ("and the 2 bodies are free to rotate about the sliding axis.\n\n");
 219     printf ("The yellow body is fixed to the world\n");
 220     printf ("The yellow body and the blue body are attached by a Piston joint with\n");
 221     printf ("the axis along the x direction.\n");
 222     printf ("The purple object is a geometry obstacle.\n");
 223
 224     printKeyBoardShortCut();
 225 }
 226
 227
 228 void setPositionBodies (int val)
 229 {
 230     const dVector3 POS1 = {0,0,1.5,0};
 231     const dVector3 POS2 = {0,0,1.5,0};
 232     const dVector3 ANCHOR = {0,0,1.5,0};
 233
 234     for (int i=0; i<3; ++i)
 235     {
 236         pos1[i] = POS1[i];
 237         pos2[i] = POS2[i];
 238         anchor[i] = ANCHOR[i];
 239     }
 240
 241     if (body[BODY1])
 242     {
 243         dBodySetLinearVel (body[BODY1], 0,0,0);
 244         dBodySetAngularVel (body[BODY1], 0,0,0);
 245     }
 246
 247     if (body[BODY2])
 248     {
 249         dBodySetLinearVel (body[BODY2], 0,0,0);
 250         dBodySetAngularVel (body[BODY2], 0,0,0);
 251     }
 252
 253     switch (val)
 254     {
 255     case 3:
 256         pos1[Z] += -0.5;
 257         anchor[Z] -= 0.25;
 258         break;
 259     case 2:
 260         pos1[Z] -= 0.5;
 261         anchor[Z] -= 0.5;
 262         break;
 263     case 1:
 264         pos1[Z] += -0.5;
 265         break;
 266     default: // This is also case 0
 267         // Nothing to be done
 268         break;
 269     }
 270
 271     const dMatrix3 R =
 272     {
 273         1,0,0,0,
 274         0,1,0,0,
 275         0,0,1,0
 276     };
 277
 278     if (body[BODY1])
 279     {
 280         dBodySetPosition (body[BODY1], pos1[X], pos1[Y], pos1[Z]);
 281         dBodySetRotation (body[BODY1], R);
 282     }
 283
 284     if (body[BODY2])
 285     {
 286         dBodySetPosition (body[BODY2], pos2[X], pos2[Y], pos2[Z]);
 287         dBodySetRotation (body[BODY2], R);
 288     }
 289
 290
 291
 292     if (joint)
 293     {
 294         joint->attach (body[BODY1], body[BODY2]);
 295         if (joint->getType() == dJointTypePiston)
 296             dJointSetPistonAnchor(joint->id(), anchor[X], anchor[Y], anchor[Z]);
 297     }
 298
 299 }
 300
 301
 302 // function to update camera position at each step.
 303 void update()
 304 {
 305 //   static FILE *file = fopen("x:/sim/src/libode/tstsrcSF/export.dat", "w");
 306
 307 //   static int cnt = 0;
 308 //   char str[24];
 309 //   sprintf(str, "%06d",cnt++);
 310
 311 //   dWorldExportDIF(world, file, str);
 312 }
 313
 314
 315 // called when a key pressed
 316 static void command (int cmd)
 317 {
 318     switch (cmd)
 319     {
 320     case 'h' :
 321     case 'H' :
 322     case '?' :
 323         printKeyBoardShortCut();
 324         break;
 325
 326         // Force
 327     case 'q' :
 328     case 'Q' :
 329         dBodyAddForce (body[BODY1],4,0,0);
 330         break;
 331     case 'w' :
 332     case 'W' :
 333         dBodyAddForce (body[BODY1],-4,0,0);
 334         break;
 335
 336     case 'a' :
 337     case 'A' :
 338         dBodyAddForce (body[BODY1],0,40,0);
 339         break;
 340     case 's' :
 341     case 'S' :
 342         dBodyAddForce (body[BODY1],0,-40,0);
 343         break;
 344
 345     case 'z' :
 346     case 'Z' :
 347         dBodyAddForce (body[BODY1],0,0,4);
 348         break;
 349     case 'x' :
 350     case 'X' :
 351         dBodyAddForce (body[BODY1],0,0,-4);
 352         break;
 353
 354         // Torque
 355     case 'e':
 356     case 'E':
 357         dBodyAddTorque (body[BODY1],0.1,0,0);
 358         break;
 359     case 'r':
 360     case 'R':
 361         dBodyAddTorque (body[BODY1],-0.1,0,0);
 362         break;
 363
 364     case 'd':
 365     case 'D':
 366         dBodyAddTorque (body[BODY1],0, 0.1,0);
 367         break;
 368     case 'f':
 369     case 'F':
 370         dBodyAddTorque (body[BODY1],0,-0.1,0);
 371         break;
 372
 373     case 'c':
 374     case 'C':
 375         dBodyAddTorque (body[BODY1],0.1,0,0);
 376         break;
 377     case 'v':
 378     case 'V':
 379         dBodyAddTorque (body[BODY1],-0.1,0,0);
 380         break;
 381
 382     case 't':
 383     case 'T':
 384         if (joint->getType() == dJointTypePiston)
 385             dJointAddPistonForce (joint->id(),1);
 386         else
 387             dJointAddSliderForce (joint->id(),1);
 388         break;
 389     case 'y':
 390     case 'Y':
 391         if (joint->getType() == dJointTypePiston)
 392             dJointAddPistonForce (joint->id(),-1);
 393         else
 394             dJointAddSliderForce (joint->id(),-1);
 395         break;
 396
 397
 398     case '8' :
 399         dJointAttach(joint->id(), body[0], 0);
 400         break;
 401     case '9' :
 402         dJointAttach(joint->id(), 0, body[0]);
 403         break;
 404
 405     case 'i':
 406     case 'I' :
 407         joint->setParam (dParamLoStop, 0);
 408         joint->setParam (dParamHiStop, 0);
 409         break;
 410
 411     case 'o':
 412     case 'O' :
 413         joint->setParam (dParamLoStop2, 0);
 414         joint->setParam (dParamHiStop2, 0);
 415         break;
 416
 417     case 'k':
 418     case 'K':
 419         joint->setParam (dParamLoStop2, -45.0*3.14159267/180.0);
 420         joint->setParam (dParamHiStop2,  45.0*3.14159267/180.0);
 421         break;
 422     case 'l':
 423     case 'L':
 424         joint->setParam (dParamLoStop2, -dInfinity);
 425         joint->setParam (dParamHiStop2, dInfinity);
 426         break;
 427
 428         // Velocity of joint
 429     case ',':
 430     case '<' :
 431     {
 432         dReal vel = joint->getParam (dParamVel) - VEL_INC;
 433         joint->setParam (dParamVel, vel);
 434         std::cout<<"Velocity = "<<vel<<"  FMax = 2"<<'\n';
 435     }
 436     break;
 437
 438     case '.':
 439     case '>' :
 440     {
 441         dReal vel = joint->getParam (dParamVel) + VEL_INC;
 442         joint->setParam (dParamVel, vel);
 443         std::cout<<"Velocity = "<<vel<<"  FMax = 2"<<'\n';
 444     }
 445     break;
 446
 447     case 'p' :
 448     case 'P' :
 449     {
 450         switch (joint->getType() )
 451         {
 452         case dJointTypeSlider :
 453         {
 454             dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
 455             std::cout<<"Position ="<<sj->getPosition() <<"\n";
 456         }
 457         break;
 458         case dJointTypePiston :
 459         {
 460             dPistonJoint *rj = reinterpret_cast<dPistonJoint *> (joint);
 461             std::cout<<"Position ="<<rj->getPosition() <<"\n";
 462         }
 463         break;
 464         default:
 465         {} // keep the compiler happy
 466         }
 467     }
 468     break;
 469
 470     case '+' :
 471         (++tc) %= 4;
 472         setPositionBodies (tc);
 473         break;
 474     case '-' :
 475         (--tc) %= 4;
 476         setPositionBodies (tc);
 477         break;
 478
 479
 480     }
 481 }
 482
 483 static void drawBox (dGeomID id, int R, int G, int B)
 484 {
 485     if (!id)
 486         return;
 487
 488     const dReal *pos = dGeomGetPosition (id);
 489     const dReal *rot = dGeomGetRotation (id);
 490     dsSetColor (R,G,B);
 491
 492     dVector3 l;
 493     dGeomBoxGetLengths (id, l);
 494     dsDrawBox (pos, rot, l);
 495 }
 496
 497
 498 // simulation loop
 499 static void simLoop (int pause)
 500 {
 501     const dReal *rot;
 502     dVector3 ax;
 503     dReal l=0;
 504
 505     switch (joint->getType() )
 506     {
 507     case dJointTypeSlider :
 508         ( (dSliderJoint *) joint)->getAxis (ax);
 509         l = ( (dSliderJoint *) joint)->getPosition();
 510         break;
 511     case dJointTypePiston :
 512         ( (dPistonJoint *) joint)->getAxis (ax);
 513         l = ( (dPistonJoint *) joint)->getPosition();
 514         break;
 515     default:
 516     {} // keep the compiler happy
 517     }
 518
 519
 520     if (!pause)
 521     {
 522         double simstep = 0.01; // 1ms simulation steps
 523         double dt = dsElapsedTime();
 524
 525         int nrofsteps = (int) ceilf (dt/simstep);
 526         if (!nrofsteps)
 527             nrofsteps = 1;
 528
 529         for (int i=0; i<nrofsteps && !pause; i++)
 530         {
 531             dSpaceCollide (space,0,&nearCallback);
 532             dWorldStep (world, simstep);
 533
 534             dJointGroupEmpty (contactgroup);
 535         }
 536
 537         update();
 538
 539
 540         dReal radius, length;
 541
 542         dsSetTexture (DS_WOOD);
 543
 544         drawBox (geom[BODY2], 1,1,0);
 545
 546         drawBox (geom[RECT], 0,0,1);
 547
 548         if ( geom[BODY1] )
 549         {
 550             const dReal *pos = dGeomGetPosition (geom[BODY1]);
 551             rot = dGeomGetRotation (geom[BODY1]);
 552             dsSetColor (0,0,1);
 553
 554             dGeomCapsuleGetParams (geom[BODY1], &radius, &length);
 555             dsDrawCapsule (pos, rot, length, radius);
 556         }
 557
 558
 559         drawBox (geom[OBS], 1,0,1);
 560
 561
 562         // Draw the prismatic axis
 563         if ( geom[BODY1] )
 564         {
 565             const dReal *pos = dGeomGetPosition (geom[BODY1]);
 566             rot = dGeomGetRotation (geom[BODY2]);
 567             dVector3 p;
 568             p[X] = pos[X] - l*ax[X];
 569             p[Y] = pos[Y] - l*ax[Y];
 570             p[Z] = pos[Z] - l*ax[Z];
 571             dsSetColor (1,0,0);
 572             dsDrawCylinder (p, rot, 3.75, 1.05*AXIS_RADIUS);
 573         }
 574
 575
 576         if (joint->getType() == dJointTypePiston )
 577         {
 578             dVector3 anchor;
 579             dJointGetPistonAnchor(joint->id(), anchor);
 580
 581             // Draw the rotoide axis
 582             rot = dGeomGetRotation (geom[BODY2]);
 583             dsSetColor (1,0.5,0);
 584             dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS);
 585
 586
 587             dsSetColor (0,1,1);
 588             rot = dGeomGetRotation (geom[BODY1]);
 589             dsDrawSphere (anchor, rot, 1.5*RADIUS);
 590         }
 591
 592     }
 593 }
 594
 595
 596 void Help (char **argv)
 597 {
 598     printf ("%s ", argv[0]);
 599     printf (" -h | --help   : print this help\n");
 600     printf (" -s | --slider : Set the joint as a slider\n");
 601     printf (" -p | --piston : Set the joint as a Piston. (Default joint)\n");
 602     printf (" -1 | --offset1 : Create an offset between the 2 bodies\n");
 603     printf ("                  Offset one of the body by z=-0.5 and keep the anchor\n");
 604     printf ("                  point in the middle of the fixed body\n");
 605     printf (" -2 | --offset2 : Create an offset between the 2 bodies\n");
 606     printf ("                  Offset one of the body by z=-0.5 and set the anchor\n");
 607     printf ("                  point in the middle of the movable body\n");
 608     printf (" -3 | --offset3 : Create an offset between the 2 bodies\n");
 609     printf ("                  Offset one of the body by z=-0.5 and set the anchor\n");
 610     printf ("                  point in the middle of the 2 bodies\n");
 611     printf (" -t | --texture-path path  : Path to the texture.\n");
 612     printf ("                             Default = %s\n", DRAWSTUFF_TEXTURE_PATH);
 613     printf (" -n | --notFixed : In free space with no gravity mode");
 614     printf ("-notex          : Don't use texture\n");
 615     printf ("-noshadow       : No shadow\n");
 616     printf ("-noshadows      : No shadows\n");
 617     printf ("-pause          : Initial pause\n");
 618     printf ("--------------------------------------------------\n");
 619     printf ("Hit any key to continue:");
 620     getchar();
 621
 622     exit (0);
 623 }
 624
 625 int main (int argc, char **argv)
 626 {
 627     dInitODE2(0);
 628     bool fixed  = true;
 629
 630     // setup pointers to drawstuff callback functions
 631     dsFunctions fn;
 632     fn.version = DS_VERSION;
 633     fn.start = &start;
 634     fn.step = &simLoop;
 635     fn.command = &command;
 636     fn.stop = 0;
 637     fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
 638
 639     dVector3 offset;
 640     dSetZero (offset, 4);
 641
 642     // Default test case
 643
 644     if (argc >= 2 )
 645     {
 646         for (int i=1; i < argc; ++i)
 647         {
 648             //static int tata = 0;
 649
 650             if (1)
 651             {
 652                 if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
 653                     Help (argv);
 654
 655                 if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) )
 656                     type = dJointTypeSlider;
 657
 658                 if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) )
 659                 {
 660                     int j = i+1;
 661                     if ( j >= argc      ||  // Check if we have enough arguments
 662                             argv[j][0] == '\0' ||  // We should have a path here
 663                             argv[j][0] == '-' ) // We should have a path not a command line
 664                         Help (argv);
 665                     else
 666                         fn.path_to_textures = argv[++i]; // Increase i since we use this argument
 667                 }
 668             }
 669
 670
 671             if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) )
 672                 tc = 1;
 673
 674             if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) )
 675                 tc = 2;
 676
 677             if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) )
 678                 tc = 3;
 679
 680             if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) )
 681                 fixed = false;
 682         }
 683     }
 684
 685     world = dWorldCreate();
 686     dWorldSetERP (world, 0.8);
 687
 688     space = dSimpleSpaceCreate (0);
 689     contactgroup = dJointGroupCreate (0);
 690     geom[GROUND] = dCreatePlane (space, 0,0,1,0);
 691     dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
 692     dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
 693
 694     dMass m;
 695     dMatrix3 R;
 696
 697
 698     // Create the Obstacle
 699     geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]);
 700     dGeomSetCategoryBits (geom[OBS], catBits[OBS]);
 701     dGeomSetCollideBits (geom[OBS], catBits[ALL]);
 702     //Rotation of 45deg around y
 703     dRFromAxisAndAngle (R, 1,1,0, -0.25*PI);
 704     dGeomSetRotation (geom[OBS], R);
 705     dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5);
 706
 707
 708     //Rotation of 90deg around y
 709     // Will orient the Z axis along X
 710     dRFromAxisAndAngle (R, 0,1,0, -0.5*PI);
 711
 712
 713     // Create Body2 (Wiil be attached to the world)
 714     body[BODY2] = dBodyCreate (world);
 715     // Main axis of cylinder is along X=1
 716     dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
 717     dMassAdjust (&m, Mass1);
 718     geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
 719     dGeomSetBody (geom[BODY2], body[BODY2]);
 720     dGeomSetOffsetRotation (geom[BODY2], R);
 721     dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]);
 722     dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) );
 723     dBodySetMass (body[BODY2], &m);
 724
 725
 726     // Create Body 1 (Slider on the prismatic axis)
 727     body[BODY1] = dBodyCreate (world);
 728     // Main axis of capsule is along X=1
 729     dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH);
 730     dMassAdjust (&m, Mass1);
 731     geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH);
 732     dGeomSetBody (geom[BODY1], body[BODY1]);
 733     dGeomSetOffsetRotation (geom[BODY1], R);
 734     dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]);
 735     dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]);
 736
 737     dMass mRect;
 738     dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
 739     dMassAdd (&m, &mRect);
 740     // TODO: translate m?
 741     geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
 742     dGeomSetBody (geom[RECT], body[BODY1]);
 743     dGeomSetOffsetPosition (geom[RECT],
 744                             (BODY1_LENGTH-RECT_SIDES[0]) /2.0,
 745                             0.0,
 746                             -RADIUS -RECT_SIDES[2]/2.0);
 747     dGeomSetCategoryBits (geom[RECT], catBits[RECT]);
 748     dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) );
 749
 750     dBodySetMass (body[BODY1], &m);
 751
 752
 753
 754     setPositionBodies (tc);
 755
 756
 757     if ( fixed )
 758     {
 759         // Attache external cylinder to the world
 760         dJointID fixed = dJointCreateFixed (world,0);
 761         dJointAttach (fixed , NULL, body[BODY2]);
 762         dJointSetFixed (fixed );
 763         dWorldSetGravity (world,0,0,-0.8);
 764     }
 765     else
 766     {
 767         dWorldSetGravity (world,0,0,0);
 768     }
 769
 770
 771
 772
 773     // The static is here only to help debugging
 774     switch (type)
 775     {
 776     case dJointTypeSlider :
 777     {
 778         dSliderJoint *sj = new dSliderJoint (world, 0);
 779         sj->attach (body[BODY1], body[BODY2]);
 780         sj->setAxis (1, 0, 0);
 781         joint = sj;
 782     }
 783     break;
 784
 785     case dJointTypePiston : // fall through default
 786     default:
 787     {
 788         dPistonJoint *pj = new dPistonJoint (world, 0);
 789         pj->attach (body[BODY1], body[BODY2]);
 790         pj->setAxis (1, 0, 0);
 791
 792         dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]);
 793
 794         joint = pj;
 795     }
 796     break;
 797     };
 798
 799
 800     // run simulation
 801     dsSimulationLoop (argc,argv,400,300,&fn);
 802
 803     delete joint;
 804     dJointGroupDestroy (contactgroup);
 805     dSpaceDestroy (space);
 806     dWorldDestroy (world);
 807     dCloseODE();
 808     return 0;
 809 }
 810
 811
 812
 813