/utility/matrix.h - Annotate - BRIX5 - Research for Cognitive Interaction

adafruit_bno055 / utility / matrix.h @ 67f3cff5

History | View | Annotate | Download (5.075 KB)

-bc1c0c1
+/*
     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 = (double*)malloc(r*r);
         memset(_cell, 0, r*r);
+        }
     Matrix(const Matrix &v)
+    {
         int r = sizeof(double)*N;
         _cell = (double*)malloc(r*r);
         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()
+    {
         free(_cell);
+    }
     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;
 };
 };
 #endif