adafruit_bno055 / utility / matrix.h @ a2da8ab6
History | View | Annotate | Download (5.027 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 x, int y) |
107 |
{ |
108 |
return _cell[x*N+y];
|
109 |
} |
110 |
|
111 |
double& cell(int x, int y) |
112 |
{ |
113 |
return _cell[x*N+y];
|
114 |
} |
115 |
|
116 |
|
117 |
Matrix operator + (Matrix m) |
118 |
{ |
119 |
Matrix ret; |
120 |
for(int x = 0; x < N; x++) |
121 |
{ |
122 |
for(int y = 0; y < N; y++) |
123 |
{ |
124 |
ret._cell[x*N+y] = _cell[x*N+y] + m._cell[x*N+y]; |
125 |
} |
126 |
} |
127 |
return ret;
|
128 |
} |
129 |
|
130 |
Matrix operator - (Matrix m) |
131 |
{ |
132 |
Matrix ret; |
133 |
for(int x = 0; x < N; x++) |
134 |
{ |
135 |
for(int y = 0; y < N; y++) |
136 |
{ |
137 |
ret._cell[x*N+y] = _cell[x*N+y] - m._cell[x*N+y]; |
138 |
} |
139 |
} |
140 |
return ret;
|
141 |
} |
142 |
|
143 |
Matrix operator * (double scalar)
|
144 |
{ |
145 |
Matrix ret; |
146 |
for(int x = 0; x < N; x++) |
147 |
{ |
148 |
for(int y = 0; y < N; y++) |
149 |
{ |
150 |
ret._cell[x*N+y] = _cell[x*N+y] * scalar; |
151 |
} |
152 |
} |
153 |
return ret;
|
154 |
} |
155 |
|
156 |
Matrix operator * (Matrix m) |
157 |
{ |
158 |
Matrix ret; |
159 |
for(int x = 0; x < N; x++) |
160 |
{ |
161 |
for(int y = 0; y < N; y++) |
162 |
{ |
163 |
Vector<N> row = row_to_vector(x); |
164 |
Vector<N> col = m.col_to_vector(y); |
165 |
ret.cell(x, y) = row.dot(col); |
166 |
} |
167 |
} |
168 |
return ret;
|
169 |
} |
170 |
|
171 |
Matrix transpose() |
172 |
{ |
173 |
Matrix ret; |
174 |
for(int x = 0; x < N; x++) |
175 |
{ |
176 |
for(int y = 0; y < N; y++) |
177 |
{ |
178 |
ret.cell(y, x) = cell(x, y); |
179 |
} |
180 |
} |
181 |
return ret;
|
182 |
} |
183 |
|
184 |
Matrix<N-1> minor_matrix(int row, int col) |
185 |
{ |
186 |
int colCount = 0, rowCount = 0; |
187 |
Matrix<N-1> ret;
|
188 |
for(int i = 0; i < N; i++ ) |
189 |
{ |
190 |
if( i != row )
|
191 |
{ |
192 |
for(int j = 0; j < N; j++ ) |
193 |
{ |
194 |
if( j != col )
|
195 |
{ |
196 |
ret(rowCount, colCount) = cell(i, j); |
197 |
colCount++; |
198 |
} |
199 |
} |
200 |
rowCount++; |
201 |
} |
202 |
} |
203 |
return ret;
|
204 |
} |
205 |
|
206 |
double determinant()
|
207 |
{ |
208 |
if(N == 1) |
209 |
return cell(0, 0); |
210 |
|
211 |
float det = 0.0; |
212 |
for(int i = 0; i < N; i++ ) |
213 |
{ |
214 |
Matrix<N-1> minor = minor_matrix(0, i); |
215 |
det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant(); |
216 |
} |
217 |
return det;
|
218 |
} |
219 |
|
220 |
Matrix invert() |
221 |
{ |
222 |
Matrix ret; |
223 |
float det = determinant();
|
224 |
|
225 |
for(int x = 0; x < N; x++) |
226 |
{ |
227 |
for(int y = 0; y < N; y++) |
228 |
{ |
229 |
Matrix<N-1> minor = minor_matrix(y, x);
|
230 |
ret(x, y) = det*minor.determinant(); |
231 |
if( (x+y)%2 == 1) |
232 |
ret(x, y) = -ret(x, y); |
233 |
} |
234 |
} |
235 |
return ret;
|
236 |
} |
237 |
|
238 |
private:
|
239 |
double* _cell;
|
240 |
double _cell_data[N*N];
|
241 |
}; |
242 |
|
243 |
|
244 |
}; |
245 |
|
246 |
#endif
|
247 |
|