matrix.h

00001 /*
00002     Inertial Measurement Unit Maths Library
00003     Copyright (C) 2013-2014  Samuel Cowen
00004     www.camelsoftware.com
00005 
00006     This program is free software: you can redistribute it and/or modify
00007     it under the terms of the GNU General Public License as published by
00008     the Free Software Foundation, either version 3 of the License, or
00009     (at your option) any later version.
00010 
00011     This program is distributed in the hope that it will be useful,
00012     but WITHOUT ANY WARRANTY; without even the implied warranty of
00013     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014     GNU General Public License for more details.
00015 
00016     You should have received a copy of the GNU General Public License
00017     along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 */
00019 
00020 #ifndef IMUMATH_MATRIX_HPP
00021 #define IMUMATH_MATRIX_HPP
00022 
00023 #include <stdlib.h>
00024 #include <string.h>
00025 #include <stdint.h>
00026 #include <math.h>
00027 
00028 namespace imu
00029 {
00030 
00031 
00032 template <uint8_t N> class Matrix
00033 {
00034 public:
00035     Matrix()
00036     {
00037         int r = sizeof(double)*N;
00038         _cell = &_cell_data[0];
00039         memset(_cell, 0, r*r);
00040     }
00041 
00042     Matrix(const Matrix &v)
00043     {
00044         int r = sizeof(double)*N;
00045         _cell = &_cell_data[0];
00046         memset(_cell, 0, r*r);
00047         for (int x = 0; x < N; x++ )
00048         {
00049             for(int y = 0; y < N; y++)
00050             {
00051                 _cell[x*N+y] = v._cell[x*N+y];
00052             }
00053         }
00054     }
00055 
00056     ~Matrix()
00057     {
00058     }
00059 
00060     void operator = (Matrix m)
00061     {
00062         for(int x = 0; x < N; x++)
00063         {
00064             for(int y = 0; y < N; y++)
00065             {
00066                 cell(x, y) = m.cell(x, y);
00067             }
00068         }
00069     }
00070 
00071     Vector<N> row_to_vector(int y)
00072     {
00073         Vector<N> ret;
00074         for(int i = 0; i < N; i++)
00075         {
00076             ret[i] = _cell[y*N+i];
00077         }
00078         return ret;
00079     }
00080 
00081     Vector<N> col_to_vector(int x)
00082     {
00083         Vector<N> ret;
00084         for(int i = 0; i < N; i++)
00085         {
00086             ret[i] = _cell[i*N+x];
00087         }
00088         return ret;
00089     }
00090 
00091     void vector_to_row(Vector<N> v, int row)
00092     {
00093         for(int i = 0; i < N; i++)
00094         {
00095             cell(row, i) = v(i);
00096         }
00097     }
00098 
00099     void vector_to_col(Vector<N> v, int col)
00100     {
00101         for(int i = 0; i < N; i++)
00102         {
00103             cell(i, col) = v(i);
00104         }
00105     }
00106 
00107     double& operator ()(int x, int y)
00108     {
00109         return _cell[x*N+y];
00110     }
00111 
00112     double& cell(int x, int y)
00113     {
00114         return _cell[x*N+y];
00115     }
00116 
00117 
00118     Matrix operator + (Matrix m)
00119     {
00120         Matrix ret;
00121         for(int x = 0; x < N; x++)
00122         {
00123             for(int y = 0; y < N; y++)
00124             {
00125                 ret._cell[x*N+y] = _cell[x*N+y] + m._cell[x*N+y];
00126             }
00127         }
00128         return ret;
00129     }
00130 
00131     Matrix operator - (Matrix m)
00132     {
00133         Matrix ret;
00134         for(int x = 0; x < N; x++)
00135         {
00136             for(int y = 0; y < N; y++)
00137             {
00138                 ret._cell[x*N+y] = _cell[x*N+y] - m._cell[x*N+y];
00139             }
00140         }
00141         return ret;
00142     }
00143 
00144     Matrix operator * (double scalar)
00145     {
00146         Matrix ret;
00147         for(int x = 0; x < N; x++)
00148         {
00149             for(int y = 0; y < N; y++)
00150             {
00151                 ret._cell[x*N+y] = _cell[x*N+y] * scalar;
00152             }
00153         }
00154         return ret;
00155     }
00156 
00157     Matrix operator * (Matrix m)
00158     {
00159         Matrix ret;
00160         for(int x = 0; x < N; x++)
00161         {
00162             for(int y = 0; y < N; y++)
00163             {
00164                 Vector<N> row = row_to_vector(x);
00165                 Vector<N> col = m.col_to_vector(y);
00166                 ret.cell(x, y) = row.dot(col);
00167             }
00168         }
00169         return ret;
00170     }
00171 
00172     Matrix transpose()
00173     {
00174         Matrix ret;
00175         for(int x = 0; x < N; x++)
00176         {
00177             for(int y = 0; y < N; y++)
00178             {
00179                 ret.cell(y, x) = cell(x, y);
00180             }
00181         }
00182         return ret;
00183     }
00184 
00185     Matrix<N-1> minor_matrix(int row, int col)
00186     {
00187         int colCount = 0, rowCount = 0;
00188         Matrix<N-1> ret;
00189         for(int i = 0; i < N; i++ )
00190         {
00191             if( i != row )
00192             {
00193                 for(int j = 0; j < N; j++ )
00194                 {
00195                     if( j != col )
00196                     {
00197                         ret(rowCount, colCount) = cell(i, j);
00198                         colCount++;
00199                     }
00200                 }
00201                 rowCount++;
00202             }
00203         }
00204         return ret;
00205     }
00206 
00207     double determinant()
00208     {
00209         if(N == 1)
00210             return cell(0, 0);
00211 
00212         float det = 0.0;
00213         for(int i = 0; i < N; i++ )
00214         {
00215             Matrix<N-1> minor = minor_matrix(0, i);
00216             det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant();
00217         }
00218         return det;
00219     }
00220 
00221     Matrix invert()
00222     {
00223         Matrix ret;
00224         float det = determinant();
00225 
00226         for(int x = 0; x < N; x++)
00227         {
00228             for(int y = 0; y < N; y++)
00229             {
00230                 Matrix<N-1> minor = minor_matrix(y, x);
00231                 ret(x, y) = det*minor.determinant();
00232                 if( (x+y)%2 == 1)
00233                     ret(x, y) = -ret(x, y);
00234             }
00235         }
00236         return ret;
00237     }
00238 
00239 private:
00240     double* _cell;
00241     double  _cell_data[N*N];
00242 };
00243 
00244 
00245 };
00246 
00247 #endif
00248