ode/demo/demo_kinematic.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 <iostream>
  24 #include <set>
  25 #include <algorithm>
  26 #include <functional>
  27 #include <ode/ode.h>
  28 #include <drawstuff/drawstuff.h>
  29 #include "texturepath.h"
  30
  31 #ifdef dDOUBLE
  32 #define dsDrawBox dsDrawBoxD
  33 #define dsDrawCylinder dsDrawCylinderD
  34 #endif
  35
  36
  37 using namespace std;
  38
  39 dWorld *world;
  40 dSpace *space;
  41 dPlane *ground;
  42 dBody *kbody;
  43 dBox *kbox;
  44 dJointGroup joints;
  45 dCylinder *kpole;
  46 dBody *matraca;
  47 dBox *matraca_geom;
  48 dHingeJoint *hinge;
  49
  50 struct Box {
  51     dBody body;
  52     dBox geom;
  53     Box() :
  54         body(*world),
  55         geom(*space, 0.2, 0.2, 0.2)
  56     {
  57         dMass mass;
  58         mass.setBox(10, 0.2, 0.2, 0.2);
  59         body.setMass(mass);
  60         geom.setData(this);
  61         geom.setBody(body);
  62     }
  63     void draw() const
  64     {
  65         dVector3 lengths;
  66         geom.getLengths(lengths);
  67         dsSetTexture(DS_WOOD);
  68         dsSetColor(0,1,0);
  69         dsDrawBox(geom.getPosition(), geom.getRotation(), lengths);
  70     }
  71 };
  72
  73 set<Box*> boxes;
  74 set<Box*> to_remove;
  75
  76 void dropBox()
  77 {
  78     Box *box = new Box();
  79
  80     dReal px = (rand() / float(RAND_MAX)) * 2 - 1;
  81     dReal py = (rand() / float(RAND_MAX)) * 2 - 1;
  82     dReal pz = 2.5;
  83     box->body.setPosition(px, py, pz);
  84
  85     boxes.insert(box);
  86 }
  87
  88 void queueRemoval(dGeomID g)
  89 {
  90     Box *b = (Box*)dGeomGetData(g);
  91     to_remove.insert(b);
  92 }
  93
  94 void removeQueued()
  95 {
  96     while (!to_remove.empty()) {
  97         Box *b = *to_remove.begin();
  98         to_remove.erase(b);
  99         boxes.erase(b);
 100         delete b;
 101     }
 102 }
 103
 104
 105 void nearCallback(void *, dGeomID g1, dGeomID g2)
 106 {
 107     if (g1 == ground->id()) {
 108         queueRemoval(g2);
 109         return;
 110     }
 111     if (g2 == ground->id()) {
 112         queueRemoval(g1);
 113         return;
 114     }
 115
 116     dBodyID b1 = dGeomGetBody(g1);
 117     dBodyID b2 = dGeomGetBody(g2);
 118
 119     if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
 120         return;
 121
 122     const int MAX_CONTACTS = 10;
 123     dContact contact[MAX_CONTACTS];
 124     int n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
 125     for (int i=0; i<n; ++i) {
 126         contact[i].surface.mode = 0;
 127         contact[i].surface.mu = 1;
 128         dJointID j = dJointCreateContact (*world, joints.id(), contact+i);
 129         dJointAttach(j, b1, b2);
 130     }
 131 }
 132
 133
 134 void
 135 simLoop(int pause)
 136 {
 137     if (!pause) {
 138         const dReal timestep = 0.04;
 139
 140         // this does a hard-coded circular motion animation
 141         static float t=0;
 142         t += timestep/4;
 143         if (t > 2*M_PI)
 144             t = 0;
 145         dVector3 next_pos = { dCos(t), dSin(t), REAL(0.5)};
 146         dVector3 vel;
 147         // vel = (next_pos - cur_pos) / timestep
 148         dSubtractVectors3(vel, next_pos, kbody->getPosition());
 149         dScaleVector3(vel, 1/timestep);
 150         kbody->setLinearVel(vel);
 151         // end of hard-coded animation
 152
 153         space->collide(0, nearCallback);
 154         removeQueued();
 155
 156         world->quickStep(timestep);
 157         joints.clear();
 158     }
 159
 160     dVector3 lengths;
 161
 162     // the moving platform
 163     kbox->getLengths(lengths);
 164     dsSetTexture(DS_WOOD);
 165     dsSetColor(.3, .3, 1);
 166     dsDrawBox(kbox->getPosition(), kbox->getRotation(), lengths);
 167     dReal length, radius;
 168     kpole->getParams(&radius, &length);
 169     dsSetTexture(DS_CHECKERED);
 170     dsSetColor(1, 1, 0);
 171     dsDrawCylinder(kpole->getPosition(), kpole->getRotation(), length, radius);
 172
 173     // the matraca
 174     matraca_geom->getLengths(lengths);
 175     dsSetColor(1,0,0);
 176     dsSetTexture(DS_WOOD);
 177     dsDrawBox(matraca_geom->getPosition(), matraca_geom->getRotation(), lengths);
 178
 179     // and the boxes
 180     for_each(boxes.begin(), boxes.end(), mem_fun(&Box::draw));
 181 }
 182
 183 void command(int c)
 184 {
 185     switch (c) {
 186         case ' ':
 187             dropBox();
 188             break;
 189     }
 190 }
 191
 192 int main(int argc, char **argv)
 193 {
 194     dInitODE();
 195
 196     // setup pointers to drawstuff callback functions
 197     dsFunctions fn;
 198     fn.version = DS_VERSION;
 199     fn.start = 0;
 200     fn.step = &simLoop;
 201     fn.command = &command;
 202     fn.stop = 0;
 203     fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
 204
 205     cout << endl << "*** Press SPACE to drop boxes **" << endl;
 206
 207     space = new dSimpleSpace();
 208     ground = new dPlane(*space, 0, 0, 1, 0);
 209
 210     world = new dWorld;
 211     world->setGravity(0, 0, -.5);
 212
 213     kbody = new dBody(*world);
 214     kbody->setKinematic();
 215     const dReal kx = 1, ky = 0, kz = .5;
 216     kbody->setPosition(kx, ky, kz);
 217     kbox = new dBox(*space, 3, 3, .5);
 218     kbox->setBody(*kbody);
 219     kpole = new dCylinder(*space, .125, 1.5);
 220     kpole->setBody(*kbody);
 221     dGeomSetOffsetPosition(kpole->id(), 0, 0, 0.8);
 222
 223     matraca = new dBody(*world);
 224     matraca->setPosition(kx+0, ky+1, kz+1);
 225     matraca_geom = new dBox(*space, 0.5, 2, 0.75);
 226     matraca_geom->setBody(*matraca);
 227     dMass mass;
 228     mass.setBox(1, 0.5, 2, 0.75);
 229     matraca->setMass(mass);
 230
 231     hinge = new dHingeJoint(*world);
 232     hinge->attach(*kbody, *matraca);
 233     hinge->setAnchor(kx, ky, kz+1);
 234     hinge->setAxis(0, 0, 1);
 235
 236     dsSimulationLoop (argc, argv, DS_SIMULATION_DEFAULT_WIDTH, DS_SIMULATION_DEFAULT_HEIGHT, &fn);
 237
 238     dCloseODE();
 239 }