adafruit_bno055 / utility / matrix.h @ 9c729628
History | View | Annotate | Download (5.188 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 &v)
|
44 |
{ |
45 |
_cell = &_cell_data[0];
|
46 |
for (int x = 0; x < N; x++ ) |
47 |
{ |
48 |
for(int y = 0; y < N; y++) |
49 |
{ |
50 |
_cell[x*N+y] = v._cell[x*N+y]; |
51 |
} |
52 |
} |
53 |
} |
54 |
|
55 |
~Matrix() |
56 |
{ |
57 |
} |
58 |
|
59 |
void operator = (Matrix m)
|
60 |
{ |
61 |
for(int x = 0; x < N; x++) |
62 |
{ |
63 |
for(int y = 0; y < N; y++) |
64 |
{ |
65 |
cell(x, y) = m.cell(x, y); |
66 |
} |
67 |
} |
68 |
} |
69 |
|
70 |
Vector<N> row_to_vector(int y)
|
71 |
{ |
72 |
Vector<N> ret; |
73 |
for(int i = 0; i < N; i++) |
74 |
{ |
75 |
ret[i] = _cell[y*N+i]; |
76 |
} |
77 |
return ret;
|
78 |
} |
79 |
|
80 |
Vector<N> col_to_vector(int x)
|
81 |
{ |
82 |
Vector<N> ret; |
83 |
for(int i = 0; i < N; i++) |
84 |
{ |
85 |
ret[i] = _cell[i*N+x]; |
86 |
} |
87 |
return ret;
|
88 |
} |
89 |
|
90 |
void vector_to_row(Vector<N> v, int row) |
91 |
{ |
92 |
for(int i = 0; i < N; i++) |
93 |
{ |
94 |
cell(row, i) = v(i); |
95 |
} |
96 |
} |
97 |
|
98 |
void vector_to_col(Vector<N> v, int col) |
99 |
{ |
100 |
for(int i = 0; i < N; i++) |
101 |
{ |
102 |
cell(i, col) = v(i); |
103 |
} |
104 |
} |
105 |
|
106 |
double operator()(int i, int j) const |
107 |
{ |
108 |
return cell(i, j);
|
109 |
} |
110 |
double& operator()(int i, int j) |
111 |
{ |
112 |
return cell(i, j);
|
113 |
} |
114 |
|
115 |
double cell(int i, int j) const |
116 |
{ |
117 |
return _cell_data[i*N+j];
|
118 |
} |
119 |
double& cell(int i, int j) |
120 |
{ |
121 |
return _cell_data[i*N+j];
|
122 |
} |
123 |
|
124 |
|
125 |
Matrix operator + (Matrix m) |
126 |
{ |
127 |
Matrix ret; |
128 |
for(int x = 0; x < N; x++) |
129 |
{ |
130 |
for(int y = 0; y < N; y++) |
131 |
{ |
132 |
ret._cell[x*N+y] = _cell[x*N+y] + m._cell[x*N+y]; |
133 |
} |
134 |
} |
135 |
return ret;
|
136 |
} |
137 |
|
138 |
Matrix operator - (Matrix m) |
139 |
{ |
140 |
Matrix ret; |
141 |
for(int x = 0; x < N; x++) |
142 |
{ |
143 |
for(int y = 0; y < N; y++) |
144 |
{ |
145 |
ret._cell[x*N+y] = _cell[x*N+y] - m._cell[x*N+y]; |
146 |
} |
147 |
} |
148 |
return ret;
|
149 |
} |
150 |
|
151 |
Matrix operator * (double scalar)
|
152 |
{ |
153 |
Matrix ret; |
154 |
for(int x = 0; x < N; x++) |
155 |
{ |
156 |
for(int y = 0; y < N; y++) |
157 |
{ |
158 |
ret._cell[x*N+y] = _cell[x*N+y] * scalar; |
159 |
} |
160 |
} |
161 |
return ret;
|
162 |
} |
163 |
|
164 |
Matrix operator * (Matrix m) |
165 |
{ |
166 |
Matrix ret; |
167 |
for(int x = 0; x < N; x++) |
168 |
{ |
169 |
for(int y = 0; y < N; y++) |
170 |
{ |
171 |
Vector<N> row = row_to_vector(x); |
172 |
Vector<N> col = m.col_to_vector(y); |
173 |
ret.cell(x, y) = row.dot(col); |
174 |
} |
175 |
} |
176 |
return ret;
|
177 |
} |
178 |
|
179 |
Matrix transpose() |
180 |
{ |
181 |
Matrix ret; |
182 |
for(int x = 0; x < N; x++) |
183 |
{ |
184 |
for(int y = 0; y < N; y++) |
185 |
{ |
186 |
ret.cell(y, x) = cell(x, y); |
187 |
} |
188 |
} |
189 |
return ret;
|
190 |
} |
191 |
|
192 |
Matrix<N-1> minor_matrix(int row, int col) |
193 |
{ |
194 |
int colCount = 0, rowCount = 0; |
195 |
Matrix<N-1> ret;
|
196 |
for(int i = 0; i < N; i++ ) |
197 |
{ |
198 |
if( i != row )
|
199 |
{ |
200 |
for(int j = 0; j < N; j++ ) |
201 |
{ |
202 |
if( j != col )
|
203 |
{ |
204 |
ret(rowCount, colCount) = cell(i, j); |
205 |
colCount++; |
206 |
} |
207 |
} |
208 |
rowCount++; |
209 |
} |
210 |
} |
211 |
return ret;
|
212 |
} |
213 |
|
214 |
double determinant()
|
215 |
{ |
216 |
if(N == 1) |
217 |
return cell(0, 0); |
218 |
|
219 |
float det = 0.0; |
220 |
for(int i = 0; i < N; i++ ) |
221 |
{ |
222 |
Matrix<N-1> minor = minor_matrix(0, i); |
223 |
det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant(); |
224 |
} |
225 |
return det;
|
226 |
} |
227 |
|
228 |
Matrix invert() |
229 |
{ |
230 |
Matrix ret; |
231 |
float det = determinant();
|
232 |
|
233 |
for(int x = 0; x < N; x++) |
234 |
{ |
235 |
for(int y = 0; y < N; y++) |
236 |
{ |
237 |
Matrix<N-1> minor = minor_matrix(y, x);
|
238 |
ret(x, y) = det*minor.determinant(); |
239 |
if( (x+y)%2 == 1) |
240 |
ret(x, y) = -ret(x, y); |
241 |
} |
242 |
} |
243 |
return ret;
|
244 |
} |
245 |
|
246 |
private:
|
247 |
double* _cell;
|
248 |
double _cell_data[N*N];
|
249 |
}; |
250 |
|
251 |
|
252 |
}; |
253 |
|
254 |
#endif
|
255 |
|