e75e8dc38ce7cb49d25e025ec4fa27b6d147d605
1 /* Copyright (C) 2021-2022 Harry Godden (hgn) - All Rights Reserved */
6 #include "vg_platform.h"
10 #define VG_PIf 3.14159265358979323846264338327950288f
11 #define VG_TAUf 6.28318530717958647692528676655900576f
13 static u32
vg_ftu32( float a
)
15 u32
*ptr
= (u32
*)(&a
);
19 static int vg_isinff( float a
)
21 return ((vg_ftu32(a
)) & 0x7FFFFFFFU
) == 0x7F800000U
;
24 static int vg_isnanf( float a
)
26 return !vg_isinff(a
) && ((vg_ftu32(a
)) & 0x7F800000U
) == 0x7F800000U
;
29 static int vg_validf( float a
)
31 return ((vg_ftu32(a
)) & 0x7F800000U
) != 0x7F800000U
;
34 static inline float vg_minf( float a
, float b
)
39 static inline float vg_maxf( float a
, float b
)
44 static inline float vg_clampf( float a
, float min
, float max
)
46 return vg_minf( max
, vg_maxf( a
, min
) );
49 static inline float vg_signf( float a
)
51 return a
< 0.0f
? -1.0f
: 1.0f
;
54 static inline float vg_fractf( float a
)
56 return a
- floorf( a
);
60 __attribute__ ((deprecated
))
61 static float stable_force( float current
, float diff
)
63 float fnew
= current
+ diff
;
65 if( fnew
* current
< 0.0f
)
71 static float vg_cfrictf( float current
, float F
)
73 return -vg_signf(current
) * vg_minf( F
, fabsf(current
) );
76 static inline int vg_min( int a
, int b
)
81 static inline int vg_max( int a
, int b
)
86 static inline float vg_rad( float deg
)
88 return deg
* VG_PIf
/ 180.0f
;
94 static inline void v2_copy( v2f a
, v2f b
)
96 b
[0] = a
[0]; b
[1] = a
[1];
99 static inline void v2_zero( v2f a
)
101 a
[0] = 0.f
; a
[1] = 0.f
;
104 static inline void v2i_copy( v2i a
, v2i b
)
106 b
[0] = a
[0]; b
[1] = a
[1];
109 static inline int v2i_eq( v2i a
, v2i b
)
111 return ((a
[0] == b
[0]) && (a
[1] == b
[1]));
114 static inline void v2i_add( v2i a
, v2i b
, v2i d
)
116 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
119 static inline void v2i_sub( v2i a
, v2i b
, v2i d
)
121 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
124 static inline void v2_minv( v2f a
, v2f b
, v2f dest
)
126 dest
[0] = vg_minf(a
[0], b
[0]);
127 dest
[1] = vg_minf(a
[1], b
[1]);
130 static inline void v2_maxv( v2f a
, v2f b
, v2f dest
)
132 dest
[0] = vg_maxf(a
[0], b
[0]);
133 dest
[1] = vg_maxf(a
[1], b
[1]);
136 static inline void v2_sub( v2f a
, v2f b
, v2f d
)
138 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
141 static inline float v2_dot( v2f a
, v2f b
)
143 return a
[0] * b
[0] + a
[1] * b
[1];
146 static inline float v2_cross( v2f a
, v2f b
)
148 return a
[0]*b
[1] - a
[1]*b
[0];
151 static inline void v2_add( v2f a
, v2f b
, v2f d
)
153 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
156 static inline void v2_abs( v2f a
, v2f d
)
158 d
[0] = fabsf( a
[0] );
159 d
[1] = fabsf( a
[1] );
162 static inline void v2_muls( v2f a
, float s
, v2f d
)
164 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
;
167 static inline void v2_divs( v2f a
, float s
, v2f d
)
169 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
;
172 static inline void v2_mul( v2f a
, v2f b
, v2f d
)
178 static inline void v2_div( v2f a
, v2f b
, v2f d
)
180 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1];
183 static inline void v2_muladd( v2f a
, v2f b
, v2f s
, v2f d
)
185 d
[0] = a
[0]+b
[0]*s
[0];
186 d
[1] = a
[1]+b
[1]*s
[1];
189 static inline void v2_muladds( v2f a
, v2f b
, float s
, v2f d
)
195 static inline float v2_length2( v2f a
)
197 return a
[0]*a
[0] + a
[1]*a
[1];
200 static inline float v2_length( v2f a
)
202 return sqrtf( v2_length2( a
) );
205 static inline float v2_dist2( v2f a
, v2f b
)
208 v2_sub( a
, b
, delta
);
209 return v2_length2( delta
);
212 static inline float v2_dist( v2f a
, v2f b
)
214 return sqrtf( v2_dist2( a
, b
) );
217 static inline void v2_lerp( v2f a
, v2f b
, float t
, v2f d
)
219 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
220 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
223 static inline void v2_normalize( v2f a
)
225 v2_muls( a
, 1.0f
/ v2_length( a
), a
);
228 static void v2_normalize_clamp( v2f a
)
230 float l2
= v2_length2( a
);
232 v2_muls( a
, 1.0f
/sqrtf(l2
), a
);
235 static inline void v2_floor( v2f a
, v2f b
)
237 b
[0] = floorf( a
[0] );
238 b
[1] = floorf( a
[1] );
241 static inline void v2_fill( v2f a
, float v
)
247 /* copysign of b to a */
248 static inline void v2_copysign( v2f a
, v2f b
)
250 a
[0] = copysignf( a
[0], b
[0] );
251 a
[1] = copysignf( a
[1], b
[1] );
257 static inline void v3_zero( v3f a
)
259 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
;
262 static inline void v3_copy( v3f a
, v3f b
)
264 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2];
267 static inline void v3_add( v3f a
, v3f b
, v3f d
)
269 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
272 static inline void v3i_add( v3i a
, v3i b
, v3i d
)
274 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
277 static inline void v4_add( v4f a
, v4f b
, v4f d
)
285 static inline void v3_sub( v3f a
, v3f b
, v3f d
)
287 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
290 static inline void v3i_sub( v3i a
, v3i b
, v3i d
)
292 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
295 static inline void v3_mul( v3f a
, v3f b
, v3f d
)
297 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1]; d
[2] = a
[2]*b
[2];
300 static inline void v3_div( v3f a
, v3f b
, v3f d
)
302 d
[0] = b
[0]!=0.0f
? a
[0]/b
[0]: INFINITY
;
303 d
[1] = b
[1]!=0.0f
? a
[1]/b
[1]: INFINITY
;
304 d
[2] = b
[2]!=0.0f
? a
[2]/b
[2]: INFINITY
;
307 static inline void v3_muls( v3f a
, float s
, v3f d
)
309 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
;
312 static inline void v3_fill( v3f a
, float v
)
319 static inline void v3_divs( v3f a
, float s
, v3f d
)
322 v3_fill( d
, INFINITY
);
331 static inline void v3_muladds( v3f a
, v3f b
, float s
, v3f d
)
333 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
; d
[2] = a
[2]+b
[2]*s
;
336 static inline void v3_muladd( v2f a
, v2f b
, v2f s
, v2f d
)
338 d
[0] = a
[0]+b
[0]*s
[0];
339 d
[1] = a
[1]+b
[1]*s
[1];
340 d
[2] = a
[2]+b
[2]*s
[2];
343 static inline float v3_dot( v3f a
, v3f b
)
345 return a
[0] * b
[0] + a
[1] * b
[1] + a
[2] * b
[2];
348 static inline void v3_cross( v3f a
, v3f b
, v3f dest
)
351 d
[0] = a
[1]*b
[2] - a
[2]*b
[1];
352 d
[1] = a
[2]*b
[0] - a
[0]*b
[2];
353 d
[2] = a
[0]*b
[1] - a
[1]*b
[0];
357 static inline float v3_length2( v3f a
)
359 return v3_dot( a
, a
);
362 static inline float v3_length( v3f a
)
364 return sqrtf( v3_length2( a
) );
367 static inline float v3_dist2( v3f a
, v3f b
)
370 v3_sub( a
, b
, delta
);
371 return v3_length2( delta
);
374 static inline float v3_dist( v3f a
, v3f b
)
376 return sqrtf( v3_dist2( a
, b
) );
379 static inline void v3_normalize( v3f a
)
381 v3_muls( a
, 1.f
/ v3_length( a
), a
);
384 static inline float vg_lerpf( float a
, float b
, float t
)
389 static inline double vg_lerp( double a
, double b
, double t
)
394 /* correctly lerp around circular period -pi -> pi */
395 static float vg_alerpf( float a
, float b
, float t
)
397 float d
= fmodf( b
-a
, VG_TAUf
),
398 s
= fmodf( 2.0f
*d
, VG_TAUf
) - d
;
402 static inline void v3_lerp( v3f a
, v3f b
, float t
, v3f d
)
404 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
405 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
406 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
409 static inline void v3_minv( v3f a
, v3f b
, v3f dest
)
411 dest
[0] = vg_minf(a
[0], b
[0]);
412 dest
[1] = vg_minf(a
[1], b
[1]);
413 dest
[2] = vg_minf(a
[2], b
[2]);
416 static inline void v3_maxv( v3f a
, v3f b
, v3f dest
)
418 dest
[0] = vg_maxf(a
[0], b
[0]);
419 dest
[1] = vg_maxf(a
[1], b
[1]);
420 dest
[2] = vg_maxf(a
[2], b
[2]);
423 static inline float v3_minf( v3f a
)
425 return vg_minf( vg_minf( a
[0], a
[1] ), a
[2] );
428 static inline float v3_maxf( v3f a
)
430 return vg_maxf( vg_maxf( a
[0], a
[1] ), a
[2] );
433 static inline void v3_floor( v3f a
, v3f b
)
435 b
[0] = floorf( a
[0] );
436 b
[1] = floorf( a
[1] );
437 b
[2] = floorf( a
[2] );
440 static inline void v3_ceil( v3f a
, v3f b
)
442 b
[0] = ceilf( a
[0] );
443 b
[1] = ceilf( a
[1] );
444 b
[2] = ceilf( a
[2] );
447 static inline void v3_negate( v3f a
, v3f b
)
454 static inline void v3_rotate( v3f v
, float angle
, v3f axis
, v3f d
)
465 v3_cross( k
, v
, v2
);
466 v3_muls( v2
, s
, v2
);
467 v3_add( v1
, v2
, v1
);
468 v3_muls( k
, v3_dot(k
, v
) * (1.0f
- c
), v2
);
475 static inline void v4_copy( v4f a
, v4f b
)
477 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2]; b
[3] = a
[3];
480 static inline void v4_zero( v4f a
)
482 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
; a
[3] = 0.f
;
485 static inline void v4_muls( v4f a
, float s
, v4f d
)
493 static inline void v4_muladds( v4f a
, v4f b
, float s
, v4f d
)
501 static inline void v4_lerp( v4f a
, v4f b
, float t
, v4f d
)
503 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
504 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
505 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
506 d
[3] = a
[3] + t
*(b
[3]-a
[3]);
509 static inline float v4_dot( v4f a
, v4f b
)
511 return a
[0]*b
[0] + a
[1]*b
[1] + a
[2]*b
[2] + a
[3]*b
[3];
514 static inline float v4_length( v4f a
)
516 return sqrtf( v4_dot(a
,a
) );
523 #define M2X2_INDENTIY {{1.0f, 0.0f, }, \
526 #define M2X2_ZERO {{0.0f, 0.0f, }, \
529 static inline void m2x2_copy( m2x2f a
, m2x2f b
)
531 v2_copy( a
[0], b
[0] );
532 v2_copy( a
[1], b
[1] );
535 static inline void m2x2_identity( m2x2f a
)
537 m2x2f id
= M2X2_INDENTIY
;
541 static inline void m2x2_create_rotation( m2x2f a
, float theta
)
558 #define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
559 { 0.0f, 1.0f, 0.0f, },\
560 { 0.0f, 0.0f, 1.0f, }}
562 #define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\
563 { 0.0f, 0.0f, 0.0f, },\
564 { 0.0f, 0.0f, 0.0f, }}
567 /* a X b == [b]T a == ...*/
568 static void m3x3_skew_symetric( m3x3f a
, v3f v
)
581 static void m3x3_add( m3x3f a
, m3x3f b
, m3x3f d
)
583 v3_add( a
[0], b
[0], d
[0] );
584 v3_add( a
[1], b
[1], d
[1] );
585 v3_add( a
[2], b
[2], d
[2] );
588 static inline void m3x3_copy( m3x3f a
, m3x3f b
)
590 v3_copy( a
[0], b
[0] );
591 v3_copy( a
[1], b
[1] );
592 v3_copy( a
[2], b
[2] );
595 static inline void m3x3_identity( m3x3f a
)
597 m3x3f id
= M3X3_IDENTITY
;
601 static void m3x3_diagonal( m3x3f a
, float v
)
609 static inline void m3x3_zero( m3x3f a
)
615 static inline void m3x3_inv( m3x3f src
, m3x3f dest
)
617 float a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
618 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
619 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
626 dest
[0][0] = (e
*i
-h
*f
)*det
;
627 dest
[0][1] = -(b
*i
-c
*h
)*det
;
628 dest
[0][2] = (b
*f
-c
*e
)*det
;
629 dest
[1][0] = -(d
*i
-f
*g
)*det
;
630 dest
[1][1] = (a
*i
-c
*g
)*det
;
631 dest
[1][2] = -(a
*f
-d
*c
)*det
;
632 dest
[2][0] = (d
*h
-g
*e
)*det
;
633 dest
[2][1] = -(a
*h
-g
*b
)*det
;
634 dest
[2][2] = (a
*e
-d
*b
)*det
;
637 static float m3x3_det( m3x3f m
)
639 return m
[0][0] * (m
[1][1] * m
[2][2] - m
[2][1] * m
[1][2])
640 - m
[0][1] * (m
[1][0] * m
[2][2] - m
[1][2] * m
[2][0])
641 + m
[0][2] * (m
[1][0] * m
[2][1] - m
[1][1] * m
[2][0]);
644 static inline void m3x3_transpose( m3x3f src
, m3x3f dest
)
646 float a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
647 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
648 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
661 static inline void m3x3_mul( m3x3f a
, m3x3f b
, m3x3f d
)
663 float a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
664 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
665 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
667 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
668 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
669 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2];
671 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
672 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
673 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
674 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
675 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
676 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
677 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
678 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
679 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
682 static inline void m3x3_mulv( m3x3f m
, v3f v
, v3f d
)
686 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2];
687 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2];
688 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2];
693 static inline void m3x3_projection( m3x3f dst
,
694 float const left
, float const right
, float const bottom
, float const top
)
700 rl
= 1.0f
/ (right
- left
);
701 tb
= 1.0f
/ (top
- bottom
);
703 dst
[0][0] = 2.0f
* rl
;
704 dst
[1][1] = 2.0f
* tb
;
708 static inline void m3x3_translate( m3x3f m
, v3f v
)
710 m
[2][0] = m
[0][0] * v
[0] + m
[1][0] * v
[1] + m
[2][0];
711 m
[2][1] = m
[0][1] * v
[0] + m
[1][1] * v
[1] + m
[2][1];
712 m
[2][2] = m
[0][2] * v
[0] + m
[1][2] * v
[1] + m
[2][2];
715 static inline void m3x3_scale( m3x3f m
, v3f v
)
717 v3_muls( m
[0], v
[0], m
[0] );
718 v3_muls( m
[1], v
[1], m
[1] );
719 v3_muls( m
[2], v
[2], m
[2] );
722 static inline void m3x3_scalef( m3x3f m
, float f
)
729 static inline void m3x3_rotate( m3x3f m
, float angle
)
731 float m00
= m
[0][0], m10
= m
[1][0],
732 m01
= m
[0][1], m11
= m
[1][1],
733 m02
= m
[0][2], m12
= m
[1][2];
739 m
[0][0] = m00
* c
+ m10
* s
;
740 m
[0][1] = m01
* c
+ m11
* s
;
741 m
[0][2] = m02
* c
+ m12
* s
;
743 m
[1][0] = m00
* -s
+ m10
* c
;
744 m
[1][1] = m01
* -s
+ m11
* c
;
745 m
[1][2] = m02
* -s
+ m12
* c
;
748 static inline void box_addpt( boxf a
, v3f pt
)
750 v3_minv( a
[0], pt
, a
[0] );
751 v3_maxv( a
[1], pt
, a
[1] );
754 static inline void box_concat( boxf a
, boxf b
)
756 v3_minv( a
[0], b
[0], a
[0] );
757 v3_maxv( a
[1], b
[1], a
[1] );
760 static inline void box_copy( boxf a
, boxf b
)
762 v3_copy( a
[0], b
[0] );
763 v3_copy( a
[1], b
[1] );
766 static inline int box_overlap( boxf a
, boxf b
)
769 ( a
[0][0] <= b
[1][0] && a
[1][0] >= b
[0][0] ) &&
770 ( a
[0][1] <= b
[1][1] && a
[1][1] >= b
[0][1] ) &&
771 ( a
[0][2] <= b
[1][2] && a
[1][2] >= b
[0][2] )
775 static int box_within( boxf greater
, boxf lesser
)
778 v3_sub( lesser
[0], greater
[0], a
);
779 v3_sub( lesser
[1], greater
[1], b
);
781 if( (a
[0] >= 0.0f
) && (a
[1] >= 0.0f
) && (a
[2] >= 0.0f
) &&
782 (b
[0] <= 0.0f
) && (b
[1] <= 0.0f
) && (b
[2] <= 0.0f
) )
790 static inline void box_init_inf( boxf box
)
792 v3_fill( box
[0], INFINITY
);
793 v3_fill( box
[1], -INFINITY
);
796 int ray_aabb1( boxf box
, v3f co
, v3f dir_inv
, float dist
)
801 v3_sub( box
[0], co
, v0
);
802 v3_sub( box
[1], co
, v1
);
804 v3_mul( v0
, dir_inv
, v0
);
805 v3_mul( v1
, dir_inv
, v1
);
807 tmin
= vg_minf( v0
[0], v1
[0] );
808 tmax
= vg_maxf( v0
[0], v1
[0] );
809 tmin
= vg_maxf( tmin
, vg_minf( v0
[1], v1
[1] ));
810 tmax
= vg_minf( tmax
, vg_maxf( v0
[1], v1
[1] ));
811 tmin
= vg_maxf( tmin
, vg_minf( v0
[2], v1
[2] ));
812 tmax
= vg_minf( tmax
, vg_maxf( v0
[2], v1
[2] ));
814 return (tmax
>= tmin
) && (tmin
<= dist
) && (tmax
>= 0.0f
);
817 static inline void m4x3_lookat( m4x3f m
, v3f pos
, v3f target
, v3f up
)
820 v3_sub( target
, pos
, dir
);
823 v3_copy( dir
, m
[2] );
825 v3_cross( up
, m
[2], m
[0] );
826 v3_normalize( m
[0] );
828 v3_cross( m
[2], m
[0], m
[1] );
829 v3_copy( pos
, m
[3] );
836 #define M4X4_IDENTITY {{1.0f, 0.0f, 0.0f, 0.0f },\
837 { 0.0f, 1.0f, 0.0f, 0.0f },\
838 { 0.0f, 0.0f, 1.0f, 0.0f },\
839 { 0.0f, 0.0f, 0.0f, 1.0f }}
840 #define M4X4_ZERO {{0.0f, 0.0f, 0.0f, 0.0f },\
841 { 0.0f, 0.0f, 0.0f, 0.0f },\
842 { 0.0f, 0.0f, 0.0f, 0.0f },\
843 { 0.0f, 0.0f, 0.0f, 0.0f }}
845 static void m4x4_projection( m4x4f m
, float angle
,
846 float ratio
, float fnear
, float ffar
)
848 float scale
= tanf( angle
* 0.5f
* VG_PIf
/ 180.0f
) * fnear
,
854 m
[0][0] = 2.0f
* fnear
/ (r
- l
);
860 m
[1][1] = 2.0f
* fnear
/ (t
- b
);
864 m
[2][0] = (r
+ l
) / (r
- l
);
865 m
[2][1] = (t
+ b
) / (t
- b
);
866 m
[2][2] = -(ffar
+ fnear
) / (ffar
- fnear
);
871 m
[3][2] = -2.0f
* ffar
* fnear
/ (ffar
- fnear
);
875 static void m4x4_translate( m4x4f m
, v3f v
)
877 v4_muladds( m
[3], m
[0], v
[0], m
[3] );
878 v4_muladds( m
[3], m
[1], v
[1], m
[3] );
879 v4_muladds( m
[3], m
[2], v
[2], m
[3] );
882 static inline void m4x4_copy( m4x4f a
, m4x4f b
)
884 v4_copy( a
[0], b
[0] );
885 v4_copy( a
[1], b
[1] );
886 v4_copy( a
[2], b
[2] );
887 v4_copy( a
[3], b
[3] );
890 static inline void m4x4_identity( m4x4f a
)
892 m4x4f id
= M4X4_IDENTITY
;
896 static inline void m4x4_zero( m4x4f a
)
898 m4x4f zero
= M4X4_ZERO
;
899 m4x4_copy( zero
, a
);
902 static inline void m4x4_mul( m4x4f a
, m4x4f b
, m4x4f d
)
904 float a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2], a03
= a
[0][3],
905 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2], a13
= a
[1][3],
906 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2], a23
= a
[2][3],
907 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2], a33
= a
[3][3],
909 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2], b03
= b
[0][3],
910 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2], b13
= b
[1][3],
911 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2], b23
= b
[2][3],
912 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2], b33
= b
[3][3];
914 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
+ a30
*b03
;
915 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
+ a31
*b03
;
916 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
+ a32
*b03
;
917 d
[0][3] = a03
*b00
+ a13
*b01
+ a23
*b02
+ a33
*b03
;
918 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
+ a30
*b13
;
919 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
+ a31
*b13
;
920 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
+ a32
*b13
;
921 d
[1][3] = a03
*b10
+ a13
*b11
+ a23
*b12
+ a33
*b13
;
922 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
+ a30
*b23
;
923 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
+ a31
*b23
;
924 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
+ a32
*b23
;
925 d
[2][3] = a03
*b20
+ a13
*b21
+ a23
*b22
+ a33
*b23
;
926 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
*b33
;
927 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
*b33
;
928 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
*b33
;
929 d
[3][3] = a03
*b30
+ a13
*b31
+ a23
*b32
+ a33
*b33
;
932 static inline void m4x4_mulv( m4x4f m
, v4f v
, v4f d
)
936 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0]*v
[3];
937 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1]*v
[3];
938 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2]*v
[3];
939 res
[3] = m
[0][3]*v
[0] + m
[1][3]*v
[1] + m
[2][3]*v
[2] + m
[3][3]*v
[3];
944 static inline void m4x4_inv( m4x4f a
, m4x4f d
)
946 float a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2], a03
= a
[0][3],
947 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2], a13
= a
[1][3],
948 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2], a23
= a
[2][3],
949 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2], a33
= a
[3][3],
953 t
[0] = a22
*a33
- a32
*a23
;
954 t
[1] = a21
*a33
- a31
*a23
;
955 t
[2] = a21
*a32
- a31
*a22
;
956 t
[3] = a20
*a33
- a30
*a23
;
957 t
[4] = a20
*a32
- a30
*a22
;
958 t
[5] = a20
*a31
- a30
*a21
;
960 d
[0][0] = a11
*t
[0] - a12
*t
[1] + a13
*t
[2];
961 d
[1][0] =-(a10
*t
[0] - a12
*t
[3] + a13
*t
[4]);
962 d
[2][0] = a10
*t
[1] - a11
*t
[3] + a13
*t
[5];
963 d
[3][0] =-(a10
*t
[2] - a11
*t
[4] + a12
*t
[5]);
965 d
[0][1] =-(a01
*t
[0] - a02
*t
[1] + a03
*t
[2]);
966 d
[1][1] = a00
*t
[0] - a02
*t
[3] + a03
*t
[4];
967 d
[2][1] =-(a00
*t
[1] - a01
*t
[3] + a03
*t
[5]);
968 d
[3][1] = a00
*t
[2] - a01
*t
[4] + a02
*t
[5];
970 t
[0] = a12
*a33
- a32
*a13
;
971 t
[1] = a11
*a33
- a31
*a13
;
972 t
[2] = a11
*a32
- a31
*a12
;
973 t
[3] = a10
*a33
- a30
*a13
;
974 t
[4] = a10
*a32
- a30
*a12
;
975 t
[5] = a10
*a31
- a30
*a11
;
977 d
[0][2] = a01
*t
[0] - a02
*t
[1] + a03
*t
[2];
978 d
[1][2] =-(a00
*t
[0] - a02
*t
[3] + a03
*t
[4]);
979 d
[2][2] = a00
*t
[1] - a01
*t
[3] + a03
*t
[5];
980 d
[3][2] =-(a00
*t
[2] - a01
*t
[4] + a02
*t
[5]);
982 t
[0] = a12
*a23
- a22
*a13
;
983 t
[1] = a11
*a23
- a21
*a13
;
984 t
[2] = a11
*a22
- a21
*a12
;
985 t
[3] = a10
*a23
- a20
*a13
;
986 t
[4] = a10
*a22
- a20
*a12
;
987 t
[5] = a10
*a21
- a20
*a11
;
989 d
[0][3] =-(a01
*t
[0] - a02
*t
[1] + a03
*t
[2]);
990 d
[1][3] = a00
*t
[0] - a02
*t
[3] + a03
*t
[4];
991 d
[2][3] =-(a00
*t
[1] - a01
*t
[3] + a03
*t
[5]);
992 d
[3][3] = a00
*t
[2] - a01
*t
[4] + a02
*t
[5];
994 det
= 1.0f
/ (a00
*d
[0][0] + a01
*d
[1][0] + a02
*d
[2][0] + a03
*d
[3][0]);
995 v4_muls( d
[0], det
, d
[0] );
996 v4_muls( d
[1], det
, d
[1] );
997 v4_muls( d
[2], det
, d
[2] );
998 v4_muls( d
[3], det
, d
[3] );
1002 * Planes (double precision)
1004 static inline void tri_to_plane( double a
[3], double b
[3],
1005 double c
[3], double p
[4] )
1011 edge0
[0] = b
[0] - a
[0];
1012 edge0
[1] = b
[1] - a
[1];
1013 edge0
[2] = b
[2] - a
[2];
1015 edge1
[0] = c
[0] - a
[0];
1016 edge1
[1] = c
[1] - a
[1];
1017 edge1
[2] = c
[2] - a
[2];
1019 p
[0] = edge0
[1] * edge1
[2] - edge0
[2] * edge1
[1];
1020 p
[1] = edge0
[2] * edge1
[0] - edge0
[0] * edge1
[2];
1021 p
[2] = edge0
[0] * edge1
[1] - edge0
[1] * edge1
[0];
1023 l
= sqrt(p
[0] * p
[0] + p
[1] * p
[1] + p
[2] * p
[2]);
1024 p
[3] = (p
[0] * a
[0] + p
[1] * a
[1] + p
[2] * a
[2]) / l
;
1031 static int plane_intersect3( v4f a
, v4f b
, v4f c
, v3f p
)
1033 float const epsilon
= 1e-6f
;
1036 v3_cross( a
, b
, x
);
1037 float d
= v3_dot( x
, c
);
1039 if( (d
< epsilon
) && (d
> -epsilon
) ) return 0;
1042 v3_cross( b
, c
, v0
);
1043 v3_cross( c
, a
, v1
);
1044 v3_cross( a
, b
, v2
);
1046 v3_muls( v0
, a
[3], p
);
1047 v3_muladds( p
, v1
, b
[3], p
);
1048 v3_muladds( p
, v2
, c
[3], p
);
1054 int plane_intersect2( v4f a
, v4f b
, v3f p
, v3f n
)
1056 float const epsilon
= 1e-6f
;
1059 v3_cross( a
, b
, c
);
1060 float d
= v3_length2( c
);
1062 if( (d
< epsilon
) && (d
> -epsilon
) )
1066 v3_cross( c
, b
, v0
);
1067 v3_cross( a
, c
, v1
);
1069 v3_muls( v0
, a
[3], vx
);
1070 v3_muladds( vx
, v1
, b
[3], vx
);
1071 v3_divs( vx
, d
, p
);
1077 static int plane_segment( v4f plane
, v3f a
, v3f b
, v3f co
)
1079 float d0
= v3_dot( a
, plane
) - plane
[3],
1080 d1
= v3_dot( b
, plane
) - plane
[3];
1084 float tot
= 1.0f
/( fabsf(d0
)+fabsf(d1
) );
1086 v3_muls( a
, fabsf(d1
) * tot
, co
);
1087 v3_muladds( co
, b
, fabsf(d0
) * tot
, co
);
1094 static inline double plane_polarity( double p
[4], double a
[3] )
1097 (a
[0] * p
[0] + a
[1] * p
[1] + a
[2] * p
[2])
1098 -(p
[0]*p
[3] * p
[0] + p
[1]*p
[3] * p
[1] + p
[2]*p
[3] * p
[2])
1104 static inline void q_identity( v4f q
)
1106 q
[0] = 0.0f
; q
[1] = 0.0f
; q
[2] = 0.0f
; q
[3] = 1.0f
;
1109 static inline void q_axis_angle( v4f q
, v3f axis
, float angle
)
1111 float a
= angle
*0.5f
,
1121 static inline void q_mul( v4f q
, v4f q1
, v4f d
)
1124 t
[0] = q
[3]*q1
[0] + q
[0]*q1
[3] + q
[1]*q1
[2] - q
[2]*q1
[1];
1125 t
[1] = q
[3]*q1
[1] - q
[0]*q1
[2] + q
[1]*q1
[3] + q
[2]*q1
[0];
1126 t
[2] = q
[3]*q1
[2] + q
[0]*q1
[1] - q
[1]*q1
[0] + q
[2]*q1
[3];
1127 t
[3] = q
[3]*q1
[3] - q
[0]*q1
[0] - q
[1]*q1
[1] - q
[2]*q1
[2];
1131 static inline void q_normalize( v4f q
)
1133 float s
= 1.0f
/ sqrtf(v4_dot(q
,q
));
1140 static inline void q_inv( v4f q
, v4f d
)
1142 float s
= 1.0f
/ v4_dot(q
,q
);
1149 static inline void q_nlerp( v4f a
, v4f b
, float t
, v4f d
)
1151 if( v4_dot(a
,b
) < 0.0f
){
1152 v4_muls( b
, -1.0f
, d
);
1153 v4_lerp( a
, d
, t
, d
);
1156 v4_lerp( a
, b
, t
, d
);
1161 static void euler_m3x3( v3f angles
, m3x3f d
)
1163 float cosY
= cosf( angles
[0] ),
1164 sinY
= sinf( angles
[0] ),
1165 cosP
= cosf( angles
[1] ),
1166 sinP
= sinf( angles
[1] ),
1167 cosR
= cosf( angles
[2] ),
1168 sinR
= sinf( angles
[2] );
1170 d
[2][0] = -sinY
* cosP
;
1172 d
[2][2] = cosY
* cosP
;
1174 d
[0][0] = cosY
* cosR
;
1176 d
[0][2] = sinY
* cosR
;
1178 v3_cross( d
[0], d
[2], d
[1] );
1181 static inline void q_m3x3( v4f q
, m3x3f d
)
1185 s
= l
> 0.0f
? 2.0f
/l
: 0.0f
,
1187 xx
= s
*q
[0]*q
[0], xy
= s
*q
[0]*q
[1], wx
= s
*q
[3]*q
[0],
1188 yy
= s
*q
[1]*q
[1], yz
= s
*q
[1]*q
[2], wy
= s
*q
[3]*q
[1],
1189 zz
= s
*q
[2]*q
[2], xz
= s
*q
[0]*q
[2], wz
= s
*q
[3]*q
[2];
1191 d
[0][0] = 1.0f
- yy
- zz
;
1192 d
[1][1] = 1.0f
- xx
- zz
;
1193 d
[2][2] = 1.0f
- xx
- yy
;
1202 static void m3x3_q( m3x3f m
, v4f q
)
1204 float diag
, r
, rinv
;
1206 diag
= m
[0][0] + m
[1][1] + m
[2][2];
1209 r
= sqrtf( 1.0f
+ diag
);
1211 q
[0] = rinv
* (m
[1][2] - m
[2][1]);
1212 q
[1] = rinv
* (m
[2][0] - m
[0][2]);
1213 q
[2] = rinv
* (m
[0][1] - m
[1][0]);
1216 else if( m
[0][0] >= m
[1][1] && m
[0][0] >= m
[2][2] )
1218 r
= sqrtf( 1.0f
- m
[1][1] - m
[2][2] + m
[0][0] );
1221 q
[1] = rinv
* (m
[0][1] + m
[1][0]);
1222 q
[2] = rinv
* (m
[0][2] + m
[2][0]);
1223 q
[3] = rinv
* (m
[1][2] - m
[2][1]);
1225 else if( m
[1][1] >= m
[2][2] )
1227 r
= sqrtf( 1.0f
- m
[0][0] - m
[2][2] + m
[1][1] );
1229 q
[0] = rinv
* (m
[0][1] + m
[1][0]);
1231 q
[2] = rinv
* (m
[1][2] + m
[2][1]);
1232 q
[3] = rinv
* (m
[2][0] - m
[0][2]);
1236 r
= sqrtf( 1.0f
- m
[0][0] - m
[1][1] + m
[2][2] );
1238 q
[0] = rinv
* (m
[0][2] + m
[2][0]);
1239 q
[1] = rinv
* (m
[1][2] + m
[2][1]);
1241 q
[3] = rinv
* (m
[0][1] - m
[1][0]);
1245 static void q_mulv( v4f q
, v3f v
, v3f d
)
1249 v3_muls( q
, 2.0f
*v3_dot(q
,v
), v1
);
1250 v3_muls( v
, q
[3]*q
[3] - v3_dot(q
,q
), v2
);
1251 v3_add( v1
, v2
, v1
);
1252 v3_cross( q
, v
, v2
);
1253 v3_muls( v2
, 2.0f
*q
[3], v2
);
1254 v3_add( v1
, v2
, d
);
1259 k_contact_type_default
,
1260 k_contact_type_disabled
,
1268 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
1269 { 0.0f, 1.0f, 0.0f, },\
1270 { 0.0f, 0.0f, 1.0f, },\
1271 { 0.0f, 0.0f, 0.0f }}
1273 static inline void m4x3_to_3x3( m4x3f a
, m3x3f b
)
1275 v3_copy( a
[0], b
[0] );
1276 v3_copy( a
[1], b
[1] );
1277 v3_copy( a
[2], b
[2] );
1280 static inline void m4x3_invert_affine( m4x3f a
, m4x3f b
)
1282 m3x3_transpose( a
, b
);
1283 m3x3_mulv( b
, a
[3], b
[3] );
1284 v3_negate( b
[3], b
[3] );
1287 static void m4x3_invert_full( m4x3f src
, m4x3f dst
)
1291 a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
1292 e
= src
[1][0], f
= src
[1][1], g
= src
[1][2],
1293 i
= src
[2][0], j
= src
[2][1], k
= src
[2][2],
1294 m
= src
[3][0], n
= src
[3][1], o
= src
[3][2];
1300 dst
[0][0] = f
*k
- g
*j
;
1301 dst
[1][0] =-(e
*k
- g
*i
);
1302 dst
[2][0] = e
*j
- f
*i
;
1303 dst
[3][0] =-(e
*t2
- f
*t4
+ g
*t5
);
1305 dst
[0][1] =-(b
*k
- c
*j
);
1306 dst
[1][1] = a
*k
- c
*i
;
1307 dst
[2][1] =-(a
*j
- b
*i
);
1308 dst
[3][1] = a
*t2
- b
*t4
+ c
*t5
;
1314 dst
[0][2] = b
*g
- c
*f
;
1315 dst
[1][2] =-(a
*g
- c
*e
);
1316 dst
[2][2] = a
*f
- b
*e
;
1317 dst
[3][2] =-(a
*t2
- b
*t4
+ c
* t5
);
1319 det
= 1.0f
/ (a
* dst
[0][0] + b
* dst
[1][0] + c
* dst
[2][0]);
1320 v3_muls( dst
[0], det
, dst
[0] );
1321 v3_muls( dst
[1], det
, dst
[1] );
1322 v3_muls( dst
[2], det
, dst
[2] );
1323 v3_muls( dst
[3], det
, dst
[3] );
1326 static inline void m4x3_copy( m4x3f a
, m4x3f b
)
1328 v3_copy( a
[0], b
[0] );
1329 v3_copy( a
[1], b
[1] );
1330 v3_copy( a
[2], b
[2] );
1331 v3_copy( a
[3], b
[3] );
1334 static inline void m4x3_identity( m4x3f a
)
1336 m4x3f id
= M4X3_IDENTITY
;
1340 static void m4x3_mul( m4x3f a
, m4x3f b
, m4x3f d
)
1343 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
1344 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
1345 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
1346 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2],
1347 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
1348 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
1349 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2],
1350 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2];
1352 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
1353 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
1354 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
1355 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
1356 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
1357 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
1358 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
1359 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
1360 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
1361 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
;
1362 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
;
1363 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
;
1366 static inline void m4x3_mulv( m4x3f m
, v3f v
, v3f d
)
1370 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0];
1371 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1];
1372 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2];
1378 * Transform plane ( xyz, distance )
1380 static void m4x3_mulp( m4x3f m
, v4f p
, v4f d
)
1384 v3_muls( p
, p
[3], o
);
1385 m4x3_mulv( m
, o
, o
);
1386 m3x3_mulv( m
, p
, d
);
1388 d
[3] = v3_dot( o
, d
);
1395 static void m4x3_translate( m4x3f m
, v3f v
)
1397 v3_muladds( m
[3], m
[0], v
[0], m
[3] );
1398 v3_muladds( m
[3], m
[1], v
[1], m
[3] );
1399 v3_muladds( m
[3], m
[2], v
[2], m
[3] );
1402 static void m4x3_rotate_x( m4x3f m
, float angle
)
1404 m4x3f t
= M4X3_IDENTITY
;
1415 m4x3_mul( m
, t
, m
);
1418 static void m4x3_rotate_y( m4x3f m
, float angle
)
1420 m4x3f t
= M4X3_IDENTITY
;
1431 m4x3_mul( m
, t
, m
);
1434 static void m4x3_rotate_z( m4x3f m
, float angle
)
1436 m4x3f t
= M4X3_IDENTITY
;
1447 m4x3_mul( m
, t
, m
);
1450 static void m4x3_expand( m4x3f m
, m4x4f d
)
1452 v3_copy( m
[0], d
[0] );
1453 v3_copy( m
[1], d
[1] );
1454 v3_copy( m
[2], d
[2] );
1455 v3_copy( m
[3], d
[3] );
1462 static void m4x3_decompose( m4x3f m
, v3f co
, v4f q
, v3f s
)
1464 v3_copy( m
[3], co
);
1465 s
[0] = v3_length(m
[0]);
1466 s
[1] = v3_length(m
[1]);
1467 s
[2] = v3_length(m
[2]);
1470 v3_divs( m
[0], s
[0], rot
[0] );
1471 v3_divs( m
[1], s
[1], rot
[1] );
1472 v3_divs( m
[2], s
[2], rot
[2] );
1477 static void m4x3_expand_aabb_point( m4x3f m
, boxf box
, v3f point
)
1480 m4x3_mulv( m
, point
, v
);
1482 v3_minv( box
[0], v
, box
[0] );
1483 v3_maxv( box
[1], v
, box
[1] );
1486 static void m4x3_transform_aabb( m4x3f m
, boxf box
)
1490 v3_copy( box
[0], a
);
1491 v3_copy( box
[1], b
);
1492 v3_fill( box
[0], INFINITY
);
1493 v3_fill( box
[1], -INFINITY
);
1495 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], a
[1], a
[2] } );
1496 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], a
[2] } );
1497 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], a
[2] } );
1498 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], a
[2] } );
1500 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], a
[1], b
[2] } );
1501 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], b
[2] } );
1502 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], b
[2] } );
1503 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], b
[2] } );
1507 * -----------------------------------------------------------------------------
1508 * Closest point functions
1509 * -----------------------------------------------------------------------------
1513 * These closest point tests were learned from Real-Time Collision Detection by
1516 VG_STATIC
float closest_segment_segment( v3f p1
, v3f q1
, v3f p2
, v3f q2
,
1517 float *s
, float *t
, v3f c1
, v3f c2
)
1520 v3_sub( q1
, p1
, d1
);
1521 v3_sub( q2
, p2
, d2
);
1522 v3_sub( p1
, p2
, r
);
1524 float a
= v3_length2( d1
),
1525 e
= v3_length2( d2
),
1526 f
= v3_dot( d2
, r
);
1528 const float kEpsilon
= 0.0001f
;
1530 if( a
<= kEpsilon
&& e
<= kEpsilon
)
1538 v3_sub( c1
, c2
, v0
);
1540 return v3_length2( v0
);
1546 *t
= vg_clampf( f
/ e
, 0.0f
, 1.0f
);
1550 float c
= v3_dot( d1
, r
);
1554 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
1558 float b
= v3_dot(d1
,d2
),
1563 *s
= vg_clampf((b
*f
- c
*e
)/d
, 0.0f
, 1.0f
);
1570 *t
= (b
*(*s
)+f
) / e
;
1575 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
1577 else if( *t
> 1.0f
)
1580 *s
= vg_clampf((b
-c
)/a
,0.0f
,1.0f
);
1585 v3_muladds( p1
, d1
, *s
, c1
);
1586 v3_muladds( p2
, d2
, *t
, c2
);
1589 v3_sub( c1
, c2
, v0
);
1590 return v3_length2( v0
);
1593 VG_STATIC
int point_inside_aabb( boxf box
, v3f point
)
1595 if((point
[0]<=box
[1][0]) && (point
[1]<=box
[1][1]) && (point
[2]<=box
[1][2]) &&
1596 (point
[0]>=box
[0][0]) && (point
[1]>=box
[0][1]) && (point
[2]>=box
[0][2]) )
1602 VG_STATIC
void closest_point_aabb( v3f p
, boxf box
, v3f dest
)
1604 v3_maxv( p
, box
[0], dest
);
1605 v3_minv( dest
, box
[1], dest
);
1608 VG_STATIC
void closest_point_obb( v3f p
, boxf box
,
1609 m4x3f mtx
, m4x3f inv_mtx
, v3f dest
)
1612 m4x3_mulv( inv_mtx
, p
, local
);
1613 closest_point_aabb( local
, box
, local
);
1614 m4x3_mulv( mtx
, local
, dest
);
1617 VG_STATIC
float closest_point_segment( v3f a
, v3f b
, v3f point
, v3f dest
)
1621 v3_sub( point
, a
, v1
);
1623 float t
= v3_dot( v1
, v0
) / v3_length2(v0
);
1624 t
= vg_clampf(t
,0.0f
,1.0f
);
1625 v3_muladds( a
, v0
, t
, dest
);
1629 VG_STATIC
void closest_on_triangle( v3f p
, v3f tri
[3], v3f dest
)
1634 /* Region outside A */
1635 v3_sub( tri
[1], tri
[0], ab
);
1636 v3_sub( tri
[2], tri
[0], ac
);
1637 v3_sub( p
, tri
[0], ap
);
1641 if( d1
<= 0.0f
&& d2
<= 0.0f
)
1643 v3_copy( tri
[0], dest
);
1644 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1648 /* Region outside B */
1652 v3_sub( p
, tri
[1], bp
);
1653 d3
= v3_dot( ab
, bp
);
1654 d4
= v3_dot( ac
, bp
);
1656 if( d3
>= 0.0f
&& d4
<= d3
)
1658 v3_copy( tri
[1], dest
);
1659 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1663 /* Edge region of AB */
1664 float vc
= d1
*d4
- d3
*d2
;
1665 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
1667 float v
= d1
/ (d1
-d3
);
1668 v3_muladds( tri
[0], ab
, v
, dest
);
1669 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1673 /* Region outside C */
1676 v3_sub( p
, tri
[2], cp
);
1677 d5
= v3_dot(ab
, cp
);
1678 d6
= v3_dot(ac
, cp
);
1680 if( d6
>= 0.0f
&& d5
<= d6
)
1682 v3_copy( tri
[2], dest
);
1683 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1688 float vb
= d5
*d2
- d1
*d6
;
1689 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
1691 float w
= d2
/ (d2
-d6
);
1692 v3_muladds( tri
[0], ac
, w
, dest
);
1693 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1698 float va
= d3
*d6
- d5
*d4
;
1699 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
1701 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
1703 v3_sub( tri
[2], tri
[1], bc
);
1704 v3_muladds( tri
[1], bc
, w
, dest
);
1705 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1709 /* P inside region, Q via barycentric coordinates uvw */
1710 float d
= 1.0f
/(va
+vb
+vc
),
1714 v3_muladds( tri
[0], ab
, v
, dest
);
1715 v3_muladds( dest
, ac
, w
, dest
);
1718 VG_STATIC
enum contact_type
closest_on_triangle_1( v3f p
, v3f tri
[3], v3f dest
)
1723 /* Region outside A */
1724 v3_sub( tri
[1], tri
[0], ab
);
1725 v3_sub( tri
[2], tri
[0], ac
);
1726 v3_sub( p
, tri
[0], ap
);
1730 if( d1
<= 0.0f
&& d2
<= 0.0f
)
1732 v3_copy( tri
[0], dest
);
1733 return k_contact_type_default
;
1736 /* Region outside B */
1740 v3_sub( p
, tri
[1], bp
);
1741 d3
= v3_dot( ab
, bp
);
1742 d4
= v3_dot( ac
, bp
);
1744 if( d3
>= 0.0f
&& d4
<= d3
)
1746 v3_copy( tri
[1], dest
);
1747 return k_contact_type_edge
;
1750 /* Edge region of AB */
1751 float vc
= d1
*d4
- d3
*d2
;
1752 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
1754 float v
= d1
/ (d1
-d3
);
1755 v3_muladds( tri
[0], ab
, v
, dest
);
1756 return k_contact_type_edge
;
1759 /* Region outside C */
1762 v3_sub( p
, tri
[2], cp
);
1763 d5
= v3_dot(ab
, cp
);
1764 d6
= v3_dot(ac
, cp
);
1766 if( d6
>= 0.0f
&& d5
<= d6
)
1768 v3_copy( tri
[2], dest
);
1769 return k_contact_type_edge
;
1773 float vb
= d5
*d2
- d1
*d6
;
1774 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
1776 float w
= d2
/ (d2
-d6
);
1777 v3_muladds( tri
[0], ac
, w
, dest
);
1778 return k_contact_type_edge
;
1782 float va
= d3
*d6
- d5
*d4
;
1783 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
1785 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
1787 v3_sub( tri
[2], tri
[1], bc
);
1788 v3_muladds( tri
[1], bc
, w
, dest
);
1789 return k_contact_type_edge
;
1792 /* P inside region, Q via barycentric coordinates uvw */
1793 float d
= 1.0f
/(va
+vb
+vc
),
1797 v3_muladds( tri
[0], ab
, v
, dest
);
1798 v3_muladds( dest
, ac
, w
, dest
);
1800 return k_contact_type_default
;
1804 static void closest_point_elipse( v2f p
, v2f e
, v2f o
)
1806 v2f pabs
, ei
, e2
, ve
, t
;
1809 v2_div( (v2f
){ 1.0f
, 1.0f
}, e
, ei
);
1811 v2_mul( ei
, (v2f
){ e2
[0]-e2
[1], e2
[1]-e2
[0] }, ve
);
1813 v2_fill( t
, 0.70710678118654752f
);
1815 for( int i
=0; i
<3; i
++ )
1819 v2_mul( ve
, t
, v
); /* ve*t*t*t */
1823 v2_sub( pabs
, v
, u
);
1827 v2_sub( ud
, v
, ud
);
1829 v2_muls( u
, v2_length( ud
), u
);
1834 v2_maxv( (v2f
){0.0f
,0.0f
}, w
, t
);
1839 v2_copysign( o
, p
);
1846 /* Time of intersection with ray vs triangle */
1847 static int ray_tri( v3f tri
[3], v3f co
,
1848 v3f dir
, float *dist
)
1850 float const kEpsilon
= 0.00001f
;
1852 v3f v0
, v1
, h
, s
, q
, n
;
1859 v3_sub( pb
, pa
, v0
);
1860 v3_sub( pc
, pa
, v1
);
1861 v3_cross( dir
, v1
, h
);
1862 v3_cross( v0
, v1
, n
);
1864 if( v3_dot( n
, dir
) > 0.0f
) /* Backface culling */
1868 a
= v3_dot( v0
, h
);
1870 if( a
> -kEpsilon
&& a
< kEpsilon
)
1874 v3_sub( co
, pa
, s
);
1876 u
= f
* v3_dot(s
, h
);
1877 if( u
< 0.0f
|| u
> 1.0f
)
1880 v3_cross( s
, v0
, q
);
1881 v
= f
* v3_dot( dir
, q
);
1882 if( v
< 0.0f
|| u
+v
> 1.0f
)
1885 t
= f
* v3_dot(v1
, q
);
1894 /* time of intersection with ray vs sphere */
1895 static int ray_sphere( v3f c
, float r
,
1896 v3f co
, v3f dir
, float *t
)
1901 float b
= v3_dot( m
, dir
),
1902 c1
= v3_dot( m
, m
) - r
*r
;
1904 /* Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) */
1905 if( c1
> 0.0f
&& b
> 0.0f
)
1908 float discr
= b
*b
- c1
;
1910 /* A negative discriminant corresponds to ray missing sphere */
1915 * Ray now found to intersect sphere, compute smallest t value of
1918 *t
= -b
- sqrtf( discr
);
1920 /* If t is negative, ray started inside sphere so clamp t to zero */
1928 * time of intersection of ray vs cylinder
1929 * The cylinder does not have caps but is finite
1931 * Heavily adapted from regular segment vs cylinder from:
1932 * Real-Time Collision Detection
1934 static int ray_uncapped_finite_cylinder( v3f q
, v3f p
, float r
,
1935 v3f co
, v3f dir
, float *t
)
1938 v3_muladds( co
, dir
, 1.0f
, sb
);
1942 v3_sub( sb
, co
, n
);
1944 float md
= v3_dot( m
, d
),
1945 nd
= v3_dot( n
, d
),
1946 dd
= v3_dot( d
, d
),
1947 nn
= v3_dot( n
, n
),
1948 mn
= v3_dot( m
, n
),
1950 k
= v3_dot( m
, m
) - r
*r
,
1953 if( fabsf(a
) < 0.00001f
)
1955 /* Segment runs parallel to cylinder axis */
1959 float b
= dd
*mn
- nd
*md
,
1963 return 0; /* No real roots; no intersection */
1965 *t
= (-b
- sqrtf(discr
)) / a
;
1967 return 0; /* Intersection behind ray */
1969 /* Check within cylinder segment */
1970 if( md
+ (*t
)*nd
< 0.0f
)
1973 if( md
+ (*t
)*nd
> dd
)
1976 /* Segment intersects cylinder between the endcaps; t is correct */
1981 * Time of intersection of sphere and triangle. Origin must be outside the
1982 * colliding area. This is a fairly long procedure.
1984 static int spherecast_triangle( v3f tri
[3],
1985 v3f co
, v3f dir
, float r
, float *t
, v3f n
)
1990 v3_sub( tri
[1], tri
[0], v0
);
1991 v3_sub( tri
[2], tri
[0], v1
);
1992 v3_cross( v0
, v1
, n
);
1994 v3_muladds( tri
[0], n
, r
, sum
[0] );
1995 v3_muladds( tri
[1], n
, r
, sum
[1] );
1996 v3_muladds( tri
[2], n
, r
, sum
[2] );
1999 float t_min
= INFINITY
,
2002 if( ray_tri( sum
, co
, dir
, &t1
) )
2004 t_min
= vg_minf( t_min
, t1
);
2009 * Currently disabled; ray_sphere requires |d| = 1. it is not very important.
2012 for( int i
=0; i
<3; i
++ )
2014 if( ray_sphere( tri
[i
], r
, co
, dir
, &t1
) )
2016 t_min
= vg_minf( t_min
, t1
);
2022 for( int i
=0; i
<3; i
++ )
2027 if( ray_uncapped_finite_cylinder( tri
[i0
], tri
[i1
], r
, co
, dir
, &t1
) )
2034 v3_add( dir
, co
, co1
);
2035 v3_lerp( co
, co1
, t_min
, ct
);
2037 closest_point_segment( tri
[i0
], tri
[i1
], ct
, cx
);
2038 v3_sub( ct
, cx
, n
);
2050 static inline float vg_randf(void)
2052 /* TODO: replace with our own rand */
2053 return (float)rand()/(float)(RAND_MAX
);
2056 static inline void vg_rand_dir(v3f dir
)
2058 dir
[0] = vg_randf();
2059 dir
[1] = vg_randf();
2060 dir
[2] = vg_randf();
2062 v3_muls( dir
, 2.0f
, dir
);
2063 v3_sub( dir
, (v3f
){1.0f
,1.0f
,1.0f
}, dir
);
2065 v3_normalize( dir
);
2068 static inline void vg_rand_sphere( v3f co
)
2071 v3_muls( co
, cbrtf( vg_randf() ), co
);
2074 static inline int vg_randint(int max
)
2079 static void eval_bezier_time( v3f p0
, v3f p1
, v3f h0
, v3f h1
, float t
, v3f p
)
2084 v3_muls( p1
, ttt
, p
);
2085 v3_muladds( p
, h1
, 3.0f
*tt
-3.0f
*ttt
, p
);
2086 v3_muladds( p
, h0
, 3.0f
*ttt
-6.0f
*tt
+3.0f
*t
, p
);
2087 v3_muladds( p
, p0
, 3.0f
*tt
-ttt
-3.0f
*t
+1.0f
, p
);
2090 static void eval_bezier3( v3f p0
, v3f p1
, v3f p2
, float t
, v3f p
)
2094 v3_muls( p0
, u
*u
, p
);
2095 v3_muladds( p
, p1
, 2.0f
*u
*t
, p
);
2096 v3_muladds( p
, p2
, t
*t
, p
);