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