fixed inverse and submatrix, now testing identical to gmath

[gph-cmath] / src / cgmmat.inl

diff --git a/src/cgmmat.inl b/src/cgmmat.inl
index c5d1140..f64b259 100644 (file)
--- a/src/cgmmat.inl
+++ b/src/cgmmat.inl
@@ -41,11 +41,11 @@ static inline void cgm_msubmatrix(float *m, int row, int col)
 
        subi = 0;
        for(i=0; i<4; i++) {
-               if(i == col) continue;
+               if(i == row) continue;
 
                subj = 0;
                for(j=0; j<4; j++) {
-                       if(j == row) continue;
+                       if(j == col) continue;
 
                        m[subi * 4 + subj++] = orig[i * 4 + j];
                }
@@ -61,19 +61,28 @@ static inline void cgm_mupper3(float *m)
        m[15] = 1.0f;
 }
 
-static inline float cgm_msubdet(float *m, int row, int col)
+static inline float cgm_msubdet(const float *m, int row, int col)
 {
-       cgm_msubmatrix(m, row, col);
-       return cgm_mdet(m);
+       float tmp[16];
+       float subdet00, subdet01, subdet02;
+
+       cgm_mcopy(tmp, m);
+       cgm_msubmatrix(tmp, row, col);
+
+       subdet00 = tmp[5] * tmp[10] - tmp[6] * tmp[9];
+       subdet01 = tmp[4] * tmp[10] - tmp[6] * tmp[8];
+       subdet02 = tmp[4] * tmp[9] - tmp[5] * tmp[8];
+
+       return tmp[0] * subdet00 - tmp[1] * subdet01 + tmp[2] * subdet02;
 }
 
-static inline float cgm_mcofactor(float *m, int row, int col)
+static inline float cgm_mcofactor(const float *m, int row, int col)
 {
        float min = cgm_msubdet(m, row, col);
        return (row + col) & 1 ? -min : min;
 }
 
-static inline float cgm_mdet(float *m)
+static inline float cgm_mdet(const float *m)
 {
        return m[0] * cgm_msubdet(m, 0, 0) - m[1] * cgm_msubdet(m, 0, 1) +
                m[2] * cgm_msubdet(m, 0, 2) - m[3] * cgm_msubdet(m, 0, 3);
@@ -123,6 +132,7 @@ static inline int cgm_minverse(float *m)
                        m[i * 4 + j] = cgm_mcofactor(tmp, j, i) * inv_det;      /* transposed */
                }
        }
+       return 0;
 }
 
 static inline void cgm_mtranslation(float *m, float x, float y, float z)
@@ -358,3 +368,112 @@ static inline void cgm_mprerotate_euler(float *m, float a, float b, float c, int
        cgm_mrotation_euler(m, a, b, c, mode);
        cgm_mmul(m, tmp);
 }
+
+
+static inline void cgm_mget_translation(const float *m, cgm_vec3 *res)
+{
+       res->x = m[12];
+       res->y = m[13];
+       res->z = m[14];
+}
+
+/* Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
+ * article "Quaternion Calculus and Fast Animation".
+ * adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl
+ */
+static inline void cgm_mget_rotation(const float *m, cgm_quat *res)
+{
+       static const int next[3] = {1, 2, 0};
+       float quat[4];
+       int i, j, k;
+
+       float trace = m[0] + m[5] + m[10];
+       float root;
+
+       if(trace > 0.0f) {
+               // |w| > 1/2
+               root = sqrt(trace + 1.0f);      // 2w
+               res->w = 0.5f * root;
+               root = 0.5f / root;     // 1 / 4w
+               res->x = (m[6] - m[9]) * root;
+               res->y = (m[8] - m[2]) * root;
+               res->z = (m[1] - m[4]) * root;
+       } else {
+               // |w| <= 1/2
+               i = 0;
+               if(m[5] > m[0]) {
+                       i = 1;
+               }
+               if(m[10] > m[i * 4 + i]) {
+                       i = 2;
+               }
+               j = next[i];
+               k = next[j];
+
+               root = sqrt(m[i * 4 + i] - m[j * 4 + j] - m[k * 4 + k] + 1.0f);
+               quat[i + 1] = 0.5f * root;
+               root = 0.5f / root;
+               quat[0] = (m[j + 4 + k] - m[k * 4 + j]) * root;
+               quat[j + 1] = (m[i * 4 + j] - m[j * 4 + i]) * root;
+               quat[k + 1] = (m[i * 4 + k] - m[k * 4 + i]) * root;
+               res->w = quat[0];
+               res->x = quat[1];
+               res->y = quat[2];
+               res->z = quat[3];
+       }
+}
+
+static inline void cgm_mget_scaling(const float *m, cgm_vec3 *res)
+{
+       res->x = sqrt(m[0] * m[0] + m[4] * m[4] + m[8] * m[8]);
+       res->y = sqrt(m[1] * m[1] + m[5] * m[5] + m[9] * m[9]);
+       res->z = sqrt(m[2] * m[2] + m[6] * m[6] + m[10] * m[10]);
+}
+
+static inline void cgm_mget_frustum_plane(const float *m, int p, cgm_vec4 *res)
+{
+       int row = p >> 1;
+       const float *rowptr = m + row * 4;
+
+       if((p & 1) == 0) {
+               res->x = m[12] + rowptr[0];
+               res->y = m[13] + rowptr[1];
+               res->z = m[14] + rowptr[2];
+               res->w = m[15] + rowptr[3];
+       } else {
+               res->x = m[12] - rowptr[0];
+               res->y = m[13] - rowptr[1];
+               res->z = m[14] - rowptr[2];
+               res->w = m[15] - rowptr[3];
+       }
+}
+
+static inline void cgm_mlookat(float *m, const cgm_vec3 *pos, const cgm_vec3 *targ,
+               const cgm_vec3 *up)
+{
+       float rot[16], trans[16];
+       cgm_vec3 dir = *targ, right, vup;
+
+       cgm_vsub(&dir, pos);
+       cgm_vnormalize(&dir);
+       cgm_vcross(&right, &dir, up);
+       cgm_vnormalize(&right);
+       cgm_vcross(&vup, &right, &dir);
+       cgm_vnormalize(&vup);
+
+
+       rot[0] = right.x;
+       /* TODO cont. */
+}
+
+static inline void cgm_minv_lookat(float *m, const cgm_vec3 *pos, const cgm_vec3 *targ,
+               const cgm_vec3 *up);
+
+static inline void cgm_mortho(float *m, float left, float right, float bot, float top,
+               float znear, float zfar);
+static inline void cgm_mfrustum(float *m, float left, float right, float bot, float top,
+               float znear, float zfar);
+static inline void cgm_mperspective(float *m, float vfov, float aspect, float znear, float zfar);
+
+static inline void cgm_mmirror(float *m, float a, float b, float c, float d);
+