Mathter
A configurable 3D math library for game developers.
Rotation3DBuilder.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include "../Matrix/MatrixFunction.hpp"
4 #include "../Matrix/MatrixImpl.hpp"
5 #include "../Quaternion/QuaternionImpl.hpp"
6 #include "../Vector.hpp"
7 #include "IdentityBuilder.hpp"
8 #include <stdexcept>
9 
10 
11 namespace mathter {
12 
13 
14 template <class T>
16 public:
17  Rotation3DAxisBuilder(const T& angle, int axis) : angle(angle), axis(axis) {}
19 
20  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool MPacked>
23  Set(m);
24  return m;
25  }
26 
27  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool MPacked>
30  Set(m);
31  return m;
32  }
33 
34  template <class U, eMatrixLayout Layout, bool MPacked>
37  Set(m);
38  return m;
39  }
40 
41  template <class U, eMatrixLayout Layout, bool MPacked>
44  Set(m);
45  return m;
46  }
47 
48  template <class U, bool QPacked>
49  operator Quaternion<U, QPacked>() const;
50 
51 private:
52  template <class U, int Rows, int Columns, eMatrixOrder Order, eMatrixLayout Layout, bool MPacked>
54  T C = cos(angle);
55  T S = sin(angle);
56 
57  auto elem = [&m](int i, int j) -> T& {
58  return Order == eMatrixOrder::FOLLOW_VECTOR ? m(i, j) : m(j, i);
59  };
60 
61  assert(0 <= axis && axis < 3);
62 
63  // Indices according to follow vector order
64  if (axis == 0) {
65  // Rotate around X
66  elem(0, 0) = 1;
67  elem(0, 1) = 0;
68  elem(0, 2) = 0;
69  elem(1, 0) = 0;
70  elem(1, 1) = C;
71  elem(1, 2) = S;
72  elem(2, 0) = 0;
73  elem(2, 1) = -S;
74  elem(2, 2) = C;
75  }
76  else if (axis == 1) {
77  // Rotate around Y
78  elem(0, 0) = C;
79  elem(0, 1) = 0;
80  elem(0, 2) = -S;
81  elem(1, 0) = 0;
82  elem(1, 1) = 1;
83  elem(1, 2) = 0;
84  elem(2, 0) = S;
85  elem(2, 1) = 0;
86  elem(2, 2) = C;
87  }
88  else {
89  // Rotate around Z
90  elem(0, 0) = C;
91  elem(0, 1) = S;
92  elem(0, 2) = 0;
93  elem(1, 0) = -S;
94  elem(1, 1) = C;
95  elem(1, 2) = 0;
96  elem(2, 0) = 0;
97  elem(2, 1) = 0;
98  elem(2, 2) = 1;
99  }
100 
101  // Rest
102  for (int j = 0; j < m.ColumnCount(); ++j) {
103  for (int i = (j < 3 ? 3 : 0); i < m.RowCount(); ++i) {
104  m(i, j) = T(j == i);
105  }
106  }
107  }
108 
109  const float angle;
110  const int axis;
111 };
112 
113 
114 
120 template <class T>
121 auto RotationAxis(T angle, int axis) {
122  return Rotation3DAxisBuilder(angle, axis);
123 }
124 
130 template <int Axis, class T>
131 auto RotationAxis(T angle) {
132  return Rotation3DAxisBuilder(angle, Axis);
133 }
134 
135 
138 template <class T>
139 auto RotationX(T angle) {
140  return RotationAxis<0>(angle);
141 }
142 
145 template <class T>
146 auto RotationY(T angle) {
147  return RotationAxis<1>(angle);
148 }
149 
152 template <class T>
153 auto RotationZ(T angle) {
154  return RotationAxis<2>(angle);
155 }
156 
157 
158 
159 template <class T>
161 public:
162  Rotation3DTriAxisBuilder(const std::array<T, 3>& angles, std::array<int, 3> axes) : angles(angles), axes(axes) {}
164 
165  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool MPacked>
168  Set(m);
169  return m;
170  }
171 
172  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool MPacked>
175  Set(m);
176  return m;
177  }
178 
179  template <class U, eMatrixLayout Layout, bool MPacked>
182  Set(m);
183  return m;
184  }
185 
186  template <class U, eMatrixLayout Layout, bool MPacked>
189  Set(m);
190  return m;
191  }
192 
193  template <class U, bool QPacked>
194  operator Quaternion<U, QPacked>() const;
195 
196 private:
197  template <class U, int Rows, int Columns, eMatrixOrder Order, eMatrixLayout Layout, bool MPacked>
200  if constexpr (Order == eMatrixOrder::FOLLOW_VECTOR) {
201  m.template Submatrix<3, 3>(0, 0) = MatT(RotationAxis(angles[0], axes[0])) * MatT(RotationAxis(angles[1], axes[1])) * MatT(RotationAxis(angles[2], axes[2]));
202  }
203  else {
204  m.template Submatrix<3, 3>(0, 0) = MatT(RotationAxis(angles[2], axes[2])) * MatT(RotationAxis(angles[1], axes[1])) * MatT(RotationAxis(angles[0], axes[0]));
205  }
206 
207  // Rest
208  for (int j = 0; j < m.ColumnCount(); ++j) {
209  for (int i = (j < 3 ? 3 : 0); i < m.RowCount(); ++i) {
210  m(i, j) = T(j == i);
211  }
212  }
213  }
214 
215  const std::array<T, 3> angles;
216  const std::array<int, 3> axes;
217 };
218 
219 
220 
224 template <int FirstAxis, int SecondAxis, int ThirdAxis, class T>
225 auto RotationAxis3(T angle0, T angle1, T angle2) {
226  return Rotation3DTriAxisBuilder(std::array<T, 3>{ angle0, angle1, angle2 }, std::array<int, 3>{ FirstAxis, SecondAxis, ThirdAxis });
227 }
228 
233 template <class T>
234 auto RotationEuler(T z1, T x2, T z3) {
235  return RotationAxis3<2, 0, 2>(z1, x2, z3);
236 }
237 
242 template <class T>
243 auto RotationRPY(T x1, T y2, T z3) {
244  return RotationAxis3<0, 1, 2>(x1, y2, z3);
245 }
246 
247 
248 
249 template <class T, bool Packed>
251 public:
252  Rotation3DAxisAngleBuilder(const Vector<T, 3, Packed>& axis, T angle) : axis(axis), angle(angle) {}
254 
255  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool MPacked>
258  Set(m);
259  return m;
260  }
261 
262  template <class U, eMatrixOrder Order, eMatrixLayout Layout, bool MPacked>
265  Set(m);
266  return m;
267  }
268 
269  template <class U, eMatrixLayout Layout, bool MPacked>
272  Set(m);
273  return m;
274  }
275 
276  template <class U, eMatrixLayout Layout, bool MPacked>
279  Set(m);
280  return m;
281  }
282 
283  template <class U, bool QPacked>
284  operator Quaternion<U, QPacked>() const;
285 
286 private:
287  template <class U, int Rows, int Columns, eMatrixOrder Order, eMatrixLayout Layout, bool MPacked>
289  assert(IsNormalized(axis));
290 
291  T C = cos(angle);
292  T S = sin(angle);
293 
294  // 3x3 rotation sub-matrix
297  RotMat cross = {
298  0, -u(2), u(1),
299  u(2), 0, -u(0),
300  -u(1), u(0), 0
301  };
302  RotMat rot = C * RotMat(Identity()) + S * cross + (1 - C) * (u * Transpose(u));
303 
304 
305  // Elements
306  auto elem = [&m](int i, int j) -> T& {
307  return Order == eMatrixOrder::PRECEDE_VECTOR ? m(i, j) : m(j, i);
308  };
309  for (int j = 0; j < 3; ++j) {
310  for (int i = 0; i < 3; ++i) {
311  elem(i, j) = rot(i, j);
312  }
313  }
314 
315  // Rest
316  for (int j = 0; j < m.Width(); ++j) {
317  for (int i = (j < 3 ? 3 : 0); i < m.Height(); ++i) {
318  m(i, j) = T(j == i);
319  }
320  }
321  }
322 
323  const Vector<T, 3, Packed> axis;
324  const T angle;
325 };
326 
331 template <class T, bool Vpacked, class U>
332 auto RotationAxisAngle(const Vector<T, 3, Vpacked>& axis, U angle) {
333  return Rotation3DAxisAngleBuilder(axis, T(angle));
334 }
335 
336 
339 template <class T, int Rows, int Columns, eMatrixOrder Order, eMatrixLayout Layout, bool Packed>
341  static_assert(Rows == 3 || Rows == 4);
342  static_assert(Columns == 3 || Columns == 4);
343  Vector<T, 3> rows[3] = {
344  { m(0, 0), m(0, 1), m(0, 2) },
345  { m(1, 0), m(1, 1), m(1, 2) },
346  { m(2, 0), m(2, 1), m(2, 2) },
347  };
348  return (std::abs(Dot(rows[0], rows[1])) + std::abs(Dot(rows[0], rows[2])) + std::abs(Dot(rows[1], rows[2]))) < T(0.0005) // rows are orthogonal to each other
349  && IsNormalized(rows[0]) && IsNormalized(rows[1]) && IsNormalized(rows[2]) // all rows are normalized
350  && Determinant(Matrix<T, 3, 3, Order, Layout, Packed>(m.template Submatrix<3, 3>(0, 0))) > 0; // not an improper rotation
351 }
352 
353 
354 template <class T>
355 template <class U, bool QPacked>
357  using QuatT = Quaternion<U, QPacked>;
358  switch (axis) {
359  case 0: return QuatT(RotationAxisAngle(Vector<U, 3, QPacked>(1, 0, 0), angle));
360  case 1: return QuatT(RotationAxisAngle(Vector<U, 3, QPacked>(0, 1, 0), angle));
361  case 2: return QuatT(RotationAxisAngle(Vector<U, 3, QPacked>(0, 0, 1), angle));
362  }
363  assert(false);
364  throw std::invalid_argument("Axis must be 0, 1, or 2.");
365 }
366 
367 template <class T>
368 template <class U, bool QPacked>
370  using QuatT = Quaternion<U, QPacked>;
371  return QuatT(RotationAxis(angles[2], axes[2])) * QuatT(RotationAxis(angles[1], axes[1])) * QuatT(RotationAxis(angles[0], axes[0]));
372 }
373 
374 template <class T, bool Packed>
375 template <class U, bool QPacked>
377  auto halfAngle = U(angle) * U(0.5);
378  return Quaternion(std::cos(halfAngle), Vector<U, 3, QPacked>(axis) * std::sin(halfAngle));
379 }
380 
381 
382 
383 } // namespace mathter
auto Identity()
Creates an identity matrix or identity quaternion.
Definition: IdentityBuilder.hpp:42
bool IsRotationMatrix3D(const Matrix< T, Rows, Columns, Order, Layout, Packed > &m)
Determines if the matrix is a proper rotation matrix.
Definition: Rotation3DBuilder.hpp:340
Allows you to do quaternion math and represent rotation in a compact way.
Definition: Definitions.hpp:69
Matrix< T, Columns, Rows, Order, Layout, Packed > Transpose(const Matrix< T, Rows, Columns, Order, Layout, Packed > &m)
Transposes the matrix in-place.
Definition: MatrixFunction.hpp:34
Rotation3DAxisBuilder(const T &angle, int axis)
Definition: Rotation3DBuilder.hpp:17
Rotation3DTriAxisBuilder(const std::array< T, 3 > &angles, std::array< int, 3 > axes)
Definition: Rotation3DBuilder.hpp:162
auto RotationAxisAngle(const Vector< T, 3, Vpacked > &axis, U angle)
Rotates around an arbitrary axis.
Definition: Rotation3DBuilder.hpp:332
auto RotationY(T angle)
Rotates around the Y axis according to the right (left) hand rule.
Definition: Rotation3DBuilder.hpp:146
T Dot(const Vector< T, Dim, Packed > &lhs, const Vector< T, Dim, Packed > &rhs)
Calculates the scalar product (dot product) of the two arguments.
Definition: VectorFunction.hpp:111
Definition: Rotation3DBuilder.hpp:15
auto RotationRPY(T x1, T y2, T z3)
Rotation matrix from roll-pitch-yaw angles. Rotations are X-Y-Z.
Definition: Rotation3DBuilder.hpp:243
Definition: Approx.hpp:11
Rotation3DAxisBuilder & operator=(const Rotation3DAxisBuilder &)=delete
Definition: Rotation3DBuilder.hpp:250
auto RotationAxis(T angle, int axis)
Rotates around coordinate axis.
Definition: Rotation3DBuilder.hpp:121
constexpr int Height() const
Returns the number of rows of the matrix.
Definition: MatrixImpl.hpp:39
auto RotationEuler(T z1, T x2, T z3)
Rotation matrix from Euler angles. Rotations are Z-X-Z.
Definition: Rotation3DBuilder.hpp:234
constexpr int ColumnCount() const
Returns the number of columns of the matrix.
Definition: MatrixImpl.hpp:27
Definition: Definitions.hpp:63
constexpr int Width() const
Returns the number of columns of the matrix.
Definition: MatrixImpl.hpp:35
Definition: Rotation3DBuilder.hpp:160
auto RotationX(T angle)
Rotates around the X axis according to the right (left) hand rule.
Definition: Rotation3DBuilder.hpp:139
Rotation3DAxisAngleBuilder(const Vector< T, 3, Packed > &axis, T angle)
Definition: Rotation3DBuilder.hpp:252
T Determinant(const Matrix< T, Dim, Dim, Order, Layout, Packed > &m)
Returns the determinant of the matrix.
Definition: MatrixFunction.hpp:21
auto RotationZ(T angle)
Rotates around the Z axis according to the right (left) hand rule.
Definition: Rotation3DBuilder.hpp:153
auto RotationAxis3(T angle0, T angle1, T angle2)
Rotates around three axes in succession.
Definition: Rotation3DBuilder.hpp:225
constexpr int RowCount() const
Returns the number of rows of the matrix.
Definition: MatrixImpl.hpp:31
bool IsNormalized(const Quaternion< T, Packed > &q)
Check if the quaternion is a unit quaternion, with some tolerance for floats.
Definition: QuaternionFunction.hpp:78