-/* Solves (p + v * t) . (p + v * t) == r * r for smallest t. */
-
-static float v_sol(const float p[3], const float v[3], float r)
-{
- float a = v_dot(v, v);
- float b = v_dot(v, p) * 2.0f;
- float c = v_dot(p, p) - r * r;
- float d = b * b - 4.0f * a * c;
-
- if (a == 0.0f) return LARGE;
- if (d < 0.0f) return LARGE;
-
- if (d == 0.0f)
- return -b * 0.5f / a;
- else
- {
- float t0 = 0.5f * (-b - fsqrtf(d)) / a;
- float t1 = 0.5f * (-b + fsqrtf(d)) / a;
- float t = (t0 < t1) ? t0 : t1;
-
- return (t < 0.0f) ? LARGE : t;
- }
-}
-
-/*---------------------------------------------------------------------------*/
-
-/*
- * Compute the earliest time and position of the intersection of a
- * sphere and a vertex.
- *
- * The sphere has radius R and moves along vector V from point P. The
- * vertex moves along vector W from point Q in a coordinate system
- * based at O.
- */
-static float v_vert(float Q[3],
- const float o[3],
- const float q[3],
- const float w[3],
- const float p[3],
- const float v[3], float r)
-{
- float O[3], P[3], V[3];
- float t = LARGE;
-
- v_add(O, o, q);
- v_sub(P, p, O);
- v_sub(V, v, w);
-
- if (v_dot(P, V) < 0.0f)
- {
- t = v_sol(P, V, r);
-
- if (t < LARGE)
- v_mad(Q, O, w, t);
- }
- return t;
-}
-
-/*
- * Compute the earliest time and position of the intersection of a
- * sphere and an edge.
- *
- * The sphere has radius R and moves along vector V from point P. The
- * edge moves along vector W from point Q in a coordinate system based
- * at O. The edge extends along the length of vector U.
- */
-static float v_edge(float Q[3],
- const float o[3],
- const float q[3],
- const float u[3],
- const float w[3],
- const float p[3],
- const float v[3], float r)
-{
- float d[3], e[3];
- float P[3], V[3];
- float du, eu, uu, s, t;
-
- v_sub(d, p, o);
- v_sub(d, d, q);
- v_sub(e, v, w);
-
- du = v_dot(d, u);
- eu = v_dot(e, u);
- uu = v_dot(u, u);
-
- v_mad(P, d, u, -du / uu);
- v_mad(V, e, u, -eu / uu);
-
- t = v_sol(P, V, r);
- s = (du + eu * t) / uu;
-
- if (0.0f < t && t < LARGE && 0.0f < s && s < 1.0f)
- {
- v_mad(d, o, w, t);
- v_mad(e, q, u, s);
- v_add(Q, e, d);
- }
- else
- t = LARGE;
-
- return t;
-}
-
-/*
- * Compute the earliest time and position of the intersection of a
- * sphere and a plane.
- *
- * The sphere has radius R and moves along vector V from point P. The
- * plane moves along vector W. The plane has normal N and is
- * positioned at distance D from the origin O along that normal.
- */
-static float v_side(float Q[3],
- const float o[3],
- const float w[3],
- const float n[3], float d,
- const float p[3],
- const float v[3], float r)
-{
- float vn = v_dot(v, n);
- float wn = v_dot(w, n);
- float t = LARGE;
-
- if (vn - wn <= 0.0f)
- {
- float on = v_dot(o, n);
- float pn = v_dot(p, n);
-
- float u = (r + d + on - pn) / (vn - wn);
- float a = ( d + on - pn) / (vn - wn);
-
- if (0.0f <= u)
- {
- t = u;
-
- v_mad(Q, p, v, +t);
- v_mad(Q, Q, n, -r);
- }
- else if (0.0f <= a)
- {
- t = 0;
-
- v_mad(Q, p, v, +t);
- v_mad(Q, Q, n, -r);
- }
- }
- return t;
-}
-
-/*---------------------------------------------------------------------------*/
-
-/*
- * Compute the new linear and angular velocities of a bouncing ball.
- * Q gives the position of the point of impact and W gives the
- * velocity of the object being impacted.
- */
-static float sol_bounce(struct s_ball *up,
- const float q[3],
- const float w[3], float dt)
-{
- float n[3], r[3], d[3], vn, wn;
- float *p = up->p;
- float *v = up->v;
-
- /* Find the normal of the impact. */
-
- v_sub(r, p, q);
- v_sub(d, v, w);
- v_nrm(n, r);
-
- /* Find the new angular velocity. */
-
- v_crs(up->w, d, r);
- v_scl(up->w, up->w, -1.0f / (up->r * up->r));
-
- /* Find the new linear velocity. */
-
- vn = v_dot(v, n);
- wn = v_dot(w, n);
-
- v_mad(v, v, n, 1.7 * (wn - vn));
-
- v_mad(p, q, n, up->r);
-
- /* Return the "energy" of the impact, to determine the sound amplitude. */
-
- return fabsf(v_dot(n, d));
-}
-
-/*---------------------------------------------------------------------------*/
-
-/*
- * Compute the states of all switches after DT seconds have passed.
- */
-static void sol_swch_step(struct s_file *fp, float dt)
-{
- int xi;
-
- for (xi = 0; xi < fp->xc; xi++)
- {
- struct s_swch *xp = fp->xv + xi;
-
- if (xp->t > 0)
- {
- xp->t -= dt;
-
- if (xp->t <= 0)
- {
- int pi = xp->pi;
- int pj = xp->pi;
-
- do /* Tortoise and hare cycle traverser. */
- {
- fp->pv[pi].f = xp->f0;
- fp->pv[pj].f = xp->f0;
-
- pi = fp->pv[pi].pi;
- pj = fp->pv[pj].pi;
- pj = fp->pv[pj].pi;
- }
- while (pi != pj);
-
- xp->f = xp->f0;
- }
- }
- }
-}
-
-/*
- * Compute the positions of all bodies after DT seconds have passed.
- */
-static void sol_body_step(struct s_file *fp, float dt)
-{
- int i;
-
- for (i = 0; i < fp->bc; i++)
- {
- struct s_body *bp = fp->bv + i;
- struct s_path *pp = fp->pv + bp->pi;
-
- if (bp->pi >= 0 && pp->f)
- {
- bp->t += dt;
-
- if (bp->t >= pp->t)
- {
-/* TODO: Confirm that this fixes the repeated path inaccuracy. */
-/* bp->t -= pp->t; */
- bp->t = 0;
- bp->pi = pp->pi;
- }
- }
- }
-}
-
-/*
- * Compute the positions of all balls after DT seconds have passed.
- */
-static void sol_ball_step(struct s_file *fp, float dt)
-{
- int i;
-
- for (i = 0; i < fp->uc; i++)
- {
- struct s_ball *up = fp->uv + i;
-
- v_mad(up->p, up->p, up->v, dt);
-
- if (v_len(up->w) > 0.0f)
- {
- float M[16];
- float w[3];
- float e[3][3];
-
- v_nrm(w, up->w);
- m_rot(M, w, v_len(up->w) * dt);
-
- m_vxfm(e[0], M, up->e[0]);
- m_vxfm(e[1], M, up->e[1]);
- m_vxfm(e[2], M, up->e[2]);
-
- v_crs(up->e[2], e[0], e[1]);
- v_crs(up->e[1], e[2], e[0]);
- v_crs(up->e[0], e[1], e[2]);
-
- v_nrm(up->e[0], up->e[0]);
- v_nrm(up->e[1], up->e[1]);
- v_nrm(up->e[2], up->e[2]);
- }
- }
-}
-
-/*---------------------------------------------------------------------------*/
-
-static float sol_test_vert(float dt,
- float T[3],
- const struct s_ball *up,
- const struct s_vert *vp,
- const float o[3],
- const float w[3])
-{
- return v_vert(T, o, vp->p, w, up->p, up->v, up->r);
-}
-
-static float sol_test_edge(float dt,
- float T[3],
- const struct s_ball *up,
- const struct s_file *fp,
- const struct s_edge *ep,
- const float o[3],
- const float w[3])
-{
- float q[3];
- float u[3];
-
- v_cpy(q, fp->vv[ep->vi].p);
- v_sub(u, fp->vv[ep->vj].p,
- fp->vv[ep->vi].p);
-
- return v_edge(T, o, q, u, w, up->p, up->v, up->r);
-}
-
-static float sol_test_side(float dt,
- float T[3],
- const struct s_ball *up,
- const struct s_file *fp,
- const struct s_lump *lp,
- const struct s_side *sp,
- const float o[3],
- const float w[3])
-{
- float t = v_side(T, o, w, sp->n, sp->d, up->p, up->v, up->r);
- int i;
-
- if (t < dt)
- for (i = 0; i < lp->sc; i++)
- {
- const struct s_side *sq = fp->sv + fp->iv[lp->s0 + i];
-
- if (sp != sq &&
- v_dot(T, sq->n) -
- v_dot(o, sq->n) -
- v_dot(w, sq->n) * t > sq->d)
- return LARGE;
- }
- return t;
-}
-
-/*---------------------------------------------------------------------------*/
-
-static float sol_test_fore(float dt,
- const struct s_ball *up,
- const struct s_side *sp,
- const float o[3],
- const float w[3])
-{
- float q[3];
-
- /* If the ball is not behind the plane, the test passes. */
-
- v_sub(q, up->p, o);
-
- if (v_dot(q, sp->n) - sp->d + up->r >= 0)
- return 1;
-
- /* if the ball is behind the plane but will hit before dt, test passes. */
- /*
- if (v_side(q, o, w, sp->n, sp->d, up->p, up->v, up->r) < dt)
- return 1;
- */
- /* If the ball is behind but moving toward the plane, test passes. */
-
- if (v_dot(up->v, sp->n) > 0)
- return 1;
-
-
- /* Else, test fails. */
-
- return 0;
-}
-
-static float sol_test_back(float dt,
- const struct s_ball *up,
- const struct s_side *sp,
- const float o[3],
- const float w[3])
-{
- float q[3];
-
- /* If the ball is not in front of the plane, the test passes. */
-
- v_sub(q, up->p, o);
-
- if (v_dot(q, sp->n) - sp->d - up->r <= 0)
- return 1;
-
- /* if the ball is behind the plane but will hit before dt, test passes. */
- /*
- if (v_side(q, o, w, sp->n, sp->d, up->p, up->v, up->r) < dt)
- return 1;
- */
- /* If the ball is in front but moving toward the plane, test passes. */
-
- if (v_dot(up->v, sp->n) < 0)
- return 1;
-
-
- /* Else, test fails. */
-
- return 0;
-}
-
-/*---------------------------------------------------------------------------*/
-
-static float sol_test_lump(float dt,
- float T[3],
- const struct s_ball *up,
- const struct s_file *fp,
- const struct s_lump *lp,
- const float o[3],
- const float w[3])
-{
- float U[3] = {0.0f, 0.0f, 0.0f}; /* init value only to avoid gcc warnings */
- float u, t = dt;
- int i;
-
- /* Short circuit a non-solid lump. */
-
- if (lp->fl & L_DETAIL) return t;
-
- /* Test all verts */
-
- if (up->r > 0.0f)
- for (i = 0; i < lp->vc; i++)
- {
- const struct s_vert *vp = fp->vv + fp->iv[lp->v0 + i];
-
- if ((u = sol_test_vert(t, U, up, vp, o, w)) < t)
- {
- v_cpy(T, U);
- t = u;
- }
- }
-
- /* Test all edges */
-
- if (up->r > 0.0f)
- for (i = 0; i < lp->ec; i++)
- {
- const struct s_edge *ep = fp->ev + fp->iv[lp->e0 + i];
-
- if ((u = sol_test_edge(t, U, up, fp, ep, o, w)) < t)
- {
- v_cpy(T, U);
- t = u;
- }
- }
-
- /* Test all sides */
-
- for (i = 0; i < lp->sc; i++)
- {
- const struct s_side *sp = fp->sv + fp->iv[lp->s0 + i];
-
- if ((u = sol_test_side(t, U, up, fp, lp, sp, o, w)) < t)
- {
- v_cpy(T, U);
- t = u;
- }
- }
- return t;
-}
-
-static float sol_test_node(float dt,
- float T[3],
- const struct s_ball *up,
- const struct s_file *fp,
- const struct s_node *np,
- const float o[3],
- const float w[3])
-{
- float U[3], u, t = dt;
- int i;
-
- /* Test all lumps */
-
- for (i = 0; i < np->lc; i++)
- {
- const struct s_lump *lp = fp->lv + np->l0 + i;
-
- if ((u = sol_test_lump(t, U, up, fp, lp, o, w)) < t)
- {
- v_cpy(T, U);
- t = u;
- }
- }
-
- /* Test in front of this node */
-
- if (np->ni >= 0 && sol_test_fore(t, up, fp->sv + np->si, o, w))
- {
- const struct s_node *nq = fp->nv + np->ni;
-
- if ((u = sol_test_node(t, U, up, fp, nq, o, w)) < t)
- {
- v_cpy(T, U);
- t = u;
- }
- }
-
- /* Test behind this node */
-
- if (np->nj >= 0 && sol_test_back(t, up, fp->sv + np->si, o, w))
- {
- const struct s_node *nq = fp->nv + np->nj;
-
- if ((u = sol_test_node(t, U, up, fp, nq, o, w)) < t)
- {
- v_cpy(T, U);
- t = u;
- }
- }
-
- return t;
-}
-
-static float sol_test_body(float dt,
- float T[3], float V[3],
- const struct s_ball *up,
- const struct s_file *fp,
- const struct s_body *bp)
-{
- float U[3], O[3], W[3], u, t = dt;
-
- const struct s_node *np = fp->nv + bp->ni;
-
- sol_body_p(O, fp, bp);
- sol_body_v(W, fp, bp);
-
- if ((u = sol_test_node(t, U, up, fp, np, O, W)) < t)
- {
- v_cpy(T, U);
- v_cpy(V, W);
- t = u;
- }
- return t;
-}
-
-static float sol_test_file(float dt,
- float T[3], float V[3],
- const struct s_ball *up,
- const struct s_file *fp)
-{
- float U[3], W[3], u, t = dt;
- int i;
-
- for (i = 0; i < fp->bc; i++)
- {
- const struct s_body *bp = fp->bv + i;
-
- if ((u = sol_test_body(t, U, W, up, fp, bp)) < t)
- {
- v_cpy(T, U);
- v_cpy(V, W);
- t = u;
- }
- }
- return t;
-}
-
-/*---------------------------------------------------------------------------*/
-
-/*
- * Step the physics forward DT seconds under the influence of gravity
- * vector G. If the ball gets pinched between two moving solids, this
- * loop might not terminate. It is better to do something physically
- * impossible than to lock up the game. So, if we make more than C
- * iterations, punt it.
- */
-
-float sol_step(struct s_file *fp, const float *g, float dt, int ui, int *m)
-{
- float P[3], V[3], v[3], r[3], d, e, nt, b = 0.0f, tt = dt;
- int c = 16;
-
- if (ui < fp->uc)
- {
- struct s_ball *up = fp->uv + ui;
-
- /* If the ball is in contact with a surface, apply friction. */
-
- v_cpy(v, up->v);
- v_cpy(up->v, g);
-
- if (m && sol_test_file(tt, P, V, up, fp) < 0.0005f)
- {
- v_cpy(up->v, v);
- v_sub(r, P, up->p);
-
- if ((d = v_dot(r, g) / (v_len(r) * v_len(g))) > 0.999f)
- {
- if ((e = (v_len(up->v) - dt)) > 0.0f)
- {
- /* Scale the linear velocity. */
-
- v_nrm(up->v, up->v);
- v_scl(up->v, up->v, e);
-
- /* Scale the angular velocity. */
-
- v_sub(v, V, up->v);
- v_crs(up->w, v, r);
- v_scl(up->w, up->w, -1.0f / (up->r * up->r));
- }
- else
- {
- /* Friction has brought the ball to a stop. */
-
- up->v[0] = 0.0f;
- up->v[1] = 0.0f;
- up->v[2] = 0.0f;
-
- (*m)++;
- }
- }
- else v_mad(up->v, v, g, tt);
- }
- else v_mad(up->v, v, g, tt);
-
- /* Test for collision. */
-
- while (c > 0 && tt > 0 && tt > (nt = sol_test_file(tt, P, V, up, fp)))
- {
- sol_body_step(fp, nt);
- sol_swch_step(fp, nt);
- sol_ball_step(fp, nt);
-
- tt -= nt;
-
- if (b < (d = sol_bounce(up, P, V, nt)))
- b = d;
-
- c--;
- }
-
- sol_body_step(fp, tt);
- sol_swch_step(fp, tt);
- sol_ball_step(fp, tt);
- }
- return b;
-}
-
-/*---------------------------------------------------------------------------*/
-
-struct s_item *sol_item_test(struct s_file *fp, float *p, float item_r)
-{
- const float *ball_p = fp->uv->p;
- const float ball_r = fp->uv->r;
-
- int hi;
-
- for (hi = 0; hi < fp->hc; hi++)
- {
- float r[3];
-
- r[0] = ball_p[0] - fp->hv[hi].p[0];
- r[1] = ball_p[1] - fp->hv[hi].p[1];
- r[2] = ball_p[2] - fp->hv[hi].p[2];
-
- if (fp->hv[hi].t != ITEM_NONE && v_len(r) < ball_r + item_r)
- {
- p[0] = fp->hv[hi].p[0];
- p[1] = fp->hv[hi].p[1];
- p[2] = fp->hv[hi].p[2];
-
- return &fp->hv[hi];
- }
- }
- return NULL;
-}
-
-struct s_goal *sol_goal_test(struct s_file *fp, float *p, int ui)
-{
- const float *ball_p = fp->uv[ui].p;
- const float ball_r = fp->uv[ui].r;
- int zi;
-
- for (zi = 0; zi < fp->zc; zi++)
- {
- float r[3];
-
- r[0] = ball_p[0] - fp->zv[zi].p[0];
- r[1] = ball_p[2] - fp->zv[zi].p[2];
- r[2] = 0;
-
- if (v_len(r) < fp->zv[zi].r * 1.1 - ball_r &&
- ball_p[1] > fp->zv[zi].p[1] &&
- ball_p[1] < fp->zv[zi].p[1] + GOAL_HEIGHT / 2)
- {
- p[0] = fp->zv[zi].p[0];
- p[1] = fp->zv[zi].p[1];
- p[2] = fp->zv[zi].p[2];
-
- return &fp->zv[zi];
- }
- }
- return NULL;
-}
-
-/*
- * Test if the ball UI is inside a jump. Return 1 if yes and fill P
- * with the destination position, return 0 if not, and return 2 if the
- * ball is on the border of a jump.
- */
-int sol_jump_test(struct s_file *fp, float *p, int ui)
-{
- const float *ball_p = fp->uv[ui].p;
- const float ball_r = fp->uv[ui].r;
- int ji;
- float l;
- int res = 0;
-
- for (ji = 0; ji < fp->jc; ji++)
- {
- float r[3];
-
- r[0] = ball_p[0] - fp->jv[ji].p[0];
- r[1] = ball_p[2] - fp->jv[ji].p[2];
- r[2] = 0;
-
- l = v_len(r) - fp->jv[ji].r;
- if (l < 0 &&
- ball_p[1] > fp->jv[ji].p[1] &&
- ball_p[1] < fp->jv[ji].p[1] + JUMP_HEIGHT / 2)
- {
- if (l < - ball_r )
- {
- p[0] = fp->jv[ji].q[0] + (ball_p[0] - fp->jv[ji].p[0]);
- p[1] = fp->jv[ji].q[1] + (ball_p[1] - fp->jv[ji].p[1]);
- p[2] = fp->jv[ji].q[2] + (ball_p[2] - fp->jv[ji].p[2]);
-
- return 1;
- }
- else
- res = 2;
- }
- }
- return res;
-}
-
-/*
- * Test and process the event the ball UI enters a switch. Return 1 if
- * a visible switch is activated, return 0 otherwise (no switch is
- * activated or only invisible switches).
- */
-int sol_swch_test(struct s_file *fp, int ui)
-{
- const float *ball_p = fp->uv[ui].p;
- const float ball_r = fp->uv[ui].r;
- int xi;
- int res = 0;
-
- for (xi = 0; xi < fp->xc; xi++)
- {
- struct s_swch *xp = fp->xv + xi;
-
- if (xp->t0 == 0 || xp->f == xp->f0)
- {
- float l;
- float r[3];
-
- r[0] = ball_p[0] - xp->p[0];
- r[1] = ball_p[2] - xp->p[2];
- r[2] = 0;
-
- l = v_len(r) - xp->r;
-
- if (l < ball_r &&
- ball_p[1] > xp->p[1] &&
- ball_p[1] < xp->p[1] + SWCH_HEIGHT / 2)
- {
- if (!xp->e && l < - ball_r)
- {
- int pi = xp->pi;
- int pj = xp->pi;
-
- /* The ball enters. */
-
- if (xp->t0 == 0)
- xp->e = 1;
-
- /* Toggle the state, update the path. */
-
- xp->f = xp->f ? 0 : 1;
-
- do /* Tortoise and hare cycle traverser. */
- {
- fp->pv[pi].f = xp->f;
- fp->pv[pj].f = xp->f;
-
- pi = fp->pv[pi].pi;
- pj = fp->pv[pj].pi;
- pj = fp->pv[pj].pi;
- }
- while (pi != pj);
-
- /* It toggled to non-default state, start the timer. */
-
- if (xp->f != xp->f0)
- xp->t = xp->t0;
-
- /* If visible, set the result. */
-
- if (!xp->i)
- res = 1;
- }
- }
-
- /* The ball exits. */
-
- else if (xp->e)
- xp->e = 0;
- }
- }
- return res;
-}
-
-/*---------------------------------------------------------------------------*/
-
-void put_file_state(FILE *fout, struct s_file *fp)
-{
- /* Write the position and orientation of the ball. */
-
- put_array(fout, fp->uv[0].p, 3);
- put_array(fout, fp->uv[0].e[0], 3);
- put_array(fout, fp->uv[0].e[1], 3);
-}
-
-void get_file_state(FILE *fin, struct s_file *fp)
-{
- /* Read the position and orientation of the ball. */
-
- get_array(fin, fp->uv[0].p, 3);
- get_array(fin, fp->uv[0].e[0], 3);
- get_array(fin, fp->uv[0].e[1], 3);
-
- /* Compute the 3rd vector of the ball orientation basis. */
-
- v_crs(fp->uv[0].e[2], fp->uv[0].e[0], fp->uv[0].e[1]);
-}
-
-/*---------------------------------------------------------------------------*/
-