cxr/cxr_math.h

   1 /* Copyright (C) 2021 Harry Godden (hgn)
   2  *
   3  * Straightforward implementations for:
   4  *   Vector 2,3,4
   5  *   Simple Matrices in 3x3 and 4x3
   6  *   Plane maths
   7  *   Other useful geometric functions
   8  */
   9
  10 #ifndef CXR_MATH_H
  11 #define CXR_MATH_H
  12
  13 #include "cxr_types.h"
  14
  15 #define CXR_INLINE static inline
  16 #define CXR_PIf 3.14159265358979323846264338327950288f
  17
  18 CXR_INLINE double cxr_minf( double a, double b )
  19 {
  20         return a < b? a: b;
  21 }
  22
  23 CXR_INLINE double cxr_maxf( double a, double b )
  24 {
  25         return a > b? a: b;
  26 }
  27
  28 CXR_INLINE int cxr_min( int a, int b )
  29 {
  30         return a < b? a: b;
  31 }
  32
  33 CXR_INLINE int cxr_max( int a, int b )
  34 {
  35         return a > b? a: b;
  36 }
  37
  38 CXR_INLINE double cxr_clampf( double v, double a, double b )
  39 {
  40    return cxr_minf( b, cxr_maxf( a, v ) );
  41 }
  42
  43 CXR_INLINE double cxr_rad( double deg )
  44 {
  45         return deg * CXR_PIf / 180.0f;
  46 }
  47
  48 /*
  49  * Vector 2 Functions
  50  */
  51 CXR_INLINE void v2_zero( v2f a )
  52 {
  53    a[0] = 0.0; a[1] = 0.0;
  54 }
  55
  56 CXR_INLINE void v2_fill( v2f a, double v )
  57 {
  58    a[0] = v; a[1] = v;
  59 }
  60
  61 CXR_INLINE void v2_copy( v2f a, v2f b )
  62 {
  63         b[0] = a[0]; b[1] = a[1];
  64 }
  65
  66 CXR_INLINE void v2_minv( v2f a, v2f b, v2f dest )
  67 {
  68         dest[0] = cxr_minf(a[0], b[0]);
  69         dest[1] = cxr_minf(a[1], b[1]);
  70 }
  71
  72 CXR_INLINE void v2_maxv( v2f a, v2f b, v2f dest )
  73 {
  74         dest[0] = cxr_maxf(a[0], b[0]);
  75         dest[1] = cxr_maxf(a[1], b[1]);
  76 }
  77
  78 CXR_INLINE void v2_sub( v2f a, v2f b, v2f d )
  79 {
  80         d[0] = a[0]-b[0]; d[1] = a[1]-b[1];
  81 }
  82
  83 CXR_INLINE double v2_cross( v2f a, v2f b )
  84 {
  85         return a[0] * b[1] - a[1] * b[0];
  86 }
  87
  88 CXR_INLINE void v2_add( v2f a, v2f b, v2f d )
  89 {
  90         d[0] = a[0]+b[0]; d[1] = a[1]+b[1];
  91 }
  92
  93 CXR_INLINE void v2_muls( v2f a, double s, v2f d )
  94 {
  95         d[0] = a[0]*s; d[1] = a[1]*s;
  96 }
  97
  98 CXR_INLINE void v2_mul( v2f a, v2f b, v2f d )
  99 {
 100         d[0] = a[0]*b[0]; d[1] = a[1]*b[1];
 101 }
 102
 103 CXR_INLINE void v2_muladds( v2f a, v2f b, double s, v2f d )
 104 {
 105         d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s;
 106 }
 107
 108 CXR_INLINE double v2_dot( v2f a, v2f b )
 109 {
 110         return a[0] * b[0] + a[1] * b[1];
 111 }
 112
 113 CXR_INLINE void v2_div( v2f a, v2f b, v2f d )
 114 {
 115         d[0] = a[0]/b[0]; d[1] = a[1]/b[1];
 116 }
 117
 118 CXR_INLINE double v2_length2( v2f a )
 119 {
 120         return v2_dot( a, a );
 121 }
 122
 123 CXR_INLINE double v2_length( v2f a )
 124 {
 125         return sqrt( v2_length2( a ) );
 126 }
 127
 128 CXR_INLINE double v2_dist2( v2f a, v2f b )
 129 {
 130         v2f delta;
 131         v2_sub( a, b, delta );
 132         return v2_length2( delta );
 133 }
 134
 135 CXR_INLINE double v2_dist( v2f a, v2f b )
 136 {
 137         return sqrt( v2_dist2( a, b ) );
 138 }
 139
 140 CXR_INLINE void v2_normalize( v2f a )
 141 {
 142         v2_muls( a, 1.0 / v2_length( a ), a );
 143 }
 144
 145 /*
 146  * Vector 3 Functions
 147  */
 148
 149 CXR_INLINE void v3_zero( v3f a )
 150 {
 151         a[0] = 0.f; a[1] = 0.f; a[2] = 0.f;
 152 }
 153
 154 CXR_INLINE void v3_copy( v3f a, v3f b )
 155 {
 156         b[0] = a[0]; b[1] = a[1]; b[2] = a[2];
 157 }
 158
 159 CXR_INLINE void v3_add( v3f a, v3f b, v3f d )
 160 {
 161         d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2];
 162 }
 163
 164 CXR_INLINE void v3_sub( v3f a, v3f b, v3f d )
 165 {
 166         d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2];
 167 }
 168
 169 CXR_INLINE void v3_mul( v3f a, v3f b, v3f d )
 170 {
 171         d[0] = a[0]*b[0]; d[1] = a[1]*b[1]; d[2] = a[2]*b[2];
 172 }
 173
 174 CXR_INLINE void v3_div( v3f a, v3f b, v3f d )
 175 {
 176         d[0] = a[0]/b[0]; d[1] = a[1]/b[1];     d[2] = a[2]/b[2];
 177 }
 178
 179 CXR_INLINE void v3_muls( v3f a, double s, v3f d )
 180 {
 181         d[0] = a[0]*s; d[1] = a[1]*s; d[2] = a[2]*s;
 182 }
 183
 184 CXR_INLINE void v3_divs( v3f a, double s, v3f d )
 185 {
 186         d[0] = a[0]/s; d[1] = a[1]/s; d[2] = a[2]/s;
 187 }
 188
 189 CXR_INLINE void v3_muladds( v3f a, v3f b, double s, v3f d )
 190 {
 191         d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s; d[2] = a[2]+b[2]*s;
 192 }
 193
 194 CXR_INLINE double v3_dot( v3f a, v3f b )
 195 {
 196         return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
 197 }
 198
 199 CXR_INLINE void v3_cross( v3f a, v3f b, v3f d )
 200 {
 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];
 204 }
 205
 206 CXR_INLINE double v3_length2( v3f a )
 207 {
 208         return v3_dot( a, a );
 209 }
 210
 211 CXR_INLINE double v3_length( v3f a )
 212 {
 213         return sqrt( v3_length2( a ) );
 214 }
 215
 216 CXR_INLINE double v3_dist2( v3f a, v3f b )
 217 {
 218         v3f delta;
 219         v3_sub( a, b, delta );
 220         return v3_length2( delta );
 221 }
 222
 223 CXR_INLINE double v3_dist( v3f a, v3f b )
 224 {
 225         return sqrt( v3_dist2( a, b ) );
 226 }
 227
 228 CXR_INLINE void v3_normalize( v3f a )
 229 {
 230         v3_muls( a, 1.0 / v3_length( a ), a );
 231 }
 232
 233 CXR_INLINE void v3_negate( v3f a, v3f dest )
 234 {
 235    v3_muls( a, -1.0, dest );
 236 }
 237
 238 CXR_INLINE double cxr_lerpf( double a, double b, double t )
 239 {
 240         return a + t*(b-a);
 241 }
 242
 243 CXR_INLINE void v3_lerp( v3f a, v3f b, double t, v3f d )
 244 {
 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]);
 248 }
 249
 250 CXR_INLINE void v3_minv( v3f a, v3f b, v3f dest )
 251 {
 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]);
 255 }
 256
 257 CXR_INLINE void v3_maxv( v3f a, v3f b, v3f dest )
 258 {
 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]);
 262 }
 263
 264 CXR_INLINE double v3_minf( v3f a )
 265 {
 266         return cxr_minf( cxr_minf( a[0], a[1] ), a[2] );
 267 }
 268
 269 CXR_INLINE double v3_maxf( v3f a )
 270 {
 271         return cxr_maxf( cxr_maxf( a[0], a[1] ), a[2] );
 272 }
 273
 274 CXR_INLINE void v3_fill( v3f a, double v )
 275 {
 276         a[0] = v;
 277         a[1] = v;
 278         a[2] = v;
 279 }
 280
 281 /*
 282  * Vector 4 Functions
 283  */
 284 CXR_INLINE void v4_copy( v4f a, v4f b )
 285 {
 286         b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; b[3] = a[3];
 287 }
 288
 289 CXR_INLINE void v4_zero( v4f a )
 290 {
 291         a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; a[3] = 0.f;
 292 }
 293
 294 CXR_INLINE void v4_muls( v4f a, double s, v4f d )
 295 {
 296         d[0] = a[0]*s; d[1] = a[1]*s; d[2] = a[2]*s; d[3] = a[3]*s;
 297 }
 298
 299 /*
 300  * Matrix 3x3
 301  */
 302
 303 CXR_INLINE void m3x3_inv_transpose( m3x3f src, m3x3f dest )
 304 {
 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];
 308
 309         double det =    1.f /
 310                                         (+a*(e*i-h*f)
 311                 -b*(d*i-f*g)
 312                 +c*(d*h-e*g));
 313
 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;
 323 }
 324
 325 CXR_INLINE void m3x3_mulv( m3x3f m, v3f v, v3f d )
 326 {
 327         v3f res;
 328
 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];
 332
 333         v3_copy( res, d );
 334 }
 335
 336 /*
 337  * Matrix 4x3
 338  */
 339
 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 }}
 344
 345 CXR_INLINE void m4x3_to_3x3( m4x3f a, m3x3f b )
 346 {
 347         v3_copy( a[0], b[0] );
 348         v3_copy( a[1], b[1] );
 349         v3_copy( a[2], b[2] );
 350 }
 351
 352 CXR_INLINE void m4x3_copy( m4x3f a, m4x3f b )
 353 {
 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] );
 358 }
 359
 360 CXR_INLINE void m4x3_identity( m4x3f a )
 361 {
 362         m4x3f id = M4X3_IDENTITY;
 363         m4x3_copy( id, a );
 364 }
 365
 366 CXR_INLINE void m4x3_mul( m4x3f a, m4x3f b, m4x3f d )
 367 {
 368         double
 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];
 377
 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;
 390 }
 391
 392 CXR_INLINE void m4x3_mulv( m4x3f m, v3f v, v3f d )
 393 {
 394         v3f res;
 395
 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];
 399
 400         v3_copy( res, d );
 401 }
 402
 403 /*
 404  * Affine transformations
 405  */
 406 CXR_INLINE void m4x3_translate( m4x3f m, v3f v )
 407 {
 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] );
 411 }
 412
 413 CXR_INLINE void m4x3_scale( m4x3f m, double s )
 414 {
 415         v3_muls( m[0], s, m[0] );
 416         v3_muls( m[1], s, m[1] );
 417         v3_muls( m[2], s, m[2] );
 418 }
 419
 420 CXR_INLINE void m4x3_rotate_x( m4x3f m, double angle )
 421 {
 422         m4x3f t = M4X3_IDENTITY;
 423         double c, s;
 424
 425         c = cosf( angle );
 426         s = sinf( angle );
 427
 428         t[1][1] =  c;
 429         t[1][2] =  s;
 430         t[2][1] = -s;
 431         t[2][2] =  c;
 432
 433         m4x3_mul( m, t, m );
 434 }
 435
 436 CXR_INLINE void m4x3_rotate_y( m4x3f m, double angle )
 437 {
 438         m4x3f t = M4X3_IDENTITY;
 439         double c, s;
 440
 441         c = cosf( angle );
 442         s = sinf( angle );
 443
 444         t[0][0] =  c;
 445         t[0][2] = -s;
 446         t[2][0] =  s;
 447         t[2][2] =  c;
 448
 449         m4x3_mul( m, t, m );
 450 }
 451
 452 CXR_INLINE void m4x3_rotate_z( m4x3f m, double angle )
 453 {
 454         m4x3f t = M4X3_IDENTITY;
 455         double c, s;
 456
 457         c = cosf( angle );
 458         s = sinf( angle );
 459
 460         t[0][0] =  c;
 461         t[0][1] =  s;
 462         t[1][0] = -s;
 463         t[1][1] =  c;
 464
 465         m4x3_mul( m, t, m );
 466 }
 467
 468 CXR_INLINE void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point )
 469 {
 470         v3f v;
 471         m4x3_mulv( m, point, v );
 472
 473         v3_minv( box[0], v, box[0] );
 474         v3_maxv( box[1], v, box[1] );
 475 }
 476
 477 CXR_INLINE void box_concat( boxf a, boxf b )
 478 {
 479         v3_minv( a[0], b[0], a[0] );
 480         v3_maxv( a[1], b[1], a[1] );
 481 }
 482
 483 CXR_INLINE void box_copy( boxf a, boxf b )
 484 {
 485         v3_copy( a[0], b[0] );
 486         v3_copy( a[1], b[1] );
 487 }
 488
 489 CXR_INLINE void m4x3_transform_aabb( m4x3f m, boxf box )
 490 {
 491         v3f a; v3f b;
 492
 493         v3_copy( box[0], a );
 494         v3_copy( box[1], b );
 495         v3_fill( box[0],  INFINITY );
 496         v3_fill( box[1], -INFINITY );
 497
 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] } );
 506 }
 507
 508 CXR_INLINE void tri_normal( v3f p0, v3f p1, v3f p2, v3f normal )
 509 {
 510    v3f v0, v1;
 511    v3_sub( p1, p0, v0 );
 512    v3_sub( p2, p0, v1 );
 513    v3_cross( v0, v1, normal );
 514    v3_normalize( normal );
 515 }
 516
 517 CXR_INLINE void tri_to_plane( v3f a, v3f b, v3f c, v4f plane )
 518 {
 519    tri_normal( a,b,c, plane );
 520    plane[3] = v3_dot( plane, a );
 521 }
 522
 523 CXR_INLINE int plane_intersect( v4f a, v4f b, v4f c, v3f p )
 524 {
 525         double const epsilon = 0.001;
 526
 527    v3f x, bc, ca, ab;
 528         double d;
 529
 530    v3_cross( a, b, x );
 531    d = v3_dot( x, c );
 532
 533         if( d < epsilon && d > -epsilon ) return 0;
 534
 535    v3_cross(b,c,bc);
 536    v3_cross(c,a,ca);
 537    v3_cross(a,b,ab);
 538
 539    v3_muls(       bc, -a[3], p );
 540    v3_muladds( p, ca, -b[3], p );
 541    v3_muladds( p, ab, -c[3], p );
 542
 543    v3_negate( p, p );
 544    v3_divs( p, d, p );
 545
 546         return 1;
 547 }
 548
 549 CXR_INLINE void normal_to_plane( v3f normal, v3f p, v4f plane )
 550 {
 551    v3_copy( normal, plane );
 552    plane[3] = v3_dot( normal, p );
 553 }
 554
 555 CXR_INLINE double plane_polarity( v4f p, v3f a )
 556 {
 557         return
 558        v3_dot( a, p )
 559      - (p[0]*p[3]*p[0] + p[1]*p[3]*p[1] + p[2]*p[3]*p[2]);
 560 }
 561
 562 CXR_INLINE void plane_project_point( v4f plane, v3f a, v3f d )
 563 {
 564    v3f ref, delta;
 565    v3_muls( plane, plane[3], ref );
 566
 567    v3_sub( a, ref, delta );
 568    v3_muladds( a, plane, -v3_dot(delta,plane), d );
 569 }
 570
 571 CXR_INLINE double line_line_dist( v3f pa0, v3f pa1, v3f pb0, v3f pb1 )
 572 {
 573    v3f va, vb, n, delta;
 574    v3_sub( pa1, pa0, va );
 575    v3_sub( pb1, pb0, vb );
 576
 577    v3_cross( va, vb, n );
 578    v3_normalize( n );
 579
 580    v3_sub( pb0, pa0, delta );
 581
 582    return fabs( v3_dot( n, delta ) );
 583 }
 584
 585 CXR_INLINE double segment_segment_dist( v3f a0, v3f a1, v3f b0, v3f b1,
 586    v3f a, v3f b )
 587 {
 588    v3f r,u,v;
 589    v3_sub( b0, a0, r );
 590    v3_sub( a1, a0, u );
 591    v3_sub( b1, b0, v );
 592
 593    double ru = v3_dot( r,u ),
 594           rv = v3_dot( r,v ),
 595           uu = v3_dot( u,u ),
 596           uv = v3_dot( u,v ),
 597           vv = v3_dot( v,v );
 598
 599    double det = uu*vv - uv*uv,
 600           s,
 601           t;
 602
 603    if( det < 1e-6 *uu*vv )
 604    {
 605       s = ru/uu;
 606       t = 0.0;
 607    }
 608    else
 609    {
 610       s = (ru*vv - rv*uv)/det;
 611       t = (ru*uv - rv*uu)/det;
 612    }
 613
 614    s = cxr_clampf( s, 0.0, 1.0 );
 615    t = cxr_clampf( t, 0.0, 1.0 );
 616
 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);
 619
 620    v3_muladds( a0, u, S, a );
 621    v3_muladds( b0, v, T, b );
 622
 623    return v3_dist( a, b );
 624 }
 625
 626 #endif /* CXR_MATH_H */