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