ode/demo/demo_I.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 /*
  24
  25 test that the rotational physics is correct.
  26
  27 an "anchor body" has a number of other randomly positioned bodies
  28 ("particles") attached to it by ball-and-socket joints, giving it some
  29 random effective inertia tensor. the effective inertia matrix is calculated,
  30 and then this inertia is assigned to another "test" body. a random torque is
  31 applied to both bodies and the difference in angular velocity and orientation
  32 is observed after a number of iterations.
  33
  34 typical errors for each test cycle are about 1e-5 ... 1e-4.
  35
  36 */
  37
  38
  39 #include <time.h>
  40 #include <ode/ode.h>
  41 #include <drawstuff/drawstuff.h>
  42 #include "texturepath.h"
  43
  44 #ifdef _MSC_VER
  45 #pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
  46 #endif
  47
  48 // select correct drawing functions
  49
  50 #ifdef dDOUBLE
  51 #define dsDrawBox dsDrawBoxD
  52 #define dsDrawSphere dsDrawSphereD
  53 #define dsDrawCylinder dsDrawCylinderD
  54 #define dsDrawCapsule dsDrawCapsuleD
  55 #endif
  56
  57
  58 // some constants
  59
  60 #define NUM 10                  // number of particles
  61 #define SIDE 0.1                // visual size of the particles
  62
  63
  64 // dynamics objects an globals
  65
  66 static dWorldID world=0;
  67 static dBodyID anchor_body,particle[NUM],test_body;
  68 static dJointID particle_joint[NUM];
  69 static dReal torque[3];
  70 static int iteration;
  71
  72
  73 // start simulation - set viewpoint
  74
  75 static void start()
  76 {
  77   dAllocateODEDataForThread(dAllocateMaskAll);
  78
  79   static float xyz[3] = {1.5572f,-1.8886f,1.5700f};
  80   static float hpr[3] = {118.5000f,-17.0000f,0.0000f};
  81   dsSetViewpoint (xyz,hpr);
  82 }
  83
  84
  85 // compute the mass parameters of a particle set. q = particle positions,
  86 // pm = particle masses
  87
  88 #define _I(i,j) I[(i)*4+(j)]
  89
  90 void computeMassParams (dMass *m, dReal q[NUM][3], dReal pm[NUM])
  91 {
  92   int i,j;
  93   dMassSetZero (m);
  94   for (i=0; i<NUM; i++) {
  95     m->mass += pm[i];
  96     for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j];
  97     m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]);
  98     m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]);
  99     m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]);
 100     m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]);
 101     m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]);
 102     m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]);
 103   }
 104   for (j=0; j<3; j++) m->c[j] /= m->mass;
 105   m->_I(1,0) = m->_I(0,1);
 106   m->_I(2,0) = m->_I(0,2);
 107   m->_I(2,1) = m->_I(1,2);
 108 }
 109
 110
 111 void reset_test()
 112 {
 113   int i;
 114   dMass m,anchor_m;
 115   dReal q[NUM][3], pm[NUM];     // particle positions and masses
 116   dReal pos1[3] = {1,0,1};      // point of reference (POR)
 117   dReal pos2[3] = {-1,0,1};     // point of reference (POR)
 118
 119   // make random particle positions (relative to POR) and masses
 120   for (i=0; i<NUM; i++) {
 121     pm[i] = dRandReal()+0.1;
 122     q[i][0] = dRandReal()-0.5;
 123     q[i][1] = dRandReal()-0.5;
 124     q[i][2] = dRandReal()-0.5;
 125   }
 126
 127   // adjust particle positions so centor of mass = POR
 128   computeMassParams (&m,q,pm);
 129   for (i=0; i<NUM; i++) {
 130     q[i][0] -= m.c[0];
 131     q[i][1] -= m.c[1];
 132     q[i][2] -= m.c[2];
 133   }
 134
 135   if (world) dWorldDestroy (world);
 136   world = dWorldCreate();
 137
 138   anchor_body = dBodyCreate (world);
 139   dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]);
 140   dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE);
 141   dMassAdjust (&anchor_m,0.1);
 142   dBodySetMass (anchor_body,&anchor_m);
 143
 144   for (i=0; i<NUM; i++) {
 145     particle[i] = dBodyCreate (world);
 146     dBodySetPosition (particle[i],
 147                       pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]);
 148     dMassSetBox (&m,1,SIDE,SIDE,SIDE);
 149     dMassAdjust (&m,pm[i]);
 150     dBodySetMass (particle[i],&m);
 151   }
 152
 153   for (i=0; i < NUM; i++) {
 154     particle_joint[i] = dJointCreateBall (world,0);
 155     dJointAttach (particle_joint[i],anchor_body,particle[i]);
 156     const dReal *p = dBodyGetPosition (particle[i]);
 157     dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]);
 158   }
 159
 160   // make test_body with the same mass and inertia of the anchor_body plus
 161   // all the particles
 162
 163   test_body = dBodyCreate (world);
 164   dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]);
 165   computeMassParams (&m,q,pm);
 166   m.mass += anchor_m.mass;
 167   for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i];
 168   dBodySetMass (test_body,&m);
 169
 170   // rotate the test and anchor bodies by a random amount
 171   dQuaternion qrot;
 172   for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5;
 173   dNormalize4 (qrot);
 174   dBodySetQuaternion (anchor_body,qrot);
 175   dBodySetQuaternion (test_body,qrot);
 176   dMatrix3 R;
 177   dQtoR (qrot,R);
 178   for (i=0; i<NUM; i++) {
 179     dVector3 v;
 180     dMultiply0 (v,R,&q[i][0],3,3,1);
 181     dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]);
 182   }
 183
 184   // set random torque
 185   for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1;
 186
 187
 188   iteration=0;
 189 }
 190
 191
 192 // simulation loop
 193
 194 static void simLoop (int pause)
 195 {
 196   if (!pause) {
 197     dBodyAddTorque (anchor_body,torque[0],torque[1],torque[2]);
 198     dBodyAddTorque (test_body,torque[0],torque[1],torque[2]);
 199     dWorldStep (world,0.03);
 200
 201     iteration++;
 202     if (iteration >= 100) {
 203       // measure the difference between the anchor and test bodies
 204       const dReal *w1 = dBodyGetAngularVel (anchor_body);
 205       const dReal *w2 = dBodyGetAngularVel (test_body);
 206       const dReal *q1 = dBodyGetQuaternion (anchor_body);
 207       const dReal *q2 = dBodyGetQuaternion (test_body);
 208       dReal maxdiff = dMaxDifference (w1,w2,1,3);
 209       printf ("w-error = %.4e  (%.2f,%.2f,%.2f) and (%.2f,%.2f,%.2f)\n",
 210               maxdiff,w1[0],w1[1],w1[2],w2[0],w2[1],w2[2]);
 211       maxdiff = dMaxDifference (q1,q2,1,4);
 212       printf ("q-error = %.4e\n",maxdiff);
 213       reset_test();
 214     }
 215   }
 216
 217   dReal sides[3] = {SIDE,SIDE,SIDE};
 218   dReal sides2[3] = {6*SIDE,6*SIDE,6*SIDE};
 219   dReal sides3[3] = {3*SIDE,3*SIDE,3*SIDE};
 220   dsSetColor (1,1,1);
 221   dsDrawBox (dBodyGetPosition(anchor_body), dBodyGetRotation(anchor_body),
 222              sides3);
 223   dsSetColor (1,0,0);
 224   dsDrawBox (dBodyGetPosition(test_body), dBodyGetRotation(test_body), sides2);
 225   dsSetColor (1,1,0);
 226   for (int i=0; i<NUM; i++)
 227     dsDrawBox (dBodyGetPosition (particle[i]),
 228                dBodyGetRotation (particle[i]), sides);
 229 }
 230
 231
 232 int main (int argc, char **argv)
 233 {
 234   // setup pointers to drawstuff callback functions
 235   dsFunctions fn;
 236   fn.version = DS_VERSION;
 237   fn.start = &start;
 238   fn.step = &simLoop;
 239   fn.command = 0;
 240   fn.stop = 0;
 241   fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
 242
 243   dInitODE2(0);
 244   dRandSetSeed (time(0));
 245   reset_test();
 246
 247   // run simulation
 248   dsSimulationLoop (argc,argv,352,288,&fn);
 249
 250   dWorldDestroy (world);
 251   dCloseODE();
 252   return 0;
 253 }