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 }