1//************************************ bs::framework - Copyright 2018 Marko Pintera **************************************//
2//*********** Licensed under the MIT license. See LICENSE.md for full terms. This notice is not to be removed. ***********//
3#pragma once
4
5#include "Prerequisites/BsPrerequisitesUtil.h"
6#include "Math/BsMath.h"
7#include "Math/BsVector3.h"
8
9namespace bs
10{
11 /** @addtogroup Math
12 * @{
13 */
14
15 /** Represents a quaternion used for 3D rotations. */
16 class BS_UTILITY_EXPORT Quaternion
17 {
18 private:
19 struct EulerAngleOrderData
20 {
21 int a, b, c;
22 };
23
24 public:
25 Quaternion() = default;
26 constexpr Quaternion(const Quaternion&) = default;
27 constexpr Quaternion& operator=(const Quaternion&) = default;
28
29 constexpr Quaternion(BS_ZERO zero)
30 : x(0.0f), y(0.0f), z(0.0f), w(0.0f)
31 { }
32
33 constexpr Quaternion(BS_IDENTITY)
34 : x(0.0f), y(0.0f), z(0.0f), w(1.0f)
35 { }
36
37 constexpr Quaternion(float w, float x, float y, float z)
38 :x(x), y(y), z(z), w(w)
39 { }
40
41 /** Construct a quaternion from a rotation matrix. */
42 explicit Quaternion(const Matrix3& rot)
43 {
44 fromRotationMatrix(rot);
45 }
46
47 /** Construct a quaternion from an angle/axis. */
48 explicit Quaternion(const Vector3& axis, const Radian& angle)
49 {
50 fromAxisAngle(axis, angle);
51 }
52
53 /** Construct a quaternion from 3 orthonormal local axes. */
54 explicit Quaternion(const Vector3& xaxis, const Vector3& yaxis, const Vector3& zaxis)
55 {
56 fromAxes(xaxis, yaxis, zaxis);
57 }
58
59 /**
60 * Construct a quaternion from euler angles, YXZ ordering.
61 *
62 * @see Quaternion::fromEulerAngles
63 */
64 explicit Quaternion(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle)
65 {
66 fromEulerAngles(xAngle, yAngle, zAngle);
67 }
68
69 /**
70 * Construct a quaternion from euler angles, custom ordering.
71 *
72 * @see Quaternion::fromEulerAngles
73 */
74 explicit Quaternion(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle, EulerAngleOrder order)
75 {
76 fromEulerAngles(xAngle, yAngle, zAngle, order);
77 }
78
79 /** Exchange the contents of this quaternion with another. */
80 void swap(Quaternion& other)
81 {
82 std::swap(w, other.w);
83 std::swap(x, other.x);
84 std::swap(y, other.y);
85 std::swap(z, other.z);
86 }
87
88 float operator[] (const size_t i) const
89 {
90 assert(i < 4);
91
92 return *(&x+i);
93 }
94
95 float& operator[] (const size_t i)
96 {
97 assert(i < 4);
98
99 return *(&x+i);
100 }
101
102 /**
103 * Initializes the quaternion from a 3x3 rotation matrix.
104 *
105 * @note It's up to the caller to ensure the matrix is orthonormal.
106 */
107 void fromRotationMatrix(const Matrix3& mat);
108
109 /**
110 * Initializes the quaternion from an angle axis pair. Quaternion will represent a rotation of "angle" radians
111 * around "axis".
112 */
113 void fromAxisAngle(const Vector3& axis, const Radian& angle);
114
115 /**
116 * Initializes the quaternion from orthonormal set of axes. Quaternion will represent a rotation from base axes
117 * to the specified set of axes.
118 *
119 * @note It's up to the caller to ensure the axes are orthonormal.
120 */
121 void fromAxes(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis);
122
123 /**
124 * Creates a quaternion from the provided Pitch/Yaw/Roll angles.
125 *
126 * @param[in] xAngle Rotation about x axis. (AKA Pitch)
127 * @param[in] yAngle Rotation about y axis. (AKA Yaw)
128 * @param[in] zAngle Rotation about z axis. (AKA Roll)
129 *
130 * @note
131 * Since different values will be produced depending in which order are the rotations applied, this method assumes
132 * they are applied in YXZ order. If you need a specific order, use the overloaded fromEulerAngles() method instead.
133 */
134 void fromEulerAngles(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle);
135
136 /**
137 * Creates a quaternion from the provided Pitch/Yaw/Roll angles.
138 *
139 * @param[in] xAngle Rotation about x axis. (AKA Pitch)
140 * @param[in] yAngle Rotation about y axis. (AKA Yaw)
141 * @param[in] zAngle Rotation about z axis. (AKA Roll)
142 * @param[in] order The order in which rotations will be extracted. Different values can be retrieved depending
143 * on the order.
144 */
145 void fromEulerAngles(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle, EulerAngleOrder order);
146
147 /**
148 * Converts a quaternion to a rotation matrix.
149 */
150 void toRotationMatrix(Matrix3& mat) const;
151
152 /**
153 * Converts a quaternion to an angle axis pair.
154 *
155 * @param[out] axis The axis around the which rotation takes place.
156 * @param[out] angle The angle in radians determining amount of rotation around the axis.
157 */
158 void toAxisAngle(Vector3& axis, Radian& angle) const;
159
160 /**
161 * Converts a quaternion to an orthonormal set of axes.
162 *
163 * @param[out] xAxis The X axis.
164 * @param[out] yAxis The Y axis.
165 * @param[out] zAxis The Z axis.
166 */
167 void toAxes(Vector3& xAxis, Vector3& yAxis, Vector3& zAxis) const;
168
169 /**
170 * Extracts Pitch/Yaw/Roll rotations from this quaternion.
171 *
172 * @param[out] xAngle Rotation about x axis. (AKA Pitch)
173 * @param[out] yAngle Rotation about y axis. (AKA Yaw)
174 * @param[out] zAngle Rotation about z axis. (AKA Roll)
175 *
176 * @return True if unique solution was found, false otherwise.
177 */
178 bool toEulerAngles(Radian& xAngle, Radian& yAngle, Radian& zAngle) const;
179
180 /** Gets the positive x-axis of the coordinate system transformed by this quaternion. */
181 Vector3 xAxis() const;
182
183 /** Gets the positive y-axis of the coordinate system transformed by this quaternion. */
184 Vector3 yAxis() const;
185
186 /** Gets the positive z-axis of the coordinate system transformed by this quaternion. */
187 Vector3 zAxis() const;
188
189
190 Quaternion operator+ (const Quaternion& rhs) const
191 {
192 return Quaternion(w + rhs.w, x + rhs.x, y + rhs.y, z + rhs.z);
193 }
194
195 Quaternion operator- (const Quaternion& rhs) const
196 {
197 return Quaternion(w - rhs.w, x - rhs.x, y - rhs.y, z - rhs.z);
198 }
199
200 Quaternion operator* (const Quaternion& rhs) const
201 {
202 return Quaternion
203 (
204 w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z,
205 w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y,
206 w * rhs.y + y * rhs.w + z * rhs.x - x * rhs.z,
207 w * rhs.z + z * rhs.w + x * rhs.y - y * rhs.x
208 );
209 }
210
211 Quaternion operator* (float rhs) const
212 {
213 return Quaternion(rhs * w, rhs * x, rhs * y, rhs * z);
214 }
215
216 Quaternion operator/ (float rhs) const
217 {
218 assert(rhs != 0.0);
219
220 const float inv = 1.0f / rhs;
221 return Quaternion(w * inv, x * inv, y * inv, z * inv);
222 }
223
224 Quaternion operator- () const
225 {
226 return Quaternion(-w, -x, -y, -z);
227 }
228
229 bool operator== (const Quaternion& rhs) const
230 {
231 return (rhs.x == x) && (rhs.y == y) && (rhs.z == z) && (rhs.w == w);
232 }
233
234 bool operator!= (const Quaternion& rhs) const
235 {
236 return !operator==(rhs);
237 }
238
239 Quaternion& operator+= (const Quaternion& rhs)
240 {
241 w += rhs.w;
242 x += rhs.x;
243 y += rhs.y;
244 z += rhs.z;
245
246 return *this;
247 }
248
249 Quaternion& operator-= (const Quaternion& rhs)
250 {
251 w -= rhs.w;
252 x -= rhs.x;
253 y -= rhs.y;
254 z -= rhs.z;
255
256 return *this;
257 }
258
259 Quaternion& operator*= (const Quaternion& rhs)
260 {
261 float newW = w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z;
262 float newX = w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y;
263 float newY = w * rhs.y + y * rhs.w + z * rhs.x - x * rhs.z;
264 float newZ = w * rhs.z + z * rhs.w + x * rhs.y - y * rhs.x;
265
266 w = newW;
267 x = newX;
268 y = newY;
269 z = newZ;
270
271 return *this;
272 }
273
274 friend Quaternion operator* (float lhs, const Quaternion& rhs)
275 {
276 return Quaternion(lhs * rhs.w, lhs * rhs.x, lhs * rhs.y, lhs * rhs.z);
277 }
278
279 /** Calculates the dot product of this quaternion and another. */
280 float dot(const Quaternion& other) const
281 {
282 return w * other.w + x * other.x + y * other.y + z * other.z;
283 }
284
285 /** Normalizes this quaternion, and returns the previous length. */
286 float normalize()
287 {
288 float len = w*w + x*x + y*y + z*z;
289 float factor = 1.0f / Math::sqrt(len);
290 *this = *this * factor;
291 return len;
292 }
293
294 /**
295 * Gets the inverse.
296 *
297 * @note Quaternion must be non-zero.
298 */
299 Quaternion inverse() const;
300
301 /** Rotates the provided vector. */
302 Vector3 rotate(const Vector3& vec) const;
303
304 /**
305 * Orients the quaternion so its negative z axis points to the provided direction.
306 *
307 * @param[in] forwardDir Direction to orient towards.
308 */
309 void lookRotation(const Vector3& forwardDir);
310
311 /**
312 * Orients the quaternion so its negative z axis points to the provided direction.
313 *
314 * @param[in] forwardDir Direction to orient towards.
315 * @param[in] upDir Constrains y axis orientation to a plane this vector lies on. This rule might be broken
316 * if forward and up direction are nearly parallel.
317 */
318 void lookRotation(const Vector3& forwardDir, const Vector3& upDir);
319
320 /** Query if any of the components of the quaternion are not a number. */
321 bool isNaN() const
322 {
323 return Math::isNaN(x) || Math::isNaN(y) || Math::isNaN(z) || Math::isNaN(w);
324 }
325
326 /** Calculates the dot product between two quaternions. */
327 static float dot(const Quaternion& lhs, const Quaternion& rhs)
328 {
329 return lhs.w * rhs.w + lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z;
330 }
331
332 /** Normalizes the provided quaternion. */
333 static Quaternion normalize(const Quaternion& q)
334 {
335 float len = dot(q, q);
336 float factor = 1.0f / Math::sqrt(len);
337
338 return q * factor;
339 }
340
341 /**
342 * Performs spherical interpolation between two quaternions. Spherical interpolation neatly interpolates between
343 * two rotations without modifying the size of the vector it is applied to (unlike linear interpolation).
344 */
345 static Quaternion slerp(float t, const Quaternion& p, const Quaternion& q, bool shortestPath = true);
346
347 /**
348 * Linearly interpolates between the two quaternions using @p t. t should be in [0, 1] range, where t = 0
349 * corresponds to the left vector, while t = 1 corresponds to the right vector.
350 */
351 static Quaternion lerp(float t, const Quaternion& a, const Quaternion& b)
352 {
353 float d = dot(a, b);
354 float flip = d >= 0.0f ? 1.0f : -1.0f;
355
356 Quaternion output = flip * (1.0f - t) * a + t * b;
357 return normalize(output);
358 }
359
360 /** Gets the shortest arc quaternion to rotate this vector to the destination vector. */
361 static Quaternion getRotationFromTo(const Vector3& from, const Vector3& dest, const Vector3& fallbackAxis = Vector3::ZERO);
362
363 /** Returns the minimum of all the quaternion components as a new quaternion. */
364 static Quaternion min(const Quaternion& a, const Quaternion& b)
365 {
366 return Quaternion(std::min(a.x, b.x), std::min(a.y, b.y), std::min(a.z, b.z), std::min(a.w, b.w));
367 }
368
369 /** Returns the maximum of all the quaternion components as a new quaternion. */
370 static Quaternion max(const Quaternion& a, const Quaternion& b)
371 {
372 return Quaternion(std::max(a.x, b.x), std::max(a.y, b.y), std::max(a.z, b.z), std::max(a.w, b.w));
373 }
374
375 static constexpr const float EPSILON = 1e-03f;
376
377 static const Quaternion ZERO;
378 static const Quaternion IDENTITY;
379
380 float x, y, z, w; // Note: Order is relevant, don't break it
381 };
382
383 /** @} */
384
385 /** @cond SPECIALIZATIONS */
386 BS_ALLOW_MEMCPY_SERIALIZATION(Quaternion);
387 /** @endcond */
388}
389
390/** @cond SPECIALIZATIONS */
391namespace std
392{
393 template<> class numeric_limits<bs::Quaternion>
394 {
395 public:
396 constexpr static bs::Quaternion infinity()
397 {
398 return bs::Quaternion(
399 std::numeric_limits<float>::infinity(),
400 std::numeric_limits<float>::infinity(),
401 std::numeric_limits<float>::infinity(),
402 std::numeric_limits<float>::infinity());
403 }
404 };
405}
406/** @endcond */
407