1 // Copyright (C) 2021 Harry Godden (hgn)
4 // ==================================================================================================================
6 #define CSR_INLINE static inline
7 #define CSR_PIf 3.14159265358979323846264338327950288f
9 // Simple min/max replacements
10 CSR_INLINE
float csr_minf( float a
, float b
)
15 CSR_INLINE
float csr_maxf( float a
, float b
)
20 CSR_INLINE
int csr_min( int a
, int b
)
25 CSR_INLINE
int csr_max( int a
, int b
)
30 // Convert double precision vec3 into single
31 CSR_INLINE
void v3d_v3f( double a
[3], float b
[3] )
38 // Convert degrees to radians
39 CSR_INLINE
float csr_rad( float deg
)
41 return deg
* CSR_PIf
/ 180.0f
;
45 // ==================================================================================================================
47 CSR_INLINE
void v2_copy( v2f a
, v2f b
)
49 b
[0] = a
[0]; b
[1] = a
[1];
52 CSR_INLINE
void v2_minv( v2f a
, v2f b
, v2f dest
)
54 dest
[0] = csr_minf(a
[0], b
[0]);
55 dest
[1] = csr_minf(a
[1], b
[1]);
58 CSR_INLINE
void v2_maxv( v2f a
, v2f b
, v2f dest
)
60 dest
[0] = csr_maxf(a
[0], b
[0]);
61 dest
[1] = csr_maxf(a
[1], b
[1]);
64 CSR_INLINE
void v2_sub( v2f a
, v2f b
, v2f d
)
66 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
69 CSR_INLINE
float v2_cross( v2f a
, v2f b
)
71 return a
[0] * b
[1] - a
[1] * b
[0];
74 CSR_INLINE
void v2_add( v2f a
, v2f b
, v2f d
)
76 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
79 CSR_INLINE
void v2_muls( v2f a
, float s
, v2f d
)
81 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
;
84 CSR_INLINE
void v2_mul( v2f a
, v2f b
, v2f d
)
86 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1];
89 CSR_INLINE
void v2_div( v2f a
, v2f b
, v2f d
)
91 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1];
95 // ==================================================================================================================
97 CSR_INLINE
void v3_zero( v3f a
)
99 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
;
102 CSR_INLINE
void v3_copy( v3f a
, v3f b
)
104 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2];
107 CSR_INLINE
void v3_add( v3f a
, v3f b
, v3f d
)
109 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
112 CSR_INLINE
void v3_sub( v3f a
, v3f b
, v3f d
)
114 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
117 CSR_INLINE
void v3_mul( v3f a
, v3f b
, v3f d
)
119 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1]; d
[2] = a
[2]*b
[2];
122 CSR_INLINE
void v3_div( v3f a
, v3f b
, v3f d
)
124 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1]; d
[2] = a
[2]/b
[2];
127 CSR_INLINE
void v3_muls( v3f a
, float s
, v3f d
)
129 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
;
132 CSR_INLINE
void v3_divs( v3f a
, float s
, v3f d
)
134 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
; d
[2] = a
[2]/s
;
137 CSR_INLINE
void v3_muladds( v3f a
, v3f b
, float s
, v3f d
)
139 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
; d
[2] = a
[2]+b
[2]*s
;
142 CSR_INLINE
float v3_dot( v3f a
, v3f b
)
144 return a
[0] * b
[0] + a
[1] * b
[1] + a
[2] * b
[2];
147 CSR_INLINE
void v3_cross( v3f a
, v3f b
, v3f d
)
149 d
[0] = a
[1] * b
[2] - a
[2] * b
[1];
150 d
[1] = a
[2] * b
[0] - a
[0] * b
[2];
151 d
[2] = a
[0] * b
[1] - a
[1] * b
[0];
154 CSR_INLINE
float v3_length2( v3f a
)
156 return v3_dot( a
, a
);
159 CSR_INLINE
float v3_length( v3f a
)
161 return sqrtf( v3_length2( a
) );
164 CSR_INLINE
float v3_dist2( v3f a
, v3f b
)
167 v3_sub( a
, b
, delta
);
168 return v3_length2( delta
);
171 CSR_INLINE
float v3_dist( v3f a
, v3f b
)
173 return sqrtf( v3_dist2( a
, b
) );
176 CSR_INLINE
void v3_normalize( v3f a
)
178 v3_muls( a
, 1.f
/ v3_length( a
), a
);
181 CSR_INLINE
float csr_lerpf( float a
, float b
, float t
)
186 CSR_INLINE
void v3_lerp( v3f a
, v3f b
, float t
, v3f d
)
188 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
189 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
190 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
193 CSR_INLINE
void v3_minv( v3f a
, v3f b
, v3f dest
)
195 dest
[0] = csr_minf(a
[0], b
[0]);
196 dest
[1] = csr_minf(a
[1], b
[1]);
197 dest
[2] = csr_minf(a
[2], b
[2]);
200 CSR_INLINE
void v3_maxv( v3f a
, v3f b
, v3f dest
)
202 dest
[0] = csr_maxf(a
[0], b
[0]);
203 dest
[1] = csr_maxf(a
[1], b
[1]);
204 dest
[2] = csr_maxf(a
[2], b
[2]);
207 CSR_INLINE
float v3_minf( v3f a
)
209 return csr_minf( csr_minf( a
[0], a
[1] ), a
[2] );
212 CSR_INLINE
float v3_maxf( v3f a
)
214 return csr_maxf( csr_maxf( a
[0], a
[1] ), a
[2] );
217 CSR_INLINE
void v3_fill( v3f a
, float v
)
225 // ==================================================================================================================
227 CSR_INLINE
void v4_copy( v4f a
, v4f b
)
229 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2]; b
[3] = a
[3];
232 CSR_INLINE
void v4_zero( v4f a
)
234 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
; a
[3] = 0.f
;
238 //======================================================================================================
240 CSR_INLINE
void m3x3_inv_transpose( m3x3f src
, m3x3f dest
)
242 float a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
243 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
244 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
251 dest
[0][0] = (e
*i
-h
*f
)*det
;
252 dest
[1][0] = -(b
*i
-c
*h
)*det
;
253 dest
[2][0] = (b
*f
-c
*e
)*det
;
254 dest
[0][1] = -(d
*i
-f
*g
)*det
;
255 dest
[1][1] = (a
*i
-c
*g
)*det
;
256 dest
[2][1] = -(a
*f
-d
*c
)*det
;
257 dest
[0][2] = (d
*h
-g
*e
)*det
;
258 dest
[1][2] = -(a
*h
-g
*b
)*det
;
259 dest
[2][2] = (a
*e
-d
*b
)*det
;
262 CSR_INLINE
void m3x3_mulv( m3x3f m
, v3f v
, v3f d
)
266 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2];
267 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2];
268 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2];
274 // ==================================================================================================================
276 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
277 { 0.0f, 1.0f, 0.0f, },\
278 { 0.0f, 0.0f, 1.0f, },\
279 { 0.0f, 0.0f, 0.0f }}
281 CSR_INLINE
void m4x3_to_3x3( m4x3f a
, m3x3f b
)
283 v3_copy( a
[0], b
[0] );
284 v3_copy( a
[1], b
[1] );
285 v3_copy( a
[2], b
[2] );
288 CSR_INLINE
void m4x3_copy( m4x3f a
, m4x3f b
)
290 v3_copy( a
[0], b
[0] );
291 v3_copy( a
[1], b
[1] );
292 v3_copy( a
[2], b
[2] );
293 v3_copy( a
[3], b
[3] );
296 CSR_INLINE
void m4x3_identity( m4x3f a
)
298 m4x3f id
= M4X3_IDENTITY
;
302 CSR_INLINE
void m4x3_mul( m4x3f a
, m4x3f b
, m4x3f d
)
305 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
306 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
307 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
308 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2],
309 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
310 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
311 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2],
312 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2];
314 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
315 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
316 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
317 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
318 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
319 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
320 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
321 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
322 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
323 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
;
324 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
;
325 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
;
328 CSR_INLINE
void m4x3_mulv( m4x3f m
, v3f v
, v3f d
)
332 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0];
333 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1];
334 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2];
341 CSR_INLINE
void m4x3_translate( m4x3f m
, v3f v
)
343 v3_muladds( m
[3], m
[0], v
[0], m
[3] );
344 v3_muladds( m
[3], m
[1], v
[1], m
[3] );
345 v3_muladds( m
[3], m
[2], v
[2], m
[3] );
348 CSR_INLINE
void m4x3_scale( m4x3f m
, float s
)
350 v3_muls( m
[0], s
, m
[0] );
351 v3_muls( m
[1], s
, m
[1] );
352 v3_muls( m
[2], s
, m
[2] );
355 CSR_INLINE
void m4x3_rotate_x( m4x3f m
, float angle
)
357 m4x3f t
= M4X3_IDENTITY
;
371 CSR_INLINE
void m4x3_rotate_y( m4x3f m
, float angle
)
373 m4x3f t
= M4X3_IDENTITY
;
387 CSR_INLINE
void m4x3_rotate_z( m4x3f m
, float angle
)
389 m4x3f t
= M4X3_IDENTITY
;
403 // Warning: These functions are unoptimized..
404 CSR_INLINE
void m4x3_expand_aabb_point( m4x3f m
, boxf box
, v3f point
)
407 m4x3_mulv( m
, point
, v
);
409 v3_minv( box
[0], v
, box
[0] );
410 v3_maxv( box
[1], v
, box
[1] );
413 CSR_INLINE
void box_concat( boxf a
, boxf b
)
415 v3_minv( a
[0], b
[0], a
[0] );
416 v3_maxv( a
[1], b
[1], a
[1] );
419 CSR_INLINE
void box_copy( boxf a
, boxf b
)
421 v3_copy( a
[0], b
[0] );
422 v3_copy( a
[1], b
[1] );
425 CSR_INLINE
void m4x3_transform_aabb( m4x3f m
, boxf box
)
429 v3_copy( box
[0], a
);
430 v3_copy( box
[1], b
);
431 v3_fill( box
[0], INFINITY
);
432 v3_fill( box
[1], -INFINITY
);
434 m4x3_expand_aabb_point( m
, box
, a
);
435 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], a
[2] } );
436 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], a
[2] } );
437 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], a
[2] } );
438 m4x3_expand_aabb_point( m
, box
, b
);
439 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], b
[2] } );
440 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], b
[2] } );
441 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], b
[2] } );
444 // Planes (double precision)
445 // ==================================================================================================================
447 CSR_INLINE
void tri_to_plane( double a
[3], double b
[3], double c
[3], double p
[4] )
453 edge0
[0] = b
[0] - a
[0];
454 edge0
[1] = b
[1] - a
[1];
455 edge0
[2] = b
[2] - a
[2];
457 edge1
[0] = c
[0] - a
[0];
458 edge1
[1] = c
[1] - a
[1];
459 edge1
[2] = c
[2] - a
[2];
461 p
[0] = edge0
[1] * edge1
[2] - edge0
[2] * edge1
[1];
462 p
[1] = edge0
[2] * edge1
[0] - edge0
[0] * edge1
[2];
463 p
[2] = edge0
[0] * edge1
[1] - edge0
[1] * edge1
[0];
465 l
= sqrt(p
[0] * p
[0] + p
[1] * p
[1] + p
[2] * p
[2]);
466 p
[3] = (p
[0] * a
[0] + p
[1] * a
[1] + p
[2] * a
[2]) / l
;
473 CSR_INLINE
int plane_intersect( double a
[4], double b
[4], double c
[4], double p
[4] )
475 double const epsilon
= 1e-8f
;
480 x
[0] = a
[1] * b
[2] - a
[2] * b
[1];
481 x
[1] = a
[2] * b
[0] - a
[0] * b
[2];
482 x
[2] = a
[0] * b
[1] - a
[1] * b
[0];
484 d
= x
[0] * c
[0] + x
[1] * c
[1] + x
[2] * c
[2];
486 if( d
< epsilon
&& d
> -epsilon
) return 0;
488 p
[0] = (b
[1] * c
[2] - b
[2] * c
[1]) * -a
[3];
489 p
[1] = (b
[2] * c
[0] - b
[0] * c
[2]) * -a
[3];
490 p
[2] = (b
[0] * c
[1] - b
[1] * c
[0]) * -a
[3];
492 p
[0] += (c
[1] * a
[2] - c
[2] * a
[1]) * -b
[3];
493 p
[1] += (c
[2] * a
[0] - c
[0] * a
[2]) * -b
[3];
494 p
[2] += (c
[0] * a
[1] - c
[1] * a
[0]) * -b
[3];
496 p
[0] += (a
[1] * b
[2] - a
[2] * b
[1]) * -c
[3];
497 p
[1] += (a
[2] * b
[0] - a
[0] * b
[2]) * -c
[3];
498 p
[2] += (a
[0] * b
[1] - a
[1] * b
[0]) * -c
[3];
507 CSR_INLINE
double plane_polarity( double p
[4], double a
[3] )
510 (a
[0] * p
[0] + a
[1] * p
[1] + a
[2] * p
[2])
511 -(p
[0]*p
[3] * p
[0] + p
[1]*p
[3] * p
[1] + p
[2]*p
[3] * p
[2])