Matrix inversion

fast_matrix.hpp

@@ -6,7 +6,8 @@
 #include<boost/simd/constant/one.hpp>
 #include<boost/simd/function/aligned_store.hpp>
 #include<boost/simd/function/dot.hpp>
-#include <boost/simd/function/shuffle.hpp>
+#include<boost/simd/function/shuffle.hpp>
+#include<boost/simd/function/none.hpp>
 
 namespace bs = boost::simd;
 
@@ -19,6 +20,9 @@ constexpr int nearest_power_of_two(int min_value, int current_value) {
 template <typename T, int MatrixSize, int MaxVecSize>
 class BaseMatrix;
 
+template <typename T, int MatrixSize, int MaxVecSize>
+class IdentityMatrix;
+
 template<typename T, int MatrixSize, int MaxVecSize>
 class Matrix: public BaseMatrix<T, MatrixSize, MaxVecSize> {
 };
@@ -100,7 +104,7 @@ static inline void matrix_mul_m_m(M &a, M &b, M &c) {
 }
 
 template <typename M>
-static inline void matrix_mul_tm_m(M &a, M &b, M &c) {
+static inline void matrix_mul_mt_m(M &a, M &b, M &c) {
 /*
  * Computes the following matrix product C = t(A)*B
  * where A, B and C are BaseMatrix objects.
@@ -174,27 +178,58 @@ static constexpr int blend_for_size(int i, int c) {
 }
 
 template <typename pack_t, int Size>
-void protect_for_division(pack_t &vector) {
+pack_t& protect_for_division(pack_t &vector) {
     vector = bs::shuffle<bs::pattern<blend_for_size<Size>>>(vector, bs::One<pack_t>());
+    return vector;
 }
 
 template <typename M, int N>
-void matrix_inv(M &matrix, M &inverse) {
+void matrix_inv(M &A, M &X) {
     using pack_t = bs::pack<typename M::ElementType, M::VecSize>;
+    static BaseMatrix<typename M::ElementType, M::MatSize, M::MaxVecSize> Id(1);
+    M R, P, AP, AlphaP, AlphaAP;
+    pack_t alpha, rs_old, rs_new, tmp;
+
+    X = Id;
+    matrix_sub(Id, A, R);
+    P = R;
+
+    rs_old = column_dot_product(R, R);
+    for (int i=0; i<N && !bs::none(rs_old); i++) {
+        matrix_mul_m_m(A, P, AP);
+        tmp = column_dot_product(P, AP);
+        alpha = rs_old / protect_for_division<pack_t, M::MatSize>(tmp);
+        column_scalar_product(alpha, P, AlphaP);
+        matrix_add(X, AlphaP, X);
+        column_scalar_product(alpha, AP, AlphaAP);
+        matrix_sub(R, AlphaAP, R);
+        rs_new = column_dot_product(R, R);
+        tmp = rs_new / protect_for_division<pack_t, M::MatSize>(rs_old);
+        column_scalar_product(tmp, P, P);
+        matrix_add(R, P, P);
+        rs_old = rs_new;
+    } /*
+    matrix_mul_m_m(A, P, AP);
+    tmp = column_dot_product(P, AP);
+    alpha = rs_old / protect_for_division<pack_t, M::MatSize>(tmp);
+    column_scalar_product(alpha, P, AlphaP);
+    matrix_add(X, AlphaP, X);
+    */
 }
 
-template <typename T, int MatrixSize, int MaxVecSize>
+template <typename T, int MatrixSize, int MaximumVectorSize>
 class BaseMatrix {
 /* Class for matrix object
  * Matrix object are linearized 2D array with padded line to fit vector size
  */
-    static_assert(MatrixSize <= MaxVecSize, "Matrix size must be less than maximum vector size");
-    static_assert((MaxVecSize >= 4) & !(MaxVecSize & (MaxVecSize - 1)), "Maximum vector size must be a power of two.");
+    static_assert(MatrixSize <= MaximumVectorSize, "Matrix size must be less than maximum vector size");
+    static_assert((MaximumVectorSize >= 4) & !(MaximumVectorSize & (MaximumVectorSize - 1)), "Maximum vector size must be a power of two.");
 
     protected:
     static constexpr int MatSize = MatrixSize;
+    static constexpr int MaxVecSize = MaximumVectorSize;
     // We use the smallest vector size possible
-    static constexpr int VecSize = nearest_power_of_two(MatrixSize, MaxVecSize);
+    static constexpr int VecSize = nearest_power_of_two(MatrixSize, MaximumVectorSize);
 
     using matrix_t = BaseMatrix<T, MatrixSize, MaxVecSize>;
     using pack_t = bs::pack<T, VecSize>;
@@ -202,7 +237,7 @@ class BaseMatrix {
 
     public:
     BaseMatrix() = default;
-    BaseMatrix(float const a[]) {
+    BaseMatrix(T const a[]) {
         if (MatSize == VecSize) { // In this case there is no padding, we can copy directly the array
             std::memcpy(this->array, a, sizeof(T)*MatSize*VecSize);
         }
@@ -213,7 +248,13 @@ class BaseMatrix {
         }
     }
 
-    matrix_t operator=(float const a[]) {
+    BaseMatrix(T const a) {
+        for (int i=0; i<MatrixSize; i++) {
+            array[VecSize*i+i] = a;
+        }
+    }
+
+    matrix_t operator=(T const a[]) {
         if (MatSize == VecSize) {
             std::memcpy(this->array, a, sizeof(T)*MatSize*VecSize);
         }
@@ -243,20 +284,22 @@ class BaseMatrix {
     }
 
 
+    private:
     friend void matrix_add<matrix_t>(matrix_t &a, matrix_t &b, matrix_t &c);
     friend void matrix_sub<matrix_t>(matrix_t &a, matrix_t &b, matrix_t &c);
     friend void matrix_mul_m_m<matrix_t>(matrix_t &a, matrix_t &b, matrix_t &c);
-    friend void matrix_mul_tm_m<matrix_t>(matrix_t &a, matrix_t &b, matrix_t &c);
+    friend void matrix_mul_mt_m<matrix_t>(matrix_t &a, matrix_t &b, matrix_t &c);
     friend void matrix_mul_m_mt<matrix_t>(matrix_t &a, matrix_t &b, matrix_t &c);
     friend bs::pack<ElementType, VecSize> column_dot_product<matrix_t>(matrix_t &a, matrix_t &b);
     friend void column_scalar_product<matrix_t, pack_t>(pack_t &scalar, matrix_t &b, matrix_t &c);
+    template <typename M, int N> friend void matrix_inv(M &a, M &b);
     friend std::ostream & operator<<<T, MatrixSize, MaxVecSize>(std::ostream &os, matrix_t &mat);
 
     protected:
     alignas(sizeof(T)*VecSize) float array[MatSize*VecSize] = {0};
 
 };
-
+/*
 template<typename T, int MatrixSize, int MaxVecSize>
 class IdentityMatrix: public BaseMatrix<T, MatrixSize, MaxVecSize> {
     using BaseMatrix<T, MatrixSize, MaxVecSize>::VecSize;
@@ -267,4 +310,4 @@ class IdentityMatrix: public BaseMatrix<T, MatrixSize, MaxVecSize> {
             this->array[VecSize*i+i] = 1;
         }
     }
-};
+};*/