src/sim/simpledynamics.cpp

   1 /* An extremely simple physics package
   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 "simpledynamics.h"
  11 #include "visualizer.h"
  12
  13 /*****************************************************************************
  14  *  VEKTOR OPS                                                               *
  15  *****************************************************************************/
  16 #define ND 3
  17 #define elt_at(x, i)   ((flo*)x)[i]
  18 #define elt_ati(x, i) &((flo*)x)[i]
  19 // This structure can be used to represent either a 3D point or 3D vector
  20
  21 static inline Vek* vek_fil (Vek *x, flo val) {
  22   int i;
  23   for (i=0; i<ND; i++)
  24     *elt_ati(x, i) = val;
  25   return x;
  26 }
  27
  28 static inline Vek* vek_mul (Vek* x, flo val) {
  29   int i;
  30   for (i=0; i<ND; i++)
  31     *elt_ati(x, i) *= val;
  32   return x;
  33 }
  34
  35 static void vek_out (Vek* x) {
  36   int i;
  37   for (i=0; i<ND; i++)
  38     post(" %.2f", elt_at(x, i));
  39 }
  40
  41 static inline Vek* vek_cpy (Vek* x, Vek* y) {
  42   int i;
  43   for (i=0; i<ND; i++)
  44     *elt_ati(x, i) = elt_at(y, i);
  45   return x;
  46 }
  47
  48 static inline Vek* vek_set (Vek* v, flo x, flo y, flo z) {
  49   v->x = x;
  50   v->y = y;
  51   v->z = z;
  52   return v;
  53 }
  54
  55 static inline Vek* vek_add (Vek* d, Vek* s) {
  56   int i;
  57   for (i=0; i<ND; i++)
  58     *elt_ati(d, i) += elt_at(s, i);
  59   return d;
  60 }
  61
  62 static inline Vek* vek_sub (Vek* d, Vek* s) {
  63   int i;
  64   for (i=0; i<ND; i++)
  65     *elt_ati(d, i) -= elt_at(s, i);
  66   return d;
  67 }
  68
  69 static inline flo vek_dot (Vek* x, Vek* y) {
  70   int i;
  71   flo d = 0.0;
  72   for (i=0; i<ND; i++)
  73     d += elt_at(x, i) * elt_at(y, i);
  74   return d;
  75 }
  76
  77 static inline flo vek_len_sqr (Vek* x) {
  78   return vek_dot(x, x);
  79 }
  80
  81 static inline flo vek_len (Vek* x) {
  82   return (flo)sqrt((double)vek_len_sqr(x));
  83 }
  84
  85 static inline flo vek_dst_sqr (Vek* x, Vek* y) {
  86   Vek t;
  87   vek_cpy(&t, x);
  88   vek_sub(&t, y);
  89   return vek_len_sqr(&t);
  90 }
  91
  92 static inline flo vek_dst (Vek* x, Vek* y) {
  93   return (flo)sqrt(vek_dst_sqr(x, y));
  94 }
  95
  96 /*****************************************************************************
  97  *  SIMPLE BODY                                                              *
  98  *****************************************************************************/
  99 static flo q[4] = {1,0,0,0}; // null orientation quaternion
 100 static flo w[3] = {0,0,0}; // null angular velocity vector
 101 const flo* SimpleBody::orientation() {return q; }
 102 const flo* SimpleBody::ang_velocity() { return w; }
 103
 104 SimpleBody::~SimpleBody() { parent->bodies.remove(parloc); }
 105
 106 void SimpleBody::preupdate() { set_velocity(0,0,0); }
 107
 108 // assumes display is centered
 109 void SimpleBody::visualize() {
 110 #ifdef WANT_GLUT
 111   flo x, y;
 112   if(!parent->is_show_bot) return; // don't display unless should be shown
 113   palette->use_color(SIMPLE_BODY);
 114   glPushMatrix();
 115   glScalef(radius, radius, radius);
 116   if (parent->is_mobile) {
 117     glLineWidth(2);
 118     if (parent->parent->volume->dimensions()==3) {
 119       glPushMatrix();
 120       glRotatef(90.0, 1.0, 0.0, 0.0);
 121       draw_circle(1);
 122       glPopMatrix();
 123       glPushMatrix();
 124       glRotatef(90.0, 0.0, 1.0, 0.0);
 125       draw_circle(1);
 126       glPopMatrix();
 127     }
 128     draw_circle(1);
 129     if (parent->is_show_heading) {
 130       glBegin(GL_LINES);
 131       glVertex2f(0, 0);
 132       Vek vec(v);
 133       flo l=vek_len(&vec);
 134       if(l>0) vek_mul(&vec,1/l); // normalize vel
 135       glVertex3f(vec.x, vec.y, vec.z);
 136       glEnd();
 137     }
 138   } else {
 139     glBegin(GL_POINTS);
 140     glVertex2f(0, 0);
 141     glEnd();
 142   }
 143   glPopMatrix();
 144 #endif // WANT_GLUT
 145 }
 146
 147 void SimpleBody::render_selection() {
 148 #ifdef WANT_GLUT
 149   flo x, y;
 150   glScalef(radius, radius, radius);
 151   if (parent->parent->volume->dimensions()==3) {
 152     glPushMatrix();
 153     glRotatef(90.0, 1.0, 0.0, 0.0);
 154     draw_disk(1);
 155     glPopMatrix();
 156     glPushMatrix();
 157     glRotatef(90.0, 0.0, 1.0, 0.0);
 158     draw_disk(1);
 159     glPopMatrix();
 160   }
 161   draw_disk(1);
 162 #endif // WANT_GLUT
 163 }
 164
 165 void SimpleBody::dump_state(FILE* out, int verbosity) {
 166   if(verbosity==0) {
 167     if(parent->dumpmask & 0x01) fprintf(out," %.2f",p[0]);
 168     if(parent->dumpmask & 0x02) fprintf(out," %.2f",p[1]);
 169     if(parent->dumpmask & 0x04) fprintf(out," %.2f",p[2]);
 170     if(parent->dumpmask & 0x08) fprintf(out," %.2f",v[0]);
 171     if(parent->dumpmask & 0x10) fprintf(out," %.2f",v[1]);
 172     if(parent->dumpmask & 0x20) fprintf(out," %.2f",v[2]);
 173     if(parent->dumpmask & 0x40) fprintf(out," %.2f",radius);
 174   } else {
 175     fprintf(out,"Position=[%.2f %.2f %.2f], ",p[0],p[1],p[2]);
 176     fprintf(out,"Velocity=[%.2f %.2f %.2f] (Speed=%.2f), ",v[0],v[1],v[2],
 177             sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
 178     fprintf(out,"Radius=%.2f\n",radius);
 179   }
 180 }
 181
 182 /*****************************************************************************
 183  *  BOUNDARIES                                                               *
 184  *****************************************************************************/
 185 Point north_wall_normal (  0.0, -1.0,  0.0 );
 186 Point south_wall_normal (  0.0,  1.0,  0.0 );
 187 Point east_wall_normal  ( -1.0,  0.0,  0.0 );
 188 Point west_wall_normal  (  1.0,  0.0,  0.0 );
 189 Point top_wall_normal   (  0.0,  0.0, -1.0 );
 190 Point bottom_wall_normal(  0.0,  0.0,  1.0 );
 191
 192 Point* wall_normals[N_WALLS]
 193   = { &north_wall_normal, &south_wall_normal, &east_wall_normal,
 194       &west_wall_normal, &top_wall_normal, &bottom_wall_normal };
 195
 196 Point north_wall_point  (  0.0,  1.0,  0.0 );
 197 Point south_wall_point  (  0.0, -1.0,  0.0 );
 198 Point east_wall_point   (  1.0,  0.0,  0.0 );
 199 Point west_wall_point   ( -1.0,  0.0,  0.0 );
 200 Point top_wall_point    (  0.0,  0.0,  1.0 );
 201 Point bottom_wall_point (  0.0,  0.0, -1.0 );
 202
 203 Point* wall_points[N_WALLS]
 204   = { &north_wall_point, &south_wall_point, &east_wall_point,
 205       &west_wall_point, &top_wall_point, &bottom_wall_point };
 206
 207 /*****************************************************************************
 208  *  SIMPLE DYNAMICS                                                          *
 209  *****************************************************************************/
 210 #define K_BODY_RAD 0.0870 // constant matched against previous visualization
 211 #define MAX_V 100 // default ceiling on velocity
 212 SimpleDynamics::SimpleDynamics(Args* args, SpatialComputer* parent, int n)
 213   : BodyDynamics(parent) {
 214   flo width=parent->volume->r - parent->volume->l;
 215   flo height=parent->volume->t - parent->volume->b;
 216   // read options
 217   body_radius = (args->extract_switch("-rad")) ? args->pop_number()
 218     : K_BODY_RAD*sqrt((width*height)/(flo)n);
 219   is_show_heading = args->extract_switch("-h"); // heading direction tick
 220   speed_lim = (args->extract_switch("-S"))?args->pop_number():MAX_V;
 221   is_walls = ((args->extract_switch("-w") & !args->extract_switch("-nw")))
 222     ? TRUE : FALSE;
 223   is_hard_floor = args->extract_switch("-floor"); // equiv to "stalk"
 224   is_mobile = args->extract_switch("-m");
 225   is_show_bot = !args->extract_switch("-hide-body");
 226   args->undefault(&can_dump,"-Ddynamics","-NDdynamics");
 227   dumpmask = args->extract_switch("-Ddynamics-mask") ?
 228     (int)args->pop_number() : -1;
 229   // make the walls
 230   for (int j=0; j<N_WALLS; j++) {
 231     walls[j].x = wall_points[j]->x*width*0.4;
 232     walls[j].y = wall_points[j]->y*height*0.4;
 233     if(parent->volume->dimensions()==3) {
 234       Rect3* r = (Rect3*)parent->volume;
 235       walls[j].z = wall_points[j]->z*(r->c-r->f)*0.4;
 236     } else {
 237       walls[j].z = wall_points[j]->z;
 238     }
 239   }
 240   // register to simulate hardware
 241   parent->hardware.patch(this,MOV_FN);
 242   parent->hardware.patch(this,RADIUS_SET_FN);
 243   parent->hardware.patch(this,RADIUS_GET_FN);
 244 }
 245
 246
 247 BOOL SimpleDynamics::handle_key(KeyEvent* key) {
 248   if(key->normal && !key->ctrl) {
 249     switch(key->key) {
 250     case 'w': is_walls = !is_walls; return TRUE;
 251     case 'b': is_show_bot = !is_show_bot; return TRUE;
 252     case 'm': is_mobile = !is_mobile; return TRUE;
 253     case 'h': is_show_heading = !is_show_heading; return TRUE;
 254     }
 255   }
 256   return FALSE;
 257 }
 258
 259 void SimpleDynamics::visualize() {
 260   // only the walls need any drawing here: all else goes through Device
 261   // and we aren't drawing walls, either
 262 }
 263
 264 Body* SimpleDynamics::new_body(Device* d, flo x, flo y, flo z) {
 265   SimpleBody* b = new SimpleBody(this,d,x,y,z,body_radius);
 266   b->parloc = bodies.add(b);
 267   return b;
 268 }
 269
 270 void SimpleDynamics::dump_header(FILE* out) {
 271   if(can_dump) {
 272     if(dumpmask & 0x01) fprintf(out," \"X\"");
 273     if(dumpmask & 0x02) fprintf(out," \"Y\"");
 274     if(dumpmask & 0x04) fprintf(out," \"Z\"");
 275     if(dumpmask & 0x08) fprintf(out," \"V_X\"");
 276     if(dumpmask & 0x10) fprintf(out," \"V_Y\"");
 277     if(dumpmask & 0x20) fprintf(out," \"V_Z\"");
 278     if(dumpmask & 0x40) fprintf(out," \"RADIUS\"");
 279   }
 280 }
 281
 282
 283 /*****************************************************************************
 284  *  INCREMENTAL EVOLUTION & ACTUATION                                        *
 285  *****************************************************************************/
 286 // Note: although the old sim-bot physics spoke of force, they act only at
 287 //  the level of velocity.
 288
 289 // in simple dynamics, just step based on velocity and position
 290 #define K_BOUND   0.75  // restoring force from walls
 291 BOOL SimpleDynamics::evolve(SECONDS dt) {
 292   if(!is_mobile) return FALSE;
 293   for(int i=0;i<bodies.max_id();i++) {
 294     Body* b = (Body*)bodies.get(i);
 295     if(b) {
 296       // device moves itself
 297       Vek dp(b->velocity());
 298       flo len = vek_dot(&dp,&dp);
 299       if (len > speed_lim) vek_mul(&dp,speed_lim/len); // limit velocity
 300       vek_mul(&dp,dt);
 301       // device is moved by walls
 302       if (is_walls) {
 303         for (int j=0; j<N_WALLS; j++) {
 304           Vek vec(b->position());
 305           vek_sub(&vec, &walls[j]);
 306           flo d = vek_dot(&vec, wall_normals[j]);
 307           if (d < 0.0) {
 308             vek_cpy(&vec, wall_normals[j]);
 309             vek_mul(&vec, -d * K_BOUND * dt);
 310             vek_add(&dp, &vec);
 311           }
 312         }
 313       }
 314       // adjust the position
 315       Vek pos(b->position());
 316       b->moved = (dp.x||dp.y||dp.z);
 317       vek_add(&pos,&dp);
 318       // Hard floor at z=0: if the calculated pos has z < 0, reset to 0
 319       if(is_hard_floor && pos.z<0) { pos.z=0; b->moved=TRUE; }
 320       b->set_position(pos.x,pos.y,pos.z);
 321     }
 322   }
 323   return TRUE;
 324 }
 325
 326
 327 // There is no subtlety here: mov just sets velocity directly
 328 void SimpleDynamics::mov(VEC_VAL *v) {
 329   flo x = NUM_GET(&v->elts[0]);
 330   flo y = NUM_GET(&v->elts[1]);
 331   flo z = v->n > 2 ? NUM_GET(&v->elts[2]) : 0.0;
 332   device->body->set_velocity(x,y,z);
 333 }
 334 // sensing & actuation of body radius
 335 NUM_VAL SimpleDynamics::radius_set (NUM_VAL val)
 336 { return ((SimpleBody*)device->body)->radius = val; }
 337 NUM_VAL SimpleDynamics::radius_get (VOID)
 338 { return ((SimpleBody*)device->body)->radius; }