1 /* Copyright (C) 2021 Harry Godden (hgn)
3 * Straightforward implementations for:
5 * Simple Matrices in 3x3 and 4x3
7 * Other useful geometric functions
13 #include "cxr_types.h"
15 #define CXR_INLINE static inline
16 #define CXR_PIf 3.14159265358979323846264338327950288f
18 CXR_INLINE
double cxr_minf( double a
, double b
)
23 CXR_INLINE
double cxr_maxf( double a
, double b
)
28 CXR_INLINE
int cxr_min( int a
, int b
)
33 CXR_INLINE
int cxr_max( int a
, int b
)
38 CXR_INLINE
double cxr_clampf( double v
, double a
, double b
)
40 return cxr_minf( b
, cxr_maxf( a
, v
) );
43 CXR_INLINE
double cxr_rad( double deg
)
45 return deg
* CXR_PIf
/ 180.0f
;
51 CXR_INLINE
void v2_zero( v2f a
)
53 a
[0] = 0.0; a
[1] = 0.0;
56 CXR_INLINE
void v2_fill( v2f a
, double v
)
61 CXR_INLINE
void v2_copy( v2f a
, v2f b
)
63 b
[0] = a
[0]; b
[1] = a
[1];
66 CXR_INLINE
void v2_minv( v2f a
, v2f b
, v2f dest
)
68 dest
[0] = cxr_minf(a
[0], b
[0]);
69 dest
[1] = cxr_minf(a
[1], b
[1]);
72 CXR_INLINE
void v2_maxv( v2f a
, v2f b
, v2f dest
)
74 dest
[0] = cxr_maxf(a
[0], b
[0]);
75 dest
[1] = cxr_maxf(a
[1], b
[1]);
78 CXR_INLINE
void v2_sub( v2f a
, v2f b
, v2f d
)
80 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
83 CXR_INLINE
double v2_cross( v2f a
, v2f b
)
85 return a
[0] * b
[1] - a
[1] * b
[0];
88 CXR_INLINE
void v2_add( v2f a
, v2f b
, v2f d
)
90 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
93 CXR_INLINE
void v2_muls( v2f a
, double s
, v2f d
)
95 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
;
98 CXR_INLINE
void v2_mul( v2f a
, v2f b
, v2f d
)
100 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1];
103 CXR_INLINE
void v2_muladds( v2f a
, v2f b
, double s
, v2f d
)
105 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
;
108 CXR_INLINE
double v2_dot( v2f a
, v2f b
)
110 return a
[0] * b
[0] + a
[1] * b
[1];
113 CXR_INLINE
void v2_div( v2f a
, v2f b
, v2f d
)
115 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1];
118 CXR_INLINE
double v2_length2( v2f a
)
120 return v2_dot( a
, a
);
123 CXR_INLINE
double v2_length( v2f a
)
125 return sqrt( v2_length2( a
) );
128 CXR_INLINE
double v2_dist2( v2f a
, v2f b
)
131 v2_sub( a
, b
, delta
);
132 return v2_length2( delta
);
135 CXR_INLINE
double v2_dist( v2f a
, v2f b
)
137 return sqrt( v2_dist2( a
, b
) );
140 CXR_INLINE
void v2_normalize( v2f a
)
142 v2_muls( a
, 1.0 / v2_length( a
), a
);
149 CXR_INLINE
void v3_zero( v3f a
)
151 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
;
154 CXR_INLINE
void v3_copy( v3f a
, v3f b
)
156 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2];
159 CXR_INLINE
void v3_add( v3f a
, v3f b
, v3f d
)
161 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
164 CXR_INLINE
void v3_sub( v3f a
, v3f b
, v3f d
)
166 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
169 CXR_INLINE
void v3_mul( v3f a
, v3f b
, v3f d
)
171 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1]; d
[2] = a
[2]*b
[2];
174 CXR_INLINE
void v3_div( v3f a
, v3f b
, v3f d
)
176 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1]; d
[2] = a
[2]/b
[2];
179 CXR_INLINE
void v3_muls( v3f a
, double s
, v3f d
)
181 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
;
184 CXR_INLINE
void v3_divs( v3f a
, double s
, v3f d
)
186 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
; d
[2] = a
[2]/s
;
189 CXR_INLINE
void v3_muladds( v3f a
, v3f b
, double s
, v3f d
)
191 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
; d
[2] = a
[2]+b
[2]*s
;
194 CXR_INLINE
double v3_dot( v3f a
, v3f b
)
196 return a
[0] * b
[0] + a
[1] * b
[1] + a
[2] * b
[2];
199 CXR_INLINE
void v3_cross( v3f a
, v3f b
, v3f d
)
201 d
[0] = a
[1] * b
[2] - a
[2] * b
[1];
202 d
[1] = a
[2] * b
[0] - a
[0] * b
[2];
203 d
[2] = a
[0] * b
[1] - a
[1] * b
[0];
206 CXR_INLINE
double v3_length2( v3f a
)
208 return v3_dot( a
, a
);
211 CXR_INLINE
double v3_length( v3f a
)
213 return sqrt( v3_length2( a
) );
216 CXR_INLINE
double v3_dist2( v3f a
, v3f b
)
219 v3_sub( a
, b
, delta
);
220 return v3_length2( delta
);
223 CXR_INLINE
double v3_dist( v3f a
, v3f b
)
225 return sqrt( v3_dist2( a
, b
) );
228 CXR_INLINE
void v3_normalize( v3f a
)
230 v3_muls( a
, 1.0 / v3_length( a
), a
);
233 CXR_INLINE
void v3_negate( v3f a
, v3f dest
)
235 v3_muls( a
, -1.0, dest
);
238 CXR_INLINE
double cxr_lerpf( double a
, double b
, double t
)
243 CXR_INLINE
void v3_lerp( v3f a
, v3f b
, double t
, v3f d
)
245 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
246 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
247 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
250 CXR_INLINE
void v3_minv( v3f a
, v3f b
, v3f dest
)
252 dest
[0] = cxr_minf(a
[0], b
[0]);
253 dest
[1] = cxr_minf(a
[1], b
[1]);
254 dest
[2] = cxr_minf(a
[2], b
[2]);
257 CXR_INLINE
void v3_maxv( v3f a
, v3f b
, v3f dest
)
259 dest
[0] = cxr_maxf(a
[0], b
[0]);
260 dest
[1] = cxr_maxf(a
[1], b
[1]);
261 dest
[2] = cxr_maxf(a
[2], b
[2]);
264 CXR_INLINE
double v3_minf( v3f a
)
266 return cxr_minf( cxr_minf( a
[0], a
[1] ), a
[2] );
269 CXR_INLINE
double v3_maxf( v3f a
)
271 return cxr_maxf( cxr_maxf( a
[0], a
[1] ), a
[2] );
274 CXR_INLINE
void v3_fill( v3f a
, double v
)
284 CXR_INLINE
void v4_copy( v4f a
, v4f b
)
286 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2]; b
[3] = a
[3];
289 CXR_INLINE
void v4_zero( v4f a
)
291 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
; a
[3] = 0.f
;
294 CXR_INLINE
void v4_muls( v4f a
, double s
, v4f d
)
296 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
; d
[3] = a
[3]*s
;
303 CXR_INLINE
void m3x3_inv_transpose( m3x3f src
, m3x3f dest
)
305 double a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
306 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
307 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
314 dest
[0][0] = (e
*i
-h
*f
)*det
;
315 dest
[1][0] = -(b
*i
-c
*h
)*det
;
316 dest
[2][0] = (b
*f
-c
*e
)*det
;
317 dest
[0][1] = -(d
*i
-f
*g
)*det
;
318 dest
[1][1] = (a
*i
-c
*g
)*det
;
319 dest
[2][1] = -(a
*f
-d
*c
)*det
;
320 dest
[0][2] = (d
*h
-g
*e
)*det
;
321 dest
[1][2] = -(a
*h
-g
*b
)*det
;
322 dest
[2][2] = (a
*e
-d
*b
)*det
;
325 CXR_INLINE
void m3x3_mulv( m3x3f m
, v3f v
, v3f d
)
329 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2];
330 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2];
331 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2];
340 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
341 { 0.0f, 1.0f, 0.0f, },\
342 { 0.0f, 0.0f, 1.0f, },\
343 { 0.0f, 0.0f, 0.0f }}
345 CXR_INLINE
void m4x3_to_3x3( m4x3f a
, m3x3f b
)
347 v3_copy( a
[0], b
[0] );
348 v3_copy( a
[1], b
[1] );
349 v3_copy( a
[2], b
[2] );
352 CXR_INLINE
void m4x3_copy( m4x3f a
, m4x3f b
)
354 v3_copy( a
[0], b
[0] );
355 v3_copy( a
[1], b
[1] );
356 v3_copy( a
[2], b
[2] );
357 v3_copy( a
[3], b
[3] );
360 CXR_INLINE
void m4x3_identity( m4x3f a
)
362 m4x3f id
= M4X3_IDENTITY
;
366 CXR_INLINE
void m4x3_mul( m4x3f a
, m4x3f b
, m4x3f d
)
369 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
370 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
371 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
372 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2],
373 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
374 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
375 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2],
376 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2];
378 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
379 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
380 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
381 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
382 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
383 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
384 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
385 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
386 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
387 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
;
388 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
;
389 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
;
392 CXR_INLINE
void m4x3_mulv( m4x3f m
, v3f v
, v3f d
)
396 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0];
397 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1];
398 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2];
404 * Affine transformations
406 CXR_INLINE
void m4x3_translate( m4x3f m
, v3f v
)
408 v3_muladds( m
[3], m
[0], v
[0], m
[3] );
409 v3_muladds( m
[3], m
[1], v
[1], m
[3] );
410 v3_muladds( m
[3], m
[2], v
[2], m
[3] );
413 CXR_INLINE
void m4x3_scale( m4x3f m
, double s
)
415 v3_muls( m
[0], s
, m
[0] );
416 v3_muls( m
[1], s
, m
[1] );
417 v3_muls( m
[2], s
, m
[2] );
420 CXR_INLINE
void m4x3_rotate_x( m4x3f m
, double angle
)
422 m4x3f t
= M4X3_IDENTITY
;
436 CXR_INLINE
void m4x3_rotate_y( m4x3f m
, double angle
)
438 m4x3f t
= M4X3_IDENTITY
;
452 CXR_INLINE
void m4x3_rotate_z( m4x3f m
, double angle
)
454 m4x3f t
= M4X3_IDENTITY
;
468 CXR_INLINE
void m4x3_expand_aabb_point( m4x3f m
, boxf box
, v3f point
)
471 m4x3_mulv( m
, point
, v
);
473 v3_minv( box
[0], v
, box
[0] );
474 v3_maxv( box
[1], v
, box
[1] );
477 CXR_INLINE
void box_concat( boxf a
, boxf b
)
479 v3_minv( a
[0], b
[0], a
[0] );
480 v3_maxv( a
[1], b
[1], a
[1] );
483 CXR_INLINE
void box_copy( boxf a
, boxf b
)
485 v3_copy( a
[0], b
[0] );
486 v3_copy( a
[1], b
[1] );
489 CXR_INLINE
void m4x3_transform_aabb( m4x3f m
, boxf box
)
493 v3_copy( box
[0], a
);
494 v3_copy( box
[1], b
);
495 v3_fill( box
[0], INFINITY
);
496 v3_fill( box
[1], -INFINITY
);
498 m4x3_expand_aabb_point( m
, box
, a
);
499 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], a
[2] } );
500 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], a
[2] } );
501 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], a
[2] } );
502 m4x3_expand_aabb_point( m
, box
, b
);
503 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], b
[2] } );
504 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], b
[2] } );
505 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], b
[2] } );
508 CXR_INLINE
void tri_normal( v3f p0
, v3f p1
, v3f p2
, v3f normal
)
511 v3_sub( p1
, p0
, v0
);
512 v3_sub( p2
, p0
, v1
);
513 v3_cross( v0
, v1
, normal
);
514 v3_normalize( normal
);
517 CXR_INLINE
void tri_to_plane( v3f a
, v3f b
, v3f c
, v4f plane
)
519 tri_normal( a
,b
,c
, plane
);
520 plane
[3] = v3_dot( plane
, a
);
523 CXR_INLINE
int plane_intersect( v4f a
, v4f b
, v4f c
, v3f p
)
525 double const epsilon
= 0.001;
533 if( d
< epsilon
&& d
> -epsilon
) return 0;
539 v3_muls( bc
, -a
[3], p
);
540 v3_muladds( p
, ca
, -b
[3], p
);
541 v3_muladds( p
, ab
, -c
[3], p
);
549 CXR_INLINE
void normal_to_plane( v3f normal
, v3f p
, v4f plane
)
551 v3_copy( normal
, plane
);
552 plane
[3] = v3_dot( normal
, p
);
555 CXR_INLINE
double plane_polarity( v4f p
, v3f a
)
559 - (p
[0]*p
[3]*p
[0] + p
[1]*p
[3]*p
[1] + p
[2]*p
[3]*p
[2]);
562 CXR_INLINE
void plane_project_point( v4f plane
, v3f a
, v3f d
)
565 v3_muls( plane
, plane
[3], ref
);
567 v3_sub( a
, ref
, delta
);
568 v3_muladds( a
, plane
, -v3_dot(delta
,plane
), d
);
571 CXR_INLINE
double line_line_dist( v3f pa0
, v3f pa1
, v3f pb0
, v3f pb1
)
573 v3f va
, vb
, n
, delta
;
574 v3_sub( pa1
, pa0
, va
);
575 v3_sub( pb1
, pb0
, vb
);
577 v3_cross( va
, vb
, n
);
580 v3_sub( pb0
, pa0
, delta
);
582 return fabs( v3_dot( n
, delta
) );
585 CXR_INLINE
double segment_segment_dist( v3f a0
, v3f a1
, v3f b0
, v3f b1
,
593 double ru
= v3_dot( r
,u
),
599 double det
= uu
*vv
- uv
*uv
,
603 if( det
< 1e-6 *uu
*vv
)
610 s
= (ru
*vv
- rv
*uv
)/det
;
611 t
= (ru
*uv
- rv
*uu
)/det
;
614 s
= cxr_clampf( s
, 0.0, 1.0 );
615 t
= cxr_clampf( t
, 0.0, 1.0 );
617 double S
= cxr_clampf((t
*uv
+ ru
)/uu
, 0.0, 1.0),
618 T
= cxr_clampf((s
*uv
- rv
)/vv
, 0.0, 1.0);
620 v3_muladds( a0
, u
, S
, a
);
621 v3_muladds( b0
, v
, T
, b
);
623 return v3_dist( a
, b
);
626 #endif /* CXR_MATH_H */