From eeb00bf74c64c2e8be34d328499612c92fdde9c1 Mon Sep 17 00:00:00 2001
From: Eric Wasylishen <ewasylishen@gmail.com>
Date: Sun, 23 Apr 2017 17:41:19 -0600
Subject: [PATCH] common: tidy qmat

---
 common/qvec.cc         | 28 +++++------------
 include/common/qvec.hh | 69 ++++++++++++++++++++++--------------------
 light/test_light.cc    |  8 ++---
 3 files changed, 49 insertions(+), 56 deletions(-)

diff --git a/common/qvec.cc b/common/qvec.cc
index 384fcbcf..a5ce66e7 100644
--- a/common/qvec.cc
+++ b/common/qvec.cc
@@ -104,22 +104,9 @@ static bool gluInvertMatrixd(const double m[16], double invOut[16])
 
 qmat4x4d qv::invert(const qmat4x4d &input, bool *ok)
 {
-    double flat_in[16];
-    double flat_out[16];
-    
-    for (int i=0; i<4; i++)
-        for (int j=0; j<4; j++)
-            flat_in[4*i+j] = input[i][j];
-    
-    *ok = gluInvertMatrixd(flat_in, flat_out);
-    if (!*ok)
-        return qmat4x4d();
-    
-    qmat4x4d result;
-    for (int i=0; i<4; i++)
-        for (int j=0; j<4; j++)
-            result[i][j] = flat_out[4*i+j];
-    return result;
+    qmat4x4d res;
+    *ok = gluInvertMatrixd(input.m_values, res.m_values);
+    return res;
 }
 
 qmat4x4f qv::invert(const qmat4x4f &input, bool *ok)
