2 * Copyright (C) 2003 Robert Kooima
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.
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.
21 #include "solid_sim.h"
22 #include "solid_all.h"
23 #include "solid_cmd.h"
28 /*---------------------------------------------------------------------------*/
29 /* Solves (p + v * t) . (p + v * t) == r * r for smallest t. */
32 * Given vectors A = P, B = V * t, C = A + B, |C| = r, solve for
35 * Some useful dot product properties:
37 * 1) A . A = |A| * |A|
38 * 2) A . (B + C) = A . B + A . C
39 * 3) (r * A) . B = r * (A . B)
41 * Deriving a quadratic equation:
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
52 * This equation is solved using the quadratic formula.
55 static float v_sol(const float p[3], const float v[3], float r)
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;
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;
67 if (d < 0.0f) return LARGE;
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;
74 return (t < 0.0f) ? LARGE : t;
76 else return -b * 0.5f / a;
79 /*---------------------------------------------------------------------------*/
82 * Compute the earliest time and position of the intersection of a
83 * sphere and a vertex.
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
89 static float v_vert(float Q[3],
94 const float v[3], float r)
96 float O[3], P[3], V[3];
103 if (v_dot(P, V) < 0.0f)
114 * Compute the earliest time and position of the intersection of a
115 * sphere and an edge.
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.
121 static float v_edge(float Q[3],
127 const float v[3], float r)
131 float du, eu, uu, s, t;
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.
149 v_mad(P, d, u, -du / uu);
151 /* First, test for intersection. */
153 if (v_dot(P, P) < r * r)
155 /* The sphere already intersects the line of the edge. */
157 if (du < 0 || du > uu)
160 * The sphere is behind the endpoints of the edge, and
161 * can't hit the edge without hitting the vertices first.
166 /* The sphere already intersects the edge. */
168 if (v_dot(P, e) >= 0)
180 v_mad(V, e, u, -eu / uu);
183 s = (du + eu * t) / uu; /* Projection of D + E * t on U. */
185 if (0.0f <= t && t < LARGE && 0.0f < s && s < 1.0f)
198 * Compute the earliest time and position of the intersection of a
199 * sphere and a plane.
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.
205 static float v_side(float Q[3],
208 const float n[3], float d,
210 const float v[3], float r)
212 float vn = v_dot(v, n);
213 float wn = v_dot(w, n);
218 float on = v_dot(o, n);
219 float pn = v_dot(p, n);
221 float u = (r + d + on - pn) / (vn - wn);
222 float a = ( d + on - pn) / (vn - wn);
242 /*---------------------------------------------------------------------------*/
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.
249 static float sol_bounce(struct s_ball *up,
251 const float w[3], float dt)
253 float n[3], r[3], d[3], vn, wn;
257 /* Find the normal of the impact. */
263 /* Find the new angular velocity. */
266 v_scl(up->w, up->w, -1.0f / (up->r * up->r));
268 /* Find the new linear velocity. */
273 v_mad(v, v, n, 1.7 * (wn - vn));
275 v_mad(p, q, n, up->r);
277 /* Return the "energy" of the impact, to determine the sound amplitude. */
279 return fabsf(v_dot(n, d));
282 /*---------------------------------------------------------------------------*/
284 static float sol_test_vert(float dt,
286 const struct s_ball *up,
287 const struct s_vert *vp,
291 return v_vert(T, o, vp->p, w, up->p, up->v, up->r);
294 static float sol_test_edge(float dt,
296 const struct s_ball *up,
297 const struct s_file *fp,
298 const struct s_edge *ep,
305 v_cpy(q, fp->vv[ep->vi].p);
306 v_sub(u, fp->vv[ep->vj].p,
309 return v_edge(T, o, q, u, w, up->p, up->v, up->r);
312 static float sol_test_side(float dt,
314 const struct s_ball *up,
315 const struct s_file *fp,
316 const struct s_lump *lp,
317 const struct s_side *sp,
321 float t = v_side(T, o, w, sp->n, sp->d, up->p, up->v, up->r);
325 for (i = 0; i < lp->sc; i++)
327 const struct s_side *sq = fp->sv + fp->iv[lp->s0 + i];
332 v_dot(w, sq->n) * t > sq->d)
338 /*---------------------------------------------------------------------------*/
340 static int sol_test_fore(float dt,
341 const struct s_ball *up,
342 const struct s_side *sp,
348 /* If the ball is not behind the plane, the test passes. */
353 if (v_dot(q, sp->n) - d + up->r >= 0)
356 /* If it's not behind the plane after DT seconds, the test passes. */
358 v_mad(q, q, up->v, dt);
359 d += v_dot(w, sp->n) * dt;
361 if (v_dot(q, sp->n) - d + up->r >= 0)
364 /* Else, test fails. */
369 static int sol_test_back(float dt,
370 const struct s_ball *up,
371 const struct s_side *sp,
377 /* If the ball is not in front of the plane, the test passes. */
382 if (v_dot(q, sp->n) - d - up->r <= 0)
385 /* If it's not in front of the plane after DT seconds, the test passes. */
387 v_mad(q, q, up->v, dt);
388 d += v_dot(w, sp->n) * dt;
390 if (v_dot(q, sp->n) - d - up->r <= 0)
393 /* Else, test fails. */
398 /*---------------------------------------------------------------------------*/
400 static float sol_test_lump(float dt,
402 const struct s_ball *up,
403 const struct s_file *fp,
404 const struct s_lump *lp,
408 float U[3] = { 0.0f, 0.0f, 0.0f };
412 /* Short circuit a non-solid lump. */
414 if (lp->fl & L_DETAIL) return t;
419 for (i = 0; i < lp->vc; i++)
421 const struct s_vert *vp = fp->vv + fp->iv[lp->v0 + i];
423 if ((u = sol_test_vert(t, U, up, vp, o, w)) < t)
433 for (i = 0; i < lp->ec; i++)
435 const struct s_edge *ep = fp->ev + fp->iv[lp->e0 + i];
437 if ((u = sol_test_edge(t, U, up, fp, ep, o, w)) < t)
446 for (i = 0; i < lp->sc; i++)
448 const struct s_side *sp = fp->sv + fp->iv[lp->s0 + i];
450 if ((u = sol_test_side(t, U, up, fp, lp, sp, o, w)) < t)
459 static float sol_test_node(float dt,
461 const struct s_ball *up,
462 const struct s_file *fp,
463 const struct s_node *np,
467 float U[3], u, t = dt;
472 for (i = 0; i < np->lc; i++)
474 const struct s_lump *lp = fp->lv + np->l0 + i;
476 if ((u = sol_test_lump(t, U, up, fp, lp, o, w)) < t)
483 /* Test in front of this node */
485 if (np->ni >= 0 && sol_test_fore(t, up, fp->sv + np->si, o, w))
487 const struct s_node *nq = fp->nv + np->ni;
489 if ((u = sol_test_node(t, U, up, fp, nq, o, w)) < t)
496 /* Test behind this node */
498 if (np->nj >= 0 && sol_test_back(t, up, fp->sv + np->si, o, w))
500 const struct s_node *nq = fp->nv + np->nj;
502 if ((u = sol_test_node(t, U, up, fp, nq, o, w)) < t)
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)
518 float U[3], O[3], E[4], W[3], A[3], u;
520 const struct s_node *np = fp->nv + bp->ni;
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);
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.
534 * Linear velocity of a point rotating about the origin:
538 if (E[0] != 1.0f || v_dot(A, A) != 0.0f)
540 /* The body has a non-identity orientation or it is rotating. */
543 float e[4], p0[3], p1[3];
544 const float z[3] = { 0 };
546 /* First, calculate position at start and end of time interval. */
553 v_mad(p1, p1, up->v, dt);
554 v_mad(p1, p1, W, -dt);
555 sol_body_e(e, fp, bp, dt);
559 /* Set up ball struct with values relative to body. */
565 /* Calculate velocity from start/end positions and time. */
567 v_sub(ball.v, p1, p0);
568 v_scl(ball.v, ball.v, 1.0f / dt);
570 if ((u = sol_test_node(dt, U, &ball, fp, np, z, z)) < dt)
574 /* Compute the final orientation. */
576 q_by_axisangle(d, A, v_len(A) * u);
580 /* Return world space coordinates. */
589 /* Express "non-ball" velocity. */
599 if ((u = sol_test_node(dt, U, up, fp, np, O, W)) < dt)
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)
614 float U[3], W[3], u, t = dt;
617 for (i = 0; i < fp->bc; i++)
619 const struct s_body *bp = fp->bv + i;
621 if ((u = sol_test_body(t, U, W, up, fp, bp)) < t)
631 /*---------------------------------------------------------------------------*/
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.
641 float sol_step(struct s_file *fp, const float *g, float dt, int ui, int *m)
643 float P[3], V[3], v[3], r[3], a[3], d, e, nt, b = 0.0f, tt = dt;
650 struct s_ball *up = fp->uv + ui;
652 /* If the ball is in contact with a surface, apply friction. */
658 if (m && sol_test_file(tt, P, V, up, fp) < 0.0005f)
663 if ((d = v_dot(r, g) / (v_len(r) * v_len(g))) > 0.999f)
665 if ((e = (v_len(up->v) - dt)) > 0.0f)
667 /* Scale the linear velocity. */
670 v_scl(up->v, up->v, e);
672 /* Scale the angular velocity. */
676 v_scl(up->w, up->w, -1.0f / (up->r * up->r));
680 /* Friction has brought the ball to a stop. */
689 else v_mad(up->v, v, g, tt);
691 else v_mad(up->v, v, g, tt);
693 /* Test for collision. */
695 for (c = 16; c > 0 && tt > 0; c--)
700 /* HACK: avoid stepping across path changes. */
704 for (bi = 0; bi < fp->bc; bi++)
706 struct s_body *bp = fp->bv + bi;
710 struct s_path *pp = fp->pv + bp->pi;
715 if (bp->t + st > pp->t)
720 /* Miss collisions if we reach the iteration limit. */
723 nt = sol_test_file(st, P, V, up, fp);
727 cmd.type = CMD_STEP_SIMULATION;
731 sol_body_step(fp, nt);
732 sol_swch_step(fp, nt);
733 sol_ball_step(fp, nt);
736 if (b < (d = sol_bounce(up, P, V, nt)))
744 sol_pendulum(up, a, g, dt);
750 /*---------------------------------------------------------------------------*/
752 void sol_init_sim(struct s_file *fp)
757 void sol_quit_sim(void)
762 /*---------------------------------------------------------------------------*/