Mathter
A configurable 3D math library for game developers.
QuaternionImpl.hpp
Go to the documentation of this file.
1 //==============================================================================
2 // This software is distributed under The Unlicense.
3 // For more information, please refer to <http://unlicense.org/>
4 //==============================================================================
5 
6 #pragma once
7 
8 
9 #include "../Common/MathUtil.hpp"
10 #include "../Common/Traits.hpp"
11 #include "../Matrix.hpp"
12 #include "../Utility.hpp"
13 #include "../Vector.hpp"
14 
15 #include <limits>
16 #include <cmath>
17 
18 
19 namespace mathter {
20 
21 
30 template <class T, bool Packed = false>
31 class Quaternion {
32 public:
33  static constexpr bool SimdAccelerated = traits::HasSimd<Vector<T, 4, Packed>>::value;
34  union {
35  struct {
36  T s, i, j, k;
37  };
38  struct {
39  T w, x, y, z;
40  };
42  };
43 
44  //-----------------------------------------------
45  // Constructors
46  //-----------------------------------------------
49 
50  Quaternion(const Quaternion& rhs) : vec(rhs.vec) {}
51 
53  Quaternion(T scalar, T x, T y, T z) : w(scalar), x(x), y(y), z(z) {}
54 
56  Quaternion(T scalar, const Vector<T, 3, true>& vector) : w(scalar), x(vector.x), y(vector.y), z(vector.z) {}
57 
59  Quaternion(T scalar, const Vector<T, 3, false>& vector) : w(scalar), x(vector.x), y(vector.y), z(vector.z) {}
60 
62  explicit Quaternion(const Vector<T, 3, true>& vector) : Quaternion(0, vector) {}
63 
64  template <class U, bool P>
65  Quaternion(const Quaternion<U, P>& rhs) : vec(rhs.vec) {}
66 
69  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool PackedA>
71  FromMatrix(rhs);
72  }
75  template <class U, eMatrixLayout Layout, bool PackedA>
77  FromMatrix(rhs);
78  }
81  template <class U, eMatrixLayout Layout, bool PackedA>
83  FromMatrix(rhs);
84  }
87  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool PackedA>
89  FromMatrix(rhs);
90  }
91 
92  explicit Quaternion(const Vector<T, 4, false>& vec) : vec(vec) {}
93 
94  //-----------------------------------------------
95  // Assignment
96  //-----------------------------------------------
98  vec = rhs.vec;
99  return *this;
100  }
101 
103  template <class U, bool P>
105  vec = rhs.vec;
106  return *this;
107  }
108 
109 
112  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool PackedA>
114  FromMatrix(rhs);
115  return *this;
116  }
119  template <class U, eMatrixLayout Layout, bool PackedA>
121  FromMatrix(rhs);
122  return *this;
123  }
126  template <class U, eMatrixLayout Layout, bool PackedA>
128  FromMatrix(rhs);
129  return *this;
130  }
133  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool PackedA>
135  FromMatrix(rhs);
136  return *this;
137  }
138 
139  //-----------------------------------------------
140  // Functions
141  //-----------------------------------------------
142 
144  const T ScalarPart() const {
145  return s;
146  }
149  return { x, y, z };
150  }
151 
154  const T Angle() const {
155  return sign_nonzero(s) * 2 * std::acos(std::clamp(abs(s) / Length(vec), T(-1), T(1)));
156  }
159  const Vector<T, 3, Packed> Axis() const {
160  auto direction = VectorPart();
161  return SafeNormalize(direction);
162  }
163 
164  //-----------------------------------------------
165  // Matrix conversions
166  //-----------------------------------------------
167 
169  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool PackedA>
170  explicit operator Matrix<U, 3, 3, Order, Layout, PackedA>() const {
171  return ToMatrix<U, 3, 3, Order, Layout, PackedA>();
172  }
174  template <class U, eMatrixLayout Layout, bool PackedA>
176  return ToMatrix<U, 3, 4, eMatrixOrder::PRECEDE_VECTOR, Layout, PackedA>();
177  }
179  template <class U, eMatrixLayout Layout, bool PackedA>
181  return ToMatrix<U, 4, 3, eMatrixOrder::FOLLOW_VECTOR, Layout, PackedA>();
182  }
184  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool PackedA>
185  explicit operator Matrix<U, 4, 4, Order, Layout, PackedA>() const {
186  return ToMatrix<U, 4, 4, Order, Layout, PackedA>();
187  }
188 
189  //-----------------------------------------------
190  // Truncate to vector
191  //-----------------------------------------------
192 
194  template <class U, bool PackedA>
195  explicit operator Vector<U, 3, PackedA>() const {
196  return { x, y, z };
197  }
198 
199 protected:
200  //-----------------------------------------------
201  // Matrix conversion helpers
202  //-----------------------------------------------
203  template <class U, int Rows, int Columns, eMatrixOrder Order, eMatrixLayout Layout, bool PackedA>
205  assert(IsNormalized(vec));
207  auto elem = [&mat](int i, int j) -> U& {
208  return Order == eMatrixOrder::PRECEDE_VECTOR ? mat(i, j) : mat(j, i);
209  };
210  elem(0, 0) = 1 - 2 * (j * j + k * k);
211  elem(0, 1) = 2 * (i * j - k * s);
212  elem(0, 2) = 2 * (i * k + j * s);
213  elem(1, 0) = 2 * (i * j + k * s);
214  elem(1, 1) = 1 - 2 * (i * i + k * k);
215  elem(1, 2) = 2 * (j * k - i * s);
216  elem(2, 0) = 2 * (i * k - j * s);
217  elem(2, 1) = 2 * (j * k + i * s);
218  elem(2, 2) = 1 - 2 * (i * i + j * j);
219 
220  // Rest
221  for (int j = 0; j < mat.Width(); ++j) {
222  for (int i = (j < 3 ? 3 : 0); i < mat.Height(); ++i) {
223  mat(i, j) = T(j == i);
224  }
225  }
226 
227  return mat;
228  }
229 
230  template <class U, int Rows, int Columns, eMatrixOrder Order, eMatrixLayout Layout, bool PackedA>
231  void FromMatrix(const Matrix<U, Rows, Columns, Order, Layout, PackedA>& mat) {
232  assert(IsRotationMatrix3D(mat));
233  auto elem = [&mat](int i, int j) -> U {
234  return Order == eMatrixOrder::PRECEDE_VECTOR ? mat(i, j) : mat(j, i);
235  };
236  w = std::sqrt(1 + elem(0, 0) + elem(1, 1) + elem(2, 2)) * T(0.5);
237  T div = T(1) / (T(4) * w);
238  x = (elem(2, 1) - elem(1, 2)) * div;
239  y = (elem(0, 2) - elem(2, 0)) * div;
240  z = (elem(1, 0) - elem(0, 1)) * div;
241  }
242 };
243 
244 
245 } // namespace mathter
Quaternion & operator=(const Matrix< U, 3, 4, eMatrixOrder::PRECEDE_VECTOR, Layout, PackedA > &rhs)
Convert a rotation matrix to equivalent quaternion.
Definition: QuaternionImpl.hpp:120
static constexpr bool SimdAccelerated
Definition: QuaternionImpl.hpp:33
bool IsRotationMatrix3D(const Matrix< T, Rows, Columns, Order, Layout, Packed > &m)
Determines if the matrix is a proper rotation matrix.
Definition: Rotation3DBuilder.hpp:340
Quaternion(const Vector< T, 4, false > &vec)
Definition: QuaternionImpl.hpp:92
Quaternion(const Quaternion &rhs)
Definition: QuaternionImpl.hpp:50
Allows you to do quaternion math and represent rotation in a compact way.
Definition: Definitions.hpp:69
T x
Definition: QuaternionImpl.hpp:39
Quaternion(const Quaternion< U, P > &rhs)
Definition: QuaternionImpl.hpp:65
Quaternion()
Does NOT zero-initialize values.
Definition: QuaternionImpl.hpp:48
Quaternion(const Matrix< U, 4, 4, Order, Layout, PackedA > &rhs)
Convert a rotation matrix to equivalent quaternion.
Definition: QuaternionImpl.hpp:88
T j
Definition: QuaternionImpl.hpp:36
Quaternion & operator=(const Matrix< U, 4, 4, Order, Layout, PackedA > &rhs)
Convert a rotation matrix to equivalent quaternion.
Definition: QuaternionImpl.hpp:134
T k
Definition: QuaternionImpl.hpp:36
T sign_nonzero(T arg)
Definition: MathUtil.hpp:11
Quaternion(const Matrix< U, 3, 4, eMatrixOrder::PRECEDE_VECTOR, Layout, PackedA > &rhs)
Convert a rotation matrix to equivalent quaternion.
Definition: QuaternionImpl.hpp:76
Quaternion(T scalar, const Vector< T, 3, false > &vector)
Sets the scalar part (w) and the vector part (xyz). This is not AxisAngle rotation.
Definition: QuaternionImpl.hpp:59
const T ScalarPart() const
Returns the scalar part (w) of (w + xi + yj + zk).
Definition: QuaternionImpl.hpp:144
Definition: Approx.hpp:11
const Vector< T, 3, Packed > VectorPart() const
Returns the vector part (x, y, z) of (w + xi + yj + zk).
Definition: QuaternionImpl.hpp:148
Quaternion(const Matrix< U, 4, 3, eMatrixOrder::FOLLOW_VECTOR, Layout, PackedA > &rhs)
Convert a rotation matrix to equivalent quaternion.
Definition: QuaternionImpl.hpp:82
Quaternion & operator=(const Matrix< U, 3, 3, Order, Layout, PackedA > &rhs)
Convert a rotation matrix to equivalent quaternion.
Definition: QuaternionImpl.hpp:113
Quaternion & operator=(const Quaternion &rhs)
Definition: QuaternionImpl.hpp:97
constexpr int Height() const
Returns the number of rows of the matrix.
Definition: MatrixImpl.hpp:39
T z
Definition: QuaternionImpl.hpp:39
const T Angle() const
Returns the angle of the rotation represented by quaternion.
Definition: QuaternionImpl.hpp:154
Definition: Definitions.hpp:63
constexpr int Width() const
Returns the number of columns of the matrix.
Definition: MatrixImpl.hpp:35
Quaternion & operator=(const Quaternion< U, P > &rhs)
Convert from quaternion with different base type and packing.
Definition: QuaternionImpl.hpp:104
Quaternion & operator=(const Matrix< U, 4, 3, eMatrixOrder::FOLLOW_VECTOR, Layout, PackedA > &rhs)
Convert a rotation matrix to equivalent quaternion.
Definition: QuaternionImpl.hpp:127
const Vector< T, 3, Packed > Axis() const
Returns the axis of rotation represented by quaternion.
Definition: QuaternionImpl.hpp:159
Quaternion(const Vector< T, 3, true > &vector)
Sets the scalar part to zero, and the vector part to given argument.
Definition: QuaternionImpl.hpp:62
T i
Definition: QuaternionImpl.hpp:36
Quaternion(const Matrix< U, 3, 3, Order, Layout, PackedA > &rhs)
Convert a rotation matrix to equivalent quaternion.
Definition: QuaternionImpl.hpp:70
Vector< T, 4, Packed > vec
Definition: QuaternionImpl.hpp:41
Quaternion(T scalar, const Vector< T, 3, true > &vector)
Sets the scalar part (w) and the vector part (xyz). This is not AxisAngle rotation.
Definition: QuaternionImpl.hpp:56
T y
Definition: QuaternionImpl.hpp:39
Definition: Traits.hpp:218
T Length(const Quaternion< T, Packed > &q)
Returns the absolute value of the quaternion.
Definition: QuaternionFunction.hpp:60
T w
Definition: QuaternionImpl.hpp:39
Quaternion(T scalar, T x, T y, T z)
Set values directly.
Definition: QuaternionImpl.hpp:53
T s
Definition: QuaternionImpl.hpp:36
Vector< T, Dim, Packed > SafeNormalize(const Vector< T, Dim, Packed > &v)
Makes a unit vector, but keeps direction. Leans towards (1,0,0...) for nullvectors, costs more.
Definition: VectorFunction.hpp:76
bool IsNormalized(const Quaternion< T, Packed > &q)
Check if the quaternion is a unit quaternion, with some tolerance for floats.
Definition: QuaternionFunction.hpp:78