@@ -129,10 +116,11 @@ qmat4x4f qv::invert(const qmat4x4f &input, bool *ok)
 
 qmat2x2f qv::invert(const qmat2x2f &m, bool *ok)
 {
-    float a = m[0][0];
-    float b = m[1][0];
-    float c = m[0][1];
-    float d = m[1][1];
+    // http://www.mathwords.com/i/inverse_of_a_matrix.htm
+    float a = m.at(0,0);
+    float b = m.at(0,1);
+    float c = m.at(1,0);
+    float d = m.at(1,1);
     
     float det = a*d - b*c;
     if (det == 0) {
diff --git a/include/common/qvec.hh b/include/common/qvec.hh
index c5f6893b..8bca1fa7 100644
--- a/include/common/qvec.hh
+++ b/include/common/qvec.hh
@@ -238,33 +238,36 @@ using qplane3f = qplane3<float>;
 using qplane3d = qplane3<double>;
 
 /**
- * M row, N column matrix
+ * M row, N column matrix.
  */
 template <int M, int N, class T>
 class qmat {
-protected:
-    qvec<M, T> m_cols[N];
+public:
+    /** 
+     * Column-major order. [ (row0,col0), (row1,col0), .. ]
+     */
+    T m_values[M*N];
     
 public:
     /**
      * Identity matrix if square, otherwise fill with 0
      */
     qmat() {
-        for (int i=0; i<N; i++)
-            m_cols[i] = qvec<M, T>(0);
+        for (int i=0; i<M*N; i++)
+            m_values[i] = 0;
         
         if (M == N) {
             // identity matrix
             for (int i=0; i<N; i++) {
-                m_cols[i][i] = 1;
+                this->at(i,i) = 1;
             }
         }
     }
     
     // copy constructor
     qmat(const qmat<M,N,T> &other) {
-        for (int i=0; i<N; i++)
-            this->m_cols[i] = other.m_cols[i];
+        for (int i=0; i<M*N; i++)
+            this->m_values[i] = other.m_values[i];
     }
 
     /**
@@ -272,47 +275,49 @@ public:
      */
     template <class T2>
     qmat(const qmat<M, N, T2> &other) {
-        for (int i=0; i<N; i++)
-            m_cols[i] = qvec<M, T>(other[i]);
+        for (int i=0; i<M*N; i++)
+            this->m_values[i] = static_cast<T>(other.m_values[i]);
     }
 
-    // initializer list
+    // initializer list, column-major order
     qmat(std::initializer_list<T> list) {
         assert(list.size() == M*N);
         const T *listPtr = list.begin();
         
-        for (int j=0; j<N; j++) {
-            for (int i=0; i<M; i++) {
-                this->m_cols[j][i] = listPtr[(j*M)+i];
-            }
+        for (int i=0; i<M*N; i++) {
+            this->m_values[i] = listPtr[i];
         }
     }
 
     bool operator==(const qmat<M,N,T> &other) const {
-        for (int i=0; i<N; i++)
-            if (this->m_cols[i] != other.m_cols[i])
+        for (int i=0; i<M*N; i++)
+            if (this->m_values[i] != other.m_values[i])
                 return false;
         return true;
     }
     
-    // access to columns
+    // access to elements
     
-    qvec<M, T> operator[](const size_t idx) const {
-        assert(idx >= 0 && idx < N);
-        return m_cols[idx];
+    T& at(int row, int col) {
+        assert(row >= 0 && row < M);
+        assert(col >= 0 && col < N);
+        return m_values[col * M + row];
     }
     
-    qvec<M, T> &operator[](const size_t idx) {
-        assert(idx >= 0 && idx < N);
-        return m_cols[idx];
+    T at(int row, int col) const {
+        assert(row >= 0 && row < M);
+        assert(col >= 0 && col < N);
+        return m_values[col * M + row];
     }
     
     // multiplication by a vector
     
     qvec<M,T> operator*(const qvec<N, T> &vec) const {
-        qvec<M,T> res;
-        for (int j=0; j<N; j++) {
-            res += this->m_cols[j] * vec[j];
+        qvec<M,T> res(0);
+        for (int i=0; i<M; i++) { // for each row
+            for (int j=0; j<N; j++) { // for each col
+                res[i] += this->at(i, j) * vec[j];
+            }
         }
         return res;
     }
@@ -326,9 +331,9 @@ public:
             for (int j=0; j<P; j++) {
                 T val = 0;
                 for (int k=0; k<N; k++) {
-                    val += (*this)[i][k] * other[k][j];
+                    val += this->at(i,k) * other.at(k,j);
                 }
-                res[i][j] = val;
+                res.at(i,j) = val;
             }
         }
         return res;
@@ -337,9 +342,9 @@ public:
     // multiplication by a scalar
     
     qmat<M,N,T> operator*(const T scalar) const {
-        qmat<M,N,T> res;
-        for (int j=0; j<N; j++) {
-            res[j] = this->m_cols[j] * scalar;
+        qmat<M,N,T> res(*this);
+        for (int i=0; i<M*N; i++) {
+            res.m_values[i] *= scalar;
         }
         return res;
     }
diff --git a/light/test_light.cc b/light/test_light.cc
index 897b01de..d8c11882 100644
--- a/light/test_light.cc
+++ b/light/test_light.cc
@@ -875,7 +875,7 @@ TEST(qvec, matrix2x2inv) {
     qmat2x2f randMat;
     for (int i=0; i<2; i++)
         for (int j=0; j<2; j++)
-            randMat[i][j] = dis(engine);
+            randMat.at(i,j) = dis(engine);
     
     bool ok;
     qmat2x2f randInv = qv::invert(randMat, &ok);
@@ -885,7 +885,7 @@ TEST(qvec, matrix2x2inv) {
     for (int i=0; i<2; i++) {
         for (int j=0; j<2; j++) {
             float exp = (i == j) ? 1.0f : 0.0f;
-            ASSERT_TRUE(fabs(exp - prod[i][j]) < 0.001);
+            ASSERT_TRUE(fabs(exp - prod.at(i,j)) < 0.001);
         }
     }
 }
@@ -897,7 +897,7 @@ TEST(qvec, matrix4x4inv) {
     qmat4x4f randMat;
     for (int i=0; i<4; i++)
         for (int j=0; j<4; j++)
-            randMat[i][j] = dis(engine);
+            randMat.at(i,j) = dis(engine);
     
     bool ok;
     qmat4x4f randInv = qv::invert(randMat, &ok);
@@ -907,7 +907,7 @@ TEST(qvec, matrix4x4inv) {
     for (int i=0; i<4; i++) {
         for (int j=0; j<4; j++) {
             float exp = (i == j) ? 1.0f : 0.0f;
-            ASSERT_TRUE(fabs(exp - prod[i][j]) < 0.001);
+            ASSERT_TRUE(fabs(exp - prod.at(i,j)) < 0.001);
         }
     }
 }