adafruit_bno055 / utility / matrix.h @ 9f03e367
History | View | Annotate | Download (5.118 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 | 9f03e367 | Gé Vissers | return *this;
|
63 | 4bc1c0c1 | Kevin Townsend | } |
64 | |||
65 | 9f03e367 | Gé Vissers | Vector<N> row_to_vector(int i) const |
66 | 4bc1c0c1 | Kevin Townsend | { |
67 | Vector<N> ret; |
||
68 | 9f03e367 | Gé Vissers | for (int j = 0; j < N; j++) |
69 | 4bc1c0c1 | Kevin Townsend | { |
70 | 9f03e367 | Gé Vissers | ret[j] = cell(i, j); |
71 | 4bc1c0c1 | Kevin Townsend | } |
72 | return ret;
|
||
73 | } |
||
74 | |||
75 | 9f03e367 | Gé Vissers | Vector<N> col_to_vector(int j) const |
76 | 4bc1c0c1 | Kevin Townsend | { |
77 | Vector<N> ret; |
||
78 | 9f03e367 | Gé Vissers | for (int i = 0; i < N; i++) |
79 | 4bc1c0c1 | Kevin Townsend | { |
80 | 9f03e367 | Gé Vissers | ret[i] = cell(i, j); |
81 | 4bc1c0c1 | Kevin Townsend | } |
82 | return ret;
|
||
83 | } |
||
84 | |||
85 | 9f03e367 | Gé Vissers | void vector_to_row(const Vector<N>& v, int i) |
86 | 4bc1c0c1 | Kevin Townsend | { |
87 | 9f03e367 | Gé Vissers | for (int j = 0; j < N; j++) |
88 | 4bc1c0c1 | Kevin Townsend | { |
89 | 9f03e367 | Gé Vissers | cell(i, j) = v[j]; |
90 | 4bc1c0c1 | Kevin Townsend | } |
91 | } |
||
92 | |||
93 | 9f03e367 | Gé Vissers | void vector_to_col(const Vector<N>& v, int j) |
94 | 4bc1c0c1 | Kevin Townsend | { |
95 | 9f03e367 | Gé Vissers | for (int i = 0; i < N; i++) |
96 | 4bc1c0c1 | Kevin Townsend | { |
97 | 9f03e367 | Gé Vissers | cell(i, j) = v[i]; |
98 | 4bc1c0c1 | Kevin Townsend | } |
99 | } |
||
100 | |||
101 | 9c729628 | Gé Vissers | double operator()(int i, int j) const |
102 | 4bc1c0c1 | Kevin Townsend | { |
103 | 9c729628 | Gé Vissers | return cell(i, j);
|
104 | } |
||
105 | double& operator()(int i, int j) |
||
106 | { |
||
107 | return cell(i, j);
|
||
108 | 4bc1c0c1 | Kevin Townsend | } |
109 | |||
110 | 9c729628 | Gé Vissers | double cell(int i, int j) const |
111 | { |
||
112 | return _cell_data[i*N+j];
|
||
113 | } |
||
114 | double& cell(int i, int j) |
||
115 | 4bc1c0c1 | Kevin Townsend | { |
116 | 9c729628 | Gé Vissers | return _cell_data[i*N+j];
|
117 | 4bc1c0c1 | Kevin Townsend | } |
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 | d964148c | Tony DiCola | double _cell_data[N*N];
|
244 | 4bc1c0c1 | Kevin Townsend | }; |
245 | |||
246 | |||
247 | }; |
||
248 | |||
249 | #endif
|