2 * Copyright (C) 2003-2010 Neverball authors
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.
16 /* Random code used in more than one place. */
18 #include "solid_all.h"
19 #include "solid_cmd.h"
20 #include "solid_vary.h"
26 /*---------------------------------------------------------------------------*/
28 static float erp(float t)
30 return 3.0f * t * t - 2.0f * t * t * t;
34 static float derp(float t)
36 return 6.0f * t - 6.0f * t * t;
40 void sol_body_p(float p[3], const struct s_vary *vary, int pi, float t)
46 const struct b_path *pp = vary->base->pv + pi;
47 const struct b_path *pq = vary->base->pv + pp->pi;
49 float s = MIN(t / pp->t, 1.0f);
51 v_sub(v, pq->p, pp->p);
52 v_mad(p, pp->p, v, pp->s ? erp(s) : s);
62 void sol_body_v(float v[3],
63 const struct s_vary *vary,
64 int pi, float t, float dt)
66 if (pi >= 0 && vary->pv[pi].f)
70 sol_body_p(p, vary, pi, t);
71 sol_body_p(q, vary, pi, t + dt);
87 void sol_body_e(float e[4],
88 const struct s_vary *vary,
89 const struct v_body *bp,
92 struct b_path *pp = vary->base->pv + bp->pi;
96 struct b_path *pq = vary->base->pv + pp->pi;
98 if (pp->fl & P_ORIENTED || pq->fl & P_ORIENTED)
100 if (!vary->pv[bp->pi].f)
103 q_slerp(e, pp->e, pq->e, (bp->t + dt) / pp->t);
114 void sol_body_w(float w[3],
115 const struct s_vary *vary,
116 const struct v_body *bp)
118 struct b_path *pp = vary->base->pv + bp->pi;
120 if (bp->pi >= 0 && vary->pv[bp->pi].f)
122 struct b_path *pq = vary->base->pv + pp->pi;
124 if (pp->fl & P_ORIENTED || pq->fl & P_ORIENTED)
142 /* Match slerp by using the short path. */
152 q_as_axisangle(d, w, &a);
153 v_scl(w, w, a / pp->t);
163 /*---------------------------------------------------------------------------*/
166 * Integrate the rotation of the given basis E under angular velocity W
169 void sol_rotate(float e[3][3], const float w[3], float dt)
173 float a[3], M[16], f[3][3];
175 /* Compute the rotation matrix. */
178 m_rot(M, a, v_len(w) * dt);
180 /* Apply it to the basis. */
182 m_vxfm(f[0], M, e[0]);
183 m_vxfm(f[1], M, e[1]);
184 m_vxfm(f[2], M, e[2]);
186 /* Re-orthonormalize the basis. */
188 v_crs(e[2], f[0], f[1]);
189 v_crs(e[1], f[2], f[0]);
190 v_crs(e[0], f[1], f[2]);
198 /*---------------------------------------------------------------------------*/
201 * Compute the new angular velocity and orientation of a ball pendulum.
202 * A gives the accelleration of the ball. G gives the gravity vector.
204 void sol_pendulum(struct v_ball *up,
206 const float g[3], float dt)
208 float v[3], A[3], F[3], r[3], Y[3], T[3] = { 0.0f, 0.0f, 0.0f };
210 const float m = 5.000f;
211 const float ka = 0.500f;
212 const float kd = 0.995f;
214 /* Find the total force over DT. */
219 /* Find the force. */
223 /* Find the position of the pendulum. */
225 v_scl(r, up->E[1], -up->r);
227 /* Find the torque on the pendulum. */
229 if (fabsf(v_dot(r, F)) > 0.0f)
232 /* Apply the torque and dampen the angular velocity. */
234 v_mad(up->W, up->W, T, dt);
235 v_scl(up->W, up->W, kd);
237 /* Apply the angular velocity to the pendulum basis. */
239 sol_rotate(up->E, up->W, dt);
241 /* Apply a torque turning the pendulum toward the ball velocity. */
243 v_mad(v, up->v, up->E[1], v_dot(up->v, up->E[1]));
244 v_crs(Y, v, up->E[2]);
245 v_scl(Y, up->E[1], 2 * v_dot(Y, up->E[1]));
247 sol_rotate(up->E, Y, dt);
250 /*---------------------------------------------------------------------------*/
252 static void sol_path_flag(struct s_vary *vary, int pi, int f)
256 if (vary->pv[pi].f == f)
261 cmd.type = CMD_PATH_FLAG;
262 cmd.pathflag.pi = pi;
263 cmd.pathflag.f = vary->pv[pi].f;
267 static void sol_path_loop(struct s_vary *vary, int p0, int f)
273 do /* Tortoise and hare cycle traverser. */
275 sol_path_flag(vary, pi, f);
277 pi = vary->base->pv[pi].pi;
278 pj = vary->base->pv[pj].pi;
279 pj = vary->base->pv[pj].pi;
284 * At this point, the indices point to a node in the loop, but we
285 * still need to walk any remaining nodes in that loop. This is
286 * essentially the second part of the tortoise and hare algorithm
287 * which finds the start of the loop, although we only care about
288 * walking the remaining nodes.
296 sol_path_flag(vary, pi, f);
298 pi = vary->base->pv[pi].pi;
299 pj = vary->base->pv[pj].pi;
301 while (pi != pj && pi != pk);
304 /*---------------------------------------------------------------------------*/
307 * Compute the states of all switches after DT seconds have passed.
309 void sol_swch_step(struct s_vary *vary, float dt, int ms)
315 for (xi = 0; xi < vary->xc; xi++)
317 struct v_swch *xp = vary->xv + xi;
319 if (xp->tm < xp->base->tm)
324 if (xp->tm >= xp->base->tm)
326 sol_path_loop(vary, xp->base->pi, xp->base->f);
330 cmd.type = CMD_SWCH_TOGGLE;
331 cmd.swchtoggle.xi = xi;
339 * Compute the positions of all bodies after DT seconds have passed.
341 void sol_body_step(struct s_vary *vary, float dt, int ms)
347 for (i = 0; i < vary->bc; i++)
349 struct v_body *bp = vary->bv + i;
350 struct v_path *pp = vary->pv + bp->pi;
352 if (bp->pi >= 0 && pp->f)
357 if (bp->tm >= pp->base->tm)
361 bp->pi = pp->base->pi;
363 cmd.type = CMD_BODY_TIME;
365 cmd.bodytime.t = bp->t;
368 cmd.type = CMD_BODY_PATH;
370 cmd.bodypath.pi = bp->pi;
378 * Compute the positions of all balls after DT seconds have passed.
380 void sol_ball_step(struct s_vary *vary, float dt)
384 for (i = 0; i < vary->uc; i++)
386 struct v_ball *up = vary->uv + i;
388 v_mad(up->p, up->p, up->v, dt);
390 sol_rotate(up->e, up->w, dt);
394 /*---------------------------------------------------------------------------*/
396 int sol_item_test(struct s_vary *vary, float *p, float item_r)
398 const float *ball_p = vary->uv->p;
399 const float ball_r = vary->uv->r;
402 for (hi = 0; hi < vary->hc; hi++)
404 struct v_item *hp = vary->hv + hi;
407 v_sub(r, ball_p, hp->p);
409 if (hp->t != ITEM_NONE && v_len(r) < ball_r + item_r)
421 struct b_goal *sol_goal_test(struct s_vary *vary, float *p, int ui)
423 const float *ball_p = vary->uv[ui].p;
424 const float ball_r = vary->uv[ui].r;
427 for (zi = 0; zi < vary->base->zc; zi++)
429 struct b_goal *zp = vary->base->zv + zi;
432 r[0] = ball_p[0] - zp->p[0];
433 r[1] = ball_p[2] - zp->p[2];
436 if (v_len(r) + ball_r < zp->r &&
437 ball_p[1] > zp->p[1] &&
438 ball_p[1] < zp->p[1] + GOAL_HEIGHT / 2)
451 * Test for a ball entering a teleporter.
453 int sol_jump_test(struct s_vary *vary, float *p, int ui)
455 const float *ball_p = vary->uv[ui].p;
456 const float ball_r = vary->uv[ui].r;
459 for (ji = 0; ji < vary->base->jc; ji++)
461 struct b_jump *jp = vary->base->jv + ji;
464 r[0] = ball_p[0] - jp->p[0];
465 r[1] = ball_p[2] - jp->p[2];
468 /* Distance of the far side from the edge of the halo. */
470 d = v_len(r) + ball_r - jp->r;
473 * The "inside" distance, which must be cleared before being
474 * able to trigger a teleporter, is the ball's radius. (This
475 * is different from switches.)
479 ball_p[1] > jp->p[1] &&
480 ball_p[1] < jp->p[1] + JUMP_HEIGHT / 2)
486 p[0] = jp->q[0] + (ball_p[0] - jp->p[0]);
487 p[1] = jp->q[1] + (ball_p[1] - jp->p[1]);
488 p[2] = jp->q[2] + (ball_p[2] - jp->p[2]);
494 return in ? JUMP_INSIDE : JUMP_OUTSIDE;
498 * Test for a ball entering a switch.
500 int sol_swch_test(struct s_vary *vary, int ui)
502 const float *ball_p = vary->uv[ui].p;
503 const float ball_r = vary->uv[ui].r;
507 int xi, rc = SWCH_OUTSIDE;
509 for (xi = 0; xi < vary->xc; xi++)
511 struct v_swch *xp = vary->xv + xi;
513 /* FIXME enter/exit events don't work for timed switches */
515 if (xp->base->t == 0 || xp->f == xp->base->f)
519 r[0] = ball_p[0] - xp->base->p[0];
520 r[1] = ball_p[2] - xp->base->p[2];
523 /* Distance of the far side from the edge of the halo. */
525 d = v_len(r) + ball_r - xp->base->r;
528 * The "inside" distance, which must be cleared before
529 * being able to trigger a switch, is the ball's diameter.
530 * (This is different from teleporters.)
533 if (d <= ball_r * 2 &&
534 ball_p[1] > xp->base->p[1] &&
535 ball_p[1] < xp->base->p[1] + SWCH_HEIGHT / 2)
537 if (!xp->e && d <= 0.0f)
539 /* The ball enters. */
541 if (xp->base->tm == 0)
545 cmd.type = CMD_SWCH_ENTER;
546 cmd.swchenter.xi = xi;
550 /* Toggle the state, update the path. */
552 xp->f = xp->f ? 0 : 1;
554 cmd.type = CMD_SWCH_TOGGLE;
555 cmd.swchtoggle.xi = xi;
558 sol_path_loop(vary, xp->base->pi, xp->f);
560 /* It toggled to non-default state, start the timer. */
562 if (xp->f != xp->base->f)
568 /* If visible, set the result. */
575 /* The ball exits. */
581 cmd.type = CMD_SWCH_EXIT;
582 cmd.swchexit.xi = xi;
590 /*---------------------------------------------------------------------------*/