async possible race
[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 if( v4_dot(a,b) < 0.0f ){
633 v4f c;
634 v4_muls( b, -1.0f, c );
635 v4_lerp( a, c, 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 v3f v;
1199 m4x3_mulv( m, point, v );
1200
1201 v3_minv( box[0], v, box[0] );
1202 v3_maxv( box[1], v, box[1] );
1203 }
1204
1205 static void m4x3_expand_aabb_aabb( m4x3f m, boxf boxa, boxf boxb ){
1206 v3f a; v3f b;
1207 v3_copy( boxb[0], a );
1208 v3_copy( boxb[1], b );
1209 m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], a[1], a[2] } );
1210 m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], b[1], a[2] } );
1211 m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], b[1], a[2] } );
1212 m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], a[1], a[2] } );
1213 m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], a[1], b[2] } );
1214 m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], b[1], b[2] } );
1215 m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], b[1], b[2] } );
1216 m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], a[1], b[2] } );
1217 }
1218 static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up )
1219 {
1220 v3f dir;
1221 v3_sub( target, pos, dir );
1222 v3_normalize( dir );
1223
1224 v3_copy( dir, m[2] );
1225
1226 v3_cross( up, m[2], m[0] );
1227 v3_normalize( m[0] );
1228
1229 v3_cross( m[2], m[0], m[1] );
1230 v3_copy( pos, m[3] );
1231 }
1232
1233 /*
1234 * -----------------------------------------------------------------------------
1235 * Section 4.d 4x4 matrices
1236 * -----------------------------------------------------------------------------
1237 */
1238
1239 #define M4X4_IDENTITY {{1.0f, 0.0f, 0.0f, 0.0f },\
1240 { 0.0f, 1.0f, 0.0f, 0.0f },\
1241 { 0.0f, 0.0f, 1.0f, 0.0f },\
1242 { 0.0f, 0.0f, 0.0f, 1.0f }}
1243 #define M4X4_ZERO {{0.0f, 0.0f, 0.0f, 0.0f },\
1244 { 0.0f, 0.0f, 0.0f, 0.0f },\
1245 { 0.0f, 0.0f, 0.0f, 0.0f },\
1246 { 0.0f, 0.0f, 0.0f, 0.0f }}
1247
1248 static void m4x4_projection( m4x4f m, f32 angle,
1249 f32 ratio, f32 fnear, f32 ffar )
1250 {
1251 f32 scale = tanf( angle * 0.5f * VG_PIf / 180.0f ) * fnear,
1252 r = ratio * scale,
1253 l = -r,
1254 t = scale,
1255 b = -t;
1256
1257 m[0][0] = 2.0f * fnear / (r - l);
1258 m[0][1] = 0.0f;
1259 m[0][2] = 0.0f;
1260 m[0][3] = 0.0f;
1261
1262 m[1][0] = 0.0f;
1263 m[1][1] = 2.0f * fnear / (t - b);
1264 m[1][2] = 0.0f;
1265 m[1][3] = 0.0f;
1266
1267 m[2][0] = (r + l) / (r - l);
1268 m[2][1] = (t + b) / (t - b);
1269 m[2][2] = -(ffar + fnear) / (ffar - fnear);
1270 m[2][3] = -1.0f;
1271
1272 m[3][0] = 0.0f;
1273 m[3][1] = 0.0f;
1274 m[3][2] = -2.0f * ffar * fnear / (ffar - fnear);
1275 m[3][3] = 0.0f;
1276 }
1277
1278 static void m4x4_translate( m4x4f m, v3f v )
1279 {
1280 v4_muladds( m[3], m[0], v[0], m[3] );
1281 v4_muladds( m[3], m[1], v[1], m[3] );
1282 v4_muladds( m[3], m[2], v[2], m[3] );
1283 }
1284
1285 static inline void m4x4_copy( m4x4f a, m4x4f b )
1286 {
1287 v4_copy( a[0], b[0] );
1288 v4_copy( a[1], b[1] );
1289 v4_copy( a[2], b[2] );
1290 v4_copy( a[3], b[3] );
1291 }
1292
1293 static inline void m4x4_identity( m4x4f a )
1294 {
1295 m4x4f id = M4X4_IDENTITY;
1296 m4x4_copy( id, a );
1297 }
1298
1299 static inline void m4x4_zero( m4x4f a )
1300 {
1301 m4x4f zero = M4X4_ZERO;
1302 m4x4_copy( zero, a );
1303 }
1304
1305 static inline void m4x4_mul( m4x4f a, m4x4f b, m4x4f d )
1306 {
1307 f32 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3],
1308 a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3],
1309 a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3],
1310 a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3],
1311
1312 b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], b03 = b[0][3],
1313 b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], b13 = b[1][3],
1314 b20 = b[2][0], b21 = b[2][1], b22 = b[2][2], b23 = b[2][3],
1315 b30 = b[3][0], b31 = b[3][1], b32 = b[3][2], b33 = b[3][3];
1316
1317 d[0][0] = a00*b00 + a10*b01 + a20*b02 + a30*b03;
1318 d[0][1] = a01*b00 + a11*b01 + a21*b02 + a31*b03;
1319 d[0][2] = a02*b00 + a12*b01 + a22*b02 + a32*b03;
1320 d[0][3] = a03*b00 + a13*b01 + a23*b02 + a33*b03;
1321 d[1][0] = a00*b10 + a10*b11 + a20*b12 + a30*b13;
1322 d[1][1] = a01*b10 + a11*b11 + a21*b12 + a31*b13;
1323 d[1][2] = a02*b10 + a12*b11 + a22*b12 + a32*b13;
1324 d[1][3] = a03*b10 + a13*b11 + a23*b12 + a33*b13;
1325 d[2][0] = a00*b20 + a10*b21 + a20*b22 + a30*b23;
1326 d[2][1] = a01*b20 + a11*b21 + a21*b22 + a31*b23;
1327 d[2][2] = a02*b20 + a12*b21 + a22*b22 + a32*b23;
1328 d[2][3] = a03*b20 + a13*b21 + a23*b22 + a33*b23;
1329 d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30*b33;
1330 d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31*b33;
1331 d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32*b33;
1332 d[3][3] = a03*b30 + a13*b31 + a23*b32 + a33*b33;
1333 }
1334
1335 static inline void m4x4_mulv( m4x4f m, v4f v, v4f d )
1336 {
1337 v4f res;
1338
1339 res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]*v[3];
1340 res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]*v[3];
1341 res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]*v[3];
1342 res[3] = m[0][3]*v[0] + m[1][3]*v[1] + m[2][3]*v[2] + m[3][3]*v[3];
1343
1344 v4_copy( res, d );
1345 }
1346
1347 static inline void m4x4_inv( m4x4f a, m4x4f d )
1348 {
1349 f32 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3],
1350 a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3],
1351 a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3],
1352 a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3],
1353 det,
1354 t[6];
1355
1356 t[0] = a22*a33 - a32*a23;
1357 t[1] = a21*a33 - a31*a23;
1358 t[2] = a21*a32 - a31*a22;
1359 t[3] = a20*a33 - a30*a23;
1360 t[4] = a20*a32 - a30*a22;
1361 t[5] = a20*a31 - a30*a21;
1362
1363 d[0][0] = a11*t[0] - a12*t[1] + a13*t[2];
1364 d[1][0] =-(a10*t[0] - a12*t[3] + a13*t[4]);
1365 d[2][0] = a10*t[1] - a11*t[3] + a13*t[5];
1366 d[3][0] =-(a10*t[2] - a11*t[4] + a12*t[5]);
1367
1368 d[0][1] =-(a01*t[0] - a02*t[1] + a03*t[2]);
1369 d[1][1] = a00*t[0] - a02*t[3] + a03*t[4];
1370 d[2][1] =-(a00*t[1] - a01*t[3] + a03*t[5]);
1371 d[3][1] = a00*t[2] - a01*t[4] + a02*t[5];
1372
1373 t[0] = a12*a33 - a32*a13;
1374 t[1] = a11*a33 - a31*a13;
1375 t[2] = a11*a32 - a31*a12;
1376 t[3] = a10*a33 - a30*a13;
1377 t[4] = a10*a32 - a30*a12;
1378 t[5] = a10*a31 - a30*a11;
1379
1380 d[0][2] = a01*t[0] - a02*t[1] + a03*t[2];
1381 d[1][2] =-(a00*t[0] - a02*t[3] + a03*t[4]);
1382 d[2][2] = a00*t[1] - a01*t[3] + a03*t[5];
1383 d[3][2] =-(a00*t[2] - a01*t[4] + a02*t[5]);
1384
1385 t[0] = a12*a23 - a22*a13;
1386 t[1] = a11*a23 - a21*a13;
1387 t[2] = a11*a22 - a21*a12;
1388 t[3] = a10*a23 - a20*a13;
1389 t[4] = a10*a22 - a20*a12;
1390 t[5] = a10*a21 - a20*a11;
1391
1392 d[0][3] =-(a01*t[0] - a02*t[1] + a03*t[2]);
1393 d[1][3] = a00*t[0] - a02*t[3] + a03*t[4];
1394 d[2][3] =-(a00*t[1] - a01*t[3] + a03*t[5]);
1395 d[3][3] = a00*t[2] - a01*t[4] + a02*t[5];
1396
1397 det = 1.0f / (a00*d[0][0] + a01*d[1][0] + a02*d[2][0] + a03*d[3][0]);
1398 v4_muls( d[0], det, d[0] );
1399 v4_muls( d[1], det, d[1] );
1400 v4_muls( d[2], det, d[2] );
1401 v4_muls( d[3], det, d[3] );
1402 }
1403
1404 /*
1405 * -----------------------------------------------------------------------------
1406 * Section 5.a Boxes
1407 * -----------------------------------------------------------------------------
1408 */
1409
1410 static inline void box_addpt( boxf a, v3f pt )
1411 {
1412 v3_minv( a[0], pt, a[0] );
1413 v3_maxv( a[1], pt, a[1] );
1414 }
1415
1416 static inline void box_concat( boxf a, boxf b )
1417 {
1418 v3_minv( a[0], b[0], a[0] );
1419 v3_maxv( a[1], b[1], a[1] );
1420 }
1421
1422 static inline void box_copy( boxf a, boxf b )
1423 {
1424 v3_copy( a[0], b[0] );
1425 v3_copy( a[1], b[1] );
1426 }
1427
1428 static inline int box_overlap( boxf a, boxf b )
1429 {
1430 return
1431 ( a[0][0] <= b[1][0] && a[1][0] >= b[0][0] ) &&
1432 ( a[0][1] <= b[1][1] && a[1][1] >= b[0][1] ) &&
1433 ( a[0][2] <= b[1][2] && a[1][2] >= b[0][2] )
1434 ;
1435 }
1436
1437 static int box_within( boxf greater, boxf lesser )
1438 {
1439 v3f a, b;
1440 v3_sub( lesser[0], greater[0], a );
1441 v3_sub( lesser[1], greater[1], b );
1442
1443 if( (a[0] >= 0.0f) && (a[1] >= 0.0f) && (a[2] >= 0.0f) &&
1444 (b[0] <= 0.0f) && (b[1] <= 0.0f) && (b[2] <= 0.0f) )
1445 {
1446 return 1;
1447 }
1448
1449 return 0;
1450 }
1451
1452 static inline void box_init_inf( boxf box ){
1453 v3_fill( box[0], INFINITY );
1454 v3_fill( box[1], -INFINITY );
1455 }
1456
1457 /*
1458 * -----------------------------------------------------------------------------
1459 * Section 5.b Planes
1460 * -----------------------------------------------------------------------------
1461 */
1462
1463 static inline void tri_to_plane( f64 a[3], f64 b[3],
1464 f64 c[3], f64 p[4] )
1465 {
1466 f64 edge0[3];
1467 f64 edge1[3];
1468 f64 l;
1469
1470 edge0[0] = b[0] - a[0];
1471 edge0[1] = b[1] - a[1];
1472 edge0[2] = b[2] - a[2];
1473
1474 edge1[0] = c[0] - a[0];
1475 edge1[1] = c[1] - a[1];
1476 edge1[2] = c[2] - a[2];
1477
1478 p[0] = edge0[1] * edge1[2] - edge0[2] * edge1[1];
1479 p[1] = edge0[2] * edge1[0] - edge0[0] * edge1[2];
1480 p[2] = edge0[0] * edge1[1] - edge0[1] * edge1[0];
1481
1482 l = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
1483 p[3] = (p[0] * a[0] + p[1] * a[1] + p[2] * a[2]) / l;
1484
1485 p[0] = p[0] / l;
1486 p[1] = p[1] / l;
1487 p[2] = p[2] / l;
1488 }
1489
1490 static int plane_intersect3( v4f a, v4f b, v4f c, v3f p )
1491 {
1492 f32 const epsilon = 1e-6f;
1493
1494 v3f x;
1495 v3_cross( a, b, x );
1496 f32 d = v3_dot( x, c );
1497
1498 if( (d < epsilon) && (d > -epsilon) ) return 0;
1499
1500 v3f v0, v1, v2;
1501 v3_cross( b, c, v0 );
1502 v3_cross( c, a, v1 );
1503 v3_cross( a, b, v2 );
1504
1505 v3_muls( v0, a[3], p );
1506 v3_muladds( p, v1, b[3], p );
1507 v3_muladds( p, v2, c[3], p );
1508 v3_divs( p, d, p );
1509
1510 return 1;
1511 }
1512
1513 int plane_intersect2( v4f a, v4f b, v3f p, v3f n )
1514 {
1515 f32 const epsilon = 1e-6f;
1516
1517 v4f c;
1518 v3_cross( a, b, c );
1519 f32 d = v3_length2( c );
1520
1521 if( (d < epsilon) && (d > -epsilon) )
1522 return 0;
1523
1524 v3f v0, v1, vx;
1525 v3_cross( c, b, v0 );
1526 v3_cross( a, c, v1 );
1527
1528 v3_muls( v0, a[3], vx );
1529 v3_muladds( vx, v1, b[3], vx );
1530 v3_divs( vx, d, p );
1531 v3_copy( c, n );
1532
1533 return 1;
1534 }
1535
1536 static int plane_segment( v4f plane, v3f a, v3f b, v3f co )
1537 {
1538 f32 d0 = v3_dot( a, plane ) - plane[3],
1539 d1 = v3_dot( b, plane ) - plane[3];
1540
1541 if( d0*d1 < 0.0f )
1542 {
1543 f32 tot = 1.0f/( fabsf(d0)+fabsf(d1) );
1544
1545 v3_muls( a, fabsf(d1) * tot, co );
1546 v3_muladds( co, b, fabsf(d0) * tot, co );
1547 return 1;
1548 }
1549
1550 return 0;
1551 }
1552
1553 static inline f64 plane_polarity( f64 p[4], f64 a[3] )
1554 {
1555 return
1556 (a[0] * p[0] + a[1] * p[1] + a[2] * p[2])
1557 -(p[0]*p[3] * p[0] + p[1]*p[3] * p[1] + p[2]*p[3] * p[2])
1558 ;
1559 }
1560
1561 /*
1562 * -----------------------------------------------------------------------------
1563 * Section 5.c Closest point functions
1564 * -----------------------------------------------------------------------------
1565 */
1566
1567 /*
1568 * These closest point tests were learned from Real-Time Collision Detection by
1569 * Christer Ericson
1570 */
1571 VG_STATIC f32 closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
1572 f32 *s, f32 *t, v3f c1, v3f c2)
1573 {
1574 v3f d1,d2,r;
1575 v3_sub( q1, p1, d1 );
1576 v3_sub( q2, p2, d2 );
1577 v3_sub( p1, p2, r );
1578
1579 f32 a = v3_length2( d1 ),
1580 e = v3_length2( d2 ),
1581 f = v3_dot( d2, r );
1582
1583 const f32 kEpsilon = 0.0001f;
1584
1585 if( a <= kEpsilon && e <= kEpsilon )
1586 {
1587 *s = 0.0f;
1588 *t = 0.0f;
1589 v3_copy( p1, c1 );
1590 v3_copy( p2, c2 );
1591
1592 v3f v0;
1593 v3_sub( c1, c2, v0 );
1594
1595 return v3_length2( v0 );
1596 }
1597
1598 if( a<= kEpsilon )
1599 {
1600 *s = 0.0f;
1601 *t = vg_clampf( f / e, 0.0f, 1.0f );
1602 }
1603 else
1604 {
1605 f32 c = v3_dot( d1, r );
1606 if( e <= kEpsilon )
1607 {
1608 *t = 0.0f;
1609 *s = vg_clampf( -c / a, 0.0f, 1.0f );
1610 }
1611 else
1612 {
1613 f32 b = v3_dot(d1,d2),
1614 d = a*e-b*b;
1615
1616 if( d != 0.0f )
1617 {
1618 *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
1619 }
1620 else
1621 {
1622 *s = 0.0f;
1623 }
1624
1625 *t = (b*(*s)+f) / e;
1626
1627 if( *t < 0.0f )
1628 {
1629 *t = 0.0f;
1630 *s = vg_clampf( -c / a, 0.0f, 1.0f );
1631 }
1632 else if( *t > 1.0f )
1633 {
1634 *t = 1.0f;
1635 *s = vg_clampf((b-c)/a,0.0f,1.0f);
1636 }
1637 }
1638 }
1639
1640 v3_muladds( p1, d1, *s, c1 );
1641 v3_muladds( p2, d2, *t, c2 );
1642
1643 v3f v0;
1644 v3_sub( c1, c2, v0 );
1645 return v3_length2( v0 );
1646 }
1647
1648 VG_STATIC int point_inside_aabb( boxf box, v3f point )
1649 {
1650 if((point[0]<=box[1][0]) && (point[1]<=box[1][1]) && (point[2]<=box[1][2]) &&
1651 (point[0]>=box[0][0]) && (point[1]>=box[0][1]) && (point[2]>=box[0][2]) )
1652 return 1;
1653 else
1654 return 0;
1655 }
1656
1657 VG_STATIC void closest_point_aabb( v3f p, boxf box, v3f dest )
1658 {
1659 v3_maxv( p, box[0], dest );
1660 v3_minv( dest, box[1], dest );
1661 }
1662
1663 VG_STATIC void closest_point_obb( v3f p, boxf box,
1664 m4x3f mtx, m4x3f inv_mtx, v3f dest )
1665 {
1666 v3f local;
1667 m4x3_mulv( inv_mtx, p, local );
1668 closest_point_aabb( local, box, local );
1669 m4x3_mulv( mtx, local, dest );
1670 }
1671
1672 VG_STATIC f32 closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
1673 {
1674 v3f v0, v1;
1675 v3_sub( b, a, v0 );
1676 v3_sub( point, a, v1 );
1677
1678 f32 t = v3_dot( v1, v0 ) / v3_length2(v0);
1679 t = vg_clampf(t,0.0f,1.0f);
1680 v3_muladds( a, v0, t, dest );
1681 return t;
1682 }
1683
1684 VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
1685 {
1686 v3f ab, ac, ap;
1687 f32 d1, d2;
1688
1689 /* Region outside A */
1690 v3_sub( tri[1], tri[0], ab );
1691 v3_sub( tri[2], tri[0], ac );
1692 v3_sub( p, tri[0], ap );
1693
1694 d1 = v3_dot(ab,ap);
1695 d2 = v3_dot(ac,ap);
1696 if( d1 <= 0.0f && d2 <= 0.0f )
1697 {
1698 v3_copy( tri[0], dest );
1699 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1700 return;
1701 }
1702
1703 /* Region outside B */
1704 v3f bp;
1705 f32 d3, d4;
1706
1707 v3_sub( p, tri[1], bp );
1708 d3 = v3_dot( ab, bp );
1709 d4 = v3_dot( ac, bp );
1710
1711 if( d3 >= 0.0f && d4 <= d3 )
1712 {
1713 v3_copy( tri[1], dest );
1714 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1715 return;
1716 }
1717
1718 /* Edge region of AB */
1719 f32 vc = d1*d4 - d3*d2;
1720 if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
1721 {
1722 f32 v = d1 / (d1-d3);
1723 v3_muladds( tri[0], ab, v, dest );
1724 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1725 return;
1726 }
1727
1728 /* Region outside C */
1729 v3f cp;
1730 f32 d5, d6;
1731 v3_sub( p, tri[2], cp );
1732 d5 = v3_dot(ab, cp);
1733 d6 = v3_dot(ac, cp);
1734
1735 if( d6 >= 0.0f && d5 <= d6 )
1736 {
1737 v3_copy( tri[2], dest );
1738 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1739 return;
1740 }
1741
1742 /* Region of AC */
1743 f32 vb = d5*d2 - d1*d6;
1744 if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
1745 {
1746 f32 w = d2 / (d2-d6);
1747 v3_muladds( tri[0], ac, w, dest );
1748 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1749 return;
1750 }
1751
1752 /* Region of BC */
1753 f32 va = d3*d6 - d5*d4;
1754 if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
1755 {
1756 f32 w = (d4-d3) / ((d4-d3) + (d5-d6));
1757 v3f bc;
1758 v3_sub( tri[2], tri[1], bc );
1759 v3_muladds( tri[1], bc, w, dest );
1760 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
1761 return;
1762 }
1763
1764 /* P inside region, Q via barycentric coordinates uvw */
1765 f32 d = 1.0f/(va+vb+vc),
1766 v = vb*d,
1767 w = vc*d;
1768
1769 v3_muladds( tri[0], ab, v, dest );
1770 v3_muladds( dest, ac, w, dest );
1771 }
1772
1773 enum contact_type
1774 {
1775 k_contact_type_default,
1776 k_contact_type_disabled,
1777 k_contact_type_edge
1778 };
1779
1780 VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
1781 {
1782 v3f ab, ac, ap;
1783 f32 d1, d2;
1784
1785 /* Region outside A */
1786 v3_sub( tri[1], tri[0], ab );
1787 v3_sub( tri[2], tri[0], ac );
1788 v3_sub( p, tri[0], ap );
1789
1790 d1 = v3_dot(ab,ap);
1791 d2 = v3_dot(ac,ap);
1792 if( d1 <= 0.0f && d2 <= 0.0f )
1793 {
1794 v3_copy( tri[0], dest );
1795 return k_contact_type_default;
1796 }
1797
1798 /* Region outside B */
1799 v3f bp;
1800 f32 d3, d4;
1801
1802 v3_sub( p, tri[1], bp );
1803 d3 = v3_dot( ab, bp );
1804 d4 = v3_dot( ac, bp );
1805
1806 if( d3 >= 0.0f && d4 <= d3 )
1807 {
1808 v3_copy( tri[1], dest );
1809 return k_contact_type_edge;
1810 }
1811
1812 /* Edge region of AB */
1813 f32 vc = d1*d4 - d3*d2;
1814 if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
1815 {
1816 f32 v = d1 / (d1-d3);
1817 v3_muladds( tri[0], ab, v, dest );
1818 return k_contact_type_edge;
1819 }
1820
1821 /* Region outside C */
1822 v3f cp;
1823 f32 d5, d6;
1824 v3_sub( p, tri[2], cp );
1825 d5 = v3_dot(ab, cp);
1826 d6 = v3_dot(ac, cp);
1827
1828 if( d6 >= 0.0f && d5 <= d6 )
1829 {
1830 v3_copy( tri[2], dest );
1831 return k_contact_type_edge;
1832 }
1833
1834 /* Region of AC */
1835 f32 vb = d5*d2 - d1*d6;
1836 if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
1837 {
1838 f32 w = d2 / (d2-d6);
1839 v3_muladds( tri[0], ac, w, dest );
1840 return k_contact_type_edge;
1841 }
1842
1843 /* Region of BC */
1844 f32 va = d3*d6 - d5*d4;
1845 if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
1846 {
1847 f32 w = (d4-d3) / ((d4-d3) + (d5-d6));
1848 v3f bc;
1849 v3_sub( tri[2], tri[1], bc );
1850 v3_muladds( tri[1], bc, w, dest );
1851 return k_contact_type_edge;
1852 }
1853
1854 /* P inside region, Q via barycentric coordinates uvw */
1855 f32 d = 1.0f/(va+vb+vc),
1856 v = vb*d,
1857 w = vc*d;
1858
1859 v3_muladds( tri[0], ab, v, dest );
1860 v3_muladds( dest, ac, w, dest );
1861
1862 return k_contact_type_default;
1863 }
1864
1865 static void closest_point_elipse( v2f p, v2f e, v2f o )
1866 {
1867 v2f pabs, ei, e2, ve, t;
1868
1869 v2_abs( p, pabs );
1870 v2_div( (v2f){ 1.0f, 1.0f }, e, ei );
1871 v2_mul( e, e, e2 );
1872 v2_mul( ei, (v2f){ e2[0]-e2[1], e2[1]-e2[0] }, ve );
1873
1874 v2_fill( t, 0.70710678118654752f );
1875
1876 for( int i=0; i<3; i++ ){
1877 v2f v, u, ud, w;
1878
1879 v2_mul( ve, t, v ); /* ve*t*t*t */
1880 v2_mul( v, t, v );
1881 v2_mul( v, t, v );
1882
1883 v2_sub( pabs, v, u );
1884 v2_normalize( u );
1885
1886 v2_mul( t, e, ud );
1887 v2_sub( ud, v, ud );
1888
1889 v2_muls( u, v2_length( ud ), u );
1890
1891 v2_add( v, u, w );
1892 v2_mul( w, ei, w );
1893
1894 v2_maxv( (v2f){0.0f,0.0f}, w, t );
1895 v2_normalize( t );
1896 }
1897
1898 v2_mul( t, e, o );
1899 v2_copysign( o, p );
1900 }
1901
1902 /*
1903 * -----------------------------------------------------------------------------
1904 * Section 5.d Raycasts & Spherecasts
1905 * -----------------------------------------------------------------------------
1906 */
1907
1908 int ray_aabb1( boxf box, v3f co, v3f dir_inv, f32 dist )
1909 {
1910 v3f v0, v1;
1911 f32 tmin, tmax;
1912
1913 v3_sub( box[0], co, v0 );
1914 v3_sub( box[1], co, v1 );
1915
1916 v3_mul( v0, dir_inv, v0 );
1917 v3_mul( v1, dir_inv, v1 );
1918
1919 tmin = vg_minf( v0[0], v1[0] );
1920 tmax = vg_maxf( v0[0], v1[0] );
1921 tmin = vg_maxf( tmin, vg_minf( v0[1], v1[1] ));
1922 tmax = vg_minf( tmax, vg_maxf( v0[1], v1[1] ));
1923 tmin = vg_maxf( tmin, vg_minf( v0[2], v1[2] ));
1924 tmax = vg_minf( tmax, vg_maxf( v0[2], v1[2] ));
1925
1926 return (tmax >= tmin) && (tmin <= dist) && (tmax >= 0.0f);
1927 }
1928
1929 /* Time of intersection with ray vs triangle */
1930 static int ray_tri( v3f tri[3], v3f co,
1931 v3f dir, f32 *dist )
1932 {
1933 f32 const kEpsilon = 0.00001f;
1934
1935 v3f v0, v1, h, s, q, n;
1936 f32 a,f,u,v,t;
1937
1938 f32 *pa = tri[0],
1939 *pb = tri[1],
1940 *pc = tri[2];
1941
1942 v3_sub( pb, pa, v0 );
1943 v3_sub( pc, pa, v1 );
1944 v3_cross( dir, v1, h );
1945 v3_cross( v0, v1, n );
1946
1947 if( v3_dot( n, dir ) > 0.0f ) /* Backface culling */
1948 return 0;
1949
1950 /* Parralel */
1951 a = v3_dot( v0, h );
1952
1953 if( a > -kEpsilon && a < kEpsilon )
1954 return 0;
1955
1956 f = 1.0f/a;
1957 v3_sub( co, pa, s );
1958
1959 u = f * v3_dot(s, h);
1960 if( u < 0.0f || u > 1.0f )
1961 return 0;
1962
1963 v3_cross( s, v0, q );
1964 v = f * v3_dot( dir, q );
1965 if( v < 0.0f || u+v > 1.0f )
1966 return 0;
1967
1968 t = f * v3_dot(v1, q);
1969 if( t > kEpsilon )
1970 {
1971 *dist = t;
1972 return 1;
1973 }
1974 else return 0;
1975 }
1976
1977 /* time of intersection with ray vs sphere */
1978 static int ray_sphere( v3f c, f32 r,
1979 v3f co, v3f dir, f32 *t )
1980 {
1981 v3f m;
1982 v3_sub( co, c, m );
1983
1984 f32 b = v3_dot( m, dir ),
1985 c1 = v3_dot( m, m ) - r*r;
1986
1987 /* Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) */
1988 if( c1 > 0.0f && b > 0.0f )
1989 return 0;
1990
1991 f32 discr = b*b - c1;
1992
1993 /* A negative discriminant corresponds to ray missing sphere */
1994 if( discr < 0.0f )
1995 return 0;
1996
1997 /*
1998 * Ray now found to intersect sphere, compute smallest t value of
1999 * intersection
2000 */
2001 *t = -b - sqrtf( discr );
2002
2003 /* If t is negative, ray started inside sphere so clamp t to zero */
2004 if( *t < 0.0f )
2005 *t = 0.0f;
2006
2007 return 1;
2008 }
2009
2010 /*
2011 * time of intersection of ray vs cylinder
2012 * The cylinder does not have caps but is finite
2013 *
2014 * Heavily adapted from regular segment vs cylinder from:
2015 * Real-Time Collision Detection
2016 */
2017 static int ray_uncapped_finite_cylinder( v3f q, v3f p, f32 r,
2018 v3f co, v3f dir, f32 *t )
2019 {
2020 v3f d, m, n, sb;
2021 v3_muladds( co, dir, 1.0f, sb );
2022
2023 v3_sub( q, p, d );
2024 v3_sub( co, p, m );
2025 v3_sub( sb, co, n );
2026
2027 f32 md = v3_dot( m, d ),
2028 nd = v3_dot( n, d ),
2029 dd = v3_dot( d, d ),
2030 nn = v3_dot( n, n ),
2031 mn = v3_dot( m, n ),
2032 a = dd*nn - nd*nd,
2033 k = v3_dot( m, m ) - r*r,
2034 c = dd*k - md*md;
2035
2036 if( fabsf(a) < 0.00001f )
2037 {
2038 /* Segment runs parallel to cylinder axis */
2039 return 0;
2040 }
2041
2042 f32 b = dd*mn - nd*md,
2043 discr = b*b - a*c;
2044
2045 if( discr < 0.0f )
2046 return 0; /* No real roots; no intersection */
2047
2048 *t = (-b - sqrtf(discr)) / a;
2049 if( *t < 0.0f )
2050 return 0; /* Intersection behind ray */
2051
2052 /* Check within cylinder segment */
2053 if( md + (*t)*nd < 0.0f )
2054 return 0;
2055
2056 if( md + (*t)*nd > dd )
2057 return 0;
2058
2059 /* Segment intersects cylinder between the endcaps; t is correct */
2060 return 1;
2061 }
2062
2063 /*
2064 * Time of intersection of sphere and triangle. Origin must be outside the
2065 * colliding area. This is a fairly long procedure.
2066 */
2067 static int spherecast_triangle( v3f tri[3],
2068 v3f co, v3f dir, f32 r, f32 *t, v3f n )
2069 {
2070 v3f sum[3];
2071 v3f v0, v1;
2072
2073 v3_sub( tri[1], tri[0], v0 );
2074 v3_sub( tri[2], tri[0], v1 );
2075 v3_cross( v0, v1, n );
2076 v3_normalize( n );
2077 v3_muladds( tri[0], n, r, sum[0] );
2078 v3_muladds( tri[1], n, r, sum[1] );
2079 v3_muladds( tri[2], n, r, sum[2] );
2080
2081 int hit = 0;
2082 f32 t_min = INFINITY,
2083 t1;
2084
2085 if( ray_tri( sum, co, dir, &t1 ) ){
2086 t_min = vg_minf( t_min, t1 );
2087 hit = 1;
2088 }
2089
2090 /*
2091 * Currently disabled; ray_sphere requires |d| = 1. it is not very important.
2092 */
2093 #if 0
2094 for( int i=0; i<3; i++ ){
2095 if( ray_sphere( tri[i], r, co, dir, &t1 ) ){
2096 t_min = vg_minf( t_min, t1 );
2097 hit = 1;
2098 }
2099 }
2100 #endif
2101
2102 for( int i=0; i<3; i++ ){
2103 int i0 = i,
2104 i1 = (i+1)%3;
2105
2106 if( ray_uncapped_finite_cylinder( tri[i0], tri[i1], r, co, dir, &t1 ) ){
2107 if( t1 < t_min ){
2108 t_min = t1;
2109
2110 v3f co1, ct, cx;
2111 v3_add( dir, co, co1 );
2112 v3_lerp( co, co1, t_min, ct );
2113
2114 closest_point_segment( tri[i0], tri[i1], ct, cx );
2115 v3_sub( ct, cx, n );
2116 v3_normalize( n );
2117 }
2118
2119 hit = 1;
2120 }
2121 }
2122
2123 *t = t_min;
2124 return hit;
2125 }
2126
2127 /*
2128 * -----------------------------------------------------------------------------
2129 * Section 5.e Curves
2130 * -----------------------------------------------------------------------------
2131 */
2132
2133 static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, f32 t, v3f p )
2134 {
2135 f32 tt = t*t,
2136 ttt = tt*t;
2137
2138 v3_muls( p1, ttt, p );
2139 v3_muladds( p, h1, 3.0f*tt -3.0f*ttt, p );
2140 v3_muladds( p, h0, 3.0f*ttt -6.0f*tt +3.0f*t, p );
2141 v3_muladds( p, p0, 3.0f*tt -ttt -3.0f*t +1.0f, p );
2142 }
2143
2144 static void eval_bezier3( v3f p0, v3f p1, v3f p2, f32 t, v3f p )
2145 {
2146 f32 u = 1.0f-t;
2147
2148 v3_muls( p0, u*u, p );
2149 v3_muladds( p, p1, 2.0f*u*t, p );
2150 v3_muladds( p, p2, t*t, p );
2151 }
2152
2153 /*
2154 * -----------------------------------------------------------------------------
2155 * Section 5.f Volumes
2156 * -----------------------------------------------------------------------------
2157 */
2158
2159 static float vg_sphere_volume( float radius ){
2160 float r3 = radius*radius*radius;
2161 return (4.0f/3.0f) * VG_PIf * r3;
2162 }
2163
2164 /*
2165 * -----------------------------------------------------------------------------
2166 * Section 6.a PSRNG and some distributions
2167 * -----------------------------------------------------------------------------
2168 */
2169
2170 /* An implementation of the MT19937 Algorithm for the Mersenne Twister
2171 * by Evan Sultanik. Based upon the pseudocode in: M. Matsumoto and
2172 * T. Nishimura, "Mersenne Twister: A 623-dimensionally
2173 * equidistributed uniform pseudorandom number generator," ACM
2174 * Transactions on Modeling and Computer Simulation Vol. 8, No. 1,
2175 * January pp.3-30 1998.
2176 *
2177 * http://www.sultanik.com/Mersenne_twister
2178 * https://github.com/ESultanik/mtwister/blob/master/mtwister.c
2179 */
2180
2181 #define MT_UPPER_MASK 0x80000000
2182 #define MT_LOWER_MASK 0x7fffffff
2183 #define MT_TEMPERING_MASK_B 0x9d2c5680
2184 #define MT_TEMPERING_MASK_C 0xefc60000
2185
2186 #define MT_STATE_VECTOR_LENGTH 624
2187
2188 /* changes to STATE_VECTOR_LENGTH also require changes to this */
2189 #define MT_STATE_VECTOR_M 397
2190
2191 struct {
2192 u32 mt[MT_STATE_VECTOR_LENGTH];
2193 i32 index;
2194 }
2195 static vg_rand;
2196
2197 static void vg_rand_seed( unsigned long seed )
2198 {
2199 /* set initial seeds to mt[STATE_VECTOR_LENGTH] using the generator
2200 * from Line 25 of Table 1 in: Donald Knuth, "The Art of Computer
2201 * Programming," Vol. 2 (2nd Ed.) pp.102.
2202 */
2203 vg_rand.mt[0] = seed & 0xffffffff;
2204 for( vg_rand.index=1; vg_rand.index<MT_STATE_VECTOR_LENGTH; vg_rand.index++){
2205 vg_rand.mt[vg_rand.index] =
2206 (6069 * vg_rand.mt[vg_rand.index-1]) & 0xffffffff;
2207 }
2208 }
2209
2210 /*
2211 * Generates a pseudo-randomly generated long.
2212 */
2213 static u32 vg_randu32(void)
2214 {
2215 u32 y;
2216 /* mag[x] = x * 0x9908b0df for x = 0,1 */
2217 static u32 mag[2] = {0x0, 0x9908b0df};
2218 if( vg_rand.index >= MT_STATE_VECTOR_LENGTH || vg_rand.index < 0 ){
2219 /* generate STATE_VECTOR_LENGTH words at a time */
2220 int kk;
2221 if( vg_rand.index >= MT_STATE_VECTOR_LENGTH+1 || vg_rand.index < 0 ){
2222 vg_rand_seed( 4357 );
2223 }
2224 for( kk=0; kk<MT_STATE_VECTOR_LENGTH-MT_STATE_VECTOR_M; kk++ ){
2225 y = (vg_rand.mt[kk] & MT_UPPER_MASK) |
2226 (vg_rand.mt[kk+1] & MT_LOWER_MASK);
2227 vg_rand.mt[kk] = vg_rand.mt[kk+MT_STATE_VECTOR_M] ^
2228 (y >> 1) ^ mag[y & 0x1];
2229 }
2230 for( ; kk<MT_STATE_VECTOR_LENGTH-1; kk++ ){
2231 y = (vg_rand.mt[kk] & MT_UPPER_MASK) |
2232 (vg_rand.mt[kk+1] & MT_LOWER_MASK);
2233 vg_rand.mt[kk] =
2234 vg_rand.mt[ kk+(MT_STATE_VECTOR_M-MT_STATE_VECTOR_LENGTH)] ^
2235 (y >> 1) ^ mag[y & 0x1];
2236 }
2237 y = (vg_rand.mt[MT_STATE_VECTOR_LENGTH-1] & MT_UPPER_MASK) |
2238 (vg_rand.mt[0] & MT_LOWER_MASK);
2239 vg_rand.mt[MT_STATE_VECTOR_LENGTH-1] =
2240 vg_rand.mt[MT_STATE_VECTOR_M-1] ^ (y >> 1) ^ mag[y & 0x1];
2241 vg_rand.index = 0;
2242 }
2243 y = vg_rand.mt[vg_rand.index++];
2244 y ^= (y >> 11);
2245 y ^= (y << 7) & MT_TEMPERING_MASK_B;
2246 y ^= (y << 15) & MT_TEMPERING_MASK_C;
2247 y ^= (y >> 18);
2248 return y;
2249 }
2250
2251 /*
2252 * Generates a pseudo-randomly generated f64 in the range [0..1].
2253 */
2254 static inline f64 vg_randf64(void)
2255 {
2256 return (f64)vg_randu32()/(f64)0xffffffff;
2257 }
2258
2259 static inline f64 vg_randf64_range( f64 min, f64 max )
2260 {
2261 return vg_lerp( min, max, (f64)vg_randf64() );
2262 }
2263
2264 static inline void vg_rand_dir( v3f dir )
2265 {
2266 dir[0] = vg_randf64();
2267 dir[1] = vg_randf64();
2268 dir[2] = vg_randf64();
2269
2270 v3_muls( dir, 2.0f, dir );
2271 v3_sub( dir, (v3f){1.0f,1.0f,1.0f}, dir );
2272
2273 v3_normalize( dir );
2274 }
2275
2276 static inline void vg_rand_sphere( v3f co )
2277 {
2278 vg_rand_dir(co);
2279 v3_muls( co, cbrtf( vg_randf64() ), co );
2280 }
2281
2282 #endif /* VG_M_H */