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 2012 Andrew Tridgell, all rights reserved.
17 : // Refactored by Jonathan Challinger
18 : #pragma once
19 :
20 : #include "definitions.h"
21 : #include "matrix3.h"
22 : #include <cmath>
23 : #if MATH_CHECK_INDEXES
24 : #include <assert.h>
25 : #endif
26 : #include <math.h>
27 :
28 : template <typename T>
29 : class QuaternionT {
30 : public:
31 : T q1, q2, q3, q4;
32 :
33 : // constructor creates a quaternion equivalent
34 : // to roll=0, pitch=0, yaw=0
35 1868675207 : QuaternionT()
36 : {
37 1868675207 : q1 = 1;
38 1868675207 : q2 = q3 = q4 = 0;
39 1868675207 : }
40 :
41 : // setting constructor
42 472273798 : QuaternionT(const T _q1, const T _q2, const T _q3, const T _q4) :
43 472273798 : q1(_q1), q2(_q2), q3(_q3), q4(_q4)
44 : {
45 472273798 : }
46 :
47 : // setting constructor
48 15549 : QuaternionT(const T _q[4]) :
49 15549 : q1(_q[0]), q2(_q[1]), q3(_q[2]), q4(_q[3])
50 : {
51 15549 : }
52 :
53 : // check if any elements are NAN
54 195238128 : bool is_nan(void) const WARN_IF_UNUSED
55 : {
56 195238128 : return isnan(q1) || isnan(q2) || isnan(q3) || isnan(q4);
57 : }
58 :
59 : // populate the supplied rotation matrix equivalent from this quaternion
60 : void rotation_matrix(Matrix3f &m) const;
61 : void rotation_matrix(Matrix3d &m) const;
62 :
63 : // make this quaternion equivalent to the supplied matrix
64 : void from_rotation_matrix(const Matrix3<T> &m);
65 :
66 : // create a quaternion from a given rotation
67 : void from_rotation(enum Rotation rotation);
68 :
69 : // rotate this quaternion by the given rotation
70 : void rotate(enum Rotation rotation);
71 :
72 : // convert a vector from earth to body frame
73 : void earth_to_body(Vector3<T> &v) const;
74 :
75 : // create a quaternion from Euler angles using 321 euler ordering
76 : void from_euler(T roll, T pitch, T yaw);
77 : void from_euler(const Vector3<T> &v);
78 :
79 : // create a quaternion from Euler angles applied in yaw, roll, pitch order (312)
80 : // instead of the normal yaw, pitch, roll order
81 : void from_vector312(T roll, T pitch, T yaw);
82 :
83 : // convert this quaternion to a rotation vector where the direction of the vector represents
84 : // the axis of rotation and the length of the vector represents the angle of rotation
85 : void to_axis_angle(Vector3<T> &v) const;
86 :
87 : // create a quaternion from a rotation vector where the direction of the vector represents
88 : // the axis of rotation and the length of the vector represents the angle of rotation
89 : void from_axis_angle(Vector3<T> v);
90 :
91 : // create a quaternion from its axis-angle representation
92 : // the axis vector must be length 1. the rotation angle theta is in radians
93 : void from_axis_angle(const Vector3<T> &axis, T theta);
94 :
95 : // rotate by the provided rotation vector
96 : void rotate(const Vector3<T> &v);
97 :
98 : // create a quaternion from a rotation vector
99 : // only use with small angles. I.e. length of v should less than 0.17 radians (i.e. 10 degrees)
100 : void from_axis_angle_fast(Vector3<T> v);
101 :
102 : // create a quaternion from its axis-angle representation
103 : // the axis vector must be length 1, theta should less than 0.17 radians (i.e. 10 degrees)
104 : void from_axis_angle_fast(const Vector3<T> &axis, T theta);
105 :
106 : // create a quaternion by integrating an angular velocity over some time_delta, which is
107 : // assumed to be small
108 : void from_angular_velocity(const Vector3<T>& angular_velocity, float time_delta);
109 :
110 : // rotate by the provided rotation vector
111 : // only use with small angles. I.e. length of v should less than 0.17 radians (i.e. 10 degrees)
112 : void rotate_fast(const Vector3<T> &v);
113 :
114 : // get euler roll angle in radians
115 : T get_euler_roll() const;
116 :
117 : // get euler pitch angle in radians
118 : T get_euler_pitch() const;
119 :
120 : // get euler yaw angle in radians
121 : T get_euler_yaw() const;
122 :
123 : // create eulers (in radians) from a quaternion, using 321 ordering
124 : void to_euler(float &roll, float &pitch, float &yaw) const;
125 81122781 : void to_euler(Vector3f &rpy) const {
126 81122781 : to_euler(rpy.x, rpy.y, rpy.z);
127 81122781 : }
128 : void to_euler(double &roll, double &pitch, double &yaw) const;
129 2988 : void to_euler(Vector3d &rpy) const {
130 2988 : to_euler(rpy.x, rpy.y, rpy.z);
131 2988 : }
132 :
133 : // create eulers from a quaternion with 312 ordering
134 : Vector3<T> to_vector312(void) const;
135 :
136 : T length_squared(void) const;
137 : T length(void) const;
138 : void normalize();
139 :
140 : // Checks if each element of the quaternion is zero
141 : bool is_zero(void) const;
142 :
143 : // zeros the quaternion to [0, 0, 0, 0], an invalid quaternion
144 : // See initialize() if you want the zero rotation quaternion
145 : void zero(void);
146 :
147 : // Checks if the quaternion is unit_length within a tolerance
148 : // Returns True: if its magnitude is close to unit length +/- 1E-3
149 : // This limit is somewhat greater than sqrt(FLT_EPSL)
150 : bool is_unit_length(void) const;
151 :
152 : // initialise the quaternion to no rotation
153 2713225 : void initialise()
154 : {
155 2713225 : q1 = 1.0f;
156 2713225 : q2 = q3 = q4 = 0.0f;
157 2713225 : }
158 :
159 : QuaternionT<T> inverse(void) const;
160 :
161 : // reverse the rotation of this quaternion
162 : void invert();
163 :
164 : // allow a quaternion to be used as an array, 0 indexed
165 695423355 : T & operator[](uint8_t i)
166 : {
167 695423355 : T *_v = &q1;
168 : #if MATH_CHECK_INDEXES
169 : assert(i < 4);
170 : #endif
171 695423355 : return _v[i];
172 : }
173 :
174 88 : const T & operator[](uint8_t i) const
175 : {
176 88 : const T *_v = &q1;
177 : #if MATH_CHECK_INDEXES
178 : assert(i < 4);
179 : #endif
180 88 : return _v[i];
181 : }
182 :
183 : QuaternionT<T> operator*(const QuaternionT<T> &v) const;
184 : Vector3<T> operator*(const Vector3<T> &v) const;
185 : QuaternionT<T> &operator*=(const QuaternionT<T> &v);
186 : QuaternionT<T> operator/(const QuaternionT<T> &v) const;
187 :
188 : // angular difference between quaternions
189 : QuaternionT<T> angular_difference(const QuaternionT<T> &v) const;
190 :
191 : // absolute (e.g. always positive) earth-frame roll-pitch difference (in radians) between this Quaternion and another
192 : T roll_pitch_difference(const QuaternionT<T> &v) const;
193 :
194 : // double/float conversion
195 0 : QuaternionT<double> todouble(void) const {
196 0 : return QuaternionT<double>(q1,q2,q3,q4);
197 : }
198 48579808 : QuaternionT<float> tofloat(void) const {
199 48579808 : return QuaternionT<float>(q1,q2,q3,q4);
200 : }
201 : };
202 :
203 : typedef QuaternionT<float> Quaternion;
204 : typedef QuaternionT<double> QuaternionD;
205 :
206 :
207 :
|
