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_vary.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
 151     /* First, test for intersection. */
 152
 153     if (v_dot(P, P) < r * r)
 154     {
 155         /* The sphere already intersects the line of the edge. */
 156
 157         if (du < 0 || du > uu)
 158         {
 159             /*
 160              * The sphere is behind the endpoints of the edge, and
 161              * can't hit the edge without hitting the vertices first.
 162              */
 163             return LARGE;
 164         }
 165
 166         /* The sphere already intersects the edge. */
 167
 168         if (v_dot(P, e) >= 0)
 169         {
 170             /* Moving apart. */
 171             return LARGE;
 172         }
 173
 174         v_nrm(P, P);
 175         v_mad(Q, p, P, -r);
 176
 177         return 0;
 178     }
 179
 180     v_mad(V, e, u, -eu / uu);
 181
 182     t = v_sol(P, V, r);
 183     s = (du + eu * t) / uu; /* Projection of D + E * t on U. */
 184
 185     if (0.0f <= t && t < LARGE && 0.0f < s && s < 1.0f)
 186     {
 187         v_mad(d, o, w, t);
 188         v_mad(e, q, u, s);
 189         v_add(Q, e, d);
 190     }
 191     else
 192         t = LARGE;
 193
 194     return t;
 195 }
 196
 197 /*
 198  * Compute  the earliest  time and  position of  the intersection  of a
 199  * sphere and a plane.
 200  *
 201  * The sphere has radius R and moves along vector V from point P.  The
 202  * plane  moves  along  vector  W.   The  plane has  normal  N  and  is
 203  * positioned at distance D from the origin O along that normal.
 204  */
 205 static float v_side(float Q[3],
 206                     const float o[3],
 207                     const float w[3],
 208                     const float n[3], float d,
 209                     const float p[3],
 210                     const float v[3], float r)
 211 {
 212     float vn = v_dot(v, n);
 213     float wn = v_dot(w, n);
 214     float t  = LARGE;
 215
 216     if (vn - wn <= 0.0f)
 217     {
 218         float on = v_dot(o, n);
 219         float pn = v_dot(p, n);
 220
 221         float u = (r + d + on - pn) / (vn - wn);
 222         float a = (    d + on - pn) / (vn - wn);
 223
 224         if (0.0f <= u)
 225         {
 226             t = u;
 227
 228             v_mad(Q, p, v, +t);
 229             v_mad(Q, Q, n, -r);
 230         }
 231         else if (0.0f <= a)
 232         {
 233             t = 0;
 234
 235             v_mad(Q, p, v, +t);
 236             v_mad(Q, Q, n, -r);
 237         }
 238     }
 239     return t;
 240 }
 241
 242 /*---------------------------------------------------------------------------*/
 243
 244 /*
 245  * Compute the new  linear and angular velocities of  a bouncing ball.
 246  * Q  gives the  position  of the  point  of impact  and  W gives  the
 247  * velocity of the object being impacted.
 248  */
 249 static float sol_bounce(struct v_ball *up,
 250                         const float q[3],
 251                         const float w[3], float dt)
 252 {
 253     float n[3], r[3], d[3], vn, wn;
 254     float *p = up->p;
 255     float *v = up->v;
 256
 257     /* Find the normal of the impact. */
 258
 259     v_sub(r, p, q);
 260     v_sub(d, v, w);
 261     v_nrm(n, r);
 262
 263     /* Find the new angular velocity. */
 264
 265     v_crs(up->w, d, r);
 266     v_scl(up->w, up->w, -1.0f / (up->r * up->r));
 267
 268     /* Find the new linear velocity. */
 269
 270     vn = v_dot(v, n);
 271     wn = v_dot(w, n);
 272
 273     v_mad(v, v, n, 1.7 * (wn - vn));
 274
 275     v_mad(p, q, n, up->r);
 276
 277     /* Return the "energy" of the impact, to determine the sound amplitude. */
 278
 279     return fabsf(v_dot(n, d));
 280 }
 281
 282 /*---------------------------------------------------------------------------*/
 283
 284 static float sol_test_vert(float dt,
 285                            float T[3],
 286                            const struct v_ball *up,
 287                            const struct b_vert *vp,
 288                            const float o[3],
 289                            const float w[3])
 290 {
 291     return v_vert(T, o, vp->p, w, up->p, up->v, up->r);
 292 }
 293
 294 static float sol_test_edge(float dt,
 295                            float T[3],
 296                            const struct v_ball *up,
 297                            const struct s_base *base,
 298                            const struct b_edge *ep,
 299                            const float o[3],
 300                            const float w[3])
 301 {
 302     float q[3];
 303     float u[3];
 304
 305     v_cpy(q, base->vv[ep->vi].p);
 306     v_sub(u, base->vv[ep->vj].p, base->vv[ep->vi].p);
 307
 308     return v_edge(T, o, q, u, w, up->p, up->v, up->r);
 309 }
 310
 311 static float sol_test_side(float dt,
 312                            float T[3],
 313                            const struct v_ball *up,
 314                            const struct s_base *base,
 315                            const struct b_lump *lp,
 316                            const struct b_side *sp,
 317                            const float o[3],
 318                            const float w[3])
 319 {
 320     float t = v_side(T, o, w, sp->n, sp->d, up->p, up->v, up->r);
 321     int i;
 322
 323     if (t < dt)
 324         for (i = 0; i < lp->sc; i++)
 325         {
 326             const struct b_side *sq = base->sv + base->iv[lp->s0 + i];
 327
 328             if (sp != sq &&
 329                 v_dot(T, sq->n) -
 330                 v_dot(o, sq->n) -
 331                 v_dot(w, sq->n) * t > sq->d)
 332                 return LARGE;
 333         }
 334     return t;
 335 }
 336
 337 /*---------------------------------------------------------------------------*/
 338
 339 static int sol_test_fore(float dt,
 340                          const struct v_ball *up,
 341                          const struct b_side *sp,
 342                          const float o[3],
 343                          const float w[3])
 344 {
 345     float q[3], d;
 346
 347     /* If the ball is not behind the plane, the test passes. */
 348
 349     v_sub(q, up->p, o);
 350     d = sp->d;
 351
 352     if (v_dot(q, sp->n) - d + up->r >= 0)
 353         return 1;
 354
 355     /* If it's not behind the plane after DT seconds, the test passes. */
 356
 357     v_mad(q, q, up->v, dt);
 358     d += v_dot(w, sp->n) * dt;
 359
 360     if (v_dot(q, sp->n) - d + up->r >= 0)
 361         return 1;
 362
 363     /* Else, test fails. */
 364
 365     return 0;
 366 }
 367
 368 static int sol_test_back(float dt,
 369                          const struct v_ball *up,
 370                          const struct b_side *sp,
 371                          const float o[3],
 372                          const float w[3])
 373 {
 374     float q[3], d;
 375
 376     /* If the ball is not in front of the plane, the test passes. */
 377
 378     v_sub(q, up->p, o);
 379     d = sp->d;
 380
 381     if (v_dot(q, sp->n) - d - up->r <= 0)
 382         return 1;
 383
 384     /* If it's not in front of the plane after DT seconds, the test passes. */
 385
 386     v_mad(q, q, up->v, dt);
 387     d += v_dot(w, sp->n) * dt;
 388
 389     if (v_dot(q, sp->n) - d - up->r <= 0)
 390         return 1;
 391
 392     /* Else, test fails. */
 393
 394     return 0;
 395 }
 396
 397 /*---------------------------------------------------------------------------*/
 398
 399 static float sol_test_lump(float dt,
 400                            float T[3],
 401                            const struct v_ball *up,
 402                            const struct s_base *base,
 403                            const struct b_lump *lp,
 404                            const float o[3],
 405                            const float w[3])
 406 {
 407     float U[3] = { 0.0f, 0.0f, 0.0f };
 408     float u, t = dt;
 409     int i;
 410
 411     /* Short circuit a non-solid lump. */
 412
 413     if (lp->fl & L_DETAIL) return t;
 414
 415     /* Test all verts */
 416
 417     if (up->r > 0.0f)
 418         for (i = 0; i < lp->vc; i++)
 419         {
 420             const struct b_vert *vp = base->vv + base->iv[lp->v0 + i];
 421
 422             if ((u = sol_test_vert(t, U, up, vp, o, w)) < t)
 423             {
 424                 v_cpy(T, U);
 425                 t = u;
 426             }
 427         }
 428
 429     /* Test all edges */
 430
 431     if (up->r > 0.0f)
 432         for (i = 0; i < lp->ec; i++)
 433         {
 434             const struct b_edge *ep = base->ev + base->iv[lp->e0 + i];
 435
 436             if ((u = sol_test_edge(t, U, up, base, ep, o, w)) < t)
 437             {
 438                 v_cpy(T, U);
 439                 t = u;
 440             }
 441         }
 442
 443     /* Test all sides */
 444
 445     for (i = 0; i < lp->sc; i++)
 446     {
 447         const struct b_side *sp = base->sv + base->iv[lp->s0 + i];
 448
 449         if ((u = sol_test_side(t, U, up, base, lp, sp, o, w)) < t)
 450         {
 451             v_cpy(T, U);
 452             t = u;
 453         }
 454     }
 455     return t;
 456 }
 457
 458 static float sol_test_node(float dt,
 459                            float T[3],
 460                            const struct v_ball *up,
 461                            const struct s_base *base,
 462                            const struct b_node *np,
 463                            const float o[3],
 464                            const float w[3])
 465 {
 466     float U[3], u, t = dt;
 467     int i;
 468
 469     /* Test all lumps */
 470
 471     for (i = 0; i < np->lc; i++)
 472     {
 473         const struct b_lump *lp = base->lv + np->l0 + i;
 474
 475         if ((u = sol_test_lump(t, U, up, base, lp, o, w)) < t)
 476         {
 477             v_cpy(T, U);
 478             t = u;
 479         }
 480     }
 481
 482     /* Test in front of this node */
 483
 484     if (np->ni >= 0 && sol_test_fore(t, up, base->sv + np->si, o, w))
 485     {
 486         const struct b_node *nq = base->nv + np->ni;
 487
 488         if ((u = sol_test_node(t, U, up, base, nq, o, w)) < t)
 489         {
 490             v_cpy(T, U);
 491             t = u;
 492         }
 493     }
 494
 495     /* Test behind this node */
 496
 497     if (np->nj >= 0 && sol_test_back(t, up, base->sv + np->si, o, w))
 498     {
 499         const struct b_node *nq = base->nv + np->nj;
 500
 501         if ((u = sol_test_node(t, U, up, base, nq, o, w)) < t)
 502         {
 503             v_cpy(T, U);
 504             t = u;
 505         }
 506     }
 507
 508     return t;
 509 }
 510
 511 static float sol_test_body(float dt,
 512                            float T[3], float V[3],
 513                            const struct v_ball *up,
 514                            const struct s_vary *vary,
 515                            const struct v_body *bp)
 516 {
 517     float U[3], O[3], E[4], W[3], A[3], u;
 518
 519     const struct b_node *np = vary->base->nv + bp->base->ni;
 520
 521     sol_body_p(O, vary, bp->pi, bp->t);
 522     sol_body_v(W, vary, bp->pi, bp->t, dt);
 523     sol_body_e(E, vary, bp, 0);
 524     sol_body_w(A, vary, bp);
 525
 526     /*
 527      * For rotating bodies, rather than rotate every normal and vertex
 528      * of the body, we temporarily pretend the ball is rotating and
 529      * moving about a static body.
 530      */
 531
 532     /*
 533      * Linear velocity of a point rotating about the origin:
 534      * v = w x p
 535      */
 536
 537     if (E[0] != 1.0f || v_dot(A, A) != 0.0f)
 538     {
 539         /* The body has a non-identity orientation or it is rotating. */
 540
 541         struct v_ball ball;
 542         float e[4], p0[3], p1[3];
 543         const float z[3] = { 0 };
 544
 545         /* First, calculate position at start and end of time interval. */
 546
 547         v_sub(p0, up->p, O);
 548         v_cpy(p1, p0);
 549         q_conj(e, E);
 550         q_rot(p0, e, p0);
 551
 552         v_mad(p1, p1, up->v, dt);
 553         v_mad(p1, p1, W, -dt);
 554         sol_body_e(e, vary, bp, dt);
 555         q_conj(e, e);
 556         q_rot(p1, e, p1);
 557
 558         /* Set up ball struct with values relative to body. */
 559
 560         ball = *up;
 561
 562         v_cpy(ball.p, p0);
 563
 564         /* Calculate velocity from start/end positions and time. */
 565
 566         v_sub(ball.v, p1, p0);
 567         v_scl(ball.v, ball.v, 1.0f / dt);
 568
 569         if ((u = sol_test_node(dt, U, &ball, vary->base, np, z, z)) < dt)
 570         {
 571             float d[4];
 572
 573             /* Compute the final orientation. */
 574
 575             q_by_axisangle(d, A, v_len(A) * u);
 576             q_mul(e, E, d);
 577             q_nrm(e, e);
 578
 579             /* Return world space coordinates. */
 580
 581             q_rot(T, e, U);
 582             v_add(T, O, T);
 583
 584             /* Move forward. */
 585
 586             v_mad(T, T, W, u);
 587
 588             /* Express "non-ball" velocity. */
 589
 590             q_rot(V, e, ball.v);
 591             v_sub(V, up->v, V);
 592
 593             dt = u;
 594         }
 595     }
 596     else
 597     {
 598         if ((u = sol_test_node(dt, U, up, vary->base, np, O, W)) < dt)
 599         {
 600             v_cpy(T, U);
 601             v_cpy(V, W);
 602             dt = u;
 603         }
 604     }
 605     return dt;
 606 }
 607
 608 static float sol_test_file(float dt,
 609                            float T[3], float V[3],
 610                            const struct v_ball *up,
 611                            const struct s_vary *vary)
 612 {
 613     float U[3], W[3], u, t = dt;
 614     int i;
 615
 616     for (i = 0; i < vary->bc; i++)
 617     {
 618         const struct v_body *bp = vary->bv + i;
 619
 620         if ((u = sol_test_body(t, U, W, up, vary, bp)) < t)
 621         {
 622             v_cpy(T, U);
 623             v_cpy(V, W);
 624             t = u;
 625         }
 626     }
 627     return t;
 628 }
 629
 630 /*---------------------------------------------------------------------------*/
 631
 632 /*
 633  * Track simulation steps in integer milliseconds.
 634  */
 635
 636 static float ms_accum;
 637
 638 static void ms_init(void)
 639 {
 640     ms_accum = 0.0f;
 641 }
 642
 643 static int ms_step(float dt)
 644 {
 645     int ms = 0;
 646
 647     ms_accum += dt;
 648
 649     while (ms_accum >= 0.001f)
 650     {
 651         ms_accum -= 0.001f;
 652         ms += 1;
 653     }
 654
 655     return ms;
 656 }
 657
 658 static int ms_peek(float dt)
 659 {
 660     int ms = 0;
 661     float at;
 662
 663     at = ms_accum + dt;
 664
 665     while (at >= 0.001f)
 666     {
 667         at -= 0.001f;
 668         ms += 1;
 669     }
 670
 671     return ms;
 672 }
 673
 674 /*---------------------------------------------------------------------------*/
 675
 676 /*
 677  * Step the physics forward DT  seconds under the influence of gravity
 678  * vector G.  If the ball gets pinched between two moving solids, this
 679  * loop might not terminate.  It  is better to do something physically
 680  * impossible than  to lock up the game.   So, if we make  more than C
 681  * iterations, punt it.
 682  */
 683
 684 float sol_step(struct s_vary *vary, const float *g, float dt, int ui, int *m)
 685 {
 686     float P[3], V[3], v[3], r[3], a[3], d, e, nt, b = 0.0f, tt = dt;
 687     int c;
 688
 689     union cmd cmd;
 690
 691     if (ui < vary->uc)
 692     {
 693         struct v_ball *up = vary->uv + ui;
 694
 695         /* If the ball is in contact with a surface, apply friction. */
 696
 697         v_cpy(a, up->v);
 698         v_cpy(v, up->v);
 699         v_cpy(up->v, g);
 700
 701         if (m && sol_test_file(tt, P, V, up, vary) < 0.0005f)
 702         {
 703             v_cpy(up->v, v);
 704             v_sub(r, P, up->p);
 705
 706             if ((d = v_dot(r, g) / (v_len(r) * v_len(g))) > 0.999f)
 707             {
 708                 if ((e = (v_len(up->v) - dt)) > 0.0f)
 709                 {
 710                     /* Scale the linear velocity. */
 711
 712                     v_nrm(up->v, up->v);
 713                     v_scl(up->v, up->v, e);
 714
 715                     /* Scale the angular velocity. */
 716
 717                     v_sub(v, V, up->v);
 718                     v_crs(up->w, v, r);
 719                     v_scl(up->w, up->w, -1.0f / (up->r * up->r));
 720                 }
 721                 else
 722                 {
 723                     /* Friction has brought the ball to a stop. */
 724
 725                     up->v[0] = 0.0f;
 726                     up->v[1] = 0.0f;
 727                     up->v[2] = 0.0f;
 728
 729                     (*m)++;
 730                 }
 731             }
 732             else v_mad(up->v, v, g, tt);
 733         }
 734         else v_mad(up->v, v, g, tt);
 735
 736         /* Test for collision. */
 737
 738         for (c = 16; c > 0 && tt > 0; c--)
 739         {
 740             float st;
 741             int bi, ms;
 742
 743             /* HACK: avoid stepping across path changes. */
 744
 745             st = tt;
 746
 747             for (bi = 0; bi < vary->bc; bi++)
 748             {
 749                 struct v_body *bp = vary->bv + bi;
 750
 751                 if (bp->pi >= 0)
 752                 {
 753                     struct v_path *pp = vary->pv + bp->pi;
 754
 755                     if (!pp->f)
 756                         continue;
 757
 758                     if (bp->tm + ms_peek(st) > pp->base->tm)
 759                         st = MS_TO_TIME(pp->base->tm - bp->tm);
 760                 }
 761             }
 762
 763             /* Miss collisions if we reach the iteration limit. */
 764
 765             if (c > 1)
 766                 nt = sol_test_file(st, P, V, up, vary);
 767             else
 768                 nt = tt;
 769
 770             cmd.type       = CMD_STEP_SIMULATION;
 771             cmd.stepsim.dt = nt;
 772             sol_cmd_enq(&cmd);
 773
 774             ms = ms_step(nt);
 775
 776             sol_body_step(vary, nt, ms);
 777             sol_swch_step(vary, nt, ms);
 778             sol_ball_step(vary, nt);
 779
 780             if (nt < st)
 781                 if (b < (d = sol_bounce(up, P, V, nt)))
 782                     b = d;
 783
 784             tt -= nt;
 785         }
 786
 787         v_sub(a, up->v, a);
 788
 789         sol_pendulum(up, a, g, dt);
 790     }
 791
 792     return b;
 793 }
 794
 795 /*---------------------------------------------------------------------------*/
 796
 797 void sol_init_sim(struct s_vary *vary)
 798 {
 799     ms_init();
 800 }
 801
 802 void sol_quit_sim(void)
 803 {
 804     return;
 805 }
 806
 807 /*---------------------------------------------------------------------------*/