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