[216] | 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 | |
---|
| 26 | #ifdef _MSC_VER |
---|
| 27 | #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints |
---|
| 28 | #endif |
---|
| 29 | |
---|
| 30 | // select correct drawing functions |
---|
| 31 | #ifdef dDOUBLE |
---|
| 32 | #define dsDrawBox dsDrawBoxD |
---|
| 33 | #endif |
---|
| 34 | |
---|
| 35 | |
---|
| 36 | // some constants |
---|
| 37 | #define SIDE (0.5f) // side length of a box |
---|
| 38 | #define MASS (1.0) // mass of a box |
---|
| 39 | |
---|
| 40 | |
---|
| 41 | // dynamics and collision objects |
---|
| 42 | static dWorldID world; |
---|
| 43 | static dBodyID body[2]; |
---|
| 44 | static dJointID hinge; |
---|
| 45 | |
---|
| 46 | |
---|
| 47 | // state set by keyboard commands |
---|
| 48 | static int occasional_error = 0; |
---|
| 49 | |
---|
| 50 | |
---|
| 51 | // start simulation - set viewpoint |
---|
| 52 | |
---|
| 53 | static void start() |
---|
| 54 | { |
---|
| 55 | static float xyz[3] = {1.0382f,-1.0811f,1.4700f}; |
---|
| 56 | static float hpr[3] = {135.0000f,-19.5000f,0.0000f}; |
---|
| 57 | dsSetViewpoint (xyz,hpr); |
---|
| 58 | printf ("Press 'e' to start/stop occasional error.\n"); |
---|
| 59 | } |
---|
| 60 | |
---|
| 61 | |
---|
| 62 | // called when a key pressed |
---|
| 63 | |
---|
| 64 | static void command (int cmd) |
---|
| 65 | { |
---|
| 66 | if (cmd == 'e' || cmd == 'E') { |
---|
| 67 | occasional_error ^= 1; |
---|
| 68 | } |
---|
| 69 | } |
---|
| 70 | |
---|
| 71 | |
---|
| 72 | // simulation loop |
---|
| 73 | |
---|
| 74 | static void simLoop (int pause) |
---|
| 75 | { |
---|
| 76 | const dReal kd = -0.3; // angular damping constant |
---|
| 77 | if (!pause) { |
---|
| 78 | // add an oscillating torque to body 0, and also damp its rotational motion |
---|
| 79 | static dReal a=0; |
---|
| 80 | const dReal *w = dBodyGetAngularVel (body[0]); |
---|
| 81 | dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a)); |
---|
| 82 | dWorldStep (world,0.05); |
---|
| 83 | a += 0.01; |
---|
| 84 | |
---|
| 85 | // occasionally re-orient one of the bodies to create a deliberate error. |
---|
| 86 | if (occasional_error) { |
---|
| 87 | static int count = 0; |
---|
| 88 | if ((count % 20)==0) { |
---|
| 89 | // randomly adjust orientation of body[0] |
---|
| 90 | const dReal *R1; |
---|
| 91 | dMatrix3 R2,R3; |
---|
| 92 | R1 = dBodyGetRotation (body[0]); |
---|
| 93 | dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5, |
---|
| 94 | dRandReal()-0.5,dRandReal()-0.5); |
---|
| 95 | dMultiply0 (R3,R1,R2,3,3,3); |
---|
| 96 | dBodySetRotation (body[0],R3); |
---|
| 97 | |
---|
| 98 | // randomly adjust position of body[0] |
---|
| 99 | const dReal *pos = dBodyGetPosition (body[0]); |
---|
| 100 | dBodySetPosition (body[0], |
---|
| 101 | pos[0]+0.2*(dRandReal()-0.5), |
---|
| 102 | pos[1]+0.2*(dRandReal()-0.5), |
---|
| 103 | pos[2]+0.2*(dRandReal()-0.5)); |
---|
| 104 | } |
---|
| 105 | count++; |
---|
| 106 | } |
---|
| 107 | } |
---|
| 108 | |
---|
| 109 | dReal sides1[3] = {SIDE,SIDE,SIDE}; |
---|
| 110 | dReal sides2[3] = {SIDE,SIDE,SIDE*0.8f}; |
---|
| 111 | dsSetTexture (DS_WOOD); |
---|
| 112 | dsSetColor (1,1,0); |
---|
| 113 | dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1); |
---|
| 114 | dsSetColor (0,1,1); |
---|
| 115 | dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2); |
---|
| 116 | } |
---|
| 117 | |
---|
| 118 | |
---|
| 119 | int main (int argc, char **argv) |
---|
| 120 | { |
---|
| 121 | // setup pointers to drawstuff callback functions |
---|
| 122 | dsFunctions fn; |
---|
| 123 | fn.version = DS_VERSION; |
---|
| 124 | fn.start = &start; |
---|
| 125 | fn.step = &simLoop; |
---|
| 126 | fn.command = &command; |
---|
| 127 | fn.stop = 0; |
---|
| 128 | fn.path_to_textures = "../../drawstuff/textures"; |
---|
| 129 | if(argc==2) |
---|
| 130 | { |
---|
| 131 | fn.path_to_textures = argv[1]; |
---|
| 132 | } |
---|
| 133 | |
---|
| 134 | // create world |
---|
| 135 | dInitODE(); |
---|
| 136 | world = dWorldCreate(); |
---|
| 137 | |
---|
| 138 | dMass m; |
---|
| 139 | dMassSetBox (&m,1,SIDE,SIDE,SIDE); |
---|
| 140 | dMassAdjust (&m,MASS); |
---|
| 141 | |
---|
| 142 | dQuaternion q; |
---|
| 143 | dQFromAxisAndAngle (q,1,1,0,0.25*M_PI); |
---|
| 144 | |
---|
| 145 | body[0] = dBodyCreate (world); |
---|
| 146 | dBodySetMass (body[0],&m); |
---|
| 147 | dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1); |
---|
| 148 | dBodySetQuaternion (body[0],q); |
---|
| 149 | |
---|
| 150 | body[1] = dBodyCreate (world); |
---|
| 151 | dBodySetMass (body[1],&m); |
---|
| 152 | dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1); |
---|
| 153 | dBodySetQuaternion (body[1],q); |
---|
| 154 | |
---|
| 155 | hinge = dJointCreateHinge (world,0); |
---|
| 156 | dJointAttach (hinge,body[0],body[1]); |
---|
| 157 | dJointSetHingeAnchor (hinge,0,0,1); |
---|
| 158 | dJointSetHingeAxis (hinge,1,-1,1.41421356); |
---|
| 159 | |
---|
| 160 | // run simulation |
---|
| 161 | dsSimulationLoop (argc,argv,352,288,&fn); |
---|
| 162 | |
---|
| 163 | dWorldDestroy (world); |
---|
| 164 | dCloseODE(); |
---|
| 165 | return 0; |
---|
| 166 | } |
---|