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