api
[vg.git] / vg_m.h
1 /* Copyright (C) 2021-2022 Harry Godden (hgn) - All Rights Reserved */
2
3 #ifndef VG_M_H
4 #define VG_M_H
5
6 #include "vg_platform.h"
7 #include <math.h>
8 #include <stdlib.h>
9
10 #define VG_PIf 3.14159265358979323846264338327950288f
11 #define VG_TAUf 6.28318530717958647692528676655900576f
12
13 static u32 vg_ftu32( float a )
14 {
15 u32 *ptr = (u32 *)(&a);
16 return *ptr;
17 }
18
19 static int vg_isinff( float a )
20 {
21 return ((vg_ftu32(a)) & 0x7FFFFFFFU) == 0x7F800000U;
22 }
23
24 static int vg_isnanf( float a )
25 {
26 return !vg_isinff(a) && ((vg_ftu32(a)) & 0x7F800000U) == 0x7F800000U;
27 }
28
29 static int vg_validf( float a )
30 {
31 return ((vg_ftu32(a)) & 0x7F800000U) != 0x7F800000U;
32 }
33
34 static inline float vg_minf( float a, float b )
35 {
36 return a < b? a: b;
37 }
38
39 static inline float vg_maxf( float a, float b )
40 {
41 return a > b? a: b;
42 }
43
44 static inline float vg_clampf( float a, float min, float max )
45 {
46 return vg_minf( max, vg_maxf( a, min ) );
47 }
48
49 static inline float vg_signf( float a )
50 {
51 return a < 0.0f? -1.0f: 1.0f;
52 }
53
54 static inline float vg_fractf( float a )
55 {
56 return a - floorf( a );
57 }
58
59
60 __attribute__ ((deprecated))
61 static float stable_force( float current, float diff )
62 {
63 float fnew = current + diff;
64
65 if( fnew * current < 0.0f )
66 return 0.0f;
67
68 return fnew;
69 }
70
71 static float vg_cfrictf( float current, float F )
72 {
73 return -vg_signf(current) * vg_minf( F, fabsf(current) );
74 }
75
76 static inline int vg_min( int a, int b )
77 {
78 return a < b? a: b;
79 }
80
81 static inline int vg_max( int a, int b )
82 {
83 return a > b? a: b;
84 }
85
86 static inline float vg_rad( float deg )
87 {
88 return deg * VG_PIf / 180.0f;
89 }
90
91 /*
92 * Vector 3
93 */
94 static inline void v2_copy( v2f a, v2f b )
95 {
96 b[0] = a[0]; b[1] = a[1];
97 }
98
99 static inline void v2_zero( v2f a )
100 {
101 a[0] = 0.f; a[1] = 0.f;
102 }
103
104 static inline void v2i_copy( v2i a, v2i b )
105 {
106 b[0] = a[0]; b[1] = a[1];
107 }
108
109 static inline int v2i_eq( v2i a, v2i b )
110 {
111 return ((a[0] == b[0]) && (a[1] == b[1]));
112 }
113
114 static inline void v2i_add( v2i a, v2i b, v2i d )
115 {
116 d[0] = a[0]+b[0]; d[1] = a[1]+b[1];
117 }
118
119 static inline void v2i_sub( v2i a, v2i b, v2i d )
120 {
121 d[0] = a[0]-b[0]; d[1] = a[1]-b[1];
122 }
123
124 static inline void v2_minv( v2f a, v2f b, v2f dest )
125 {
126 dest[0] = vg_minf(a[0], b[0]);
127 dest[1] = vg_minf(a[1], b[1]);
128 }
129
130 static inline void v2_maxv( v2f a, v2f b, v2f dest )
131 {
132 dest[0] = vg_maxf(a[0], b[0]);
133 dest[1] = vg_maxf(a[1], b[1]);
134 }
135
136 static inline void v2_sub( v2f a, v2f b, v2f d )
137 {
138 d[0] = a[0]-b[0]; d[1] = a[1]-b[1];
139 }
140
141 static inline float v2_dot( v2f a, v2f b )
142 {
143 return a[0] * b[0] + a[1] * b[1];
144 }
145
146 static inline float v2_cross( v2f a, v2f b )
147 {
148 return a[0]*b[1] - a[1]*b[0];
149 }
150
151 static inline void v2_add( v2f a, v2f b, v2f d )
152 {
153 d[0] = a[0]+b[0]; d[1] = a[1]+b[1];
154 }
155
156 static inline void v2_abs( v2f a, v2f d )
157 {
158 d[0] = fabsf( a[0] );
159 d[1] = fabsf( a[1] );
160 }
161
162 static inline void v2_muls( v2f a, float s, v2f d )
163 {
164 d[0] = a[0]*s; d[1] = a[1]*s;
165 }
166
167 static inline void v2_divs( v2f a, float s, v2f d )
168 {
169 d[0] = a[0]/s; d[1] = a[1]/s;
170 }
171
172 static inline void v2_mul( v2f a, v2f b, v2f d )
173 {
174 d[0] = a[0]*b[0];
175 d[1] = a[1]*b[1];
176 }
177
178 static inline void v2_div( v2f a, v2f b, v2f d )
179 {
180 d[0] = a[0]/b[0]; d[1] = a[1]/b[1];
181 }
182
183 static inline void v2_muladd( v2f a, v2f b, v2f s, v2f d )
184 {
185 d[0] = a[0]+b[0]*s[0];
186 d[1] = a[1]+b[1]*s[1];
187 }
188
189 static inline void v2_muladds( v2f a, v2f b, float s, v2f d )
190 {
191 d[0] = a[0]+b[0]*s;
192 d[1] = a[1]+b[1]*s;
193 }
194
195 static inline float v2_length2( v2f a )
196 {
197 return a[0]*a[0] + a[1]*a[1];
198 }
199
200 static inline float v2_length( v2f a )
201 {
202 return sqrtf( v2_length2( a ) );
203 }
204
205 static inline float v2_dist2( v2f a, v2f b )
206 {
207 v2f delta;
208 v2_sub( a, b, delta );
209 return v2_length2( delta );
210 }
211
212 static inline float v2_dist( v2f a, v2f b )
213 {
214 return sqrtf( v2_dist2( a, b ) );
215 }
216
217 static inline void v2_lerp( v2f a, v2f b, float t, v2f d )
218 {
219 d[0] = a[0] + t*(b[0]-a[0]);
220 d[1] = a[1] + t*(b[1]-a[1]);
221 }
222
223 static inline void v2_normalize( v2f a )
224 {
225 v2_muls( a, 1.0f / v2_length( a ), a );
226 }
227
228 static void v2_normalize_clamp( v2f a )
229 {
230 float l2 = v2_length2( a );
231 if( l2 > 1.0f )
232 v2_muls( a, 1.0f/sqrtf(l2), a );
233 }
234
235 static inline void v2_floor( v2f a, v2f b )
236 {
237 b[0] = floorf( a[0] );
238 b[1] = floorf( a[1] );
239 }
240
241 static inline void v2_fill( v2f a, float v )
242 {
243 a[0] = v;
244 a[1] = v;
245 }
246
247 /* copysign of b to a */
248 static inline void v2_copysign( v2f a, v2f b )
249 {
250 a[0] = copysignf( a[0], b[0] );
251 a[1] = copysignf( a[1], b[1] );
252 }
253
254 /*
255 * Vector 3
256 */
257 static inline void v3_zero( v3f a )
258 {
259 a[0] = 0.f; a[1] = 0.f; a[2] = 0.f;
260 }
261
262 static inline void v3_copy( v3f a, v3f b )
263 {
264 b[0] = a[0]; b[1] = a[1]; b[2] = a[2];
265 }
266
267 static inline void v3_add( v3f a, v3f b, v3f d )
268 {
269 d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2];
270 }
271
272 static inline void v3i_add( v3i a, v3i b, v3i d )
273 {
274 d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2];
275 }
276
277 static inline void v4_add( v4f a, v4f b, v4f d )
278 {
279 d[0] = a[0]+b[0];
280 d[1] = a[1]+b[1];
281 d[2] = a[2]+b[2];
282 d[3] = a[3]+b[3];
283 }
284
285 static inline void v3_sub( v3f a, v3f b, v3f d )
286 {
287 d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2];
288 }
289
290 static inline void v3i_sub( v3i a, v3i b, v3i d )
291 {
292 d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2];
293 }
294
295 static inline void v3_mul( v3f a, v3f b, v3f d )
296 {
297 d[0] = a[0]*b[0]; d[1] = a[1]*b[1]; d[2] = a[2]*b[2];
298 }
299
300 static inline void v3_div( v3f a, v3f b, v3f d )
301 {
302 d[0] = b[0]!=0.0f? a[0]/b[0]: INFINITY;
303 d[1] = b[1]!=0.0f? a[1]/b[1]: INFINITY;
304 d[2] = b[2]!=0.0f? a[2]/b[2]: INFINITY;
305 }
306
307 static inline void v3_muls( v3f a, float s, v3f d )
308 {
309 d[0] = a[0]*s; d[1] = a[1]*s; d[2] = a[2]*s;
310 }
311
312 static inline void v3_fill( v3f a, float v )
313 {
314 a[0] = v;
315 a[1] = v;
316 a[2] = v;
317 }
318
319 static inline void v3_divs( v3f a, float s, v3f d )
320 {
321 if( s == 0.0f )
322 v3_fill( d, INFINITY );
323 else
324 {
325 d[0] = a[0]/s;
326 d[1] = a[1]/s;
327 d[2] = a[2]/s;
328 }
329 }
330
331 static inline void v3_muladds( v3f a, v3f b, float s, v3f d )
332 {
333 d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s; d[2] = a[2]+b[2]*s;
334 }
335
336 static inline void v3_muladd( v2f a, v2f b, v2f s, v2f d )
337 {
338 d[0] = a[0]+b[0]*s[0];
339 d[1] = a[1]+b[1]*s[1];
340 d[2] = a[2]+b[2]*s[2];
341 }
342
343 static inline float v3_dot( v3f a, v3f b )
344 {
345 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
346 }
347
348 static inline void v3_cross( v3f a, v3f b, v3f dest )
349 {
350 v3f d;
351 d[0] = a[1]*b[2] - a[2]*b[1];
352 d[1] = a[2]*b[0] - a[0]*b[2];
353 d[2] = a[0]*b[1] - a[1]*b[0];
354 v3_copy( d, dest );
355 }
356
357 static inline float v3_length2( v3f a )
358 {
359 return v3_dot( a, a );
360 }
361
362 static inline float v3_length( v3f a )
363 {
364 return sqrtf( v3_length2( a ) );
365 }
366
367 static inline float v3_dist2( v3f a, v3f b )
368 {
369 v3f delta;
370 v3_sub( a, b, delta );
371 return v3_length2( delta );
372 }
373
374 static inline float v3_dist( v3f a, v3f b )
375 {
376 return sqrtf( v3_dist2( a, b ) );
377 }
378
379 static inline void v3_normalize( v3f a )
380 {
381 v3_muls( a, 1.f / v3_length( a ), a );
382 }
383
384 static inline float vg_lerpf( float a, float b, float t )
385 {
386 return a + t*(b-a);
387 }
388
389 static inline double vg_lerp( double a, double b, double t )
390 {
391 return a + t*(b-a);
392 }
393
394 /* correctly lerp around circular period -pi -> pi */
395 static float vg_alerpf( float a, float b, float t )
396 {
397 float d = fmodf( b-a, VG_TAUf ),
398 s = fmodf( 2.0f*d, VG_TAUf ) - d;
399 return a + s*t;
400 }
401
402 static inline void v3_lerp( v3f a, v3f b, float t, v3f d )
403 {
404 d[0] = a[0] + t*(b[0]-a[0]);
405 d[1] = a[1] + t*(b[1]-a[1]);
406 d[2] = a[2] + t*(b[2]-a[2]);
407 }
408
409 static inline void v3_minv( v3f a, v3f b, v3f dest )
410 {
411 dest[0] = vg_minf(a[0], b[0]);
412 dest[1] = vg_minf(a[1], b[1]);
413 dest[2] = vg_minf(a[2], b[2]);
414 }
415
416 static inline void v3_maxv( v3f a, v3f b, v3f dest )
417 {
418 dest[0] = vg_maxf(a[0], b[0]);
419 dest[1] = vg_maxf(a[1], b[1]);
420 dest[2] = vg_maxf(a[2], b[2]);
421 }
422
423 static inline float v3_minf( v3f a )
424 {
425 return vg_minf( vg_minf( a[0], a[1] ), a[2] );
426 }
427
428 static inline float v3_maxf( v3f a )
429 {
430 return vg_maxf( vg_maxf( a[0], a[1] ), a[2] );
431 }
432
433 static inline void v3_floor( v3f a, v3f b )
434 {
435 b[0] = floorf( a[0] );
436 b[1] = floorf( a[1] );
437 b[2] = floorf( a[2] );
438 }
439
440 static inline void v3_ceil( v3f a, v3f b )
441 {
442 b[0] = ceilf( a[0] );
443 b[1] = ceilf( a[1] );
444 b[2] = ceilf( a[2] );
445 }
446
447 static inline void v3_negate( v3f a, v3f b )
448 {
449 b[0] = -a[0];
450 b[1] = -a[1];
451 b[2] = -a[2];
452 }
453
454 static inline void v3_rotate( v3f v, float angle, v3f axis, v3f d )
455 {
456 v3f v1, v2, k;
457 float c, s;
458
459 c = cosf( angle );
460 s = sinf( angle );
461
462 v3_copy( axis, k );
463 v3_normalize( k );
464 v3_muls( v, c, v1 );
465 v3_cross( k, v, v2 );
466 v3_muls( v2, s, v2 );
467 v3_add( v1, v2, v1 );
468 v3_muls( k, v3_dot(k, v) * (1.0f - c), v2);
469 v3_add( v1, v2, d );
470 }
471
472 /*
473 * Vector 4
474 */
475 static inline void v4_copy( v4f a, v4f b )
476 {
477 b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; b[3] = a[3];
478 }
479
480 static inline void v4_zero( v4f a )
481 {
482 a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; a[3] = 0.f;
483 }
484
485 static inline void v4_muls( v4f a, float s, v4f d )
486 {
487 d[0] = a[0]*s;
488 d[1] = a[1]*s;
489 d[2] = a[2]*s;
490 d[3] = a[3]*s;
491 }
492
493 static inline void v4_muladds( v4f a, v4f b, float s, v4f d )
494 {
495 d[0] = a[0]+b[0]*s;
496 d[1] = a[1]+b[1]*s;
497 d[2] = a[2]+b[2]*s;
498 d[3] = a[3]+b[3]*s;
499 }
500
501 static inline void v4_lerp( v4f a, v4f b, float t, v4f d )
502 {
503 d[0] = a[0] + t*(b[0]-a[0]);
504 d[1] = a[1] + t*(b[1]-a[1]);
505 d[2] = a[2] + t*(b[2]-a[2]);
506 d[3] = a[3] + t*(b[3]-a[3]);
507 }
508
509 static inline float v4_dot( v4f a, v4f b )
510 {
511 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
512 }
513
514 static inline float v4_length( v4f a )
515 {
516 return sqrtf( v4_dot(a,a) );
517 }
518
519 /*
520 * Matrix 2x2
521 */
522
523 #define M2X2_INDENTIY {{1.0f, 0.0f, }, \
524 { 0.0f, 1.0f, }}
525
526 #define M2X2_ZERO {{0.0f, 0.0f, }, \
527 { 0.0f, 0.0f, }}
528
529 static inline void m2x2_copy( m2x2f a, m2x2f b )
530 {
531 v2_copy( a[0], b[0] );
532 v2_copy( a[1], b[1] );
533 }
534
535 static inline void m2x2_identity( m2x2f a )
536 {
537 m2x2f id = M2X2_INDENTIY;
538 m2x2_copy( id, a );
539 }
540
541 static inline void m2x2_create_rotation( m2x2f a, float theta )
542 {
543 float s, c;
544
545 s = sinf( theta );
546 c = cosf( theta );
547
548 a[0][0] = c;
549 a[0][1] = -s;
550 a[1][0] = s;
551 a[1][1] = c;
552 }
553
554 /*
555 * Matrix 3x3
556 */
557
558 #define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
559 { 0.0f, 1.0f, 0.0f, },\
560 { 0.0f, 0.0f, 1.0f, }}
561
562 #define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\
563 { 0.0f, 0.0f, 0.0f, },\
564 { 0.0f, 0.0f, 0.0f, }}
565
566
567 /* a X b == [b]T a == ...*/
568 static void m3x3_skew_symetric( m3x3f a, v3f v )
569 {
570 a[0][0] = 0.0f;
571 a[0][1] = v[2];
572 a[0][2] = -v[1];
573 a[1][0] = -v[2];
574 a[1][1] = 0.0f;
575 a[1][2] = v[0];
576 a[2][0] = v[1];
577 a[2][1] = -v[0];
578 a[2][2] = 0.0f;
579 }
580
581 static void m3x3_add( m3x3f a, m3x3f b, m3x3f d )
582 {
583 v3_add( a[0], b[0], d[0] );
584 v3_add( a[1], b[1], d[1] );
585 v3_add( a[2], b[2], d[2] );
586 }
587
588 static inline void m3x3_copy( m3x3f a, m3x3f b )
589 {
590 v3_copy( a[0], b[0] );
591 v3_copy( a[1], b[1] );
592 v3_copy( a[2], b[2] );
593 }
594
595 static inline void m3x3_identity( m3x3f a )
596 {
597 m3x3f id = M3X3_IDENTITY;
598 m3x3_copy( id, a );
599 }
600
601 static void m3x3_diagonal( m3x3f a, float v )
602 {
603 m3x3_identity( a );
604 a[0][0] = v;
605 a[1][1] = v;
606 a[2][2] = v;
607 }
608
609 static void m3x3_setdiagonalv3( m3x3f a, v3f v )
610 {
611 a[0][0] = v[0];
612 a[1][1] = v[1];
613 a[2][2] = v[2];
614 }
615
616 static inline void m3x3_zero( m3x3f a )
617 {
618 m3x3f z = M3X3_ZERO;
619 m3x3_copy( z, a );
620 }
621
622 static inline void m3x3_inv( m3x3f src, m3x3f dest )
623 {
624 float a = src[0][0], b = src[0][1], c = src[0][2],
625 d = src[1][0], e = src[1][1], f = src[1][2],
626 g = src[2][0], h = src[2][1], i = src[2][2];
627
628 float det = 1.f /
629 (+a*(e*i-h*f)
630 -b*(d*i-f*g)
631 +c*(d*h-e*g));
632
633 dest[0][0] = (e*i-h*f)*det;
634 dest[0][1] = -(b*i-c*h)*det;
635 dest[0][2] = (b*f-c*e)*det;
636 dest[1][0] = -(d*i-f*g)*det;
637 dest[1][1] = (a*i-c*g)*det;
638 dest[1][2] = -(a*f-d*c)*det;
639 dest[2][0] = (d*h-g*e)*det;
640 dest[2][1] = -(a*h-g*b)*det;
641 dest[2][2] = (a*e-d*b)*det;
642 }
643
644 static float m3x3_det( m3x3f m )
645 {
646 return m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])
647 - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
648 + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
649 }
650
651 static inline void m3x3_transpose( m3x3f src, m3x3f dest )
652 {
653 float a = src[0][0], b = src[0][1], c = src[0][2],
654 d = src[1][0], e = src[1][1], f = src[1][2],
655 g = src[2][0], h = src[2][1], i = src[2][2];
656
657 dest[0][0] = a;
658 dest[0][1] = d;
659 dest[0][2] = g;
660 dest[1][0] = b;
661 dest[1][1] = e;
662 dest[1][2] = h;
663 dest[2][0] = c;
664 dest[2][1] = f;
665 dest[2][2] = i;
666 }
667
668 static inline void m3x3_mul( m3x3f a, m3x3f b, m3x3f d )
669 {
670 float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2],
671 a10 = a[1][0], a11 = a[1][1], a12 = a[1][2],
672 a20 = a[2][0], a21 = a[2][1], a22 = a[2][2],
673
674 b00 = b[0][0], b01 = b[0][1], b02 = b[0][2],
675 b10 = b[1][0], b11 = b[1][1], b12 = b[1][2],
676 b20 = b[2][0], b21 = b[2][1], b22 = b[2][2];
677
678 d[0][0] = a00*b00 + a10*b01 + a20*b02;
679 d[0][1] = a01*b00 + a11*b01 + a21*b02;
680 d[0][2] = a02*b00 + a12*b01 + a22*b02;
681 d[1][0] = a00*b10 + a10*b11 + a20*b12;
682 d[1][1] = a01*b10 + a11*b11 + a21*b12;
683 d[1][2] = a02*b10 + a12*b11 + a22*b12;
684 d[2][0] = a00*b20 + a10*b21 + a20*b22;
685 d[2][1] = a01*b20 + a11*b21 + a21*b22;
686 d[2][2] = a02*b20 + a12*b21 + a22*b22;
687 }
688
689 static inline void m3x3_mulv( m3x3f m, v3f v, v3f d )
690 {
691 v3f res;
692
693 res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
694 res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
695 res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
696
697 v3_copy( res, d );
698 }
699
700 static inline void m3x3_projection( m3x3f dst,
701 float const left, float const right, float const bottom, float const top )
702 {
703 float rl, tb;
704
705 m3x3_zero( dst );
706
707 rl = 1.0f / (right - left);
708 tb = 1.0f / (top - bottom);
709
710 dst[0][0] = 2.0f * rl;
711 dst[1][1] = 2.0f * tb;
712 dst[2][2] = 1.0f;
713 }
714
715 static inline void m3x3_translate( m3x3f m, v3f v )
716 {
717 m[2][0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0];
718 m[2][1] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1];
719 m[2][2] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2];
720 }
721
722 static inline void m3x3_scale( m3x3f m, v3f v )
723 {
724 v3_muls( m[0], v[0], m[0] );
725 v3_muls( m[1], v[1], m[1] );
726 v3_muls( m[2], v[2], m[2] );
727 }
728
729 static inline void m3x3_scalef( m3x3f m, float f )
730 {
731 v3f v;
732 v3_fill( v, f );
733 m3x3_scale( m, v );
734 }
735
736 static inline void m3x3_rotate( m3x3f m, float angle )
737 {
738 float m00 = m[0][0], m10 = m[1][0],
739 m01 = m[0][1], m11 = m[1][1],
740 m02 = m[0][2], m12 = m[1][2];
741 float c, s;
742
743 s = sinf( angle );
744 c = cosf( angle );
745
746 m[0][0] = m00 * c + m10 * s;
747 m[0][1] = m01 * c + m11 * s;
748 m[0][2] = m02 * c + m12 * s;
749
750 m[1][0] = m00 * -s + m10 * c;
751 m[1][1] = m01 * -s + m11 * c;
752 m[1][2] = m02 * -s + m12 * c;
753 }
754
755 static inline void box_addpt( boxf a, v3f pt )
756 {
757 v3_minv( a[0], pt, a[0] );
758 v3_maxv( a[1], pt, a[1] );
759 }
760
761 static inline void box_concat( boxf a, boxf b )
762 {
763 v3_minv( a[0], b[0], a[0] );
764 v3_maxv( a[1], b[1], a[1] );
765 }
766
767 static inline void box_copy( boxf a, boxf b )
768 {
769 v3_copy( a[0], b[0] );
770 v3_copy( a[1], b[1] );
771 }
772
773 static inline int box_overlap( boxf a, boxf b )
774 {
775 return
776 ( a[0][0] <= b[1][0] && a[1][0] >= b[0][0] ) &&
777 ( a[0][1] <= b[1][1] && a[1][1] >= b[0][1] ) &&
778 ( a[0][2] <= b[1][2] && a[1][2] >= b[0][2] )
779 ;
780 }
781
782 static int box_within( boxf greater, boxf lesser )
783 {
784 v3f a, b;
785 v3_sub( lesser[0], greater[0], a );
786 v3_sub( lesser[1], greater[1], b );
787
788 if( (a[0] >= 0.0f) && (a[1] >= 0.0f) && (a[2] >= 0.0f) &&
789 (b[0] <= 0.0f) && (b[1] <= 0.0f) && (b[2] <= 0.0f) )
790 {
791 return 1;
792 }
793
794 return 0;
795 }
796
797 static inline void box_init_inf( boxf box )
798 {
799 v3_fill( box[0], INFINITY );
800 v3_fill( box[1], -INFINITY );
801 }
802
803 int ray_aabb1( boxf box, v3f co, v3f dir_inv, float dist )
804 {
805 v3f v0, v1;
806 float tmin, tmax;
807
808 v3_sub( box[0], co, v0 );
809 v3_sub( box[1], co, v1 );
810
811 v3_mul( v0, dir_inv, v0 );
812 v3_mul( v1, dir_inv, v1 );
813
814 tmin = vg_minf( v0[0], v1[0] );
815 tmax = vg_maxf( v0[0], v1[0] );
816 tmin = vg_maxf( tmin, vg_minf( v0[1], v1[1] ));
817 tmax = vg_minf( tmax, vg_maxf( v0[1], v1[1] ));
818 tmin = vg_maxf( tmin, vg_minf( v0[2], v1[2] ));
819 tmax = vg_minf( tmax, vg_maxf( v0[2], v1[2] ));
820
821 return (tmax >= tmin) && (tmin <= dist) && (tmax >= 0.0f);
822 }
823
824 static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up )
825 {
826 v3f dir;
827 v3_sub( target, pos, dir );
828 v3_normalize( dir );
829
830 v3_copy( dir, m[2] );
831
832 v3_cross( up, m[2], m[0] );
833 v3_normalize( m[0] );
834
835 v3_cross( m[2], m[0], m[1] );
836 v3_copy( pos, m[3] );
837 }
838
839 /*
840 * Matrix 4x4
841 */
842
843 #define M4X4_IDENTITY {{1.0f, 0.0f, 0.0f, 0.0f },\
844 { 0.0f, 1.0f, 0.0f, 0.0f },\
845 { 0.0f, 0.0f, 1.0f, 0.0f },\
846 { 0.0f, 0.0f, 0.0f, 1.0f }}
847 #define M4X4_ZERO {{0.0f, 0.0f, 0.0f, 0.0f },\
848 { 0.0f, 0.0f, 0.0f, 0.0f },\
849 { 0.0f, 0.0f, 0.0f, 0.0f },\
850 { 0.0f, 0.0f, 0.0f, 0.0f }}
851
852 static void m4x4_projection( m4x4f m, float angle,
853 float ratio, float fnear, float ffar )
854 {
855 float scale = tanf( angle * 0.5f * VG_PIf / 180.0f ) * fnear,
856 r = ratio * scale,
857 l = -r,
858 t = scale,
859 b = -t;
860
861 m[0][0] = 2.0f * fnear / (r - l);
862 m[0][1] = 0.0f;
863 m[0][2] = 0.0f;
864 m[0][3] = 0.0f;
865
866 m[1][0] = 0.0f;
867 m[1][1] = 2.0f * fnear / (t - b);
868 m[1][2] = 0.0f;
869 m[1][3] = 0.0f;
870
871 m[2][0] = (r + l) / (r - l);
872 m[2][1] = (t + b) / (t - b);
873 m[2][2] = -(ffar + fnear) / (ffar - fnear);
874 m[2][3] = -1.0f;
875
876 m[3][0] = 0.0f;
877 m[3][1] = 0.0f;
878 m[3][2] = -2.0f * ffar * fnear / (ffar - fnear);
879 m[3][3] = 0.0f;
880 }
881
882 static void m4x4_translate( m4x4f m, v3f v )
883 {
884 v4_muladds( m[3], m[0], v[0], m[3] );
885 v4_muladds( m[3], m[1], v[1], m[3] );
886 v4_muladds( m[3], m[2], v[2], m[3] );
887 }
888
889 static inline void m4x4_copy( m4x4f a, m4x4f b )
890 {
891 v4_copy( a[0], b[0] );
892 v4_copy( a[1], b[1] );
893 v4_copy( a[2], b[2] );
894 v4_copy( a[3], b[3] );
895 }
896
897 static inline void m4x4_identity( m4x4f a )
898 {
899 m4x4f id = M4X4_IDENTITY;
900 m4x4_copy( id, a );
901 }
902
903 static inline void m4x4_zero( m4x4f a )
904 {
905 m4x4f zero = M4X4_ZERO;
906 m4x4_copy( zero, a );
907 }
908
909 static inline void m4x4_mul( m4x4f a, m4x4f b, m4x4f d )
910 {
911 float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3],
912 a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3],
913 a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3],
914 a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3],
915
916 b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], b03 = b[0][3],
917 b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], b13 = b[1][3],
918 b20 = b[2][0], b21 = b[2][1], b22 = b[2][2], b23 = b[2][3],
919 b30 = b[3][0], b31 = b[3][1], b32 = b[3][2], b33 = b[3][3];
920
921 d[0][0] = a00*b00 + a10*b01 + a20*b02 + a30*b03;
922 d[0][1] = a01*b00 + a11*b01 + a21*b02 + a31*b03;
923 d[0][2] = a02*b00 + a12*b01 + a22*b02 + a32*b03;
924 d[0][3] = a03*b00 + a13*b01 + a23*b02 + a33*b03;
925 d[1][0] = a00*b10 + a10*b11 + a20*b12 + a30*b13;
926 d[1][1] = a01*b10 + a11*b11 + a21*b12 + a31*b13;
927 d[1][2] = a02*b10 + a12*b11 + a22*b12 + a32*b13;
928 d[1][3] = a03*b10 + a13*b11 + a23*b12 + a33*b13;
929 d[2][0] = a00*b20 + a10*b21 + a20*b22 + a30*b23;
930 d[2][1] = a01*b20 + a11*b21 + a21*b22 + a31*b23;
931 d[2][2] = a02*b20 + a12*b21 + a22*b22 + a32*b23;
932 d[2][3] = a03*b20 + a13*b21 + a23*b22 + a33*b23;
933 d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30*b33;
934 d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31*b33;
935 d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32*b33;
936 d[3][3] = a03*b30 + a13*b31 + a23*b32 + a33*b33;
937 }
938
939 static inline void m4x4_mulv( m4x4f m, v4f v, v4f d )
940 {
941 v4f res;
942
943 res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]*v[3];
944 res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]*v[3];
945 res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]*v[3];
946 res[3] = m[0][3]*v[0] + m[1][3]*v[1] + m[2][3]*v[2] + m[3][3]*v[3];
947
948 v4_copy( res, d );
949 }
950
951 static inline void m4x4_inv( m4x4f a, m4x4f d )
952 {
953 float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3],
954 a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3],
955 a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3],
956 a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3],
957 det,
958 t[6];
959
960 t[0] = a22*a33 - a32*a23;
961 t[1] = a21*a33 - a31*a23;
962 t[2] = a21*a32 - a31*a22;
963 t[3] = a20*a33 - a30*a23;
964 t[4] = a20*a32 - a30*a22;
965 t[5] = a20*a31 - a30*a21;
966
967 d[0][0] = a11*t[0] - a12*t[1] + a13*t[2];
968 d[1][0] =-(a10*t[0] - a12*t[3] + a13*t[4]);
969 d[2][0] = a10*t[1] - a11*t[3] + a13*t[5];
970 d[3][0] =-(a10*t[2] - a11*t[4] + a12*t[5]);
971
972 d[0][1] =-(a01*t[0] - a02*t[1] + a03*t[2]);
973 d[1][1] = a00*t[0] - a02*t[3] + a03*t[4];
974 d[2][1] =-(a00*t[1] - a01*t[3] + a03*t[5]);
975 d[3][1] = a00*t[2] - a01*t[4] + a02*t[5];
976
977 t[0] = a12*a33 - a32*a13;
978 t[1] = a11*a33 - a31*a13;
979 t[2] = a11*a32 - a31*a12;
980 t[3] = a10*a33 - a30*a13;
981 t[4] = a10*a32 - a30*a12;
982 t[5] = a10*a31 - a30*a11;
983
984 d[0][2] = a01*t[0] - a02*t[1] + a03*t[2];
985 d[1][2] =-(a00*t[0] - a02*t[3] + a03*t[4]);
986 d[2][2] = a00*t[1] - a01*t[3] + a03*t[5];
987 d[3][2] =-(a00*t[2] - a01*t[4] + a02*t[5]);
988
989 t[0] = a12*a23 - a22*a13;
990 t[1] = a11*a23 - a21*a13;
991 t[2] = a11*a22 - a21*a12;
992 t[3] = a10*a23 - a20*a13;
993 t[4] = a10*a22 - a20*a12;
994 t[5] = a10*a21 - a20*a11;
995
996 d[0][3] =-(a01*t[0] - a02*t[1] + a03*t[2]);
997 d[1][3] = a00*t[0] - a02*t[3] + a03*t[4];
998 d[2][3] =-(a00*t[1] - a01*t[3] + a03*t[5]);
999 d[3][3] = a00*t[2] - a01*t[4] + a02*t[5];
1000
1001 det = 1.0f / (a00*d[0][0] + a01*d[1][0] + a02*d[2][0] + a03*d[3][0]);
1002 v4_muls( d[0], det, d[0] );
1003 v4_muls( d[1], det, d[1] );
1004 v4_muls( d[2], det, d[2] );
1005 v4_muls( d[3], det, d[3] );
1006 }
1007
1008 /*
1009 * Planes (double precision)
1010 */
1011 static inline void tri_to_plane( double a[3], double b[3],
1012 double c[3], double p[4] )
1013 {
1014 double edge0[3];
1015 double edge1[3];
1016 double l;
1017
1018 edge0[0] = b[0] - a[0];
1019 edge0[1] = b[1] - a[1];
1020 edge0[2] = b[2] - a[2];
1021
1022 edge1[0] = c[0] - a[0];
1023 edge1[1] = c[1] - a[1];
1024 edge1[2] = c[2] - a[2];
1025
1026 p[0] = edge0[1] * edge1[2] - edge0[2] * edge1[1];
1027 p[1] = edge0[2] * edge1[0] - edge0[0] * edge1[2];
1028 p[2] = edge0[0] * edge1[1] - edge0[1] * edge1[0];
1029
1030 l = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
1031 p[3] = (p[0] * a[0] + p[1] * a[1] + p[2] * a[2]) / l;
1032
1033 p[0] = p[0] / l;
1034 p[1] = p[1] / l;
1035 p[2] = p[2] / l;
1036 }
1037
1038 static int plane_intersect3( v4f a, v4f b, v4f c, v3f p )
1039 {
1040 float const epsilon = 1e-6f;
1041
1042 v3f x;
1043 v3_cross( a, b, x );
1044 float d = v3_dot( x, c );
1045
1046 if( (d < epsilon) && (d > -epsilon) ) return 0;
1047
1048 v3f v0, v1, v2;
1049 v3_cross( b, c, v0 );
1050 v3_cross( c, a, v1 );
1051 v3_cross( a, b, v2 );
1052
1053 v3_muls( v0, a[3], p );
1054 v3_muladds( p, v1, b[3], p );
1055 v3_muladds( p, v2, c[3], p );
1056 v3_divs( p, d, p );
1057
1058 return 1;
1059 }
1060
1061 int plane_intersect2( v4f a, v4f b, v3f p, v3f n )
1062 {
1063 float const epsilon = 1e-6f;
1064
1065 v4f c;
1066 v3_cross( a, b, c );
1067 float d = v3_length2( c );
1068
1069 if( (d < epsilon) && (d > -epsilon) )
1070 return 0;
1071
1072 v3f v0, v1, vx;
1073 v3_cross( c, b, v0 );
1074 v3_cross( a, c, v1 );
1075
1076 v3_muls( v0, a[3], vx );
1077 v3_muladds( vx, v1, b[3], vx );
1078 v3_divs( vx, d, p );
1079 v3_copy( c, n );
1080
1081 return 1;
1082 }
1083
1084 static int plane_segment( v4f plane, v3f a, v3f b, v3f co )
1085 {
1086 float d0 = v3_dot( a, plane ) - plane[3],
1087 d1 = v3_dot( b, plane ) - plane[3];
1088
1089 if( d0*d1 < 0.0f )
1090 {
1091 float tot = 1.0f/( fabsf(d0)+fabsf(d1) );
1092
1093 v3_muls( a, fabsf(d1) * tot, co );
1094 v3_muladds( co, b, fabsf(d0) * tot, co );
1095 return 1;
1096 }
1097
1098 return 0;
1099 }
1100
1101 static inline double plane_polarity( double p[4], double a[3] )
1102 {
1103 return
1104 (a[0] * p[0] + a[1] * p[1] + a[2] * p[2])
1105 -(p[0]*p[3] * p[0] + p[1]*p[3] * p[1] + p[2]*p[3] * p[2])
1106 ;
1107 }
1108
1109 /* Quaternions */
1110
1111 static inline void q_identity( v4f q )
1112 {
1113 q[0] = 0.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 1.0f;
1114 }
1115
1116 static inline void q_axis_angle( v4f q, v3f axis, float angle )
1117 {
1118 float a = angle*0.5f,
1119 c = cosf(a),
1120 s = sinf(a);
1121
1122 q[0] = s*axis[0];
1123 q[1] = s*axis[1];
1124 q[2] = s*axis[2];
1125 q[3] = c;
1126 }
1127
1128 static inline void q_mul( v4f q, v4f q1, v4f d )
1129 {
1130 v4f t;
1131 t[0] = q[3]*q1[0] + q[0]*q1[3] + q[1]*q1[2] - q[2]*q1[1];
1132 t[1] = q[3]*q1[1] - q[0]*q1[2] + q[1]*q1[3] + q[2]*q1[0];
1133 t[2] = q[3]*q1[2] + q[0]*q1[1] - q[1]*q1[0] + q[2]*q1[3];
1134 t[3] = q[3]*q1[3] - q[0]*q1[0] - q[1]*q1[1] - q[2]*q1[2];
1135 v4_copy( t, d );
1136 }
1137
1138 static inline void q_normalize( v4f q )
1139 {
1140 float s = 1.0f/ sqrtf(v4_dot(q,q));
1141 q[0] *= s;
1142 q[1] *= s;
1143 q[2] *= s;
1144 q[3] *= s;
1145 }
1146
1147 static inline void q_inv( v4f q, v4f d )
1148 {
1149 float s = 1.0f / v4_dot(q,q);
1150 d[0] = -q[0]*s;
1151 d[1] = -q[1]*s;
1152 d[2] = -q[2]*s;
1153 d[3] = q[3]*s;
1154 }
1155
1156 static inline void q_nlerp( v4f a, v4f b, float t, v4f d )
1157 {
1158 if( v4_dot(a,b) < 0.0f ){
1159 v4_muls( b, -1.0f, d );
1160 v4_lerp( a, d, t, d );
1161 }
1162 else
1163 v4_lerp( a, b, t, d );
1164
1165 q_normalize( d );
1166 }
1167
1168 static void euler_m3x3( v3f angles, m3x3f d )
1169 {
1170 float cosY = cosf( angles[0] ),
1171 sinY = sinf( angles[0] ),
1172 cosP = cosf( angles[1] ),
1173 sinP = sinf( angles[1] ),
1174 cosR = cosf( angles[2] ),
1175 sinR = sinf( angles[2] );
1176
1177 d[2][0] = -sinY * cosP;
1178 d[2][1] = sinP;
1179 d[2][2] = cosY * cosP;
1180
1181 d[0][0] = cosY * cosR;
1182 d[0][1] = sinR;
1183 d[0][2] = sinY * cosR;
1184
1185 v3_cross( d[0], d[2], d[1] );
1186 }
1187
1188 static inline void q_m3x3( v4f q, m3x3f d )
1189 {
1190 float
1191 l = v4_length(q),
1192 s = l > 0.0f? 2.0f/l: 0.0f,
1193
1194 xx = s*q[0]*q[0], xy = s*q[0]*q[1], wx = s*q[3]*q[0],
1195 yy = s*q[1]*q[1], yz = s*q[1]*q[2], wy = s*q[3]*q[1],
1196 zz = s*q[2]*q[2], xz = s*q[0]*q[2], wz = s*q[3]*q[2];
1197
1198 d[0][0] = 1.0f - yy - zz;
1199 d[1][1] = 1.0f - xx - zz;
1200 d[2][2] = 1.0f - xx - yy;
1201 d[0][1] = xy + wz;
1202 d[1][2] = yz + wx;
1203 d[2][0] = xz + wy;
1204 d[1][0] = xy - wz;
1205 d[2][1] = yz - wx;
1206 d[0][2] = xz - wy;
1207 }
1208
1209 static void m3x3_q( m3x3f m, v4f q )
1210 {
1211 float diag, r, rinv;
1212
1213 diag = m[0][0] + m[1][1] + m[2][2];
1214 if( diag >= 0.0f )
1215 {
1216 r = sqrtf( 1.0f + diag );
1217 rinv = 0.5f / r;
1218 q[0] = rinv * (m[1][2] - m[2][1]);
1219 q[1] = rinv * (m[2][0] - m[0][2]);
1220 q[2] = rinv * (m[0][1] - m[1][0]);
1221 q[3] = r * 0.5f;
1222 }
1223 else if( m[0][0] >= m[1][1] && m[0][0] >= m[2][2] )
1224 {
1225 r = sqrtf( 1.0f - m[1][1] - m[2][2] + m[0][0] );
1226 rinv = 0.5f / r;
1227 q[0] = r * 0.5f;
1228 q[1] = rinv * (m[0][1] + m[1][0]);
1229 q[2] = rinv * (m[0][2] + m[2][0]);
1230 q[3] = rinv * (m[1][2] - m[2][1]);
1231 }
1232 else if( m[1][1] >= m[2][2] )
1233 {
1234 r = sqrtf( 1.0f - m[0][0] - m[2][2] + m[1][1] );
1235 rinv = 0.5f / r;
1236 q[0] = rinv * (m[0][1] + m[1][0]);
1237 q[1] = r * 0.5f;
1238 q[2] = rinv * (m[1][2] + m[2][1]);
1239 q[3] = rinv * (m[2][0] - m[0][2]);
1240 }
1241 else
1242 {
1243 r = sqrtf( 1.0f - m[0][0] - m[1][1] + m[2][2] );
1244 rinv = 0.5f / r;
1245 q[0] = rinv * (m[0][2] + m[2][0]);
1246 q[1] = rinv * (m[1][2] + m[2][1]);
1247 q[2] = r * 0.5f;
1248 q[3] = rinv * (m[0][1] - m[1][0]);
1249 }
1250 }
1251
1252 static void q_mulv( v4f q, v3f v, v3f d )
1253 {
1254 v3f v1, v2;
1255
1256 v3_muls( q, 2.0f*v3_dot(q,v), v1 );
1257 v3_muls( v, q[3]*q[3] - v3_dot(q,q), v2 );
1258 v3_add( v1, v2, v1 );
1259 v3_cross( q, v, v2 );
1260 v3_muls( v2, 2.0f*q[3], v2 );
1261 v3_add( v1, v2, d );
1262 }
1263
1264 enum contact_type
1265 {
1266 k_contact_type_default,
1267 k_contact_type_disabled,
1268 k_contact_type_edge
1269 };
1270
1271 /*
1272 * Matrix 4x3
1273 */
1274
1275 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
1276 { 0.0f, 1.0f, 0.0f, },\
1277 { 0.0f, 0.0f, 1.0f, },\
1278 { 0.0f, 0.0f, 0.0f }}
1279
1280 static inline void m4x3_to_3x3( m4x3f a, m3x3f b )
1281 {
1282 v3_copy( a[0], b[0] );
1283 v3_copy( a[1], b[1] );
1284 v3_copy( a[2], b[2] );
1285 }
1286
1287 static inline void m4x3_invert_affine( m4x3f a, m4x3f b )
1288 {
1289 m3x3_transpose( a, b );
1290 m3x3_mulv( b, a[3], b[3] );
1291 v3_negate( b[3], b[3] );
1292 }
1293
1294 static void m4x3_invert_full( m4x3f src, m4x3f dst )
1295 {
1296 float t2, t4, t5,
1297 det,
1298 a = src[0][0], b = src[0][1], c = src[0][2],
1299 e = src[1][0], f = src[1][1], g = src[1][2],
1300 i = src[2][0], j = src[2][1], k = src[2][2],
1301 m = src[3][0], n = src[3][1], o = src[3][2];
1302
1303 t2 = j*o - n*k;
1304 t4 = i*o - m*k;
1305 t5 = i*n - m*j;
1306
1307 dst[0][0] = f*k - g*j;
1308 dst[1][0] =-(e*k - g*i);
1309 dst[2][0] = e*j - f*i;
1310 dst[3][0] =-(e*t2 - f*t4 + g*t5);
1311
1312 dst[0][1] =-(b*k - c*j);
1313 dst[1][1] = a*k - c*i;
1314 dst[2][1] =-(a*j - b*i);
1315 dst[3][1] = a*t2 - b*t4 + c*t5;
1316
1317 t2 = f*o - n*g;
1318 t4 = e*o - m*g;
1319 t5 = e*n - m*f;
1320
1321 dst[0][2] = b*g - c*f ;
1322 dst[1][2] =-(a*g - c*e );
1323 dst[2][2] = a*f - b*e ;
1324 dst[3][2] =-(a*t2 - b*t4 + c * t5);
1325
1326 det = 1.0f / (a * dst[0][0] + b * dst[1][0] + c * dst[2][0]);
1327 v3_muls( dst[0], det, dst[0] );
1328 v3_muls( dst[1], det, dst[1] );
1329 v3_muls( dst[2], det, dst[2] );
1330 v3_muls( dst[3], det, dst[3] );
1331 }
1332
1333 static inline void m4x3_copy( m4x3f a, m4x3f b )
1334 {
1335 v3_copy( a[0], b[0] );
1336 v3_copy( a[1], b[1] );
1337 v3_copy( a[2], b[2] );
1338 v3_copy( a[3], b[3] );
1339 }
1340
1341 static inline void m4x3_identity( m4x3f a )
1342 {
1343 m4x3f id = M4X3_IDENTITY;
1344 m4x3_copy( id, a );
1345 }
1346
1347 static void m4x3_mul( m4x3f a, m4x3f b, m4x3f d )
1348 {
1349 float
1350 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2],
1351 a10 = a[1][0], a11 = a[1][1], a12 = a[1][2],
1352 a20 = a[2][0], a21 = a[2][1], a22 = a[2][2],
1353 a30 = a[3][0], a31 = a[3][1], a32 = a[3][2],
1354 b00 = b[0][0], b01 = b[0][1], b02 = b[0][2],
1355 b10 = b[1][0], b11 = b[1][1], b12 = b[1][2],
1356 b20 = b[2][0], b21 = b[2][1], b22 = b[2][2],
1357 b30 = b[3][0], b31 = b[3][1], b32 = b[3][2];
1358
1359 d[0][0] = a00*b00 + a10*b01 + a20*b02;
1360 d[0][1] = a01*b00 + a11*b01 + a21*b02;
1361 d[0][2] = a02*b00 + a12*b01 + a22*b02;
1362 d[1][0] = a00*b10 + a10*b11 + a20*b12;
1363 d[1][1] = a01*b10 + a11*b11 + a21*b12;
1364 d[1][2] = a02*b10 + a12*b11 + a22*b12;
1365 d[2][0] = a00*b20 + a10*b21 + a20*b22;
1366 d[2][1] = a01*b20 + a11*b21 + a21*b22;
1367 d[2][2] = a02*b20 + a12*b21 + a22*b22;
1368 d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30;
1369 d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31;
1370 d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32;
1371 }
1372
1373 #if 0 /* shat appf mingw wstringop-overflow */
1374 inline
1375 #endif
1376 static void m4x3_mulv( m4x3f m, v3f v, v3f d )
1377 {
1378 v3f res;
1379
1380 res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0];
1381 res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1];
1382 res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2];
1383
1384 v3_copy( res, d );
1385 }
1386
1387 /*
1388 * Transform plane ( xyz, distance )
1389 */
1390 static void m4x3_mulp( m4x3f m, v4f p, v4f d )
1391 {
1392 v3f o;
1393
1394 v3_muls( p, p[3], o );
1395 m4x3_mulv( m, o, o );
1396 m3x3_mulv( m, p, d );
1397
1398 d[3] = v3_dot( o, d );
1399 }
1400
1401 /*
1402 * Affine transforms
1403 */
1404
1405 static void m4x3_translate( m4x3f m, v3f v )
1406 {
1407 v3_muladds( m[3], m[0], v[0], m[3] );
1408 v3_muladds( m[3], m[1], v[1], m[3] );
1409 v3_muladds( m[3], m[2], v[2], m[3] );
1410 }
1411
1412 static void m4x3_rotate_x( m4x3f m, float angle )
1413 {
1414 m4x3f t = M4X3_IDENTITY;
1415 float c, s;
1416
1417 c = cosf( angle );
1418 s = sinf( angle );
1419
1420 t[1][1] = c;
1421 t[1][2] = s;
1422 t[2][1] = -s;
1423 t[2][2] = c;
1424
1425 m4x3_mul( m, t, m );
1426 }
1427
1428 static void m4x3_rotate_y( m4x3f m, float angle )
1429 {
1430 m4x3f t = M4X3_IDENTITY;
1431 float c, s;
1432
1433 c = cosf( angle );
1434 s = sinf( angle );
1435
1436 t[0][0] = c;
1437 t[0][2] = -s;
1438 t[2][0] = s;
1439 t[2][2] = c;
1440
1441 m4x3_mul( m, t, m );
1442 }
1443
1444 static void m4x3_rotate_z( m4x3f m, float angle )
1445 {
1446 m4x3f t = M4X3_IDENTITY;
1447 float c, s;
1448
1449 c = cosf( angle );
1450 s = sinf( angle );
1451
1452 t[0][0] = c;
1453 t[0][1] = s;
1454 t[1][0] = -s;
1455 t[1][1] = c;
1456
1457 m4x3_mul( m, t, m );
1458 }
1459
1460 static void m4x3_expand( m4x3f m, m4x4f d )
1461 {
1462 v3_copy( m[0], d[0] );
1463 v3_copy( m[1], d[1] );
1464 v3_copy( m[2], d[2] );
1465 v3_copy( m[3], d[3] );
1466 d[0][3] = 0.0f;
1467 d[1][3] = 0.0f;
1468 d[2][3] = 0.0f;
1469 d[3][3] = 1.0f;
1470 }
1471
1472 static void m4x3_decompose( m4x3f m, v3f co, v4f q, v3f s )
1473 {
1474 v3_copy( m[3], co );
1475 s[0] = v3_length(m[0]);
1476 s[1] = v3_length(m[1]);
1477 s[2] = v3_length(m[2]);
1478
1479 m3x3f rot;
1480 v3_divs( m[0], s[0], rot[0] );
1481 v3_divs( m[1], s[1], rot[1] );
1482 v3_divs( m[2], s[2], rot[2] );
1483
1484 m3x3_q( rot, q );
1485 }
1486
1487 static void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point )
1488 {
1489 v3f v;
1490 m4x3_mulv( m, point, v );
1491
1492 v3_minv( box[0], v, box[0] );
1493 v3_maxv( box[1], v, box[1] );
1494 }
1495
1496 static void m4x3_transform_aabb( m4x3f m, boxf box )
1497 {
1498 v3f a; v3f b;
1499
1500 v3_copy( box[0], a );
1501 v3_copy( box[1], b );
1502 v3_fill( box[0], INFINITY );
1503 v3_fill( box[1], -INFINITY );
1504
1505 m4x3_expand_aabb_point( m, box, (v3f){ a[0], a[1], a[2] } );
1506 m4x3_expand_aabb_point( m, box, (v3f){ a[0], b[1], a[2] } );
1507 m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], a[2] } );
1508 m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], a[2] } );
1509
1510 m4x3_expand_aabb_point( m, box, (v3f){ a[0], a[1], b[2] } );
1511 m4x3_expand_aabb_point( m, box, (v3f){ a[0], b[1], b[2] } );
1512 m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], b[2] } );
1513 m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], b[2] } );
1514 }
1515
1516 /*
1517 * -----------------------------------------------------------------------------
1518 * Closest point functions
1519 * -----------------------------------------------------------------------------
1520 */
1521
1522 /*
1523 * These closest point tests were learned from Real-Time Collision Detection by
1524 * Christer Ericson
1525 */
1526 VG_STATIC float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
1527 float *s, float *t, v3f c1, v3f c2)
1528 {
1529 v3f d1,d2,r;
1530 v3_sub( q1, p1, d1 );
1531 v3_sub( q2, p2, d2 );
1532 v3_sub( p1, p2, r );
1533
1534 float a = v3_length2( d1 ),
1535 e = v3_length2( d2 ),
1536 f = v3_dot( d2, r );
1537
1538 const float kEpsilon = 0.0001f;
1539
1540 if( a <= kEpsilon && e <= kEpsilon )
1541 {
1542 *s = 0.0f;
1543 *t = 0.0f;
1544 v3_copy( p1, c1 );
1545 v3_copy( p2, c2 );
1546
1547 v3f v0;
1548 v3_sub( c1, c2, v0 );
1549
1550 return v3_length2( v0 );
1551 }
1552
1553 if( a<= kEpsilon )
1554 {
1555 *s = 0.0f;
1556 *t = vg_clampf( f / e, 0.0f, 1.0f );
1557 }
1558 else
1559 {
1560 float c = v3_dot( d1, r );
1561 if( e <= kEpsilon )
1562 {
1563 *t = 0.0f;
1564 *s = vg_clampf( -c / a, 0.0f, 1.0f );
1565 }
1566 else
1567 {
1568 float b = v3_dot(d1,d2),
1569 d = a*e-b*b;
1570
1571 if( d != 0.0f )
1572 {
1573 *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
1574 }
1575 else
1576 {
1577 *s = 0.0f;
1578 }
1579
1580 *t = (b*(*s)+f) / e;
1581
1582 if( *t < 0.0f )
1583 {
1584 *t = 0.0f;
1585 *s = vg_clampf( -c / a, 0.0f, 1.0f );
1586 }
1587 else if( *t > 1.0f )
1588 {
1589 *t = 1.0f;
1590 *s = vg_clampf((b-c)/a,0.0f,1.0f);
1591 }
1592 }
1593 }
1594
1595 v3_muladds( p1, d1, *s, c1 );
1596 v3_muladds( p2, d2, *t, c2 );
1597
1598 v3f v0;
1599 v3_sub( c1, c2, v0 );
1600 return v3_length2( v0 );
1601 }
1602
1603 VG_STATIC int point_inside_aabb( boxf box, v3f point )
1604 {
1605 if((point[0]<=box[1][0]) && (point[1]<=box[1][1]) && (point[2]<=box[1][2]) &&
1606 (point[0]>=box[0][0]) && (point[1]>=box[0][1]) && (point[2]>=box[0][2]) )
1607 return 1;
1608 else
1609 return 0;
1610 }
1611
1612 VG_STATIC void closest_point_aabb( v3f p, boxf box, v3f dest )
1613 {
1614 v3_maxv( p, box[0], dest );
1615 v3_minv( dest, box[1], dest );
1616 }
1617
1618 VG_STATIC void closest_point_obb( v3f p, boxf box,
1619 m4x3f mtx, m4x3f inv_mtx, v3f dest )
1620 {
1621 v3f local;
1622 m4x3_mulv( inv_mtx, p, local );
1623 closest_point_aabb( local, box, local );
1624 m4x3_mulv( mtx, local, dest );
1625 }
1626
1627 VG_STATIC float closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
1628 {
1629 v3f v0, v1;
1630 v3_sub( b, a, v0 );
1631 v3_sub( point, a, v1 );
1632
1633 float t = v3_dot( v1, v0 ) / v3_length2(v0);
1634 t = vg_clampf(t,0.0f,1.0f);
1635 v3_muladds( a, v0, t, dest );
1636 return t;
1637 }
1638
1639 VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
1640 {
1641 v3f ab, ac, ap;
1642 float d1, d2;
1643
1644 /* Region outside A */
1645 v3_sub( tri[1], tri[0], ab );
1646 v3_sub( tri[2], tri[0], ac );
1647 v3_sub( p, tri[0], ap );
1648
1649 d1 = v3_dot(ab,ap);
1650 d2 = v3_dot(ac,ap);
1651 if( d1 <= 0.0f && d2 <= 0.0f )
1652 {
1653 v3_copy( tri[0], dest );
1654 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1655 return;
1656 }
1657
1658 /* Region outside B */
1659 v3f bp;
1660 float d3, d4;
1661
1662 v3_sub( p, tri[1], bp );
1663 d3 = v3_dot( ab, bp );
1664 d4 = v3_dot( ac, bp );
1665
1666 if( d3 >= 0.0f && d4 <= d3 )
1667 {
1668 v3_copy( tri[1], dest );
1669 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1670 return;
1671 }
1672
1673 /* Edge region of AB */
1674 float vc = d1*d4 - d3*d2;
1675 if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
1676 {
1677 float v = d1 / (d1-d3);
1678 v3_muladds( tri[0], ab, v, dest );
1679 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1680 return;
1681 }
1682
1683 /* Region outside C */
1684 v3f cp;
1685 float d5, d6;
1686 v3_sub( p, tri[2], cp );
1687 d5 = v3_dot(ab, cp);
1688 d6 = v3_dot(ac, cp);
1689
1690 if( d6 >= 0.0f && d5 <= d6 )
1691 {
1692 v3_copy( tri[2], dest );
1693 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1694 return;
1695 }
1696
1697 /* Region of AC */
1698 float vb = d5*d2 - d1*d6;
1699 if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
1700 {
1701 float w = d2 / (d2-d6);
1702 v3_muladds( tri[0], ac, w, dest );
1703 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1704 return;
1705 }
1706
1707 /* Region of BC */
1708 float va = d3*d6 - d5*d4;
1709 if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
1710 {
1711 float w = (d4-d3) / ((d4-d3) + (d5-d6));
1712 v3f bc;
1713 v3_sub( tri[2], tri[1], bc );
1714 v3_muladds( tri[1], bc, w, dest );
1715 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1716 return;
1717 }
1718
1719 /* P inside region, Q via barycentric coordinates uvw */
1720 float d = 1.0f/(va+vb+vc),
1721 v = vb*d,
1722 w = vc*d;
1723
1724 v3_muladds( tri[0], ab, v, dest );
1725 v3_muladds( dest, ac, w, dest );
1726 }
1727
1728 VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
1729 {
1730 v3f ab, ac, ap;
1731 float d1, d2;
1732
1733 /* Region outside A */
1734 v3_sub( tri[1], tri[0], ab );
1735 v3_sub( tri[2], tri[0], ac );
1736 v3_sub( p, tri[0], ap );
1737
1738 d1 = v3_dot(ab,ap);
1739 d2 = v3_dot(ac,ap);
1740 if( d1 <= 0.0f && d2 <= 0.0f )
1741 {
1742 v3_copy( tri[0], dest );
1743 return k_contact_type_default;
1744 }
1745
1746 /* Region outside B */
1747 v3f bp;
1748 float d3, d4;
1749
1750 v3_sub( p, tri[1], bp );
1751 d3 = v3_dot( ab, bp );
1752 d4 = v3_dot( ac, bp );
1753
1754 if( d3 >= 0.0f && d4 <= d3 )
1755 {
1756 v3_copy( tri[1], dest );
1757 return k_contact_type_edge;
1758 }
1759
1760 /* Edge region of AB */
1761 float vc = d1*d4 - d3*d2;
1762 if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
1763 {
1764 float v = d1 / (d1-d3);
1765 v3_muladds( tri[0], ab, v, dest );
1766 return k_contact_type_edge;
1767 }
1768
1769 /* Region outside C */
1770 v3f cp;
1771 float d5, d6;
1772 v3_sub( p, tri[2], cp );
1773 d5 = v3_dot(ab, cp);
1774 d6 = v3_dot(ac, cp);
1775
1776 if( d6 >= 0.0f && d5 <= d6 )
1777 {
1778 v3_copy( tri[2], dest );
1779 return k_contact_type_edge;
1780 }
1781
1782 /* Region of AC */
1783 float vb = d5*d2 - d1*d6;
1784 if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
1785 {
1786 float w = d2 / (d2-d6);
1787 v3_muladds( tri[0], ac, w, dest );
1788 return k_contact_type_edge;
1789 }
1790
1791 /* Region of BC */
1792 float va = d3*d6 - d5*d4;
1793 if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
1794 {
1795 float w = (d4-d3) / ((d4-d3) + (d5-d6));
1796 v3f bc;
1797 v3_sub( tri[2], tri[1], bc );
1798 v3_muladds( tri[1], bc, w, dest );
1799 return k_contact_type_edge;
1800 }
1801
1802 /* P inside region, Q via barycentric coordinates uvw */
1803 float d = 1.0f/(va+vb+vc),
1804 v = vb*d,
1805 w = vc*d;
1806
1807 v3_muladds( tri[0], ab, v, dest );
1808 v3_muladds( dest, ac, w, dest );
1809
1810 return k_contact_type_default;
1811 }
1812
1813
1814 static void closest_point_elipse( v2f p, v2f e, v2f o )
1815 {
1816 v2f pabs, ei, e2, ve, t;
1817
1818 v2_abs( p, pabs );
1819 v2_div( (v2f){ 1.0f, 1.0f }, e, ei );
1820 v2_mul( e, e, e2 );
1821 v2_mul( ei, (v2f){ e2[0]-e2[1], e2[1]-e2[0] }, ve );
1822
1823 v2_fill( t, 0.70710678118654752f );
1824
1825 for( int i=0; i<3; i++ )
1826 {
1827 v2f v, u, ud, w;
1828
1829 v2_mul( ve, t, v ); /* ve*t*t*t */
1830 v2_mul( v, t, v );
1831 v2_mul( v, t, v );
1832
1833 v2_sub( pabs, v, u );
1834 v2_normalize( u );
1835
1836 v2_mul( t, e, ud );
1837 v2_sub( ud, v, ud );
1838
1839 v2_muls( u, v2_length( ud ), u );
1840
1841 v2_add( v, u, w );
1842 v2_mul( w, ei, w );
1843
1844 v2_maxv( (v2f){0.0f,0.0f}, w, t );
1845 v2_normalize( t );
1846 }
1847
1848 v2_mul( t, e, o );
1849 v2_copysign( o, p );
1850 }
1851
1852 /*
1853 * Raycasts
1854 */
1855
1856 /* Time of intersection with ray vs triangle */
1857 static int ray_tri( v3f tri[3], v3f co,
1858 v3f dir, float *dist )
1859 {
1860 float const kEpsilon = 0.00001f;
1861
1862 v3f v0, v1, h, s, q, n;
1863 float a,f,u,v,t;
1864
1865 float *pa = tri[0],
1866 *pb = tri[1],
1867 *pc = tri[2];
1868
1869 v3_sub( pb, pa, v0 );
1870 v3_sub( pc, pa, v1 );
1871 v3_cross( dir, v1, h );
1872 v3_cross( v0, v1, n );
1873
1874 if( v3_dot( n, dir ) > 0.0f ) /* Backface culling */
1875 return 0;
1876
1877 /* Parralel */
1878 a = v3_dot( v0, h );
1879
1880 if( a > -kEpsilon && a < kEpsilon )
1881 return 0;
1882
1883 f = 1.0f/a;
1884 v3_sub( co, pa, s );
1885
1886 u = f * v3_dot(s, h);
1887 if( u < 0.0f || u > 1.0f )
1888 return 0;
1889
1890 v3_cross( s, v0, q );
1891 v = f * v3_dot( dir, q );
1892 if( v < 0.0f || u+v > 1.0f )
1893 return 0;
1894
1895 t = f * v3_dot(v1, q);
1896 if( t > kEpsilon )
1897 {
1898 *dist = t;
1899 return 1;
1900 }
1901 else return 0;
1902 }
1903
1904 /* time of intersection with ray vs sphere */
1905 static int ray_sphere( v3f c, float r,
1906 v3f co, v3f dir, float *t )
1907 {
1908 v3f m;
1909 v3_sub( co, c, m );
1910
1911 float b = v3_dot( m, dir ),
1912 c1 = v3_dot( m, m ) - r*r;
1913
1914 /* Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) */
1915 if( c1 > 0.0f && b > 0.0f )
1916 return 0;
1917
1918 float discr = b*b - c1;
1919
1920 /* A negative discriminant corresponds to ray missing sphere */
1921 if( discr < 0.0f )
1922 return 0;
1923
1924 /*
1925 * Ray now found to intersect sphere, compute smallest t value of
1926 * intersection
1927 */
1928 *t = -b - sqrtf( discr );
1929
1930 /* If t is negative, ray started inside sphere so clamp t to zero */
1931 if( *t < 0.0f )
1932 *t = 0.0f;
1933
1934 return 1;
1935 }
1936
1937 /*
1938 * time of intersection of ray vs cylinder
1939 * The cylinder does not have caps but is finite
1940 *
1941 * Heavily adapted from regular segment vs cylinder from:
1942 * Real-Time Collision Detection
1943 */
1944 static int ray_uncapped_finite_cylinder( v3f q, v3f p, float r,
1945 v3f co, v3f dir, float *t )
1946 {
1947 v3f d, m, n, sb;
1948 v3_muladds( co, dir, 1.0f, sb );
1949
1950 v3_sub( q, p, d );
1951 v3_sub( co, p, m );
1952 v3_sub( sb, co, n );
1953
1954 float md = v3_dot( m, d ),
1955 nd = v3_dot( n, d ),
1956 dd = v3_dot( d, d ),
1957 nn = v3_dot( n, n ),
1958 mn = v3_dot( m, n ),
1959 a = dd*nn - nd*nd,
1960 k = v3_dot( m, m ) - r*r,
1961 c = dd*k - md*md;
1962
1963 if( fabsf(a) < 0.00001f )
1964 {
1965 /* Segment runs parallel to cylinder axis */
1966 return 0;
1967 }
1968
1969 float b = dd*mn - nd*md,
1970 discr = b*b - a*c;
1971
1972 if( discr < 0.0f )
1973 return 0; /* No real roots; no intersection */
1974
1975 *t = (-b - sqrtf(discr)) / a;
1976 if( *t < 0.0f )
1977 return 0; /* Intersection behind ray */
1978
1979 /* Check within cylinder segment */
1980 if( md + (*t)*nd < 0.0f )
1981 return 0;
1982
1983 if( md + (*t)*nd > dd )
1984 return 0;
1985
1986 /* Segment intersects cylinder between the endcaps; t is correct */
1987 return 1;
1988 }
1989
1990 /*
1991 * Time of intersection of sphere and triangle. Origin must be outside the
1992 * colliding area. This is a fairly long procedure.
1993 */
1994 static int spherecast_triangle( v3f tri[3],
1995 v3f co, v3f dir, float r, float *t, v3f n )
1996 {
1997 v3f sum[3];
1998 v3f v0, v1;
1999
2000 v3_sub( tri[1], tri[0], v0 );
2001 v3_sub( tri[2], tri[0], v1 );
2002 v3_cross( v0, v1, n );
2003 v3_normalize( n );
2004 v3_muladds( tri[0], n, r, sum[0] );
2005 v3_muladds( tri[1], n, r, sum[1] );
2006 v3_muladds( tri[2], n, r, sum[2] );
2007
2008 int hit = 0;
2009 float t_min = INFINITY,
2010 t1;
2011
2012 if( ray_tri( sum, co, dir, &t1 ) )
2013 {
2014 t_min = vg_minf( t_min, t1 );
2015 hit = 1;
2016 }
2017
2018 /*
2019 * Currently disabled; ray_sphere requires |d| = 1. it is not very important.
2020 */
2021 #if 0
2022 for( int i=0; i<3; i++ )
2023 {
2024 if( ray_sphere( tri[i], r, co, dir, &t1 ) )
2025 {
2026 t_min = vg_minf( t_min, t1 );
2027 hit = 1;
2028 }
2029 }
2030 #endif
2031
2032 for( int i=0; i<3; i++ )
2033 {
2034 int i0 = i,
2035 i1 = (i+1)%3;
2036
2037 if( ray_uncapped_finite_cylinder( tri[i0], tri[i1], r, co, dir, &t1 ) )
2038 {
2039 if( t1 < t_min )
2040 {
2041 t_min = t1;
2042
2043 v3f co1, ct, cx;
2044 v3_add( dir, co, co1 );
2045 v3_lerp( co, co1, t_min, ct );
2046
2047 closest_point_segment( tri[i0], tri[i1], ct, cx );
2048 v3_sub( ct, cx, n );
2049 v3_normalize( n );
2050 }
2051
2052 hit = 1;
2053 }
2054 }
2055
2056 *t = t_min;
2057 return hit;
2058 }
2059
2060 static inline float vg_randf(void)
2061 {
2062 /* TODO: replace with our own rand */
2063 return (float)rand()/(float)(RAND_MAX);
2064 }
2065
2066 static inline void vg_rand_dir(v3f dir)
2067 {
2068 dir[0] = vg_randf();
2069 dir[1] = vg_randf();
2070 dir[2] = vg_randf();
2071
2072 v3_muls( dir, 2.0f, dir );
2073 v3_sub( dir, (v3f){1.0f,1.0f,1.0f}, dir );
2074
2075 v3_normalize( dir );
2076 }
2077
2078 static inline void vg_rand_sphere( v3f co )
2079 {
2080 vg_rand_dir(co);
2081 v3_muls( co, cbrtf( vg_randf() ), co );
2082 }
2083
2084 static inline int vg_randint(int max)
2085 {
2086 return rand()%max;
2087 }
2088
2089 static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p )
2090 {
2091 float tt = t*t,
2092 ttt = tt*t;
2093
2094 v3_muls( p1, ttt, p );
2095 v3_muladds( p, h1, 3.0f*tt -3.0f*ttt, p );
2096 v3_muladds( p, h0, 3.0f*ttt -6.0f*tt +3.0f*t, p );
2097 v3_muladds( p, p0, 3.0f*tt -ttt -3.0f*t +1.0f, p );
2098 }
2099
2100 static void eval_bezier3( v3f p0, v3f p1, v3f p2, float t, v3f p )
2101 {
2102 float u = 1.0f-t;
2103
2104 v3_muls( p0, u*u, p );
2105 v3_muladds( p, p1, 2.0f*u*t, p );
2106 v3_muladds( p, p2, t*t, p );
2107 }
2108
2109 #endif /* VG_M_H */