?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
generate platform_ops.h from platform_ops.proto, and reduce dependencies
[proto.git] / src / sim / odedynamics.cpp
blobc5841d6c7de540a5b34802747341f0981862698f
1 /* Newtonian physics simulation using the Open Dynamics Engine
2 Copyright (C) 2005-2008, Jonathan Bachrach, Jacob Beal, and contributors
3 listed in the AUTHORS file in the MIT Proto distribution's top directory.
4
5 This file is part of MIT Proto, and is distributed under the terms of
6 the GNU General Public License, with a linking exception, as described
7 in the file LICENSE in the MIT Proto distribution's top directory. */
8
9 #include "config.h"
10 #include "odedynamics.h"
11 #include "visualizer.h"
12
13 /*****************************************************************************
14 * GEOM DRAWING *
15 *****************************************************************************/
16 // Note: this largely duplicates some of the code in drawing_primitives.cpp
17
18 static void set_transform(const float pos[3], const float R[12]) {
19 #ifdef WANT_GLUT
20 GLfloat matrix[16];
21 matrix[0]=R[0]; matrix[1]=R[4]; matrix[2]=R[8]; matrix[3]=0;
22 matrix[4]=R[1]; matrix[5]=R[5]; matrix[6]=R[9]; matrix[7]=0;
23 matrix[8]=R[2]; matrix[9]=R[6]; matrix[10]=R[10]; matrix[11]=0;
24 matrix[12]=pos[0]; matrix[13]=pos[1]; matrix[14]=pos[2]; matrix[15]=1;
25 glPushMatrix();
26 glMultMatrixf(matrix);
27 #endif // WANT_GLUT
28 }
29
30 static void do_draw_wire_box(const float sides[3]) {
31 #ifdef WANT_GLUT
32 float w = sides[0]/2, h = sides[1]/2, d = sides[2]/2;
33 glBegin( GL_LINE_STRIP );
34 // top
35 glVertex3d(-w, -h, -d);
36 glVertex3d(-w, h, -d);
37 glVertex3d( w, h, -d);
38 glVertex3d( w, -h, -d);
39 glVertex3d(-w, -h, -d);
40 // bot
41 glVertex3d(-w, -h, d);
42 glVertex3d(-w, h, d);
43 glVertex3d( w, h, d);
44 glVertex3d( w, -h, d);
45 glVertex3d(-w, -h, d);
46 // sides
47 glVertex3d( w, -h, d);
48 glVertex3d( w, -h, -d);
49 glVertex3d( w, h, -d);
50 glVertex3d( w, h, d);
51 glVertex3d(-w, h, d);
52 glVertex3d(-w, h, -d);
53 glEnd();
54 #endif // WANT_GLUT
55 }
56
57
58 static void do_draw_box(const float sides[3]) {
59 #ifdef WANT_GLUT
60 float lx = sides[0]*0.5f, ly = sides[1]*0.5f, lz = sides[2]*0.5f;
61 // sides
62 glBegin (GL_TRIANGLE_STRIP);
63 glNormal3f (-1,0,0);
64 glVertex3f (-lx,-ly,-lz);
65 glVertex3f (-lx,-ly,lz);
66 glVertex3f (-lx,ly,-lz);
67 glVertex3f (-lx,ly,lz);
68 glNormal3f (0,1,0);
69 glVertex3f (lx,ly,-lz);
70 glVertex3f (lx,ly,lz);
71 glNormal3f (1,0,0);
72 glVertex3f (lx,-ly,-lz);
73 glVertex3f (lx,-ly,lz);
74 glNormal3f (0,-1,0);
75 glVertex3f (-lx,-ly,-lz);
76 glVertex3f (-lx,-ly,lz);
77 glEnd();
78 // top face
79 glBegin (GL_TRIANGLE_FAN);
80 glNormal3f (0,0,1);
81 glVertex3f (-lx,-ly,lz);
82 glVertex3f (lx,-ly,lz);
83 glVertex3f (lx,ly,lz);
84 glVertex3f (-lx,ly,lz);
85 glEnd();
86 // bottom face
87 glBegin (GL_TRIANGLE_FAN);
88 glNormal3f (0,0,-1);
89 glVertex3f (-lx,-ly,-lz);
90 glVertex3f (-lx,ly,-lz);
91 glVertex3f (lx,ly,-lz);
92 glVertex3f (lx,-ly,-lz);
93 glEnd();
94 #endif // WANT_GLUT
95 }
96
97 void draw_box(const float *pos, const float *R, const float *sides) {
98 #ifdef WANT_GLUT
99 set_transform(pos, R);
100 do_draw_box(sides);
101 glPopMatrix();
102 #endif // WANT_GLUT
103 }
104
105 void draw_wire_box(const float *pos, const float *R, const float *sides) {
106 #ifdef WANT_GLUT
107 set_transform(pos, R);
108 do_draw_wire_box(sides);
109 glPopMatrix();
110 #endif // WANT_GLUT
111 }
112
113 /*****************************************************************************
114 * ODE BODY *
115 *****************************************************************************/
116
117 ODEBody::ODEBody(ODEDynamics *parent, Device* container, flo x, flo y, flo z,
118 flo r) : Body(container) {
119 this->parent=parent; moved=FALSE;
120 for(int i=0;i<3;i++) desired_v[i]=0;
121 did_bump=false;
122 // create and attach body, shape, and mass
123 body = dBodyCreate(parent->world);
124 geom = dCreateBox(parent->space,r*2,r*2,r*2);
125 dMass m; dMassSetBox(&m,parent->density,r*2,r*2,r*2);
126 dGeomSetBody(geom,body); dBodySetMass(body,&m);
127 // set position and orientation
128 dBodySetPosition(body, x, y, z);
129 dQuaternion Q; dQFromAxisAndAngle (Q,0,0,1,0);
130 dBodySetQuaternion (body,Q);
131 // set up back-pointer
132 dBodySetData(body,(void*)this);
133 dGeomSetData(geom,(void*)this);
134 }
135
136 ODEBody::~ODEBody() {
137 dBodyDestroy(body); dGeomDestroy(geom);
138 parent->bodies.remove(parloc);
139 }
140
141 void ODEBody::visualize() {
142 #ifdef WANT_GLUT
143 if(!parent->is_show_bot) return; // don't display unless should be shown
144
145 // get location/size information
146 const flo pos[3] = {0,0,0};
147 const dReal *R = dGeomGetRotation(geom);
148 dVector3 sides; dGeomBoxGetLengths(geom,sides);
149
150 // choose base color
151 BOOL pushed=FALSE;
152 if(container->is_selected) { palette->use_color(ODE_SELECTED);
153 } else if(!dBodyIsEnabled(body)) { palette->use_color(ODE_DISABLED);
154 } else {
155 if(parent->is_multicolored_bots) {
156 flo h = (360.0*parloc)/parent->bodies.max_id();
157 flo r, g, b; hsv_to_rgb(h, 1, 1, &r, &g, &b);
158 palette->push_color(ODE_BOT,r/255,g/255,b/255,0.7);
159 pushed=TRUE;
160 }
161 palette->use_color(ODE_BOT);
162 }
163 draw_box(pos,R,sides);
164 palette->use_color(ODE_EDGES); // draw edges in separate color
165 draw_wire_box(pos,R,sides);
166 if(pushed) palette->pop_color(ODE_BOT);
167 #endif // WANT_GLUT
168 }
169
170 void ODEBody::render_selection() {
171 const flo pos[3] = {0,0,0};
172 const dReal *R = dGeomGetRotation(geom);
173 dVector3 sides; dGeomBoxGetLengths(geom,sides);
174 draw_box(pos,R,sides);
175 }
176
177 void ODEBody::dump_state(FILE* out, int verbosity) {
178 if(verbosity==0) {
179 const flo *v = position(); fprintf(out," %.2f %.2f %.2f",v[0],v[1],v[2]);
180 v = velocity(); fprintf(out," %.2f %.2f %.2f",v[0],v[1],v[2]);
181 v = orientation(); fprintf(out," %.2f %.2f %.2f %.2f",v[0],v[1],v[2],v[3]);
182 v = ang_velocity(); fprintf(out," %.2f %.2f %.2f",v[0],v[1],v[2]);
183 } else {
184 const flo *v = position();
185 fprintf(out,"Position=[%.2f %.2f %.2f], ",v[0],v[1],v[2]);
186 v = velocity();
187 fprintf(out,"Velocity=[%.2f %.2f %.2f] (Speed=%.2f)\n",v[0],v[1],v[2],
188 sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
189 v = orientation();
190 fprintf(out,"Orientation=[%.2f %.2f %.2f %.2f], ",v[0],v[1],v[2],v[3]);
191 v = ang_velocity();
192 fprintf(out,"Angular Velocity=[%.2f %.2f %.2f]\n",v[0],v[1],v[2]);
193 }
194 }
195
196 #define K_MOVE 10
197 void ODEBody::drive() {
198 const dReal *cur = velocity();
199 dReal force[3];
200 force[0] = K_MOVE * (desired_v[0] - cur[0]);
201 force[1] = K_MOVE * (desired_v[1] - cur[1]);
202 force[2] = (parent->parent->is_3d())? K_MOVE * (desired_v[2] - cur[2]) : 0;
203 dBodyAddForce(body, force[0], force[1], force[2]);
204 }
205
206 /*****************************************************************************
207 * COLLISION HANDLING *
208 *****************************************************************************/
209
210 #define WALL_DATA -1 // identifier for walls
211 #define MAX_CONTACTS 8 // maximum number of contact points per body
212
213 static int isWall (dGeomID g) { return dGeomGetData(g)==((void*)WALL_DATA); }
214
215 static void nearCallback (void *data, dGeomID o1, dGeomID o2) {
216 int i, numc;
217 ODEDynamics *dyn = (ODEDynamics*)data;
218
219 // exit without doing anything if the two bodies are connected by a joint
220 dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2);
221 if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) return;
222 // exit without doing anything if the walls are off and one object is a wall
223 if(!dyn->is_walls && (isWall(o1) || isWall(o2))) { return; }
224
225 dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per pair
226 if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
227 sizeof(dContact))) {
228 for (i=0; i<numc; i++) {
229 contact[i].surface.mode = dContactBounce | dContactSoftCFM;
230 contact[i].surface.mu = 0;
231 // contact[i].surface.mu = dInfinity;
232 contact[i].surface.mu2 = 0;
233 // contact[i].surface.bounce = 0.1;
234 contact[i].surface.bounce = 0.5;
235 contact[i].surface.bounce_vel = 0.1;
236 contact[i].surface.soft_cfm = 0.0; // give
237 dJointID c=dJointCreateContact(dyn->world,dyn->contactgroup,&contact[i]);
238 dJointAttach(c,b1,b2);
239 }
240 // record bumps
241 if(b1 && (b2 || isWall(o2))) ((ODEBody*)dBodyGetData(b1))->did_bump=TRUE;
242 if(b2 && (b1 || isWall(o1))) ((ODEBody*)dBodyGetData(b2))->did_bump=TRUE;
243 }
244 }
245
246 /*****************************************************************************
247 * ODE DYNAMICS *
248 *****************************************************************************/
249
250 #define DENSITY (0.125/2) // default density
251 #define MAX_V 100 // default ceiling on velocity
252 #define SUBSTEP 0.001 // default sub-step size
253 #define K_BODY_RAD 0.0870 // constant matched against previous visualization
254
255 void ODEDynamics::make_walls() {
256 flo pen_w=parent->volume->r - parent->volume->l;
257 flo pen_h=parent->volume->t - parent->volume->b;
258 flo wall_width = 5;
259 dQuaternion Q;
260
261 pen = dCreateBox(0,pen_w+2*wall_width,pen_h+2*wall_width,10*pen_h);
262
263 // side walls
264 dQFromAxisAndAngle (Q,0,0,1,M_PI/2);
265 walls[0] = dCreateBox(space, pen_h+2*wall_width, wall_width, 10*pen_h);
266 dGeomSetQuaternion (walls[0],Q);
267 dGeomSetPosition (walls[0], pen_w/2+wall_width/2, 0, 0);
268 dGeomSetData(walls[0], (void*)WALL_DATA);
269
270 walls[1] = dCreateBox(space, pen_h+2*wall_width, wall_width, 10*pen_h);
271 dGeomSetQuaternion (walls[1],Q);
272 dGeomSetPosition (walls[1], -pen_w/2-wall_width/2, 0, 0);
273 dGeomSetData(walls[1], (void*)WALL_DATA);
274
275 dQFromAxisAndAngle (Q,0,0,1,0);
276 walls[2] = dCreateBox(space, pen_w+2*wall_width, wall_width, 10*pen_h);
277 dGeomSetQuaternion (walls[2],Q);
278 dGeomSetPosition (walls[2], 0, pen_h/2+wall_width/2, 0);
279 dGeomSetData(walls[2], (void*)WALL_DATA);
280
281 walls[3] = dCreateBox(space, pen_w+2*wall_width, wall_width, 10*pen_h);
282 dGeomSetQuaternion (walls[3],Q);
283 dGeomSetPosition (walls[3], 0, -pen_h/2-wall_width/2, 0);
284 dGeomSetData(walls[3], (void*)WALL_DATA);
285
286 // corner walls
287 dQFromAxisAndAngle (Q,0,0,1,M_PI/4);
288
289 walls[4] = dCreateBox(space, 2*wall_width, 2*wall_width, 10*pen_h);
290 dGeomSetQuaternion (walls[4],Q);
291 dGeomSetPosition (walls[4], -pen_w/2, -pen_h/2, 0);
292 dGeomSetData(walls[4], (void*)WALL_DATA);
293
294 walls[5] = dCreateBox(space, 2*wall_width, 2*wall_width, 10*pen_h);
295 dGeomSetQuaternion (walls[5],Q);
296 dGeomSetPosition (walls[5], pen_w/2, -pen_h/2, 0);
297 dGeomSetData(walls[5], (void*)WALL_DATA);
298
299 walls[6] = dCreateBox(space, 2*wall_width, 2*wall_width, 10*pen_h);
300 dGeomSetQuaternion (walls[6],Q);
301 dGeomSetPosition (walls[6], pen_w/2, pen_h/2, 0);
302 dGeomSetData(walls[6], (void*)WALL_DATA);
303
304 walls[7] = dCreateBox(space, 2*wall_width, 2*wall_width, 10*pen_h);
305 dGeomSetQuaternion (walls[7],Q);
306 dGeomSetPosition (walls[7], -pen_w/2, pen_h/2, 0);
307 dGeomSetData(walls[7], (void*)WALL_DATA);
308 }
309
310 // Note: ODE's manual claims that dCloseODE is optional
311 BOOL ODEDynamics::inited = FALSE;
312 ODEDynamics::ODEDynamics(Args* args, SpatialComputer* p, int n)
313 : BodyDynamics(p) {
314 if(!inited) { dInitODE(); inited=TRUE; } // initialize if not yet done so
315 time_slop=0;
316 flo width=parent->volume->r - parent->volume->l;
317 flo height=parent->volume->t - parent->volume->b;
318 // read options
319 body_radius = (args->extract_switch("-rad")) ? args->pop_number()
320 : K_BODY_RAD*sqrt((width*height)/(flo)n);
321 density = (args->extract_switch("-density")) ? args->pop_number() : DENSITY;
322 speed_lim = (args->extract_switch("-S"))?args->pop_number():MAX_V;
323 substep = (args->extract_switch("-substep"))?args->pop_number() : SUBSTEP;
324 if(substep <= 0) {
325 post("-substep must be greater than zero; using default %f",SUBSTEP);
326 substep = SUBSTEP;
327 }
328 is_mobile = args->extract_switch("-m");
329 is_show_bot = !args->extract_switch("-hide-body");
330 is_walls = ((args->extract_switch("-w") & !args->extract_switch("-nw")))
331 ? TRUE : FALSE;
332 is_draw_walls = args->extract_switch("-draw-walls");
333 is_inescapable = args->extract_switch("-inescapable");
334 is_multicolored_bots = args->extract_switch("-rainbow-bots");
335 args->undefault(&can_dump,"-Ddynamics","-NDdynamics");
336 // register to simulate hardware
337 parent->hardware.patch(this,MOV_FN);
338 parent->hardware.patch(this,READ_BUMP_FN);
339 // Initialize ODE and make the walls
340 world = dWorldCreate();
341 space = dHashSpaceCreate(0);
342 contactgroup = dJointGroupCreate(0);
343 if(p->volume->dimensions()==2) dWorldSetGravity(world,0,0,-9.81);
344 dWorldSetCFM(world,1e-5);
345 dWorldSetAutoDisableFlag(world,0); // nothing ever disables
346 dWorldSetAutoDisableAverageSamplesCount(world, 10);
347 dWorldSetContactMaxCorrectingVel(world,0.1);
348 dWorldSetContactSurfaceLayer(world,0.001);
349 if(p->volume->dimensions()==2) dCreatePlane(space,0,0,1,-body_radius);
350 make_walls();
351 }
352
353 ODEDynamics::~ODEDynamics() {
354 dJointGroupDestroy(contactgroup);
355 for(int i=0;i<ODE_N_WALLS;i++) dGeomDestroy(walls[i]);
356 dGeomDestroy(pen);
357 dSpaceDestroy(space);
358 dWorldDestroy(world);
359 }
360
361 BOOL ODEDynamics::handle_key(KeyEvent* key) {
362 if(key->normal && !key->ctrl) {
363 switch(key->key) {
364 case 'w': is_walls = !is_walls; return TRUE;
365 case 'b': is_show_bot = !is_show_bot; return TRUE;
366 case 'm': is_mobile = !is_mobile; return TRUE;
367 }
368 }
369 return FALSE;
370 }
371
372 void ODEDynamics::visualize() {
373 #ifdef WANT_GLUT
374 if(is_walls && is_draw_walls) {
375 for(int i=0; i<ODE_N_WALLS; i++) {
376 const dReal *pos = dGeomGetPosition(walls[i]);
377 const dReal *R = dGeomGetRotation(walls[i]);
378 dVector3 sides; dGeomBoxGetLengths(walls[i],sides);
379 palette->use_color(ODE_WALL);
380 draw_box(pos,R,sides);
381 palette->use_color(ODE_EDGES);
382 draw_wire_box(pos,R,sides);
383 }
384 }
385 #endif // WANT_GLUT
386 }
387
388 Body* ODEDynamics::new_body(Device* d, flo x, flo y, flo z) {
389 ODEBody* b = new ODEBody(this,d,x,y,z,body_radius);
390 b->parloc = bodies.add(b);
391 return b;
392 }
393
394 void ODEDynamics::dump_header(FILE* out) {
395 if(can_dump) {
396 fprintf(out," \"X\" \"Y\" \"Z\"");
397 fprintf(out," \"V_X\" \"V_Y\" \"V_Z\"");
398 fprintf(out," \"Q_X\" \"Q_Y\" \"Q_Z\" \"Q_W\"");
399 fprintf(out," \"W_X\" \"W_Y\" \"W_Z\"");
400 }
401 }
402
403 BOOL ODEDynamics::evolve(SECONDS dt) {
404 if(!is_mobile) return FALSE;
405 time_slop+=dt;
406 while(time_slop>0) {
407 dSpaceCollide(space,this,&nearCallback);
408 // add forces
409 for(int i=0;i<bodies.max_id();i++)
410 { ODEBody* b = (ODEBody*)bodies.get(i); if(b) b->drive(); }
411 dWorldQuickStep(world,substep);
412 dJointGroupEmpty(contactgroup);
413 if(is_walls && is_inescapable) reset_escapes();
414 time_slop -= substep;
415 }
416 for(int i=0;i<bodies.max_id();i++) // mark everything as moved
417 { Body* b = (Body*)bodies.get(i); if(b) b->moved=TRUE; }
418 return TRUE;
419 }
420
421 // return escaped bots to a new starting position
422 void ODEDynamics::reset_escapes() {
423 for(int i=0;i<bodies.max_id();i++) {
424 ODEBody* b = (ODEBody*)bodies.get(i);
425 if(b) {
426 dContact contact;
427 if(dCollide(pen,b->geom,1,&contact.geom,sizeof(dContact))) continue;
428 // if not in pen, reset
429 const flo *p = b->position();
430 //post("You cannot escape! [%.2f %.2f %.2f]\n",p[0],p[1],p[2]);
431 METERS loc[3];
432 if(parent->distribution->next_location(loc)) {
433 b->set_position(loc[0],loc[1],loc[2]);
434 b->set_velocity(0,0,0); b->set_ang_velocity(0,0,0); // start still
435 } else { // if there's no starting position available, die instead
436 hardware->set_vm_context(b->container);
437 die(TRUE);
438 }
439 }
440 }
441 }
442
443 // hardware emulation
444 void ODEDynamics::mov(VEC_VAL *v) {
445 ODEBody* b = (ODEBody*)device->body;
446 b->desired_v[0] = NUM_GET(&v->elts[0]);
447 b->desired_v[1] = NUM_GET(&v->elts[1]);
448 b->desired_v[2] = v->n > 2 ? NUM_GET(&v->elts[2]) : 0.0;
449 dReal len = sqrt(b->desired_v[0]*b->desired_v[0] +
450 b->desired_v[1]*b->desired_v[1] +
451 b->desired_v[2]*b->desired_v[2]);
452 if(len>speed_lim) {
453 for(int i=0;i<3;i++) b->desired_v[i] *= speed_lim/len;
454 }
455 }
456
457 NUM_VAL ODEDynamics::read_bump (VOID) {
458 return (float)((ODEBody*)device->body)->did_bump;
459 }
Proto, the world's premier language for spatial computing!