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