git.mutantstargoat.com Git - bootcensus/blob - src/census/cgmath/cgmmat.inl

   1 /* gph-cmath - C graphics math library
   2  * Copyright (C) 2018 John Tsiombikas <nuclear@member.fsf.org>
   3  *
   4  * This program is free software. Feel free to use, modify, and/or redistribute
   5  * it under the terms of the MIT/X11 license. See LICENSE for details.
   6  * If you intend to redistribute parts of the code without the LICENSE file
   7  * replace this paragraph with the full contents of the LICENSE file.
   8  */
   9 static inline void cgm_mcopy(float *dest, const float *src)
  10 {
  11         memcpy(dest, src, 16 * sizeof(float));
  12 }
  13
  14 static inline void cgm_mzero(float *m)
  15 {
  16         static float z[16];
  17         cgm_mcopy(m, z);
  18 }
  19
  20 static inline void cgm_midentity(float *m)
  21 {
  22         static float id[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
  23         cgm_mcopy(m, id);
  24 }
  25
  26 static inline void cgm_mmul(float *a, const float *b)
  27 {
  28         int i, j;
  29         float res[16];
  30         float *resptr = res;
  31         float *arow = a;
  32
  33         for(i=0; i<4; i++) {
  34                 for(j=0; j<4; j++) {
  35                         *resptr++ = arow[0] * b[j] + arow[1] * b[4 + j] +
  36                                 arow[2] * b[8 + j] + arow[3] * b[12 + j];
  37                 }
  38                 arow += 4;
  39         }
  40         cgm_mcopy(a, res);
  41 }
  42
  43 static inline void cgm_mpremul(float *a, const float *b)
  44 {
  45         int i, j;
  46         float res[16];
  47         float *resptr = res;
  48         const float *brow = b;
  49
  50         for(i=0; i<4; i++) {
  51                 for(j=0; j<4; j++) {
  52                         *resptr++ = brow[0] * a[j] + brow[1] * a[4 + j] +
  53                                 brow[2] * a[8 + j] + brow[3] * a[12 + j];
  54                 }
  55                 brow += 4;
  56         }
  57         cgm_mcopy(a, res);
  58 }
  59
  60 static inline void cgm_msubmatrix(float *m, int row, int col)
  61 {
  62         float orig[16];
  63         int i, j, subi, subj;
  64
  65         cgm_mcopy(orig, m);
  66
  67         subi = 0;
  68         for(i=0; i<4; i++) {
  69                 if(i == row) continue;
  70
  71                 subj = 0;
  72                 for(j=0; j<4; j++) {
  73                         if(j == col) continue;
  74
  75                         m[subi * 4 + subj++] = orig[i * 4 + j];
  76                 }
  77                 subi++;
  78         }
  79
  80         cgm_mupper3(m);
  81 }
  82
  83 static inline void cgm_mupper3(float *m)
  84 {
  85         m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
  86         m[15] = 1.0f;
  87 }
  88
  89 static inline float cgm_msubdet(const float *m, int row, int col)
  90 {
  91         float tmp[16];
  92         float subdet00, subdet01, subdet02;
  93
  94         cgm_mcopy(tmp, m);
  95         cgm_msubmatrix(tmp, row, col);
  96
  97         subdet00 = tmp[5] * tmp[10] - tmp[6] * tmp[9];
  98         subdet01 = tmp[4] * tmp[10] - tmp[6] * tmp[8];
  99         subdet02 = tmp[4] * tmp[9] - tmp[5] * tmp[8];
 100
 101         return tmp[0] * subdet00 - tmp[1] * subdet01 + tmp[2] * subdet02;
 102 }
 103
 104 static inline float cgm_mcofactor(const float *m, int row, int col)
 105 {
 106         float min = cgm_msubdet(m, row, col);
 107         return (row + col) & 1 ? -min : min;
 108 }
 109
 110 static inline float cgm_mdet(const float *m)
 111 {
 112         return m[0] * cgm_msubdet(m, 0, 0) - m[1] * cgm_msubdet(m, 0, 1) +
 113                 m[2] * cgm_msubdet(m, 0, 2) - m[3] * cgm_msubdet(m, 0, 3);
 114 }
 115
 116 static inline void cgm_mtranspose(float *m)
 117 {
 118         int i, j;
 119         for(i=0; i<4; i++) {
 120                 for(j=0; j<i; j++) {
 121                         int a = i * 4 + j;
 122                         int b = j * 4 + i;
 123                         float tmp = m[a];
 124                         m[a] = m[b];
 125                         m[b] = tmp;
 126                 }
 127         }
 128 }
 129
 130 static inline void cgm_mcofmatrix(float *m)
 131 {
 132         float tmp[16];
 133         int i, j;
 134
 135         cgm_mcopy(tmp, m);
 136
 137         for(i=0; i<4; i++) {
 138                 for(j=0; j<4; j++) {
 139                         m[i * 4 + j] = cgm_mcofactor(tmp, i, j);
 140                 }
 141         }
 142 }
 143
 144 static inline int cgm_minverse(float *m)
 145 {
 146         int i, j;
 147         float tmp[16];
 148         float inv_det;
 149         float det = cgm_mdet(m);
 150         if(det == 0.0f) return -1;
 151         inv_det = 1.0f / det;
 152
 153         cgm_mcopy(tmp, m);
 154
 155         for(i=0; i<4; i++) {
 156                 for(j=0; j<4; j++) {
 157                         m[i * 4 + j] = cgm_mcofactor(tmp, j, i) * inv_det;      /* transposed */
 158                 }
 159         }
 160         return 0;
 161 }
 162
 163 static inline void cgm_mtranslation(float *m, float x, float y, float z)
 164 {
 165         cgm_midentity(m);
 166         m[12] = x;
 167         m[13] = y;
 168         m[14] = z;
 169 }
 170
 171 static inline void cgm_mscaling(float *m, float sx, float sy, float sz)
 172 {
 173         cgm_mzero(m);
 174         m[0] = sx;
 175         m[5] = sy;
 176         m[10] = sz;
 177         m[15] = 1.0f;
 178 }
 179
 180 static inline void cgm_mrotation_x(float *m, float angle)
 181 {
 182         float sa = sin(angle);
 183         float ca = cos(angle);
 184
 185         cgm_midentity(m);
 186         m[5] = ca;
 187         m[6] = sa;
 188         m[9] = -sa;
 189         m[10] = ca;
 190 }
 191
 192 static inline void cgm_mrotation_y(float *m, float angle)
 193 {
 194         float sa = sin(angle);
 195         float ca = cos(angle);
 196
 197         cgm_midentity(m);
 198         m[0] = ca;
 199         m[2] = -sa;
 200         m[8] = sa;
 201         m[10] = ca;
 202 }
 203
 204 static inline void cgm_mrotation_z(float *m, float angle)
 205 {
 206         float sa = sin(angle);
 207         float ca = cos(angle);
 208
 209         cgm_midentity(m);
 210         m[0] = ca;
 211         m[1] = sa;
 212         m[4] = -sa;
 213         m[5] = ca;
 214 }
 215
 216 static inline void cgm_mrotation_axis(float *m, int idx, float angle)
 217 {
 218         switch(idx) {
 219         case 0:
 220                 cgm_mrotation_x(m, angle);
 221                 break;
 222         case 1:
 223                 cgm_mrotation_y(m, angle);
 224                 break;
 225         case 2:
 226                 cgm_mrotation_z(m, angle);
 227                 break;
 228         }
 229 }
 230
 231 static inline void cgm_mrotation(float *m, float angle, float x, float y, float z)
 232 {
 233         float sa = sin(angle);
 234         float ca = cos(angle);
 235         float invca = 1.0f - ca;
 236         float xsq = x * x;
 237         float ysq = y * y;
 238         float zsq = z * z;
 239
 240         cgm_mzero(m);
 241         m[15] = 1.0f;
 242
 243         m[0] = xsq + (1.0f - xsq) * ca;
 244         m[4] = x * y * invca - z * sa;
 245         m[8] = x * z * invca + y * sa;
 246
 247         m[1] = x * y * invca + z * sa;
 248         m[5] = ysq + (1.0f - ysq) * ca;
 249         m[9] = y * z * invca - x * sa;
 250
 251         m[2] = x * z * invca - y * sa;
 252         m[6] = y * z * invca + x * sa;
 253         m[10] = zsq + (1.0f - zsq) * ca;
 254 }
 255
 256 static inline void cgm_mrotation_euler(float *m, float a, float b, float c, int mode)
 257 {
 258         /* this array must match the EulerMode enum */
 259         static const int axis[][3] = {
 260                 {0, 1, 2}, {0, 2, 1},
 261                 {1, 0, 2}, {1, 2, 0},
 262                 {2, 0, 1}, {2, 1, 0},
 263                 {2, 0, 2}, {2, 1, 2},
 264                 {1, 0, 1}, {1, 2, 1},
 265                 {0, 1, 0}, {0, 2, 0}
 266         };
 267
 268         float ma[16], mb[16];
 269         cgm_mrotation_axis(ma, axis[mode][0], a);
 270         cgm_mrotation_axis(mb, axis[mode][1], b);
 271         cgm_mrotation_axis(m, axis[mode][2], c);
 272         cgm_mmul(m, mb);
 273         cgm_mmul(m, ma);
 274 }
 275
 276 static inline void cgm_mrotation_quat(float *m, const cgm_quat *q)
 277 {
 278         float xsq2 = 2.0f * q->x * q->x;
 279         float ysq2 = 2.0f * q->y * q->y;
 280         float zsq2 = 2.0f * q->z * q->z;
 281         float sx = 1.0f - ysq2 - zsq2;
 282         float sy = 1.0f - xsq2 - zsq2;
 283         float sz = 1.0f - xsq2 - ysq2;
 284
 285         m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
 286         m[15] = 1.0f;
 287
 288         m[0] = sx;
 289         m[1] = 2.0f * q->x * q->y + 2.0f * q->w * q->z;
 290         m[2] = 2.0f * q->z * q->x - 2.0f * q->w * q->y;
 291         m[4] = 2.0f * q->x * q->y - 2.0f * q->w * q->z;
 292         m[5] = sy;
 293         m[6] = 2.0f * q->y * q->z + 2.0f * q->w * q->x;
 294         m[8] = 2.0f * q->z * q->x + 2.0f * q->w * q->y;
 295         m[9] = 2.0f * q->y * q->z - 2.0f * q->w * q->x;
 296         m[10] = sz;
 297 }
 298
 299 static inline void cgm_mtranslate(float *m, float x, float y, float z)
 300 {
 301         float tm[16];
 302         cgm_mtranslation(tm, x, y, z);
 303         cgm_mmul(m, tm);
 304 }
 305
 306 static inline void cgm_mscale(float *m, float sx, float sy, float sz)
 307 {
 308         float sm[16];
 309         cgm_mscaling(sm, sx, sy, sz);
 310         cgm_mmul(m, sm);
 311 }
 312
 313 static inline void cgm_mrotate_x(float *m, float angle)
 314 {
 315         float rm[16];
 316         cgm_mrotation_x(rm, angle);
 317         cgm_mmul(m, rm);
 318 }
 319
 320 static inline void cgm_mrotate_y(float *m, float angle)
 321 {
 322         float rm[16];
 323         cgm_mrotation_y(rm, angle);
 324         cgm_mmul(m, rm);
 325 }
 326
 327 static inline void cgm_mrotate_z(float *m, float angle)
 328 {
 329         float rm[16];
 330         cgm_mrotation_z(rm, angle);
 331         cgm_mmul(m, rm);
 332 }
 333
 334 static inline void cgm_mrotate_axis(float *m, int idx, float angle)
 335 {
 336         float rm[16];
 337         cgm_mrotation_axis(rm, idx, angle);
 338         cgm_mmul(m, rm);
 339 }
 340
 341 static inline void cgm_mrotate(float *m, float angle, float x, float y, float z)
 342 {
 343         float rm[16];
 344         cgm_mrotation(rm, angle, x, y, z);
 345         cgm_mmul(m, rm);
 346 }
 347
 348 static inline void cgm_mrotate_euler(float *m, float a, float b, float c, int mode)
 349 {
 350         float rm[16];
 351         cgm_mrotation_euler(rm, a, b, c, mode);
 352         cgm_mmul(m, rm);
 353 }
 354
 355 static inline void cgm_mrotate_quat(float *m, const cgm_quat *q)
 356 {
 357         float rm[16];
 358         cgm_mrotation_quat(rm, q);
 359         cgm_mmul(m, rm);
 360 }
 361
 362
 363 static inline void cgm_mpretranslate(float *m, float x, float y, float z)
 364 {
 365         float tm[16];
 366         cgm_mtranslation(tm, x, y, z);
 367         cgm_mpremul(m, tm);
 368 }
 369
 370 static inline void cgm_mprescale(float *m, float sx, float sy, float sz)
 371 {
 372         float sm[16];
 373         cgm_mscaling(sm, sx, sy, sz);
 374         cgm_mpremul(m, sm);
 375 }
 376
 377 static inline void cgm_mprerotate_x(float *m, float angle)
 378 {
 379         float rm[16];
 380         cgm_mrotation_x(rm, angle);
 381         cgm_mpremul(m, rm);
 382 }
 383
 384 static inline void cgm_mprerotate_y(float *m, float angle)
 385 {
 386         float rm[16];
 387         cgm_mrotation_y(rm, angle);
 388         cgm_mpremul(m, rm);
 389 }
 390
 391 static inline void cgm_mprerotate_z(float *m, float angle)
 392 {
 393         float rm[16];
 394         cgm_mrotation_z(rm, angle);
 395         cgm_mpremul(m, rm);
 396 }
 397
 398 static inline void cgm_mprerotate_axis(float *m, int idx, float angle)
 399 {
 400         float rm[16];
 401         cgm_mrotation_axis(rm, idx, angle);
 402         cgm_mpremul(m, rm);
 403 }
 404
 405 static inline void cgm_mprerotate(float *m, float angle, float x, float y, float z)
 406 {
 407         float rm[16];
 408         cgm_mrotation(rm, angle, x, y, z);
 409         cgm_mpremul(m, rm);
 410 }
 411
 412 static inline void cgm_mprerotate_euler(float *m, float a, float b, float c, int mode)
 413 {
 414         float rm[16];
 415         cgm_mrotation_euler(rm, a, b, c, mode);
 416         cgm_mpremul(m, rm);
 417 }
 418
 419 static inline void cgm_mprerotate_quat(float *m, const cgm_quat *q)
 420 {
 421         float rm[16];
 422         cgm_mrotation_quat(rm, q);
 423         cgm_mpremul(m, rm);
 424 }
 425
 426
 427 static inline void cgm_mget_translation(const float *m, cgm_vec3 *res)
 428 {
 429         res->x = m[12];
 430         res->y = m[13];
 431         res->z = m[14];
 432 }
 433
 434 /* Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
 435  * article "Quaternion Calculus and Fast Animation".
 436  * adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl
 437  */
 438 static inline void cgm_mget_rotation(const float *m, cgm_quat *res)
 439 {
 440         static const int next[3] = {1, 2, 0};
 441         float quat[4];
 442         int i, j, k;
 443
 444         float trace = m[0] + m[5] + m[10];
 445         float root;
 446
 447         if(trace > 0.0f) {
 448                 // |w| > 1/2
 449                 root = sqrt(trace + 1.0f);      // 2w
 450                 res->w = 0.5f * root;
 451                 root = 0.5f / root;     // 1 / 4w
 452                 res->x = (m[6] - m[9]) * root;
 453                 res->y = (m[8] - m[2]) * root;
 454                 res->z = (m[1] - m[4]) * root;
 455         } else {
 456                 // |w| <= 1/2
 457                 i = 0;
 458                 if(m[5] > m[0]) {
 459                         i = 1;
 460                 }
 461                 if(m[10] > m[i * 4 + i]) {
 462                         i = 2;
 463                 }
 464                 j = next[i];
 465                 k = next[j];
 466
 467                 root = sqrt(m[i * 4 + i] - m[j * 4 + j] - m[k * 4 + k] + 1.0f);
 468                 quat[i + 1] = 0.5f * root;
 469                 root = 0.5f / root;
 470                 quat[0] = (m[j + 4 + k] - m[k * 4 + j]) * root;
 471                 quat[j + 1] = (m[i * 4 + j] - m[j * 4 + i]) * root;
 472                 quat[k + 1] = (m[i * 4 + k] - m[k * 4 + i]) * root;
 473                 res->w = quat[0];
 474                 res->x = quat[1];
 475                 res->y = quat[2];
 476                 res->z = quat[3];
 477         }
 478 }
 479
 480 static inline void cgm_mget_scaling(const float *m, cgm_vec3 *res)
 481 {
 482         res->x = sqrt(m[0] * m[0] + m[4] * m[4] + m[8] * m[8]);
 483         res->y = sqrt(m[1] * m[1] + m[5] * m[5] + m[9] * m[9]);
 484         res->z = sqrt(m[2] * m[2] + m[6] * m[6] + m[10] * m[10]);
 485 }
 486
 487 static inline void cgm_mget_frustum_plane(const float *m, int p, cgm_vec4 *res)
 488 {
 489         int row = p >> 1;
 490         const float *rowptr = m + row * 4;
 491
 492         if((p & 1) == 0) {
 493                 res->x = m[12] + rowptr[0];
 494                 res->y = m[13] + rowptr[1];
 495                 res->z = m[14] + rowptr[2];
 496                 res->w = m[15] + rowptr[3];
 497         } else {
 498                 res->x = m[12] - rowptr[0];
 499                 res->y = m[13] - rowptr[1];
 500                 res->z = m[14] - rowptr[2];
 501                 res->w = m[15] - rowptr[3];
 502         }
 503 }
 504
 505 static inline void cgm_mlookat(float *m, const cgm_vec3 *pos, const cgm_vec3 *targ,
 506                 const cgm_vec3 *up)
 507 {
 508         float trans[16];
 509         cgm_vec3 dir = *targ, right, vup;
 510
 511         cgm_vsub(&dir, pos);
 512         cgm_vnormalize(&dir);
 513         cgm_vcross(&right, &dir, up);
 514         cgm_vnormalize(&right);
 515         cgm_vcross(&vup, &right, &dir);
 516         cgm_vnormalize(&vup);
 517
 518         cgm_midentity(m);
 519         m[0] = right.x;
 520         m[1] = right.y;
 521         m[2] = right.z;
 522         m[4] = vup.x;
 523         m[5] = vup.y;
 524         m[6] = vup.z;
 525         m[8] = -dir.x;
 526         m[9] = -dir.y;
 527         m[10] = -dir.z;
 528
 529         cgm_mtranslation(trans, pos->x, pos->y, pos->z);
 530         cgm_mmul(m, trans);
 531 }
 532
 533 static inline void cgm_minv_lookat(float *m, const cgm_vec3 *pos, const cgm_vec3 *targ,
 534                 const cgm_vec3 *up)
 535 {
 536         float rot[16];
 537         cgm_vec3 dir = *targ, right, vup;
 538
 539         cgm_vsub(&dir, pos);
 540         cgm_vnormalize(&dir);
 541         cgm_vcross(&right, &dir, up);
 542         cgm_vnormalize(&right);
 543         cgm_vcross(&vup, &right, &dir);
 544         cgm_vnormalize(&vup);
 545
 546         cgm_midentity(rot);
 547         rot[0] = right.x;
 548         rot[4] = right.y;
 549         rot[8] = right.z;
 550         rot[1] = vup.x;
 551         rot[5] = vup.y;
 552         rot[9] = vup.z;
 553         rot[2] = -dir.x;
 554         rot[6] = -dir.y;
 555         rot[10] = -dir.z;
 556
 557         cgm_mtranslation(m, -pos->x, -pos->y, -pos->z);
 558         cgm_mmul(m, rot);
 559 }
 560
 561 static inline void cgm_mortho(float *m, float left, float right, float bot, float top,
 562                 float znear, float zfar)
 563 {
 564         float dx = right - left;
 565         float dy = top - bot;
 566         float dz = zfar - znear;
 567
 568         cgm_midentity(m);
 569         m[0] = 2.0f / dx;
 570         m[5] = 2.0f / dy;
 571         m[10] = -2.0f / dz;
 572         m[12] = -(right + left) / dx;
 573         m[13] = -(top + bot) / dy;
 574         m[14] = -(zfar + znear) / dz;
 575 }
 576
 577 static inline void cgm_mfrustum(float *m, float left, float right, float bot, float top,
 578                 float znear, float zfar)
 579 {
 580         float dx = right - left;
 581         float dy = top - bot;
 582         float dz = zfar - znear;
 583
 584         cgm_mzero(m);
 585         m[0] = 2.0f * znear / dx;
 586         m[5] = 2.0f * znear / dy;
 587         m[8] = (right + left) / dx;
 588         m[9] = (top + bot) / dy;
 589         m[10] = -(zfar + znear) / dz;
 590         m[14] = -2.0f * zfar * znear / dz;
 591         m[11] = -1.0f;
 592 }
 593
 594 static inline void cgm_mperspective(float *m, float vfov, float aspect, float znear, float zfar)
 595 {
 596         float s = 1.0f / (float)tan(vfov / 2.0f);
 597         float range = znear - zfar;
 598
 599         cgm_mzero(m);
 600         m[0] = s / aspect;
 601         m[5] = s;
 602         m[10] = (znear + zfar) / range;
 603         m[14] = 2.0f * znear * zfar / range;
 604         m[11] = -1.0f;
 605 }
 606
 607 static inline void cgm_mmirror(float *m, float a, float b, float c, float d)
 608 {
 609         m[0] = 1.0f - 2.0f * a * a;
 610         m[5] = 1.0f - 2.0f * b * b;
 611         m[10] = 1.0f - 2.0f * c * c;
 612         m[15] = 1.0f;
 613
 614         m[1] = m[4] = -2.0f * a * b;
 615         m[2] = m[8] = -2.0f * a * c;
 616         m[6] = m[9] = -2.0f * b * c;
 617
 618         m[12] = -2.0f * a * d;
 619         m[13] = -2.0f * b * d;
 620         m[14] = -2.0f * c * d;
 621
 622         m[3] = m[7] = m[11] = 0.0f;
 623 }