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
 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 s_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 s_ball *up,
 287                            const struct s_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 s_ball *up,
 297                            const struct s_file *fp,
 298                            const struct s_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, fp->vv[ep->vi].p);
 306     v_sub(u, fp->vv[ep->vj].p,
 307           fp->vv[ep->vi].p);
 308
 309     return v_edge(T, o, q, u, w, up->p, up->v, up->r);
 310 }
 311
 312 static float sol_test_side(float dt,
 313                            float T[3],
 314                            const struct s_ball *up,
 315                            const struct s_file *fp,
 316                            const struct s_lump *lp,
 317                            const struct s_side *sp,
 318                            const float o[3],
 319                            const float w[3])
 320 {
 321     float t = v_side(T, o, w, sp->n, sp->d, up->p, up->v, up->r);
 322     int i;
 323
 324     if (t < dt)
 325         for (i = 0; i < lp->sc; i++)
 326         {
 327             const struct s_side *sq = fp->sv + fp->iv[lp->s0 + i];
 328
 329             if (sp != sq &&
 330                 v_dot(T, sq->n) -
 331                 v_dot(o, sq->n) -
 332                 v_dot(w, sq->n) * t > sq->d)
 333                 return LARGE;
 334         }
 335     return t;
 336 }
 337
 338 /*---------------------------------------------------------------------------*/
 339
 340 static int sol_test_fore(float dt,
 341                          const struct s_ball *up,
 342                          const struct s_side *sp,
 343                          const float o[3],
 344                          const float w[3])
 345 {
 346     float q[3], d;
 347
 348     /* If the ball is not behind the plane, the test passes. */
 349
 350     v_sub(q, up->p, o);
 351     d = sp->d;
 352
 353     if (v_dot(q, sp->n) - d + up->r >= 0)
 354         return 1;
 355
 356     /* If it's not behind the plane after DT seconds, the test passes. */
 357
 358     v_mad(q, q, up->v, dt);
 359     d += v_dot(w, sp->n) * dt;
 360
 361     if (v_dot(q, sp->n) - d + up->r >= 0)
 362         return 1;
 363
 364     /* Else, test fails. */
 365
 366     return 0;
 367 }
 368
 369 static int sol_test_back(float dt,
 370                          const struct s_ball *up,
 371                          const struct s_side *sp,
 372                          const float o[3],
 373                          const float w[3])
 374 {
 375     float q[3], d;
 376
 377     /* If the ball is not in front of the plane, the test passes. */
 378
 379     v_sub(q, up->p, o);
 380     d = sp->d;
 381
 382     if (v_dot(q, sp->n) - d - up->r <= 0)
 383         return 1;
 384
 385     /* If it's not in front of the plane after DT seconds, the test passes. */
 386
 387     v_mad(q, q, up->v, dt);
 388     d += v_dot(w, sp->n) * dt;
 389
 390     if (v_dot(q, sp->n) - d - up->r <= 0)
 391         return 1;
 392
 393     /* Else, test fails. */
 394
 395     return 0;
 396 }
 397
 398 /*---------------------------------------------------------------------------*/
 399
 400 static float sol_test_lump(float dt,
 401                            float T[3],
 402                            const struct s_ball *up,
 403                            const struct s_file *fp,
 404                            const struct s_lump *lp,
 405                            const float o[3],
 406                            const float w[3])
 407 {
 408     float U[3] = { 0.0f, 0.0f, 0.0f };
 409     float u, t = dt;
 410     int i;
 411
 412     /* Short circuit a non-solid lump. */
 413
 414     if (lp->fl & L_DETAIL) return t;
 415
 416     /* Test all verts */
 417
 418     if (up->r > 0.0f)
 419         for (i = 0; i < lp->vc; i++)
 420         {
 421             const struct s_vert *vp = fp->vv + fp->iv[lp->v0 + i];
 422
 423             if ((u = sol_test_vert(t, U, up, vp, o, w)) < t)
 424             {
 425                 v_cpy(T, U);
 426                 t = u;
 427             }
 428         }
 429
 430     /* Test all edges */
 431
 432     if (up->r > 0.0f)
 433         for (i = 0; i < lp->ec; i++)
 434         {
 435             const struct s_edge *ep = fp->ev + fp->iv[lp->e0 + i];
 436
 437             if ((u = sol_test_edge(t, U, up, fp, ep, o, w)) < t)
 438             {
 439                 v_cpy(T, U);
 440                 t = u;
 441             }
 442         }
 443
 444     /* Test all sides */
 445
 446     for (i = 0; i < lp->sc; i++)
 447     {
 448         const struct s_side *sp = fp->sv + fp->iv[lp->s0 + i];
 449
 450         if ((u = sol_test_side(t, U, up, fp, lp, sp, o, w)) < t)
 451         {
 452             v_cpy(T, U);
 453             t = u;
 454         }
 455     }
 456     return t;
 457 }
 458
 459 static float sol_test_node(float dt,
 460                            float T[3],
 461                            const struct s_ball *up,
 462                            const struct s_file *fp,
 463                            const struct s_node *np,
 464                            const float o[3],
 465                            const float w[3])
 466 {
 467     float U[3], u, t = dt;
 468     int i;
 469
 470     /* Test all lumps */
 471
 472     for (i = 0; i < np->lc; i++)
 473     {
 474         const struct s_lump *lp = fp->lv + np->l0 + i;
 475
 476         if ((u = sol_test_lump(t, U, up, fp, lp, o, w)) < t)
 477         {
 478             v_cpy(T, U);
 479             t = u;
 480         }
 481     }
 482
 483     /* Test in front of this node */
 484
 485     if (np->ni >= 0 && sol_test_fore(t, up, fp->sv + np->si, o, w))
 486     {
 487         const struct s_node *nq = fp->nv + np->ni;
 488
 489         if ((u = sol_test_node(t, U, up, fp, nq, o, w)) < t)
 490         {
 491             v_cpy(T, U);
 492             t = u;
 493         }
 494     }
 495
 496     /* Test behind this node */
 497
 498     if (np->nj >= 0 && sol_test_back(t, up, fp->sv + np->si, o, w))
 499     {
 500         const struct s_node *nq = fp->nv + np->nj;
 501
 502         if ((u = sol_test_node(t, U, up, fp, nq, o, w)) < t)
 503         {
 504             v_cpy(T, U);
 505             t = u;
 506         }
 507     }
 508
 509     return t;
 510 }
 511
 512 static float sol_test_body(float dt,
 513                            float T[3], float V[3],
 514                            const struct s_ball *up,
 515                            const struct s_file *fp,
 516                            const struct s_body *bp)
 517 {
 518     float U[3], O[3], E[4], W[3], A[3], u;
 519
 520     const struct s_node *np = fp->nv + bp->ni;
 521
 522     sol_body_p(O, fp, bp->pi, bp->t);
 523     sol_body_v(W, fp, bp->pi, bp->t, dt);
 524     sol_body_e(E, fp, bp, 0);
 525     sol_body_w(A, fp, bp);
 526
 527     /*
 528      * For rotating bodies, rather than rotate every normal and vertex
 529      * of the body, we temporarily pretend the ball is rotating and
 530      * moving about a static body.
 531      */
 532
 533     /*
 534      * Linear velocity of a point rotating about the origin:
 535      * v = w x p
 536      */
 537
 538     if (E[0] != 1.0f || v_dot(A, A) != 0.0f)
 539     {
 540         /* The body has a non-identity orientation or it is rotating. */
 541
 542         struct s_ball ball;
 543         float e[4], p0[3], p1[3];
 544         const float z[3] = { 0 };
 545
 546         /* First, calculate position at start and end of time interval. */
 547
 548         v_sub(p0, up->p, O);
 549         v_cpy(p1, p0);
 550         q_conj(e, E);
 551         q_rot(p0, e, p0);
 552
 553         v_mad(p1, p1, up->v, dt);
 554         v_mad(p1, p1, W, -dt);
 555         sol_body_e(e, fp, bp, dt);
 556         q_conj(e, e);
 557         q_rot(p1, e, p1);
 558
 559         /* Set up ball struct with values relative to body. */
 560
 561         ball = *up;
 562
 563         v_cpy(ball.p, p0);
 564
 565         /* Calculate velocity from start/end positions and time. */
 566
 567         v_sub(ball.v, p1, p0);
 568         v_scl(ball.v, ball.v, 1.0f / dt);
 569
 570         if ((u = sol_test_node(dt, U, &ball, fp, np, z, z)) < dt)
 571         {
 572             float d[4];
 573
 574             /* Compute the final orientation. */
 575
 576             q_by_axisangle(d, A, v_len(A) * u);
 577             q_mul(e, E, d);
 578             q_nrm(e, e);
 579
 580             /* Return world space coordinates. */
 581
 582             q_rot(T, e, U);
 583             v_add(T, O, T);
 584
 585             /* Move forward. */
 586
 587             v_mad(T, T, W, u);
 588
 589             /* Express "non-ball" velocity. */
 590
 591             q_rot(V, e, ball.v);
 592             v_sub(V, up->v, V);
 593
 594             dt = u;
 595         }
 596     }
 597     else
 598     {
 599         if ((u = sol_test_node(dt, U, up, fp, np, O, W)) < dt)
 600         {
 601             v_cpy(T, U);
 602             v_cpy(V, W);
 603             dt = u;
 604         }
 605     }
 606     return dt;
 607 }
 608
 609 static float sol_test_file(float dt,
 610                            float T[3], float V[3],
 611                            const struct s_ball *up,
 612                            const struct s_file *fp)
 613 {
 614     float U[3], W[3], u, t = dt;
 615     int i;
 616
 617     for (i = 0; i < fp->bc; i++)
 618     {
 619         const struct s_body *bp = fp->bv + i;
 620
 621         if ((u = sol_test_body(t, U, W, up, fp, bp)) < t)
 622         {
 623             v_cpy(T, U);
 624             v_cpy(V, W);
 625             t = u;
 626         }
 627     }
 628     return t;
 629 }
 630
 631 /*---------------------------------------------------------------------------*/
 632
 633 /*
 634  * Step the physics forward DT  seconds under the influence of gravity
 635  * vector G.  If the ball gets pinched between two moving solids, this
 636  * loop might not terminate.  It  is better to do something physically
 637  * impossible than  to lock up the game.   So, if we make  more than C
 638  * iterations, punt it.
 639  */
 640
 641 float sol_step(struct s_file *fp, const float *g, float dt, int ui, int *m)
 642 {
 643     float P[3], V[3], v[3], r[3], a[3], d, e, nt, b = 0.0f, tt = dt;
 644     int c;
 645
 646     union cmd cmd;
 647
 648     if (ui < fp->uc)
 649     {
 650         struct s_ball *up = fp->uv + ui;
 651
 652         /* If the ball is in contact with a surface, apply friction. */
 653
 654         v_cpy(a, up->v);
 655         v_cpy(v, up->v);
 656         v_cpy(up->v, g);
 657
 658         if (m && sol_test_file(tt, P, V, up, fp) < 0.0005f)
 659         {
 660             v_cpy(up->v, v);
 661             v_sub(r, P, up->p);
 662
 663             if ((d = v_dot(r, g) / (v_len(r) * v_len(g))) > 0.999f)
 664             {
 665                 if ((e = (v_len(up->v) - dt)) > 0.0f)
 666                 {
 667                     /* Scale the linear velocity. */
 668
 669                     v_nrm(up->v, up->v);
 670                     v_scl(up->v, up->v, e);
 671
 672                     /* Scale the angular velocity. */
 673
 674                     v_sub(v, V, up->v);
 675                     v_crs(up->w, v, r);
 676                     v_scl(up->w, up->w, -1.0f / (up->r * up->r));
 677                 }
 678                 else
 679                 {
 680                     /* Friction has brought the ball to a stop. */
 681
 682                     up->v[0] = 0.0f;
 683                     up->v[1] = 0.0f;
 684                     up->v[2] = 0.0f;
 685
 686                     (*m)++;
 687                 }
 688             }
 689             else v_mad(up->v, v, g, tt);
 690         }
 691         else v_mad(up->v, v, g, tt);
 692
 693         /* Test for collision. */
 694
 695         for (c = 16; c > 0 && tt > 0; c--)
 696         {
 697             float st;
 698             int bi;
 699
 700             /* HACK: avoid stepping across path changes. */
 701
 702             st = tt;
 703
 704             for (bi = 0; bi < fp->bc; bi++)
 705             {
 706                 struct s_body *bp = fp->bv + bi;
 707
 708                 if (bp->pi >= 0)
 709                 {
 710                     struct s_path *pp = fp->pv + bp->pi;
 711
 712                     if (!pp->f)
 713                         continue;
 714
 715                     if (bp->t + st > pp->t)
 716                         st = pp->t - bp->t;
 717                 }
 718             }
 719
 720             /* Miss collisions if we reach the iteration limit. */
 721
 722             if (c > 1)
 723                 nt = sol_test_file(st, P, V, up, fp);
 724             else
 725                 nt = tt;
 726
 727             cmd.type       = CMD_STEP_SIMULATION;
 728             cmd.stepsim.dt = nt;
 729             sol_cmd_enq(&cmd);
 730
 731             sol_body_step(fp, nt);
 732             sol_swch_step(fp, nt);
 733             sol_ball_step(fp, nt);
 734
 735             if (nt < st)
 736                 if (b < (d = sol_bounce(up, P, V, nt)))
 737                     b = d;
 738
 739             tt -= nt;
 740         }
 741
 742         v_sub(a, up->v, a);
 743
 744         sol_pendulum(up, a, g, dt);
 745     }
 746
 747     return b;
 748 }
 749
 750 /*---------------------------------------------------------------------------*/
 751
 752 void sol_init_sim(struct s_file *fp)
 753 {
 754     return;
 755 }
 756
 757 void sol_quit_sim(void)
 758 {
 759     return;
 760 }
 761
 762 /*---------------------------------------------------------------------------*/