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

   1 /*
   2  * Copyright (C) 2003 Robert Kooima
   3  *
   4  * NEVERBALL is  free software; you can redistribute  it and/or modify
   5  * it under the  terms of the GNU General  Public License as published
   6  * by the Free  Software Foundation; either version 2  of the License,
   7  * or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT  ANY  WARRANTY;  without   even  the  implied  warranty  of
  11  * MERCHANTABILITY or  FITNESS FOR A PARTICULAR PURPOSE.   See the GNU
  12  * General Public License for more details.
  13  */
  14
  15 #include <math.h>
  16
  17 #include "vec3.h"
  18 #include "common.h"
  19
  20 #include "solid.h"
  21 #include "solid_sim.h"
  22 #include "solid_all.h"
  23 #include "solid_cmd.h"
  24
  25 #define LARGE 1.0e+5f
  26 #define SMALL 1.0e-3f
  27
  28 /*---------------------------------------------------------------------------*/
  29 /* Solves (p + v * t) . (p + v * t) == r * r for smallest t.                 */
  30
  31 /*
  32  * Given vectors A = P, B = V * t, C = A + B, |C| = r, solve for
  33  * smallest t.
  34  *
  35  * Some useful dot product properties:
  36  *
  37  * 1) A . A = |A| * |A|
  38  * 2) A . (B + C) = A . B + A . C
  39  * 3) (r * A) . B = r * (A . B)
  40  *
  41  * Deriving a quadratic equation:
  42  *
  43  * C . C = r * r                                     (1)
  44  * (A + B) . (A + B) = r * r
  45  * A . (A + B) + B . (A + B) = r * r                 (2)
  46  * A . A + A . B + B . A + B . B = r * r             (2)
  47  * A . A + 2 * (A . B) + B . B = r * r
  48  * P . P + 2 * (P . V * t) + (V * t . V * t) = r * r
  49  * P . P + 2 * (P . V) * t + (V . V) * t * t = r * r (3)
  50  * (V . V) * t * t + 2 * (P . V) * t + P . P - r * r = 0
  51  *
  52  * This equation is solved using the quadratic formula.
  53  */
  54
  55 static float v_sol(const float p[3], const float v[3], float r)
  56 {
  57     float a = v_dot(v, v);
  58     float b = v_dot(v, p) * 2.0f;
  59     float c = v_dot(p, p) - r * r;
  60     float d = b * b - 4.0f * a * c;
  61
  62 /* HACK: This seems to cause failures to detect low-velocity collision
  63          Yet, the potential division by zero below seems fine.
  64     if (fabsf(a) < SMALL) return LARGE;
  65 */
  66
  67     if      (d < 0.0f) return LARGE;
  68     else if (d > 0.0f)
  69     {
  70         float t0 = 0.5f * (-b - fsqrtf(d)) / a;
  71         float t1 = 0.5f * (-b + fsqrtf(d)) / a;
  72         float t  = (t0 < t1) ? t0 : t1;
  73
  74         return (t < 0.0f) ? LARGE : t;
  75     }
  76     else return -b * 0.5f / a;
  77 }
  78
  79 /*---------------------------------------------------------------------------*/
  80
  81 /*
  82  * Compute the  earliest time  and position of  the intersection  of a
  83  * sphere and a vertex.
  84  *
  85  * The sphere has radius R and moves along vector V from point P.  The
  86  * vertex moves  along vector  W from point  Q in a  coordinate system
  87  * based at O.
  88  */
  89 static float v_vert(float Q[3],
  90                     const float o[3],
  91                     const float q[3],
  92                     const float w[3],
  93                     const float p[3],
  94                     const float v[3], float r)
  95 {
  96     float O[3], P[3], V[3];
  97     float t = LARGE;
  98
  99     v_add(O, o, q);
 100     v_sub(P, p, O);
 101     v_sub(V, v, w);
 102
 103     if (v_dot(P, V) < 0.0f)
 104     {
 105         t = v_sol(P, V, r);
 106
 107         if (t < LARGE)
 108             v_mad(Q, O, w, t);
 109     }
 110     return t;
 111 }
 112
 113 /*
 114  * Compute the  earliest time  and position of  the intersection  of a
 115  * sphere and an edge.
 116  *
 117  * The sphere has radius R and moves along vector V from point P.  The
 118  * edge moves along vector W from point Q in a coordinate system based
 119  * at O.  The edge extends along the length of vector U.
 120  */
 121 static float v_edge(float Q[3],
 122                     const float o[3],
 123                     const float q[3],
 124                     const float u[3],
 125                     const float w[3],
 126                     const float p[3],
 127                     const float v[3], float r)
 128 {
 129     float d[3], e[3];
 130     float P[3], V[3];
 131     float du, eu, uu, s, t;
 132
 133     v_sub(d, p, o);
 134     v_sub(d, d, q);
 135     v_sub(e, v, w);
 136
 137     /*
 138      * Think projections.  Vectors D, extending from the edge vertex Q
 139      * to the sphere,  and E, the relative velocity  of sphere wrt the
 140      * edge, are  made orthogonal to  the edge vector U.   Division of
 141      * the  dot products  is required  to obtain  the  true projection
 142      * ratios since U does not have unit length.
 143      */
 144
 145     du = v_dot(d, u);
 146     eu = v_dot(e, u);
 147     uu = v_dot(u, u);
 148
 149     v_mad(P, d, u, -du / uu);
 150     v_mad(V, e, u, -eu / uu);
 151
 152     t = v_sol(P, V, r);
 153     s = (du + eu * t) / uu; /* Projection of D + E * t on U. */
 154
 155     if (0.0f <= t && t < LARGE && 0.0f < s && s < 1.0f)
 156     {
 157         v_mad(d, o, w, t);
 158         v_mad(e, q, u, s);
 159         v_add(Q, e, d);
 160     }
 161     else
 162         t = LARGE;
 163
 164     return t;
 165 }
 166
 167 /*
 168  * Compute  the earliest  time and  position of  the intersection  of a
 169  * sphere and a plane.
 170  *
 171  * The sphere has radius R and moves along vector V from point P.  The
 172  * plane  moves  along  vector  W.   The  plane has  normal  N  and  is
 173  * positioned at distance D from the origin O along that normal.
 174  */
 175 static float v_side(float Q[3],
 176                     const float o[3],
 177                     const float w[3],
 178                     const float n[3], float d,
 179                     const float p[3],
 180                     const float v[3], float r)
 181 {
 182     float vn = v_dot(v, n);
 183     float wn = v_dot(w, n);
 184     float t  = LARGE;
 185
 186     if (vn - wn <= 0.0f)
 187     {
 188         float on = v_dot(o, n);
 189         float pn = v_dot(p, n);
 190
 191         float u = (r + d + on - pn) / (vn - wn);
 192         float a = (    d + on - pn) / (vn - wn);
 193
 194         if (0.0f <= u)
 195         {
 196             t = u;
 197
 198             v_mad(Q, p, v, +t);
 199             v_mad(Q, Q, n, -r);
 200         }
 201         else if (0.0f <= a)
 202         {
 203             t = 0;
 204
 205             v_mad(Q, p, v, +t);
 206             v_mad(Q, Q, n, -r);
 207         }
 208     }
 209     return t;
 210 }
 211
 212 /*---------------------------------------------------------------------------*/
 213
 214 /*
 215  * Compute the new  linear and angular velocities of  a bouncing ball.
 216  * Q  gives the  position  of the  point  of impact  and  W gives  the
 217  * velocity of the object being impacted.
 218  */
 219 static float sol_bounce(struct s_ball *up,
 220                         const float q[3],
 221                         const float w[3], float dt)
 222 {
 223     float n[3], r[3], d[3], vn, wn;
 224     float *p = up->p;
 225     float *v = up->v;
 226
 227     /* Find the normal of the impact. */
 228
 229     v_sub(r, p, q);
 230     v_sub(d, v, w);
 231     v_nrm(n, r);
 232
 233     /* Find the new angular velocity. */
 234
 235     v_crs(up->w, d, r);
 236     v_scl(up->w, up->w, -1.0f / (up->r * up->r));
 237
 238     /* Find the new linear velocity. */
 239
 240     vn = v_dot(v, n);
 241     wn = v_dot(w, n);
 242
 243     v_mad(v, v, n, 1.7 * (wn - vn));
 244
 245     v_mad(p, q, n, up->r);
 246
 247     /* Return the "energy" of the impact, to determine the sound amplitude. */
 248
 249     return fabsf(v_dot(n, d));
 250 }
 251
 252 /*---------------------------------------------------------------------------*/
 253
 254 static float sol_test_vert(float dt,
 255                            float T[3],
 256                            const struct s_ball *up,
 257                            const struct s_vert *vp,
 258                            const float o[3],
 259                            const float w[3])
 260 {
 261     return v_vert(T, o, vp->p, w, up->p, up->v, up->r);
 262 }
 263
 264 static float sol_test_edge(float dt,
 265                            float T[3],
 266                            const struct s_ball *up,
 267                            const struct s_file *fp,
 268                            const struct s_edge *ep,
 269                            const float o[3],
 270                            const float w[3])
 271 {
 272     float q[3];
 273     float u[3];
 274
 275     v_cpy(q, fp->vv[ep->vi].p);
 276     v_sub(u, fp->vv[ep->vj].p,
 277           fp->vv[ep->vi].p);
 278
 279     return v_edge(T, o, q, u, w, up->p, up->v, up->r);
 280 }
 281
 282 static float sol_test_side(float dt,
 283                            float T[3],
 284                            const struct s_ball *up,
 285                            const struct s_file *fp,
 286                            const struct s_lump *lp,
 287                            const struct s_side *sp,
 288                            const float o[3],
 289                            const float w[3])
 290 {
 291     float t = v_side(T, o, w, sp->n, sp->d, up->p, up->v, up->r);
 292     int i;
 293
 294     if (t < dt)
 295         for (i = 0; i < lp->sc; i++)
 296         {
 297             const struct s_side *sq = fp->sv + fp->iv[lp->s0 + i];
 298
 299             if (sp != sq &&
 300                 v_dot(T, sq->n) -
 301                 v_dot(o, sq->n) -
 302                 v_dot(w, sq->n) * t > sq->d)
 303                 return LARGE;
 304         }
 305     return t;
 306 }
 307
 308 /*---------------------------------------------------------------------------*/
 309
 310 static int sol_test_fore(float dt,
 311                          const struct s_ball *up,
 312                          const struct s_side *sp,
 313                          const float o[3],
 314                          const float w[3])
 315 {
 316     float q[3];
 317
 318     /* If the ball is not behind the plane, the test passes. */
 319
 320     v_sub(q, up->p, o);
 321
 322     if (v_dot(q, sp->n) - sp->d + up->r >= 0)
 323         return 1;
 324
 325     /* If it's not behind the plane after DT seconds, the test passes. */
 326
 327     v_mad(q, q, up->v, dt);
 328
 329     if (v_dot(q, sp->n) - sp->d + up->r >= 0)
 330         return 1;
 331
 332     /* Else, test fails. */
 333
 334     return 0;
 335 }
 336
 337 static int sol_test_back(float dt,
 338                          const struct s_ball *up,
 339                          const struct s_side *sp,
 340                          const float o[3],
 341                          const float w[3])
 342 {
 343     float q[3];
 344
 345     /* If the ball is not in front of the plane, the test passes. */
 346
 347     v_sub(q, up->p, o);
 348
 349     if (v_dot(q, sp->n) - sp->d - up->r <= 0)
 350         return 1;
 351
 352     /* If it's not in front of the plane after DT seconds, the test passes. */
 353
 354     v_mad(q, q, up->v, dt);
 355
 356     if (v_dot(q, sp->n) - sp->d - up->r <= 0)
 357         return 1;
 358
 359     /* Else, test fails. */
 360
 361     return 0;
 362 }
 363
 364 /*---------------------------------------------------------------------------*/
 365
 366 static float sol_test_lump(float dt,
 367                            float T[3],
 368                            const struct s_ball *up,
 369                            const struct s_file *fp,
 370                            const struct s_lump *lp,
 371                            const float o[3],
 372                            const float w[3])
 373 {
 374     float U[3] = { 0.0f, 0.0f, 0.0f };
 375     float u, t = dt;
 376     int i;
 377
 378     /* Short circuit a non-solid lump. */
 379
 380     if (lp->fl & L_DETAIL) return t;
 381
 382     /* Test all verts */
 383
 384     if (up->r > 0.0f)
 385         for (i = 0; i < lp->vc; i++)
 386         {
 387             const struct s_vert *vp = fp->vv + fp->iv[lp->v0 + i];
 388
 389             if ((u = sol_test_vert(t, U, up, vp, o, w)) < t)
 390             {
 391                 v_cpy(T, U);
 392                 t = u;
 393             }
 394         }
 395
 396     /* Test all edges */
 397
 398     if (up->r > 0.0f)
 399         for (i = 0; i < lp->ec; i++)
 400         {
 401             const struct s_edge *ep = fp->ev + fp->iv[lp->e0 + i];
 402
 403             if ((u = sol_test_edge(t, U, up, fp, ep, o, w)) < t)
 404             {
 405                 v_cpy(T, U);
 406                 t = u;
 407             }
 408         }
 409
 410     /* Test all sides */
 411
 412     for (i = 0; i < lp->sc; i++)
 413     {
 414         const struct s_side *sp = fp->sv + fp->iv[lp->s0 + i];
 415
 416         if ((u = sol_test_side(t, U, up, fp, lp, sp, o, w)) < t)
 417         {
 418             v_cpy(T, U);
 419             t = u;
 420         }
 421     }
 422     return t;
 423 }
 424
 425 static float sol_test_node(float dt,
 426                            float T[3],
 427                            const struct s_ball *up,
 428                            const struct s_file *fp,
 429                            const struct s_node *np,
 430                            const float o[3],
 431                            const float w[3])
 432 {
 433     float U[3], u, t = dt;
 434     int i;
 435
 436     /* Test all lumps */
 437
 438     for (i = 0; i < np->lc; i++)
 439     {
 440         const struct s_lump *lp = fp->lv + np->l0 + i;
 441
 442         if ((u = sol_test_lump(t, U, up, fp, lp, o, w)) < t)
 443         {
 444             v_cpy(T, U);
 445             t = u;
 446         }
 447     }
 448
 449     /* Test in front of this node */
 450
 451     if (np->ni >= 0 && sol_test_fore(t, up, fp->sv + np->si, o, w))
 452     {
 453         const struct s_node *nq = fp->nv + np->ni;
 454
 455         if ((u = sol_test_node(t, U, up, fp, nq, o, w)) < t)
 456         {
 457             v_cpy(T, U);
 458             t = u;
 459         }
 460     }
 461
 462     /* Test behind this node */
 463
 464     if (np->nj >= 0 && sol_test_back(t, up, fp->sv + np->si, o, w))
 465     {
 466         const struct s_node *nq = fp->nv + np->nj;
 467
 468         if ((u = sol_test_node(t, U, up, fp, nq, o, w)) < t)
 469         {
 470             v_cpy(T, U);
 471             t = u;
 472         }
 473     }
 474
 475     return t;
 476 }
 477
 478 static float sol_test_body(float dt,
 479                            float T[3], float V[3],
 480                            const struct s_ball *up,
 481                            const struct s_file *fp,
 482                            const struct s_body *bp)
 483 {
 484     float U[3], O[3], W[3], u, t = dt;
 485
 486     const struct s_node *np = fp->nv + bp->ni;
 487
 488     sol_body_p(O, fp, bp->pi, bp->t);
 489     sol_body_v(W, fp, bp->pi, bp->t, dt);
 490
 491     if ((u = sol_test_node(t, U, up, fp, np, O, W)) < t)
 492     {
 493         v_cpy(T, U);
 494         v_cpy(V, W);
 495         t = u;
 496     }
 497     return t;
 498 }
 499
 500 static float sol_test_file(float dt,
 501                            float T[3], float V[3],
 502                            const struct s_ball *up,
 503                            const struct s_file *fp)
 504 {
 505     float U[3], W[3], u, t = dt;
 506     int i;
 507
 508     for (i = 0; i < fp->bc; i++)
 509     {
 510         const struct s_body *bp = fp->bv + i;
 511
 512         if ((u = sol_test_body(t, U, W, up, fp, bp)) < t)
 513         {
 514             v_cpy(T, U);
 515             v_cpy(V, W);
 516             t = u;
 517         }
 518     }
 519     return t;
 520 }
 521
 522 /*---------------------------------------------------------------------------*/
 523
 524 /*
 525  * Step the physics forward DT  seconds under the influence of gravity
 526  * vector G.  If the ball gets pinched between two moving solids, this
 527  * loop might not terminate.  It  is better to do something physically
 528  * impossible than  to lock up the game.   So, if we make  more than C
 529  * iterations, punt it.
 530  */
 531
 532 float sol_step(struct s_file *fp, const float *g, float dt, int ui, int *m)
 533 {
 534     float P[3], V[3], v[3], r[3], a[3], d, e, nt, b = 0.0f, tt = dt;
 535     int c = 16;
 536
 537     union cmd cmd;
 538
 539     sol_cmd_defer = 1;
 540
 541     if (ui < fp->uc)
 542     {
 543         struct s_ball *up = fp->uv + ui;
 544
 545         /* If the ball is in contact with a surface, apply friction. */
 546
 547         v_cpy(a, up->v);
 548         v_cpy(v, up->v);
 549         v_cpy(up->v, g);
 550
 551         if (m && sol_test_file(tt, P, V, up, fp) < 0.0005f)
 552         {
 553             v_cpy(up->v, v);
 554             v_sub(r, P, up->p);
 555
 556             if ((d = v_dot(r, g) / (v_len(r) * v_len(g))) > 0.999f)
 557             {
 558                 if ((e = (v_len(up->v) - dt)) > 0.0f)
 559                 {
 560                     /* Scale the linear velocity. */
 561
 562                     v_nrm(up->v, up->v);
 563                     v_scl(up->v, up->v, e);
 564
 565                     /* Scale the angular velocity. */
 566
 567                     v_sub(v, V, up->v);
 568                     v_crs(up->w, v, r);
 569                     v_scl(up->w, up->w, -1.0f / (up->r * up->r));
 570                 }
 571                 else
 572                 {
 573                     /* Friction has brought the ball to a stop. */
 574
 575                     up->v[0] = 0.0f;
 576                     up->v[1] = 0.0f;
 577                     up->v[2] = 0.0f;
 578
 579                     (*m)++;
 580                 }
 581             }
 582             else v_mad(up->v, v, g, tt);
 583         }
 584         else v_mad(up->v, v, g, tt);
 585
 586         /* Test for collision. */
 587
 588         while (c > 0 && tt > 0 && tt > (nt = sol_test_file(tt, P, V, up, fp)))
 589         {
 590             cmd.type       = CMD_STEP_SIMULATION;
 591             cmd.stepsim.dt = nt;
 592             sol_cmd_enq(&cmd);
 593
 594             sol_body_step(fp, nt);
 595             sol_swch_step(fp, nt);
 596             sol_ball_step(fp, nt);
 597
 598             tt -= nt;
 599
 600             if (b < (d = sol_bounce(up, P, V, nt)))
 601                 b = d;
 602
 603             c--;
 604         }
 605
 606         cmd.type       = CMD_STEP_SIMULATION;
 607         cmd.stepsim.dt = tt;
 608         sol_cmd_enq(&cmd);
 609
 610         sol_body_step(fp, tt);
 611         sol_swch_step(fp, tt);
 612         sol_ball_step(fp, tt);
 613
 614         /* Apply the ball's accelleration to the pendulum. */
 615
 616         v_sub(a, up->v, a);
 617
 618         sol_pendulum(up, a, g, dt);
 619     }
 620
 621     sol_cmd_enq_deferred();
 622     sol_cmd_defer = 0;
 623
 624     return b;
 625 }
 626
 627 /*---------------------------------------------------------------------------*/
 628
 629 void sol_init_sim(struct s_file *fp)
 630 {
 631     return;
 632 }
 633
 634 void sol_quit_sim(void)
 635 {
 636     return;
 637 }
 638
 639 /*---------------------------------------------------------------------------*/