/*
* 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));
}
/*
* 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);
}
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;
}
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();
}