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