ode/demo/demo_gyroscopic.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  *************************************************************************/
  22
  23 #include <ode/ode.h>
  24 #include <drawstuff/drawstuff.h>
  25 #include "texturepath.h"
  26
  27 #ifdef _MSC_VER
  28 #pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
  29 #endif
  30
  31 // select correct drawing functions
  32
  33 #ifdef dDOUBLE
  34 #define dsDrawBox dsDrawBoxD
  35 #define dsDrawSphere dsDrawSphereD
  36 #define dsDrawCylinder dsDrawCylinderD
  37 #define dsDrawCapsule dsDrawCapsuleD
  38 #define dsDrawConvex dsDrawConvexD
  39 #endif
  40
  41 bool write_world = false;
  42 bool show_contacts = false;
  43 dWorld * world;
  44 dBody *top1, *top2;
  45 dSpace *space;
  46 dJointGroup contactgroup;
  47
  48 const dReal pinradius = 0.05f;
  49 const dReal pinlength = 1.5f;
  50 const dReal topradius = 1.0f;
  51 const dReal toplength = 0.25f;
  52 const dReal topmass = 1.0f;
  53
  54 #define MAX_CONTACTS 4
  55
  56 static void nearCallback (void *, dGeomID o1, dGeomID o2)
  57 {
  58     // for drawing the contact points
  59     dMatrix3 RI;
  60     dRSetIdentity (RI);
  61     const dReal ss[3] = {0.02,0.02,0.02};
  62
  63     int i;
  64     dBodyID b1 = dGeomGetBody(o1);
  65     dBodyID b2 = dGeomGetBody(o2);
  66
  67     dContact contact[MAX_CONTACTS];
  68     int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
  69                             sizeof(dContact));
  70
  71     for (i=0; i<numc; i++) {
  72         contact[i].surface.mode = dContactApprox1;
  73         contact[i].surface.mu = 2;
  74
  75         dJointID c = dJointCreateContact (*world,contactgroup,contact+i);
  76         dJointAttach (c,b1,b2);
  77         if (show_contacts)
  78             dsDrawBox (contact[i].geom.pos, RI, ss);
  79
  80     }
  81 }
  82
  83
  84 // start simulation - set viewpoint
  85
  86 static void start()
  87 {
  88   float xyz[3] = {4.777f, -2.084f, 2.18f};
  89   float hpr[3] = {153.0f, -14.5f, 0.0f};
  90   dsSetViewpoint (xyz,hpr);
  91   printf ("Orange top approximates conservation of angular momentum\n");
  92   printf ("Green top uses conservation of angular velocity\n");
  93   printf ("---\n");
  94   printf ("SPACE to reset\n");
  95   printf ("A to tilt the tops.\n");
  96   printf ("T to toggle showing the contact points.\n");
  97   printf ("1 to save the current state to 'state.dif'.\n");
  98 }
  99
 100
 101 char locase (char c)
 102 {
 103   if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
 104   else return c;
 105 }
 106
 107
 108 // called when a key pressed
 109 static void reset();
 110 static void tilt();
 111
 112 static void command (int cmd)
 113 {
 114     cmd = locase (cmd);
 115     if (cmd == ' ')
 116     {
 117         reset();
 118     }
 119     else if (cmd == 'a') {
 120         tilt();
 121     }
 122     else if (cmd == 't') {
 123         show_contacts = !show_contacts;
 124     }
 125     else if (cmd == '1') {
 126         write_world = true;
 127     }
 128 }
 129
 130 // simulation loop
 131
 132 static void simLoop (int pause)
 133 {
 134     dsSetColor (0,0,2);
 135     space->collide(0,&nearCallback);
 136     if (!pause)
 137         //world->quickStep(0.02);
 138         world->step(0.02);
 139
 140     if (write_world) {
 141         FILE *f = fopen ("state.dif","wt");
 142         if (f) {
 143             dWorldExportDIF (*world,f,"X");
 144             fclose (f);
 145         }
 146         write_world = false;
 147     }
 148
 149     // remove all contact joints
 150     dJointGroupEmpty (contactgroup);
 151
 152     dsSetTexture (DS_WOOD);
 153
 154     dsSetColor (1,0.5f,0);
 155     dsDrawCylinder(top1->getPosition(),
 156                     top1->getRotation(),
 157                     toplength, topradius);
 158     dsDrawCapsule(top1->getPosition(),
 159                     top1->getRotation(),
 160                     pinlength, pinradius);
 161
 162     dsSetColor (0.5f,1,0);
 163     dsDrawCylinder(top2->getPosition(),
 164                     top2->getRotation(),
 165                     toplength, topradius);
 166     dsDrawCapsule(top2->getPosition(),
 167                     top2->getRotation(),
 168                     pinlength, pinradius);
 169
 170 }
 171
 172
 173 static void reset()
 174 {
 175     dMatrix3 R;
 176     dRSetIdentity(R);
 177
 178     top1->setRotation(R);
 179     top2->setRotation(R);
 180
 181     top1->setPosition(0.8f, -2, 2);
 182     top2->setPosition(0.8f, 2, 2);
 183
 184     top1->setAngularVel(1,0,7);
 185     top2->setAngularVel(1,0,7);
 186
 187     top1->setLinearVel(0,0.2f,0);
 188     top2->setLinearVel(0,0.2f,0);
 189 }
 190
 191 static void tilt()
 192 {
 193     top1->addTorque(0, 10, 0);
 194     top2->addTorque(0, 10, 0);
 195 }
 196
 197 int main (int argc, char **argv)
 198 {
 199     // setup pointers to drawstuff callback functions
 200     dsFunctions fn;
 201     fn.version = DS_VERSION;
 202     fn.start = &start;
 203     fn.step = &simLoop;
 204     fn.command = &command;
 205     fn.stop = 0;
 206     fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
 207
 208
 209     // create world
 210     dInitODE();
 211     world = new dWorld();
 212     world->setGravity(0,0,-0.5f);
 213     world->setCFM(1e-5f);
 214     world->setLinearDamping(0.00001f);
 215     world->setAngularDamping(0.0001f);
 216
 217     space = new dSimpleSpace(0);
 218
 219     dPlane *floor = new dPlane(*space, 0,0,1,0);
 220
 221     top1 = new dBody(*world);
 222     top2 = new dBody(*world);
 223
 224     dMass m;
 225     m.setCylinderTotal(1, 3, topradius, toplength);
 226     top1->setMass(m);
 227     top2->setMass(m);
 228
 229     dGeom *g1, *g2, *pin1, *pin2;
 230     g1 = new dCylinder(*space, topradius, toplength);
 231     g1->setBody(*top1);
 232     g2 = new dCylinder(*space, topradius, toplength);
 233     g2->setBody(*top2);
 234
 235     pin1 = new dCapsule(*space, pinradius, pinlength);
 236     pin1->setBody(*top1);
 237     pin2 = new dCapsule(*space, pinradius, pinlength);
 238     pin2->setBody(*top2);
 239
 240     top2->setGyroscopicMode(false);
 241
 242     reset();
 243
 244     // run simulation
 245     dsSimulationLoop (argc, argv, DS_SIMULATION_DEFAULT_WIDTH, DS_SIMULATION_DEFAULT_HEIGHT, &fn);
 246
 247     delete g1;
 248     delete g2;
 249     delete pin1;
 250     delete pin2;
 251     delete floor;
 252     contactgroup.empty();
 253     delete top1;
 254     delete top2;
 255     delete space;
 256     delete world;
 257     dCloseODE();
 258 }