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