Line data Source code
1 : /*
2 : * matrix3.cpp
3 : * Copyright (C) Andrew Tridgell 2012
4 : *
5 : * This file is free software: you can redistribute it and/or modify it
6 : * under the terms of the GNU General Public License as published by the
7 : * Free Software Foundation, either version 3 of the License, or
8 : * (at your option) any later version.
9 : *
10 : * This file is distributed in the hope that it will be useful, but
11 : * WITHOUT ANY WARRANTY; without even the implied warranty of
12 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 : * See the GNU General Public License for more details.
14 : *
15 : * You should have received a copy of the GNU General Public License along
16 : * with this program. If not, see <http://www.gnu.org/licenses/>.
17 : */
18 :
19 : #pragma GCC optimize("O2")
20 :
21 : #include "AP_Math.h"
22 :
23 : // create a rotation matrix given some euler angles
24 : // this is based on https://github.com/ArduPilot/Datasheets/blob/main/References/EulerAngles.pdf
25 : template <typename T>
26 264104370 : void Matrix3<T>::from_euler(T roll, T pitch, T yaw)
27 : {
28 264104370 : const T cp = cosF(pitch);
29 264104370 : const T sp = sinF(pitch);
30 264104370 : const T sr = sinF(roll);
31 264104370 : const T cr = cosF(roll);
32 264104370 : const T sy = sinF(yaw);
33 264104370 : const T cy = cosF(yaw);
34 :
35 264104370 : a.x = cp * cy;
36 264104370 : a.y = (sr * sp * cy) - (cr * sy);
37 264104370 : a.z = (cr * sp * cy) + (sr * sy);
38 264104370 : b.x = cp * sy;
39 264104370 : b.y = (sr * sp * sy) + (cr * cy);
40 264104370 : b.z = (cr * sp * sy) - (sr * cy);
41 264104370 : c.x = -sp;
42 264104370 : c.y = sr * cp;
43 264104370 : c.z = cr * cp;
44 264104370 : }
45 :
46 : // calculate euler angles from a rotation matrix
47 : // this is based on https://github.com/ArduPilot/Datasheets/blob/main/References/EulerAngles.pdf
48 : template <typename T>
49 653730289 : void Matrix3<T>::to_euler(T *roll, T *pitch, T *yaw) const
50 : {
51 653730289 : if (pitch != nullptr) {
52 653730289 : *pitch = -safe_asin(c.x);
53 : }
54 653730289 : if (roll != nullptr) {
55 653730289 : *roll = atan2F(c.y, c.z);
56 : }
57 653730289 : if (yaw != nullptr) {
58 653730289 : *yaw = atan2F(b.x, a.x);
59 : }
60 653730289 : }
61 :
62 : template <typename T>
63 180718 : void Matrix3<T>::from_rotation(enum Rotation rotation)
64 : {
65 180718 : (*this).a = {1,0,0};
66 180718 : (*this).b = {0,1,0};
67 180718 : (*this).c = {0,0,1};
68 :
69 180718 : (*this).a.rotate(rotation);
70 180718 : (*this).b.rotate(rotation);
71 180718 : (*this).c.rotate(rotation);
72 180718 : (*this).transpose();
73 180718 : }
74 :
75 : /*
76 : calculate Euler angles (312 convention) for the matrix.
77 : See http://www.atacolorado.com/eulersequences.doc
78 : vector is returned in r, p, y order
79 : */
80 : template <typename T>
81 2501645 : Vector3<T> Matrix3<T>::to_euler312() const
82 : {
83 2501645 : return Vector3<T>(asinF(c.y),
84 2501645 : atan2F(-c.x, c.z),
85 2501645 : atan2F(-a.y, b.y));
86 : }
87 :
88 : /*
89 : fill the matrix from Euler angles in radians in 312 convention
90 : */
91 : template <typename T>
92 369941 : void Matrix3<T>::from_euler312(T roll, T pitch, T yaw)
93 : {
94 369941 : const T c3 = cosF(pitch);
95 369941 : const T s3 = sinF(pitch);
96 369941 : const T s2 = sinF(roll);
97 369941 : const T c2 = cosF(roll);
98 369941 : const T s1 = sinF(yaw);
99 369941 : const T c1 = cosF(yaw);
100 :
101 369941 : a.x = c1 * c3 - s1 * s2 * s3;
102 369941 : b.y = c1 * c2;
103 369941 : c.z = c3 * c2;
104 369941 : a.y = -c2*s1;
105 369941 : a.z = s3*c1 + c3*s2*s1;
106 369941 : b.x = c3*s1 + s3*s2*c1;
107 369941 : b.z = s1*s3 - s2*c1*c3;
108 369941 : c.x = -s3*c2;
109 369941 : c.y = s2;
110 369941 : }
111 :
112 : // apply an additional rotation from a body frame gyro vector
113 : // to a rotation matrix.
114 : template <typename T>
115 389453244 : void Matrix3<T>::rotate(const Vector3<T> &g)
116 : {
117 389453244 : (*this) += Matrix3<T>{
118 389453244 : a.y * g.z - a.z * g.y, a.z * g.x - a.x * g.z, a.x * g.y - a.y * g.x,
119 389453244 : b.y * g.z - b.z * g.y, b.z * g.x - b.x * g.z, b.x * g.y - b.y * g.x,
120 389453244 : c.y * g.z - c.z * g.y, c.z * g.x - c.x * g.z, c.x * g.y - c.y * g.x
121 : };
122 389453244 : }
123 :
124 : /*
125 : re-normalise a rotation matrix
126 : */
127 : template <typename T>
128 347759103 : void Matrix3<T>::normalize(void)
129 : {
130 347759103 : const T error = a * b;
131 347759103 : const Vector3<T> t0 = a - (b * (0.5f * error));
132 347759103 : const Vector3<T> t1 = b - (a * (0.5f * error));
133 347759103 : const Vector3<T> t2 = t0 % t1;
134 347759103 : a = t0 * (1.0f / t0.length());
135 347759103 : b = t1 * (1.0f / t1.length());
136 347759103 : c = t2 * (1.0f / t2.length());
137 347759103 : }
138 :
139 : // multiplication by a vector
140 : template <typename T>
141 4184596797 : Vector3<T> Matrix3<T>::operator *(const Vector3<T> &v) const
142 : {
143 8096670310 : return Vector3<T>(a.x * v.x + a.y * v.y + a.z * v.z,
144 4184596797 : b.x * v.x + b.y * v.y + b.z * v.z,
145 4184596797 : c.x * v.x + c.y * v.y + c.z * v.z);
146 : }
147 :
148 : // multiplication by a vector, extracting only the xy components
149 : template <typename T>
150 1488533 : Vector2<T> Matrix3<T>::mulXY(const Vector3<T> &v) const
151 : {
152 2977066 : return Vector2<T>(a.x * v.x + a.y * v.y + a.z * v.z,
153 1488533 : b.x * v.x + b.y * v.y + b.z * v.z);
154 : }
155 :
156 : // multiplication of transpose by a vector
157 : template <typename T>
158 252265943 : Vector3<T> Matrix3<T>::mul_transpose(const Vector3<T> &v) const
159 : {
160 481276237 : return Vector3<T>(a.x * v.x + b.x * v.y + c.x * v.z,
161 252265943 : a.y * v.x + b.y * v.y + c.y * v.z,
162 252265943 : a.z * v.x + b.z * v.y + c.z * v.z);
163 : }
164 :
165 : // multiplication by another Matrix3<T>
166 : template <typename T>
167 255303368 : Matrix3<T> Matrix3<T>::operator *(const Matrix3<T> &m) const
168 : {
169 255303368 : Matrix3<T> temp (Vector3<T>(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x,
170 255303368 : a.x * m.a.y + a.y * m.b.y + a.z * m.c.y,
171 255303368 : a.x * m.a.z + a.y * m.b.z + a.z * m.c.z),
172 510606736 : Vector3<T>(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x,
173 255303368 : b.x * m.a.y + b.y * m.b.y + b.z * m.c.y,
174 255303368 : b.x * m.a.z + b.y * m.b.z + b.z * m.c.z),
175 510606736 : Vector3<T>(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x,
176 255303368 : c.x * m.a.y + c.y * m.b.y + c.z * m.c.y,
177 255303368 : c.x * m.a.z + c.y * m.b.z + c.z * m.c.z));
178 255303368 : return temp;
179 : }
180 :
181 : template <typename T>
182 988373468 : Matrix3<T> Matrix3<T>::transposed(void) const
183 : {
184 988373468 : return Matrix3<T>(Vector3<T>(a.x, b.x, c.x),
185 988373468 : Vector3<T>(a.y, b.y, c.y),
186 1976746936 : Vector3<T>(a.z, b.z, c.z));
187 : }
188 :
189 : template <typename T>
190 6314 : T Matrix3<T>::det() const
191 : {
192 6314 : return a.x * (b.y * c.z - b.z * c.y) +
193 6314 : a.y * (b.z * c.x - b.x * c.z) +
194 6314 : a.z * (b.x * c.y - b.y * c.x);
195 : }
196 :
197 : template <typename T>
198 6308 : bool Matrix3<T>::inverse(Matrix3<T>& inv) const
199 : {
200 6308 : const T d = det();
201 :
202 6308 : if (is_zero(d)) {
203 : return false;
204 : }
205 :
206 6304 : inv.a.x = (b.y * c.z - c.y * b.z) / d;
207 6304 : inv.a.y = (a.z * c.y - a.y * c.z) / d;
208 6304 : inv.a.z = (a.y * b.z - a.z * b.y) / d;
209 6304 : inv.b.x = (b.z * c.x - b.x * c.z) / d;
210 6304 : inv.b.y = (a.x * c.z - a.z * c.x) / d;
211 6304 : inv.b.z = (b.x * a.z - a.x * b.z) / d;
212 6304 : inv.c.x = (b.x * c.y - c.x * b.y) / d;
213 6304 : inv.c.y = (c.x * a.y - a.x * c.y) / d;
214 6304 : inv.c.z = (a.x * b.y - b.x * a.y) / d;
215 :
216 6304 : return true;
217 : }
218 :
219 : template <typename T>
220 6302 : bool Matrix3<T>::invert()
221 : {
222 6302 : Matrix3<T> inv;
223 6302 : bool success = inverse(inv);
224 6302 : if (success) {
225 6300 : *this = inv;
226 : }
227 6302 : return success;
228 : }
229 :
230 : // create rotation matrix for rotation about the vector v by angle theta
231 : // See: http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/
232 : template <typename T>
233 0 : void Matrix3<T>::from_axis_angle(const Vector3<T> &v, T theta)
234 : {
235 0 : const T C = cosF(theta);
236 0 : const T S = sinF(theta);
237 0 : const T t = 1.0f - C;
238 0 : const Vector3<T> normv = v.normalized();
239 0 : const T x = normv.x;
240 0 : const T y = normv.y;
241 0 : const T z = normv.z;
242 :
243 0 : a.x = t*x*x + C;
244 0 : a.y = t*x*y - z*S;
245 0 : a.z = t*x*z + y*S;
246 0 : b.x = t*x*y + z*S;
247 0 : b.y = t*y*y + C;
248 0 : b.z = t*y*z - x*S;
249 0 : c.x = t*x*z - y*S;
250 0 : c.y = t*y*z + x*S;
251 0 : c.z = t*z*z + C;
252 0 : }
253 :
254 :
255 : // define for float and double
256 : template class Matrix3<float>;
257 : template class Matrix3<double>;
|
