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