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

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