Toyomasa Watarai / Adafruit_BNO055

Adafruit driver converted to Mbed OS 6.x.

Dependents:   Adafruit-BNO055-test

matrix.h@3:7db662f5d402, 2021-03-16 (annotated)

Committer:
MACRUM
Date:
Tue Mar 16 05:41:43 2021 +0000
Revision:
3:7db662f5d402
Parent:
0:22c544c8741a 
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simonscott 0:22c544c8741a 1 /*
simonscott 0:22c544c8741a 2 Inertial Measurement Unit Maths Library
simonscott 0:22c544c8741a 3 Copyright (C) 2013-2014 Samuel Cowen
simonscott 0:22c544c8741a 4 www.camelsoftware.com
simonscott 0:22c544c8741a 5
simonscott 0:22c544c8741a 6 This program is free software: you can redistribute it and/or modify
simonscott 0:22c544c8741a 7 it under the terms of the GNU General Public License as published by
simonscott 0:22c544c8741a 8 the Free Software Foundation, either version 3 of the License, or
simonscott 0:22c544c8741a 9 (at your option) any later version.
simonscott 0:22c544c8741a 10
simonscott 0:22c544c8741a 11 This program is distributed in the hope that it will be useful,
simonscott 0:22c544c8741a 12 but WITHOUT ANY WARRANTY; without even the implied warranty of
simonscott 0:22c544c8741a 13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
simonscott 0:22c544c8741a 14 GNU General Public License for more details.
simonscott 0:22c544c8741a 15
simonscott 0:22c544c8741a 16 You should have received a copy of the GNU General Public License
simonscott 0:22c544c8741a 17 along with this program. If not, see <http://www.gnu.org/licenses/>.
simonscott 0:22c544c8741a 18 */
simonscott 0:22c544c8741a 19
simonscott 0:22c544c8741a 20 #ifndef IMUMATH_MATRIX_HPP
simonscott 0:22c544c8741a 21 #define IMUMATH_MATRIX_HPP
simonscott 0:22c544c8741a 22
simonscott 0:22c544c8741a 23 #include <stdlib.h>
simonscott 0:22c544c8741a 24 #include <string.h>
simonscott 0:22c544c8741a 25 #include <stdint.h>
simonscott 0:22c544c8741a 26 #include <math.h>
simonscott 0:22c544c8741a 27
simonscott 0:22c544c8741a 28 namespace imu
simonscott 0:22c544c8741a 29 {
simonscott 0:22c544c8741a 30
simonscott 0:22c544c8741a 31
simonscott 0:22c544c8741a 32 template <uint8_t N> class Matrix
simonscott 0:22c544c8741a 33 {
simonscott 0:22c544c8741a 34 public:
simonscott 0:22c544c8741a 35 Matrix()
simonscott 0:22c544c8741a 36 {
simonscott 0:22c544c8741a 37 int r = sizeof(double)*N;
simonscott 0:22c544c8741a 38 _cell = &_cell_data[0];
simonscott 0:22c544c8741a 39 memset(_cell, 0, r*r);
simonscott 0:22c544c8741a 40 }
simonscott 0:22c544c8741a 41
simonscott 0:22c544c8741a 42 Matrix(const Matrix &v)
simonscott 0:22c544c8741a 43 {
simonscott 0:22c544c8741a 44 int r = sizeof(double)*N;
simonscott 0:22c544c8741a 45 _cell = &_cell_data[0];
simonscott 0:22c544c8741a 46 memset(_cell, 0, r*r);
simonscott 0:22c544c8741a 47 for (int x = 0; x < N; x++ )
simonscott 0:22c544c8741a 48 {
simonscott 0:22c544c8741a 49 for(int y = 0; y < N; y++)
simonscott 0:22c544c8741a 50 {
simonscott 0:22c544c8741a 51 _cell[x*N+y] = v._cell[x*N+y];
simonscott 0:22c544c8741a 52 }
simonscott 0:22c544c8741a 53 }
simonscott 0:22c544c8741a 54 }
simonscott 0:22c544c8741a 55
simonscott 0:22c544c8741a 56 ~Matrix()
simonscott 0:22c544c8741a 57 {
simonscott 0:22c544c8741a 58 }
simonscott 0:22c544c8741a 59
simonscott 0:22c544c8741a 60 void operator = (Matrix m)
simonscott 0:22c544c8741a 61 {
simonscott 0:22c544c8741a 62 for(int x = 0; x < N; x++)
simonscott 0:22c544c8741a 63 {
simonscott 0:22c544c8741a 64 for(int y = 0; y < N; y++)
simonscott 0:22c544c8741a 65 {
simonscott 0:22c544c8741a 66 cell(x, y) = m.cell(x, y);
simonscott 0:22c544c8741a 67 }
simonscott 0:22c544c8741a 68 }
simonscott 0:22c544c8741a 69 }
simonscott 0:22c544c8741a 70
simonscott 0:22c544c8741a 71 Vector<N> row_to_vector(int y)
simonscott 0:22c544c8741a 72 {
simonscott 0:22c544c8741a 73 Vector<N> ret;
simonscott 0:22c544c8741a 74 for(int i = 0; i < N; i++)
simonscott 0:22c544c8741a 75 {
simonscott 0:22c544c8741a 76 ret[i] = _cell[y*N+i];
simonscott 0:22c544c8741a 77 }
simonscott 0:22c544c8741a 78 return ret;
simonscott 0:22c544c8741a 79 }
simonscott 0:22c544c8741a 80
simonscott 0:22c544c8741a 81 Vector<N> col_to_vector(int x)
simonscott 0:22c544c8741a 82 {
simonscott 0:22c544c8741a 83 Vector<N> ret;
simonscott 0:22c544c8741a 84 for(int i = 0; i < N; i++)
simonscott 0:22c544c8741a 85 {
simonscott 0:22c544c8741a 86 ret[i] = _cell[i*N+x];
simonscott 0:22c544c8741a 87 }
simonscott 0:22c544c8741a 88 return ret;
simonscott 0:22c544c8741a 89 }
simonscott 0:22c544c8741a 90
simonscott 0:22c544c8741a 91 void vector_to_row(Vector<N> v, int row)
simonscott 0:22c544c8741a 92 {
simonscott 0:22c544c8741a 93 for(int i = 0; i < N; i++)
simonscott 0:22c544c8741a 94 {
simonscott 0:22c544c8741a 95 cell(row, i) = v(i);
simonscott 0:22c544c8741a 96 }
simonscott 0:22c544c8741a 97 }
simonscott 0:22c544c8741a 98
simonscott 0:22c544c8741a 99 void vector_to_col(Vector<N> v, int col)
simonscott 0:22c544c8741a 100 {
simonscott 0:22c544c8741a 101 for(int i = 0; i < N; i++)
simonscott 0:22c544c8741a 102 {
simonscott 0:22c544c8741a 103 cell(i, col) = v(i);
simonscott 0:22c544c8741a 104 }
simonscott 0:22c544c8741a 105 }
simonscott 0:22c544c8741a 106
simonscott 0:22c544c8741a 107 double& operator ()(int x, int y)
simonscott 0:22c544c8741a 108 {
simonscott 0:22c544c8741a 109 return _cell[x*N+y];
simonscott 0:22c544c8741a 110 }
simonscott 0:22c544c8741a 111
simonscott 0:22c544c8741a 112 double& cell(int x, int y)
simonscott 0:22c544c8741a 113 {
simonscott 0:22c544c8741a 114 return _cell[x*N+y];
simonscott 0:22c544c8741a 115 }
simonscott 0:22c544c8741a 116
simonscott 0:22c544c8741a 117
simonscott 0:22c544c8741a 118 Matrix operator + (Matrix m)
simonscott 0:22c544c8741a 119 {
simonscott 0:22c544c8741a 120 Matrix ret;
simonscott 0:22c544c8741a 121 for(int x = 0; x < N; x++)
simonscott 0:22c544c8741a 122 {
simonscott 0:22c544c8741a 123 for(int y = 0; y < N; y++)
simonscott 0:22c544c8741a 124 {
simonscott 0:22c544c8741a 125 ret._cell[x*N+y] = _cell[x*N+y] + m._cell[x*N+y];
simonscott 0:22c544c8741a 126 }
simonscott 0:22c544c8741a 127 }
simonscott 0:22c544c8741a 128 return ret;
simonscott 0:22c544c8741a 129 }
simonscott 0:22c544c8741a 130
simonscott 0:22c544c8741a 131 Matrix operator - (Matrix m)
simonscott 0:22c544c8741a 132 {
simonscott 0:22c544c8741a 133 Matrix ret;
simonscott 0:22c544c8741a 134 for(int x = 0; x < N; x++)
simonscott 0:22c544c8741a 135 {
simonscott 0:22c544c8741a 136 for(int y = 0; y < N; y++)
simonscott 0:22c544c8741a 137 {
simonscott 0:22c544c8741a 138 ret._cell[x*N+y] = _cell[x*N+y] - m._cell[x*N+y];
simonscott 0:22c544c8741a 139 }
simonscott 0:22c544c8741a 140 }
simonscott 0:22c544c8741a 141 return ret;
simonscott 0:22c544c8741a 142 }
simonscott 0:22c544c8741a 143
simonscott 0:22c544c8741a 144 Matrix operator * (double scalar)
simonscott 0:22c544c8741a 145 {
simonscott 0:22c544c8741a 146 Matrix ret;
simonscott 0:22c544c8741a 147 for(int x = 0; x < N; x++)
simonscott 0:22c544c8741a 148 {
simonscott 0:22c544c8741a 149 for(int y = 0; y < N; y++)
simonscott 0:22c544c8741a 150 {
simonscott 0:22c544c8741a 151 ret._cell[x*N+y] = _cell[x*N+y] * scalar;
simonscott 0:22c544c8741a 152 }
simonscott 0:22c544c8741a 153 }
simonscott 0:22c544c8741a 154 return ret;
simonscott 0:22c544c8741a 155 }
simonscott 0:22c544c8741a 156
simonscott 0:22c544c8741a 157 Matrix operator * (Matrix m)
simonscott 0:22c544c8741a 158 {
simonscott 0:22c544c8741a 159 Matrix ret;
simonscott 0:22c544c8741a 160 for(int x = 0; x < N; x++)
simonscott 0:22c544c8741a 161 {
simonscott 0:22c544c8741a 162 for(int y = 0; y < N; y++)
simonscott 0:22c544c8741a 163 {
simonscott 0:22c544c8741a 164 Vector<N> row = row_to_vector(x);
simonscott 0:22c544c8741a 165 Vector<N> col = m.col_to_vector(y);
simonscott 0:22c544c8741a 166 ret.cell(x, y) = row.dot(col);
simonscott 0:22c544c8741a 167 }
simonscott 0:22c544c8741a 168 }
simonscott 0:22c544c8741a 169 return ret;
simonscott 0:22c544c8741a 170 }
simonscott 0:22c544c8741a 171
simonscott 0:22c544c8741a 172 Matrix transpose()
simonscott 0:22c544c8741a 173 {
simonscott 0:22c544c8741a 174 Matrix ret;
simonscott 0:22c544c8741a 175 for(int x = 0; x < N; x++)
simonscott 0:22c544c8741a 176 {
simonscott 0:22c544c8741a 177 for(int y = 0; y < N; y++)
simonscott 0:22c544c8741a 178 {
simonscott 0:22c544c8741a 179 ret.cell(y, x) = cell(x, y);
simonscott 0:22c544c8741a 180 }
simonscott 0:22c544c8741a 181 }
simonscott 0:22c544c8741a 182 return ret;
simonscott 0:22c544c8741a 183 }
simonscott 0:22c544c8741a 184
simonscott 0:22c544c8741a 185 Matrix<N-1> minor_matrix(int row, int col)
simonscott 0:22c544c8741a 186 {
simonscott 0:22c544c8741a 187 int colCount = 0, rowCount = 0;
simonscott 0:22c544c8741a 188 Matrix<N-1> ret;
simonscott 0:22c544c8741a 189 for(int i = 0; i < N; i++ )
simonscott 0:22c544c8741a 190 {
simonscott 0:22c544c8741a 191 if( i != row )
simonscott 0:22c544c8741a 192 {
simonscott 0:22c544c8741a 193 for(int j = 0; j < N; j++ )
simonscott 0:22c544c8741a 194 {
simonscott 0:22c544c8741a 195 if( j != col )
simonscott 0:22c544c8741a 196 {
simonscott 0:22c544c8741a 197 ret(rowCount, colCount) = cell(i, j);
simonscott 0:22c544c8741a 198 colCount++;
simonscott 0:22c544c8741a 199 }
simonscott 0:22c544c8741a 200 }
simonscott 0:22c544c8741a 201 rowCount++;
simonscott 0:22c544c8741a 202 }
simonscott 0:22c544c8741a 203 }
simonscott 0:22c544c8741a 204 return ret;
simonscott 0:22c544c8741a 205 }
simonscott 0:22c544c8741a 206
simonscott 0:22c544c8741a 207 double determinant()
simonscott 0:22c544c8741a 208 {
simonscott 0:22c544c8741a 209 if(N == 1)
simonscott 0:22c544c8741a 210 return cell(0, 0);
simonscott 0:22c544c8741a 211
simonscott 0:22c544c8741a 212 float det = 0.0;
simonscott 0:22c544c8741a 213 for(int i = 0; i < N; i++ )
simonscott 0:22c544c8741a 214 {
simonscott 0:22c544c8741a 215 Matrix<N-1> minor = minor_matrix(0, i);
simonscott 0:22c544c8741a 216 det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant();
simonscott 0:22c544c8741a 217 }
simonscott 0:22c544c8741a 218 return det;
simonscott 0:22c544c8741a 219 }
simonscott 0:22c544c8741a 220
simonscott 0:22c544c8741a 221 Matrix invert()
simonscott 0:22c544c8741a 222 {
simonscott 0:22c544c8741a 223 Matrix ret;
simonscott 0:22c544c8741a 224 float det = determinant();
simonscott 0:22c544c8741a 225
simonscott 0:22c544c8741a 226 for(int x = 0; x < N; x++)
simonscott 0:22c544c8741a 227 {
simonscott 0:22c544c8741a 228 for(int y = 0; y < N; y++)
simonscott 0:22c544c8741a 229 {
simonscott 0:22c544c8741a 230 Matrix<N-1> minor = minor_matrix(y, x);
simonscott 0:22c544c8741a 231 ret(x, y) = det*minor.determinant();
simonscott 0:22c544c8741a 232 if( (x+y)%2 == 1)
simonscott 0:22c544c8741a 233 ret(x, y) = -ret(x, y);
simonscott 0:22c544c8741a 234 }
simonscott 0:22c544c8741a 235 }
simonscott 0:22c544c8741a 236 return ret;
simonscott 0:22c544c8741a 237 }
simonscott 0:22c544c8741a 238
simonscott 0:22c544c8741a 239 private:
simonscott 0:22c544c8741a 240 double* _cell;
simonscott 0:22c544c8741a 241 double _cell_data[N*N];
simonscott 0:22c544c8741a 242 };
simonscott 0:22c544c8741a 243
simonscott 0:22c544c8741a 244
simonscott 0:22c544c8741a 245 };
simonscott 0:22c544c8741a 246
simonscott 0:22c544c8741a 247 #endif
simonscott 0:22c544c8741a 248