Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : // Copyright 2010 Michael Smith, all rights reserved.
17 :
18 : // Inspired by:
19 : /****************************************
20 : * 3D Vector Classes
21 : * By Bill Perone ([email protected])
22 : */
23 :
24 : //
25 : // 3x3 matrix implementation.
26 : //
27 : // Note that the matrix is organised in row-normal form (the same as
28 : // applies to array indexing).
29 : //
30 : // In addition to the template, this header defines the following types:
31 : //
32 : // Matrix3i 3x3 matrix of signed integers
33 : // Matrix3ui 3x3 matrix of unsigned integers
34 : // Matrix3l 3x3 matrix of signed longs
35 : // Matrix3ul 3x3 matrix of unsigned longs
36 : // Matrix3f 3x3 matrix of signed floats
37 : //
38 : #pragma once
39 :
40 : #include "ftype.h"
41 :
42 : #include "vector3.h"
43 : #include "vector2.h"
44 :
45 : template <typename T>
46 : class Vector3;
47 :
48 : // 3x3 matrix with elements of type T
49 : template <typename T>
50 : class Matrix3 {
51 : public:
52 :
53 : // Vectors comprising the rows of the matrix
54 : Vector3<T> a, b, c;
55 :
56 : // trivial ctor
57 : // note that the Vector3 ctor will zero the vector elements
58 208868875 : constexpr Matrix3() {}
59 :
60 : // setting ctor
61 1635342578 : constexpr Matrix3(const Vector3<T> &a0, const Vector3<T> &b0, const Vector3<T> &c0)
62 1635342578 : : a(a0)
63 1635342578 : , b(b0)
64 1635342578 : , c(c0) {}
65 :
66 : // setting ctor
67 665896828 : constexpr Matrix3(const T ax, const T ay, const T az,
68 : const T bx, const T by, const T bz,
69 : const T cx, const T cy, const T cz)
70 665896828 : : a(ax,ay,az)
71 665896087 : , b(bx,by,bz)
72 665896317 : , c(cx,cy,cz) {}
73 :
74 : // function call operator
75 0 : void operator () (const Vector3<T> &a0, const Vector3<T> &b0, const Vector3<T> &c0)
76 : {
77 0 : a = a0; b = b0; c = c0;
78 0 : }
79 :
80 : // test for equality
81 0 : bool operator == (const Matrix3<T> &m)
82 : {
83 0 : return (a==m.a && b==m.b && c==m.c);
84 : }
85 :
86 : // test for inequality
87 0 : bool operator != (const Matrix3<T> &m)
88 : {
89 0 : return (a!=m.a || b!=m.b || c!=m.c);
90 : }
91 :
92 : // negation
93 0 : Matrix3<T> operator - (void) const
94 : {
95 0 : return Matrix3<T>(-a,-b,-c);
96 : }
97 :
98 : // addition
99 389453244 : Matrix3<T> operator + (const Matrix3<T> &m) const
100 : {
101 389453244 : return Matrix3<T>(a+m.a, b+m.b, c+m.c);
102 : }
103 389453244 : Matrix3<T> &operator += (const Matrix3<T> &m)
104 : {
105 389453244 : return *this = *this + m;
106 : }
107 :
108 : // subtraction
109 402 : Matrix3<T> operator - (const Matrix3<T> &m) const
110 : {
111 402 : return Matrix3<T>(a-m.a, b-m.b, c-m.c);
112 : }
113 402 : Matrix3<T> &operator -= (const Matrix3<T> &m)
114 : {
115 402 : return *this = *this - m;
116 : }
117 :
118 : // uniform scaling
119 0 : Matrix3<T> operator * (const T num) const
120 : {
121 0 : return Matrix3<T>(a*num, b*num, c*num);
122 : }
123 0 : Matrix3<T> &operator *= (const T num)
124 : {
125 0 : return *this = *this * num;
126 : }
127 0 : Matrix3<T> operator / (const T num) const
128 : {
129 0 : return Matrix3<T>(a/num, b/num, c/num);
130 : }
131 0 : Matrix3<T> &operator /= (const T num)
132 : {
133 0 : return *this = *this / num;
134 : }
135 :
136 : // allow a Matrix3 to be used as an array of vectors, 0 indexed
137 1898522311 : Vector3<T> & operator[](uint8_t i) {
138 1898522311 : Vector3<T> *_v = &a;
139 : #if MATH_CHECK_INDEXES
140 : assert(i >= 0 && i < 3);
141 : #endif
142 1898522311 : return _v[i];
143 : }
144 :
145 1049034393 : const Vector3<T> & operator[](uint8_t i) const {
146 1049034393 : const Vector3<T> *_v = &a;
147 : #if MATH_CHECK_INDEXES
148 : assert(i >= 0 && i < 3);
149 : #endif
150 1049034393 : return _v[i];
151 : }
152 :
153 : // multiplication by a vector
154 : Vector3<T> operator *(const Vector3<T> &v) const;
155 :
156 : // multiplication of transpose by a vector
157 : Vector3<T> mul_transpose(const Vector3<T> &v) const;
158 :
159 : // multiplication by a vector giving a Vector2 result (XY components)
160 : Vector2<T> mulXY(const Vector3<T> &v) const;
161 :
162 : // extract x column
163 324233 : Vector3<T> colx(void) const
164 : {
165 324233 : return Vector3<T>(a.x, b.x, c.x);
166 : }
167 :
168 : // extract y column
169 402 : Vector3<T> coly(void) const
170 : {
171 402 : return Vector3<T>(a.y, b.y, c.y);
172 : }
173 :
174 : // extract z column
175 402 : Vector3<T> colz(void) const
176 : {
177 402 : return Vector3<T>(a.z, b.z, c.z);
178 : }
179 :
180 : // multiplication by another Matrix3<T>
181 : Matrix3<T> operator *(const Matrix3<T> &m) const;
182 :
183 89279 : Matrix3<T> &operator *=(const Matrix3<T> &m)
184 : {
185 89279 : return *this = *this * m;
186 : }
187 :
188 : // transpose the matrix
189 : Matrix3<T> transposed(void) const;
190 :
191 182003 : void transpose(void)
192 : {
193 182003 : *this = transposed();
194 182003 : }
195 :
196 : /**
197 : * Calculate the determinant of this matrix.
198 : *
199 : * @return The value of the determinant.
200 : */
201 : T det() const;
202 :
203 : /**
204 : * Calculate the inverse of this matrix.
205 : *
206 : * @param inv[in] Where to store the result.
207 : *
208 : * @return If this matrix is invertible, then true is returned. Otherwise,
209 : * \p inv is unmodified and false is returned.
210 : */
211 : bool inverse(Matrix3<T>& inv) const WARN_IF_UNUSED;
212 :
213 : /**
214 : * Invert this matrix if it is invertible.
215 : *
216 : * @return Return true if this matrix could be successfully inverted and
217 : * false otherwise.
218 : */
219 : bool invert() WARN_IF_UNUSED;
220 :
221 : // zero the matrix
222 829084 : void zero(void) {
223 829084 : memset((void*)this, 0, sizeof(*this));
224 829084 : }
225 :
226 : // setup the identity matrix
227 825382 : void identity(void) {
228 825382 : zero();
229 825382 : a.x = b.y = c.z = 1;
230 825382 : }
231 :
232 : // check if any elements are NAN
233 41698936 : bool is_nan(void) WARN_IF_UNUSED
234 : {
235 41698936 : return a.is_nan() || b.is_nan() || c.is_nan();
236 : }
237 :
238 : /*
239 : create a rotation matrix from Euler angles in 321 euler ordering
240 : */
241 : void from_euler(T roll, T pitch, T yaw);
242 :
243 : /* create eulers from a rotation matrix.
244 : roll is from -Pi to Pi
245 : pitch is from -Pi/2 to Pi/2
246 : yaw is from -Pi to Pi
247 : euler order is 321
248 : */
249 : void to_euler(T *roll, T *pitch, T *yaw) const;
250 :
251 : // create matrix from rotation enum
252 : void from_rotation(enum Rotation rotation);
253 :
254 : /*
255 : calculate Euler angles (312 convention) for the matrix.
256 : See http://www.atacolorado.com/eulersequences.doc
257 : vector is returned in r, p, y order
258 : */
259 : Vector3<T> to_euler312() const;
260 :
261 : /*
262 : fill the matrix from Euler angles in radians in 312 convention
263 : */
264 : void from_euler312(T roll, T pitch, T yaw);
265 :
266 : // apply an additional rotation from a body frame gyro vector
267 : // to a rotation matrix.
268 : void rotate(const Vector3<T> &g);
269 :
270 : // create rotation matrix for rotation about the vector v by angle theta
271 : // See: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
272 : // "Rotation matrix from axis and angle"
273 : void from_axis_angle(const Vector3<T> &v, T theta);
274 :
275 : // normalize a rotation matrix
276 : void normalize(void);
277 :
278 : // double/float conversion
279 26218 : Matrix3<double> todouble(void) const {
280 26218 : return Matrix3<double>(a.todouble(), b.todouble(), c.todouble());
281 : }
282 0 : Matrix3<float> tofloat(void) const {
283 0 : return Matrix3<float>(a.tofloat(), b.tofloat(), c.tofloat());
284 : }
285 : };
286 :
287 : typedef Matrix3<int16_t> Matrix3i;
288 : typedef Matrix3<uint16_t> Matrix3ui;
289 : typedef Matrix3<int32_t> Matrix3l;
290 : typedef Matrix3<uint32_t> Matrix3ul;
291 : typedef Matrix3<float> Matrix3f;
292 : typedef Matrix3<double> Matrix3d;
|
