1 #pragma once
2 
3 /// \file Quat.h
4 /// \brief Quaternion algebra
5 /// \author Pavel Sevecek (sevecek at sirrah.troja.mff.cuni.cz)
6 /// \date 2016-2021
7 
8 #include "math/AffineMatrix.h"
9 
10 NAMESPACE_SPH_BEGIN
11 
12 /// \brief Quaternion representing an axis of rotation and a (half of) rotation angle
13 ///
14 /// Convenient holder of any rotation, as it contains only 4 components and can only represent a rotation,
15 /// compared to matrix with 9 components, representing any linear transformation.
16 class Quat {
17 private:
18     Vector v;
19 
20 public:
21     Quat() = default;
22 
23     /// \brief Creates a quaternion given rotation axis and angle of rotation.
24     ///
25     /// Axis does not have to be normalized.
Quat(const Vector & axis,const Float angle)26     Quat(const Vector& axis, const Float angle) {
27         SPH_ASSERT(getSqrLength(axis) > 0._f);
28         Vector normAxis;
29         Float length;
30         tieToTuple(normAxis, length) = getNormalizedWithLength(axis);
31 
32         const Float s = sin(0.5_f * angle);
33         const Float c = cos(0.5_f * angle);
34 
35         v = normAxis * s;
36         v[3] = c;
37     }
38 
39     /// \brief Creates a quaternion given a rotation matrix
40     ///
41     /// The matrix must be orthogonal with det(matrix) = 1
Quat(const AffineMatrix & m)42     explicit Quat(const AffineMatrix& m) {
43         SPH_ASSERT(m.translation() == Vector(0._f));
44         SPH_ASSERT(m.isOrthogonal());
45         const Float w = 0.5_f * sqrt(1._f + m(0, 0) + m(1, 1) + m(2, 2));
46         const Float n = 0.25_f / w;
47         v[X] = (m(2, 1) - m(1, 2)) * n;
48         v[Y] = (m(0, 2) - m(2, 0)) * n;
49         v[Z] = (m(1, 0) - m(0, 1)) * n;
50         v[H] = w;
51     }
52 
53     /// \brief Returns the normalized rotational axis.
axis()54     INLINE Vector axis() const {
55         SPH_ASSERT(v[H] != 1._f);
56         return v / sqrt(1._f - sqr(v[H]));
57     }
58 
59     /// \brief Returns the angle of rotation (in radians).
angle()60     INLINE Float angle() const {
61         return acos(v[H]) * 2._f;
62     }
63 
64     INLINE Float& operator[](const Size idx) {
65         return v[idx];
66     }
67 
68     INLINE Float operator[](const Size idx) const {
69         return v[idx];
70     }
71 
72     /// \brief Converts the quaternion into a rotation matrix.
convert()73     AffineMatrix convert() const {
74         const Float n = getSqrLength(v) + sqr(v[3]);
75         const Vector s = v * (n > 0._f ? 2._f / n : 0._f);
76         const Vector w = s * v[3];
77 
78         const Float xx = v[X] * s[X];
79         const Float xy = v[X] * s[Y];
80         const Float xz = v[X] * s[Z];
81         const Float yy = v[Y] * s[Y];
82         const Float yz = v[Y] * s[Z];
83         const Float zz = v[Z] * s[Z];
84 
85         return AffineMatrix( //
86             Vector(1._f - yy - zz, xy - w[Z], xz + w[Y]),
87             Vector(xy + w[Z], 1._f - xx - zz, yz - w[X]),
88             Vector(xz - w[Y], yz + w[X], 1._f - xx - yy));
89     }
90 };
91 
92 NAMESPACE_SPH_END
93