Adafruit driver converted to Mbed OS 6.x.
Dependents: Adafruit-BNO055-test
matrix.h@0:22c544c8741a, 2015-09-16 (annotated)
- Committer:
- simonscott
- Date:
- Wed Sep 16 19:48:33 2015 +0000
- Revision:
- 0:22c544c8741a
First version
Who changed what in which revision?
User | Revision | Line number | New 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 |