Adding column-wise operations

fast_matrix.hpp

@@ -3,8 +3,10 @@
 
 #include<boost/simd/pack.hpp>
 #include<boost/simd/constant/zero.hpp>
+#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>
 
 namespace bs = boost::simd;
 
@@ -47,6 +49,26 @@ std::ostream & operator<<(std::ostream &os, BaseMatrix<T, MatrixSize, MaxVecSize
     return os;
 }
 
+template <typename M>
+static inline void matrix_add(M &a, M &b, M &c) {
+    using pack_t = typename M::pack_t;
+    for (int i=0; i<M::MatSize; i++) {
+        int index = M::VecSize*i;
+        pack_t row_a(&a.array[index]), row_b(&b.array[index]);
+        bs::aligned_store(row_a + row_b, &c.array[index]);
+    }
+}
+
+template <typename M>
+static inline void matrix_sub(M &a, M &b, M &c) {
+    using pack_t = typename M::pack_t;
+    for (int i=0; i<M::MatSize; i++) {
+        int index = M::VecSize*i;
+        pack_t row_a(&a.array[index]), row_b(&b.array[index]);
+        bs::aligned_store(row_a - row_b, &c.array[index]);
+    }
+}
+
 template <typename M>
 static inline void matrix_mul_m_m(M &a, M &b, M &c) {
 /*
@@ -126,6 +148,40 @@ static inline void matrix_mul_m_mt(M &a, M &b, M &c) {
     }
 }
 
+template <typename M>
+bs::pack<typename M::ElementType, M::VecSize> column_dot_product(M &a, M &b) {
+    using pack_t = bs::pack<typename M::ElementType, M::VecSize>;
+    pack_t res = bs::Zero<pack_t>();
+    for (int i=0; i < M::MatSize; i++) {
+        pack_t col_a(&a.array[M::VecSize*i]), col_b(&b.array[M::VecSize*i]);
+        res += col_a * col_b;
+    }
+    return res;
+}
+
+template <typename M, typename pack_t>
+void column_scalar_product(pack_t &scalar, M &a, M &b) {
+    for (int i=0; i < M::MatSize; i++) {
+        pack_t row(&a.array[M::VecSize*i]);
+        bs::aligned_store(row*scalar, &b.array[M::VecSize*i]);
+    }
+}
+
+template <int Size>
+static constexpr int blend_for_size(int i, int c) {
+/* Utility function for proctect_for_division */
+    return i < Size ? i : i+c;
+}
+
+template <typename pack_t, int Size>
+void protect_for_division(pack_t &vector) {
+    vector = bs::shuffle<bs::pattern<blend_for_size<Size>>>(vector, bs::One<pack_t>());
+}
+
+template <typename M, int N>
+void matrix_inv(M &matrix, M &inverse) {
+    using pack_t = bs::pack<typename M::ElementType, M::VecSize>;
+}
 
 template <typename T, int MatrixSize, int MaxVecSize>
 class BaseMatrix {
@@ -135,11 +191,15 @@ class BaseMatrix {
     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.");
 
-    using matrix_t = BaseMatrix<T, MatrixSize, MaxVecSize>;
-    using ElementType = T;
+    protected:
     static constexpr int MatSize = MatrixSize;
     // We use the smallest vector size possible
     static constexpr int VecSize = nearest_power_of_two(MatrixSize, MaxVecSize);
+
+    using matrix_t = BaseMatrix<T, MatrixSize, MaxVecSize>;
+    using pack_t = bs::pack<T, VecSize>;
+    using ElementType = T;
+
     public:
     BaseMatrix() = default;
     BaseMatrix(float const a[]) {
@@ -169,13 +229,42 @@ class BaseMatrix {
         return *this;
     }
 
+    ElementType* dump_array() {
+        ElementType* returned_array = new ElementType[MatSize*MatSize];
+        if (MatSize == VecSize) { // In this case there is no padding, we can copy directly the array
+            std::memcpy(returned_array, this->array, sizeof(T)*MatSize*VecSize);
+        }
+        else {
+            for (int i=0; i<MatSize; i++) {
+                std::memcpy(&returned_array[i*MatSize], &this->array[i*VecSize], sizeof(T)*MatSize);
+            }
+        }
+        return returned_array;
+    }
 
+
+    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_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);
     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;
+
+    public:
+    IdentityMatrix() {
+        for (int i=0; i<MatrixSize; i++) {
+            this->array[VecSize*i+i] = 1;
+        }
+    }
+};