void sol_body_p(float p[3],
const struct s_vary *vary,
const struct v_body *bp,
float dt)
{
float v[3];
if (bp->mi >= 0)
{
const struct v_move *mp = vary->mv + bp->mi;
const struct b_path *pp = vary->base->pv + mp->pi;
const struct b_path *pq = vary->base->pv + pp->pi;
float s;
if (pp->t == 0.f)
s = 1.0e+6f;
else if (vary->pv[mp->pi].f)
s = (mp->t + dt) / pp->t;
else
s = mp->t / pp->t;
v_sub(v, pq->p, pp->p);
v_mad(p, pp->p, v, pp->s ? erp(s) : s);
return;
}
p[0] = 0.0f;
p[1] = 0.0f;
p[2] = 0.0f;
}
/*
* 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 v_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));
}
static int sol_test_back(float dt,
const struct v_ball *up,
const struct b_side *sp,
const float o[3],
const float w[3])
{
float q[3], d;
/* If the ball is not in front of the plane, the test passes. */
v_sub(q, up->p, o);
d = sp->d;
if (v_dot(q, sp->n) - d - up->r <= 0)
return 1;
/* If it's not in front of the plane after DT seconds, the test passes. */
v_mad(q, q, up->v, dt);
d += v_dot(w, sp->n) * dt;
if (v_dot(q, sp->n) - d - up->r <= 0)
return 1;
/* Else, test fails. */
return 0;
}
/*
* Compute the new angular velocity and orientation of a ball pendulum.
* A gives the accelleration of the ball. G gives the gravity vector.
*/
void sol_pendulum(struct v_ball *up,
const float a[3],
const float g[3], float dt)
{
float v[3], A[3], F[3], r[3], Y[3], T[3] = { 0.0f, 0.0f, 0.0f };
const float m = 5.000f;
const float ka = 0.500f;
const float kd = 0.995f;
/* Find the total force over DT. */
v_scl(A, a, ka);
v_mad(A, A, g, -dt);
/* Find the force. */
v_scl(F, A, m / dt);
/* Find the position of the pendulum. */
v_scl(r, up->E[1], -up->r);
/* Find the torque on the pendulum. */
if (fabsf(v_dot(r, F)) > 0.0f)
v_crs(T, F, r);
/* Apply the torque and dampen the angular velocity. */
v_mad(up->W, up->W, T, dt);
v_scl(up->W, up->W, kd);
/* Apply the angular velocity to the pendulum basis. */
sol_rotate(up->E, up->W, dt);
/* Apply a torque turning the pendulum toward the ball velocity. */
v_mad(v, up->v, up->E[1], v_dot(up->v, up->E[1]));
v_crs(Y, v, up->E[2]);
v_scl(Y, up->E[1], 2 * v_dot(Y, up->E[1]));
sol_rotate(up->E, Y, dt);
}
/*
* Compute the positions of all balls after DT seconds have passed.
*/
void sol_ball_step(struct s_vary *vary, float dt)
{
int i;
for (i = 0; i < vary->uc; i++)
{
struct v_ball *up = vary->uv + i;
v_mad(up->p, up->p, up->v, dt);
sol_rotate(up->e, up->w, dt);
}
}
/*
* 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 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;
}
void game_set_fly(float k)
{
struct s_vary *fp = &file.vary;
float x[3] = { 1.f, 0.f, 0.f };
float y[3] = { 0.f, 1.f, 0.f };
float z[3] = { 0.f, 0.f, 1.f };
float c0[3] = { 0.f, 0.f, 0.f };
float p0[3] = { 0.f, 0.f, 0.f };
float c1[3] = { 0.f, 0.f, 0.f };
float p1[3] = { 0.f, 0.f, 0.f };
float v[3];
if (!state)
return;
v_cpy(view_e[0], x);
v_cpy(view_e[1], y);
if (fp->base->zc > 0)
v_sub(view_e[2], fp->uv[ball].p, fp->base->zv[0].p);
else
v_cpy(view_e[2], z);
if (fabs(v_dot(view_e[1], view_e[2])) > 0.999)
v_cpy(view_e[2], z);
v_crs(view_e[0], view_e[1], view_e[2]);
v_crs(view_e[2], view_e[0], view_e[1]);
v_nrm(view_e[0], view_e[0]);
v_nrm(view_e[2], view_e[2]);
/* k = 0.0 view is at the ball. */
if (fp->uc > 0)
{
v_cpy(c0, fp->uv[ball].p);
v_cpy(p0, fp->uv[ball].p);
}
v_mad(p0, p0, view_e[1], view_dy);
v_mad(p0, p0, view_e[2], view_dz);
/* k = +1.0 view is s_view 0 */
if (k >= 0 && fp->base->wc > 0)
{
v_cpy(p1, fp->base->wv[0].p);
v_cpy(c1, fp->base->wv[0].q);
}
/* k = -1.0 view is s_view 1 */
if (k <= 0 && fp->base->wc > 1)
{
v_cpy(p1, fp->base->wv[1].p);
v_cpy(c1, fp->base->wv[1].q);
}
/* Interpolate the views. */
v_sub(v, p1, p0);
v_mad(view_p, p0, v, k * k);
v_sub(v, c1, c0);
v_mad(view_c, c0, v, k * k);
/* Orthonormalize the view basis. */
v_sub(view_e[2], view_p, view_c);
v_crs(view_e[0], view_e[1], view_e[2]);
v_crs(view_e[2], view_e[0], view_e[1]);
v_nrm(view_e[0], view_e[0]);
v_nrm(view_e[2], view_e[2]);
view_a = V_DEG(fatan2f(view_e[2][0], view_e[2][2]));
}
void game_update_view(float dt)
{
const float y[3] = { 0.f, 1.f, 0.f };
float dy;
float dz;
float k;
float e[3];
float d[3];
float s = 2.f * dt;
if (!state)
return;
/* Center the view about the ball. */
v_cpy(view_c, file.vary.uv[ball].p);
v_inv(view_v, file.vary.uv[ball].v);
switch (config_get_d(CONFIG_CAMERA))
{
case 2:
/* Camera 2: View vector is given by view angle. */
view_e[2][0] = fsinf(V_RAD(view_a));
view_e[2][1] = 0.f;
view_e[2][2] = fcosf(V_RAD(view_a));
s = 1.f;
break;
default:
/* View vector approaches the ball velocity vector. */
v_mad(e, view_v, y, v_dot(view_v, y));
v_inv(e, e);
k = v_dot(view_v, view_v);
v_sub(view_e[2], view_p, view_c);
v_mad(view_e[2], view_e[2], view_v, k * dt * 0.1f);
}
/* Orthonormalize the basis of the view in its new position. */
v_crs(view_e[0], view_e[1], view_e[2]);
v_crs(view_e[2], view_e[0], view_e[1]);
v_nrm(view_e[0], view_e[0]);
v_nrm(view_e[2], view_e[2]);
/* The current view (dy, dz) approaches the ideal (view_dy, view_dz). */
v_sub(d, view_p, view_c);
dy = v_dot(view_e[1], d);
dz = v_dot(view_e[2], d);
dy += (view_dy - dy) * s;
dz += (view_dz - dz) * s;
/* Compute the new view position. */
view_p[0] = view_p[1] = view_p[2] = 0.f;
v_mad(view_p, view_c, view_e[1], dy);
v_mad(view_p, view_p, view_e[2], dz);
view_a = V_DEG(fatan2f(view_e[2][0], view_e[2][2]));
}
float sol_step(struct s_vary *vary, const float *g, float dt, int ui, int *m)
{
float P[3], V[3], v[3], r[3], a[3], d, e, nt, b = 0.0f, tt = dt, t;
int c;
union cmd cmd;
if (ui < vary->uc)
{
struct v_ball *up = vary->uv + ui;
/* If the ball is in contact with a surface, apply friction. */
v_cpy(a, up->v);
v_cpy(v, up->v);
v_cpy(up->v, g);
if (m && sol_test_file(tt, P, V, up, vary) < 0.0005f)
{
v_cpy(up->v, v);
v_sub(r, P, up->p);
t = v_len(r) * v_len(g);
if (t == 0.f)
{
t = SMALL;
}
if ((d = v_dot(r, g) / t) > 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. */
for (c = 16; c > 0 && tt > 0; c--)
{
float st;
int mi, ms;
/* HACK: avoid stepping across path changes. */
st = tt;
for (mi = 0; mi < vary->mc; mi++)
{
struct v_move *mp = vary->mv + mi;
struct v_path *pp = vary->pv + mp->pi;
if (!pp->f)
continue;
if (mp->tm + ms_peek(st) > pp->base->tm)
st = MS_TO_TIME(pp->base->tm - mp->tm);
}
/* Miss collisions if we reach the iteration limit. */
if (c > 1)
nt = sol_test_file(st, P, V, up, vary);
else
nt = tt;
cmd.type = CMD_STEP_SIMULATION;
cmd.stepsim.dt = nt;
sol_cmd_enq(&cmd);
ms = ms_step(nt);
sol_move_step(vary, nt, ms);
sol_swch_step(vary, nt, ms);
sol_ball_step(vary, nt);
if (nt < st)
if (b < (d = sol_bounce(up, P, V, nt)))
b = d;
tt -= nt;
}
v_sub(a, up->v, a);
sol_pendulum(up, a, g, dt);
}
return b;
}
static float sol_test_body(float dt,
float T[3], float V[3],
const struct v_ball *up,
const struct s_vary *vary,
const struct v_body *bp)
{
float U[3], O[3], E[4], W[3], u;
const struct b_node *np = vary->base->nv + bp->base->ni;
sol_body_p(O, vary, bp, 0.0f);
sol_body_v(W, vary, bp, dt);
sol_body_e(E, vary, bp, 0.0f);
/*
* For rotating bodies, rather than rotate every normal and vertex
* of the body, we temporarily pretend the ball is rotating and
* moving about a static body.
*/
/*
* Linear velocity of a point rotating about the origin:
* v = w x p
*/
if (E[0] != 1.0f || sol_body_w(vary, bp))
{
/* The body has a non-identity orientation or it is rotating. */
struct v_ball ball;
float e[4], p0[3], p1[3];
const float z[3] = { 0 };
/* First, calculate position at start and end of time interval. */
v_sub(p0, up->p, O);
v_cpy(p1, p0);
q_conj(e, E);
q_rot(p0, e, p0);
v_mad(p1, p1, up->v, dt);
v_mad(p1, p1, W, -dt);
sol_body_e(e, vary, bp, dt);
q_conj(e, e);
q_rot(p1, e, p1);
/* Set up ball struct with values relative to body. */
ball = *up;
v_cpy(ball.p, p0);
/* Calculate velocity from start/end positions and time. */
v_sub(ball.v, p1, p0);
v_scl(ball.v, ball.v, 1.0f / dt);
if ((u = sol_test_node(dt, U, &ball, vary->base, np, z, z)) < dt)
{
/* Compute the final orientation. */
sol_body_e(e, vary, bp, u);
/* Return world space coordinates. */
q_rot(T, e, U);
v_add(T, O, T);
/* Move forward. */
v_mad(T, T, W, u);
/* Express "non-ball" velocity. */
q_rot(V, e, ball.v);
v_sub(V, up->v, V);
dt = u;
}
}
else
{
if ((u = sol_test_node(dt, U, up, vary->base, np, O, W)) < dt)
{
v_cpy(T, U);
v_cpy(V, W);
dt = u;
}
}
return dt;
}
/*
* 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);
/*
* Think projections. Vectors D, extending from the edge vertex Q
* to the sphere, and E, the relative velocity of sphere wrt the
* edge, are made orthogonal to the edge vector U. Division of
* the dot products is required to obtain the true projection
* ratios since U does not have unit length.
*/
du = v_dot(d, u);
eu = v_dot(e, u);
uu = v_dot(u, u);
v_mad(P, d, u, -du / uu);
/* First, test for intersection. */
if (v_dot(P, P) < r * r)
{
/* The sphere already intersects the line of the edge. */
if (du < 0 || du > uu)
{
/*
* The sphere is behind the endpoints of the edge, and
* can't hit the edge without hitting the vertices first.
*/
return LARGE;
}
/* The sphere already intersects the edge. */
if (v_dot(P, e) >= 0)
{
/* Moving apart. */
return LARGE;
}
v_nrm(P, P);
v_mad(Q, p, P, -r);
return 0;
}
v_mad(V, e, u, -eu / uu);
t = v_sol(P, V, r);
s = (du + eu * t) / uu; /* Projection of D + E * t on U. */
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;
}
void game_set_fly(float k)
{
struct s_file *fp = &file;
float x[3] = { 1.f, 0.f, 0.f };
float y[3] = { 0.f, 1.f, 0.f };
float z[3] = { 0.f, 0.f, 1.f };
float c0[3] = { 0.f, 0.f, 0.f };
float p0[3] = { 0.f, 0.f, 0.f };
float c1[3] = { 0.f, 0.f, 0.f };
float p1[3] = { 0.f, 0.f, 0.f };
float v[3];
v_cpy(view_e[0], x);
v_cpy(view_e[1], y);
v_cpy(view_e[2], z);
/* k = 0.0 view is at the ball. */
if (fp->uc > 0)
{
v_cpy(c0, fp->uv[0].p);
v_cpy(p0, fp->uv[0].p);
}
v_mad(p0, p0, y, view_dp);
v_mad(p0, p0, z, view_dz);
v_mad(c0, c0, y, view_dc);
/* k = +1.0 view is s_view 0 */
if (k >= 0 && fp->wc > 0)
{
v_cpy(p1, fp->wv[0].p);
v_cpy(c1, fp->wv[0].q);
}
/* k = -1.0 view is s_view 1 */
if (k <= 0 && fp->wc > 1)
{
v_cpy(p1, fp->wv[1].p);
v_cpy(c1, fp->wv[1].q);
}
/* Interpolate the views. */
v_sub(v, p1, p0);
v_mad(view_p, p0, v, k * k);
v_sub(v, c1, c0);
v_mad(view_c, c0, v, k * k);
/* Orthonormalize the view basis. */
v_sub(view_e[2], view_p, view_c);
v_crs(view_e[0], view_e[1], view_e[2]);
v_crs(view_e[2], view_e[0], view_e[1]);
v_nrm(view_e[0], view_e[0]);
v_nrm(view_e[2], view_e[2]);
}
static void game_update_view(float dt)
{
float dc = view_dc * (jump_b ? 2.0f * fabsf(jump_dt - 0.5f) : 1.0f);
float dx = view_ry * dt * 5.0f;
float k;
view_a += view_ry * dt * 90.f;
/* Center the view about the ball. */
v_cpy(view_c, file.uv->p);
v_inv(view_v, file.uv->v);
switch (config_get_d(CONFIG_CAMERA))
{
case 1: /* Camera 1: Viewpoint chases the ball position. */
v_sub(view_e[2], view_p, view_c);
break;
case 2: /* Camera 2: View vector is given by view angle. */
view_e[2][0] = fsinf(V_RAD(view_a));
view_e[2][1] = 0.f;
view_e[2][2] = fcosf(V_RAD(view_a));
dx = 0.0f;
break;
default: /* Default: View vector approaches the ball velocity vector. */
k = v_dot(view_v, view_v);
v_sub(view_e[2], view_p, view_c);
v_mad(view_e[2], view_e[2], view_v, k * dt / 4);
break;
}
/* Orthonormalize the basis of the view in its new position. */
v_crs(view_e[0], view_e[1], view_e[2]);
v_crs(view_e[2], view_e[0], view_e[1]);
v_nrm(view_e[0], view_e[0]);
v_nrm(view_e[2], view_e[2]);
/* Compute the new view position. */
k = 1.0f + v_dot(view_e[2], view_v) / 10.0f;
view_k = view_k + (k - view_k) * dt;
if (view_k < 0.5) view_k = 0.5;
v_cpy(view_p, file.uv->p);
v_mad(view_p, view_p, view_e[0], dx * view_k);
v_mad(view_p, view_p, view_e[1], view_dp * view_k);
v_mad(view_p, view_p, view_e[2], view_dz * view_k);
/* Compute the new view center. */
v_cpy(view_c, file.uv->p);
v_mad(view_c, view_c, view_e[1], dc);
/* Note the current view angle. */
view_a = V_DEG(fatan2f(view_e[2][0], view_e[2][2]));
}
static void game_update_view(float dt)
{
float dc = view.dc * (jump_b > 0 ? 2.0f * fabsf(jump_dt - 0.5f) : 1.0f);
float da = input_get_r() * dt * 90.0f;
float k;
float M[16], v[3], Y[3] = { 0.0f, 1.0f, 0.0f };
float view_v[3];
float spd = (float) cam_speed(input_get_c()) / 1000.0f;
/* Track manual rotation time. */
if (da == 0.0f)
{
if (view_time < 0.0f)
{
/* Transition time is influenced by activity time. */
view_fade = CLAMP(VIEW_FADE_MIN, -view_time, VIEW_FADE_MAX);
view_time = 0.0f;
}
/* Inactivity. */
view_time += dt;
}
else
{
if (view_time > 0.0f)
{
view_fade = 0.0f;
view_time = 0.0f;
}
/* Activity (yes, this is negative). */
view_time -= dt;
}
/* Center the view about the ball. */
v_cpy(view.c, vary.uv->p);
view_v[0] = -vary.uv->v[0];
view_v[1] = 0.0f;
view_v[2] = -vary.uv->v[2];
/* Compute view vector. */
if (spd >= 0.0f)
{
/* Viewpoint chases ball position. */
if (da == 0.0f)
{
float s;
v_sub(view.e[2], view.p, view.c);
v_nrm(view.e[2], view.e[2]);
/* Gradually restore view vector convergence rate. */
s = fpowf(view_time, 3.0f) / fpowf(view_fade, 3.0f);
s = CLAMP(0.0f, s, 1.0f);
v_mad(view.e[2], view.e[2], view_v, v_len(view_v) * spd * s * dt);
}
}
else
{
/* View vector is given by view angle. */
view.e[2][0] = fsinf(V_RAD(view.a));
view.e[2][1] = 0.0;
view.e[2][2] = fcosf(V_RAD(view.a));
}
/* Apply manual rotation. */
if (da != 0.0f)
{
m_rot(M, Y, V_RAD(da));
m_vxfm(v, M, view.e[2]);
v_cpy(view.e[2], v);
}
/* Orthonormalize the new view reference frame. */
v_crs(view.e[0], view.e[1], view.e[2]);
v_crs(view.e[2], view.e[0], view.e[1]);
v_nrm(view.e[0], view.e[0]);
v_nrm(view.e[2], view.e[2]);
/* Compute the new view position. */
k = 1.0f + v_dot(view.e[2], view_v) / 10.0f;
view_k = view_k + (k - view_k) * dt;
if (view_k < 0.5) view_k = 0.5;
v_scl(v, view.e[1], view.dp * view_k);
v_mad(v, v, view.e[2], view.dz * view_k);
v_add(view.p, v, vary.uv->p);
/* Compute the new view center. */
v_cpy(view.c, vary.uv->p);
v_mad(view.c, view.c, view.e[1], dc);
/* Note the current view angle. */
view.a = V_DEG(fatan2f(view.e[2][0], view.e[2][2]));
game_cmd_updview();
}