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