Diff: matrix.h

Revision:

0:22c544c8741a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/matrix.h	Wed Sep 16 19:48:33 2015 +0000
@@ -0,0 +1,248 @@
+/*
+    Inertial Measurement Unit Maths Library
+    Copyright (C) 2013-2014  Samuel Cowen
+    www.camelsoftware.com
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef IMUMATH_MATRIX_HPP
+#define IMUMATH_MATRIX_HPP
+
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <math.h>
+
+namespace imu
+{
+
+
+template <uint8_t N> class Matrix
+{
+public:
+    Matrix()
+    {
+        int r = sizeof(double)*N;
+        _cell = &_cell_data[0];
+        memset(_cell, 0, r*r);
+    }
+
+    Matrix(const Matrix &v)
+    {
+        int r = sizeof(double)*N;
+        _cell = &_cell_data[0];
+        memset(_cell, 0, r*r);
+        for (int x = 0; x < N; x++ )
+        {
+            for(int y = 0; y < N; y++)
+            {
+                _cell[x*N+y] = v._cell[x*N+y];
+            }
+        }
+    }
+
+    ~Matrix()
+    {
+    }
+
+    void operator = (Matrix m)
+    {
+        for(int x = 0; x < N; x++)
+        {
+            for(int y = 0; y < N; y++)
+            {
+                cell(x, y) = m.cell(x, y);
+            }
+        }
+    }
+
+    Vector<N> row_to_vector(int y)
+    {
+        Vector<N> ret;
+        for(int i = 0; i < N; i++)
+        {
+            ret[i] = _cell[y*N+i];
+        }
+        return ret;
+    }
+
+    Vector<N> col_to_vector(int x)
+    {
+        Vector<N> ret;
+        for(int i = 0; i < N; i++)
+        {
+            ret[i] = _cell[i*N+x];
+        }
+        return ret;
+    }
+
+    void vector_to_row(Vector<N> v, int row)
+    {
+        for(int i = 0; i < N; i++)
+        {
+            cell(row, i) = v(i);
+        }
+    }
+
+    void vector_to_col(Vector<N> v, int col)
+    {
+        for(int i = 0; i < N; i++)
+        {
+            cell(i, col) = v(i);
+        }
+    }
+
+    double& operator ()(int x, int y)
+    {
+        return _cell[x*N+y];
+    }
+
+    double& cell(int x, int y)
+    {
+        return _cell[x*N+y];
+    }
+
+
+    Matrix operator + (Matrix m)
+    {
+        Matrix ret;
+        for(int x = 0; x < N; x++)
+        {
+            for(int y = 0; y < N; y++)
+            {
+                ret._cell[x*N+y] = _cell[x*N+y] + m._cell[x*N+y];
+            }
+        }
+        return ret;
+    }
+
+    Matrix operator - (Matrix m)
+    {
+        Matrix ret;
+        for(int x = 0; x < N; x++)
+        {
+            for(int y = 0; y < N; y++)
+            {
+                ret._cell[x*N+y] = _cell[x*N+y] - m._cell[x*N+y];
+            }
+        }
+        return ret;
+    }
+
+    Matrix operator * (double scalar)
+    {
+        Matrix ret;
+        for(int x = 0; x < N; x++)
+        {
+            for(int y = 0; y < N; y++)
+            {
+                ret._cell[x*N+y] = _cell[x*N+y] * scalar;
+            }
+        }
+        return ret;
+    }
+
+    Matrix operator * (Matrix m)
+    {
+        Matrix ret;
+        for(int x = 0; x < N; x++)
+        {
+            for(int y = 0; y < N; y++)
+            {
+                Vector<N> row = row_to_vector(x);
+                Vector<N> col = m.col_to_vector(y);
+                ret.cell(x, y) = row.dot(col);
+            }
+        }
+        return ret;
+    }
+
+    Matrix transpose()
+    {
+        Matrix ret;
+        for(int x = 0; x < N; x++)
+        {
+            for(int y = 0; y < N; y++)
+            {
+                ret.cell(y, x) = cell(x, y);
+            }
+        }
+        return ret;
+    }
+
+    Matrix<N-1> minor_matrix(int row, int col)
+    {
+        int colCount = 0, rowCount = 0;
+        Matrix<N-1> ret;
+        for(int i = 0; i < N; i++ )
+        {
+            if( i != row )
+            {
+                for(int j = 0; j < N; j++ )
+                {
+                    if( j != col )
+                    {
+                        ret(rowCount, colCount) = cell(i, j);
+                        colCount++;
+                    }
+                }
+                rowCount++;
+            }
+        }
+        return ret;
+    }
+
+    double determinant()
+    {
+        if(N == 1)
+            return cell(0, 0);
+
+        float det = 0.0;
+        for(int i = 0; i < N; i++ )
+        {
+            Matrix<N-1> minor = minor_matrix(0, i);
+            det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant();
+        }
+        return det;
+    }
+
+    Matrix invert()
+    {
+        Matrix ret;
+        float det = determinant();
+
+        for(int x = 0; x < N; x++)
+        {
+            for(int y = 0; y < N; y++)
+            {
+                Matrix<N-1> minor = minor_matrix(y, x);
+                ret(x, y) = det*minor.determinant();
+                if( (x+y)%2 == 1)
+                    ret(x, y) = -ret(x, y);
+            }
+        }
+        return ret;
+    }
+
+private:
+    double* _cell;
+    double  _cell_data[N*N];
+};
+
+
+};
+
+#endif
+