1 // Copyright (C) 2021 Harry Godden (hgn)
3 #define CXR_INLINE static inline
4 #define CXR_PIf 3.14159265358979323846264338327950288f
6 // Simple min/max replacements
7 CXR_INLINE
double cxr_minf( double a
, double b
)
12 CXR_INLINE
double cxr_maxf( double a
, double b
)
17 CXR_INLINE
int cxr_min( int a
, int b
)
22 CXR_INLINE
int cxr_max( int a
, int b
)
27 // Convert degrees to radians
28 CXR_INLINE
double cxr_rad( double deg
)
30 return deg
* CXR_PIf
/ 180.0f
;
34 // ==================================================================================================================
36 CXR_INLINE
void v2_zero( v2f a
)
38 a
[0] = 0.0; a
[1] = 0.0;
41 CXR_INLINE
void v2_fill( v2f a
, double v
)
46 CXR_INLINE
void v2_copy( v2f a
, v2f b
)
48 b
[0] = a
[0]; b
[1] = a
[1];
51 CXR_INLINE
void v2_minv( v2f a
, v2f b
, v2f dest
)
53 dest
[0] = cxr_minf(a
[0], b
[0]);
54 dest
[1] = cxr_minf(a
[1], b
[1]);
57 CXR_INLINE
void v2_maxv( v2f a
, v2f b
, v2f dest
)
59 dest
[0] = cxr_maxf(a
[0], b
[0]);
60 dest
[1] = cxr_maxf(a
[1], b
[1]);
63 CXR_INLINE
void v2_sub( v2f a
, v2f b
, v2f d
)
65 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
68 CXR_INLINE
double v2_cross( v2f a
, v2f b
)
70 return a
[0] * b
[1] - a
[1] * b
[0];
73 CXR_INLINE
void v2_add( v2f a
, v2f b
, v2f d
)
75 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
78 CXR_INLINE
void v2_muls( v2f a
, double s
, v2f d
)
80 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
;
83 CXR_INLINE
void v2_mul( v2f a
, v2f b
, v2f d
)
85 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1];
88 CXR_INLINE
void v2_muladds( v2f a
, v2f b
, double s
, v2f d
)
90 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
;
93 CXR_INLINE
double v2_dot( v2f a
, v2f b
)
95 return a
[0] * b
[0] + a
[1] * b
[1];
98 CXR_INLINE
void v2_div( v2f a
, v2f b
, v2f d
)
100 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1];
103 CXR_INLINE
double v2_length2( v2f a
)
105 return v2_dot( a
, a
);
108 CXR_INLINE
double v2_length( v2f a
)
110 return sqrt( v2_length2( a
) );
113 CXR_INLINE
double v2_dist2( v2f a
, v2f b
)
116 v2_sub( a
, b
, delta
);
117 return v2_length2( delta
);
120 CXR_INLINE
double v2_dist( v2f a
, v2f b
)
122 return sqrt( v2_dist2( a
, b
) );
125 CXR_INLINE
void v2_normalize( v2f a
)
127 v2_muls( a
, 1.0 / v2_length( a
), a
);
131 // ==================================================================================================================
133 CXR_INLINE
void v3_zero( v3f a
)
135 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
;
138 CXR_INLINE
void v3_copy( v3f a
, v3f b
)
140 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2];
143 CXR_INLINE
void v3_add( v3f a
, v3f b
, v3f d
)
145 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
148 CXR_INLINE
void v3_sub( v3f a
, v3f b
, v3f d
)
150 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
153 CXR_INLINE
void v3_mul( v3f a
, v3f b
, v3f d
)
155 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1]; d
[2] = a
[2]*b
[2];
158 CXR_INLINE
void v3_div( v3f a
, v3f b
, v3f d
)
160 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1]; d
[2] = a
[2]/b
[2];
163 CXR_INLINE
void v3_muls( v3f a
, double s
, v3f d
)
165 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
;
168 CXR_INLINE
void v3_divs( v3f a
, double s
, v3f d
)
170 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
; d
[2] = a
[2]/s
;
173 CXR_INLINE
void v3_muladds( v3f a
, v3f b
, double s
, v3f d
)
175 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
; d
[2] = a
[2]+b
[2]*s
;
178 CXR_INLINE
double v3_dot( v3f a
, v3f b
)
180 return a
[0] * b
[0] + a
[1] * b
[1] + a
[2] * b
[2];
183 CXR_INLINE
void v3_cross( v3f a
, v3f b
, v3f d
)
185 d
[0] = a
[1] * b
[2] - a
[2] * b
[1];
186 d
[1] = a
[2] * b
[0] - a
[0] * b
[2];
187 d
[2] = a
[0] * b
[1] - a
[1] * b
[0];
190 CXR_INLINE
double v3_length2( v3f a
)
192 return v3_dot( a
, a
);
195 CXR_INLINE
double v3_length( v3f a
)
197 return sqrt( v3_length2( a
) );
200 CXR_INLINE
double v3_dist2( v3f a
, v3f b
)
203 v3_sub( a
, b
, delta
);
204 return v3_length2( delta
);
207 CXR_INLINE
double v3_dist( v3f a
, v3f b
)
209 return sqrt( v3_dist2( a
, b
) );
212 CXR_INLINE
void v3_normalize( v3f a
)
214 v3_muls( a
, 1.0 / v3_length( a
), a
);
217 CXR_INLINE
void v3_negate( v3f a
, v3f dest
)
219 v3_muls( a
, -1.0, dest
);
222 CXR_INLINE
double cxr_lerpf( double a
, double b
, double t
)
227 CXR_INLINE
void v3_lerp( v3f a
, v3f b
, double t
, v3f d
)
229 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
230 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
231 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
234 CXR_INLINE
void v3_minv( v3f a
, v3f b
, v3f dest
)
236 dest
[0] = cxr_minf(a
[0], b
[0]);
237 dest
[1] = cxr_minf(a
[1], b
[1]);
238 dest
[2] = cxr_minf(a
[2], b
[2]);
241 CXR_INLINE
void v3_maxv( v3f a
, v3f b
, v3f dest
)
243 dest
[0] = cxr_maxf(a
[0], b
[0]);
244 dest
[1] = cxr_maxf(a
[1], b
[1]);
245 dest
[2] = cxr_maxf(a
[2], b
[2]);
248 CXR_INLINE
double v3_minf( v3f a
)
250 return cxr_minf( cxr_minf( a
[0], a
[1] ), a
[2] );
253 CXR_INLINE
double v3_maxf( v3f a
)
255 return cxr_maxf( cxr_maxf( a
[0], a
[1] ), a
[2] );
258 CXR_INLINE
void v3_fill( v3f a
, double v
)
266 // ==================================================================================================================
268 CXR_INLINE
void v4_copy( v4f a
, v4f b
)
270 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2]; b
[3] = a
[3];
273 CXR_INLINE
void v4_zero( v4f a
)
275 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
; a
[3] = 0.f
;
278 CXR_INLINE
void v4_muls( v4f a
, double s
, v4f d
)
280 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
; d
[3] = a
[3]*s
;
284 //======================================================================================================
286 CXR_INLINE
void m3x3_inv_transpose( m3x3f src
, m3x3f dest
)
288 double a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
289 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
290 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
297 dest
[0][0] = (e
*i
-h
*f
)*det
;
298 dest
[1][0] = -(b
*i
-c
*h
)*det
;
299 dest
[2][0] = (b
*f
-c
*e
)*det
;
300 dest
[0][1] = -(d
*i
-f
*g
)*det
;
301 dest
[1][1] = (a
*i
-c
*g
)*det
;
302 dest
[2][1] = -(a
*f
-d
*c
)*det
;
303 dest
[0][2] = (d
*h
-g
*e
)*det
;
304 dest
[1][2] = -(a
*h
-g
*b
)*det
;
305 dest
[2][2] = (a
*e
-d
*b
)*det
;
308 CXR_INLINE
void m3x3_mulv( m3x3f m
, v3f v
, v3f d
)
312 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2];
313 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2];
314 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2];
320 // ==================================================================================================================
322 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
323 { 0.0f, 1.0f, 0.0f, },\
324 { 0.0f, 0.0f, 1.0f, },\
325 { 0.0f, 0.0f, 0.0f }}
327 CXR_INLINE
void m4x3_to_3x3( m4x3f a
, m3x3f b
)
329 v3_copy( a
[0], b
[0] );
330 v3_copy( a
[1], b
[1] );
331 v3_copy( a
[2], b
[2] );
334 CXR_INLINE
void m4x3_copy( m4x3f a
, m4x3f b
)
336 v3_copy( a
[0], b
[0] );
337 v3_copy( a
[1], b
[1] );
338 v3_copy( a
[2], b
[2] );
339 v3_copy( a
[3], b
[3] );
342 CXR_INLINE
void m4x3_identity( m4x3f a
)
344 m4x3f id
= M4X3_IDENTITY
;
348 CXR_INLINE
void m4x3_mul( m4x3f a
, m4x3f b
, m4x3f d
)
351 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
352 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
353 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
354 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2],
355 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
356 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
357 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2],
358 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2];
360 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
361 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
362 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
363 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
364 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
365 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
366 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
367 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
368 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
369 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
;
370 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
;
371 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
;
374 CXR_INLINE
void m4x3_mulv( m4x3f m
, v3f v
, v3f d
)
378 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0];
379 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1];
380 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2];
387 CXR_INLINE
void m4x3_translate( m4x3f m
, v3f v
)
389 v3_muladds( m
[3], m
[0], v
[0], m
[3] );
390 v3_muladds( m
[3], m
[1], v
[1], m
[3] );
391 v3_muladds( m
[3], m
[2], v
[2], m
[3] );
394 CXR_INLINE
void m4x3_scale( m4x3f m
, double s
)
396 v3_muls( m
[0], s
, m
[0] );
397 v3_muls( m
[1], s
, m
[1] );
398 v3_muls( m
[2], s
, m
[2] );
401 CXR_INLINE
void m4x3_rotate_x( m4x3f m
, double angle
)
403 m4x3f t
= M4X3_IDENTITY
;
417 CXR_INLINE
void m4x3_rotate_y( m4x3f m
, double angle
)
419 m4x3f t
= M4X3_IDENTITY
;
433 CXR_INLINE
void m4x3_rotate_z( m4x3f m
, double angle
)
435 m4x3f t
= M4X3_IDENTITY
;
449 // Warning: These functions are unoptimized..
450 CXR_INLINE
void m4x3_expand_aabb_point( m4x3f m
, boxf box
, v3f point
)
453 m4x3_mulv( m
, point
, v
);
455 v3_minv( box
[0], v
, box
[0] );
456 v3_maxv( box
[1], v
, box
[1] );
459 CXR_INLINE
void box_concat( boxf a
, boxf b
)
461 v3_minv( a
[0], b
[0], a
[0] );
462 v3_maxv( a
[1], b
[1], a
[1] );
465 CXR_INLINE
void box_copy( boxf a
, boxf b
)
467 v3_copy( a
[0], b
[0] );
468 v3_copy( a
[1], b
[1] );
471 CXR_INLINE
void m4x3_transform_aabb( m4x3f m
, boxf box
)
475 v3_copy( box
[0], a
);
476 v3_copy( box
[1], b
);
477 v3_fill( box
[0], INFINITY
);
478 v3_fill( box
[1], -INFINITY
);
480 m4x3_expand_aabb_point( m
, box
, a
);
481 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], a
[2] } );
482 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], a
[2] } );
483 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], a
[2] } );
484 m4x3_expand_aabb_point( m
, box
, b
);
485 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], b
[2] } );
486 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], b
[2] } );
487 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], b
[2] } );
490 CXR_INLINE
void tri_normal( v3f p0
, v3f p1
, v3f p2
, v3f normal
)
493 v3_sub( p1
, p0
, v0
);
494 v3_sub( p2
, p0
, v1
);
495 v3_cross( v0
, v1
, normal
);
496 v3_normalize( normal
);
499 CXR_INLINE
void tri_to_plane( v3f a
, v3f b
, v3f c
, v4f plane
)
501 tri_normal( a
,b
,c
, plane
);
502 plane
[3] = v3_dot( plane
, a
);
505 // TODO update this code to use normal v3_x functions
506 CXR_INLINE
void tri_to_plane1( double a
[3], double b
[3], double c
[3], double p
[4] )
512 edge0
[0] = b
[0] - a
[0];
513 edge0
[1] = b
[1] - a
[1];
514 edge0
[2] = b
[2] - a
[2];
516 edge1
[0] = c
[0] - a
[0];
517 edge1
[1] = c
[1] - a
[1];
518 edge1
[2] = c
[2] - a
[2];
520 p
[0] = edge0
[1] * edge1
[2] - edge0
[2] * edge1
[1];
521 p
[1] = edge0
[2] * edge1
[0] - edge0
[0] * edge1
[2];
522 p
[2] = edge0
[0] * edge1
[1] - edge0
[1] * edge1
[0];
524 l
= sqrt(p
[0] * p
[0] + p
[1] * p
[1] + p
[2] * p
[2]);
525 p
[3] = (p
[0] * a
[0] + p
[1] * a
[1] + p
[2] * a
[2]) / l
;
532 CXR_INLINE
int plane_intersect( double a
[4], double b
[4], double c
[4], double p
[3] )
534 double const epsilon
= 0.001;
539 x
[0] = a
[1] * b
[2] - a
[2] * b
[1];
540 x
[1] = a
[2] * b
[0] - a
[0] * b
[2];
541 x
[2] = a
[0] * b
[1] - a
[1] * b
[0];
543 d
= x
[0] * c
[0] + x
[1] * c
[1] + x
[2] * c
[2];
545 if( d
< epsilon
&& d
> -epsilon
) return 0;
547 p
[0] = (b
[1] * c
[2] - b
[2] * c
[1]) * -a
[3];
548 p
[1] = (b
[2] * c
[0] - b
[0] * c
[2]) * -a
[3];
549 p
[2] = (b
[0] * c
[1] - b
[1] * c
[0]) * -a
[3];
551 p
[0] += (c
[1] * a
[2] - c
[2] * a
[1]) * -b
[3];
552 p
[1] += (c
[2] * a
[0] - c
[0] * a
[2]) * -b
[3];
553 p
[2] += (c
[0] * a
[1] - c
[1] * a
[0]) * -b
[3];
555 p
[0] += (a
[1] * b
[2] - a
[2] * b
[1]) * -c
[3];
556 p
[1] += (a
[2] * b
[0] - a
[0] * b
[2]) * -c
[3];
557 p
[2] += (a
[0] * b
[1] - a
[1] * b
[0]) * -c
[3];
566 CXR_INLINE
void normal_to_plane( v3f normal
, v3f p
, v4f plane
)
568 v3_copy( normal
, plane
);
569 plane
[3] = v3_dot( normal
, p
);
572 CXR_INLINE
double plane_polarity( double p
[4], double a
[3] )
575 (a
[0] * p
[0] + a
[1] * p
[1] + a
[2] * p
[2])
576 -(p
[0]*p
[3] * p
[0] + p
[1]*p
[3] * p
[1] + p
[2]*p
[3] * p
[2])