git.mutantstargoat.com Git - gph-cmath/blob - src/cgmmat.inl

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