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