vcs.maemo.org Git - neverball/blob - share/solid_sim_ode.c

   1 #include <ode/ode.h>
   2 #include <stdio.h>
   3
   4 #include "solid.h"
   5 #include "solid_sim.h"
   6 #include "solid_all.h"
   7 #include "solid_cmd.h"
   8
   9 #include "array.h"
  10 #include "vec3.h"
  11 #include "cmd.h"
  12
  13 /*---------------------------------------------------------------------------*/
  14
  15 static dWorldID      world;
  16 static dSpaceID      space;
  17 static dJointGroupID group;
  18
  19 static Array lumps;
  20
  21 static dBodyID *bodies;
  22 static dBodyID *balls;
  23
  24 /*---------------------------------------------------------------------------*/
  25
  26 /*
  27  * Polygons in the same order as planes, one polygon per plane, each
  28  * starting with the number of vertices, followed by the vertex
  29  * indices in counter-clockwise order.
  30  */
  31
  32 struct convex
  33 {
  34     dReal    *planes;
  35     unsigned  nplanes;
  36     dReal    *points;
  37     unsigned  npoints;
  38     unsigned *polygons;
  39 };
  40
  41 static void make_convex(struct convex *cx, struct s_file *fp, struct s_lump *lp)
  42 {
  43     int i;
  44
  45     /*
  46      * A debug build of ODE (0.11.1 as I write this) will print a
  47      * bunch of warnings about our planes not containing the origin
  48      * and vertices having the wrong winding.  The latter is triggered
  49      * by presuming that the former shall be fulfilled, but the former
  50      * basically doesn't matter; collision detection handles
  51      * origin-facing planes just fine, provided the vertices on the
  52      * plane are still counter-clockwise wrt the plane normal.
  53      */
  54
  55     memset(cx, 0, sizeof (*cx));
  56
  57     if ((cx->planes = malloc(sizeof (dReal) * 4 * lp->sc)))
  58     {
  59         for (i = 0; i < lp->sc; i++)
  60         {
  61             struct s_side *sp = fp->sv + fp->iv[lp->s0 + i];
  62
  63             cx->planes[i * 4 + 0] = sp->n[0];
  64             cx->planes[i * 4 + 1] = sp->n[1];
  65             cx->planes[i * 4 + 2] = sp->n[2];
  66             cx->planes[i * 4 + 3] = sp->d;
  67         }
  68
  69         cx->nplanes = lp->sc;
  70     }
  71
  72     if ((cx->points = malloc(sizeof (dReal) * 3 * lp->vc)))
  73     {
  74         for (i = 0; i < lp->vc; i++)
  75         {
  76             struct s_vert *vp = fp->vv + fp->iv[lp->v0 + i];
  77
  78             cx->points[i * 3 + 0] = vp->p[0];
  79             cx->points[i * 3 + 1] = vp->p[1];
  80             cx->points[i * 3 + 2] = vp->p[2];
  81         }
  82
  83         cx->npoints = lp->vc;
  84     }
  85
  86     if ((cx->polygons = malloc(sizeof (unsigned) * lp->fc)))
  87         for (i = 0; i < lp->fc; i++)
  88             cx->polygons[i] = fp->iv[lp->f0 + i];
  89 }
  90
  91 void free_convex(struct convex *cx)
  92 {
  93     free(cx->planes);
  94     free(cx->points);
  95     free(cx->polygons);
  96 }
  97
  98 /*---------------------------------------------------------------------------*/
  99
 100 #define CAT_OBJECT 0x1
 101 #define CAT_WORLD  0x2
 102
 103 static void build_node(dBodyID body, dSpaceID parent,
 104                        struct s_node *np,
 105                        struct s_file *fp)
 106 {
 107     dSpaceID node = dHashSpaceCreate(parent);
 108     dGeomID geom;
 109     int i;
 110
 111     for (i = 0; i < np->lc; i++)
 112     {
 113         struct s_lump *lp = fp->lv + np->l0 + i;
 114         struct convex *cx;
 115
 116         if (lp->fl & L_DETAIL)
 117             continue;
 118
 119         cx = array_add(lumps);
 120
 121         make_convex(cx, fp, lp);
 122
 123         geom = dCreateConvex(node,
 124                              cx->planes, cx->nplanes,
 125                              cx->points, cx->npoints,
 126                              cx->polygons);
 127
 128         dGeomSetCategoryBits(geom, CAT_WORLD);
 129         dGeomSetCollideBits (geom, CAT_OBJECT);
 130
 131         if (body) dGeomSetBody(geom, body);
 132     }
 133
 134     if (np->ni >= 0) build_node(body, node, fp->nv + np->ni, fp);
 135     if (np->nj >= 0) build_node(body, node, fp->nv + np->nj, fp);
 136 }
 137
 138 static void build_branch(dBodyID body, dSpaceID root,
 139                          struct s_body *bp,
 140                          struct s_file *fp)
 141 {
 142     dSpaceID branch = dHashSpaceCreate(root);
 143
 144     build_node(body, branch, fp->nv + bp->ni, fp);
 145 }
 146
 147 void sol_init_sim(struct s_file *fp)
 148 {
 149     dGeomID geom;
 150
 151     int i;
 152
 153     dInitODE();
 154
 155     world  = dWorldCreate();
 156     space  = dHashSpaceCreate(0);
 157     group  = dJointGroupCreate(0);
 158
 159     lumps  = array_new(sizeof (struct convex));
 160
 161     bodies = calloc(fp->bc, sizeof (dBodyID));
 162     balls  = calloc(fp->uc, sizeof (dBodyID));
 163
 164     for (i = 0; i < fp->bc; i++)
 165     {
 166         struct s_body *bp = fp->bv + i;
 167
 168         if (bp->pi >= 0)
 169         {
 170             bodies[i] = dBodyCreate(world);
 171             dBodySetKinematic(bodies[i]);
 172         }
 173         else
 174             bodies[i] = 0;
 175
 176         build_branch(bodies[i], space, bp, fp);
 177
 178         if (bodies[i])
 179         {
 180             float p[3], w[3];
 181
 182             sol_body_p(p, fp, bp->pi, 0.0f);
 183             sol_body_w(w, fp, bp);
 184
 185             dBodySetPosition(bodies[i], p[0], p[1], p[2]);
 186             dBodySetAngularVel(bodies[i], w[0], w[1], w[2]);
 187         }
 188     }
 189
 190     for (i = 0; i < fp->uc; i++)
 191     {
 192         balls[i] = dBodyCreate(world);
 193
 194         geom = dCreateSphere(space, fp->uv[i].r);
 195
 196         dGeomSetCategoryBits(geom, CAT_OBJECT);
 197         dGeomSetCollideBits(geom, CAT_WORLD);
 198         dGeomSetData(geom, fp->uv + i);
 199         dGeomSetBody(geom, balls[i]);
 200
 201         dBodySetPosition(balls[i],
 202                          fp->uv[i].p[0],
 203                          fp->uv[i].p[1],
 204                          fp->uv[i].p[2]);
 205     }
 206 }
 207
 208 void sol_quit_sim(void)
 209 {
 210     free(balls);
 211     free(bodies);
 212
 213     while (array_len(lumps))
 214     {
 215         struct convex *cx = array_get(lumps, array_len(lumps) - 1);
 216         free_convex(cx);
 217         array_del(lumps);
 218     }
 219
 220     array_free(lumps);
 221
 222     dJointGroupDestroy(group);
 223     dSpaceDestroy(space);
 224     dWorldDestroy(world);
 225
 226     dCloseODE();
 227 }
 228
 229 /*---------------------------------------------------------------------------*/
 230
 231 static const dVector3 v_null = { 0.0, 0.0, 0.0 };
 232
 233 static const dReal *geomvel(dGeomID g)
 234 {
 235     dBodyID b;
 236     return (b = dGeomGetBody(g)) ? dBodyGetLinearVel(b) : v_null;
 237 }
 238
 239 static const dReal *pointvel(dGeomID g, dVector3 p)
 240 {
 241     dBodyID b;
 242
 243     if ((b = dGeomGetBody(g)))
 244     {
 245         static dVector3 v;
 246         dBodyGetPointVel(b, p[0], p[1], p[2], v);
 247         return v;
 248     }
 249     else
 250         return v_null;
 251 }
 252
 253 /*
 254  * Compute the "energy" of the impact, to determine the sound amplitude.
 255  */
 256 static float bump(dContactGeom *contact)
 257 {
 258     float r[3];
 259     v_sub(r, geomvel(contact->g1), pointvel(contact->g2, contact->pos));
 260     return fabsf(v_dot(contact->normal, r));
 261 }
 262
 263 static void spin(dContactGeom *contact)
 264 {
 265     struct s_ball *up;
 266     const dReal *p, *v, *w;
 267     float r[3], d[3];
 268
 269     if ((up = dGeomGetData(contact->g1)))
 270     {
 271
 272         p = dGeomGetPosition(contact->g1);
 273         v = geomvel(contact->g1);
 274         w = pointvel(contact->g2, contact->pos);
 275     }
 276     else
 277     {
 278         up = dGeomGetData(contact->g2);
 279
 280         p = dGeomGetPosition(contact->g2);
 281         v = geomvel(contact->g2);
 282         w = pointvel(contact->g1, contact->pos);
 283     }
 284
 285     v_sub(r, p, contact->pos);
 286     v_sub(d, v, w);
 287
 288     /* Find the new angular velocity. */
 289
 290     v_crs(up->w, d, r);
 291     v_scl(up->w, up->w, -1.0f / (up->r * up->r));
 292 }
 293
 294 static void collide(void *data, dGeomID o1, dGeomID o2)
 295 {
 296     dContact contacts[8];
 297     dJointID joint;
 298
 299     int n, i;
 300
 301     float *b = (float *) data;
 302     float  d;
 303
 304     if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
 305     {
 306         dSpaceCollide2(o1, o2, data, collide);
 307         return;
 308     }
 309
 310     if ((n = dCollide(o1, o2, 8, &contacts[0].geom, sizeof (dContact))) > 0)
 311     {
 312         for (i = 0; i < n; i++)
 313         {
 314             if (*b < (d = bump(&contacts[i].geom)))
 315                 *b = d;
 316
 317             spin(&contacts[i].geom);
 318
 319             contacts[i].surface.mode       = dContactBounce;
 320             contacts[i].surface.mu         = 0;
 321             contacts[i].surface.bounce     = 0.718;
 322             contacts[i].surface.bounce_vel = 0;
 323
 324             joint = dJointCreateContact(world, group, contacts + i);
 325
 326             dJointAttach(joint, dGeomGetBody(o1), dGeomGetBody(o2));
 327         }
 328     }
 329 }
 330
 331 /*---------------------------------------------------------------------------*/
 332
 333 static void import_state(struct s_file *fp, float dt)
 334 {
 335     int i;
 336
 337     for (i = 0; i < fp->uc; i++)
 338     {
 339         dBodySetPosition(balls[i],
 340                          fp->uv[i].p[0],
 341                          fp->uv[i].p[1],
 342                          fp->uv[i].p[2]);
 343
 344         dBodySetLinearVel(balls[i],
 345                           fp->uv[i].v[0],
 346                           fp->uv[i].v[1],
 347                           fp->uv[i].v[2]);
 348
 349         dGeomSphereSetRadius(dBodyGetFirstGeom(balls[i]), fp->uv[i].r);
 350     }
 351
 352     for (i = 0; i < fp->bc; i++)
 353     {
 354         struct s_body *bp = fp->bv + i;
 355         float p[3], v[3], w[3];
 356
 357         if (bodies[i])
 358         {
 359             sol_body_p(p, fp, bp->pi, bp->t);
 360             sol_body_v(v, fp, bp->pi, bp->t, dt);
 361             sol_body_w(w, fp, bp);
 362
 363             dBodySetPosition(bodies[i], p[0], p[1], p[2]);
 364             dBodySetLinearVel(bodies[i], v[0], v[1], v[2]);
 365             dBodySetAngularVel(bodies[i], w[0], w[1], w[2]);
 366         }
 367     }
 368 }
 369
 370 static void export_state(struct s_file *fp)
 371 {
 372     const dReal *v;
 373     int i;
 374
 375     for (i = 0; i < fp->uc; i++)
 376     {
 377         struct s_ball *up = fp->uv + i;
 378
 379         v = dBodyGetPosition(balls[i]);
 380         v_cpy(up->p, v);
 381
 382         v = dBodyGetLinearVel(balls[i]);
 383         v_cpy(up->v, v);
 384     }
 385
 386     for (i = 0; i < fp->bc; i++)
 387     {
 388         struct s_body *bp = fp->bv + i;
 389
 390         if (bodies[i])
 391         {
 392             v = dBodyGetQuaternion(bodies[i]);
 393             q_cpy(bp->e, v);
 394         }
 395     }
 396 }
 397
 398 /*---------------------------------------------------------------------------*/
 399
 400 float sol_step(struct s_file *fp, const float *g, float dt, int ui, int *m)
 401 {
 402     union cmd cmd;
 403     float b = 0.0f;
 404
 405     /* The simulation is advanced only once, commands don't accumulate. */
 406
 407     sol_cmd_defer = 0;
 408
 409     import_state(fp, dt);
 410
 411     dSpaceCollide(space, &b, collide);
 412     dWorldSetGravity(world, g[0], g[1], g[2]);
 413     dWorldQuickStep(world, dt);
 414     dJointGroupEmpty(group);
 415
 416     sol_body_step(fp, dt);
 417     sol_swch_step(fp, dt);
 418     sol_ball_step(fp, dt);
 419
 420     export_state(fp);
 421
 422     cmd.type       = CMD_STEP_SIMULATION;
 423     cmd.stepsim.dt = dt;
 424     sol_cmd_enq(&cmd);
 425
 426     return b;
 427 }
 428
 429 /*---------------------------------------------------------------------------*/