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