adafruit_bno055 / utility / quaternion.h @ 88b09bb5
History | View | Annotate | Download (6.541 KB)
1 | 4bc1c0c1 | Kevin Townsend | /*
|
---|---|---|---|
2 | Inertial Measurement Unit Maths Library
|
||
3 | Copyright (C) 2013-2014 Samuel Cowen
|
||
4 | www.camelsoftware.com
|
||
5 | |||
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 | |||
21 | #ifndef IMUMATH_QUATERNION_HPP
|
||
22 | #define IMUMATH_QUATERNION_HPP
|
||
23 | |||
24 | #include <stdlib.h> |
||
25 | #include <string.h> |
||
26 | #include <stdint.h> |
||
27 | #include <math.h> |
||
28 | |||
29 | 4a94251b | Gé Vissers | #include "matrix.h" |
30 | 4bc1c0c1 | Kevin Townsend | |
31 | |||
32 | namespace imu |
||
33 | { |
||
34 | |||
35 | class Quaternion |
||
36 | { |
||
37 | public:
|
||
38 | 88b09bb5 | Gé Vissers | Quaternion(): _w(1.0), _x(0.0), _y(0.0), _z(0.0) {} |
39 | 4bc1c0c1 | Kevin Townsend | |
40 | 88b09bb5 | Gé Vissers | Quaternion(double w, double x, double y, double z): |
41 | _w(w), _x(x), _y(y), _z(z) {} |
||
42 | 4bc1c0c1 | Kevin Townsend | |
43 | 88b09bb5 | Gé Vissers | Quaternion(double w, Vector<3> vec): |
44 | _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {} |
||
45 | 4bc1c0c1 | Kevin Townsend | |
46 | double& w()
|
||
47 | { |
||
48 | return _w;
|
||
49 | } |
||
50 | double& x()
|
||
51 | { |
||
52 | return _x;
|
||
53 | } |
||
54 | double& y()
|
||
55 | { |
||
56 | return _y;
|
||
57 | } |
||
58 | double& z()
|
||
59 | { |
||
60 | return _z;
|
||
61 | } |
||
62 | |||
63 | 0695bf91 | Paul Du Bois (laptop) | double w() const |
64 | { |
||
65 | return _w;
|
||
66 | } |
||
67 | double x() const |
||
68 | { |
||
69 | return _x;
|
||
70 | } |
||
71 | double y() const |
||
72 | { |
||
73 | return _y;
|
||
74 | } |
||
75 | double z() const |
||
76 | { |
||
77 | return _z;
|
||
78 | } |
||
79 | |||
80 | double magnitude() const |
||
81 | 4bc1c0c1 | Kevin Townsend | { |
82 | double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z);
|
||
83 | return sqrt(res);
|
||
84 | } |
||
85 | |||
86 | void normalize()
|
||
87 | { |
||
88 | 0695bf91 | Paul Du Bois (laptop) | double mag = magnitude();
|
89 | 4bc1c0c1 | Kevin Townsend | *this = this->scale(1/mag);
|
90 | } |
||
91 | |||
92 | 0ecc7129 | Gé Vissers | const Quaternion conjugate() const |
93 | 4bc1c0c1 | Kevin Townsend | { |
94 | 0ecc7129 | Gé Vissers | return Quaternion(_w, -_x, -_y, -_z);
|
95 | 4bc1c0c1 | Kevin Townsend | } |
96 | |||
97 | void fromAxisAngle(Vector<3> axis, double theta) |
||
98 | { |
||
99 | _w = cos(theta/2);
|
||
100 | //only need to calculate sine of half theta once
|
||
101 | double sht = sin(theta/2); |
||
102 | _x = axis.x() * sht; |
||
103 | _y = axis.y() * sht; |
||
104 | _z = axis.z() * sht; |
||
105 | } |
||
106 | |||
107 | e8e79779 | Gé Vissers | void fromMatrix(const Matrix<3>& m) |
108 | 4bc1c0c1 | Kevin Townsend | { |
109 | e8e79779 | Gé Vissers | double tr = m.trace();
|
110 | 4bc1c0c1 | Kevin Townsend | |
111 | e8e79779 | Gé Vissers | double S;
|
112 | 4bc1c0c1 | Kevin Townsend | if (tr > 0) |
113 | { |
||
114 | S = sqrt(tr+1.0) * 2; |
||
115 | _w = 0.25 * S; |
||
116 | _x = (m(2, 1) - m(1, 2)) / S; |
||
117 | _y = (m(0, 2) - m(2, 0)) / S; |
||
118 | _z = (m(1, 0) - m(0, 1)) / S; |
||
119 | } |
||
120 | e8e79779 | Gé Vissers | else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) |
121 | 4bc1c0c1 | Kevin Townsend | { |
122 | S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; |
||
123 | _w = (m(2, 1) - m(1, 2)) / S; |
||
124 | _x = 0.25 * S; |
||
125 | _y = (m(0, 1) + m(1, 0)) / S; |
||
126 | _z = (m(0, 2) + m(2, 0)) / S; |
||
127 | } |
||
128 | e8e79779 | Gé Vissers | else if (m(1, 1) > m(2, 2)) |
129 | 4bc1c0c1 | Kevin Townsend | { |
130 | S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; |
||
131 | _w = (m(0, 2) - m(2, 0)) / S; |
||
132 | _x = (m(0, 1) + m(1, 0)) / S; |
||
133 | _y = 0.25 * S; |
||
134 | _z = (m(1, 2) + m(2, 1)) / S; |
||
135 | } |
||
136 | else
|
||
137 | { |
||
138 | S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; |
||
139 | _w = (m(1, 0) - m(0, 1)) / S; |
||
140 | _x = (m(0, 2) + m(2, 0)) / S; |
||
141 | _y = (m(1, 2) + m(2, 1)) / S; |
||
142 | _z = 0.25 * S; |
||
143 | } |
||
144 | } |
||
145 | |||
146 | 0695bf91 | Paul Du Bois (laptop) | void toAxisAngle(Vector<3>& axis, float& angle) const |
147 | 4bc1c0c1 | Kevin Townsend | { |
148 | float sqw = sqrt(1-_w*_w); |
||
149 | if(sqw == 0) //it's a singularity and divide by zero, avoid |
||
150 | return;
|
||
151 | |||
152 | angle = 2 * acos(_w);
|
||
153 | axis.x() = _x / sqw; |
||
154 | axis.y() = _y / sqw; |
||
155 | axis.z() = _z / sqw; |
||
156 | } |
||
157 | |||
158 | 0695bf91 | Paul Du Bois (laptop) | Matrix<3> toMatrix() const |
159 | 4bc1c0c1 | Kevin Townsend | { |
160 | Matrix<3> ret;
|
||
161 | ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z)); |
||
162 | ret.cell(0, 1) = (2*_x*_y)-(2*_w*_z); |
||
163 | ret.cell(0, 2) = (2*_x*_z)+(2*_w*_y); |
||
164 | |||
165 | ret.cell(1, 0) = (2*_x*_y)+(2*_w*_z); |
||
166 | ret.cell(1, 1) = 1-(2*(_x*_x))-(2*(_z*_z)); |
||
167 | ret.cell(1, 2) = (2*(_y*_z))-(2*(_w*_x)); |
||
168 | |||
169 | ret.cell(2, 0) = (2*(_x*_z))-(2*_w*_y); |
||
170 | ret.cell(2, 1) = (2*_y*_z)+(2*_w*_x); |
||
171 | ret.cell(2, 2) = 1-(2*(_x*_x))-(2*(_y*_y)); |
||
172 | return ret;
|
||
173 | } |
||
174 | |||
175 | |||
176 | 0695bf91 | Paul Du Bois (laptop) | // Returns euler angles that represent the quaternion. Angles are
|
177 | // returned in rotation order and right-handed about the specified
|
||
178 | // axes:
|
||
179 | //
|
||
180 | // v[0] is applied 1st about z (ie, roll)
|
||
181 | // v[1] is applied 2nd about y (ie, pitch)
|
||
182 | // v[2] is applied 3rd about x (ie, yaw)
|
||
183 | //
|
||
184 | // Note that this means result.x() is not a rotation about x;
|
||
185 | // similarly for result.z().
|
||
186 | //
|
||
187 | Vector<3> toEuler() const |
||
188 | 4bc1c0c1 | Kevin Townsend | { |
189 | Vector<3> ret;
|
||
190 | double sqw = _w*_w;
|
||
191 | double sqx = _x*_x;
|
||
192 | double sqy = _y*_y;
|
||
193 | double sqz = _z*_z;
|
||
194 | |||
195 | ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw)); |
||
196 | ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw)); |
||
197 | ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw)); |
||
198 | |||
199 | return ret;
|
||
200 | } |
||
201 | |||
202 | 0695bf91 | Paul Du Bois (laptop) | Vector<3> toAngularVelocity(float dt) const |
203 | 4bc1c0c1 | Kevin Townsend | { |
204 | Vector<3> ret;
|
||
205 | Quaternion one(1.0, 0.0, 0.0, 0.0); |
||
206 | Quaternion delta = one - *this; |
||
207 | Quaternion r = (delta/dt); |
||
208 | r = r * 2;
|
||
209 | r = r * one; |
||
210 | |||
211 | ret.x() = r.x(); |
||
212 | ret.y() = r.y(); |
||
213 | ret.z() = r.z(); |
||
214 | return ret;
|
||
215 | } |
||
216 | |||
217 | 0695bf91 | Paul Du Bois (laptop) | Vector<3> rotateVector(Vector<2> v) const |
218 | 4bc1c0c1 | Kevin Townsend | { |
219 | Vector<3> ret(v.x(), v.y(), 0.0); |
||
220 | return rotateVector(ret);
|
||
221 | } |
||
222 | |||
223 | 0695bf91 | Paul Du Bois (laptop) | Vector<3> rotateVector(Vector<3> v) const |
224 | 4bc1c0c1 | Kevin Townsend | { |
225 | Vector<3> qv(this->x(), this->y(), this->z());
|
||
226 | Vector<3> t;
|
||
227 | t = qv.cross(v) * 2.0; |
||
228 | return v + (t * _w) + qv.cross(t);
|
||
229 | } |
||
230 | |||
231 | |||
232 | 88b09bb5 | Gé Vissers | Quaternion operator*(const Quaternion& q) const |
233 | 4bc1c0c1 | Kevin Townsend | { |
234 | 0ecc7129 | Gé Vissers | return Quaternion(
|
235 | _w*q._w - _x*q._x - _y*q._y - _z*q._z, |
||
236 | _w*q._x + _x*q._w + _y*q._z - _z*q._y, |
||
237 | _w*q._y - _x*q._z + _y*q._w + _z*q._x, |
||
238 | _w*q._z + _x*q._y - _y*q._x + _z*q._w |
||
239 | ); |
||
240 | 4bc1c0c1 | Kevin Townsend | } |
241 | |||
242 | 88b09bb5 | Gé Vissers | Quaternion operator+(const Quaternion& q) const |
243 | 4bc1c0c1 | Kevin Townsend | { |
244 | 0ecc7129 | Gé Vissers | return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z);
|
245 | 4bc1c0c1 | Kevin Townsend | } |
246 | |||
247 | 88b09bb5 | Gé Vissers | Quaternion operator-(const Quaternion& q) const |
248 | 4bc1c0c1 | Kevin Townsend | { |
249 | 0ecc7129 | Gé Vissers | return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z);
|
250 | 4bc1c0c1 | Kevin Townsend | } |
251 | |||
252 | 88b09bb5 | Gé Vissers | Quaternion operator/(double scalar) const |
253 | 4bc1c0c1 | Kevin Townsend | { |
254 | 0ecc7129 | Gé Vissers | return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar);
|
255 | 4bc1c0c1 | Kevin Townsend | } |
256 | |||
257 | 88b09bb5 | Gé Vissers | Quaternion operator*(double scalar) const |
258 | 4bc1c0c1 | Kevin Townsend | { |
259 | 0ecc7129 | Gé Vissers | return scale(scalar);
|
260 | 4bc1c0c1 | Kevin Townsend | } |
261 | |||
262 | 88b09bb5 | Gé Vissers | Quaternion scale(double scalar) const |
263 | 0695bf91 | Paul Du Bois (laptop) | { |
264 | 0ecc7129 | Gé Vissers | return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar);
|
265 | 4bc1c0c1 | Kevin Townsend | } |
266 | |||
267 | private:
|
||
268 | double _w, _x, _y, _z;
|
||
269 | }; |
||
270 | |||
271 | 0ecc7129 | Gé Vissers | } // namespace
|
272 | 4bc1c0c1 | Kevin Townsend | |
273 | #endif |