7983a7ba6b37d73c80f6d23f1d0b1e03e2703b13
[neverball] / share / solid.c
1 /*   
2  * Copyright (C) 2003 Robert Kooima
3  *
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.
8  *
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.
13  */
14
15 #include <SDL.h>
16 #include <SDL_rwops.h>
17 #include <SDL_image.h>
18
19 #include <stdio.h>
20 #include <stdlib.h>
21 #include <string.h>
22 #include <math.h>
23
24 #include "glext.h"
25 #include "vec3.h"
26 #include "geom.h" /* Only for height constants! */
27 #include "base_image.h"
28 #include "solid.h"
29 #include "base_config.h"
30 #include "binary.h"
31
32 #define MAGIC 0x4F425251  /* Neverball sol file magic number (should not change) */
33 #define SOL_VERSION  4    /* Neverball sol file format version (can change)      */
34
35 #define LARGE 1.0e+5f
36
37 /*---------------------------------------------------------------------------*/
38
39 static float erp(float t)
40 {
41     return 3.0f * t * t - 2.0f * t * t * t;
42 }
43
44 static float derp(float t)
45 {
46     return 6.0f * t     - 6.0f * t * t;
47 }
48
49 static void sol_body_v(float v[3],
50                        const struct s_file *fp,
51                        const struct s_body *bp)
52 {
53     if (bp->pi >= 0 && fp->pv[bp->pi].f)
54     {
55         const struct s_path *pp = fp->pv + bp->pi;
56         const struct s_path *pq = fp->pv + pp->pi;
57
58         v_sub(v, pq->p, pp->p);
59         v_scl(v, v, 1.0f / pp->t);
60
61         v_scl(v, v, derp(bp->t / pp->t));
62     }
63     else
64     {
65         v[0] = 0.0f;
66         v[1] = 0.0f;
67         v[2] = 0.0f;
68     }
69 }
70
71 void sol_body_p(float p[3],
72                        const struct s_file *fp,
73                        const struct s_body *bp)
74 {
75     float v[3];
76
77     if (bp->pi >= 0)
78     {
79         const struct s_path *pp = fp->pv + bp->pi;
80         const struct s_path *pq = fp->pv + pp->pi;
81
82         v_sub(v, pq->p, pp->p);
83         v_mad(p, pp->p, v, erp(bp->t / pp->t));
84     }
85     else
86     {
87         p[0] = 0.0f;
88         p[1] = 0.0f;
89         p[2] = 0.0f;
90     }
91 }
92
93 /*---------------------------------------------------------------------------*/
94
95 static void sol_load_mtrl(FILE *fin, struct s_mtrl *mp)
96 {
97     get_array(fin,  mp->a, 4);
98     get_array(fin,  mp->d, 4);
99     get_array(fin,  mp->s, 4);
100     get_array(fin,  mp->e, 4);
101     get_array(fin,  mp->h, 1);
102     get_index(fin, &mp->fl);
103
104     fread(mp->f, 1, PATHMAX, fin);
105 }
106
107 static void sol_load_vert(FILE *fin, struct s_vert *vp)
108 {
109     get_array(fin,  vp->p, 3);
110 }
111
112 static void sol_load_edge(FILE *fin, struct s_edge *ep)
113 {
114     get_index(fin, &ep->vi);
115     get_index(fin, &ep->vj);
116 }
117
118 static void sol_load_side(FILE *fin, struct s_side *sp)
119 {
120     get_array(fin,  sp->n, 3);
121     get_float(fin, &sp->d);
122 }
123
124 static void sol_load_texc(FILE *fin, struct s_texc *tp)
125 {
126     get_array(fin,  tp->u, 2);
127 }
128
129 static void sol_load_geom(FILE *fin, struct s_geom *gp)
130 {
131     get_index(fin, &gp->mi);
132     get_index(fin, &gp->ti);
133     get_index(fin, &gp->si);
134     get_index(fin, &gp->vi);
135     get_index(fin, &gp->tj);
136     get_index(fin, &gp->sj);
137     get_index(fin, &gp->vj);
138     get_index(fin, &gp->tk);
139     get_index(fin, &gp->sk);
140     get_index(fin, &gp->vk);
141 }
142
143 static void sol_load_lump(FILE *fin, struct s_lump *lp)
144 {
145     get_index(fin, &lp->fl);
146     get_index(fin, &lp->v0);
147     get_index(fin, &lp->vc);
148     get_index(fin, &lp->e0);
149     get_index(fin, &lp->ec);
150     get_index(fin, &lp->g0);
151     get_index(fin, &lp->gc);
152     get_index(fin, &lp->s0);
153     get_index(fin, &lp->sc);
154 }
155
156 static void sol_load_node(FILE *fin, struct s_node *np)
157 {
158     get_index(fin, &np->si);
159     get_index(fin, &np->ni);
160     get_index(fin, &np->nj);
161     get_index(fin, &np->l0);
162     get_index(fin, &np->lc);
163 }
164
165 static void sol_load_path(FILE *fin, struct s_path *pp)
166 {
167     get_array(fin,  pp->p, 3);
168     get_float(fin, &pp->t);
169     get_index(fin, &pp->pi);
170     get_index(fin, &pp->f);
171 }
172
173 static void sol_load_body(FILE *fin, struct s_body *bp)
174 {
175     get_index(fin, &bp->pi);
176     get_index(fin, &bp->ni);
177     get_index(fin, &bp->l0);
178     get_index(fin, &bp->lc);
179     get_index(fin, &bp->g0);
180     get_index(fin, &bp->gc);
181 }
182
183 static void sol_load_coin(FILE *fin, struct s_coin *cp)
184 {
185     get_array(fin,  cp->p, 3);
186     get_index(fin, &cp->n);
187 }
188
189 static void sol_load_goal(FILE *fin, struct s_goal *zp)
190 {
191     get_array(fin,  zp->p, 3);
192     get_float(fin, &zp->r);
193     get_index(fin, &zp->s);
194     get_index(fin, &zp->c);
195 }
196
197 static void sol_load_swch(FILE *fin, struct s_swch *xp)
198 {
199     get_array(fin,  xp->p, 3);
200     get_float(fin, &xp->r);
201     get_index(fin, &xp->pi);
202     get_float(fin, &xp->t0);
203     get_float(fin, &xp->t);
204     get_index(fin, &xp->f0);
205     get_index(fin, &xp->f);
206     get_index(fin, &xp->i);
207 }
208
209 static void sol_load_bill(FILE *fin, struct s_bill *rp)
210 {
211     get_index(fin, &rp->fl);
212     get_index(fin, &rp->mi);
213     get_float(fin, &rp->t);
214     get_float(fin, &rp->d);
215     get_array(fin,  rp->w,  3);
216     get_array(fin,  rp->h,  3);
217     get_array(fin,  rp->rx, 3);
218     get_array(fin,  rp->ry, 3);
219     get_array(fin,  rp->rz, 3);
220 }
221
222 static void sol_load_jump(FILE *fin, struct s_jump *jp)
223 {
224     get_array(fin,  jp->p, 3);
225     get_array(fin,  jp->q, 3);
226     get_float(fin, &jp->r);
227 }
228
229 static void sol_load_ball(FILE *fin, struct s_ball *bp)
230 {
231     get_array(fin,  bp->e[0], 3);
232     get_array(fin,  bp->e[1], 3);
233     get_array(fin,  bp->e[2], 3);
234     get_array(fin,  bp->p,    3);
235     get_float(fin, &bp->r);
236 }
237
238 static void sol_load_view(FILE *fin, struct s_view *wp)
239 {
240     get_array(fin,  wp->p, 3);
241     get_array(fin,  wp->q, 3);
242 }
243
244 static int sol_load_file(FILE *fin, struct s_file *fp)
245 {
246     int i;
247     int magic;
248     int version;
249
250     get_index(fin, &magic);
251     get_index(fin, &version);
252     if (magic != MAGIC || version != SOL_VERSION)
253         return 0;
254
255     get_index(fin, &fp->mc);
256     get_index(fin, &fp->vc);
257     get_index(fin, &fp->ec);
258     get_index(fin, &fp->sc);
259     get_index(fin, &fp->tc);
260     get_index(fin, &fp->gc);
261     get_index(fin, &fp->lc);
262     get_index(fin, &fp->nc);
263     get_index(fin, &fp->pc);
264     get_index(fin, &fp->bc);
265     get_index(fin, &fp->cc);
266     get_index(fin, &fp->zc);
267     get_index(fin, &fp->jc);
268     get_index(fin, &fp->xc);
269     get_index(fin, &fp->rc);
270     get_index(fin, &fp->uc);
271     get_index(fin, &fp->wc);
272     get_index(fin, &fp->ic);
273     get_index(fin, &fp->ac);
274
275     if (fp->mc)
276         fp->mv = (struct s_mtrl *) calloc(fp->mc, sizeof (struct s_mtrl));
277     if (fp->vc)
278         fp->vv = (struct s_vert *) calloc(fp->vc, sizeof (struct s_vert));
279     if (fp->ec)
280         fp->ev = (struct s_edge *) calloc(fp->ec, sizeof (struct s_edge));
281     if (fp->sc)
282         fp->sv = (struct s_side *) calloc(fp->sc, sizeof (struct s_side));
283     if (fp->tc)
284         fp->tv = (struct s_texc *) calloc(fp->tc, sizeof (struct s_texc));
285     if (fp->gc)
286         fp->gv = (struct s_geom *) calloc(fp->gc, sizeof (struct s_geom));
287     if (fp->lc)
288         fp->lv = (struct s_lump *) calloc(fp->lc, sizeof (struct s_lump));
289     if (fp->nc)
290         fp->nv = (struct s_node *) calloc(fp->nc, sizeof (struct s_node));
291     if (fp->pc)
292         fp->pv = (struct s_path *) calloc(fp->pc, sizeof (struct s_path));
293     if (fp->bc)
294         fp->bv = (struct s_body *) calloc(fp->bc, sizeof (struct s_body));
295     if (fp->cc)
296         fp->cv = (struct s_coin *) calloc(fp->cc, sizeof (struct s_coin));
297     if (fp->zc)
298         fp->zv = (struct s_goal *) calloc(fp->zc, sizeof (struct s_goal));
299     if (fp->jc)
300         fp->jv = (struct s_jump *) calloc(fp->jc, sizeof (struct s_jump));
301     if (fp->xc)
302         fp->xv = (struct s_swch *) calloc(fp->xc, sizeof (struct s_swch));
303     if (fp->rc)
304         fp->rv = (struct s_bill *) calloc(fp->rc, sizeof (struct s_bill));
305     if (fp->uc)
306         fp->uv = (struct s_ball *) calloc(fp->uc, sizeof (struct s_ball));
307     if (fp->wc)
308         fp->wv = (struct s_view *) calloc(fp->wc, sizeof (struct s_view));
309     if (fp->ic)
310         fp->iv = (int           *) calloc(fp->ic, sizeof (int));
311     if (fp->ac)
312         fp->av = (char          *) calloc(fp->ac, sizeof (char));
313
314     for (i = 0; i < fp->mc; i++) sol_load_mtrl(fin, fp->mv + i);
315     for (i = 0; i < fp->vc; i++) sol_load_vert(fin, fp->vv + i);
316     for (i = 0; i < fp->ec; i++) sol_load_edge(fin, fp->ev + i);
317     for (i = 0; i < fp->sc; i++) sol_load_side(fin, fp->sv + i);
318     for (i = 0; i < fp->tc; i++) sol_load_texc(fin, fp->tv + i);
319     for (i = 0; i < fp->gc; i++) sol_load_geom(fin, fp->gv + i);
320     for (i = 0; i < fp->lc; i++) sol_load_lump(fin, fp->lv + i);
321     for (i = 0; i < fp->nc; i++) sol_load_node(fin, fp->nv + i);
322     for (i = 0; i < fp->pc; i++) sol_load_path(fin, fp->pv + i);
323     for (i = 0; i < fp->bc; i++) sol_load_body(fin, fp->bv + i);
324     for (i = 0; i < fp->cc; i++) sol_load_coin(fin, fp->cv + i);
325     for (i = 0; i < fp->zc; i++) sol_load_goal(fin, fp->zv + i);
326     for (i = 0; i < fp->jc; i++) sol_load_jump(fin, fp->jv + i);
327     for (i = 0; i < fp->xc; i++) sol_load_swch(fin, fp->xv + i);
328     for (i = 0; i < fp->rc; i++) sol_load_bill(fin, fp->rv + i);
329     for (i = 0; i < fp->uc; i++) sol_load_ball(fin, fp->uv + i);
330     for (i = 0; i < fp->wc; i++) sol_load_view(fin, fp->wv + i);
331     for (i = 0; i < fp->ic; i++) get_index(fin, fp->iv + i);
332
333     if (fp->ac) fread(fp->av, 1, fp->ac, fin);
334     
335     return 1;
336 }
337
338 int sol_load_only_file(struct s_file *fp, const char *filename)
339 {
340     FILE *fin;
341     int res = 0;
342
343     if ((fin = fopen(filename, FMODE_RB)))
344     {
345         res = sol_load_file(fin, fp);
346         fclose(fin);
347     }
348     return res;
349 }
350
351 /*---------------------------------------------------------------------------*/
352
353 static void sol_stor_mtrl(FILE *fout, struct s_mtrl *mp)
354 {
355     put_array(fout,  mp->a, 4);
356     put_array(fout,  mp->d, 4);
357     put_array(fout,  mp->s, 4);
358     put_array(fout,  mp->e, 4);
359     put_array(fout,  mp->h, 1);
360     put_index(fout, &mp->fl);
361
362     fwrite(mp->f, 1, PATHMAX, fout);
363 }
364
365 static void sol_stor_vert(FILE *fout, struct s_vert *vp)
366 {
367     put_array(fout,  vp->p, 3);
368 }
369
370 static void sol_stor_edge(FILE *fout, struct s_edge *ep)
371 {
372     put_index(fout, &ep->vi);
373     put_index(fout, &ep->vj);
374 }
375
376 static void sol_stor_side(FILE *fout, struct s_side *sp)
377 {
378     put_array(fout,  sp->n, 3);
379     put_float(fout, &sp->d);
380 }
381
382 static void sol_stor_texc(FILE *fout, struct s_texc *tp)
383 {
384     put_array(fout,  tp->u, 2);
385 }
386
387 static void sol_stor_geom(FILE *fout, struct s_geom *gp)
388 {
389     put_index(fout, &gp->mi);
390     put_index(fout, &gp->ti);
391     put_index(fout, &gp->si);
392     put_index(fout, &gp->vi);
393     put_index(fout, &gp->tj);
394     put_index(fout, &gp->sj);
395     put_index(fout, &gp->vj);
396     put_index(fout, &gp->tk);
397     put_index(fout, &gp->sk);
398     put_index(fout, &gp->vk);
399 }
400
401 static void sol_stor_lump(FILE *fout, struct s_lump *lp)
402 {
403     put_index(fout, &lp->fl);
404     put_index(fout, &lp->v0);
405     put_index(fout, &lp->vc);
406     put_index(fout, &lp->e0);
407     put_index(fout, &lp->ec);
408     put_index(fout, &lp->g0);
409     put_index(fout, &lp->gc);
410     put_index(fout, &lp->s0);
411     put_index(fout, &lp->sc);
412 }
413
414 static void sol_stor_node(FILE *fout, struct s_node *np)
415 {
416     put_index(fout, &np->si);
417     put_index(fout, &np->ni);
418     put_index(fout, &np->nj);
419     put_index(fout, &np->l0);
420     put_index(fout, &np->lc);
421 }
422
423 static void sol_stor_path(FILE *fout, struct s_path *pp)
424 {
425     put_array(fout,  pp->p, 3);
426     put_float(fout, &pp->t);
427     put_index(fout, &pp->pi);
428     put_index(fout, &pp->f);
429 }
430
431 static void sol_stor_body(FILE *fout, struct s_body *bp)
432 {
433     put_index(fout, &bp->pi);
434     put_index(fout, &bp->ni);
435     put_index(fout, &bp->l0);
436     put_index(fout, &bp->lc);
437     put_index(fout, &bp->g0);
438     put_index(fout, &bp->gc);
439 }
440
441 static void sol_stor_coin(FILE *fout, struct s_coin *cp)
442 {
443     put_array(fout,  cp->p, 3);
444     put_index(fout, &cp->n);
445 }
446
447 static void sol_stor_goal(FILE *fout, struct s_goal *zp)
448 {
449     put_array(fout,  zp->p, 3);
450     put_float(fout, &zp->r);
451     put_index(fout, &zp->s);
452     put_index(fout, &zp->c);
453 }
454
455 static void sol_stor_swch(FILE *fout, struct s_swch *xp)
456 {
457     put_array(fout,  xp->p, 3);
458     put_float(fout, &xp->r);
459     put_index(fout, &xp->pi);
460     put_float(fout, &xp->t0);
461     put_float(fout, &xp->t);
462     put_index(fout, &xp->f0);
463     put_index(fout, &xp->f);
464     put_index(fout, &xp->i);
465 }
466
467 static void sol_stor_bill(FILE *fout, struct s_bill *rp)
468 {
469     put_index(fout, &rp->fl);
470     put_index(fout, &rp->mi);
471     put_float(fout, &rp->t);
472     put_float(fout, &rp->d);
473     put_array(fout,  rp->w,  3);
474     put_array(fout,  rp->h,  3);
475     put_array(fout,  rp->rx, 3);
476     put_array(fout,  rp->ry, 3);
477     put_array(fout,  rp->rz, 3);
478 }
479
480 static void sol_stor_jump(FILE *fout, struct s_jump *jp)
481 {
482     put_array(fout,  jp->p, 3);
483     put_array(fout,  jp->q, 3);
484     put_float(fout, &jp->r);
485 }
486
487 static void sol_stor_ball(FILE *fout, struct s_ball *bp)
488 {
489     put_array(fout,  bp->e[0], 3);
490     put_array(fout,  bp->e[1], 3);
491     put_array(fout,  bp->e[2], 3);
492     put_array(fout,  bp->p,    3);
493     put_float(fout, &bp->r);
494 }
495
496 static void sol_stor_view(FILE *fout, struct s_view *wp)
497 {
498     put_array(fout,  wp->p, 3);
499     put_array(fout,  wp->q, 3);
500 }
501
502 static void sol_stor_file(FILE *fin, struct s_file *fp)
503 {
504     int i;
505     int magic   = MAGIC;
506     int version = SOL_VERSION;
507
508     put_index(fin, &magic);
509     put_index(fin, &version);
510     
511     put_index(fin, &fp->mc);
512     put_index(fin, &fp->vc);
513     put_index(fin, &fp->ec);
514     put_index(fin, &fp->sc);
515     put_index(fin, &fp->tc);
516     put_index(fin, &fp->gc);
517     put_index(fin, &fp->lc);
518     put_index(fin, &fp->nc);
519     put_index(fin, &fp->pc);
520     put_index(fin, &fp->bc);
521     put_index(fin, &fp->cc);
522     put_index(fin, &fp->zc);
523     put_index(fin, &fp->jc);
524     put_index(fin, &fp->xc);
525     put_index(fin, &fp->rc);
526     put_index(fin, &fp->uc);
527     put_index(fin, &fp->wc);
528     put_index(fin, &fp->ic);
529     put_index(fin, &fp->ac);
530
531     for (i = 0; i < fp->mc; i++) sol_stor_mtrl(fin, fp->mv + i);
532     for (i = 0; i < fp->vc; i++) sol_stor_vert(fin, fp->vv + i);
533     for (i = 0; i < fp->ec; i++) sol_stor_edge(fin, fp->ev + i);
534     for (i = 0; i < fp->sc; i++) sol_stor_side(fin, fp->sv + i);
535     for (i = 0; i < fp->tc; i++) sol_stor_texc(fin, fp->tv + i);
536     for (i = 0; i < fp->gc; i++) sol_stor_geom(fin, fp->gv + i);
537     for (i = 0; i < fp->lc; i++) sol_stor_lump(fin, fp->lv + i);
538     for (i = 0; i < fp->nc; i++) sol_stor_node(fin, fp->nv + i);
539     for (i = 0; i < fp->pc; i++) sol_stor_path(fin, fp->pv + i);
540     for (i = 0; i < fp->bc; i++) sol_stor_body(fin, fp->bv + i);
541     for (i = 0; i < fp->cc; i++) sol_stor_coin(fin, fp->cv + i);
542     for (i = 0; i < fp->zc; i++) sol_stor_goal(fin, fp->zv + i);
543     for (i = 0; i < fp->jc; i++) sol_stor_jump(fin, fp->jv + i);
544     for (i = 0; i < fp->xc; i++) sol_stor_swch(fin, fp->xv + i);
545     for (i = 0; i < fp->rc; i++) sol_stor_bill(fin, fp->rv + i);
546     for (i = 0; i < fp->uc; i++) sol_stor_ball(fin, fp->uv + i);
547     for (i = 0; i < fp->wc; i++) sol_stor_view(fin, fp->wv + i);
548     for (i = 0; i < fp->ic; i++) put_index(fin, fp->iv + i);
549
550     fwrite(fp->av, 1, fp->ac, fin);
551 }
552
553 /*---------------------------------------------------------------------------*/
554
555 int sol_stor(struct s_file *fp, const char *filename)
556 {
557     FILE *fout;
558
559     if ((fout = fopen(filename, FMODE_WB)))
560     {
561         sol_stor_file(fout, fp);
562         fclose(fout);
563
564         return 1;
565     }
566     return 0;
567 }
568
569 void sol_free(struct s_file *fp)
570 {
571     if (fp->mv) free(fp->mv);
572     if (fp->vv) free(fp->vv);
573     if (fp->ev) free(fp->ev);
574     if (fp->sv) free(fp->sv);
575     if (fp->tv) free(fp->tv);
576     if (fp->gv) free(fp->gv);
577     if (fp->lv) free(fp->lv);
578     if (fp->nv) free(fp->nv);
579     if (fp->pv) free(fp->pv);
580     if (fp->bv) free(fp->bv);
581     if (fp->cv) free(fp->cv);
582     if (fp->zv) free(fp->zv);
583     if (fp->jv) free(fp->jv);
584     if (fp->xv) free(fp->xv);
585     if (fp->rv) free(fp->rv);
586     if (fp->uv) free(fp->uv);
587     if (fp->wv) free(fp->wv);
588     if (fp->av) free(fp->av);
589     if (fp->iv) free(fp->iv);
590
591     memset(fp, 0, sizeof (struct s_file));
592 }
593
594 /*---------------------------------------------------------------------------*/
595 /* Solves (p + v * t) . (p + v * t) == r * r for smallest t.                 */
596
597 static float v_sol(const float p[3], const float v[3], float r)
598 {
599     float a = v_dot(v, v);
600     float b = v_dot(v, p) * 2.0f;
601     float c = v_dot(p, p) - r * r;
602     float d = b * b - 4.0f * a * c;
603
604     if (a == 0.0f) return LARGE;
605     if (d <  0.0f) return LARGE;
606
607     if (d == 0.0f)
608         return -b * 0.5f / a;
609     else
610     {
611         float t0 = 0.5f * (-b - fsqrtf(d)) / a;
612         float t1 = 0.5f * (-b + fsqrtf(d)) / a;
613         float t  = (t0 < t1) ? t0 : t1;
614
615         return (t < 0.0f) ? LARGE : t;
616     }
617 }
618
619 /*---------------------------------------------------------------------------*/
620
621 /*
622  * Compute the  earliest time  and position of  the intersection  of a
623  * sphere and a vertex.
624  *
625  * The sphere has radius R and moves along vector V from point P.  The
626  * vertex moves  along vector  W from point  Q in a  coordinate system
627  * based at O.
628  */
629 static float v_vert(float Q[3],
630                     const float o[3],
631                     const float q[3],
632                     const float w[3],
633                     const float p[3],
634                     const float v[3], float r)
635 {
636     float O[3], P[3], V[3];
637     float t = LARGE;
638
639     v_add(O, o, q);
640     v_sub(P, p, O);
641     v_sub(V, v, w);
642
643     if (v_dot(P, V) < 0.0f)
644     {
645         t = v_sol(P, V, r);
646
647         if (t < LARGE)
648             v_mad(Q, O, w, t);
649     }
650     return t;
651 }
652
653 /*
654  * Compute the  earliest time  and position of  the intersection  of a
655  * sphere and an edge.
656  *
657  * The sphere has radius R and moves along vector V from point P.  The
658  * edge moves along vector W from point Q in a coordinate system based
659  * at O.  The edge extends along the length of vector U.
660  */
661 static float v_edge(float Q[3],
662                     const float o[3],
663                     const float q[3],
664                     const float u[3],
665                     const float w[3],
666                     const float p[3],
667                     const float v[3], float r)
668 {
669     float d[3], e[3];
670     float P[3], V[3];
671     float du, eu, uu, s, t;
672
673     v_sub(d, p, o);
674     v_sub(d, d, q);
675     v_sub(e, v, w);
676
677     du = v_dot(d, u);
678     eu = v_dot(e, u);
679     uu = v_dot(u, u);
680
681     v_mad(P, d, u, -du / uu);
682     v_mad(V, e, u, -eu / uu);
683
684     t = v_sol(P, V, r);
685     s = (du + eu * t) / uu;
686
687     if (0.0f < t && t < LARGE && 0.0f < s && s < 1.0f)
688     {
689         v_mad(d, o, w, t);
690         v_mad(e, q, u, s);
691         v_add(Q, e, d);
692     }
693     else
694         t = LARGE;
695
696     return t;
697 }
698
699 /*
700  * Compute  the earlist  time and  position of  the intersection  of a
701  * sphere and a plane.
702  *
703  * The sphere has radius R and moves along vector V from point P.  The
704  * plane  oves  along  vector  W.   The  plane has  normal  N  and  is
705  * positioned at distance D from the origin O along that normal.
706  */
707 static float v_side(float Q[3],
708                     const float o[3],
709                     const float w[3],
710                     const float n[3], float d,
711                     const float p[3],
712                     const float v[3], float r)
713 {
714     float vn = v_dot(v, n);
715     float wn = v_dot(w, n);
716     float t  = LARGE;
717
718     if (vn - wn <= 0.0f)
719     {
720         float on = v_dot(o, n);
721         float pn = v_dot(p, n);
722
723         float u = (r + d + on - pn) / (vn - wn);
724         float a = (    d + on - pn) / (vn - wn);
725
726         if (0.0f <= u)
727         {
728             t = u;
729
730             v_mad(Q, p, v, +t);
731             v_mad(Q, Q, n, -r);
732         }
733         else if (0.0f <= a)
734         {
735             t = 0;
736
737             v_mad(Q, p, v, +t);
738             v_mad(Q, Q, n, -r);
739         }
740     }
741     return t;
742 }
743
744 /*---------------------------------------------------------------------------*/
745
746 /*
747  * Compute the new  linear and angular velocities of  a bouncing ball.
748  * Q  gives the  position  of the  point  of impact  and  W gives  the
749  * velocity of the object being impacted.
750  */
751 static float sol_bounce(struct s_ball *up,
752                         const float q[3],
753                         const float w[3], float dt)
754 {
755     const float kb = 1.10f;
756     const float ke = 0.70f;
757     const float km = 0.20f;
758
759     float n[3], r[3], d[3], u[3], vn, wn, xn, yn;
760     float *p = up->p;
761     float *v = up->v;
762
763     /* Find the normal of the impact. */
764
765     v_sub(r, p, q);
766     v_sub(d, v, w);
767     v_nrm(n, r);
768
769     /* Find the new angular velocity. */
770
771     v_crs(up->w, d, r);
772     v_scl(up->w, up->w, -1.0f / (up->r * up->r));
773
774     /* Find the new linear velocity. */
775
776     vn = v_dot(v, n);
777     wn = v_dot(w, n);
778     xn = (vn < 0.0f) ? -vn * ke : vn;
779     yn = (wn > 0.0f) ?  wn * kb : wn;
780
781     v_mad(u, w, n, -wn);
782     v_mad(v, v, n, -vn);
783     v_mad(v, v, u, +km * dt);
784     v_mad(v, v, n, xn + yn); 
785
786     v_mad(p, q, n, up->r);
787
788     /* Return the "energy" of the impact, to determine the sound amplitude. */
789
790     return fabsf(v_dot(n, d));
791 }
792
793 /*---------------------------------------------------------------------------*/
794
795 /*
796  * Compute the states of all switches after DT seconds have passed.
797  */
798 static void sol_swch_step(struct s_file *fp, float dt)
799 {
800     int xi;
801
802     for (xi = 0; xi < fp->xc; xi++)
803     {
804         struct s_swch *xp = fp->xv + xi;
805
806         if (xp->t > 0)
807         {
808             xp->t -= dt;
809
810             if (xp->t <= 0)
811             {
812                 int pi = xp->pi;
813                 int pj = xp->pi;
814
815                 do  /* Tortoise and hare cycle traverser. */
816                 {
817                     fp->pv[pi].f = xp->f0;
818                     fp->pv[pj].f = xp->f0;
819
820                     pi = fp->pv[pi].pi;
821                     pj = fp->pv[pj].pi;
822                     pj = fp->pv[pj].pi;
823                 }
824                 while (pi != pj);
825
826                 xp->f = xp->f0;
827             }
828         }
829     }
830 }
831
832 /*
833  * Compute the positions of all bodies after DT seconds have passed.
834  */
835 static void sol_body_step(struct s_file *fp, float dt)
836 {
837     int i;
838
839     for (i = 0; i < fp->bc; i++)
840     {
841         struct s_body *bp = fp->bv + i;
842         struct s_path *pp = fp->pv + bp->pi;
843
844         if (bp->pi >= 0 && pp->f)
845         {
846             bp->t += dt;
847
848             if (bp->t >= pp->t)
849             {
850                 bp->t -= pp->t;
851                 bp->pi = pp->pi;
852             }
853         }
854     }
855 }
856
857 /*
858  * Compute the positions of all balls after DT seconds have passed.
859  */
860 static void sol_ball_step(struct s_file *fp, float dt)
861 {
862     int i;
863
864     for (i = 0; i < fp->uc; i++)
865     {
866         struct s_ball *up = fp->uv + i;
867
868         v_mad(up->p, up->p, up->v, dt);
869
870         if (v_len(up->w) > 0.0f)
871         {
872             float M[16];
873             float w[3];
874             float e[3][3];
875
876             v_nrm(w, up->w);
877             m_rot(M, w, v_len(up->w) * dt);
878
879             m_vxfm(e[0], M, up->e[0]);
880             m_vxfm(e[1], M, up->e[1]);
881             m_vxfm(e[2], M, up->e[2]);
882
883             v_crs(up->e[2], e[0], e[1]);
884             v_crs(up->e[1], e[2], e[0]);
885             v_crs(up->e[0], e[1], e[2]);
886
887             v_nrm(up->e[0], up->e[0]);
888             v_nrm(up->e[1], up->e[1]);
889             v_nrm(up->e[2], up->e[2]);
890         }
891     }
892 }
893
894 /*---------------------------------------------------------------------------*/
895
896 static float sol_test_vert(float dt,
897                            float T[3],
898                            const struct s_ball *up,
899                            const struct s_vert *vp,
900                            const float o[3],
901                            const float w[3])
902 {
903     return v_vert(T, o, vp->p, w, up->p, up->v, up->r);
904 }
905
906 static float sol_test_edge(float dt,
907                            float T[3],
908                            const struct s_ball *up,
909                            const struct s_file *fp,
910                            const struct s_edge *ep,
911                            const float o[3],
912                            const float w[3])
913 {
914     float q[3];
915     float u[3];
916
917     v_cpy(q, fp->vv[ep->vi].p);
918     v_sub(u, fp->vv[ep->vj].p,
919           fp->vv[ep->vi].p);
920
921     return v_edge(T, o, q, u, w, up->p, up->v, up->r);
922 }
923
924 static float sol_test_side(float dt,
925                            float T[3],
926                            const struct s_ball *up,
927                            const struct s_file *fp,
928                            const struct s_lump *lp,
929                            const struct s_side *sp,
930                            const float o[3],
931                            const float w[3])
932 {
933     float t = v_side(T, o, w, sp->n, sp->d, up->p, up->v, up->r);
934     int i;
935
936     if (t < dt)
937         for (i = 0; i < lp->sc; i++)
938         {
939             const struct s_side *sq = fp->sv + fp->iv[lp->s0 + i];
940
941             if (sp != sq &&
942                 v_dot(T, sq->n) -
943                 v_dot(o, sq->n) -
944                 v_dot(w, sq->n) * t > sq->d)
945                 return LARGE;
946         }
947     return t;
948 }
949
950 /*---------------------------------------------------------------------------*/
951
952 static float sol_test_fore(float dt,
953                            const struct s_ball *up,
954                            const struct s_side *sp,
955                            const float o[3],
956                            const float w[3])
957 {
958     float q[3];
959
960     /* If the ball is not behind the plane, the test passes. */
961
962     v_sub(q, up->p, o);
963
964     if (v_dot(q, sp->n) - sp->d + up->r >= 0)
965         return 1;
966
967     /* if the ball is behind the plane but will hit before dt, test passes. */
968     /*
969       if (v_side(q, o, w, sp->n, sp->d, up->p, up->v, up->r) < dt)
970       return 1;
971     */
972     /* If the ball is behind but moving toward the plane, test passes. */
973
974     if (v_dot(up->v, sp->n) > 0)
975         return 1;
976
977
978     /* Else, test fails. */
979
980     return 0;
981 }
982
983 static float sol_test_back(float dt,
984                            const struct s_ball *up,
985                            const struct s_side *sp,
986                            const float o[3],
987                            const float w[3])
988 {
989     float q[3];
990
991     /* If the ball is not in front of the plane, the test passes. */
992
993     v_sub(q, up->p, o);
994
995     if (v_dot(q, sp->n) - sp->d - up->r <= 0)
996         return 1;
997
998     /* if the ball is behind the plane but will hit before dt, test passes. */
999     /*
1000       if (v_side(q, o, w, sp->n, sp->d, up->p, up->v, up->r) < dt)
1001       return 1;
1002     */
1003     /* If the ball is in front but moving toward the plane, test passes. */
1004
1005     if (v_dot(up->v, sp->n) < 0)
1006         return 1;
1007
1008
1009     /* Else, test fails. */
1010
1011     return 0;
1012 }
1013
1014 /*---------------------------------------------------------------------------*/
1015
1016 static float sol_test_lump(float dt,
1017                            float T[3],
1018                            const struct s_ball *up,
1019                            const struct s_file *fp,
1020                            const struct s_lump *lp,
1021                            const float o[3],
1022                            const float w[3])
1023 {
1024     float U[3], u, t = dt;
1025     int i;
1026
1027     /* Short circuit a non-solid lump. */
1028
1029     if (lp->fl & L_DETAIL) return t;
1030
1031     /* Test all verts */
1032
1033     if (up->r > 0.0f)
1034         for (i = 0; i < lp->vc; i++)
1035         {
1036             const struct s_vert *vp = fp->vv + fp->iv[lp->v0 + i];
1037
1038             if ((u = sol_test_vert(t, U, up, vp, o, w)) < t)
1039             {
1040                 v_cpy(T, U);
1041                 t = u;
1042             }
1043         }
1044  
1045     /* Test all edges */
1046
1047     if (up->r > 0.0f)
1048         for (i = 0; i < lp->ec; i++)
1049         {
1050             const struct s_edge *ep = fp->ev + fp->iv[lp->e0 + i];
1051
1052             if ((u = sol_test_edge(t, U, up, fp, ep, o, w)) < t)
1053             {
1054                 v_cpy(T, U);
1055                 t = u;
1056             }
1057         }
1058
1059     /* Test all sides */
1060
1061     for (i = 0; i < lp->sc; i++)
1062     {
1063         const struct s_side *sp = fp->sv + fp->iv[lp->s0 + i];
1064
1065         if ((u = sol_test_side(t, U, up, fp, lp, sp, o, w)) < t)
1066         {
1067             v_cpy(T, U);
1068             t = u;
1069         }
1070     }
1071     return t;
1072 }
1073
1074 static float sol_test_node(float dt,
1075                            float T[3],
1076                            const struct s_ball *up,
1077                            const struct s_file *fp,
1078                            const struct s_node *np,
1079                            const float o[3],
1080                            const float w[3])
1081 {
1082     float U[3], u, t = dt;
1083     int i;
1084
1085     /* Test all lumps */
1086
1087     for (i = 0; i < np->lc; i++)
1088     {
1089         const struct s_lump *lp = fp->lv + np->l0 + i;
1090
1091         if ((u = sol_test_lump(t, U, up, fp, lp, o, w)) < t)
1092         {
1093             v_cpy(T, U);
1094             t = u;
1095         }
1096     }
1097
1098     /* Test in front of this node */
1099
1100     if (np->ni >= 0 && sol_test_fore(t, up, fp->sv + np->si, o, w))
1101     {
1102         const struct s_node *nq = fp->nv + np->ni;
1103
1104         if ((u = sol_test_node(t, U, up, fp, nq, o, w)) < t)
1105         {
1106             v_cpy(T, U);
1107             t = u;
1108         }
1109     }
1110
1111     /* Test behind this node */
1112
1113     if (np->nj >= 0 && sol_test_back(t, up, fp->sv + np->si, o, w))
1114     {
1115         const struct s_node *nq = fp->nv + np->nj;
1116
1117         if ((u = sol_test_node(t, U, up, fp, nq, o, w)) < t)
1118         {
1119             v_cpy(T, U);
1120             t = u;
1121         }
1122     }
1123
1124     return t;
1125 }
1126
1127 static float sol_test_body(float dt,
1128                            float T[3], float V[3],
1129                            const struct s_ball *up,
1130                            const struct s_file *fp,
1131                            const struct s_body *bp)
1132 {
1133     float U[3], O[3], W[3], u, t = dt;
1134
1135     const struct s_node *np = fp->nv + bp->ni;
1136
1137     sol_body_p(O, fp, bp);
1138     sol_body_v(W, fp, bp);
1139
1140     if ((u = sol_test_node(t, U, up, fp, np, O, W)) < t)
1141     {
1142         v_cpy(T, U);
1143         v_cpy(V, W);
1144         t = u;
1145     }
1146     return t;
1147 }
1148
1149 static float sol_test_file(float dt,
1150                            float T[3], float V[3],
1151                            const struct s_ball *up,
1152                            const struct s_file *fp)
1153 {
1154     float U[3], W[3], u, t = dt;
1155     int i;
1156
1157     for (i = 0; i < fp->bc; i++)
1158     {
1159         const struct s_body *bp = fp->bv + i;
1160
1161         if ((u = sol_test_body(t, U, W, up, fp, bp)) < t)
1162         {
1163             v_cpy(T, U);
1164             v_cpy(V, W);
1165             t = u;
1166         }
1167     }
1168     return t;
1169 }
1170
1171 /*---------------------------------------------------------------------------*/
1172
1173 /*
1174  * Step the physics forward DT  seconds under the influence of gravity
1175  * vector G.  If the ball gets pinched between two moving solids, this
1176  * loop might not terminate.  It  is better to do something physically
1177  * impossible than  to lock up the game.   So, if we make  more than C
1178  * iterations, punt it.
1179  */
1180
1181 float sol_step(struct s_file *fp, const float *g, float dt, int ui, int *m)
1182 {
1183     float P[3], V[3], v[3], r[3], d, e, nt, b = 0.0f, tt = dt;
1184     int c = 16;
1185
1186     if (ui < fp->uc)
1187     {
1188         struct s_ball *up = fp->uv + ui;
1189
1190         /* If the ball is in contact with a surface, apply friction. */
1191
1192         v_cpy(v, up->v);
1193         v_cpy(up->v, g);
1194
1195         if (m && sol_test_file(tt, P, V, up, fp) < 0.0005f)
1196         {
1197             v_cpy(up->v, v);
1198             v_sub(r, P, up->p);
1199
1200             if ((d = v_dot(r, g) / (v_len(r) * v_len(g))) > 0.999f)
1201             {
1202                 if ((e = (v_len(up->v) - dt)) > 0.0f)
1203                 {
1204                     /* Scale the linear velocity. */
1205
1206                     v_nrm(up->v, up->v);
1207                     v_scl(up->v, up->v, e);
1208
1209                     /* Scale the angular velocity. */
1210
1211                     v_sub(v, V, up->v);
1212                     v_crs(up->w, v, r);
1213                     v_scl(up->w, up->w, -1.0f / (up->r * up->r));
1214                 }
1215                 else
1216                 {
1217                     /* Friction has brought the ball to a stop. */
1218
1219                     up->v[0] = 0.0f;
1220                     up->v[1] = 0.0f;
1221                     up->v[2] = 0.0f;
1222
1223                     (*m)++;
1224                 }
1225             }
1226             else v_mad(up->v, v, g, tt);
1227         }
1228         else v_mad(up->v, v, g, tt);
1229
1230         /* Test for collision. */
1231
1232         while (c > 0 && tt > 0 && tt > (nt = sol_test_file(tt, P, V, up, fp)))
1233         {
1234             sol_body_step(fp, nt);
1235             sol_swch_step(fp, nt);
1236             sol_ball_step(fp, nt);
1237
1238             tt -= nt;
1239
1240             if (b < (d = sol_bounce(up, P, V, nt)))
1241                 b = d;
1242
1243             c--;
1244         }
1245
1246         sol_body_step(fp, tt);
1247         sol_swch_step(fp, tt);
1248         sol_ball_step(fp, tt);
1249     }
1250     return b;
1251 }
1252
1253 /*---------------------------------------------------------------------------*/
1254
1255 int sol_coin_test(struct s_file *fp, float *p, float coin_r)
1256 {
1257     const float *ball_p = fp->uv->p;
1258     const float  ball_r = fp->uv->r;
1259     int ci, n;
1260
1261     for (ci = 0; ci < fp->cc; ci++)
1262     {
1263         float r[3];
1264
1265         r[0] = ball_p[0] - fp->cv[ci].p[0];
1266         r[1] = ball_p[1] - fp->cv[ci].p[1];
1267         r[2] = ball_p[2] - fp->cv[ci].p[2];
1268
1269         if (fp->cv[ci].n > 0 && v_len(r) < ball_r + coin_r)
1270         {
1271             p[0] = fp->cv[ci].p[0];
1272             p[1] = fp->cv[ci].p[1];
1273             p[2] = fp->cv[ci].p[2];
1274
1275             n = fp->cv[ci].n;
1276             fp->cv[ci].n = 0;
1277
1278             return n;
1279         }
1280     }
1281     return 0;
1282 }
1283
1284 struct s_goal *sol_goal_test(struct s_file *fp, float *p, int ui)
1285 {
1286     const float *ball_p = fp->uv[ui].p;
1287     const float  ball_r = fp->uv[ui].r;
1288     int zi;
1289
1290     for (zi = 0; zi < fp->zc; zi++)
1291     {
1292         float r[3];
1293
1294         r[0] = ball_p[0] - fp->zv[zi].p[0];
1295         r[1] = ball_p[2] - fp->zv[zi].p[2];
1296         r[2] = 0;
1297
1298         if (v_len(r) < fp->zv[zi].r * 1.1 - ball_r &&
1299             ball_p[1] > fp->zv[zi].p[1] &&
1300             ball_p[1] < fp->zv[zi].p[1] + GOAL_HEIGHT / 2)
1301         {
1302             p[0] = fp->zv[zi].p[0];
1303             p[1] = fp->zv[zi].p[1];
1304             p[2] = fp->zv[zi].p[2];
1305
1306             return &fp->zv[zi];
1307         }
1308     }
1309     return NULL;
1310 }
1311
1312 int sol_jump_test(struct s_file *fp, float *p, int ui)
1313 /* Test if the ball ui in inside a jump */
1314 /* Return 1 if yes and fill p with the destination position */
1315 /* Return 0 if no */
1316 /* Return 2 if the ball is on the border of a jump */
1317 {
1318     const float *ball_p = fp->uv[ui].p;
1319     const float  ball_r = fp->uv[ui].r;
1320     int ji;
1321     float l;
1322     int res = 0;
1323
1324     for (ji = 0; ji < fp->jc; ji++)
1325     {
1326         float r[3];
1327
1328         r[0] = ball_p[0] - fp->jv[ji].p[0];
1329         r[1] = ball_p[2] - fp->jv[ji].p[2];
1330         r[2] = 0;
1331
1332         l = v_len(r) - fp->jv[ji].r;
1333         if (l < 0 &&
1334             ball_p[1] > fp->jv[ji].p[1] &&
1335             ball_p[1] < fp->jv[ji].p[1] + JUMP_HEIGHT / 2)
1336         {
1337             if (l < - ball_r )
1338             {
1339                 p[0] = fp->jv[ji].q[0] + (ball_p[0] - fp->jv[ji].p[0]);
1340                 p[1] = fp->jv[ji].q[1] + (ball_p[1] - fp->jv[ji].p[1]);
1341                 p[2] = fp->jv[ji].q[2] + (ball_p[2] - fp->jv[ji].p[2]);
1342
1343                 return 1;
1344             }
1345             else
1346                 res = 2;
1347         }
1348     }
1349     return res;
1350 }
1351
1352 int sol_swch_test(struct s_file *fp, int ui)
1353 /* In the sol fp, test and process the event the ball ui enters a switch
1354  * return 1 if a visibla switch is activated 
1355  * return 0 else (no switch is activated or only invisible switchs) */
1356 {
1357     const float *ball_p = fp->uv[ui].p;
1358     const float  ball_r = fp->uv[ui].r;
1359     int xi;
1360     float l;
1361     int res = 0; /* result */
1362
1363     for (xi = 0; xi < fp->xc; xi++)
1364     {
1365         struct s_swch *xp = fp->xv + xi;
1366
1367         if (xp->t0 == 0 || xp->f == xp->f0)
1368         {
1369             float r[3];
1370
1371             r[0] = ball_p[0] - xp->p[0];
1372             r[1] = ball_p[2] - xp->p[2];
1373             r[2] = 0;
1374
1375             l = v_len(r) - xp->r;
1376             if (l < ball_r &&
1377                 ball_p[1] > xp->p[1] &&
1378                 ball_p[1] < xp->p[1] + SWCH_HEIGHT / 2)
1379             {
1380                 if (!xp->e && l < - ball_r)
1381                 {
1382                     int pi = xp->pi;
1383                     int pj = xp->pi;
1384
1385                     /* The ball enter */
1386                     if (xp->t0 == 0)
1387                         xp->e = 1;
1388                     
1389                     /* Toggle the state, update the path. */
1390
1391                     xp->f = xp->f ? 0 : 1;
1392
1393                     do  /* Tortoise and hare cycle traverser. */
1394                     {
1395                         fp->pv[pi].f = xp->f;
1396                         fp->pv[pj].f = xp->f;
1397
1398                         pi = fp->pv[pi].pi;
1399                         pj = fp->pv[pj].pi;
1400                         pj = fp->pv[pj].pi;
1401                     }
1402                     while (pi != pj);
1403
1404                     /* It toggled to non-default state, start the timer. */
1405
1406                     if (xp->f != xp->f0)
1407                         xp->t  = xp->t0;
1408
1409                     /* If visible, set the result */
1410                     if (!xp->i)
1411                         res = 1;
1412                 }
1413             }
1414             else if (xp->e)
1415                 /* A ball go out */
1416                 xp->e = 0;
1417         }
1418     }
1419     return res;
1420 }
1421
1422 /*---------------------------------------------------------------------------*/
1423
1424 void put_file_state(FILE *fout, struct s_file *fp)
1425 {
1426     /* Write the position and orientation of the ball. */
1427
1428     put_array(fout, fp->uv[0].p,    3);
1429     put_array(fout, fp->uv[0].e[0], 3);
1430     put_array(fout, fp->uv[0].e[1], 3);
1431 }
1432
1433 void get_file_state(FILE *fin, struct s_file *fp)
1434 {
1435     /* Read the position and orientation of the ball. */
1436
1437     get_array(fin, fp->uv[0].p,    3);
1438     get_array(fin, fp->uv[0].e[0], 3);
1439     get_array(fin, fp->uv[0].e[1], 3);
1440
1441     /* Compute the 3rd vector of the ball orientatian basis. */
1442
1443     v_crs(fp->uv[0].e[2], fp->uv[0].e[0], fp->uv[0].e[1]);
1444 }
1445
1446 /*---------------------------------------------------------------------------*/
1447