Quaternion.hpp
1 /*
2 Cafu Engine, http://www.cafu.de/
3 Copyright (c) Carsten Fuchs and other contributors.
5 */
6
7 #ifndef CAFU_MATH_QUATERNION_HPP_INCLUDED
8 #define CAFU_MATH_QUATERNION_HPP_INCLUDED
9
10 #include "Vector3.hpp"
11 #include <algorithm>
12
13
14 namespace cf
15 {
16  namespace math
17  {
18  template<class T> class AnglesT;
19  template<class T> class Matrix3x3T;
20
21
22  /// This class represents a quaternion.
23  template<class T>
24  class QuaternionT
25  {
26  public:
27
28  T x;
29  T y;
30  T z;
31  T w;
32
33
34  /// The default constructor. It initializes the quaternion from the given x_, y_, z_ and w_ respectively.
35  QuaternionT(T x_=0, T y_=0, T z_=0, T w_=1)
36  : x(x_), y(y_), z(z_), w(w_) { }
37
38  /// Constructs a quaternion from an array of (at least) four values.
39  template<class C> explicit QuaternionT(const C Values[])
40  : x(T(Values[0])),
41  y(T(Values[1])),
42  z(T(Values[2])),
43  w(T(Values[3])) { }
44
45  /// Constructs a quaternion from the given angles.
46  /// See the documentation of the AnglesT class for details.
47  QuaternionT(const AnglesT<T>& Angles);
48
49  /// Constructs a quaternion from the given rotation matrix.
50  /// If the matrix is not orthonormal, the result is undefined.
51  QuaternionT(const Matrix3x3T<T>& Mat);
52
53  /// Constructs a quaternion from a rotation axis and angle.
54  /// This is useful, for example, to rotate one vector onto another, as is done in the
55  /// implementation of cf::GameSys::ComponentTransformT::LookAt().
56  /// @param Axis The axis to rotate about. This given axis must be of unit length, or else the result is undefined.
58  QuaternionT(const Vector3T<T>& Axis, const T Angle);
59
60  /// Constructs a quaternion from the first three components (x, y, z) of a unit quaternion.
61  static QuaternionT FromXYZ(const Vector3T<T>& Vec)
62  {
63  const T ww = T(1.0) - Vec.x*Vec.x - Vec.y*Vec.y - Vec.z*Vec.z;
64
65  // Note that convention is to use -sqrt(ww), not +sqrt(ww).
66  // This must be taken into account in GetXYZ().
67  return QuaternionT(Vec.x, Vec.y, Vec.z, (ww<0) ? 0 : -sqrt(ww));
68  }
69
70  /// Constructs a quaternion from three Euler angles.
71  /// @param Pitch the Euler rotation about the x-axis, in radians.
72  /// @param Yaw the Euler rotation about the y-axis, in radians.
73  /// @param Roll the Euler rotation about the z-axis, in radians.
74  /// Note that the assignment of angles to axes assumes a right-handed coordinate system where the z-axis points towards the viewer.
75  /// This is especially different from the coordinate system that class cf::math::AnglesT<T> uses, which is also right-handed, but
76  /// rotated by 90 degrees so that the z-axis points up and the y-axis away from the viewer!
77  static QuaternionT Euler(const T Pitch, const T Yaw, const T Roll);
78
79
80  /// Returns the x, y and z components as a Vector3T<T>.
82  {
83  // Properly take the sign of w into account for consistence with the FromXYZ constructor.
84  // Note that (-x, -y, -z, -w) is a different quaternion than (x, y, z, w), but it describes the *same* rotation.
85  return w<0 ? Vector3T<T>(x, y, z) : Vector3T<T>(-x, -y, -z);
86  }
87
88  /// Returns the conjugate of this quaternion.
89  /// If the quaternion is of unit length, then the conjugate is also its inverse.
90  QuaternionT<T> GetConjugate() const { return QuaternionT<T>(-x, -y, -z, w); }
91
92  /// Returns the dot product of this quaternion and B.
93  T dot(const QuaternionT<T>& B) const { return x*B.x + y*B.y + z*B.z + w*B.w; }
94
95  /// Returns the length of this quaternion.
96  T length() const { return sqrt(lengthSqr()); }
97
98  /// Returns the square of the length of this quaternion.
99  T lengthSqr() const { return x*x + y*y + z*z + w*w; }
100
101  /// Returns if this quaternion is normal, i.e. has length 1 within the given tolerance.
102  /// @param Epsilon The tolerance value.
103  bool IsNormal(const T Epsilon=0) const
104  {
105  return fabs(length()-T(1.0)) <= Epsilon;
106  }
107
108
109  /// Component access by index number (0 to 3) rather than by name.
110  /// @param Index Index of the component to access. Can only be 0, 1, 2 or 3 (for x, y, z, w).
111  /// @throws InvalidOperationE if Index is not 0, 1, 2 or 3.
112  T& operator [] (unsigned int Index)
113  {
114  switch (Index)
115  {
116  case 0: return x;
117  case 1: return y;
118  case 2: return z;
119  case 3: return w;
120  default: throw InvalidOperationE();
121  }
122  }
123
124  /// Component access by index number (0 to 3) rather than by name.
125  /// @param Index Index of the component to access. Can only be 0, 1, 2 or 3 (for x, y, z, w).
126  /// @throws InvalidOperationE if Index is not 0, 1, 2 or 3.
127  const T& operator [] (unsigned int Index) const
128  {
129  switch (Index)
130  {
131  case 0: return x;
132  case 1: return y;
133  case 2: return z;
134  case 3: return w;
135  default: throw InvalidOperationE();
136  }
137  }
138
139  /// Returns whether this quaternion and B are (bit-wise) identical.
140  /// Use this operator with care, as it comes *without* any epsilon threshold to take rounding errors into account.
141  /// @param B Quaternion to compare to.
142  bool operator == (const QuaternionT<T>& B) const
143  {
144  return x==B.x && y==B.y && z==B.z && w==B.w;
145  }
146
147  /// Returns whether this quaternion and B are not equal (bit-wise).
148  /// Use this operator with care, as it comes *without* any epsilon threshold to take rounding errors into account.
149  /// @param B Quaternion to compare to.
150  bool operator != (const QuaternionT<T>& B) const
151  {
152  return x!=B.x || y!=B.y || z!=B.z || w!=B.w;
153  }
154
155  /// The unary minus operator.
157  {
158  return QuaternionT<T>(-x, -y, -z, -w);
159  }
160
161  /// Returns the sum of this quaternion and B.
163  {
164  return QuaternionT<T>(x+B.x, y+B.y, z+B.z, w+B.w);
165  }
166
167  /// Adds B to this quaternion.
169  {
170  x+=B.x; y+=B.y; z+=B.z; w+=B.w;
171  return *this;
172  }
173
174  /// Returns the difference between this quaternion and B.
176  {
177  return QuaternionT<T>(x-B.x, y-B.y, z-B.z, w-B.w);
178  }
179
180  /// Subtracts B from this quaternion.
182  {
183  x-=B.x; y-=B.y; z-=B.z; w-=B.w;
184  return *this;
185  }
186
187  /// Returns the quaternion Q that expresses the combined rotation of this quaternion and \c B, <tt>Q = this*B</tt>.
189  {
190  return QuaternionT<T>(
191  w*B.x + x*B.w + y*B.z - z*B.y,
192  w*B.y + y*B.w + z*B.x - x*B.z,
193  w*B.z + z*B.w + x*B.y - y*B.x,
194  w*B.w - x*B.x - y*B.y - z*B.z);
195  }
196
197  /// Returns a copy of this quaternion scaled by s.
198  QuaternionT<T> operator * (const T s) const
199  {
200  return QuaternionT<T>(x*s, y*s, z*s, w*s);
201  }
202
203  /// Scales this quaternion by s.
205  {
206  x*=s; y*=s; z*=s; w*=s;
207  return *this;
208  }
209
210  /// Returns a copy of this quaternion divided by s, assuming that s is not 0.
211  QuaternionT<T> operator / (const T s) const
212  {
213  return QuaternionT<T>(x/s, y/s, z/s, w/s); // Intentionally don't multiply with the precomputed reciprocal.
214  }
215
216  /// Divides this quaternion by s, assuming that s is not 0.
218  {
219  x/=s; y/=s; z/=s; w/=s; // Intentionally don't multiply with the precomputed reciprocal.
220  return *this;
221  }
222  };
223
224
225  typedef QuaternionT<float> QuaternionfT; ///< Typedef for a QuaternionT of floats.
226  typedef QuaternionT<double> QuaterniondT; ///< Typedef for a QuaternionT of doubles.
227  }
228 }
229
230
231 /// Returns the dot product of A and B.
232 template<class T> inline T dot(const cf::math::QuaternionT<T>& A, const cf::math::QuaternionT<T>& B)
233 {
234  return A.dot(B);
235 }
236
237
238 /// Returns the length of A.
239 template<class T> inline T length(const cf::math::QuaternionT<T>& A)
240 {
241  return A.length();
242 }
243
244
245 /// Returns the normalized (unit length) version of A.
246 /// @throws DivisionByZeroE if length(A)<=Epsilon.
247 template<class T> inline cf::math::QuaternionT<T> normalize(const cf::math::QuaternionT<T>& A, const T Epsilon)
248 {
249  const T len=length(A);
250
251  // Use <= (instead of <) for consistence with Vector3T<>::normalize() and normalizeOr0().
252  if (len<=Epsilon) throw DivisionByZeroE();
253
254  return A/len;
255 }
256
257
258 /// Returns the normalized (unit length) version of A if length(A)>Epsilon, or the identity quaternion otherwise.
259 template<class T> inline cf::math::QuaternionT<T> normalizeOr0(const cf::math::QuaternionT<T>& A, const T Epsilon=0)
260 {
261  const T len=length(A);
262
263  return (len>Epsilon) ? A/len : cf::math::QuaternionT<T>();
264 }
265
266
267 /// This methods implements spherical linear interpolation:
268 /// It returns the interpolated quaternion between input quaternions P and Q, according to scalar t (at uniform angular velocity).
269 template<class T> inline cf::math::QuaternionT<T> slerp(
270  const cf::math::QuaternionT<T>& P, cf::math::QuaternionT<T> Q, const T t, const bool ShortPath=true)
271 {
272  T CosOmega=dot(P, Q);
273
274  if (CosOmega<0 && ShortPath)
275  {
276  CosOmega=-CosOmega;
277  Q=-Q;
278  }
279
280  CosOmega=std::min(CosOmega, T( 1.0));
281  CosOmega=std::max(CosOmega, T(-1.0));
282
283  // Note that the angle of the rotation that is described is actually 2*Omega.
284  const T Omega =acos(CosOmega);
285  const T SinOmega=sin (Omega);
286
287  if (SinOmega > T(0.01))
288  {
289  // Omega is at least 0,57° larger than 0° or less than 180°,
290  // thus the division below is numerically stable: implement normal slerp.
291  const T tp=sin((T(1.0)-t)*Omega)/SinOmega;
292  const T tq=sin( t *Omega)/SinOmega;
293
294  return P*tp + Q*tq;
295  }
296
297  if (CosOmega>0)
298  {
299  // Omega is close to 0°.
300  // For numerical stability, implement normalized linear interpolation.
301  return normalizeOr0(P*(T(1.0)-t) + Q*t);
302  }
303
304  // Omega is close to 180°, describing a rotation of about 360°.
305  // P and Q are thus on opposite (180°) points on the unit sphere, e.g. the north and south pole.
306  // The problem is solved by finding an arbitrary point E on the "equator". The interpolation is
307  // then done "through" that point, so that t==0 maps to P, t==0.5 to E, and t==1 to "2PE", which is Q.
308  assert(!ShortPath);
309  const T PI=T(3.14159265358979323846);
310
311  return P*sin((T(0.5)-t)*PI) + cf::math::QuaternionT<T>(-P.y, P.x, -P.w, P.z)*sin(t*PI);
312 }
313
314
315 template<class T> inline std::string convertToString(const cf::math::QuaternionT<T>& A)
316 {
317  // From MSDN documentation: "digits10 returns the number of decimal digits that the type can represent without loss of precision."
318  // For floats, that's usually 6, for doubles, that's usually 15. However, we want to use the number of *significant* decimal digits here,
319  // see http://www.open-std.org/JTC1/sc22/wg21/docs/papers/2006/n2005.pdf for details.
320  const int sigdigits=std::numeric_limits<T>::digits10 + 3;
321
322  std::ostringstream out;
323
324  out << std::setprecision(sigdigits) << "(" << A.x << ", " << A.y << ", " << A.z << ", " << A.w << ")";
325
326  return out.str();
327 }
328
329
330 template<class T> inline std::ostream& operator << (std::ostream& os, const cf::math::QuaternionT<T>& A)
331 {
332  return os << "(" << A.x << ", " << A.y << ", " << A.z << ", " << A.w << ")";
333 }
334
335 #endif
QuaternionT< T > operator*(const QuaternionT< T > &B) const
Returns the quaternion Q that expresses the combined rotation of this quaternion and B...
Definition: Quaternion.hpp:188
QuaternionT< T > operator-() const
The unary minus operator.
Definition: Quaternion.hpp:156
static QuaternionT FromXYZ(const Vector3T< T > &Vec)
Constructs a quaternion from the first three components (x, y, z) of a unit quaternion.
Definition: Quaternion.hpp:61
QuaternionT< T > & operator/=(const T s)
Divides this quaternion by s, assuming that s is not 0.
Definition: Quaternion.hpp:217
This class represents a triple of angles.
Definition: Angles.hpp:71
QuaternionT(T x_=0, T y_=0, T z_=0, T w_=1)
The default constructor. It initializes the quaternion from the given x_, y_, z_ and w_ respectively...
Definition: Quaternion.hpp:35
bool operator!=(const QuaternionT< T > &B) const
Returns whether this quaternion and B are not equal (bit-wise).
Definition: Quaternion.hpp:150
T lengthSqr() const
Returns the square of the length of this quaternion.
Definition: Quaternion.hpp:99
QuaternionT< T > operator/(const T s) const
Returns a copy of this quaternion divided by s, assuming that s is not 0.
Definition: Quaternion.hpp:211
T y
The y-component of this vector.
Definition: Vector3.hpp:41
static QuaternionT Euler(const T Pitch, const T Yaw, const T Roll)
Constructs a quaternion from three Euler angles.
Definition: Quaternion.cpp:99
QuaternionT< T > & operator*=(const T s)
Scales this quaternion by s.
Definition: Quaternion.hpp:204
This class represents a polymorphic 3-dimensional vector.
Definition: Misc.hpp:11
T length() const
Returns the length of this quaternion.
Definition: Quaternion.hpp:96
T & operator[](unsigned int Index)
Component access by index number (0 to 3) rather than by name.
Definition: Quaternion.hpp:112
bool IsNormal(const T Epsilon=0) const
Returns if this quaternion is normal, i.e.
Definition: Quaternion.hpp:103
QuaternionT< T > GetConjugate() const
Returns the conjugate of this quaternion.
Definition: Quaternion.hpp:90
Invalid operation (invalid use of method, etc.).
Definition: Errors.hpp:33
QuaternionT< T > & operator+=(const QuaternionT< T > &B)
Definition: Quaternion.hpp:168
bool operator==(const QuaternionT< T > &B) const
Returns whether this quaternion and B are (bit-wise) identical.
Definition: Quaternion.hpp:142
Vector3T< T > GetXYZ() const
Returns the x, y and z components as a Vector3T<T>.
Definition: Quaternion.hpp:81
QuaternionT< T > operator+(const QuaternionT< T > &B) const
Returns the sum of this quaternion and B.
Definition: Quaternion.hpp:162
T dot(const QuaternionT< T > &B) const
Returns the dot product of this quaternion and B.
Definition: Quaternion.hpp:93
QuaternionT(const C Values[])
Constructs a quaternion from an array of (at least) four values.
Definition: Quaternion.hpp:39
T z
The z-component of this vector.
Definition: Vector3.hpp:42
This class represents a quaternion.
Definition: Angles.hpp:18
T x
The x-component of this vector.
Definition: Vector3.hpp:40
QuaternionT< T > & operator-=(const QuaternionT< T > &B)
Subtracts B from this quaternion.
Definition: Quaternion.hpp:181
This class represents a generic 3x3 matrix.
Definition: Angles.hpp:17
Division by zero error.
Definition: Errors.hpp:24