Developer Documentation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
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  * $Revision$ *
45  * $Author$ *
46  * $Date$ *
47  * *
48 \*===========================================================================*/
49 
50 
51 
52 //=============================================================================
53 //
54 // CLASS Quaternion
55 //
56 //=============================================================================
57 
58 #ifndef ACG_QUATERNION_HH
59 #define ACG_QUATERNION_HH
60 
61 
62 //== INCLUDES =================================================================
63 
64 #include "VectorT.hh"
65 #include "Matrix4x4T.hh"
66 
67 
68 //== NAMESPACES ==============================================================
69 
70 namespace ACG {
71 
72 
73 //== CLASS DEFINITION =========================================================
74 
75 
80 template <class Scalar>
81 class QuaternionT : public VectorT<Scalar,4>
82 {
83 public:
84 
85 #define W Base::data()[0]
86 #define X Base::data()[1]
87 #define Y Base::data()[2]
88 #define Z Base::data()[3]
89 
90 
91  typedef VectorT<Scalar,4> Base;
93  typedef VectorT<Scalar,3> Vec3;
94  typedef VectorT<Scalar,4> Vec4;
95  typedef Matrix4x4T<Scalar> Matrix;
96 
97 
99  QuaternionT(Scalar _w=1.0, Scalar _x=0.0, Scalar _y=0.0, Scalar _z=0.0)
100  : Vec4(_w, _x, _y, _z) {}
101 
103  QuaternionT(const Vec3& _p)
104  : Vec4(0, _p[0], _p[1], _p[2]) {}
105 
107  QuaternionT(const Vec4& _p)
108  : Vec4(_p[0], _p[1], _p[2], _p[3]) {}
109 
111  QuaternionT(Vec3 _axis, Scalar _angle)
112  {
113  _axis.normalize();
114  Scalar theta = 0.5 * _angle;
115  Scalar sin_theta = sin(theta);
116  W = cos(theta);
117  X = sin_theta * _axis[0];
118  Y = sin_theta * _axis[1];
119  Z = sin_theta * _axis[2];
120  }
121 
123  template <class MatrixT>
124  QuaternionT(const MatrixT& _rot)
125  { init_from_matrix( _rot); }
126 
127 
129  void identity() { W=1.0; X=Y=Z=0.0; }
130 
131 
133  Quaternion conjugate() const
134  { return Quaternion(W, -X, -Y, -Z); }
135 
136 
138  Quaternion invert() const
139  { return conjugate()/Base::sqrnorm(); }
140 
141 
143  Quaternion operator*(const Quaternion& _q) const
144  { return Quaternion(W*_q.W - X*_q.X - Y*_q.Y - Z*_q.Z,
145  W*_q.X + X*_q.W + Y*_q.Z - Z*_q.Y,
146  W*_q.Y - X*_q.Z + Y*_q.W + Z*_q.X,
147  W*_q.Z + X*_q.Y - Y*_q.X + Z*_q.W); }
148 
149 
151  Quaternion& operator*=(const Quaternion& _q)
152  { return *this = *this * _q; }
153 
154 
156  template <class Vec3T>
157  Vec3T rotate(const Vec3T& _v) const
158  {
159  Quaternion q = *this * Quaternion(0,_v[0],_v[1],_v[2]) * conjugate();
160  return Vec3T(q[1], q[2], q[3]);
161  }
162 
163 
165  void axis_angle(Vec3& _axis, Scalar& _angle) const
166  {
167  if (fabs(W) > 0.999999)
168  {
169  _axis = Vec3(1,0,0);
170  _angle = 0;
171  }
172  else
173  {
174  _angle = 2.0 * acos(W);
175  _axis = Vec3(X, Y, Z).normalize();
176  }
177  }
178 
179 
180 
182  Matrix rotation_matrix() const
183  {
184  Scalar
185  ww(W*W), xx(X*X), yy(Y*Y), zz(Z*Z), wx(W*X),
186  wy(W*Y), wz(W*Z), xy(X*Y), xz(X*Z), yz(Y*Z);
187 
188  Matrix m;
189 
190  m(0,0) = ww + xx - yy - zz;
191  m(1,0) = 2.0*(xy + wz);
192  m(2,0) = 2.0*(xz - wy);
193 
194  m(0,1) = 2.0*(xy - wz);
195  m(1,1) = ww - xx + yy - zz;
196  m(2,1) = 2.0*(yz + wx);
197 
198  m(0,2) = 2.0*(xz + wy);
199  m(1,2) = 2.0*(yz - wx);
200  m(2,2) = ww - xx - yy + zz;
201 
202  m(0,3) = m(1,3) = m(2,3) = m(3,0) = m(3,1) = m(3,2) = 0.0;
203  m(3,3) = 1.0;
204 
205  return m;
206  }
207 
208 
209 
210  /*
212  Matrix right_mult_matrix() const
213  {
214  Matrix m;
215  m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
216  m(1,0) = X; m(1,1) = W; m(1,2) = -Z; m(1,3) = Y;
217  m(2,0) = Y; m(2,1) = Z; m(2,2) = W; m(2,3) = -X;
218  m(3,0) = Z; m(3,1) = -Y; m(3,2) = X; m(3,3) = W;
219  return m;
220  }
221 
222 
224  Matrix left_mult_matrix() const
225  {
226  Matrix m;
227  m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
228  m(1,0) = X; m(1,1) = W; m(1,2) = Z; m(1,3) = -Y;
229  m(2,0) = Y; m(2,1) = -Z; m(2,2) = W; m(2,3) = X;
230  m(3,0) = Z; m(3,1) = Y; m(3,2) = -X; m(3,3) = W;
231  return m;
232  }
233  */
235  Matrix right_mult_matrix() const
236  {
237  Matrix m;
238  m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
239  m(1,0) = X; m(1,1) = W; m(1,2) = Z; m(1,3) = -Y;
240  m(2,0) = Y; m(2,1) = -Z; m(2,2) = W; m(2,3) = X;
241  m(3,0) = Z; m(3,1) = Y; m(3,2) = -X; m(3,3) = W;
242  return m;
243  }
244 
245 
247  Matrix left_mult_matrix() const
248  {
249  Matrix m;
250  m(0,0) = W; m(0,1) = -X; m(0,2) = -Y; m(0,3) = -Z;
251  m(1,0) = X; m(1,1) = W; m(1,2) = -Z; m(1,3) = Y;
252  m(2,0) = Y; m(2,1) = Z; m(2,2) = W; m(2,3) = -X;
253  m(3,0) = Z; m(3,1) = -Y; m(3,2) = X; m(3,3) = W;
254  return m;
255  }
256 
257 
259  template<class MatrixT>
260  void init_from_matrix( const MatrixT& _rot)
261  {
262  Scalar trace = _rot(0,0) + _rot(1,1) + _rot(2,2);
263  if( trace > 0.0 )
264  {
265  Scalar s = 0.5 / sqrt(trace + 1.0);
266  W = 0.25 / s;
267  X = ( _rot(2,1) - _rot(1,2) ) * s;
268  Y = ( _rot(0,2) - _rot(2,0) ) * s;
269  Z = ( _rot(1,0) - _rot(0,1) ) * s;
270  }
271  else
272  {
273  if ( _rot(0,0) > _rot(1,1) && _rot(0,0) > _rot(2,2) )
274  {
275  Scalar s = 2.0 * sqrt( 1.0 + _rot(0,0) - _rot(1,1) - _rot(2,2));
276  W = (_rot(2,1) - _rot(1,2) ) / s;
277  X = 0.25 * s;
278  Y = (_rot(0,1) + _rot(1,0) ) / s;
279  Z = (_rot(0,2) + _rot(2,0) ) / s;
280  }
281  else
282  if (_rot(1,1) > _rot(2,2))
283  {
284  Scalar s = 2.0 * sqrt( 1.0 + _rot(1,1) - _rot(0,0) - _rot(2,2));
285  W = (_rot(0,2) - _rot(2,0) ) / s;
286  X = (_rot(0,1) + _rot(1,0) ) / s;
287  Y = 0.25 * s;
288  Z = (_rot(1,2) + _rot(2,1) ) / s;
289  }
290  else
291  {
292  Scalar s = 2.0 * sqrt( 1.0 + _rot(2,2) - _rot(0,0) - _rot(1,1) );
293  W = (_rot(1,0) - _rot(0,1) ) / s;
294  X = (_rot(0,2) + _rot(2,0) ) / s;
295  Y = (_rot(1,2) + _rot(2,1) ) / s;
296  Z = 0.25 * s;
297  }
298  }
299  }
300 
301 
303  Quaternion exponential() const
304  {
305  Vec3 n(X,Y,Z);
306  Scalar theta( n.norm());
307  Scalar sin_theta = sin(theta);
308  Scalar cos_theta = cos(theta);
309 
310  if( theta > 1e-6 )
311  n *= sin_theta/theta;
312  else
313  n = Vec3(0,0,0);
314 
315  return Quaternion( cos_theta, n[0], n[1], n[2]);
316  }
317 
318 
320  Quaternion logarithm() const
321  {
322  // clamp to valid input
323  double w = W;
324  if( w > 1.0) w = 1.0;
325  else if( w < -1.0) w = -1.0;
326 
327  Scalar theta_half = acos(w);
328 
329  Vec3 n(X,Y,Z);
330  Scalar n_norm( n.norm());
331 
332  if( n_norm > 1e-6 )
333  n *= theta_half/n_norm;
334  else
335  n = Vec3(0,0,0);
336 
337  return Quaternion( 0, n[0], n[1], n[2]);
338  }
339 
340  void print_info()
341  {
342  // get axis, angle and matrix
343  Vec3 axis;
344  Scalar angle;
345  this->axis_angle( axis, angle);
346  Matrix m;
347  m = this->rotation_matrix();
348 
349  std::cerr << "quaternion : " << (*this) << std::endl;
350  std::cerr << "length : " << this->norm() << std::endl;
351  std::cerr << "axis, angle: " << axis << ", " << angle*180.0/M_PI << "\n";
352  std::cerr << "rot matrix :\n";
353  std::cerr << m << std::endl;
354  }
355 
356 
357 #undef W
358 #undef X
359 #undef Y
360 #undef Z
361 };
362 
363 
364 typedef QuaternionT<float> Quaternionf;
365 typedef QuaternionT<double> Quaterniond;
366 
367 
368 //=============================================================================
369 } // namespace ACG
370 //=============================================================================
371 #endif // ACG_QUATERNION_HH defined
372 //=============================================================================
373 
Quaternion logarithm() const
quaternion logarithm (for unit quaternions)
Definition: QuaternionT.hh:320
Namespace providing different geometric functions concerning angles.
Definition: DBSCANT.cc:51
void identity()
identity rotation
Definition: QuaternionT.hh:129
Quaternion operator*(const Quaternion &_q) const
quaternion * quaternion
Definition: QuaternionT.hh:143
Matrix rotation_matrix() const
cast to rotation matrix
Definition: QuaternionT.hh:182
Quaternion invert() const
invert quaternion
Definition: QuaternionT.hh:138
Matrix left_mult_matrix() const
get matrix for mult from left (q*p = Qp)
Definition: QuaternionT.hh:247
QuaternionT(Vec3 _axis, Scalar _angle)
construct from rotation axis and angle (in radians)
Definition: QuaternionT.hh:111
QuaternionT(const Vec3 &_p)
construct from 3D point (pure imaginary quaternion)
Definition: QuaternionT.hh:103
Vec3T rotate(const Vec3T &_v) const
rotate vector
Definition: QuaternionT.hh:157
Quaternion conjugate() const
conjugate quaternion
Definition: QuaternionT.hh:133
QuaternionT(const Vec4 &_p)
construct from 4D vector
Definition: QuaternionT.hh:107
void axis_angle(Vec3 &_axis, Scalar &_angle) const
get rotation axis and angle (only valid for unit quaternions!)
Definition: QuaternionT.hh:165
Quaternion & operator*=(const Quaternion &_q)
quaternion *= quaternion
Definition: QuaternionT.hh:151
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:99
void init_from_matrix(const MatrixT &_rot)
get quaternion from rotation matrix
Definition: QuaternionT.hh:260
QuaternionT(const MatrixT &_rot)
construct from rotation matrix (only valid for rotation matrices!)
Definition: QuaternionT.hh:124
Matrix right_mult_matrix() const
get matrix for mult from right (p*q = Qp)
Definition: QuaternionT.hh:235
Quaternion exponential() const
quaternion exponential (for unit quaternions)
Definition: QuaternionT.hh:303