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