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_msetrow_v3(float *m, int idx, const cgm_vec3 *v)
  19 {
  20         m[idx] = v->x;
  21         m[idx + 4] = v->y;
  22         m[idx + 8] = v->z;
  23         m[idx + 12] = 0.0f;
  24 }
  25
  26 static inline void cgm_msetrow_v4(float *m, int idx, const cgm_vec4 *v)
  27 {
  28         m[idx] = v->x;
  29         m[idx + 4] = v->y;
  30         m[idx + 8] = v->z;
  31         m[idx + 12] = v->w;
  32 }
  33
  34 static inline void cgm_msetcol_v3(float *m, int idx, const cgm_vec3 *v)
  35 {
  36         m[idx * 4] = v->x;
  37         m[idx * 4 + 1] = v->y;
  38         m[idx * 4 + 2] = v->z;
  39         m[idx * 4 + 3] = 0.0f;
  40 }
  41
  42 static inline void cgm_msetcol_v4(float *m, int idx, const cgm_vec4 *v)
  43 {
  44         m[idx * 4] = v->x;
  45         m[idx * 4 + 1] = v->y;
  46         m[idx * 4 + 2] = v->z;
  47         m[idx * 4 + 3] = v->w;
  48 }
  49
  50 static inline void cgm_mgetrow_v3(cgm_vec3 *v, const float *m, int idx)
  51 {
  52         v->x = m[idx];
  53         v->y = m[idx + 4];
  54         v->z = m[idx + 8];
  55 }
  56
  57 static inline void cgm_mgetrow_v4(cgm_vec4 *v, const float *m, int idx)
  58 {
  59         v->x = m[idx];
  60         v->y = m[idx + 4];
  61         v->z = m[idx + 8];
  62         v->w = m[idx + 12];
  63 }
  64
  65 static inline void cgm_mgetcol_v3(cgm_vec3 *v, const float *m, int idx)
  66 {
  67         v->x = m[idx * 4];
  68         v->y = m[idx * 4 + 1];
  69         v->z = m[idx * 4 + 2];
  70 }
  71
  72 static inline void cgm_mgetcol_v4(cgm_vec4 *v, const float *m, int idx)
  73 {
  74         v->x = m[idx * 4];
  75         v->y = m[idx * 4 + 1];
  76         v->z = m[idx * 4 + 2];
  77         v->w = m[idx * 4 + 3];
  78 }
  79
  80 static inline void cgm_msubmatrix(float *m, int row, int col)
  81 {
  82         float orig[16];
  83         int i, j, subi, subj;
  84
  85         cgm_mcopy(orig, m);
  86
  87         subi = 0;
  88         for(i=0; i<4; i++) {
  89                 if(i == col) continue;
  90
  91                 subj = 0;
  92                 for(j=0; j<4; j++) {
  93                         if(j == row) continue;
  94
  95                         m[subi * 4 + subj++] = orig[i * 4 + j];
  96                 }
  97                 subi++;
  98         }
  99
 100         cgm_mupper3(m);
 101 }
 102
 103 static inline void cgm_mupper3(float *m)
 104 {
 105         m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
 106         m[15] = 1.0f;
 107 }
 108
 109 static inline float cgm_msubdet(float *m, int row, int col)
 110 {
 111         cgm_msubmatrix(m, row, col);
 112         return cgm_mdet(m);
 113 }
 114
 115 static inline float cgm_mcofactor(float *m, int row, int col)
 116 {
 117         float min = cgm_msubdet(m, row, col);
 118         return (row + col) & 1 ? -min : min;
 119 }
 120
 121 static inline float cgm_mdet(float *m)
 122 {
 123         return m[0] * cgm_msubdet(m, 0, 0) - m[1] * cgm_msubdet(m, 0, 1) +
 124                 m[2] * cgm_msubdet(m, 0, 2) - m[3] * cgm_msubdet(m, 0, 3);
 125 }
 126
 127 static inline void cgm_mtranspose(float *m)
 128 {
 129         int i, j;
 130         for(i=0; i<4; i++) {
 131                 for(j=0; j<i; j++) {
 132                         int a = i * 4 + j;
 133                         int b = j * 4 + i;
 134                         float tmp = m[a];
 135                         m[a] = m[b];
 136                         m[b] = tmp;
 137                 }
 138         }
 139 }
 140
 141 static inline void cgm_mcofmatrix(float *m)
 142 {
 143         float tmp[16];
 144         int i, j;
 145
 146         cgm_mcopy(tmp, m);
 147
 148         for(i=0; i<4; i++) {
 149                 for(j=0; j<4; j++) {
 150                         m[i * 4 + j] = cgm_mcofactor(tmp, i, j);
 151                 }
 152         }
 153 }
 154
 155 static inline int cgm_minverse(float *m)
 156 {
 157         int i, j;
 158         float tmp[16];
 159         float inv_det;
 160         float det = cgm_mdet(m);
 161         if(det == 0.0f) return -1;
 162         inv_det = 1.0f / det;
 163
 164         cgm_mcopy(tmp, m);
 165
 166         for(i=0; i<4; i++) {
 167                 for(j=0; j<4; j++) {
 168                         m[i * 4 + j] = cgm_mcofactor(tmp, j, i) * inv_det;      /* transposed */
 169                 }
 170         }
 171 }