031c61e4d2f7d787dc53a0efe7c58e9660083956
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 CXR_INLINE
double cxr_clampf( double v
, double a
, double b
)
29 return cxr_minf( b
, cxr_maxf( a
, v
) );
32 // Convert degrees to radians
33 CXR_INLINE
double cxr_rad( double deg
)
35 return deg
* CXR_PIf
/ 180.0f
;
39 // ==================================================================================================================
41 CXR_INLINE
void v2_zero( v2f a
)
43 a
[0] = 0.0; a
[1] = 0.0;
46 CXR_INLINE
void v2_fill( v2f a
, double v
)
51 CXR_INLINE
void v2_copy( v2f a
, v2f b
)
53 b
[0] = a
[0]; b
[1] = a
[1];
56 CXR_INLINE
void v2_minv( v2f a
, v2f b
, v2f dest
)
58 dest
[0] = cxr_minf(a
[0], b
[0]);
59 dest
[1] = cxr_minf(a
[1], b
[1]);
62 CXR_INLINE
void v2_maxv( v2f a
, v2f b
, v2f dest
)
64 dest
[0] = cxr_maxf(a
[0], b
[0]);
65 dest
[1] = cxr_maxf(a
[1], b
[1]);
68 CXR_INLINE
void v2_sub( v2f a
, v2f b
, v2f d
)
70 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
73 CXR_INLINE
double v2_cross( v2f a
, v2f b
)
75 return a
[0] * b
[1] - a
[1] * b
[0];
78 CXR_INLINE
void v2_add( v2f a
, v2f b
, v2f d
)
80 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
83 CXR_INLINE
void v2_muls( v2f a
, double s
, v2f d
)
85 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
;
88 CXR_INLINE
void v2_mul( v2f a
, v2f b
, v2f d
)
90 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1];
93 CXR_INLINE
void v2_muladds( v2f a
, v2f b
, double s
, v2f d
)
95 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
;
98 CXR_INLINE
double v2_dot( v2f a
, v2f b
)
100 return a
[0] * b
[0] + a
[1] * b
[1];
103 CXR_INLINE
void v2_div( v2f a
, v2f b
, v2f d
)
105 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1];
108 CXR_INLINE
double v2_length2( v2f a
)
110 return v2_dot( a
, a
);
113 CXR_INLINE
double v2_length( v2f a
)
115 return sqrt( v2_length2( a
) );
118 CXR_INLINE
double v2_dist2( v2f a
, v2f b
)
121 v2_sub( a
, b
, delta
);
122 return v2_length2( delta
);
125 CXR_INLINE
double v2_dist( v2f a
, v2f b
)
127 return sqrt( v2_dist2( a
, b
) );
130 CXR_INLINE
void v2_normalize( v2f a
)
132 v2_muls( a
, 1.0 / v2_length( a
), a
);
136 // ==================================================================================================================
138 CXR_INLINE
void v3_zero( v3f a
)
140 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
;
143 CXR_INLINE
void v3_copy( v3f a
, v3f b
)
145 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2];
148 CXR_INLINE
void v3_add( 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_sub( 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_mul( 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_div( v3f a
, v3f b
, v3f d
)
165 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1]; d
[2] = a
[2]/b
[2];
168 CXR_INLINE
void v3_muls( 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_divs( v3f a
, double s
, v3f d
)
175 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
; d
[2] = a
[2]/s
;
178 CXR_INLINE
void v3_muladds( v3f a
, v3f b
, double s
, v3f d
)
180 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
; d
[2] = a
[2]+b
[2]*s
;
183 CXR_INLINE
double v3_dot( v3f a
, v3f b
)
185 return a
[0] * b
[0] + a
[1] * b
[1] + a
[2] * b
[2];
188 CXR_INLINE
void v3_cross( v3f a
, v3f b
, v3f d
)
190 d
[0] = a
[1] * b
[2] - a
[2] * b
[1];
191 d
[1] = a
[2] * b
[0] - a
[0] * b
[2];
192 d
[2] = a
[0] * b
[1] - a
[1] * b
[0];
195 CXR_INLINE
double v3_length2( v3f a
)
197 return v3_dot( a
, a
);
200 CXR_INLINE
double v3_length( v3f a
)
202 return sqrt( v3_length2( a
) );
205 CXR_INLINE
double v3_dist2( v3f a
, v3f b
)
208 v3_sub( a
, b
, delta
);
209 return v3_length2( delta
);
212 CXR_INLINE
double v3_dist( v3f a
, v3f b
)
214 return sqrt( v3_dist2( a
, b
) );
217 CXR_INLINE
void v3_normalize( v3f a
)
219 v3_muls( a
, 1.0 / v3_length( a
), a
);
222 CXR_INLINE
void v3_negate( v3f a
, v3f dest
)
224 v3_muls( a
, -1.0, dest
);
227 CXR_INLINE
double cxr_lerpf( double a
, double b
, double t
)
232 CXR_INLINE
void v3_lerp( v3f a
, v3f b
, double t
, v3f d
)
234 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
235 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
236 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
239 CXR_INLINE
void v3_minv( v3f a
, v3f b
, v3f dest
)
241 dest
[0] = cxr_minf(a
[0], b
[0]);
242 dest
[1] = cxr_minf(a
[1], b
[1]);
243 dest
[2] = cxr_minf(a
[2], b
[2]);
246 CXR_INLINE
void v3_maxv( v3f a
, v3f b
, v3f dest
)
248 dest
[0] = cxr_maxf(a
[0], b
[0]);
249 dest
[1] = cxr_maxf(a
[1], b
[1]);
250 dest
[2] = cxr_maxf(a
[2], b
[2]);
253 CXR_INLINE
double v3_minf( v3f a
)
255 return cxr_minf( cxr_minf( a
[0], a
[1] ), a
[2] );
258 CXR_INLINE
double v3_maxf( v3f a
)
260 return cxr_maxf( cxr_maxf( a
[0], a
[1] ), a
[2] );
263 CXR_INLINE
void v3_fill( v3f a
, double v
)
271 // ==================================================================================================================
273 CXR_INLINE
void v4_copy( v4f a
, v4f b
)
275 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2]; b
[3] = a
[3];
278 CXR_INLINE
void v4_zero( v4f a
)
280 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
; a
[3] = 0.f
;
283 CXR_INLINE
void v4_muls( v4f a
, double s
, v4f d
)
285 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
; d
[3] = a
[3]*s
;
289 //======================================================================================================
291 CXR_INLINE
void m3x3_inv_transpose( m3x3f src
, m3x3f dest
)
293 double a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
294 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
295 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
302 dest
[0][0] = (e
*i
-h
*f
)*det
;
303 dest
[1][0] = -(b
*i
-c
*h
)*det
;
304 dest
[2][0] = (b
*f
-c
*e
)*det
;
305 dest
[0][1] = -(d
*i
-f
*g
)*det
;
306 dest
[1][1] = (a
*i
-c
*g
)*det
;
307 dest
[2][1] = -(a
*f
-d
*c
)*det
;
308 dest
[0][2] = (d
*h
-g
*e
)*det
;
309 dest
[1][2] = -(a
*h
-g
*b
)*det
;
310 dest
[2][2] = (a
*e
-d
*b
)*det
;
313 CXR_INLINE
void m3x3_mulv( m3x3f m
, v3f v
, v3f d
)
317 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2];
318 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2];
319 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2];
325 // ==================================================================================================================
327 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
328 { 0.0f, 1.0f, 0.0f, },\
329 { 0.0f, 0.0f, 1.0f, },\
330 { 0.0f, 0.0f, 0.0f }}
332 CXR_INLINE
void m4x3_to_3x3( m4x3f a
, m3x3f b
)
334 v3_copy( a
[0], b
[0] );
335 v3_copy( a
[1], b
[1] );
336 v3_copy( a
[2], b
[2] );
339 CXR_INLINE
void m4x3_copy( m4x3f a
, m4x3f b
)
341 v3_copy( a
[0], b
[0] );
342 v3_copy( a
[1], b
[1] );
343 v3_copy( a
[2], b
[2] );
344 v3_copy( a
[3], b
[3] );
347 CXR_INLINE
void m4x3_identity( m4x3f a
)
349 m4x3f id
= M4X3_IDENTITY
;
353 CXR_INLINE
void m4x3_mul( m4x3f a
, m4x3f b
, m4x3f d
)
356 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
357 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
358 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
359 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2],
360 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
361 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
362 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2],
363 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2];
365 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
366 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
367 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
368 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
369 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
370 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
371 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
372 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
373 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
374 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
;
375 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
;
376 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
;
379 CXR_INLINE
void m4x3_mulv( m4x3f m
, v3f v
, v3f d
)
383 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0];
384 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1];
385 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2];
392 CXR_INLINE
void m4x3_translate( m4x3f m
, v3f v
)
394 v3_muladds( m
[3], m
[0], v
[0], m
[3] );
395 v3_muladds( m
[3], m
[1], v
[1], m
[3] );
396 v3_muladds( m
[3], m
[2], v
[2], m
[3] );
399 CXR_INLINE
void m4x3_scale( m4x3f m
, double s
)
401 v3_muls( m
[0], s
, m
[0] );
402 v3_muls( m
[1], s
, m
[1] );
403 v3_muls( m
[2], s
, m
[2] );
406 CXR_INLINE
void m4x3_rotate_x( m4x3f m
, double angle
)
408 m4x3f t
= M4X3_IDENTITY
;
422 CXR_INLINE
void m4x3_rotate_y( m4x3f m
, double angle
)
424 m4x3f t
= M4X3_IDENTITY
;
438 CXR_INLINE
void m4x3_rotate_z( m4x3f m
, double angle
)
440 m4x3f t
= M4X3_IDENTITY
;
454 // Warning: These functions are unoptimized..
455 CXR_INLINE
void m4x3_expand_aabb_point( m4x3f m
, boxf box
, v3f point
)
458 m4x3_mulv( m
, point
, v
);
460 v3_minv( box
[0], v
, box
[0] );
461 v3_maxv( box
[1], v
, box
[1] );
464 CXR_INLINE
void box_concat( boxf a
, boxf b
)
466 v3_minv( a
[0], b
[0], a
[0] );
467 v3_maxv( a
[1], b
[1], a
[1] );
470 CXR_INLINE
void box_copy( boxf a
, boxf b
)
472 v3_copy( a
[0], b
[0] );
473 v3_copy( a
[1], b
[1] );
476 CXR_INLINE
void m4x3_transform_aabb( m4x3f m
, boxf box
)
480 v3_copy( box
[0], a
);
481 v3_copy( box
[1], b
);
482 v3_fill( box
[0], INFINITY
);
483 v3_fill( box
[1], -INFINITY
);
485 m4x3_expand_aabb_point( m
, box
, a
);
486 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], a
[2] } );
487 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], a
[2] } );
488 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], a
[2] } );
489 m4x3_expand_aabb_point( m
, box
, b
);
490 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], b
[2] } );
491 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], b
[2] } );
492 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], b
[2] } );
495 CXR_INLINE
void tri_normal( v3f p0
, v3f p1
, v3f p2
, v3f normal
)
498 v3_sub( p1
, p0
, v0
);
499 v3_sub( p2
, p0
, v1
);
500 v3_cross( v0
, v1
, normal
);
501 v3_normalize( normal
);
504 CXR_INLINE
void tri_to_plane( v3f a
, v3f b
, v3f c
, v4f plane
)
506 tri_normal( a
,b
,c
, plane
);
507 plane
[3] = v3_dot( plane
, a
);
510 // TODO update this code to use normal v3_x functions
511 CXR_INLINE
void tri_to_plane1( double a
[3], double b
[3], double c
[3], double p
[4] )
517 edge0
[0] = b
[0] - a
[0];
518 edge0
[1] = b
[1] - a
[1];
519 edge0
[2] = b
[2] - a
[2];
521 edge1
[0] = c
[0] - a
[0];
522 edge1
[1] = c
[1] - a
[1];
523 edge1
[2] = c
[2] - a
[2];
525 p
[0] = edge0
[1] * edge1
[2] - edge0
[2] * edge1
[1];
526 p
[1] = edge0
[2] * edge1
[0] - edge0
[0] * edge1
[2];
527 p
[2] = edge0
[0] * edge1
[1] - edge0
[1] * edge1
[0];
529 l
= sqrt(p
[0] * p
[0] + p
[1] * p
[1] + p
[2] * p
[2]);
530 p
[3] = (p
[0] * a
[0] + p
[1] * a
[1] + p
[2] * a
[2]) / l
;
537 CXR_INLINE
int plane_intersect( double a
[4], double b
[4], double c
[4], double p
[3] )
539 double const epsilon
= 0.001;
544 x
[0] = a
[1] * b
[2] - a
[2] * b
[1];
545 x
[1] = a
[2] * b
[0] - a
[0] * b
[2];
546 x
[2] = a
[0] * b
[1] - a
[1] * b
[0];
548 d
= x
[0] * c
[0] + x
[1] * c
[1] + x
[2] * c
[2];
550 if( d
< epsilon
&& d
> -epsilon
) return 0;
552 p
[0] = (b
[1] * c
[2] - b
[2] * c
[1]) * -a
[3];
553 p
[1] = (b
[2] * c
[0] - b
[0] * c
[2]) * -a
[3];
554 p
[2] = (b
[0] * c
[1] - b
[1] * c
[0]) * -a
[3];
556 p
[0] += (c
[1] * a
[2] - c
[2] * a
[1]) * -b
[3];
557 p
[1] += (c
[2] * a
[0] - c
[0] * a
[2]) * -b
[3];
558 p
[2] += (c
[0] * a
[1] - c
[1] * a
[0]) * -b
[3];
560 p
[0] += (a
[1] * b
[2] - a
[2] * b
[1]) * -c
[3];
561 p
[1] += (a
[2] * b
[0] - a
[0] * b
[2]) * -c
[3];
562 p
[2] += (a
[0] * b
[1] - a
[1] * b
[0]) * -c
[3];
571 CXR_INLINE
void normal_to_plane( v3f normal
, v3f p
, v4f plane
)
573 v3_copy( normal
, plane
);
574 plane
[3] = v3_dot( normal
, p
);
577 CXR_INLINE
double plane_polarity( double p
[4], double a
[3] )
580 (a
[0] * p
[0] + a
[1] * p
[1] + a
[2] * p
[2])
581 -(p
[0]*p
[3] * p
[0] + p
[1]*p
[3] * p
[1] + p
[2]*p
[3] * p
[2])
585 CXR_INLINE
void plane_project_point( v4f plane
, v3f a
, v3f d
)
588 v3_muls( plane
, plane
[3], ref
);
590 v3_sub( a
, ref
, delta
);
591 v3_muladds( a
, plane
, -v3_dot(delta
,plane
), d
);
594 CXR_INLINE
double line_line_dist( v3f pa0
, v3f pa1
, v3f pb0
, v3f pb1
)
596 v3f va
, vb
, n
, delta
;
597 v3_sub( pa1
, pa0
, va
);
598 v3_sub( pb1
, pb0
, vb
);
600 v3_cross( va
, vb
, n
);
603 v3_sub( pb0
, pa0
, delta
);
605 return fabs( v3_dot( n
, delta
) );
608 CXR_INLINE
double segment_segment_dist( v3f a0
, v3f a1
, v3f b0
, v3f b1
,
616 double ru
= v3_dot( r
,u
),
622 double det
= uu
*vv
- uv
*uv
,
626 if( det
< 1e-6 *uu
*vv
)
633 s
= (ru
*vv
- rv
*uv
)/det
;
634 t
= (ru
*uv
- rv
*uu
)/det
;
637 s
= cxr_clampf( s
, 0.0, 1.0 );
638 t
= cxr_clampf( t
, 0.0, 1.0 );
640 double S
= cxr_clampf((t
*uv
+ ru
)/uu
, 0.0, 1.0),
641 T
= cxr_clampf((s
*uv
- rv
)/vv
, 0.0, 1.0);
643 v3_muladds( a0
, u
, S
, a
);
644 v3_muladds( b0
, v
, T
, b
);
646 return v3_dist( a
, b
);