[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 | // This is a demo of the QuickStep and StepFast methods, |
---|
| 24 | // originally by David Whittaker. |
---|
| 25 | |
---|
| 26 | #include <ode/ode.h> |
---|
| 27 | #include <drawstuff/drawstuff.h> |
---|
| 28 | |
---|
| 29 | #ifdef _MSC_VER |
---|
| 30 | #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints |
---|
| 31 | #endif |
---|
| 32 | |
---|
| 33 | // select correct drawing functions |
---|
| 34 | |
---|
| 35 | #ifdef dDOUBLE |
---|
| 36 | #define dsDrawBox dsDrawBoxD |
---|
| 37 | #define dsDrawSphere dsDrawSphereD |
---|
| 38 | #define dsDrawCylinder dsDrawCylinderD |
---|
| 39 | #define dsDrawCapsule dsDrawCapsuleD |
---|
| 40 | #endif |
---|
| 41 | |
---|
| 42 | |
---|
| 43 | // select the method you want to test here (only uncomment *one* line) |
---|
| 44 | #define QUICKSTEP 1 |
---|
| 45 | //#define STEPFAST 1 |
---|
| 46 | |
---|
| 47 | // some constants |
---|
| 48 | |
---|
| 49 | #define LENGTH 3.5 // chassis length |
---|
| 50 | #define WIDTH 2.5 // chassis width |
---|
| 51 | #define HEIGHT 1.0 // chassis height |
---|
| 52 | #define RADIUS 0.5 // wheel radius |
---|
| 53 | #define STARTZ 1.0 // starting height of chassis |
---|
| 54 | #define CMASS 1 // chassis mass |
---|
| 55 | #define WMASS 1 // wheel mass |
---|
| 56 | #define COMOFFSET -5 // center of mass offset |
---|
| 57 | #define WALLMASS 1 // wall box mass |
---|
| 58 | #define BALLMASS 1 // ball mass |
---|
| 59 | #define FMAX 25 // car engine fmax |
---|
| 60 | #define ROWS 1 // rows of cars |
---|
| 61 | #define COLS 1 // columns of cars |
---|
| 62 | #define ITERS 20 // number of iterations |
---|
| 63 | #define WBOXSIZE 1.0 // size of wall boxes |
---|
| 64 | #define WALLWIDTH 12 // width of wall |
---|
| 65 | #define WALLHEIGHT 10 // height of wall |
---|
| 66 | #define DISABLE_THRESHOLD 0.008 // maximum velocity (squared) a body can have and be disabled |
---|
| 67 | #define DISABLE_STEPS 10 // number of steps a box has to have been disable-able before it will be disabled |
---|
| 68 | #define CANNON_X -10 // x position of cannon |
---|
| 69 | #define CANNON_Y 5 // y position of cannon |
---|
| 70 | #define CANNON_BALL_MASS 10 // mass of the cannon ball |
---|
| 71 | #define CANNON_BALL_RADIUS 0.5 |
---|
| 72 | |
---|
| 73 | //#define BOX |
---|
| 74 | #define CARS |
---|
| 75 | #define WALL |
---|
| 76 | //#define BALLS |
---|
| 77 | //#define BALLSTACK |
---|
| 78 | //#define ONEBALL |
---|
| 79 | //#define CENTIPEDE |
---|
| 80 | #define CANNON |
---|
| 81 | |
---|
| 82 | // dynamics and collision objects (chassis, 3 wheels, environment) |
---|
| 83 | |
---|
| 84 | static dWorldID world; |
---|
| 85 | static dSpaceID space; |
---|
| 86 | static dBodyID body[10000]; |
---|
| 87 | static int bodies; |
---|
| 88 | static dJointID joint[100000]; |
---|
| 89 | static int joints; |
---|
| 90 | static dJointGroupID contactgroup; |
---|
| 91 | static dGeomID ground; |
---|
| 92 | static dGeomID box[10000]; |
---|
| 93 | static int boxes; |
---|
| 94 | static dGeomID sphere[10000]; |
---|
| 95 | static int spheres; |
---|
| 96 | static dGeomID wall_boxes[10000]; |
---|
| 97 | static dBodyID wall_bodies[10000]; |
---|
| 98 | static dGeomID cannon_ball_geom; |
---|
| 99 | static dBodyID cannon_ball_body; |
---|
| 100 | static int wb_stepsdis[10000]; |
---|
| 101 | static int wb; |
---|
| 102 | static bool doFast; |
---|
| 103 | static dBodyID b; |
---|
| 104 | static dMass m; |
---|
| 105 | |
---|
| 106 | |
---|
| 107 | // things that the user controls |
---|
| 108 | |
---|
| 109 | static dReal turn = 0, speed = 0; // user commands |
---|
| 110 | static dReal cannon_angle=0,cannon_elevation=-1.2; |
---|
| 111 | |
---|
| 112 | |
---|
| 113 | |
---|
| 114 | // this is called by dSpaceCollide when two objects in space are |
---|
| 115 | // potentially colliding. |
---|
| 116 | |
---|
| 117 | static void nearCallback (void *data, dGeomID o1, dGeomID o2) |
---|
| 118 | { |
---|
| 119 | int i,n; |
---|
| 120 | |
---|
| 121 | dBodyID b1 = dGeomGetBody(o1); |
---|
| 122 | dBodyID b2 = dGeomGetBody(o2); |
---|
| 123 | if (b1 && b2 && dAreConnected(b1, b2)) |
---|
| 124 | return; |
---|
| 125 | |
---|
| 126 | const int N = 4; |
---|
| 127 | dContact contact[N]; |
---|
| 128 | n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); |
---|
| 129 | if (n > 0) { |
---|
| 130 | for (i=0; i<n; i++) { |
---|
| 131 | contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; |
---|
| 132 | if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass) |
---|
| 133 | contact[i].surface.mu = 20; |
---|
| 134 | else |
---|
| 135 | contact[i].surface.mu = 0.5; |
---|
| 136 | contact[i].surface.slip1 = 0.0; |
---|
| 137 | contact[i].surface.slip2 = 0.0; |
---|
| 138 | contact[i].surface.soft_erp = 0.8; |
---|
| 139 | contact[i].surface.soft_cfm = 0.01; |
---|
| 140 | dJointID c = dJointCreateContact (world,contactgroup,contact+i); |
---|
| 141 | dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2)); |
---|
| 142 | } |
---|
| 143 | } |
---|
| 144 | } |
---|
| 145 | |
---|
| 146 | |
---|
| 147 | // start simulation - set viewpoint |
---|
| 148 | |
---|
| 149 | static void start() |
---|
| 150 | { |
---|
| 151 | static float xyz[3] = {3.8548f,9.0843f,7.5900f}; |
---|
| 152 | static float hpr[3] = {-145.5f,-22.5f,0.25f}; |
---|
| 153 | dsSetViewpoint (xyz,hpr); |
---|
| 154 | printf ("Press:\t'a' to increase speed.\n" |
---|
| 155 | "\t'z' to decrease speed.\n" |
---|
| 156 | "\t',' to steer left.\n" |
---|
| 157 | "\t'.' to steer right.\n" |
---|
| 158 | "\t' ' to reset speed and steering.\n" |
---|
| 159 | "\t'[' to turn the cannon left.\n" |
---|
| 160 | "\t']' to turn the cannon right.\n" |
---|
| 161 | "\t'1' to raise the cannon.\n" |
---|
| 162 | "\t'2' to lower the cannon.\n" |
---|
| 163 | "\t'x' to shoot from the cannon.\n" |
---|
| 164 | "\t'f' to toggle fast step mode.\n" |
---|
| 165 | "\t'+' to increase AutoEnableDepth.\n" |
---|
| 166 | "\t'-' to decrease AutoEnableDepth.\n" |
---|
| 167 | "\t'r' to reset simulation.\n"); |
---|
| 168 | } |
---|
| 169 | |
---|
| 170 | |
---|
| 171 | void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI) |
---|
| 172 | { |
---|
| 173 | int i; |
---|
| 174 | dMass m; |
---|
| 175 | |
---|
| 176 | // chassis body |
---|
| 177 | body[bodyI] = dBodyCreate (world); |
---|
| 178 | dBodySetPosition (body[bodyI],x,y,STARTZ); |
---|
| 179 | dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); |
---|
| 180 | dMassAdjust (&m,CMASS/2.0); |
---|
| 181 | dBodySetMass (body[bodyI],&m); |
---|
| 182 | box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT); |
---|
| 183 | dGeomSetBody (box[boxI],body[bodyI]); |
---|
| 184 | |
---|
| 185 | // wheel bodies |
---|
| 186 | for (i=1; i<=4; i++) { |
---|
| 187 | body[bodyI+i] = dBodyCreate (world); |
---|
| 188 | dQuaternion q; |
---|
| 189 | dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); |
---|
| 190 | dBodySetQuaternion (body[bodyI+i],q); |
---|
| 191 | dMassSetSphere (&m,1,RADIUS); |
---|
| 192 | dMassAdjust (&m,WMASS); |
---|
| 193 | dBodySetMass (body[bodyI+i],&m); |
---|
| 194 | sphere[sphereI+i-1] = dCreateSphere (space,RADIUS); |
---|
| 195 | dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]); |
---|
| 196 | } |
---|
| 197 | dBodySetPosition (body[bodyI+1],x+0.4*LENGTH-0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5); |
---|
| 198 | dBodySetPosition (body[bodyI+2],x+0.4*LENGTH-0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5); |
---|
| 199 | dBodySetPosition (body[bodyI+3],x-0.4*LENGTH+0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5); |
---|
| 200 | dBodySetPosition (body[bodyI+4],x-0.4*LENGTH+0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5); |
---|
| 201 | |
---|
| 202 | // front and back wheel hinges |
---|
| 203 | for (i=0; i<4; i++) { |
---|
| 204 | joint[jointI+i] = dJointCreateHinge2 (world,0); |
---|
| 205 | dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]); |
---|
| 206 | const dReal *a = dBodyGetPosition (body[bodyI+i+1]); |
---|
| 207 | dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]); |
---|
| 208 | dJointSetHinge2Axis1 (joint[jointI+i],0,0,(i<2 ? 1 : -1)); |
---|
| 209 | dJointSetHinge2Axis2 (joint[jointI+i],0,1,0); |
---|
| 210 | dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8); |
---|
| 211 | dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5); |
---|
| 212 | dJointSetHinge2Param (joint[jointI+i],dParamVel2,0); |
---|
| 213 | dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX); |
---|
| 214 | } |
---|
| 215 | |
---|
| 216 | //center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint) |
---|
| 217 | dBodyID b = dBodyCreate (world); |
---|
| 218 | dBodySetPosition (b,x,y,STARTZ+COMOFFSET); |
---|
| 219 | dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); |
---|
| 220 | dMassAdjust (&m,CMASS/2.0); |
---|
| 221 | dBodySetMass (b,&m); |
---|
| 222 | dJointID j = dJointCreateFixed(world, 0); |
---|
| 223 | dJointAttach(j, body[bodyI], b); |
---|
| 224 | dJointSetFixed(j); |
---|
| 225 | //box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT); |
---|
| 226 | //dGeomSetBody (box[boxI+1],b); |
---|
| 227 | |
---|
| 228 | bodyI += 5; |
---|
| 229 | jointI += 4; |
---|
| 230 | boxI += 1; |
---|
| 231 | sphereI += 4; |
---|
| 232 | } |
---|
| 233 | |
---|
| 234 | |
---|
| 235 | void resetSimulation() |
---|
| 236 | { |
---|
| 237 | int i; |
---|
| 238 | i = 0; |
---|
| 239 | // destroy world if it exists |
---|
| 240 | if (bodies) |
---|
| 241 | { |
---|
| 242 | dJointGroupDestroy (contactgroup); |
---|
| 243 | dSpaceDestroy (space); |
---|
| 244 | dWorldDestroy (world); |
---|
| 245 | } |
---|
| 246 | |
---|
| 247 | for (i = 0; i < 1000; i++) |
---|
| 248 | wb_stepsdis[i] = 0; |
---|
| 249 | |
---|
| 250 | // recreate world |
---|
| 251 | |
---|
| 252 | world = dWorldCreate(); |
---|
| 253 | space = dHashSpaceCreate (0); |
---|
| 254 | contactgroup = dJointGroupCreate (0); |
---|
| 255 | dWorldSetGravity (world,0,0,-1.5); |
---|
| 256 | dWorldSetCFM (world, 1e-5); |
---|
| 257 | dWorldSetERP (world, 0.8); |
---|
| 258 | dWorldSetQuickStepNumIterations (world,ITERS); |
---|
| 259 | ground = dCreatePlane (space,0,0,1,0); |
---|
| 260 | |
---|
| 261 | bodies = 0; |
---|
| 262 | joints = 0; |
---|
| 263 | boxes = 0; |
---|
| 264 | spheres = 0; |
---|
| 265 | wb = 0; |
---|
| 266 | |
---|
| 267 | #ifdef CARS |
---|
| 268 | for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS) |
---|
| 269 | for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2) |
---|
| 270 | makeCar(x, y, bodies, joints, boxes, spheres); |
---|
| 271 | #endif |
---|
| 272 | #ifdef WALL |
---|
| 273 | bool offset = false; |
---|
| 274 | for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE) |
---|
| 275 | { |
---|
| 276 | offset = !offset; |
---|
| 277 | for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE) |
---|
| 278 | { |
---|
| 279 | wall_bodies[wb] = dBodyCreate (world); |
---|
| 280 | dBodySetPosition (wall_bodies[wb],-20,y,z); |
---|
| 281 | dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE); |
---|
| 282 | dMassAdjust (&m, WALLMASS); |
---|
| 283 | dBodySetMass (wall_bodies[wb],&m); |
---|
| 284 | wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE); |
---|
| 285 | dGeomSetBody (wall_boxes[wb],wall_bodies[wb]); |
---|
| 286 | //dBodyDisable(wall_bodies[wb++]); |
---|
| 287 | wb++; |
---|
| 288 | } |
---|
| 289 | } |
---|
| 290 | dMessage(0,"wall boxes: %i", wb); |
---|
| 291 | #endif |
---|
| 292 | #ifdef BALLS |
---|
| 293 | for (dReal x = -7; x <= -4; x+=1) |
---|
| 294 | for (dReal y = -1.5; y <= 1.5; y+=1) |
---|
| 295 | for (dReal z = 1; z <= 4; z+=1) |
---|
| 296 | { |
---|
| 297 | b = dBodyCreate (world); |
---|
| 298 | dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2); |
---|
| 299 | dMassSetSphere (&m,1,RADIUS); |
---|
| 300 | dMassAdjust (&m, BALLMASS); |
---|
| 301 | dBodySetMass (b,&m); |
---|
| 302 | sphere[spheres] = dCreateSphere (space,RADIUS); |
---|
| 303 | dGeomSetBody (sphere[spheres++],b); |
---|
| 304 | } |
---|
| 305 | #endif |
---|
| 306 | #ifdef ONEBALL |
---|
| 307 | b = dBodyCreate (world); |
---|
| 308 | dBodySetPosition (b,0,0,2); |
---|
| 309 | dMassSetSphere (&m,1,RADIUS); |
---|
| 310 | dMassAdjust (&m, 1); |
---|
| 311 | dBodySetMass (b,&m); |
---|
| 312 | sphere[spheres] = dCreateSphere (space,RADIUS); |
---|
| 313 | dGeomSetBody (sphere[spheres++],b); |
---|
| 314 | #endif |
---|
| 315 | #ifdef BALLSTACK |
---|
| 316 | for (dReal z = 1; z <= 6; z+=1) |
---|
| 317 | { |
---|
| 318 | b = dBodyCreate (world); |
---|
| 319 | dBodySetPosition (b,0,0,z*RADIUS*2); |
---|
| 320 | dMassSetSphere (&m,1,RADIUS); |
---|
| 321 | dMassAdjust (&m, 0.1); |
---|
| 322 | dBodySetMass (b,&m); |
---|
| 323 | sphere[spheres] = dCreateSphere (space,RADIUS); |
---|
| 324 | dGeomSetBody (sphere[spheres++],b); |
---|
| 325 | } |
---|
| 326 | #endif |
---|
| 327 | #ifdef CENTIPEDE |
---|
| 328 | dBodyID lastb = 0; |
---|
| 329 | for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1) |
---|
| 330 | { |
---|
| 331 | // chassis body |
---|
| 332 | |
---|
| 333 | b = body[bodies] = dBodyCreate (world); |
---|
| 334 | dBodySetPosition (body[bodies],-15,y,STARTZ); |
---|
| 335 | dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT); |
---|
| 336 | dMassAdjust (&m,CMASS); |
---|
| 337 | dBodySetMass (body[bodies],&m); |
---|
| 338 | box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT); |
---|
| 339 | dGeomSetBody (box[boxes++],body[bodies++]); |
---|
| 340 | |
---|
| 341 | for (dReal x = -17; x > -20; x-=RADIUS*2) |
---|
| 342 | { |
---|
| 343 | body[bodies] = dBodyCreate (world); |
---|
| 344 | dBodySetPosition(body[bodies], x, y, STARTZ); |
---|
| 345 | dMassSetSphere(&m, 1, RADIUS); |
---|
| 346 | dMassAdjust(&m, WMASS); |
---|
| 347 | dBodySetMass(body[bodies], &m); |
---|
| 348 | sphere[spheres] = dCreateSphere (space, RADIUS); |
---|
| 349 | dGeomSetBody (sphere[spheres++], body[bodies]); |
---|
| 350 | |
---|
| 351 | joint[joints] = dJointCreateHinge2 (world,0); |
---|
| 352 | if (x == -17) |
---|
| 353 | dJointAttach (joint[joints],b,body[bodies]); |
---|
| 354 | else |
---|
| 355 | dJointAttach (joint[joints],body[bodies-2],body[bodies]); |
---|
| 356 | const dReal *a = dBodyGetPosition (body[bodies++]); |
---|
| 357 | dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]); |
---|
| 358 | dJointSetHinge2Axis1 (joint[joints],0,0,1); |
---|
| 359 | dJointSetHinge2Axis2 (joint[joints],1,0,0); |
---|
| 360 | dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); |
---|
| 361 | dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); |
---|
| 362 | dJointSetHinge2Param (joint[joints],dParamLoStop,0); |
---|
| 363 | dJointSetHinge2Param (joint[joints],dParamHiStop,0); |
---|
| 364 | dJointSetHinge2Param (joint[joints],dParamVel2,-10.0); |
---|
| 365 | dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); |
---|
| 366 | |
---|
| 367 | body[bodies] = dBodyCreate (world); |
---|
| 368 | dBodySetPosition(body[bodies], -30 - x, y, STARTZ); |
---|
| 369 | dMassSetSphere(&m, 1, RADIUS); |
---|
| 370 | dMassAdjust(&m, WMASS); |
---|
| 371 | dBodySetMass(body[bodies], &m); |
---|
| 372 | sphere[spheres] = dCreateSphere (space, RADIUS); |
---|
| 373 | dGeomSetBody (sphere[spheres++], body[bodies]); |
---|
| 374 | |
---|
| 375 | joint[joints] = dJointCreateHinge2 (world,0); |
---|
| 376 | if (x == -17) |
---|
| 377 | dJointAttach (joint[joints],b,body[bodies]); |
---|
| 378 | else |
---|
| 379 | dJointAttach (joint[joints],body[bodies-2],body[bodies]); |
---|
| 380 | const dReal *b = dBodyGetPosition (body[bodies++]); |
---|
| 381 | dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]); |
---|
| 382 | dJointSetHinge2Axis1 (joint[joints],0,0,1); |
---|
| 383 | dJointSetHinge2Axis2 (joint[joints],1,0,0); |
---|
| 384 | dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); |
---|
| 385 | dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); |
---|
| 386 | dJointSetHinge2Param (joint[joints],dParamLoStop,0); |
---|
| 387 | dJointSetHinge2Param (joint[joints],dParamHiStop,0); |
---|
| 388 | dJointSetHinge2Param (joint[joints],dParamVel2,10.0); |
---|
| 389 | dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); |
---|
| 390 | } |
---|
| 391 | if (lastb) |
---|
| 392 | { |
---|
| 393 | dJointID j = dJointCreateFixed(world,0); |
---|
| 394 | dJointAttach (j, b, lastb); |
---|
| 395 | dJointSetFixed(j); |
---|
| 396 | } |
---|
| 397 | lastb = b; |
---|
| 398 | } |
---|
| 399 | #endif |
---|
| 400 | #ifdef BOX |
---|
| 401 | body[bodies] = dBodyCreate (world); |
---|
| 402 | dBodySetPosition (body[bodies],0,0,HEIGHT/2); |
---|
| 403 | dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); |
---|
| 404 | dMassAdjust (&m, 1); |
---|
| 405 | dBodySetMass (body[bodies],&m); |
---|
| 406 | box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT); |
---|
| 407 | dGeomSetBody (box[boxes++],body[bodies++]); |
---|
| 408 | #endif |
---|
| 409 | #ifdef CANNON |
---|
| 410 | cannon_ball_body = dBodyCreate (world); |
---|
| 411 | cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS); |
---|
| 412 | dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS); |
---|
| 413 | dBodySetMass (cannon_ball_body,&m); |
---|
| 414 | dGeomSetBody (cannon_ball_geom,cannon_ball_body); |
---|
| 415 | dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS); |
---|
| 416 | #endif |
---|
| 417 | } |
---|
| 418 | |
---|
| 419 | // called when a key pressed |
---|
| 420 | |
---|
| 421 | static void command (int cmd) |
---|
| 422 | { |
---|
| 423 | switch (cmd) { |
---|
| 424 | case 'a': case 'A': |
---|
| 425 | speed += 0.3; |
---|
| 426 | break; |
---|
| 427 | case 'z': case 'Z': |
---|
| 428 | speed -= 0.3; |
---|
| 429 | break; |
---|
| 430 | case ',': |
---|
| 431 | turn += 0.1; |
---|
| 432 | if (turn > 0.3) |
---|
| 433 | turn = 0.3; |
---|
| 434 | break; |
---|
| 435 | case '.': |
---|
| 436 | turn -= 0.1; |
---|
| 437 | if (turn < -0.3) |
---|
| 438 | turn = -0.3; |
---|
| 439 | break; |
---|
| 440 | case ' ': |
---|
| 441 | speed = 0; |
---|
| 442 | turn = 0; |
---|
| 443 | break; |
---|
| 444 | case 'f': case 'F': |
---|
| 445 | doFast = !doFast; |
---|
| 446 | break; |
---|
| 447 | case '+': |
---|
| 448 | dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) + 1); |
---|
| 449 | break; |
---|
| 450 | case '-': |
---|
| 451 | dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) - 1); |
---|
| 452 | break; |
---|
| 453 | case 'r': case 'R': |
---|
| 454 | resetSimulation(); |
---|
| 455 | break; |
---|
| 456 | case '[': |
---|
| 457 | cannon_angle += 0.1; |
---|
| 458 | break; |
---|
| 459 | case ']': |
---|
| 460 | cannon_angle -= 0.1; |
---|
| 461 | break; |
---|
| 462 | case '1': |
---|
| 463 | cannon_elevation += 0.1; |
---|
| 464 | break; |
---|
| 465 | case '2': |
---|
| 466 | cannon_elevation -= 0.1; |
---|
| 467 | break; |
---|
| 468 | case 'x': case 'X': { |
---|
| 469 | dMatrix3 R2,R3,R4; |
---|
| 470 | dRFromAxisAndAngle (R2,0,0,1,cannon_angle); |
---|
| 471 | dRFromAxisAndAngle (R3,0,1,0,cannon_elevation); |
---|
| 472 | dMultiply0 (R4,R2,R3,3,3,3); |
---|
| 473 | dReal cpos[3] = {CANNON_X,CANNON_Y,1}; |
---|
| 474 | for (int i=0; i<3; i++) cpos[i] += 3*R4[i*4+2]; |
---|
| 475 | dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]); |
---|
| 476 | dReal force = 10; |
---|
| 477 | dBodySetLinearVel (cannon_ball_body,force*R4[2],force*R4[6],force*R4[10]); |
---|
| 478 | dBodySetAngularVel (cannon_ball_body,0,0,0); |
---|
| 479 | break; |
---|
| 480 | } |
---|
| 481 | } |
---|
| 482 | } |
---|
| 483 | |
---|
| 484 | |
---|
| 485 | // simulation loop |
---|
| 486 | |
---|
| 487 | static void simLoop (int pause) |
---|
| 488 | { |
---|
| 489 | int i, j; |
---|
| 490 | |
---|
| 491 | dsSetTexture (DS_WOOD); |
---|
| 492 | |
---|
| 493 | if (!pause) { |
---|
| 494 | #ifdef BOX |
---|
| 495 | dBodyAddForce(body[bodies-1],lspeed,0,0); |
---|
| 496 | #endif |
---|
| 497 | for (j = 0; j < joints; j++) |
---|
| 498 | { |
---|
| 499 | dReal curturn = dJointGetHinge2Angle1 (joint[j]); |
---|
| 500 | //dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0); |
---|
| 501 | dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0); |
---|
| 502 | dJointSetHinge2Param(joint[j],dParamFMax,dInfinity); |
---|
| 503 | dJointSetHinge2Param(joint[j],dParamVel2,speed); |
---|
| 504 | dJointSetHinge2Param(joint[j],dParamFMax2,FMAX); |
---|
| 505 | dBodyEnable(dJointGetBody(joint[j],0)); |
---|
| 506 | dBodyEnable(dJointGetBody(joint[j],1)); |
---|
| 507 | } |
---|
| 508 | if (doFast) |
---|
| 509 | { |
---|
| 510 | dSpaceCollide (space,0,&nearCallback); |
---|
| 511 | #if defined(QUICKSTEP) |
---|
| 512 | dWorldQuickStep (world,0.05); |
---|
| 513 | #elif defined(STEPFAST) |
---|
| 514 | dWorldStepFast1 (world,0.05,ITERS); |
---|
| 515 | #endif |
---|
| 516 | dJointGroupEmpty (contactgroup); |
---|
| 517 | } |
---|
| 518 | else |
---|
| 519 | { |
---|
| 520 | dSpaceCollide (space,0,&nearCallback); |
---|
| 521 | dWorldStep (world,0.05); |
---|
| 522 | dJointGroupEmpty (contactgroup); |
---|
| 523 | } |
---|
| 524 | |
---|
| 525 | for (i = 0; i < wb; i++) |
---|
| 526 | { |
---|
| 527 | b = dGeomGetBody(wall_boxes[i]); |
---|
| 528 | if (dBodyIsEnabled(b)) |
---|
| 529 | { |
---|
| 530 | bool disable = true; |
---|
| 531 | const dReal *lvel = dBodyGetLinearVel(b); |
---|
| 532 | dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2]; |
---|
| 533 | if (lspeed > DISABLE_THRESHOLD) |
---|
| 534 | disable = false; |
---|
| 535 | const dReal *avel = dBodyGetAngularVel(b); |
---|
| 536 | dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2]; |
---|
| 537 | if (aspeed > DISABLE_THRESHOLD) |
---|
| 538 | disable = false; |
---|
| 539 | |
---|
| 540 | if (disable) |
---|
| 541 | wb_stepsdis[i]++; |
---|
| 542 | else |
---|
| 543 | wb_stepsdis[i] = 0; |
---|
| 544 | |
---|
| 545 | if (wb_stepsdis[i] > DISABLE_STEPS) |
---|
| 546 | { |
---|
| 547 | dBodyDisable(b); |
---|
| 548 | dsSetColor(0.5,0.5,1); |
---|
| 549 | } |
---|
| 550 | else |
---|
| 551 | dsSetColor(1,1,1); |
---|
| 552 | |
---|
| 553 | } |
---|
| 554 | else |
---|
| 555 | dsSetColor(0.4,0.4,0.4); |
---|
| 556 | dVector3 ss; |
---|
| 557 | dGeomBoxGetLengths (wall_boxes[i], ss); |
---|
| 558 | dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss); |
---|
| 559 | } |
---|
| 560 | } |
---|
| 561 | else |
---|
| 562 | { |
---|
| 563 | for (i = 0; i < wb; i++) |
---|
| 564 | { |
---|
| 565 | b = dGeomGetBody(wall_boxes[i]); |
---|
| 566 | if (dBodyIsEnabled(b)) |
---|
| 567 | dsSetColor(1,1,1); |
---|
| 568 | else |
---|
| 569 | dsSetColor(0.4,0.4,0.4); |
---|
| 570 | dVector3 ss; |
---|
| 571 | dGeomBoxGetLengths (wall_boxes[i], ss); |
---|
| 572 | dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss); |
---|
| 573 | } |
---|
| 574 | } |
---|
| 575 | |
---|
| 576 | dsSetColor (0,1,1); |
---|
| 577 | dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; |
---|
| 578 | for (i = 0; i < boxes; i++) |
---|
| 579 | dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides); |
---|
| 580 | dsSetColor (1,1,1); |
---|
| 581 | for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]), |
---|
| 582 | dGeomGetRotation(sphere[i]),RADIUS); |
---|
| 583 | |
---|
| 584 | // draw the cannon |
---|
| 585 | dsSetColor (1,1,0); |
---|
| 586 | dMatrix3 R2,R3,R4; |
---|
| 587 | dRFromAxisAndAngle (R2,0,0,1,cannon_angle); |
---|
| 588 | dRFromAxisAndAngle (R3,0,1,0,cannon_elevation); |
---|
| 589 | dMultiply0 (R4,R2,R3,3,3,3); |
---|
| 590 | dReal cpos[3] = {CANNON_X,CANNON_Y,1}; |
---|
| 591 | dReal csides[3] = {2,2,2}; |
---|
| 592 | dsDrawBox (cpos,R2,csides); |
---|
| 593 | for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2]; |
---|
| 594 | dsDrawCylinder (cpos,R4,3,0.5); |
---|
| 595 | |
---|
| 596 | // draw the cannon ball |
---|
| 597 | dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body), |
---|
| 598 | CANNON_BALL_RADIUS); |
---|
| 599 | } |
---|
| 600 | |
---|
| 601 | int main (int argc, char **argv) |
---|
| 602 | { |
---|
| 603 | doFast = true; |
---|
| 604 | |
---|
| 605 | // setup pointers to drawstuff callback functions |
---|
| 606 | dsFunctions fn; |
---|
| 607 | fn.version = DS_VERSION; |
---|
| 608 | fn.start = &start; |
---|
| 609 | fn.step = &simLoop; |
---|
| 610 | fn.command = &command; |
---|
| 611 | fn.stop = 0; |
---|
| 612 | fn.path_to_textures = "../../drawstuff/textures"; |
---|
| 613 | if(argc==2) |
---|
| 614 | { |
---|
| 615 | fn.path_to_textures = argv[1]; |
---|
| 616 | } |
---|
| 617 | |
---|
| 618 | dInitODE(); |
---|
| 619 | |
---|
| 620 | bodies = 0; |
---|
| 621 | joints = 0; |
---|
| 622 | boxes = 0; |
---|
| 623 | spheres = 0; |
---|
| 624 | |
---|
| 625 | resetSimulation(); |
---|
| 626 | |
---|
| 627 | // run simulation |
---|
| 628 | dsSimulationLoop (argc,argv,352,288,&fn); |
---|
| 629 | |
---|
| 630 | dJointGroupDestroy (contactgroup); |
---|
| 631 | dSpaceDestroy (space); |
---|
| 632 | dWorldDestroy (world); |
---|
| 633 | dCloseODE(); |
---|
| 634 | return 0; |
---|
| 635 | } |
---|