vmmlib  1.10.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/math.hpp>
38 #include <vmmlib/types.hpp>
39 #include <vmmlib/vector.hpp> // used inline
40 
41 #include <algorithm>
42 #include <cassert>
43 #include <cmath>
44 #include <cstdlib>
45 #include <iomanip>
46 #include <iostream>
47 #include <limits>
48 
49 
50 // - declaration - //
51 
52 #define QUATERNION_TRACE_EPSILON 1e-5
53 
54 namespace vmml
55 {
56 
57 template < typename T > class quaternion
58 {
59 public:
60  typedef vector< 3, T > vec3;
61 
63  quaternion() : array() { array[3] = 1.; }
64  quaternion( T x, T y, T z, T w );
65 
67  quaternion( T angle, vec3 axis );
68 
69  // uses the top-left 3x3 part of the supplied matrix as rotation matrix
70  template< size_t M >
71  quaternion( const matrix< M, M, T >& rotation_matrix_,
72  typename enable_if< M >= 3 >::type* = 0 );
73 
75  bool equals( const quaternion& other,
76  T tolerance = std::numeric_limits< T >::epsilon( )) const;
77 
78  T x() const { return array[0]; }
79  T y() const { return array[1]; }
80  T z() const { return array[2]; }
81  T w() const { return array[3]; }
82 
83  void zero();
84  void identity();
85 
86  template< size_t D > void set( const matrix< D, D, T >& rotation_matrix_ );
87 
88  bool operator==( const T& a ) const;
89  bool operator!=( const T& a ) const;
90 
91  bool operator==( const quaternion& a ) const;
92  bool operator!=( const quaternion& a ) const;
93 
94  bool is_akin( const quaternion& a,
95  const T& delta = std::numeric_limits< T >::epsilon() );
96 
97  void conjugate();
98  quaternion get_conjugate() const;
99 
100  T abs() const;
101  T squared_abs() const;
102 
103  T normalize();
104  quaternion get_normalized() const;
105 
106  quaternion negate() const;
107  quaternion operator-() const;
108 
109  quaternion& operator=(const quaternion& other);
110 
111  //
112  // quaternion/quaternion operations
113  //
114  quaternion operator+( const quaternion< T >& a ) const;
115  quaternion operator-( const quaternion< T >& a ) const;
116  // caution: a * q != q * a in general
117  quaternion operator*( const quaternion< T >& a ) const;
118  void operator+=( const quaternion< T >& a );
119  void operator-=( const quaternion< T >& a );
120  // caution: a *= q != q *= a in general
121  void operator*=( const quaternion< T >& a );
122 
123  //
124  // quaternion/scalar operations
125  //
126  quaternion operator*( T a ) const;
127  quaternion operator/( T a ) const;
128 
129  void operator*=( T a );
130  void operator/=( T a );
131 
132  // vec3 = this x b
133  vector< 3, T > cross( const quaternion< T >& b ) const;
134 
135  T dot( const quaternion< T >& a ) const;
136  static T dot( const quaternion< T >& a, const quaternion< T >& b );
137 
138  // returns multiplicative inverse
139  quaternion inverse();
140 
141  void normal( const quaternion& aa, const quaternion& bb, const quaternion& cc, const quaternion& dd );
142  quaternion normal( const quaternion& aa, const quaternion& bb, const quaternion& cc );
143 
144  static quaternion slerp( T a, const quaternion& p,
145  const quaternion& q, const T epsilon = 1e-13 );
146 
147  matrix< 3, 3, T > get_rotation_matrix() const;
148 
149  template< size_t D > void get_rotation_matrix( matrix< D, D, T >& result ) const;
150 
151  static const quaternion IDENTITY;
152  static const quaternion QUATERI;
153  static const quaternion QUATERJ;
154  static const quaternion QUATERK;
155 
156  friend std::ostream& operator<< ( std::ostream& os, const quaternion& q )
157  {
158  const std::ios::fmtflags flags = os.flags();
159  const int prec = os.precision();
160 
161  os.setf( std::ios::right, std::ios::adjustfield );
162  os.precision( 5 );
163 
164  os << "( "
165  << std::setw(10) << q.x() << " "
166  << std::setw(10) << q.y() << " "
167  << std::setw(10) << q.z() << " "
168  << std::setw(10) << q.w() << " "
169  << ")";
170 
171  os.precision( prec );
172  os.setf( flags );
173  return os;
174  }
175 
176 private:
177  T array[4];
178 };
179 }
180 
181 #include <vmmlib/matrix.hpp>
182 
183 namespace vmml
184 {
185 // - implementation - //
186 
187 template < typename T >
188 const quaternion< T > quaternion< T >::IDENTITY( 0, 0, 0, 1 );
189 
190 template < typename T >
191 const quaternion< T > quaternion< T >::QUATERI( 1, 0, 0, 0 );
192 
193 template < typename T >
194 const quaternion< T > quaternion< T >::QUATERJ( 0, 1, 0, 0 );
195 
196 template < typename T >
197 const quaternion< T > quaternion< T >::QUATERK( 0, 0, 1, 0 );
198 
199 template < typename T >
200 quaternion< T >::quaternion( T x_, T y_, T z_, T w_ )
201 {
202  array[0] = x_;
203  array[1] = y_;
204  array[2] = z_;
205  array[3] = w_;
206 }
207 
208 
209 template< typename T > template< size_t M >
210 quaternion< T >::quaternion( const matrix< M, M, T >& rotation_matrix_,
211  typename enable_if< M >= 3 >::type* )
212 {
213  this->template set< M >( rotation_matrix_ );
214 }
215 
216 template< typename T >
217 quaternion< T >::quaternion( const T angle, vec3 axis )
218 {
219  axis.normalize();
220  const T sinAngle2 = std::sin( angle * 0.5 );
221  array[0] = axis.x() * sinAngle2;
222  array[1] = axis.y() * sinAngle2;
223  array[2] = axis.z() * sinAngle2;
224  array[3] = std::cos( angle * 0.5 );
225 }
226 
227 template< typename T >
228 bool quaternion< T >::equals( const quaternion& other, const T tolerance ) const
229 {
230  return std::abs( array[0] - other.array[0] ) <= tolerance &&
231  std::abs( array[1] - other.array[1] ) <= tolerance &&
232  std::abs( array[2] - other.array[2] ) <= tolerance &&
233  std::abs( array[3] - other.array[3] ) <= tolerance;
234 }
235 
236 
237 // top-left 3x3 is interpreted as rot matrix.
238 template < typename T > template< size_t D >
240 {
241  T trace = M( 0, 0 ) + M( 1, 1 ) + M( 2,2 ) + 1.0;
242 
243  // very small traces may introduce a big numerical error
244  if( trace > QUATERNION_TRACE_EPSILON )
245  {
246  T s = 0.5 / std::sqrt( trace );
247  array[0] = M( 2, 1 ) - M( 1, 2 );
248  array[0] *= s;
249 
250  array[1] = M( 0, 2 ) - M( 2, 0 );
251  array[1] *= s;
252 
253  array[2] = M( 1, 0 ) - M( 0, 1 );
254  array[2] *= s;
255 
256  array[3] = 0.25 / s;
257  }
258  else
259  {
260  vector< 3, T > diag( M( 0, 0 ), M( 1, 1 ), M( 2, 2 ) );
261  size_t largest = diag.find_max_index();
262 
263  // 0, 0 is largest
264  if ( largest == 0 )
265  {
266  T s = 0.5 / std::sqrt( 1.0 + M( 0, 0 ) - M( 1, 1 ) - M( 2, 2 ) );
267  array[0] = 0.25 / s;
268 
269  array[1] = M( 0,1 ) + M( 1,0 );
270  array[1] *= s;
271 
272  array[2] = M( 0,2 ) + M( 2,0 );
273  array[2] *= s;
274 
275  array[3] = M( 1,2 ) - M( 2,1 );
276  array[3] *= s;
277  }
278  else if ( largest == 1 )
279  {
280  T s = 0.5 / std::sqrt( 1.0 + M( 1,1 ) - M( 0,0 ) - M( 2,2 ) );
281  array[0] = M( 0,1 ) + M( 1,0 );
282  array[0] *= s;
283 
284  array[1] = 0.25 / s;
285 
286  array[2] = M( 1,2 ) + M( 2,1 );
287  array[2] *= s;
288 
289  array[3] = M( 0,2 ) - M( 2,0 );
290  array[3] *= s;
291  }
292  // 2, 2 is largest
293  else if ( largest == 2 )
294  {
295  T s = 0.5 / std::sqrt( 1.0 + M( 2,2 ) - M( 0,0 ) - M( 1,1 ) );
296  array[0] = M( 0,2 ) + M( 2,0 );
297  array[0] *= s;
298 
299  array[1] = M( 1,2 ) + M( 2,1 );
300  array[1] *= s;
301 
302  array[2] = 0.25 / s;
303 
304  array[3] = M( 0,1 ) - M( 1,0 );
305  array[3] *= s;
306  }
307  else
308  {
309  throw std::runtime_error( "no clue why, but should not get here" );
310  }
311  }
312 }
313 
314 
315 
316 template < typename T >
317 void quaternion< T >::zero()
318 {
319  ::memset( array, 0, sizeof( array ));
320 }
321 
322 
323 
324 template < typename T >
325 void quaternion< T >::identity()
326 {
327  (*this) = IDENTITY;
328 }
329 
330 template < typename T >
331 bool quaternion< T >::operator==( const quaternion& rhs ) const
332 {
333  return (
334  array[3] == rhs.array[3] &&
335  array[0] == rhs.array[0] &&
336  array[1] == rhs.array[1] &&
337  array[2] == rhs.array[2]
338  );
339 }
340 
341 
342 
343 template < typename T >
344 bool
345 quaternion< T >::operator!=( const quaternion& a ) const
346 {
347  return ! this->operator==( a );
348 }
349 
350 template < typename T >
351 void quaternion< T >::conjugate()
352 {
353  array[0] = -array[0];
354  array[1] = -array[1];
355  array[2] = -array[2];
356 }
357 
358 
359 
360 template < typename T >
361 quaternion< T > quaternion< T >::get_conjugate() const
362 {
363  return quaternion< T > ( -array[0], -array[1], -array[2], array[3] );
364 }
365 
366 
367 
368 template < typename T >
369 T
370 quaternion< T >::abs() const
371 {
372  return std::sqrt( squared_abs() );
373 }
374 
375 
376 
377 template < typename T >
378 T quaternion< T >::squared_abs() const
379 {
380  return array[0] * array[0] + array[1] * array[1] + array[2] * array[2] + array[3] * array[3];
381 }
382 
383 
384 
385 template < typename T >
386 quaternion< T > quaternion< T >::inverse()
387 {
388  quaternion< T > q( *this );
389  q.conjugate();
390 
391  T tmp = squared_abs();
392  tmp = static_cast< T >( 1.0 ) / tmp;
393  return q * tmp;
394 }
395 
396 
397 
398 template < typename T >
399 T quaternion< T >::normalize()
400 {
401  T len = abs();
402  if( len == 0.0 )
403  return 0.0;
404  len = 1.0f / len;
405  this->operator*=( len );
406  return len;
407 }
408 
409 
410 
411 template < typename T >
412 quaternion< T >
413 quaternion< T >::get_normalized() const
414 {
415  quaternion< T > q( *this );
416  q.normalize();
417  return q;
418 }
419 
420 
421 
422 //
423 // quaternion/quaternion operations
424 //
425 
426 template < typename T >
427 quaternion< T >
428 quaternion< T >::operator+( const quaternion< T >& rhs ) const
429 {
430  return quaternion( array[0] + rhs.array[0], array[1] + rhs.array[1], array[2] + rhs.array[2], array[3] + rhs.array[3] );
431 }
432 
433 
434 
435 template < typename T >
436 quaternion< T >
437 quaternion< T >::operator-( const quaternion< T >& rhs ) const
438 {
439  return quaternion( array[0] - rhs.array[0], array[1] - rhs.array[1], array[2] - rhs.array[2], array[3] - rhs.array[3] );
440 }
441 
442 
443 
444 // returns Grasssmann product
445 template < typename T >
446 quaternion< T > quaternion< T >::operator*( const quaternion< T >& rhs ) const
447 {
448  quaternion< T > ret( *this );
449  ret *= rhs;
450  return ret;
451 }
452 
453 
454 
455 // Grassmann product
456 template < typename T >
457 void quaternion< T >::operator*=( const quaternion< T >& q )
458 {
459  #if 0
460  quaternion< T > orig( *this );
461  array[0] = orig.array[3] * a.array[0] + orig.array[0] * a.array[3] + orig.array[1] * a.array[2] - orig.array[2] * a.array[1];
462  array[1] = orig.array[3] * a.array[1] + orig.array[1] * a.array[3] + orig.array[2] * a.array[0] - orig.array[0] * a.array[2];
463  array[2] = orig.array[3] * a.array[2] + orig.array[2] * a.array[3] + orig.array[0] * a.array[1] - orig.array[1] * a.array[0];
464  array[3] = orig.array[3] * a.array[3] - orig.array[0] * a.array[0] - orig.array[1] * a.array[1] - orig.array[2] * a.array[2];
465  #else
466 
467  // optimized version, 7 less mul, but 15 more add/subs
468  // after Henrik Engstrom, from a gamedev.net article.
469 
470  const T& a_ = array[ 3 ];
471  const T& b_ = array[ 0 ];
472  const T& c = array[ 1 ];
473  const T& d = array[ 2 ];
474  const T& _x = q.array[ 3 ];
475  const T& _y = q.array[ 0 ];
476  const T& _z = q.array[ 1 ];
477  const T& _w = q.array[ 2 ];
478 
479  const T tmp_00 = (d - c) * (_z - _w);
480  const T tmp_01 = (a_ + b_) * (_x + _y);
481  const T tmp_02 = (a_ - b_) * (_z + _w);
482  const T tmp_03 = (c + d) * (_x - _y);
483  const T tmp_04 = (d - b_) * (_y - _z);
484  const T tmp_05 = (d + b_) * (_y + _z);
485  const T tmp_06 = (a_ + c) * (_x - _w);
486  const T tmp_07 = (a_ - c) * (_x + _w);
487  const T tmp_08 = tmp_05 + tmp_06 + tmp_07;
488  const T tmp_09 = 0.5 * (tmp_04 + tmp_08);
489 
490  array[ 3 ] = tmp_00 + tmp_09 - tmp_05;
491  array[ 0 ] = tmp_01 + tmp_09 - tmp_08;
492  array[ 1 ] = tmp_02 + tmp_09 - tmp_07;
493  array[ 2 ] = tmp_03 + tmp_09 - tmp_06;
494 
495  #endif
496 }
497 
498 
499 
500 
501 
502 template < typename T >
503 quaternion< T >
504 quaternion< T >::operator-() const
505 {
506  return quaternion( -array[0], -array[1], -array[2], -array[3] );
507 }
508 
509 
510 
511 template < typename T >
512 void quaternion< T >::operator+=( const quaternion< T >& q )
513 {
514  array[ 0 ] += q.array[ 0 ];
515  array[ 1 ] += q.array[ 1 ];
516  array[ 2 ] += q.array[ 2 ];
517  array[ 3 ] += q.array[ 3 ];
518 }
519 
520 
521 
522 template < typename T >
523 void quaternion< T >::operator-=( const quaternion< T >& q )
524 {
525  array[ 0 ] -= q.array[ 0 ];
526  array[ 1 ] -= q.array[ 1 ];
527  array[ 2 ] -= q.array[ 2 ];
528  array[ 3 ] -= q.array[ 3 ];
529 }
530 
531 
532 
533 //
534 // quaternion/scalar operations
535 //
536 
537 template < typename T >
538 quaternion< T >
539 quaternion< T >::operator*( const T a_ ) const
540 {
541  return quaternion( array[0] * a_, array[1] * a_, array[2] * a_, array[3] * a_ );
542 }
543 
544 
545 
546 template < typename T >
547 quaternion< T >
548 quaternion< T >::operator/( T a_ ) const
549 {
550  if ( a_ == 0.0 )
551  throw std::runtime_error( "Division by zero." );
552 
553  a_ = 1.0 / a_;
554  return quaternion( array[0] * a_, array[1] * a_, array[2] * a_, array[3] * a_ );
555 }
556 
557 
558 
559 template < typename T >
560 void quaternion< T >::operator*=( T q )
561 {
562  array[ 0 ] *= q;
563  array[ 1 ] *= q;
564  array[ 2 ] *= q;
565  array[ 3 ] *= q;
566 }
567 
568 
569 
570 template < typename T >
571 void quaternion< T >::operator/=( T q )
572 {
573  if ( q == 0.0 )
574  throw std::runtime_error( "Division by zero" );
575 
576  q = 1.0f / q;
577  this->operator*=( q );
578 }
579 
580 
581 template < typename T >
582 vector< 3, T > quaternion< T >::cross( const quaternion< T >& bb ) const
583 {
584  vector< 3, T > result;
585 
586  result.array[ 0 ] = array[1] * bb.array[2] - array[2] * bb.array[1];
587  result.array[ 1 ] = array[2] * bb.array[0] - array[0] * bb.array[2];
588  result.array[ 2 ] = array[0] * bb.array[1] - array[1] * bb.array[0];
589 
590  return result;
591 }
592 
593 
594 
595 template < typename T >
596 T quaternion< T >::dot( const quaternion< T >& q ) const
597 {
598  return array[3] * q.array[3] + array[0] * q.array[0] + array[1] * q.array[1] + array[2] * q.array[2];
599 }
600 
601 
602 
603 template < typename T >
604 T quaternion< T >::
605 dot( const quaternion< T >& p, const quaternion< T >& q )
606 {
607  return p.array[3] * q.array[3] + p.array[0] * q.array[0] + p.array[1] * q.array[1] + p.array[2] * q.array[2];
608 }
609 
610 
611 
612 template < typename T >
613 void quaternion< T >::normal( const quaternion< T >& aa,
614  const quaternion< T >& bb,
615  const quaternion< T >& cc,
616  const quaternion< T >& dd )
617 {
618  //right hand system, CCW triangle
619  const quaternion< T > quat_t = bb - aa;
620  const quaternion< T > quat_u = cc - aa;
621  const quaternion< T > quat_v = dd - aa;
622  cross( quat_t );
623  cross( quat_u );
624  cross( quat_v );
625  normalize();
626 }
627 
628 
629 
630 template < typename T >
631 quaternion< T > quaternion< T >::normal( const quaternion< T >& aa,
632  const quaternion< T >& bb,
633  const quaternion< T >& cc )
634 {
635  quaternion< T > tmp;
636  tmp.normal( *this, aa, bb, cc );
637  return tmp;
638 }
639 
640 template < typename T >
641 matrix< 3, 3, T >
642 quaternion< T >::get_rotation_matrix() const
643 {
644  matrix< 3, 3, T > result;
645  get_rotation_matrix< 3 >( result );
646  return result;
647 }
648 
649 
650 
651 template < typename T > template< size_t D >
652 void quaternion< T >::get_rotation_matrix( matrix< D, D, T >& M ) const
653 {
654  T w2 = array[3] * array[3];
655  T x2 = array[0] * array[0];
656  T y2 = array[1] * array[1];
657  T z2 = array[2] * array[2];
658  T wx = array[3] * array[0];
659  T wy = array[3] * array[1];
660  T wz = array[3] * array[2];
661  T xy = array[0] * array[1];
662  T xz = array[0] * array[2];
663  T yz = array[1] * array[2];
664 
665  M( 0, 0 ) = w2 + x2 - y2 - z2;
666  M( 0, 1 ) = 2. * (xy - wz);
667  M( 0, 2 ) = 2. * (xz + wy);
668  M( 1, 0 ) = 2. * (xy + wz);
669  M( 1, 1 ) = w2 - x2 + y2 - z2;
670  M( 1, 2 ) = 2. * (yz - wx);
671  M( 2, 0 ) = 2. * (xz - wy);
672  M( 2, 1 ) = 2. * (yz + wx);
673  M( 2, 2 ) = w2 - x2 - y2 + z2;
674 
675 }
676 
677 template< typename T >
678 quaternion< T > quaternion< T >::
679 slerp( T a, const quaternion< T >& p, const quaternion< T >& q, const T epsilon )
680 {
681  quaternion< T > px = p.get_normalized();
682  quaternion< T > qx = q.get_normalized();
683 
684  T cosine = px.dot( qx );
685 
686  // check if inverted rotation is needed
687  if ( cosine < 0.0 )
688  {
689  cosine = -cosine;
690  qx = -qx;
691  }
692 
693  const T abs_cos = static_cast< T >( fabs(cosine) );
694  const T one_x = static_cast< T >( 1. - epsilon );
695  if( abs_cos < one_x )
696  {
697  // standard slerp
698  T sine = std::sqrt( 1. - ( cosine * cosine ) );
699  T angle = atan2( sine, cosine );
700  T coeff1 = std::sin( ( 1.0 - a ) * angle) / sine;
701  T coeff2 = std::sin( a * angle ) / sine;
702 
703  qx *= coeff2;
704  px *= coeff1;
705 
706  px += qx;
707  }
708  else
709  {
710  // linear interpolation for very small angles
711  px *= 1. - a;
712  qx *= a;
713  px += qx;
714  px.normalize();
715  }
716 
717  return px;
718 }
719 
720 
721 template < typename T >
722 quaternion< T >& quaternion< T >::operator=(const quaternion& other)
723 {
724  memcpy( array, other.array, 4 * sizeof( T ) );
725  return *this;
726 }
727 
728 }
729 #endif
bool equals(const quaternion &other, T tolerance=std::numeric_limits< T >::epsilon()) const
Definition: quaternion.hpp:228
quaternion()
Construct an identity quaternion.
Definition: quaternion.hpp:63
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
T array[M]
storage
Definition: vector.hpp:331