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