vmmlib  1.11.0
Templatized C++ vector and matrix math library
quaternion.hpp
1 /*
2  * Copyright (c) 2006-2016, Visualization and Multimedia Lab,
3  * University of Zurich <http://vmml.ifi.uzh.ch>,
4  * Eyescale Software GmbH,
5  * Blue Brain Project, EPFL
6  *
7  * This file is part of VMMLib <https://github.com/VMML/vmmlib/>
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * Redistributions of source code must retain the above copyright notice, this
13  * list of conditions and the following disclaimer. Redistributions in binary
14  * form must reproduce the above copyright notice, this list of conditions and
15  * the following disclaimer in the documentation and/or other materials provided
16  * with the distribution. Neither the name of the Visualization and Multimedia
17  * Lab, University of Zurich nor the names of its contributors may be used to
18  * endorse or promote products derived from this software without specific prior
19  * written permission.
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
24  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef __VMML__QUATERNION__HPP__
34 #define __VMML__QUATERNION__HPP__
35 
36 #include <vmmlib/enable_if.hpp>
37 #include <vmmlib/types.hpp>
38 #include <vmmlib/vector.hpp> // used inline
39 
40 #include <algorithm>
41 #include <cmath>
42 #include <cstdlib>
43 #include <iomanip>
44 #include <iostream>
45 #include <limits>
46 
47 namespace vmml
48 {
49 template < typename T > class Quaternion
50 {
51 public:
53  Quaternion() : array() { array[3] = 1.; }
54  Quaternion( T x, T y, T z, T w );
55 
57  Quaternion( T angle, vector< 3, T > axis );
58 
59  // uses the top-left 3x3 part of the supplied matrix as rotation matrix
60  template< size_t M >
61  Quaternion( const Matrix< M, M, T >& rotation_matrix_,
62  typename enable_if< M >= 3 >::type* = 0 );
63 
67  bool equals( const Quaternion& other,
68  T tolerance = std::numeric_limits< T >::epsilon( )) const;
69 
70  T x() const { return array[0]; }
71  T y() const { return array[1]; }
72  T z() const { return array[2]; }
73  T w() const { return array[3]; }
74 
76  bool operator==( const Quaternion& a ) const;
77 
79  bool operator!=( const Quaternion& a ) const;
80 
82  Quaternion operator-() const;
83 
85  Quaternion inverse() const;
86 
87  Quaternion getConjugate() const;
88 
89  T abs() const;
90  T absSquare() const;
91 
93  Matrix< 3, 3, T > getRotationMatrix() const;
95 
96  void normalize();
97 
98  Quaternion& operator=(const Quaternion& other);
99 
102  Quaternion operator+( const Quaternion< T >& a ) const;
103  Quaternion operator-( const Quaternion< T >& a ) const;
104 
105  // caution: a * q != q * a in general
106  Quaternion operator*( const Quaternion< T >& a ) const;
107  void operator+=( const Quaternion< T >& a );
108  void operator-=( const Quaternion< T >& a );
109 
110  // caution: a *= q != q *= a in general
111  void operator*=( const Quaternion< T >& a );
113 
116  Quaternion operator*( T a ) const;
117  Quaternion operator/( T a ) const;
118 
119  void operator*=( T a );
120  void operator/=( T a );
122 
123  friend std::ostream& operator<< ( std::ostream& os, const Quaternion& q )
124  {
125  const std::ios::fmtflags flags = os.flags();
126  const int prec = os.precision();
127 
128  os.setf( std::ios::right, std::ios::adjustfield );
129  os.precision( 5 );
130 
131  os << "( "
132  << std::setw(10) << q.x() << " "
133  << std::setw(10) << q.y() << " "
134  << std::setw(10) << q.z() << " "
135  << std::setw(10) << q.w() << " "
136  << ")";
137 
138  os.precision( prec );
139  os.setf( flags );
140  return os;
141  }
142 
143 private:
144  T array[4];
145 };
146 }
147 
148 #include <vmmlib/matrix.hpp>
149 
150 namespace vmml
151 {
154 template < typename T >
155 T dot( const Quaternion< T >& p, const Quaternion< T >& q )
156 {
157  return p.array[3] * q.array[3] + p.array[0] * q.array[0] +
158  p.array[1] * q.array[1] + p.array[2] * q.array[2];
159 }
160 
161 template < typename T >
162 vector< 3, T > cross( const Quaternion< T >& p, const Quaternion< T >& q )
163 {
164  return vector< 3, T >( p.array[1] * q.array[2] - p.array[2] * q.array[1],
165  p.array[2] * q.array[0] - p.array[0] * q.array[2],
166  p.array[0] * q.array[1] - p.array[1] * q.array[0] );
167 }
169 
170 template < typename T > Quaternion< T >::Quaternion( T x_, T y_, T z_, T w_ )
171 {
172  array[0] = x_;
173  array[1] = y_;
174  array[2] = z_;
175  array[3] = w_;
176 }
177 
178 template< typename T > template< size_t M >
179 Quaternion< T >::Quaternion( const Matrix< M, M, T >& rot,
180  typename enable_if< M >= 3 >::type* )
181 {
182  const T trace = rot( 0, 0 ) + rot( 1, 1 ) + rot( 2,2 ) + T( 1 );
183  static const T TRACE_EPSILON = 1e-5;
184 
185  // very small traces may introduce a big numerical error
186  if( trace > TRACE_EPSILON )
187  {
188  T s = 0.5 / std::sqrt( trace );
189  array[0] = rot( 2, 1 ) - rot( 1, 2 );
190  array[0] *= s;
191 
192  array[1] = rot( 0, 2 ) - rot( 2, 0 );
193  array[1] *= s;
194 
195  array[2] = rot( 1, 0 ) - rot( 0, 1 );
196  array[2] *= s;
197 
198  array[3] = 0.25 / s;
199  }
200  else
201  {
202  const vector< 3, T > diag( rot( 0, 0 ), rot( 1, 1 ), rot( 2, 2 ) );
203  const size_t largest = diag.find_max_index();
204 
205  // 0, 0 is largest
206  if ( largest == 0 )
207  {
208  const T s = 0.5 / std::sqrt( T( 1 ) + rot( 0,0 ) - rot( 1,1 ) -
209  rot( 2,2 ));
210  array[0] = 0.25 / s;
211 
212  array[1] = rot( 0,1 ) + rot( 1,0 );
213  array[1] *= s;
214 
215  array[2] = rot( 0,2 ) + rot( 2,0 );
216  array[2] *= s;
217 
218  array[3] = rot( 1,2 ) - rot( 2,1 );
219  array[3] *= s;
220  }
221  else if ( largest == 1 )
222  {
223  const T s = 0.5 / std::sqrt( T( 1 ) + rot( 1,1 ) - rot( 0,0 ) -
224  rot( 2,2 ));
225  array[0] = rot( 0,1 ) + rot( 1,0 );
226  array[0] *= s;
227 
228  array[1] = 0.25 / s;
229 
230  array[2] = rot( 1,2 ) + rot( 2,1 );
231  array[2] *= s;
232 
233  array[3] = rot( 0,2 ) - rot( 2,0 );
234  array[3] *= s;
235  }
236  // 2, 2 is largest
237  else if ( largest == 2 )
238  {
239  const T s = 0.5 / std::sqrt( T( 1 ) + rot( 2,2 ) - rot( 0,0 ) -
240  rot( 1,1 ));
241  array[0] = rot( 0,2 ) + rot( 2,0 );
242  array[0] *= s;
243 
244  array[1] = rot( 1,2 ) + rot( 2,1 );
245  array[1] *= s;
246 
247  array[2] = 0.25 / s;
248 
249  array[3] = rot( 0,1 ) - rot( 1,0 );
250  array[3] *= s;
251  }
252  else
253  {
254  throw std::runtime_error( "no clue why, but should not get here" );
255  }
256  }
257 }
258 
259 
260 template< typename T >
262 {
263  axis.normalize();
264  const T sinAngle2 = std::sin( angle * 0.5 );
265  array[0] = axis.x() * sinAngle2;
266  array[1] = axis.y() * sinAngle2;
267  array[2] = axis.z() * sinAngle2;
268  array[3] = std::cos( angle * 0.5 );
269 }
270 
271 template< typename T >
272 bool Quaternion< T >::equals( const Quaternion& other, const T tolerance ) const
273 {
274  return std::abs( array[0] - other.array[0] ) <= tolerance &&
275  std::abs( array[1] - other.array[1] ) <= tolerance &&
276  std::abs( array[2] - other.array[2] ) <= tolerance &&
277  std::abs( array[3] - other.array[3] ) <= tolerance;
278 }
279 
280 template < typename T >
281 bool Quaternion< T >::operator==( const Quaternion& rhs ) const
282 {
283  return array[0] == rhs.array[0] && array[1] == rhs.array[1] &&
284  array[2] == rhs.array[2] && array[3] == rhs.array[3];
285 }
286 
287 template < typename T >
289 {
290  return ! this->operator==( a );
291 }
292 
293 template < typename T > Quaternion< T > Quaternion< T >::getConjugate() const
294 {
295  return Quaternion< T >( -array[0], -array[1], -array[2], array[3] );
296 }
297 
298 template < typename T > T Quaternion< T >::abs() const
299 {
300  return std::sqrt( absSquare( ));
301 }
302 
303 template < typename T >T Quaternion< T >::absSquare() const
304 {
305  return array[0] * array[0] + array[1] * array[1] +
306  array[2] * array[2] + array[3] * array[3];
307 }
308 
309 template < typename T > Quaternion< T > Quaternion< T >::inverse() const
310 {
311  const Quaternion< T >& q = getConjugate();
312  const T tmp = T( 1 ) / absSquare();
313  return q * tmp;
314 }
315 
316 template < typename T > void Quaternion< T >::normalize()
317 {
318  T len = abs();
319  if( len == T( 0 ))
320  return;
321  len = T( 1 ) / len;
322  this->operator*=( len );
323 }
324 
325 //
326 // Quaternion/Quaternion operations
327 //
328 
329 template < typename T >
330 Quaternion< T > Quaternion< T >::operator+( const Quaternion< T >& rhs ) const
331 {
332  return Quaternion( array[0] + rhs.array[0], array[1] + rhs.array[1],
333  array[2] + rhs.array[2], array[3] + rhs.array[3] );
334 }
335 
336 template < typename T >
337 Quaternion< T > Quaternion< T >::operator-( const Quaternion< T >& rhs ) const
338 {
339  return Quaternion( array[0] - rhs.array[0], array[1] - rhs.array[1],
340  array[2] - rhs.array[2], array[3] - rhs.array[3] );
341 }
342 
343 // returns Grasssmann product
344 template < typename T >
345 Quaternion< T > Quaternion< T >::operator*( const Quaternion< T >& rhs ) const
346 {
347  Quaternion< T > ret( *this );
348  ret *= rhs;
349  return ret;
350 }
351 
352 // Grassmann product
353 template < typename T >
354 void Quaternion< T >::operator*=( const Quaternion< T >& q )
355 {
356  // optimized version, 7 less mul, but 15 more add/subs
357  // after Henrik Engstrom, from a gamedev.net article.
358  const T& a_ = array[ 3 ];
359  const T& b_ = array[ 0 ];
360  const T& c = array[ 1 ];
361  const T& d = array[ 2 ];
362  const T& _x = q.array[ 3 ];
363  const T& _y = q.array[ 0 ];
364  const T& _z = q.array[ 1 ];
365  const T& _w = q.array[ 2 ];
366 
367  const T tmp_00 = (d - c) * (_z - _w);
368  const T tmp_01 = (a_ + b_) * (_x + _y);
369  const T tmp_02 = (a_ - b_) * (_z + _w);
370  const T tmp_03 = (c + d) * (_x - _y);
371  const T tmp_04 = (d - b_) * (_y - _z);
372  const T tmp_05 = (d + b_) * (_y + _z);
373  const T tmp_06 = (a_ + c) * (_x - _w);
374  const T tmp_07 = (a_ - c) * (_x + _w);
375  const T tmp_08 = tmp_05 + tmp_06 + tmp_07;
376  const T tmp_09 = 0.5 * (tmp_04 + tmp_08);
377 
378  array[ 0 ] = tmp_01 + tmp_09 - tmp_08;
379  array[ 1 ] = tmp_02 + tmp_09 - tmp_07;
380  array[ 2 ] = tmp_03 + tmp_09 - tmp_06;
381  array[ 3 ] = tmp_00 + tmp_09 - tmp_05;
382 }
383 
384 template < typename T > Quaternion< T > Quaternion< T >::operator-() const
385 {
386  return Quaternion( -array[0], -array[1], -array[2], -array[3] );
387 }
388 
389 template < typename T >
391 {
392  array[ 0 ] += q.array[ 0 ];
393  array[ 1 ] += q.array[ 1 ];
394  array[ 2 ] += q.array[ 2 ];
395  array[ 3 ] += q.array[ 3 ];
396 }
397 
398 template < typename T >
399 void Quaternion< T >::operator-=( const Quaternion< T >& q )
400 {
401  array[ 0 ] -= q.array[ 0 ];
402  array[ 1 ] -= q.array[ 1 ];
403  array[ 2 ] -= q.array[ 2 ];
404  array[ 3 ] -= q.array[ 3 ];
405 }
406 
407 //
408 // Quaternion/scalar operations
409 //
410 
411 template < typename T >
412 Quaternion< T > Quaternion< T >::operator*( const T a_ ) const
413 {
414  return Quaternion( array[0] * a_, array[1] * a_,
415  array[2] * a_, array[3] * a_ );
416 }
417 
418 template < typename T >
419 Quaternion< T > Quaternion< T >::operator/( T a_ ) const
420 {
421  if ( a_ == T( 0 ))
422  throw std::runtime_error( "Division by zero." );
423 
424  a_ = T( 1 ) / a_;
425  return Quaternion( array[0] * a_, array[1] * a_, array[2] * a_,
426  array[3] * a_ );
427 }
428 
429 template < typename T > void Quaternion< T >::operator*=( T q )
430 {
431  array[ 0 ] *= q;
432  array[ 1 ] *= q;
433  array[ 2 ] *= q;
434  array[ 3 ] *= q;
435 }
436 
437 template < typename T > void Quaternion< T >::operator/=( T q )
438 {
439  if ( q == T( 0 ))
440  throw std::runtime_error( "Division by zero" );
441 
442  q = T( 1 ) / q;
443  this->operator*=( q );
444 }
445 
446 template < typename T >
448 {
449  const T w2 = array[3] * array[3];
450  const T x2 = array[0] * array[0];
451  const T y2 = array[1] * array[1];
452  const T z2 = array[2] * array[2];
453  const T wx = array[3] * array[0];
454  const T wy = array[3] * array[1];
455  const T wz = array[3] * array[2];
456  const T xy = array[0] * array[1];
457  const T xz = array[0] * array[2];
458  const T yz = array[1] * array[2];
459 
460  Matrix< 3, 3, T > matrix;
461  matrix( 0, 0 ) = w2 + x2 - y2 - z2;
462  matrix( 0, 1 ) = 2. * (xy - wz);
463  matrix( 0, 2 ) = 2. * (xz + wy);
464  matrix( 1, 0 ) = 2. * (xy + wz);
465  matrix( 1, 1 ) = w2 - x2 + y2 - z2;
466  matrix( 1, 2 ) = 2. * (yz - wx);
467  matrix( 2, 0 ) = 2. * (xz - wy);
468  matrix( 2, 1 ) = 2. * (yz + wx);
469  matrix( 2, 2 ) = w2 - x2 - y2 + z2;
470  return matrix;
471 }
472 
473 template < typename T >
475 {
476  ::memcpy( array, other.array, 4 * sizeof( T ) );
477  return *this;
478 }
479 
480 }
481 #endif
bool equals(const Quaternion &other, T tolerance=std::numeric_limits< T >::epsilon()) const
Definition: quaternion.hpp:272
heavily inspired by boost::enable_if http://www.boost.org, file: boost/utility/enable_if.hpp, Copyright 2003 Jaakko Järvi, Jeremiah Willcock, Andrew Lumsdaine
Definition: aabb.hpp:40
bool operator==(const Quaternion &a) const
Definition: quaternion.hpp:281
Matrix< 3, 3, T > getRotationMatrix() const
Definition: quaternion.hpp:447
Quaternion()
Construct an identity quaternion.
Definition: quaternion.hpp:53
Quaternion operator-() const
Definition: quaternion.hpp:384
bool operator!=(const Quaternion &a) const
Definition: quaternion.hpp:288
Matrix with R rows and C columns of element type T.
Definition: matrix.hpp:51
Quaternion inverse() const
Definition: quaternion.hpp:309