Developer Documentation
QuaternionT.hh
1 /*===========================================================================*\
2  * *
3  * OpenFlipper *
4  * Copyright (c) 2001-2015, RWTH-Aachen University *
5  * Department of Computer Graphics and Multimedia *
6  * All rights reserved. *
7  * www.openflipper.org *
8  * *
9  *---------------------------------------------------------------------------*
10  * This file is part of OpenFlipper. *
11  *---------------------------------------------------------------------------*
12  * *
13  * Redistribution and use in source and binary forms, with or without *
14  * modification, are permitted provided that the following conditions *
15  * are met: *
16  * *
17  * 1. Redistributions of source code must retain the above copyright notice, *
18  * this list of conditions and the following disclaimer. *
19  * *
20  * 2. Redistributions in binary form must reproduce the above copyright *
21  * notice, this list of conditions and the following disclaimer in the *
22  * documentation and/or other materials provided with the distribution. *
23  * *
24  * 3. Neither the name of the copyright holder nor the names of its *
25  * contributors may be used to endorse or promote products derived from *
26  * this software without specific prior written permission. *
27  * *
28  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS *
29  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
30  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A *
31  * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
32  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, *
33  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, *
34  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR *
35  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF *
36  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING *
37  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS *
38  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
39  * *
40 \*===========================================================================*/
41 
42 
43 
44 
45 
46 //=============================================================================
47 //
48 // CLASS Quaternion
49 //
50 //=============================================================================
51 
52 #ifndef ACG_QUATERNION_HH
53 #define ACG_QUATERNION_HH
54 
55 
56 //== INCLUDES =================================================================
57 
58 #include "VectorT.hh"
59 #include "Matrix4x4T.hh"
60 
61 
62 //== NAMESPACES ==============================================================
63 
64 namespace ACG {
65 
66 
67 //== CLASS DEFINITION =========================================================
68 
69 
74 template <class Scalar>
75 class QuaternionT : public VectorT<Scalar,4>
76 {
77 public:
78 
79 #define W Base::data()[0]
80 #define X Base::data()[1]
81 #define Y Base::data()[2]
82 #define Z Base::data()[3]
83 
84 
85  typedef VectorT<Scalar,4> Base;
87  typedef VectorT<Scalar,3> Vec3;
88  typedef VectorT<Scalar,4> Vec4;
89  typedef Matrix4x4T<Scalar> Matrix;
90 
91 
93  QuaternionT(Scalar _w=1.0, Scalar _x=0.0, Scalar _y=0.0, Scalar _z=0.0)
94  : Vec4(_w, _x, _y, _z) {}
95 
97  QuaternionT(const Vec3& _p)
98  : Vec4(0, _p[0], _p[1], _p[2]) {}
99 
101  QuaternionT(const Vec4& _p)
102  : Vec4(_p[0], _p[1], _p[2], _p[3]) {}
103 
105  QuaternionT(Vec3 _axis, Scalar _angle)
106  {
107  _axis.normalize();
108  Scalar theta = 0.5 * _angle;
109  Scalar sin_theta = sin(theta);
110  W = cos(theta);
111  X = sin_theta * _axis[0];
112  Y = sin_theta * _axis[1];
113  Z = sin_theta * _axis[2];
114  }
115 
117  template <class MatrixT>
118  QuaternionT(const MatrixT& _rot)
119  { init_from_matrix( _rot); }
120 
121 
123  void identity() { W=1.0; X=Y=Z=0.0; }
124 
125 
127  Quaternion conjugate() const
128  { return Quaternion(W, -X, -Y, -Z); }
129 
130 
132  Quaternion invert() const
133  { return conjugate()/Base::sqrnorm(); }
134 
135 
137  Quaternion operator*(const Quaternion& _q) const
138  { return Quaternion(W*_q.W - X*_q.X - Y*_q.Y - Z*_q.Z,
139  W*_q.X + X*_q.W + Y*_q.Z - Z*_q.Y,
140  W*_q.Y - X*_q.Z + Y*_q.W + Z*_q.X,
141  W*_q.Z + X*_q.Y - Y*_q.X + Z*_q.W); }
142 
143 
145  Quaternion& operator*=(const Quaternion& _q)
146  { return *this = *this * _q; }
147 
148 
150  template <class Vec3T>
151  Vec3T rotate(const Vec3T& _v) const
152  {
153  Quaternion q = *this * Quaternion(0,_v[0],_v[1],_v[2]) * conjugate();
154  return Vec3T(q[1], q[2], q[3]);
155  }
156 
157 
159  void axis_angle(Vec3& _axis, Scalar& _angle) const
160  {
161  if (fabs(W) > 0.999999)
162  {
163  _axis = Vec3(1,0,0);
164  _angle = 0;
165  }
166  else
167  {
168  _angle = 2.0 * acos(W);
169  _axis = Vec3(X, Y, Z).normalize();
170  }
171  }
172 
173 
174 
176  Matrix rotation_matrix() const
177  {
178  Scalar
179  ww(W*W), xx(X*X), yy(Y*Y), zz(Z*Z), wx(W*X),
180  wy(W*Y), wz(W*Z), xy(X*Y), xz(X*Z), yz(Y*Z);
181 
182  Matrix m;
183 
184  m(0,0) = ww + xx - yy - zz;
185  m(1,0) = 2.0*(xy + wz);
186  m(2,0) = 2.0*(xz - wy);
187 
188  m(0,1) = 2.0*(xy - wz);
189  m(1,1) = ww - xx + yy - zz;
190  m(2,1) = 2.0*(yz + wx);
191 
192  m(0,2) = 2.0*(xz + wy);
193  m(1,2) = 2.0*(yz - wx);
194  m(2,2) = ww - xx - yy + zz;
195 
196  m(0,3) = m(1,3) = m(2,3) = m(3,0) = m(3,1) = m(3,2) = 0.0;
197  m(3,3) = 1.0;
198 
199  return m;
200  }
201 
202 
203 
204  /*
206  Matrix right_mult_matrix() const
207  {
208  Matrix m;
209  m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
210  m(1,0) = X; m(1,1) = W; m(1,2) = -Z; m(1,3) = Y;
211  m(2,0) = Y; m(2,1) = Z; m(2,2) = W; m(2,3) = -X;
212  m(3,0) = Z; m(3,1) = -Y; m(3,2) = X; m(3,3) = W;
213  return m;
214  }
215 
216 
218  Matrix left_mult_matrix() const
219  {
220  Matrix m;
221  m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
222  m(1,0) = X; m(1,1) = W; m(1,2) = Z; m(1,3) = -Y;
223  m(2,0) = Y; m(2,1) = -Z; m(2,2) = W; m(2,3) = X;
224  m(3,0) = Z; m(3,1) = Y; m(3,2) = -X; m(3,3) = W;
225  return m;
226  }
227  */
229  Matrix right_mult_matrix() const
230  {
231  Matrix m;
232  m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
233  m(1,0) = X; m(1,1) = W; m(1,2) = Z; m(1,3) = -Y;
234  m(2,0) = Y; m(2,1) = -Z; m(2,2) = W; m(2,3) = X;
235  m(3,0) = Z; m(3,1) = Y; m(3,2) = -X; m(3,3) = W;
236  return m;
237  }
238 
239 
241  Matrix left_mult_matrix() const
242  {
243  Matrix m;
244  m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
245  m(1,0) = X; m(1,1) = W; m(1,2) = -Z; m(1,3) = Y;
246  m(2,0) = Y; m(2,1) = Z; m(2,2) = W; m(2,3) = -X;
247  m(3,0) = Z; m(3,1) = -Y; m(3,2) = X; m(3,3) = W;
248  return m;
249  }
250 
251 
253  template<class MatrixT>
254  void init_from_matrix( const MatrixT& _rot)
255  {
256  Scalar trace = _rot(0,0) + _rot(1,1) + _rot(2,2);
257  if( trace > 0.0 )
258  {
259  Scalar s = 0.5 / sqrt(trace + 1.0);
260  W = 0.25 / s;
261  X = ( _rot(2,1) - _rot(1,2) ) * s;
262  Y = ( _rot(0,2) - _rot(2,0) ) * s;
263  Z = ( _rot(1,0) - _rot(0,1) ) * s;
264  }
265  else
266  {
267  if ( _rot(0,0) > _rot(1,1) && _rot(0,0) > _rot(2,2) )
268  {
269  Scalar s = 2.0 * sqrt( 1.0 + _rot(0,0) - _rot(1,1) - _rot(2,2));
270  W = (_rot(2,1) - _rot(1,2) ) / s;
271  X = 0.25 * s;
272  Y = (_rot(0,1) + _rot(1,0) ) / s;
273  Z = (_rot(0,2) + _rot(2,0) ) / s;
274  }
275  else
276  if (_rot(1,1) > _rot(2,2))
277  {
278  Scalar s = 2.0 * sqrt( 1.0 + _rot(1,1) - _rot(0,0) - _rot(2,2));
279  W = (_rot(0,2) - _rot(2,0) ) / s;
280  X = (_rot(0,1) + _rot(1,0) ) / s;
281  Y = 0.25 * s;
282  Z = (_rot(1,2) + _rot(2,1) ) / s;
283  }
284  else
285  {
286  Scalar s = 2.0 * sqrt( 1.0 + _rot(2,2) - _rot(0,0) - _rot(1,1) );
287  W = (_rot(1,0) - _rot(0,1) ) / s;
288  X = (_rot(0,2) + _rot(2,0) ) / s;
289  Y = (_rot(1,2) + _rot(2,1) ) / s;
290  Z = 0.25 * s;
291  }
292  }
293  }
294 
295 
297  Quaternion exponential() const
298  {
299  Vec3 n(X,Y,Z);
300  Scalar theta( n.norm());
301  Scalar sin_theta = sin(theta);
302  Scalar cos_theta = cos(theta);
303 
304  if( theta > 1e-6 )
305  n *= sin_theta/theta;
306  else
307  n = Vec3(0,0,0);
308 
309  return Quaternion( cos_theta, n[0], n[1], n[2]);
310  }
311 
312 
314  Quaternion logarithm() const
315  {
316  // clamp to valid input
317  double w = W;
318  if( w > 1.0) w = 1.0;
319  else if( w < -1.0) w = -1.0;
320 
321  Scalar theta_half = acos(w);
322 
323  Vec3 n(X,Y,Z);
324  Scalar n_norm( n.norm());
325 
326  if( n_norm > 1e-6 )
327  n *= theta_half/n_norm;
328  else
329  n = Vec3(0,0,0);
330 
331  return Quaternion( 0, n[0], n[1], n[2]);
332  }
333 
334  void print_info()
335  {
336  // get axis, angle and matrix
337  Vec3 axis;
338  Scalar angle;
339  this->axis_angle( axis, angle);
340  Matrix m;
341  m = this->rotation_matrix();
342 
343  std::cerr << "quaternion : " << (*this) << std::endl;
344  std::cerr << "length : " << this->norm() << std::endl;
345  std::cerr << "axis, angle: " << axis << ", " << angle*180.0/M_PI << "\n";
346  std::cerr << "rot matrix :\n";
347  std::cerr << m << std::endl;
348  }
349 
350 
351 #undef W
352 #undef X
353 #undef Y
354 #undef Z
355 };
356 
357 
360 
361 
362 //=============================================================================
363 } // namespace ACG
364 //=============================================================================
365 #endif // ACG_QUATERNION_HH defined
366 //=============================================================================
367 
Quaternion exponential() const
quaternion exponential (for unit quaternions)
Definition: QuaternionT.hh:297
Namespace providing different geometric functions concerning angles.
Quaternion operator*(const Quaternion &_q) const
quaternion * quaternion
Definition: QuaternionT.hh:137
Matrix rotation_matrix() const
cast to rotation matrix
Definition: QuaternionT.hh:176
Quaternion conjugate() const
conjugate quaternion
Definition: QuaternionT.hh:127
auto norm() const -> decltype(std::sqrt(std::declval< VectorT< S, DIM >>().sqrnorm()))
compute euclidean norm
Definition: Vector11T.hh:409
decltype(std::declval< S >() *std::declval< S >()) sqrnorm() const
compute squared euclidean norm
Definition: Vector11T.hh:397
auto normalize() -> decltype(*this/=std::declval< VectorT< S, DIM >>().norm())
Definition: Vector11T.hh:429
QuaternionT(const Vec3 &_p)
construct from 3D point (pure imaginary quaternion)
Definition: QuaternionT.hh:97
QuaternionT(Scalar _w=1.0, Scalar _x=0.0, Scalar _y=0.0, Scalar _z=0.0)
construct from 4 Scalars (default)
Definition: QuaternionT.hh:93
Matrix left_mult_matrix() const
get matrix for mult from left (q*p = Qp)
Definition: QuaternionT.hh:241
Matrix right_mult_matrix() const
get matrix for mult from right (p*q = Qp)
Definition: QuaternionT.hh:229
void init_from_matrix(const MatrixT &_rot)
get quaternion from rotation matrix
Definition: QuaternionT.hh:254
QuaternionT(Vec3 _axis, Scalar _angle)
construct from rotation axis and angle (in radians)
Definition: QuaternionT.hh:105
QuaternionT(const MatrixT &_rot)
construct from rotation matrix (only valid for rotation matrices!)
Definition: QuaternionT.hh:118
Quaternion logarithm() const
quaternion logarithm (for unit quaternions)
Definition: QuaternionT.hh:314
QuaternionT(const Vec4 &_p)
construct from 4D vector
Definition: QuaternionT.hh:101
void identity()
identity rotation
Definition: QuaternionT.hh:123
void axis_angle(Vec3 &_axis, Scalar &_angle) const
get rotation axis and angle (only valid for unit quaternions!)
Definition: QuaternionT.hh:159
Quaternion invert() const
invert quaternion
Definition: QuaternionT.hh:132
Vec3T rotate(const Vec3T &_v) const
rotate vector
Definition: QuaternionT.hh:151
Quaternion & operator*=(const Quaternion &_q)
quaternion *= quaternion
Definition: QuaternionT.hh:145