matrix: adding permutation matrix computing

src/lol/math/matrix.h

@@ -570,6 +570,52 @@ T const & determinant(mat_t<T, 1, 1> const &m)
     return m[0][0];
 }
 
+/*
+ * Compute permutation matrix
+ */
+
+template<typename T, int N>
+mat_t<T, N, N> p_matrix(mat_t<T, N, N> const & m)
+{
+    int used[N];
+    int order[N];
+
+    for (int i = 0 ; i < N ; ++i)
+    {
+        used[i] = 0;
+        order[i] = -1;
+    }
+
+    for (int i = 0 ; i < N ; ++i)
+    {
+        int index_max = -1;
+
+        for (int j = 0 ; j < N ; ++j)
+        {
+            while (j < N && used[j])
+                ++j;
+
+            if (j >= N)
+                break;
+
+            if (index_max == -1 || m[i][j] > m[i][index_max])
+                index_max = j;
+        }
+
+        ASSERT(index_max != -1);
+        ASSERT(index_max < N);
+
+        order[i] = index_max;
+        used[index_max] = 1;
+    }
+
+    mat_t<T, N, N> P;
+    for (int i = 0 ; i < N ; ++i)
+        P[order[i]][i] = 1.0f;
+
+    return P;
+}
+
 /*
  * Compute L matrix inverse
  */

src/t/math/matrix.cpp

@@ -95,6 +95,47 @@ lolunit_declare_fixture(MatrixTest)
             lolunit_assert_doubles_equal(m2[i][j], mat2(1.f)[i][j], 1e-5);
     }
 
+    lolunit_declare_test(PMatrixTest)
+    {
+        mat4 m1(vec4(0,  1,  0,  0),
+               vec4(1,  0,  0,  0),
+               vec4(0,  0,  0,  1),
+               vec4(0,  0,  1,  0));
+
+        mat4 P1 = p_matrix(m1);
+        P1 = P1 * m1;
+
+        for (int i = 0 ; i < 4 ; ++i)
+        {
+            for (int j = 0 ; j < 4 ; ++j)
+            {
+                if (i == j)
+                    lolunit_assert_equal(P1[i][j], 1);
+                else
+                    lolunit_assert_equal(P1[i][j], 0);
+            }
+        }
+
+        mat4 m2(vec4(0,  1,  0,  0),
+               vec4(0,  0,  1,  0),
+               vec4(0,  0,  0,  1),
+               vec4(1,  0,  0,  0));
+
+        mat4 P2 = p_matrix(m2);
+        P2 = P2 * m2;
+
+        for (int i = 0 ; i < 4 ; ++i)
+        {
+            for (int j = 0 ; j < 4 ; ++j)
+            {
+                if (i == j)
+                    lolunit_assert_equal(P2[i][j], 1);
+                else
+                    lolunit_assert_equal(P2[i][j], 0);
+            }
+        }
+    }
+
 #if 0 /* XXX: LU decomposition is currently broken */
     lolunit_declare_test(lu_decomposition_3x3)
     {