a

Dependencies:   mbed mbed-rtos

Revision:
1:ceee5a608e7c
diff -r c3e774091195 -r ceee5a608e7c include/Linalg/linalg.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Linalg/linalg.h	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,473 @@
+#ifndef LINALG_H
+#define LINALG_H
+
+// #include <mbed.h>
+#include <stdint.h>
+#include <array>
+
+namespace linalg
+{
+    template <class T, uint32_t M, uint32_t N>
+    class CMatrix
+    {
+    public:
+        using CThisType = CMatrix<T,M,N>;
+        using CTransposeType = CMatrix<T,N,M>;
+        using CContainerType = std::array<std::array<T,N>,M>;
+        using CDataType =T;
+
+        template <uint32_t P>
+        using CLeftMultipliableType = CMatrix<T,P,M>;
+
+        template <uint32_t P>
+        using CRightMultipliableType = CMatrix<T,N,P>;
+
+        template <uint32_t P>
+        using CLeftMultiplicationResultType = CMatrix<T,P,N>;
+
+        template <uint32_t P>
+        using CRightMultiplicationResultType = CMatrix<T,M,P>;
+
+        friend class CMatrix<T,N,M>; 
+
+        CMatrix() : m_data() {}
+        CMatrix(const CThisType& f_matrix) : m_data(f_matrix.m_data) {}
+        CMatrix(const CThisType&& f_matrix) : m_data(f_matrix.m_data) {}
+        CMatrix(const CContainerType& f_data) : m_data(f_data) {}
+        CMatrix(const CContainerType&& f_data) : m_data(f_data) {}
+
+        CThisType& operator=(const CThisType& f_matrix)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] = f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return *this;
+        }
+        CThisType& operator=(const CThisType&& f_matrix)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] = f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return *this;
+        }
+
+        std::array<T,N>& operator[](uint32_t f_row)
+        {
+            return m_data[f_row];
+        }
+
+        CDataType& operator()(uint32_t f_row, uint32_t f_col)
+        {
+            return m_data[f_row][f_col];
+        }
+
+        const std::array<T,N>& operator[](uint32_t f_row) const
+        {
+            return m_data[f_row];
+        }
+
+        const CDataType& operator()(uint32_t f_row, uint32_t f_col) const
+        {
+            return m_data[f_row][f_col];
+        }
+
+        // template<>
+        // CMatrix<T,1,1>::operator CDataType()
+        // {
+        //     return m_data[0][0];
+        // }
+
+        CThisType& operator+() {return *this;}
+        CThisType operator-()
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = -this->m_data[l_row][l_col];
+                }
+            }
+            return l_matrix;
+        }
+        CThisType operator+(const CThisType& f_matrix)
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] + f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return l_matrix;
+        }
+        CThisType operator-(const CThisType& f_matrix)
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] - f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return l_matrix;
+        }
+        CThisType& operator+=(const CThisType& f_matrix)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] += f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return *this;
+        }
+        CThisType& operator-=(const CThisType& f_matrix)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] -= f_matrix.m_data[l_row][l_col];
+                }
+            }
+            return *this;
+        }
+
+        CThisType operator+(const CDataType& f_val)
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] + f_val;
+                }
+            }
+            return l_matrix;
+        }
+        CThisType operator-(const CDataType& f_val)
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] - f_val;
+                }
+            }
+            return l_matrix;
+        }
+        CThisType& operator+=(const CDataType& f_val)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] += f_val;
+                }
+            }
+            return *this;
+        }
+        CThisType& operator-=(const CDataType& f_val)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] -= f_val;
+                }
+            }
+            return *this;
+        }
+        CThisType& operator*=(const CDataType& f_val)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] *= f_val;
+                }
+            }
+            return *this;
+        }
+        CThisType& operator*=(const CThisType& f_val)
+        {
+            CThisType& l_thisRef(*this);
+            l_thisRef = l_thisRef * f_val;
+            return l_thisRef;
+        }
+        CThisType& operator/=(const CDataType& f_val)
+        {
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    this->m_data[l_row][l_col] /= f_val;
+                }
+            }
+            return *this;
+        }
+        CThisType operator*(const CDataType& f_val)
+        {
+            CThisType l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_matrix.m_data[l_row][l_col] = this->m_data[l_row][l_col] * f_val;
+                }
+            }
+            return l_matrix;
+        }
+        template <uint32_t P>
+        CRightMultiplicationResultType<P> operator*(const CRightMultipliableType<P>& f_matrix)
+        {
+            CRightMultiplicationResultType<P> l_matrix;
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < P; ++l_col)
+                {
+                    l_matrix[l_row][l_col] = 0;
+                    for (uint32_t l_idx = 0; l_idx < N; ++l_idx)
+                    {
+                        l_matrix[l_row][l_col] += this->m_data[l_row][l_idx] * f_matrix[l_idx][l_col];
+                    }
+                }
+            }
+            return l_matrix;
+        }
+        CThisType inv();
+
+        CTransposeType transpose()
+        {
+            CTransposeType l_trsp;
+
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_trsp.m_data[l_col][l_row] = this->m_data[l_row][l_col];
+                }
+            }
+
+            return l_trsp;
+        }
+
+        template <uint32_t P>
+        CRightMultiplicationResultType<P> solve(const CRightMultipliableType<P>& f_B)
+        {
+            return CLUDecomposition(*this).solve();
+        }
+
+        static CThisType zeros()
+        {
+            CThisType l_res;
+
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_res[l_row][l_col] = 0;
+                }
+            }
+
+            return l_res;
+        }
+
+        static CThisType ones()
+        {
+            CThisType l_res;
+
+            for (uint32_t l_row = 0; l_row < M; ++l_row)
+            {
+                for (uint32_t l_col = 0; l_col < N; ++l_col)
+                {
+                    l_res[l_row][l_col] = 1;
+                }
+            }
+
+            return l_res;
+        }
+
+        static CThisType eye()
+        {
+            CThisType l_res(CThisType::zeros());
+            uint32_t l_minDim = std::min(M,N);
+
+            for (uint32_t l_row = 0; l_row < l_minDim; ++l_row)
+            {
+                l_res[l_row][l_row] = 1;
+            }
+
+            return l_res;
+        }
+
+    protected:
+        std::array<std::array<T,N>,M> m_data;
+    };
+
+    template<class T, uint32_t N>
+    class CLUDecomposition
+    {
+    public:
+        using CThisType = CLUDecomposition<T,N>;
+        using COriginalType = CMatrix<T,N,N>;
+        using CDataType =T;
+
+        template <uint32_t P>
+        using CLeftMultipliableType = CMatrix<T,P,N>;
+
+        template <uint32_t P>
+        using CRightMultipliableType = CMatrix<T,N,P>;
+
+        template <uint32_t P>
+        using CLeftMultiplicationResultType = CMatrix<T,P,N>;
+
+        template <uint32_t P>
+        using CRightMultiplicationResultType = CMatrix<T,N,P>;
+
+        CLUDecomposition(const CThisType& f_decomposition) : m_L(f_decomposition.m_L), m_U(f_decomposition.m_U), m_P(f_decomposition.m_P) {}
+        CLUDecomposition(const CThisType&& f_decomposition) : m_L(f_decomposition.m_L), m_U(f_decomposition.m_U), m_P(f_decomposition.m_P) {}
+        CLUDecomposition(const COriginalType& f_matrix) : m_L(), m_U(), m_P() {decompose(f_matrix);}
+        CLUDecomposition(const COriginalType&& f_matrix) : m_L(), m_U(), m_P() {decompose(f_matrix);}
+
+        operator COriginalType()
+        {
+            return m_L * m_U;
+        }
+
+        COriginalType inv()
+        {
+            return triUInv() * triLInv();
+        }
+
+        COriginalType triLInv()
+        {
+            COriginalType l_invL(m_L);
+
+            for (uint32_t l_jdx = 0; l_jdx < N; ++l_jdx)
+            {
+                l_invL[l_jdx][l_jdx] = 1/l_invL[l_jdx][l_jdx];
+
+                for (uint32_t l_idx = l_jdx+1; l_idx < N; ++l_idx)
+                {
+                    l_invL[l_idx][l_jdx] = 0;
+
+                    for (uint32_t l_kdx = l_jdx; l_kdx < l_idx; ++l_kdx)
+                    {
+                        l_invL[l_idx][l_jdx] -= m_L[l_idx][l_kdx]*l_invL[l_kdx][l_jdx];
+                    }
+
+                    l_invL[l_idx][l_jdx] /= l_invL[l_idx][l_idx];
+                }
+            }
+
+            return l_invL;
+        }
+
+        COriginalType triUInv()
+        {
+            COriginalType l_invU(m_U);
+
+            for (int32_t l_jdx = (N-1); l_jdx >= 0; --l_jdx)
+            {
+                l_invU[l_jdx][l_jdx] = 1/l_invU[l_jdx][l_jdx];
+
+                for (int32_t l_idx = (l_jdx-1); l_idx >= 0; --l_idx)
+                {
+                    l_invU[l_idx][l_jdx] = 0;
+
+                    for (int32_t l_kdx = (l_idx+1); l_kdx < (l_jdx+1); ++l_kdx)
+                    {
+                        l_invU[l_idx][l_jdx] -= m_U[l_idx][l_kdx]*l_invU[l_kdx][l_jdx];
+                    }
+
+                    l_invU[l_idx][l_jdx] /= l_invU[l_idx][l_idx];
+                }
+            }
+
+            return l_invU;
+        }
+
+        template <uint32_t P>
+        CRightMultiplicationResultType<P> solve(const CRightMultipliableType<P>& f_B)
+        {
+            CRightMultipliableType<P> l_B;
+
+            for (uint32_t l_idx = 0; l_idx < N; ++l_idx)
+            {
+                for (uint32_t l_jdx = 0; l_jdx < P; ++l_jdx)
+                {
+                    l_B[l_idx][l_jdx] = f_B[m_P[l_idx]][l_jdx];
+                }
+            }
+
+            return sTriL(sTriU(l_B));
+        }
+
+        template <uint32_t P>
+        CRightMultiplicationResultType<P> sTriU(const CRightMultipliableType<P>& f_B)
+        {
+            return triUInv()*f_B;
+        }
+
+        template <uint32_t P>
+        CRightMultiplicationResultType<P> sTriL(const CRightMultipliableType<P>& f_B)
+        {
+            // return triLInv()*f_B;
+        }
+
+
+    // private:
+        void decompose(const CMatrix<T,N,N>& f_matrix)
+                {
+            m_U = f_matrix;
+            for (uint32_t l_kdx = 0; l_kdx < (N-1); ++l_kdx)
+            {
+                m_L[l_kdx][l_kdx] = 1;
+                m_P[l_kdx] = l_kdx;
+
+                for(uint32_t l_idx = l_kdx+1; l_idx < N; ++l_idx)
+                {
+                    m_L[l_idx][l_kdx] = m_U[l_idx][l_kdx]/m_U[l_kdx][l_kdx];
+                    m_U[l_idx][l_kdx] = 0;
+                    for(uint32_t l_jdx = l_kdx+1; l_jdx < N; ++l_jdx)
+                    {
+                        m_U[l_idx][l_jdx] = m_U[l_idx][l_jdx] - m_L[l_idx][l_kdx]*m_U[l_kdx][l_jdx];
+                    }
+                }
+            }
+            m_L[N-1][N-1] = 1;
+            m_P[N-1] = N-1;
+        }
+        CMatrix<T,N,N> m_L;
+        CMatrix<T,N,N> m_U;
+        std::array<uint32_t,N> m_P;
+    };
+
+    template <class T, uint32_t N>
+    using CColVector = CMatrix<T,N,1>;
+
+    template <class T, uint32_t N>
+    using CVector = CColVector<T,N>;
+
+    template <class T, uint32_t N>
+    using CRowVector = CMatrix<T,1,N>;
+};
+
+#include "linalg.inl"
+
+#endif // LINALG_H
+