scsl  1.1.1
Shimmering Clarity Standard Library
Quaternion.h
Go to the documentation of this file.
1 
23 #ifndef SCMATH_GEOM_QUATERNION_H
24 #define SCMATH_GEOM_QUATERNION_H
25 
26 
27 #include <cassert>
28 #include <cmath>
29 #include <initializer_list>
30 #include <iostream>
31 #include <ostream>
32 
33 #include <scmp/Math.h>
34 #include <scmp/geom/Vector.h>
35 
36 
37 namespace scmp {
38 namespace geom {
39 
40 
60 template<typename T>
61 class Quaternion {
62 public:
64  Quaternion() : v(Vector<T, 3>{0.0, 0.0, 0.0}), w(1.0)
65  {
66  scmp::DefaultEpsilon(this->eps);
67  v.SetEpsilon(this->eps);
68  };
69 
70 
82  Quaternion(Vector<T, 3> _axis, T _angle) : v(_axis), w(_angle)
83  {
84  this->constrainAngle();
85  scmp::DefaultEpsilon(this->eps);
86  v.SetEpsilon(this->eps);
87  };
88 
89 
95  v(Vector<T, 3>{vector[1], vector[2], vector[3]}),
96  w(vector[0])
97  {
98  this->constrainAngle();
99  scmp::DefaultEpsilon(this->eps);
100  v.SetEpsilon(this->eps);
101  }
102 
103 
109  Quaternion(std::initializer_list<T> ilst)
110  {
111  auto it = ilst.begin();
112 
113  this->v = Vector<T, 3>{it[1], it[2], it[3]};
114  this->w = it[0];
115 
116  this->constrainAngle();
117  scmp::DefaultEpsilon(this->eps);
118  v.SetEpsilon(this->eps);
119  }
120 
121 
125  void
126  SetEpsilon(T epsilon)
127  {
128  this->eps = epsilon;
129  this->v.SetEpsilon(epsilon);
130  }
131 
132 
137  Axis() const
138  {
139  return this->v;
140  }
141 
142 
146  T
147  Angle() const
148  {
149  return this->w;
150  }
151 
152 
157  T
158  Dot(const Quaternion<T> &other) const
159  {
160  double innerProduct = this->v[0] * other.v[0];
161 
162  innerProduct += (this->v[1] * other.v[1]);
163  innerProduct += (this->v[2] * other.v[2]);
164  innerProduct += (this->w * other.w);
165  return innerProduct;
166  }
167 
168 
175  T
176  Norm() const
177  {
178  T n = 0;
179 
180  n += (this->v[0] * this->v[0]);
181  n += (this->v[1] * this->v[1]);
182  n += (this->v[2] * this->v[2]);
183  n += (this->w * this->w);
184 
185  return std::sqrt(n);
186  }
187 
188 
192  Quaternion
194  {
195  return *this / this->Norm();
196  }
197 
201  Quaternion
202  Conjugate() const
203  {
204  return Quaternion(Vector<T, 4>{this->w, -this->v[0], -this->v[1], -this->v[2]});
205  }
206 
207 
211  Quaternion
212  Inverse() const
213  {
214  T _norm = this->Norm();
215 
216  return this->Conjugate() / (_norm * _norm);
217  }
218 
219 
223  bool
224  IsIdentity() const {
225  return this->v.IsZero() &&
226  scmp::WithinTolerance(this->w, (T)1.0, this->eps);
227  }
228 
229 
233  bool
235  {
236  auto normal = this->Norm();
237  return scmp::WithinTolerance(normal, (T) 1.0, this->eps);
238  }
239 
240 
248  AsVector() const
249  {
250  return Vector<T, 4>{this->w, this->v[0], this->v[1], this->v[2]};
251  }
252 
253 
260  {
261  return (this->Conjugate() * vr * (*this)).Axis();
262  }
263 
264 
275  Euler() const
276  {
277  T yaw, pitch, roll;
278  T a = this->w, a2 = a * a;
279  T b = this->v[0], b2 = b * b;
280  T c = this->v[1], c2 = c * c;
281  T d = this->v[2], d2 = d * d;
282 
283  yaw = std::atan2(2 * ((a * b) + (c * d)), a2 - b2 - c2 + d2);
284  pitch = std::asin(2 * ((b * d) - (a * c)));
285  roll = std::atan2(2 * ((a * d) + (b * c)), a2 + b2 - c2 - d2);
286 
287  return Vector<T, 3>{yaw, pitch, roll};
288  }
289 
290 
295  Quaternion
296  operator+(const Quaternion<T> &other) const
297  {
298  return Quaternion(this->v + other.v, this->w + other.w);
299  }
300 
301 
306  Quaternion
307  operator-(const Quaternion<T> &other) const
308  {
309  return Quaternion(this->v - other.v, this->w - other.w);
310  }
311 
312 
317  Quaternion
318  operator*(const T k) const
319  {
320  return Quaternion(this->v * k, this->w * k);
321  }
322 
323 
328  Quaternion
329  operator/(const T k) const
330  {
331  return Quaternion(this->v / k, this->w / k);
332  }
333 
334 
343  Quaternion
344  operator*(const Vector<T, 3> &vector) const
345  {
346  return Quaternion(vector * this->w + this->v.Cross(vector),
347  (T) 0.0);
348  }
349 
350 
355  Quaternion
356  operator*(const Quaternion<T> &other) const
357  {
358  T angle = (this->w * other.w) -
359  (this->v * other.v);
360  Vector<T, 3> axis = (other.v * this->w) +
361  (this->v * other.w) +
362  (this->v.Cross(other.v));
363  return Quaternion(axis, angle);
364  }
365 
366 
371  bool
372  operator==(const Quaternion<T> &other) const
373  {
374  return (this->v == other.v) &&
375  (scmp::WithinTolerance(this->w, other.w, this->eps));
376  }
377 
378 
383  bool
384  operator!=(const Quaternion<T> &other) const
385  {
386  return !(*this == other);
387  }
388 
389 
398  friend std::ostream &
399  operator<<(std::ostream &outs, const Quaternion<T> &q)
400  {
401  outs << q.w << " + " << q.v;
402  return outs;
403  }
404 
405 private:
406  static constexpr T minRotation = -4 * M_PI;
407  static constexpr T maxRotation = 4 * M_PI;
408 
409  Vector<T, 3> v; // Axis of rotation
410  T w; // Angle of rotation
411  T eps;
412 
413  void
414  constrainAngle()
415  {
416  if (this->w < 0.0) {
417  this->w = std::fmod(this->w, this->minRotation);
418  }
419  else {
420  this->w = std::fmod(this->w, this->maxRotation);
421  }
422  }
423 };
424 
425 
430 
434 
438 
439 
452 
464 Quaterniond MakeQuaternion(Vector3D axis, double angle);
465 
466 
478 template <typename T>
481 {
482  return Quaternion<T>(axis.UnitVector() * std::sin(angle / (T)2.0),
483  std::cos(angle / (T)2.0));
484 }
485 
486 
497 
498 
509 
510 
523 template <typename T>
526 {
527  return (p + (q - p) * t).UnitQuaternion();
528 }
529 
530 
544 template <typename T>
545 Quaternion<T>
547 {
548  assert(p.IsUnitQuaternion());
549  assert(q.IsUnitQuaternion());
550 
551  T dp = p.Dot(q);
552  T sign = dp < 0.0 ? -1.0 : 1.0;
553  T omega = std::acos(dp * sign);
554  T sin_omega = std::sin(omega); // Compute once.
555 
556  if (dp > 0.99999) {
557  return LERP(p, q * sign, t);
558  }
559 
560  return (p * std::sin((1.0 - t) * omega) / sin_omega) +
561  (q * sign * std::sin(omega*t) / sin_omega);
562 }
563 
564 
571 
572 
573 } // namespace geom
574 } // namespace wr
575 
576 
577 #endif // SCMATH_GEOM_QUATERNION_H
Common math functions.
Linear algebraic vector class.
Quaternions provide a representation of Orientation and rotations in three dimensions.
Definition: Quaternion.h:61
T Dot(const Quaternion< T > &other) const
Compute the Dot product of two quaternions.
Definition: Quaternion.h:158
Quaternion operator/(const T k) const
Scalar division.
Definition: Quaternion.h:329
bool operator==(const Quaternion< T > &other) const
Quaternion equivalence.
Definition: Quaternion.h:372
T Angle() const
Return the Angle of rotation of this Quaternion.
Definition: Quaternion.h:147
void SetEpsilon(T epsilon)
Set the comparison tolerance for this Quaternion.
Definition: Quaternion.h:126
Quaternion()
Construct an identity Quaternion.
Definition: Quaternion.h:64
Vector< T, 3 > Axis() const
Return the Axis of rotation of this Quaternion.
Definition: Quaternion.h:137
Quaternion operator-(const Quaternion< T > &other) const
Quaternion subtraction.
Definition: Quaternion.h:307
Quaternion(Vector< T, 4 > vector)
Definition: Quaternion.h:94
Quaternion operator*(const Quaternion< T > &other) const
Quaternion Hamilton multiplication.
Definition: Quaternion.h:356
Vector< T, 4 > AsVector() const
Convert to Vector form.
Definition: Quaternion.h:248
friend std::ostream & operator<<(std::ostream &outs, const Quaternion< T > &q)
Output a Quaternion to a stream in the form a + <i, j, k>.
Definition: Quaternion.h:399
Quaterniond MakeQuaternion(Vector3D axis, double angle)
Convience Quaternion construction function.
Quaternionf MakeQuaternion(Vector3F axis, float angle)
Convenience Quaternion construction function.
Quaternion Conjugate() const
Compute the Conjugate of a Quaternion.
Definition: Quaternion.h:202
bool IsIdentity() const
Determine whether this is an identity Quaternion.
Definition: Quaternion.h:224
Quaternion Inverse() const
Compute the Inverse of a Quaternion.
Definition: Quaternion.h:212
bool operator!=(const Quaternion< T > &other) const
Quaternion non-equivalence.
Definition: Quaternion.h:384
Quaternion operator*(const T k) const
Scalar multiplication.
Definition: Quaternion.h:318
Vector< T, 3 > Euler() const
Return Euler angles for this Quaternion.
Definition: Quaternion.h:275
Vector< T, 3 > Rotate(Vector< T, 3 > vr) const
Rotate Vector vr about this Quaternion.
Definition: Quaternion.h:259
bool IsUnitQuaternion() const
Determine whether this is a unit Quaternion.
Definition: Quaternion.h:234
Quaternion(std::initializer_list< T > ilst)
An initializer list containing values for w, x, y, and z.
Definition: Quaternion.h:109
Quaternion UnitQuaternion()
Return the unit Quaternion.
Definition: Quaternion.h:193
T Norm() const
Compute the Norm of a Quaternion.
Definition: Quaternion.h:176
Quaternion(Vector< T, 3 > _axis, T _angle)
Construct a Quaternion with an Axis and Angle of rotation.
Definition: Quaternion.h:82
Quaternion< T > MakeQuaternion(Vector< T, 3 > axis, T angle)
Convience Quaternion construction function.
Definition: Quaternion.h:480
Quaternion operator*(const Vector< T, 3 > &vector) const
Quaternion Hamilton multiplication with a three- dimensional vector.
Definition: Quaternion.h:344
Quaterniond DoubleQuaternionFromEuler(Vector3D euler)
COnstruct a Quaternion from Euler angles.
Quaternionf FloatQuaternionFromEuler(Vector3F euler)
COnstruct a Quaternion from Euler angles.
Quaternion operator+(const Quaternion< T > &other) const
Quaternion addition.
Definition: Quaternion.h:296
Vectors represent a direction and Magnitude.
Definition: Vector.h:57
Vector Cross(const Vector< T, N > &other) const
Compute the cross product of two vectors.
Definition: Vector.h:292
bool IsZero() const
Determine whether this is a zero vector.
Definition: Vector.h:158
void SetEpsilon(T eps)
Set equivalence tolerance.
Definition: Vector.h:148
Vector UnitVector() const
Obtain the unit vector for this vector.
Definition: Vector.h:173
Quaternion< double > Quaterniond
Type alias for a double Quaternion.
Definition: Quaternion.h:437
Quaternion< float > Quaternionf
Type alias for a float Quaternion.
Definition: Quaternion.h:433
Quaternion< T > ShortestSLERP(Quaternion< T > p, Quaternion< T > q, T t)
Shortest distance spherical linear interpolation.
Definition: Quaternion.h:546
void QuaternionSelfTest()
Internal consistency check.
Quaternion< T > LERP(Quaternion< T > p, Quaternion< T > q, T t)
Linear interpolation for two Quaternions.
Definition: Quaternion.h:525
Shimmering Clarity Math & Physics toolkit.
Definition: estimation.h:31
void DefaultEpsilon(double &epsilon)
Get the default epsilon value.