Gamp v0.0.7-36-g24b1eb6
Gamp: Graphics, Audio, Multimedia and Processing
Loading...
Searching...
No Matches
quaternion.hpp
Go to the documentation of this file.
1/*
2 * Author: Sven Gothel <sgothel@jausoft.com>
3 * Copyright Gothel Software e.K.
4 *
5 * SPDX-License-Identifier: MIT
6 *
7 * This Source Code Form is subject to the terms of the MIT License
8 * If a copy of the MIT was not distributed with this file,
9 * you can obtain one at https://opensource.org/license/mit/.
10 */
11
12#ifndef JAU_MATH_QUATERNION_HPP_
13#define JAU_MATH_QUATERNION_HPP_
14
15#include <cmath>
16#include <cstdarg>
17#include <limits>
18#include <string>
19#include <iostream>
20
21#include <jau/float_math.hpp>
22#include <jau/math/vec3f.hpp>
23#include <jau/math/mat4f.hpp>
24
25namespace jau::math {
26
27 /** \addtogroup Math
28 *
29 * @{
30 */
31
32/**
33 * Quaternion implementation supporting
34 * <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q34">Gimbal-Lock</a> free rotations.
35 * <p>
36 * All matrix operation provided are in column-major order,
37 * as specified in the OpenGL fixed function pipeline, i.e. compatibility profile.
38 * See {@link FloatUtil}.
39 * </p>
40 * <p>
41 * See <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html">Matrix-FAQ</a>
42 * </p>
43 * <p>
44 * See <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/index.htm">euclideanspace.com-Quaternion</a>
45 * </p>
46 */
47template<typename Value_type,
48 std::enable_if_t<std::is_floating_point_v<Value_type>, bool> = true >
49class alignas(Value_type) Quaternion {
50 public:
53 typedef const value_type* const_pointer;
57 typedef const value_type* const_iterator;
58
61
62 constexpr static const value_type zero = value_type(0);
63 constexpr static const value_type one = value_type(1);
64 constexpr static const value_type two = value_type(2);
65 constexpr static const value_type half = one/two;
66
67 /**
68 * Quaternion Epsilon, used with equals method to determine if two Quaternions are close enough to be considered equal.
69 * <p>
70 * Using {@value}, which is ~8.4 times `std::numeric_limits<value_type>::epsilon()`.
71 * </p>
72 */
73 constexpr static const value_type allowed_deviation = value_type(8.4) * std::numeric_limits<value_type>::epsilon(); // 8.4 * EPSILON(1.1920929E-7f) = 1.0E-6f; double deviation: 1.0E-8f
74
75 private:
76 value_type m_x, m_y, m_z, m_w;
77
78 public:
79
80 constexpr Quaternion() noexcept
81 : m_x(0), m_y(0), m_z(0), m_w(1) {}
82
83 constexpr Quaternion(const value_type x, const value_type y, const value_type z, const value_type w) noexcept
84 : m_x(x), m_y(y), m_z(z), m_w(w) {}
85
86 constexpr Quaternion(const Quaternion& o) noexcept = default;
87 constexpr Quaternion(Quaternion&& o) noexcept = default;
88 constexpr Quaternion& operator=(const Quaternion&) noexcept = default;
89 constexpr Quaternion& operator=(Quaternion&&) noexcept = default;
90
91 /**
92 * See {@link #magnitude()} for special handling of {@link FloatUtil#EPSILON epsilon},
93 * which is not applied here.
94 * @return the squared magnitude of this quaternion.
95 */
96 constexpr value_type magnitudeSquared() const noexcept {
97 return m_w*m_w + m_x*m_x + m_y*m_y + m_z*m_z;
98 }
99
100 /**
101 * Return the magnitude of this quaternion, i.e. sqrt({@link #magnitudeSquared()})
102 * <p>
103 * A magnitude of zero shall equal {@link #isIdentity() identity},
104 * as performed by {@link #normalize()}.
105 * </p>
106 * <p>
107 * Implementation Details:
108 * <ul>
109 * <li> returns 0f if {@link #magnitudeSquared()} is {@link jau::is_zero(value_type, value_type) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
110 * <li> returns 1f if {@link #magnitudeSquared()} is {@link FloatUtil#isEqual(value_type, value_type, value_type) equals 1f} using {@link FloatUtil#EPSILON epsilon}</li>
111 * </ul>
112 * </p>
113 */
115 const value_type magnitudeSQ = magnitudeSquared();
116 if ( jau::is_zero(magnitudeSQ) ) {
117 return zero;
118 }
119 if ( jau::equals(one, magnitudeSQ) ) {
120 return one;
121 }
122 return std::sqrt(magnitudeSQ);
123 }
124
125 constexpr value_type w() const noexcept { return m_w; }
126
127 constexpr void setW(value_type w) noexcept { m_w = w; }
128
129 constexpr value_type x() const noexcept { return m_x; }
130
131 constexpr void setX(value_type x) noexcept { m_x = x; }
132
133 constexpr value_type y() const noexcept { return m_y; }
134
135 constexpr void setY(value_type y) noexcept { m_y = y; }
136
137 constexpr value_type z() const noexcept { return m_z; }
138
139 constexpr void setZ(value_type z) noexcept { m_z = z; }
140
141 /**
142 * Returns the dot product of this quaternion with the given x,y,z and m_w components.
143 */
144 constexpr value_type dot(value_type x, value_type y, value_type z, value_type w) const noexcept {
145 return m_x * x + m_y * y + m_z * z + m_w * w;
146 }
147
148 /**
149 * Returns the dot product of this quaternion with the given quaternion
150 */
151 constexpr value_type dot(const Quaternion& quat) const noexcept {
152 return dot(quat.x(), quat.y(), quat.z(), quat.w());
153 }
154
155 /**
156 * Returns <code>true</code> if this quaternion has identity.
157 * <p>
158 * Implementation uses epsilon to compare
159 * {@link #w() W} {@link jau::equals() against 1f} and
160 * {@link #x() X}, {@link #y() Y} and {@link #z() Z}
161 * {@link jau::is_zero3f() against zero}.
162 * </p>
163 */
164 constexpr bool isIdentity() const noexcept {
165 return jau::equals(one, m_w) && jau::is_zero3f(m_x, m_y, m_z);
166 // return m_w == 1f && m_x == 0f && m_y == 0f && m_z == 0f;
167 }
168
169 /***
170 * Set this quaternion to identity (x=0,y=0,z=0,w=1)
171 * @return this quaternion for chaining.
172 */
173 constexpr Quaternion& setIdentity() noexcept {
174 m_x = m_y = m_z = zero; m_w = one;
175 return *this;
176 }
177
178 /**
179 * Normalize a quaternion required if to be used as a rotational quaternion.
180 * <p>
181 * Implementation Details:
182 * <ul>
183 * <li> {@link #setIdentity()} if {@link #magnitude()} is {@link jau::isZero() is zero} using epsilon</li>
184 * </ul>
185 * </p>
186 * @return this quaternion for chaining.
187 */
188 constexpr Quaternion& normalize() noexcept {
189 const value_type norm = magnitude();
190 if ( jau::is_zero(norm) ) {
191 setIdentity();
192 } else {
193 const value_type invNorm = one/norm;
194 m_w *= invNorm;
195 m_x *= invNorm;
196 m_y *= invNorm;
197 m_z *= invNorm;
198 }
199 return *this;
200 }
201
202 /**
203 * Conjugates this quaternion <code>[-x, -y, -z, w]</code>.
204 * @return this quaternion for chaining.
205 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q49">Matrix-FAQ Q49</a>
206 */
207 constexpr Quaternion& conjugate() noexcept {
208 m_x = -m_x;
209 m_y = -m_y;
210 m_z = -m_z;
211 return *this;
212 }
213
214 /**
215 * Invert the quaternion If rotational, will produce a the inverse rotation
216 * <p>
217 * Implementation Details:
218 * <ul>
219 * <li> {@link #conjugate() conjugates} if {@link #magnitudeSquared()} is is {@link FloatUtil#isEqual(value_type, value_type, value_type) equals 1f} using {@link FloatUtil#EPSILON epsilon}</li>
220 * </ul>
221 * </p>
222 * @return this quaternion for chaining.
223 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q50">Matrix-FAQ Q50</a>
224 */
225 constexpr Quaternion& invert() noexcept {
226 const value_type magnitudeSQ = magnitudeSquared();
227 if ( jau::equals(one, magnitudeSQ) ) {
228 conjugate();
229 } else {
230 const value_type invmsq = one/magnitudeSQ;
231 m_w *= invmsq;
232 m_x = -m_x * invmsq;
233 m_y = -m_y * invmsq;
234 m_z = -m_z * invmsq;
235 }
236 return *this;
237 }
238
239 /**
240 * Set all values of this quaternion using the given components.
241 * @return this quaternion for chaining.
242 */
243 constexpr Quaternion& set(const value_type x, const value_type y, const value_type z, const value_type w) noexcept {
244 m_x = x;
245 m_y = y;
246 m_z = z;
247 m_w = w;
248 return *this;
249 }
250
251 /**
252 * Add a quaternion: this = this + rhs, returns this
253 *
254 * @param q quaternion
255 * @return this quaternion for chaining.
256 * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#add">euclideanspace.com-QuaternionAdd</a>
257 */
258 constexpr Quaternion& operator+=(const Quaternion& rhs ) noexcept {
259 m_x += rhs.m_x;
260 m_y += rhs.m_y;
261 m_z += rhs.m_z;
262 m_w += rhs.m_w;
263 return *this;
264 }
265
266 /**
267 * Subtract a quaternion: this = this - rhs, returns this
268 *
269 * @param q quaternion
270 * @return this quaternion for chaining.
271 * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#add">euclideanspace.com-QuaternionAdd</a>
272 */
273 constexpr Quaternion& operator-=(const Quaternion& rhs) noexcept {
274 m_x -= rhs.m_x;
275 m_y -= rhs.m_y;
276 m_z -= rhs.m_z;
277 m_w -= rhs.m_w;
278 return *this;
279 }
280
281 /**
282 * Multiply this quaternion: this = this * rhs, returns this
283 *
284 * @param q a quaternion to multiply with
285 * @return this quaternion for chaining.
286 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q53">Matrix-FAQ Q53</a>
287 * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#mul">euclideanspace.com-QuaternionMul</a>
288 */
289 constexpr Quaternion& operator*=(const Quaternion& rhs) noexcept {
290 return set( m_w * rhs.m_x + m_x * rhs.m_w + m_y * rhs.m_z - m_z * rhs.m_y,
291 m_w * rhs.m_y - m_x * rhs.m_z + m_y * rhs.m_w + m_z * rhs.m_x,
292 m_w * rhs.m_z + m_x * rhs.m_y - m_y * rhs.m_x + m_z * rhs.m_w,
293 m_w * rhs.m_w - m_x * rhs.m_x - m_y * rhs.m_y - m_z * rhs.m_z );
294 }
295
296 /**
297 * Scale this quaternion by a scalar: this = this * rhs, returns this
298 *
299 * @param n a value_type constant
300 * @return this quaternion for chaining.
301 * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm#scale">euclideanspace.com-QuaternionScale</a>
302 */
303 constexpr Quaternion& operator*=(const value_type rhs) noexcept {
304 m_x *= rhs;
305 m_y *= rhs;
306 m_z *= rhs;
307 m_w *= rhs;
308 return *this;
309 }
310
311 /**
312 * Rotate this quaternion by the given angle and axis.
313 * <p>
314 * The axis must be a normalized vector.
315 * </p>
316 * <p>
317 * A rotational quaternion is made from the given angle and axis.
318 * </p>
319 *
320 * @param angle in radians
321 * @param axisX x-coord of rotation axis
322 * @param axisY y-coord of rotation axis
323 * @param axisZ m_z-coord of rotation axis
324 * @return this quaternion for chaining.
325 */
326 constexpr_cxx26 Quaternion& rotateByAngleNormalAxis(const value_type angle, const value_type axisX, const value_type axisY, const value_type axisZ) noexcept {
327 if( jau::is_zero3f(axisX, axisY, axisZ) ) {
328 // no change
329 return *this;
330 }
331 const value_type halfAngle = half * angle;
332 const value_type sin = std::sin(halfAngle);
333 const value_type qw = std::cos(halfAngle);
334 const value_type qx = sin * axisX;
335 const value_type qy = sin * axisY;
336 const value_type qz = sin * axisZ;
337 return set( m_x * qw + m_y * qz - m_z * qy + m_w * qx,
338 -m_x * qz + m_y * qw + m_z * qx + m_w * qy,
339 m_x * qy - m_y * qx + m_z * qw + m_w * qz,
340 -m_x * qx - m_y * qy - m_z * qz + m_w * qw);
341 }
342
343 /**
344 * Rotate this quaternion by the given angle and axis.
345 * <p>
346 * The axis must be a normalized vector.
347 * </p>
348 * <p>
349 * A rotational quaternion is made from the given angle and axis.
350 * </p>
351 *
352 * @param angle in radians
353 * @param axis Vec3 coord of rotation axis
354 * @return this quaternion for chaining.
355 */
357 return rotateByAngleNormalAxis(angle, axis.x, axis.y, axis.z);
358 }
359
360 /**
361 * Rotate this quaternion around X axis with the given angle in radians
362 *
363 * @param angle in radians
364 * @return this quaternion for chaining.
365 */
367 const value_type halfAngle = half * angle;
368 return rotateByAngleX(std::sin(halfAngle), std::cos(halfAngle));
369 }
370 /** Rotate this quaternion around X axis with the given angle's sin + cos values */
371 constexpr Quaternion& rotateByAngleX(const value_type sin, const value_type cos) noexcept {
372 return set( m_x * cos + m_w * sin,
373 m_y * cos + m_z * sin,
374 -m_y * sin + m_z * cos,
375 -m_x * sin + m_w * cos);
376 }
377
378 /**
379 * Rotate this quaternion around Y axis with the given angle in radians
380 *
381 * @param angle in radians
382 * @return this quaternion for chaining.
383 */
385 const value_type halfAngle = half * angle;
386 return rotateByAngleY(std::sin(halfAngle), std::cos(halfAngle));
387 }
388 /** Rotate this quaternion around Y axis with the given angle's sin + cos values */
389 constexpr Quaternion& rotateByAngleY(const value_type sin, const value_type cos) noexcept {
390 return set( m_x * cos - m_z * sin,
391 m_y * cos + m_w * sin,
392 m_x * sin + m_z * cos,
393 -m_y * sin + m_w * cos);
394 }
395
396 /**
397 * Rotate this quaternion around Z axis with the given angle in radians
398 *
399 * @param angle in radians
400 * @return this quaternion for chaining.
401 */
403 const value_type halfAngle = half * angle;
404 return rotateByAngleZ(std::sin(halfAngle), std::cos(halfAngle));
405 }
406 /** Rotate this quaternion around Y axis with the given angle's sin + cos values */
407 constexpr Quaternion& rotateByAngleZ(const value_type sin, const value_type cos) noexcept {
408 return set( m_x * cos + m_y * sin,
409 -m_x * sin + m_y * cos,
410 m_z * cos + m_w * sin,
411 -m_z * sin + m_w * cos);
412 }
413
414 /**
415 * Rotates this quaternion from the given Euler rotation array <code>angradXYZ</code> in radians.
416 * <p>
417 * The <code>angradXYZ</code> array is laid out in natural order:
418 * <ul>
419 * <li>x - bank</li>
420 * <li>y - heading</li>
421 * <li>z - attitude</li>
422 * </ul>
423 * </p>
424 * For details see {@link #rotateByEuler(value_type, value_type, value_type)}.
425 * @param angradXYZ euler angle array in radians
426 * @return this quaternion for chaining.
427 * @see #rotateByEuler(value_type, value_type, value_type)
428 */
429 Quaternion& rotateByEuler(const Vec3& angradXYZ) noexcept {
430 return rotateByEuler(angradXYZ.x, angradXYZ.y, angradXYZ.z);
431 }
432
433 /**
434 * Rotates this quaternion from the given Euler rotation angles in radians.
435 * <p>
436 * The rotations are applied in the given order and using chained rotation per axis:
437 * <ul>
438 * <li>y - heading - {@link #rotateByAngleY(value_type)}</li>
439 * <li>z - attitude - {@link #rotateByAngleZ(value_type)}</li>
440 * <li>x - bank - {@link #rotateByAngleX(value_type)}</li>
441 * </ul>
442 * </p>
443 * <p>
444 * Implementation Details:
445 * <ul>
446 * <li> NOP if all angles are {@link jau::is_zero(value_type, value_type) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
447 * <li> result is {@link #normalize()}ed</li>
448 * </ul>
449 * </p>
450 * @param bankX the Euler pitch angle in radians. (rotation about the X axis)
451 * @param headingY the Euler yaw angle in radians. (rotation about the Y axis)
452 * @param attitudeZ the Euler roll angle in radians. (rotation about the Z axis)
453 * @return this quaternion for chaining.
454 * @see #rotateByAngleY(value_type)
455 * @see #rotateByAngleZ(value_type)
456 * @see #rotateByAngleX(value_type)
457 * @see #setFromEuler(value_type, value_type, value_type)
458 */
459 constexpr_cxx26 Quaternion& rotateByEuler(const value_type bankX, const value_type headingY, const value_type attitudeZ) noexcept {
460 if ( jau::is_zero3f(bankX, headingY, attitudeZ) ) {
461 return *this;
462 } else {
463 // setFromEuler muls: ( 8 + 4 ) , + quat muls 24 = 36
464 // this: 8 + 8 + 8 + 4 = 28 muls
465 return rotateByAngleY(headingY).rotateByAngleZ(attitudeZ).rotateByAngleX(bankX).normalize();
466 }
467 }
468
469 /***
470 * Rotate the given vector by this quaternion
471 * @param in vector to be rotated
472 *
473 * @return new Vec3 result
474 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q63">Matrix-FAQ Q63</a>
475 */
476 Vec3 rotateVector(const Vec3& in) noexcept {
477 Vec3 out;
478 rotateVector(in, out);
479 return out;
480 }
481
482 /***
483 * Rotate the given vector by this quaternion
484 * @param in vector to be rotated
485 * @param vecOut result storage for rotated vector, maybe equal to vecIn for in-place rotation
486 * @return the given out store for chaining
487 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q63">Matrix-FAQ Q63</a>
488 */
489 Vec3& rotateVector(const Vec3& in, Vec3& out) noexcept {
490 if( in.is_zero() ) {
491 out.set(0, 0, 0);
492 } else {
493 const value_type vecX = in.x;
494 const value_type vecY = in.y;
495 const value_type vecZ = in.z;
496 const value_type x_x = m_x*m_x;
497 const value_type y_y = m_y*m_y;
498 const value_type z_z = m_z*m_z;
499 const value_type w_w = m_w*m_w;
500
501 out.x = w_w * vecX
502 + x_x * vecX
503 - z_z * vecX
504 - y_y * vecX
505 + two * ( m_y*m_w*vecZ - m_z*m_w*vecY + m_y*m_x*vecY + m_z*m_x*vecZ );
506 ;
507
508 out.y = y_y * vecY
509 - z_z * vecY
510 + w_w * vecY
511 - x_x * vecY
512 + two * ( m_x*m_y*vecX + m_z*m_y*vecZ + m_w*m_z*vecX - m_x*m_w*vecZ );
513
514 out.z = z_z * vecZ
515 - y_y * vecZ
516 - x_x * vecZ
517 + w_w * vecZ
518 + two * ( m_x*m_z*vecX + m_y*m_z*vecY - m_w*m_y*vecX + m_w*m_x*vecY );
519 }
520 return out;
521 }
522
523 /**
524 * Set this quaternion to a spherical linear interpolation
525 * between the given start and end quaternions by the given change amount.
526 * <p>
527 * Note: Method <i>does not</i> normalize this quaternion!
528 * </p>
529 *
530 * @param a start quaternion
531 * @param b end quaternion
532 * @param changeAmnt value_type between 0 and 1 representing interpolation.
533 * @return this quaternion for chaining.
534 * @see <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/">euclideanspace.com-QuaternionSlerp</a>
535 */
536 constexpr_cxx26 Quaternion& setSlerp(const Quaternion& a, const Quaternion& b, const value_type changeAmnt) noexcept {
537 // std::cerr << "Slerp.0: A " << a << ", B " << b << ", t " << changeAmnt << std::endl;
538 if (changeAmnt == zero) {
539 *this = a;
540 } else if (changeAmnt == one) {
541 *this = b;
542 } else {
543 value_type bx = b.m_x;
544 value_type by = b.m_y;
545 value_type bz = b.m_z;
546 value_type bw = b.m_w;
547
548 // Calculate angle between them (quat dot product)
549 value_type cosHalfTheta = a.m_x * bx + a.m_y * by + a.m_z * bz + a.m_w * bw;
550
551 value_type scale0, scale1;
552
553 if( cosHalfTheta >= 0.95f ) {
554 // quaternions are close, just use linear interpolation
555 scale0 = one - changeAmnt;
556 scale1 = changeAmnt;
557 } else if ( cosHalfTheta <= -0.99f ) {
558 // the quaternions are nearly opposite,
559 // we can pick any axis normal to a,b to do the rotation
560 scale0 = half;
561 scale1 = half;
562 } else {
563 if( cosHalfTheta <= -std::numeric_limits<value_type>::epsilon() ) { // FIXME: .. or shall we use the upper bound 'cosHalfTheta < EPSILON' ?
564 // Negate the second quaternion and the result of the dot product (Inversion)
565 bx *= -one;
566 by *= -one;
567 bz *= -one;
568 bw *= -one;
569 cosHalfTheta *= -one;
570 }
571 const value_type halfTheta = std::acos(cosHalfTheta);
572 const value_type sinHalfTheta = std::sqrt(one - cosHalfTheta*cosHalfTheta);
573 // if theta = 180 degrees then result is not fully defined
574 // we could rotate around any axis normal to qa or qb
575 if ( std::abs(sinHalfTheta) < 0.001f ){ // fabs is floating point absolute
576 scale0 = half;
577 scale1 = half;
578 // throw new InternalError("XXX"); // FIXME should not be reached due to above inversion ?
579 } else {
580 // Calculate the scale for q1 and q2, according to the angle and
581 // it's sine value
582 scale0 = std::sin((one - changeAmnt) * halfTheta) / sinHalfTheta;
583 scale1 = std::sin(changeAmnt * halfTheta) / sinHalfTheta;
584 }
585 }
586
587 m_x = a.m_x * scale0 + bx * scale1;
588 m_y = a.m_y * scale0 + by * scale1;
589 m_z = a.m_z * scale0 + bz * scale1;
590 m_w = a.m_w * scale0 + bw * scale1;
591 }
592 return *this;
593 }
594
595 /**
596 * Set this quaternion to equal the rotation required
597 * to point the z-axis at <i>direction</i> and the y-axis to <i>up</i>.
598 * <p>
599 * Implementation generates a 3x3 matrix
600 * and is equal with ProjectFloat's lookAt(..).<br/>
601 * </p>
602 * Implementation Details:
603 * <ul>
604 * <li> result is {@link #normalize()}ed</li>
605 * </ul>
606 * </p>
607 * @param directionIn where to <i>look</i> at
608 * @param upIn a vector indicating the local <i>up</i> direction.
609 * @param xAxisOut vector storing the <i>orthogonal</i> x-axis of the coordinate system.
610 * @param yAxisOut vector storing the <i>orthogonal</i> y-axis of the coordinate system.
611 * @param zAxisOut vector storing the <i>orthogonal</i> m_z-axis of the coordinate system.
612 * @return this quaternion for chaining.
613 * @see <a href="http://www.euclideanspace.com/maths/algebra/vectors/lookat/index.htm">euclideanspace.com-LookUp</a>
614 */
615 Quaternion& setLookAt(const Vec3& directionIn, const Vec3& upIn,
616 Vec3& xAxisOut, Vec3& yAxisOut, Vec3& zAxisOut) noexcept {
617 // Z = norm(dir)
618 (zAxisOut = directionIn).normalize();
619
620 // X = upIn m_x Z
621 // (borrow yAxisOut for upNorm)
622 (yAxisOut = upIn).normalize();
623 xAxisOut.cross(yAxisOut, zAxisOut).normalize();
624
625 // Y = Z m_x X
626 //
627 yAxisOut.cross(zAxisOut, xAxisOut).normalize();
628
629 /**
630 const value_type m00 = xAxisOut[0];
631 const value_type m01 = yAxisOut[0];
632 const value_type m02 = zAxisOut[0];
633 const value_type m10 = xAxisOut[1];
634 const value_type m11 = yAxisOut[1];
635 const value_type m12 = zAxisOut[1];
636 const value_type m20 = xAxisOut[2];
637 const value_type m21 = yAxisOut[2];
638 const value_type m22 = zAxisOut[2];
639 */
640 return setFromAxes(xAxisOut, yAxisOut, zAxisOut).normalize();
641 }
642
643 //
644 // Conversions
645 //
646
647 /**
648 * Initialize this quaternion from two vectors
649 * <pre>
650 * q = (s,v) = (v1•v2 , v1 × v2),
651 * angle = angle(v1, v2) = v1•v2
652 * axis = normal(v1 x v2)
653 * </pre>
654 * <p>
655 * Implementation Details:
656 * <ul>
657 * <li> set_identity() if square vector-length is jau::is_zero2f(value_type, value_type) using epsilon</li>
658 * </ul>
659 * </p>
660 * @param v1 not normalized
661 * @param v2 not normalized
662 * @return this quaternion for chaining.
663 */
664 constexpr_cxx26 Quaternion& setFromVectors(const Vec3& v1, const Vec3& v2) noexcept {
665 const value_type factor = v1.length() * v2.length();
666 if ( jau::is_zero(factor) ) {
667 return setIdentity();
668 } else {
669 const value_type dot = v1.dot(v2) / factor; // normalize
670 const value_type theta = std::acos(std::max(-one, std::min(dot, one))); // clipping [-1..1]
671
672 Vec3 tmpPivotVec = v1.cross(v2);
673
674 if ( dot < zero && jau::is_zero( tmpPivotVec.length() ) ) {
675 // Vectors parallel and opposite direction, therefore a rotation of 180 degrees about any vector
676 // perpendicular to this vector will rotate vector a onto vector b.
677 //
678 // The following guarantees the dot-product will be 0.0.
679 int dominantIndex;
680 if (std::abs(v1.x) > std::abs(v1.y)) {
681 if (std::abs(v1.x) > std::abs(v1.z)) {
682 dominantIndex = 0;
683 } else {
684 dominantIndex = 2;
685 }
686 } else {
687 if (std::abs(v1.y) > std::abs(v1.z)) {
688 dominantIndex = 1;
689 } else {
690 dominantIndex = 2;
691 }
692 }
693 tmpPivotVec[dominantIndex] = -v1[ (dominantIndex + 1) % 3 ];
694 tmpPivotVec[(dominantIndex + 1) % 3] = v1[ dominantIndex ];
695 tmpPivotVec[(dominantIndex + 2) % 3] = zero;
696 }
697 return setFromAngleAxis(theta, tmpPivotVec);
698 }
699 }
700
701 /**
702 * Initialize this quaternion from two normalized vectors
703 * <pre>
704 * q = (s,v) = (v1•v2 , v1 × v2),
705 * angle = angle(v1, v2) = v1•v2
706 * axis = v1 x v2
707 * </pre>
708 * <p>
709 * Implementation Details:
710 * <ul>
711 * <li> {@link #setIdentity()} if square vector-length is {@link jau::is_zero(value_type, value_type) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
712 * </ul>
713 * </p>
714 * @param v1 normalized
715 * @param v2 normalized
716 * @return this quaternion for chaining.
717 */
718 constexpr_cxx26 Quaternion& setFromNormalVectors(const Vec3& v1, const Vec3& v2) noexcept {
719 const value_type factor = v1.length() * v2.length();
720 if ( jau::is_zero(factor) ) {
721 return setIdentity();
722 } else {
723 const value_type dot = v1.dot(v2) / factor; // normalize
724 const value_type theta = std::acos(std::max(-one, std::min(dot, one))); // clipping [-1..1]
725
726 Vec3 tmpPivotVec = v1.cross(v2);
727
728 if ( dot < zero && jau::is_zero( tmpPivotVec.length() ) ) {
729 // Vectors parallel and opposite direction, therefore a rotation of 180 degrees about any vector
730 // perpendicular to this vector will rotate vector a onto vector b.
731 //
732 // The following guarantees the dot-product will be 0.0.
733 int dominantIndex;
734 if (std::abs(v1.x) > std::abs(v1.y)) {
735 if (std::abs(v1.x) > std::abs(v1.z)) {
736 dominantIndex = 0;
737 } else {
738 dominantIndex = 2;
739 }
740 } else {
741 if (std::abs(v1.y) > std::abs(v1.z)) {
742 dominantIndex = 1;
743 } else {
744 dominantIndex = 2;
745 }
746 }
747 tmpPivotVec[dominantIndex] = -v1[ (dominantIndex + 1) % 3 ];
748 tmpPivotVec[(dominantIndex + 1) % 3] = v1[ dominantIndex ];
749 tmpPivotVec[(dominantIndex + 2) % 3] = zero;
750 }
751 return setFromAngleNormalAxis(theta, tmpPivotVec);
752 }
753 }
754
755 /***
756 * Initialize this quaternion with given non-normalized axis vector and rotation angle
757 * <p>
758 * Implementation Details:
759 * <ul>
760 * <li> {@link #setIdentity()} if axis is {@link jau::is_zero(value_type, value_type) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
761 * </ul>
762 * </p>
763 * @param angle rotation angle (rads)
764 * @param vector axis vector not normalized
765 * @return this quaternion for chaining.
766 *
767 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56">Matrix-FAQ Q56</a>
768 * @see #toAngleAxis(Vec3)
769 */
770 Quaternion& setFromAngleAxis(const value_type angle, const Vec3& vector) noexcept {
771 return setFromAngleNormalAxis(angle, Vec3(vector).normalize());
772 }
773
774 /***
775 * Initialize this quaternion with given normalized axis vector and rotation angle
776 * <p>
777 * Implementation Details:
778 * <ul>
779 * <li> {@link #setIdentity()} if axis is {@link jau::is_zero(value_type, value_type) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
780 * </ul>
781 * </p>
782 * @param angle rotation angle (rads)
783 * @param vector axis vector normalized
784 * @return this quaternion for chaining.
785 *
786 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56">Matrix-FAQ Q56</a>
787 * @see #toAngleAxis(Vec3)
788 */
789 constexpr_cxx26 Quaternion& setFromAngleNormalAxis(const value_type angle, const Vec3& vector) noexcept {
790 if( vector.is_zero() ) {
791 setIdentity();
792 } else {
793 const value_type halfangle = angle * half;
794 const value_type sin = std::sin(halfangle);
795 m_x = vector.x * sin;
796 m_y = vector.y * sin;
797 m_z = vector.z * sin;
798 m_w = std::cos(halfangle);
799 }
800 return *this;
801 }
802
803 /**
804 * Transform the rotational quaternion to axis based rotation angles
805 *
806 * @param axis storage for computed axis
807 * @return the rotation angle in radians
808 * @see #setFromAngleAxis(value_type, Vec3, Vec3)
809 */
811 const value_type sqrLength = m_x*m_x + m_y*m_y + m_z*m_z;
812 value_type angle;
813 if ( jau::is_zero(sqrLength) ) { // length is ~0
814 angle = zero;
815 axis.set( one, zero, zero );
816 } else {
817 angle = std::acos(m_w) * two;
818 const value_type invLength = one / std::sqrt(sqrLength);
819 axis.set( m_x * invLength,
820 m_y * invLength,
821 m_z * invLength );
822 }
823 return angle;
824 }
825
826 /**
827 * Initializes this quaternion from the given Euler rotation array <code>angradXYZ</code> in radians.
828 * <p>
829 * The <code>angradXYZ</code> vector is laid out in natural order:
830 * <ul>
831 * <li>x - bank</li>
832 * <li>y - heading</li>
833 * <li>z - attitude</li>
834 * </ul>
835 * </p>
836 * For details see {@link #setFromEuler(value_type, value_type, value_type)}.
837 * @param angradXYZ euler angle vector in radians holding x-bank, m_y-heading and m_z-attitude
838 * @return this quaternion for chaining.
839 * @see #setFromEuler(value_type, value_type, value_type)
840 */
841 constexpr_cxx26 Quaternion& setFromEuler(const Vec3& angradXYZ) noexcept {
842 return setFromEuler(angradXYZ.x, angradXYZ.y, angradXYZ.z);
843 }
844
845 /**
846 * Initializes this quaternion from the given Euler rotation angles in radians.
847 * <p>
848 * The rotations are applied in the given order:
849 * <ul>
850 * <li>y - heading</li>
851 * <li>z - attitude</li>
852 * <li>x - bank</li>
853 * </ul>
854 * </p>
855 * <p>
856 * Implementation Details:
857 * <ul>
858 * <li> {@link #setIdentity()} if all angles are {@link jau::is_zero(value_type, value_type) is zero} using {@link FloatUtil#EPSILON epsilon}</li>
859 * <li> result is {@link #normalize()}ed</li>
860 * </ul>
861 * </p>
862 * @param bankX the Euler pitch angle in radians. (rotation about the X axis)
863 * @param headingY the Euler yaw angle in radians. (rotation about the Y axis)
864 * @param attitudeZ the Euler roll angle in radians. (rotation about the Z axis)
865 * @return this quaternion for chaining.
866 *
867 * @see <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60">Matrix-FAQ Q60</a>
868 * @see <a href="http://vered.rose.utoronto.ca/people/david_dir/GEMS/GEMS.html">Gems</a>
869 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm">euclideanspace.com-eulerToQuaternion</a>
870 * @see #toEuler(Vec3)
871 */
872 constexpr_cxx26 Quaternion& setFromEuler(const value_type bankX, const value_type headingY, const value_type attitudeZ) noexcept {
873 if ( jau::is_zero3f(bankX, headingY, attitudeZ) ) {
874 return setIdentity();
875 } else {
876 value_type angle = headingY * half;
877 const value_type sinHeadingY = std::sin(angle);
878 const value_type cosHeadingY = std::cos(angle);
879 angle = attitudeZ * half;
880 const value_type sinAttitudeZ = std::sin(angle);
881 const value_type cosAttitudeZ = std::cos(angle);
882 angle = bankX * half;
883 const value_type sinBankX = std::sin(angle);
884 const value_type cosBankX = std::cos(angle);
885
886 // variables used to reduce multiplication calls.
887 const value_type cosHeadingXcosAttitude = cosHeadingY * cosAttitudeZ;
888 const value_type sinHeadingXsinAttitude = sinHeadingY * sinAttitudeZ;
889 const value_type cosHeadingXsinAttitude = cosHeadingY * sinAttitudeZ;
890 const value_type sinHeadingXcosAttitude = sinHeadingY * cosAttitudeZ;
891
892 m_w = cosHeadingXcosAttitude * cosBankX - sinHeadingXsinAttitude * sinBankX;
893 m_x = cosHeadingXcosAttitude * sinBankX + sinHeadingXsinAttitude * cosBankX;
894 m_y = sinHeadingXcosAttitude * cosBankX + cosHeadingXsinAttitude * sinBankX;
895 m_z = cosHeadingXsinAttitude * cosBankX - sinHeadingXcosAttitude * sinBankX;
896 return normalize();
897 }
898 }
899
900 /**
901 * Transform this quaternion to Euler rotation angles in radians (pitchX, yawY and rollZ).
902 * <p>
903 * The <code>result</code> array is laid out in natural order:
904 * <ul>
905 * <li>x - bank</li>
906 * <li>y - heading</li>
907 * <li>z - attitude</li>
908 * </ul>
909 * </p>
910 *
911 * @return new Vec3 euler angle result vector filled with x-bank, y-heading and m_z-attitude
912 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm">euclideanspace.com-quaternionToEuler</a>
913 * @see #setFromEuler(value_type, value_type, value_type)
914 */
916 const value_type sqw = m_w*m_w;
917 const value_type sqx = m_x*m_x;
918 const value_type sqy = m_y*m_y;
919 const value_type sqz = m_z*m_z;
920 const value_type unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise, is correction factor
921 const value_type test = m_x*m_y + m_z*m_w;
922
923 if (test > 0.499f * unit) { // singularity at north pole
924 return Vec3( zero, // m_x-bank
925 two * std::atan2(m_x, m_w), // y-heading
926 M_PI_2 ); // z-attitude
927 } else if (test < -0.499f * unit) { // singularity at south pole
928 return Vec3( zero, // m_x-bank
929 -two * std::atan2(m_x, m_w), // m_y-heading
930 -M_PI_2 ); // m_z-attitude
931 } else {
932 return Vec3( std::atan2(two * m_x * m_w - two * m_y * m_z, -sqx + sqy - sqz + sqw), // m_x-bank
933 std::atan2(two * m_y * m_w - two * m_x * m_z, sqx - sqy - sqz + sqw), // m_y-heading
934 std::asin( two * test / unit) ); // z-attitude
935 }
936 }
937
938 /**
939 * Compute the quaternion from a 3x3 column rotation matrix
940 * <p>
941 * See <a href="ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z">Graphics Gems Code</a>,<br/>
942 * <a href="http://mathworld.wolfram.com/MatrixTrace.html">MatrixTrace</a>.
943 * </p>
944 * <p>
945 * Buggy <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55">Matrix-FAQ Q55</a>
946 * </p>
947 *
948 * @return this quaternion for chaining.
949 * @see #setFromMatrix(Matrix4f)
950 */
951 constexpr Quaternion& setFromMat(const value_type m00, const value_type m01, const value_type m02,
952 const value_type m10, const value_type m11, const value_type m12,
953 const value_type m20, const value_type m21, const value_type m22) noexcept {
954 // Note: Other implementations uses 'T' w/o '+1f' and compares 'T >= 0' while adding missing 1f in sqrt expr.
955 // However .. this causes setLookAt(..) to fail and actually violates the 'trace definition'.
956
957 // The trace T is the sum of the diagonal elements; see
958 // http://mathworld.wolfram.com/MatrixTrace.html
959 const value_type T = m00 + m11 + m22 + one;
960 if ( T > zero ) {
961 const value_type S = half / std::sqrt(T); // S = 1 / ( 2 t )
962 m_w = 0.25f / S; // m_w = 1 / ( 4 S ) = t / 2
963 m_x = ( m21 - m12 ) * S;
964 m_y = ( m02 - m20 ) * S;
965 m_z = ( m10 - m01 ) * S;
966 } else if ( m00 > m11 && m00 > m22) {
967 const value_type S = half / std::sqrt(one + m00 - m11 - m22); // S=4*qx
968 m_w = ( m21 - m12 ) * S;
969 m_x = 0.25f / S;
970 m_y = ( m10 + m01 ) * S;
971 m_z = ( m02 + m20 ) * S;
972 } else if ( m11 > m22 ) {
973 const value_type S = half / std::sqrt(one + m11 - m00 - m22); // S=4*qy
974 m_w = ( m02 - m20 ) * S;
975 m_x = ( m20 + m01 ) * S;
976 m_y = 0.25f / S;
977 m_z = ( m21 + m12 ) * S;
978 } else {
979 const value_type S = half / std::sqrt(one + m22 - m00 - m11); // S=4*qz
980 m_w = ( m10 - m01 ) * S;
981 m_x = ( m02 + m20 ) * S;
982 m_y = ( m21 + m12 ) * S;
983 m_z = 0.25f / S;
984 }
985 return *this;
986 }
987
988 /**
989 * Compute the quaternion from a 3x3 column rotation matrix from Mat4 instance
990 * <p>
991 * See <a href="ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z">Graphics Gems Code</a>,<br/>
992 * <a href="http://mathworld.wolfram.com/MatrixTrace.html">MatrixTrace</a>.
993 * </p>
994 * <p>
995 * Buggy <a href="http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55">Matrix-FAQ Q55</a>
996 * </p>
997 *
998 * @return this quaternion for chaining.
999 * @see Matrix4f#getRotation(Quaternion)
1000 * @see #setFromMatrix(value_type, value_type, value_type, value_type, value_type, value_type, value_type, value_type, value_type)
1001 */
1002 constexpr Quaternion& setFromMat(const Mat4& m) noexcept {
1003 return setFromMat(m.m00, m.m01, m.m02, m.m10, m.m11, m.m12, m.m20, m.m21, m.m22);
1004 }
1005
1006 /**
1007 * Initializes this quaternion to represent a rotation formed by the given three <i>orthogonal</i> axes.
1008 * <p>
1009 * No validation whether the axes are <i>orthogonal</i> is performed.
1010 * </p>
1011 *
1012 * @param xAxis vector representing the <i>orthogonal</i> x-axis of the coordinate system.
1013 * @param yAxis vector representing the <i>orthogonal</i> y-axis of the coordinate system.
1014 * @param zAxis vector representing the <i>orthogonal</i> m_z-axis of the coordinate system.
1015 * @return this quaternion for chaining.
1016 */
1017 constexpr Quaternion& setFromAxes(const Vec3& xAxis, const Vec3& yAxis, const Vec3& zAxis) noexcept {
1018 return setFromMat(xAxis.x, yAxis.x, zAxis.x,
1019 xAxis.y, yAxis.y, zAxis.y,
1020 xAxis.z, yAxis.z, zAxis.z);
1021 }
1022
1023 /**
1024 * Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
1025 *
1026 * Implementation Details:
1027 * - makes identity matrix if {@link #magnitudeSquared()} is {@link jau::is_zero(value_type, value_type) is zero} using {@link FloatUtil#EPSILON epsilon}
1028 * - Mat4 fields [m00 .. m22] define the rotation
1029 *
1030 * @return resulting normalized column matrix 4x4
1031 * @see [Matrix-FAQ Q54](http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54)
1032 * @see #setFromMatrix(value_type, value_type, value_type, value_type, value_type, value_type, value_type, value_type, value_type)
1033 * @see Matrix4f#setToRotation(Quaternion)
1034 */
1035 constexpr Mat4 toMatrix() const noexcept {
1036 Mat4 m; toMatrix(m); return m;
1037 }
1038
1039 /**
1040 * Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
1041 *
1042 * Implementation Details:
1043 * - makes identity matrix if {@link #magnitudeSquared()} is {@link jau::is_zero(value_type, value_type) is zero} using {@link FloatUtil#EPSILON epsilon}
1044 * - Mat4 fields [m00 .. m22] define the rotation
1045 *
1046 * @param out store for the resulting normalized column matrix 4x4
1047 * @return the given matrix store
1048 * @see [Matrix-FAQ Q54](http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54)
1049 * @see #setFromMatrix(value_type, value_type, value_type, value_type, value_type, value_type, value_type, value_type, value_type)
1050 * @see Matrix4f#setToRotation(Quaternion)
1051 */
1052 constexpr Mat4& toMatrix(Mat4& m) const noexcept {
1053 // pre-multiply scaled-reciprocal-magnitude to reduce multiplications
1054 const value_type norm = magnitudeSquared();
1055 if ( jau::is_zero(norm) ) {
1056 // identity matrix -> srecip = 0f
1057 m.loadIdentity();
1058 return m;
1059 }
1060 value_type srecip;
1061 if ( jau::equals(one, norm) ) {
1062 srecip = two;
1063 } else {
1064 srecip = two / norm;
1065 }
1066 const value_type xs = srecip * m_x;
1067 const value_type ys = srecip * m_y;
1068 const value_type zs = srecip * m_z;
1069
1070 const value_type xx = m_x * xs;
1071 const value_type xy = m_x * ys;
1072 const value_type xz = m_x * zs;
1073 const value_type xw = xs * m_w;
1074 const value_type yy = m_y * ys;
1075 const value_type yz = m_y * zs;
1076 const value_type yw = ys * m_w;
1077 const value_type zz = m_z * zs;
1078 const value_type zw = zs * m_w;
1079
1080 m.m00 = one - ( yy + zz );
1081 m.m01 = ( xy - zw );
1082 m.m02 = ( xz + yw );
1083 m.m03 = zero;
1084
1085 m.m10 = ( xy + zw );
1086 m.m11 = one - ( xx + zz );
1087 m.m12 = ( yz - xw );
1088 m.m13 = zero;
1089
1090 m.m20 = ( xz - yw );
1091 m.m21 = ( yz + xw );
1092 m.m22 = one - ( xx + yy );
1093 m.m23 = zero;
1094
1095 m.m30 = m.m31 = m.m32 = zero;
1096 m.m33 = one;
1097 return m;
1098 }
1099
1100 /**
1101 * Extracts this quaternion's <i>orthogonal</i> rotation axes.
1102 *
1103 * @param xAxis vector representing the <i>orthogonal</i> x-axis of the coordinate system.
1104 * @param yAxis vector representing the <i>orthogonal</i> y-axis of the coordinate system.
1105 * @param zAxis vector representing the <i>orthogonal</i> m_z-axis of the coordinate system.
1106 * @param tmp temporary Matrix4 used for toMatrix()
1107 */
1108 constexpr void toAxes(Vec3& xAxis, Vec3& yAxis, Vec3& zAxis, Matrix4<value_type>& tmp) const noexcept {
1109 toMatrix(tmp);
1110 tmp.getColumn(2, zAxis);
1111 tmp.getColumn(1, yAxis);
1112 tmp.getColumn(0, xAxis);
1113 }
1114
1115 /**
1116 * Extracts this quaternion's <i>orthogonal</i> rotation axes.
1117 *
1118 * @param xAxis vector representing the <i>orthogonal</i> x-axis of the coordinate system.
1119 * @param yAxis vector representing the <i>orthogonal</i> y-axis of the coordinate system.
1120 * @param zAxis vector representing the <i>orthogonal</i> m_z-axis of the coordinate system.
1121 */
1122 constexpr void toAxes(Vec3& xAxis, Vec3& yAxis, Vec3& zAxis) const noexcept {
1124 toAxes(xAxis, yAxis, zAxis, tmp);
1125 }
1126
1127 //
1128 // std overrides
1129 //
1130
1131 /**
1132 * @param o the object to compare for equality
1133 * @return true if this quaternion and the provided quaternion have roughly the same x, m_y, m_z and m_w values.
1134 */
1135 constexpr bool operator==(const Quaternion& o) const noexcept {
1136 if (this == &o) {
1137 return true;
1138 }
1139 return std::abs(m_x - o.m_x) <= allowed_deviation &&
1140 std::abs(m_y - o.m_y) <= allowed_deviation &&
1141 std::abs(m_z - o.m_z) <= allowed_deviation &&
1142 std::abs(m_w - o.m_w) <= allowed_deviation;
1143 }
1144
1145 std::string toString() const noexcept {
1146 return "Quat[x "+std::to_string(m_x)+", y "+std::to_string(m_y)+", z "+std::to_string(m_z)+", w "+std::to_string(m_w)+"]";
1147 }
1148};
1149
1150template<typename T,
1151 std::enable_if_t<std::is_floating_point_v<T>, bool> = true>
1152constexpr Quaternion<T> operator+(const Quaternion<T>& lhs, const Quaternion<T>& rhs ) noexcept {
1153 Quaternion<T> r(lhs); r += rhs; return r;
1154}
1155
1156template<typename T,
1157 std::enable_if_t<std::is_floating_point_v<T>, bool> = true>
1158constexpr Quaternion<T> operator-(const Quaternion<T>& lhs, const Quaternion<T>& rhs ) noexcept {
1159 Quaternion<T> r(lhs); r -= rhs; return r;
1160}
1161
1162template<typename T,
1163 std::enable_if_t<std::is_floating_point_v<T>, bool> = true>
1164constexpr Quaternion<T> operator*(const Quaternion<T>& lhs, const Quaternion<T>& rhs ) noexcept {
1165 Quaternion<T> r(lhs); r *= rhs; return r;
1166}
1167
1168template<typename T,
1169 std::enable_if_t<std::is_floating_point_v<T>, bool> = true>
1170constexpr Quaternion<T> operator*(const Quaternion<T>& lhs, const T s ) noexcept {
1171 Quaternion<T> r(lhs); r *= s; return r;
1172}
1173
1174template<typename T,
1175 std::enable_if_t<std::is_floating_point_v<T>, bool> = true>
1176constexpr Quaternion<T> operator*(const T s, const Quaternion<T>& rhs) noexcept {
1177 Quaternion<T> r(rhs); r *= s; return r;
1178}
1179
1180template<typename T,
1181 std::enable_if_t<std::is_floating_point_v<T>, bool> = true>
1182std::ostream& operator<<(std::ostream& out, const Quaternion<T>& v) noexcept {
1183 return out << v.toString();
1184}
1185
1187static_assert(alignof(float) == alignof(Quat4f));
1188
1189
1190template<typename Value_type,
1191 typename std::enable_if_t<std::is_floating_point_v<Value_type>, bool> sf_type>
1196 m0.setToRotation(r);
1197 r.toMatrix(m0);
1198
1199 q.toMatrix(*this);
1200 return *this;
1201}
1202
1203
1204template<typename Value_type,
1205 typename std::enable_if_t<std::is_floating_point_v<Value_type>, bool> sf_type>
1208 return res.setFromMat(m00, m01, m02, m10, m11, m12, m20, m21, m22);
1209}
1210
1211template<typename Value_type,
1212 typename std::enable_if_t<std::is_floating_point_v<Value_type>, bool> sf_type>
1216 return mul( quat.toMatrix(tmp) );
1217}
1218
1219/**@}*/
1220
1221} // namespace jau::math
1222
1223#endif // JAU_MATH_QUATERNION_HPP_
Basic 4x4 value_type matrix implementation using fields for intensive use-cases (host operations).
Definition mat4f.hpp:101
Quaternion< value_type, std::is_floating_point_v< Value_type > > Quat
Definition mat4f.hpp:114
constexpr_cxx26 Matrix4 & rotate(const value_type ang_rad, const value_type x, const value_type y, const value_type z) noexcept
Rotate this matrix about give axis and angle in radians, i.e.
Definition mat4f.hpp:1414
constexpr Matrix4() noexcept
Creates a new identity matrix.
Definition mat4f.hpp:144
Quaternion implementation supporting Gimbal-Lock free rotations.
constexpr_cxx26 Quaternion & rotateByAngleX(const value_type angle) noexcept
Rotate this quaternion around X axis with the given angle in radians.
constexpr Quaternion & operator+=(const Quaternion &rhs) noexcept
Add a quaternion: this = this + rhs, returns this.
Vec3 rotateVector(const Vec3 &in) noexcept
constexpr Quaternion & normalize() noexcept
Normalize a quaternion required if to be used as a rotational quaternion.
constexpr Quaternion(const value_type x, const value_type y, const value_type z, const value_type w) noexcept
constexpr Quaternion & set(const value_type x, const value_type y, const value_type z, const value_type w) noexcept
Set all values of this quaternion using the given components.
constexpr Quaternion(Quaternion &&o) noexcept=default
constexpr Mat4 toMatrix() const noexcept
Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
constexpr Quaternion & conjugate() noexcept
Conjugates this quaternion [-x, -y, -z, w].
constexpr void toAxes(Vec3 &xAxis, Vec3 &yAxis, Vec3 &zAxis, Matrix4< value_type > &tmp) const noexcept
Extracts this quaternion's orthogonal rotation axes.
constexpr_cxx26 Quaternion & setFromEuler(const Vec3 &angradXYZ) noexcept
Initializes this quaternion from the given Euler rotation array angradXYZ in radians.
constexpr Quaternion & operator-=(const Quaternion &rhs) noexcept
Subtract a quaternion: this = this - rhs, returns this.
constexpr Quaternion & operator*=(const value_type rhs) noexcept
Scale this quaternion by a scalar: this = this * rhs, returns this.
constexpr_cxx26 Quaternion & setFromNormalVectors(const Vec3 &v1, const Vec3 &v2) noexcept
Initialize this quaternion from two normalized vectors.
constexpr_cxx26 Quaternion & rotateByAngleNormalAxis(const value_type angle, const value_type axisX, const value_type axisY, const value_type axisZ) noexcept
Rotate this quaternion by the given angle and axis.
constexpr bool isIdentity() const noexcept
Returns true if this quaternion has identity.
constexpr value_type dot(value_type x, value_type y, value_type z, value_type w) const noexcept
Returns the dot product of this quaternion with the given x,y,z and m_w components.
std::string toString() const noexcept
constexpr_cxx26 Quaternion & rotateByEuler(const value_type bankX, const value_type headingY, const value_type attitudeZ) noexcept
Rotates this quaternion from the given Euler rotation angles in radians.
constexpr Quaternion & invert() noexcept
Invert the quaternion If rotational, will produce a the inverse rotation.
constexpr_cxx26 Vec3 toEuler() noexcept
Transform this quaternion to Euler rotation angles in radians (pitchX, yawY and rollZ).
constexpr Quaternion & setFromMat(const Mat4 &m) noexcept
Compute the quaternion from a 3x3 column rotation matrix from Mat4 instance.
constexpr_cxx26 value_type magnitude() const noexcept
Return the magnitude of this quaternion, i.e.
constexpr Mat4 & toMatrix(Mat4 &m) const noexcept
Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
constexpr void setY(value_type y) noexcept
Vec3 & rotateVector(const Vec3 &in, Vec3 &out) noexcept
constexpr Quaternion() noexcept
constexpr Quaternion & setIdentity() noexcept
Quaternion & setLookAt(const Vec3 &directionIn, const Vec3 &upIn, Vec3 &xAxisOut, Vec3 &yAxisOut, Vec3 &zAxisOut) noexcept
Set this quaternion to equal the rotation required to point the z-axis at direction and the y-axis to...
constexpr_cxx26 Quaternion & setFromAngleNormalAxis(const value_type angle, const Vec3 &vector) noexcept
constexpr value_type dot(const Quaternion &quat) const noexcept
Returns the dot product of this quaternion with the given quaternion.
Quaternion & setFromAngleAxis(const value_type angle, const Vec3 &vector) noexcept
constexpr Quaternion & rotateByAngleY(const value_type sin, const value_type cos) noexcept
Rotate this quaternion around Y axis with the given angle's sin + cos values.
constexpr Quaternion & operator=(Quaternion &&) noexcept=default
constexpr Quaternion & operator*=(const Quaternion &rhs) noexcept
Multiply this quaternion: this = this * rhs, returns this.
Quaternion & rotateByEuler(const Vec3 &angradXYZ) noexcept
Rotates this quaternion from the given Euler rotation array angradXYZ in radians.
constexpr Quaternion & operator=(const Quaternion &) noexcept=default
constexpr Quaternion & setFromAxes(const Vec3 &xAxis, const Vec3 &yAxis, const Vec3 &zAxis) noexcept
constexpr_cxx26 value_type toAngleAxis(Vec3 &axis) const noexcept
Transform the rotational quaternion to axis based rotation angles.
constexpr bool operator==(const Quaternion &o) const noexcept
constexpr void setX(value_type x) noexcept
constexpr_cxx26 Quaternion & setFromVectors(const Vec3 &v1, const Vec3 &v2) noexcept
Initialize this quaternion from two vectors.
constexpr Quaternion & setFromMat(const value_type m00, const value_type m01, const value_type m02, const value_type m10, const value_type m11, const value_type m12, const value_type m20, const value_type m21, const value_type m22) noexcept
Compute the quaternion from a 3x3 column rotation matrix.
constexpr_cxx26 Quaternion & setSlerp(const Quaternion &a, const Quaternion &b, const value_type changeAmnt) noexcept
Set this quaternion to a spherical linear interpolation between the given start and end quaternions b...
constexpr Quaternion & rotateByAngleZ(const value_type sin, const value_type cos) noexcept
Rotate this quaternion around Y axis with the given angle's sin + cos values.
constexpr void setZ(value_type z) noexcept
constexpr Quaternion(const Quaternion &o) noexcept=default
constexpr_cxx26 Quaternion & rotateByAngleNormalAxis(const value_type angle, const Vec3 &axis) noexcept
Rotate this quaternion by the given angle and axis.
constexpr_cxx26 Quaternion & rotateByAngleZ(value_type angle) noexcept
Rotate this quaternion around Z axis with the given angle in radians.
constexpr Quaternion & rotateByAngleX(const value_type sin, const value_type cos) noexcept
Rotate this quaternion around X axis with the given angle's sin + cos values.
constexpr void toAxes(Vec3 &xAxis, Vec3 &yAxis, Vec3 &zAxis) const noexcept
Extracts this quaternion's orthogonal rotation axes.
constexpr_cxx26 Quaternion & rotateByAngleY(value_type angle) noexcept
Rotate this quaternion around Y axis with the given angle in radians.
constexpr void setW(value_type w) noexcept
constexpr_cxx26 Quaternion & setFromEuler(const value_type bankX, const value_type headingY, const value_type attitudeZ) noexcept
Initializes this quaternion from the given Euler rotation angles in radians.
3D vector using three value_type components.
Definition vec3f.hpp:45
constexpr value_type length() const noexcept
Return the length of a vector, a.k.a the norm or magnitude
Definition vec3f.hpp:255
#define constexpr_cxx26
std::enable_if_t< std::is_floating_point_v< T >, bool > constexpr equals(const T &a, const T &b, const T &epsilon=std::numeric_limits< T >::epsilon()) noexcept
Returns true if both values are equal, i.e.
std::enable_if_t< std::is_floating_point_v< T >, bool > constexpr is_zero(const T &a, const T &epsilon=std::numeric_limits< T >::epsilon()) noexcept
Returns true if the given value is less than epsilon, w/ epsilon > 0.
std::enable_if_t< std::is_floating_point_v< T >, bool > constexpr is_zero3f(const T &a, const T &b, const T &c, const T &epsilon=std::numeric_limits< T >::epsilon()) noexcept
Returns true if all given values a, b and c are less than epsilon, w/ epsilon > 0.
constexpr Quaternion< T > operator-(const Quaternion< T > &lhs, const Quaternion< T > &rhs) noexcept
Quat & getRotation(Quat &res) const noexcept
Returns the rotation [m00 .
Quaternion< float > Quat4f
constexpr Matrix4< T > operator*(const Matrix4< T > &lhs, const Matrix4< T > &rhs) noexcept
Definition mat4f.hpp:1951
constexpr Quaternion< T > operator+(const Quaternion< T > &lhs, const Quaternion< T > &rhs) noexcept
Matrix4 & setToRotation(const Quat &q)
Set this matrix to rotation using the given Quaternion.
std::ostream & operator<<(std::ostream &out, const Addr48Bit &a)
uint8_t Value_type