vmmlib  1.7.0
 All Classes Namespaces Functions Pages
quaternion.hpp
1 /*
2  * Copyright (c) 2006-2014, 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/vector.hpp>
37 #include <vmmlib/matrix.hpp>
38 #include <vmmlib/math.hpp>
39 #include <vmmlib/enable_if.hpp>
40 
41 #include <vmmlib/exception.hpp>
42 #include <vmmlib/vmmlib_config.hpp>
43 
44 #include <algorithm>
45 #include <cassert>
46 #include <cmath>
47 #include <cstdlib>
48 #include <iomanip>
49 #include <iostream>
50 #include <limits>
51 
52 
53 // - declaration - //
54 
55 #define QUATERNION_TRACE_EPSILON 1e-5
56 
57 namespace vmml
58 {
59 
60 template < typename T >
61 class quaternion : private vector< 4, T >
62 {
63 public:
64  typedef vector< 4, T > super;
65 
66  using super::operator();
67  //using super::operator=;
68  using super::at;
69  using super::x;
70  using super::y;
71  using super::z;
72  using super::w;
73  using super::array;
74  using super::find_min;
75  using super::find_max;
76  using super::find_min_index;
77  using super::find_max_index;
78  using super::iter_set;
79 
80  //constructors
81  quaternion() { w() = 1.; }
82  quaternion( T x, T y, T z, T w );
83 
84  quaternion( const vector< 3, T >& xyz , T w );
85  // initializes the quaternion with xyz, sets w to zero
86  quaternion( const vector< 3, T >& xyz );
87 
88  // uses the top-left 3x3 part of the supplied matrix as rotation matrix
89  template< size_t M >
90  quaternion( const matrix< M, M, T >& rotation_matrix_,
91  typename enable_if< M >= 3 >::type* = 0 );
92 
93  void zero();
94  void identity();
95 
96  template< size_t D > void set( const matrix< D, D, T >& rotation_matrix_ );
97 
98  void set( T ww, T xx, T yy, T zz);
99  void set( vector< 3, T >& xyz, T w );
100 
101  template< typename input_iterator_t >
102  void set( input_iterator_t begin_, input_iterator_t end_ );
103 
104  bool operator==( const T& a ) const;
105  bool operator!=( const T& a ) const;
106 
107  bool operator==( const quaternion& a ) const;
108  bool operator!=( const quaternion& a ) const;
109 
110  bool operator==( const vector< 4, T >& a ) const;
111  bool operator!=( const vector< 4, T >& a ) const;
112 
113  bool is_akin( const quaternion& a,
114  const T& delta = std::numeric_limits< T >::epsilon() );
115 
116  void conjugate();
117  quaternion get_conjugate() const;
118 
119  T abs() const;
120  T squared_abs() const;
121 
122  T normalize();
123  quaternion get_normalized() const;
124 
125  quaternion negate() const;
126  quaternion operator-() const;
127 
128  quaternion& operator=(const quaternion& other);
129  const vector< 4, T >& operator=( const vector< 4, T >& other );
130 
131  //
132  // quaternion/quaternion operations
133  //
134  quaternion operator+( const quaternion< T >& a ) const;
135  quaternion operator-( const quaternion< T >& a ) const;
136  // caution: a * q != q * a in general
137  quaternion operator*( const quaternion< T >& a ) const;
138  void operator+=( const quaternion< T >& a );
139  void operator-=( const quaternion< T >& a );
140  // caution: a *= q != q *= a in general
141  void operator*=( const quaternion< T >& a );
142 
143  //
144  // quaternion/scalar operations
145  //
146  quaternion operator*( T a ) const;
147  quaternion operator/( T a ) const;
148 
149  void operator*=( T a );
150  void operator/=( T a );
151 
152  //
153  //quaternion/vector operations
154  //
155  quaternion operator+( const vector< 3, T >& a ) const;
156  quaternion operator-( const vector< 3, T >& a ) const;
157  quaternion operator*( const vector< 3, T >& a ) const;
158 
159  void operator+=( const vector< 3, T >& a );
160  void operator-=( const vector< 3, T >& a );
161  void operator*=( const vector< 3, T >& a );
162 
163  // vec3 = this x b
164  vector< 3, T > cross( const quaternion< T >& b ) const;
165 
166  T dot( const quaternion< T >& a ) const;
167  static T dot( const quaternion< T >& a, const quaternion< T >& b );
168 
169  // returns multiplicative inverse
170  quaternion inverse();
171 
172  void normal( const quaternion& aa, const quaternion& bb, const quaternion& cc, const quaternion& dd );
173  quaternion normal( const quaternion& aa, const quaternion& bb, const quaternion& cc );
174 
175  // to combine two rotations, multiply the respective quaternions before using rotate
176  // instead of rotating twice for increased performance, but be aware of non-commutativity!
177  void rotate( T theta, const vector< 3, T >& a );
178  quaternion rotate( T theta, vector< 3, T >& axis, const vector< 3, T >& a );
179  quaternion rotate_x( T theta, const vector< 3, T >& a );
180  quaternion rotate_y( T theta, const vector< 3, T >& a );
181  quaternion rotate_z( T theta, const vector< 3, T >& a );
182 
183  static quaternion slerp( T a, const quaternion& p,
184  const quaternion& q, const T epsilon = 1e-13 );
185 
186  matrix< 3, 3, T > get_rotation_matrix() const;
187 
188  template< size_t D > void get_rotation_matrix( matrix< D, D, T >& result ) const;
189 
190  friend std::ostream& operator<< ( std::ostream& os, const quaternion& q )
191  {
192  os << "(";
193  size_t index = 0;
194  for( ; index < 3; ++index )
195  {
196  os << q.at( index ) << ", ";
197  }
198  os << q.at( index ) << ") ";
199  return os;
200  };
201 
202  static const quaternion IDENTITY;
203  static const quaternion QUATERI;
204  static const quaternion QUATERJ;
205  static const quaternion QUATERK;
206 
207 }; // class quaternion
208 
209 #ifndef VMMLIB_NO_TYPEDEFS
210 
213 
214 #endif
215 
216 // - implementation - //
217 
218 template < typename T >
219 const quaternion< T > quaternion< T >::IDENTITY( 0, 0, 0, 1 );
220 
221 template < typename T >
222 const quaternion< T > quaternion< T >::QUATERI( 1, 0, 0, 0 );
223 
224 template < typename T >
225 const quaternion< T > quaternion< T >::QUATERJ( 0, 1, 0, 0 );
226 
227 template < typename T >
228 const quaternion< T > quaternion< T >::QUATERK( 0, 0, 1, 0 );
229 
230 template < typename T >
231 quaternion< T >::quaternion( T x_, T y_, T z_, T w_ )
232 {
233  x() = x_;
234  y() = y_;
235  z() = z_;
236  w() = w_;
237 }
238 
239 
240 
241 template < typename T >
242 quaternion< T >::quaternion( const vector< 3, T >& xyz, T w_ )
243 {
244  super::set( xyz, w_ );
245 }
246 
247 
248 
249 
250 template < typename T >
251 quaternion< T >::quaternion( const vector< 3, T >& xyz )
252 {
253  super::set( xyz, static_cast< T >( 0.0 ) );
254 }
255 
256 
257 
258 template< typename T > template< size_t M >
259 quaternion< T >::quaternion( const matrix< M, M, T >& rotation_matrix_,
260  typename enable_if< M >= 3 >::type* )
261 {
262  this->template set< M >( rotation_matrix_ );
263 }
264 
265 
266 
267  // top-left 3x3 is interpreted as rot matrix.
268 template < typename T > template< size_t D >
269 void quaternion< T >::set( const matrix< D, D, T >& M )
270 {
271  T trace = M( 0, 0 ) + M( 1, 1 ) + M( 2,2 ) + 1.0;
272 
273  // very small traces may introduce a big numerical error
274  if( trace > QUATERNION_TRACE_EPSILON )
275  {
276  T s = 0.5 / sqrt( trace );
277  x() = M( 2, 1 ) - M( 1, 2 );
278  x() *= s;
279 
280  y() = M( 0, 2 ) - M( 2, 0 );
281  y() *= s;
282 
283  z() = M( 1, 0 ) - M( 0, 1 );
284  z() *= s;
285 
286  w() = 0.25 / s;
287  }
288  else
289  {
290  vector< 3, T > diag( M( 0, 0 ), M( 1, 1 ), M( 2, 2 ) );
291  size_t largest = diag.find_max_index();
292 
293  // 0, 0 is largest
294  if ( largest == 0 )
295  {
296  T s = 0.5 / sqrt( 1.0 + M( 0, 0 ) - M( 1, 1 ) - M( 2, 2 ) );
297  x() = 0.25 / s;
298 
299  y() = M( 0,1 ) + M( 1,0 );
300  y() *= s;
301 
302  z() = M( 0,2 ) + M( 2,0 );
303  z() *= s;
304 
305  w() = M( 1,2 ) - M( 2,1 );
306  w() *= s;
307  }
308  else if ( largest == 1 )
309  {
310  T s = 0.5 / sqrt( 1.0 + M( 1,1 ) - M( 0,0 ) - M( 2,2 ) );
311  x() = M( 0,1 ) + M( 1,0 );
312  x() *= s;
313 
314  y() = 0.25 / s;
315 
316  z() = M( 1,2 ) + M( 2,1 );
317  z() *= s;
318 
319  w() = M( 0,2 ) - M( 2,0 );
320  w() *= s;
321  }
322  // 2, 2 is largest
323  else if ( largest == 2 )
324  {
325  T s = 0.5 / sqrt( 1.0 + M( 2,2 ) - M( 0,0 ) - M( 1,1 ) );
326  x() = M( 0,2 ) + M( 2,0 );
327  x() *= s;
328 
329  y() = M( 1,2 ) + M( 2,1 );
330  y() *= s;
331 
332  z() = 0.25 / s;
333 
334  w() = M( 0,1 ) - M( 1,0 );
335  w() *= s;
336  }
337  else
338  {
339  (*this) = super::ZERO;
340  assert( 0 );
341  }
342  }
343 }
344 
345 
346 
347 template < typename T >
348 void quaternion< T >::zero()
349 {
350  (*this) = super::ZERO;
351 }
352 
353 
354 
355 template < typename T >
356 void quaternion< T >::identity()
357 {
358  (*this) = IDENTITY;
359 }
360 
361 
362 
363 template < typename T >
364 void quaternion< T >::set( T xx, T yy, T zz, T ww )
365 {
366  x() = xx;
367  y() = yy;
368  z() = zz;
369  w() = ww;
370 }
371 
372 
373 
374 template< typename T >
375 void quaternion< T >::set( vector< 3, T >& xyz, T _w )
376 {
377  x() = xyz.x();
378  y() = xyz.y();
379  z() = xyz.z();
380  w() = _w;
381 }
382 
383 
384 
385 template < typename T >
386 template< typename input_iterator_t >
387 void quaternion< T >::set( input_iterator_t begin_, input_iterator_t end_ )
388 {
389  super::template set< input_iterator_t >( begin_, end_ );
390 }
391 
392 
393 
394 template < typename T >
395 bool quaternion< T >::operator==( const T& a ) const
396 {
397  return ( w() == a && x() == 0 && y() == 0 && z() == 0 );
398 }
399 
400 
401 
402 template < typename T >
403 bool quaternion< T >::operator!=( const T& a ) const
404 {
405  return ( w() != a || x() != 0 || y() != 0 || z() != 0 );
406 }
407 
408 
409 template < typename T >
410 bool quaternion< T >::operator==( const vector< 4, T >& a ) const
411 {
412  return this->operator==(
413  reinterpret_cast< const quaternion< T >& >( a )
414  );
415 }
416 
417 
418 template < typename T >
419 bool
420 quaternion< T >::operator!=( const vector< 4, T >& a ) const
421 {
422  return ! this->operator==( a );
423 }
424 
425 
426 
427 template < typename T >
428 bool
429 quaternion< T >::operator==( const quaternion& a ) const
430 {
431  return (
432  w() == a.w() &&
433  x() == a.x() &&
434  y() == a.y() &&
435  z() == a.z()
436  );
437 }
438 
439 
440 
441 template < typename T >
442 bool
443 quaternion< T >::operator!=( const quaternion& a ) const
444 {
445  return ! this->operator==( a );
446 }
447 
448 
449 
450 template < typename T >
451 bool
452 quaternion< T >::is_akin( const quaternion& a, const T& delta )
453 {
454  if( fabsf( w() - a.w() ) > delta ||
455  fabsf( x() - a.x() ) > delta ||
456  fabsf( y() - a.y() ) > delta ||
457  fabsf( z() - a.z() ) > delta
458  )
459  return false;
460  return true;
461 }
462 
463 
464 
465 template < typename T >
466 void quaternion< T >::conjugate()
467 {
468  x() = -x();
469  y() = -y();
470  z() = -z();
471 }
472 
473 
474 
475 template < typename T >
476 quaternion< T > quaternion< T >::get_conjugate() const
477 {
478  return quaternion< T > ( -x(), -y(), -z(), w() );
479 }
480 
481 
482 
483 template < typename T >
484 T
485 quaternion< T >::abs() const
486 {
487  return sqrt( squared_abs() );
488 }
489 
490 
491 
492 template < typename T >
493 T quaternion< T >::squared_abs() const
494 {
495  return x() * x() + y() * y() + z() * z() + w() * w();
496 }
497 
498 
499 
500 template < typename T >
501 quaternion< T > quaternion< T >::inverse()
502 {
503  quaternion< T > q( *this );
504  q.conjugate();
505 
506  T tmp = squared_abs();
507  tmp = static_cast< T >( 1.0 ) / tmp;
508  return q * tmp;
509 }
510 
511 
512 
513 template < typename T >
514 T quaternion< T >::normalize()
515 {
516  T len = abs();
517  if( len == 0.0 )
518  return 0.0;
519  len = 1.0f / len;
520  this->operator*=( len );
521  return len;
522 }
523 
524 
525 
526 template < typename T >
527 quaternion< T >
528 quaternion< T >::get_normalized() const
529 {
530  quaternion< T > q( *this );
531  q.normalize();
532  return q;
533 }
534 
535 
536 
537 //
538 // quaternion/quaternion operations
539 //
540 
541 template < typename T >
542 quaternion< T >
543 quaternion< T >::operator+( const quaternion< T >& a ) const
544 {
545  return quaternion( x() + a.x(), y() + a.y(), z() + a.z(), w() + a.w() );
546 }
547 
548 
549 
550 template < typename T >
551 quaternion< T >
552 quaternion< T >::operator-( const quaternion< T >& a ) const
553 {
554  return quaternion( x() - a.x(), y() - a.y(), z() - a.z(), w() - a.w() );
555 }
556 
557 
558 
559 // returns Grasssmann product
560 template < typename T >
561 quaternion< T >
562 quaternion< T >::operator*( const quaternion< T >& a ) const
563 {
564  quaternion< T > ret( *this );
565  ret *= a;
566  return ret;
567 }
568 
569 
570 
571 // Grassmann product
572 template < typename T >
573 void quaternion< T >::operator*=( const quaternion< T >& q )
574 {
575  #if 0
576  quaternion< T > orig( *this );
577  x() = orig.w() * a.x() + orig.x() * a.w() + orig.y() * a.z() - orig.z() * a.y();
578  y() = orig.w() * a.y() + orig.y() * a.w() + orig.z() * a.x() - orig.x() * a.z();
579  z() = orig.w() * a.z() + orig.z() * a.w() + orig.x() * a.y() - orig.y() * a.x();
580  w() = orig.w() * a.w() - orig.x() * a.x() - orig.y() * a.y() - orig.z() * a.z();
581  #else
582 
583  // optimized version, 7 less mul, but 15 more add/subs
584  // after Henrik Engstrom, from a gamedev.net article.
585 
586  T* _array = super::array;
587 
588  const T& a = _array[ 3 ];
589  const T& b = _array[ 0 ];
590  const T& c = _array[ 1 ];
591  const T& d = _array[ 2 ];
592  const T& _x = q.array[ 3 ];
593  const T& _y = q.array[ 0 ];
594  const T& _z = q.array[ 1 ];
595  const T& _w = q.array[ 2 ];
596 
597  const T tmp_00 = (d - c) * (_z - _w);
598  const T tmp_01 = (a + b) * (_x + _y);
599  const T tmp_02 = (a - b) * (_z + _w);
600  const T tmp_03 = (c + d) * (_x - _y);
601  const T tmp_04 = (d - b) * (_y - _z);
602  const T tmp_05 = (d + b) * (_y + _z);
603  const T tmp_06 = (a + c) * (_x - _w);
604  const T tmp_07 = (a - c) * (_x + _w);
605  const T tmp_08 = tmp_05 + tmp_06 + tmp_07;
606  const T tmp_09 = 0.5 * (tmp_04 + tmp_08);
607 
608  _array[ 3 ] = tmp_00 + tmp_09 - tmp_05;
609  _array[ 0 ] = tmp_01 + tmp_09 - tmp_08;
610  _array[ 1 ] = tmp_02 + tmp_09 - tmp_07;
611  _array[ 2 ] = tmp_03 + tmp_09 - tmp_06;
612 
613  #endif
614 }
615 
616 
617 
618 
619 
620 template < typename T >
621 quaternion< T >
622 quaternion< T >::operator-() const
623 {
624  return quaternion( -x(), -y(), -z(), -w() );
625 }
626 
627 
628 
629 template < typename T >
630 void quaternion< T >::operator+=( const quaternion< T >& q )
631 {
632  array[ 0 ] += q.array[ 0 ];
633  array[ 1 ] += q.array[ 1 ];
634  array[ 2 ] += q.array[ 2 ];
635  array[ 3 ] += q.array[ 3 ];
636 }
637 
638 
639 
640 template < typename T >
641 void quaternion< T >::operator-=( const quaternion< T >& q )
642 {
643  array[ 0 ] -= q.array[ 0 ];
644  array[ 1 ] -= q.array[ 1 ];
645  array[ 2 ] -= q.array[ 2 ];
646  array[ 3 ] -= q.array[ 3 ];
647 }
648 
649 
650 
651 //
652 // quaternion/scalar operations
653 //
654 
655 template < typename T >
656 quaternion< T >
657 quaternion< T >::operator*( const T a ) const
658 {
659  return quaternion( x() * a, y() * a, z() * a, w() * a );
660 }
661 
662 
663 
664 template < typename T >
665 quaternion< T >
666 quaternion< T >::operator/( T a ) const
667 {
668  if ( a == 0.0 )
669  {
670  VMMLIB_ERROR( "Division by zero.", VMMLIB_HERE );
671  }
672  a = 1.0 / a;
673  return quaternion( x() * a, y() * a, z() * a, w() * a );
674 }
675 
676 
677 
678 template < typename T >
679 void quaternion< T >::operator*=( T q )
680 {
681  array[ 0 ] *= q;
682  array[ 1 ] *= q;
683  array[ 2 ] *= q;
684  array[ 3 ] *= q;
685 }
686 
687 
688 
689 template < typename T >
690 void quaternion< T >::operator/=( T q )
691 {
692  if ( q == 0.0 )
693  {
694  VMMLIB_ERROR( "Division by zero", VMMLIB_HERE );
695  }
696  q = 1.0f / q;
697  this->operator*=( q );
698 }
699 
700 
701 //quaternion/vector operations
702 
703 template < typename T >
704 quaternion< T >
705 quaternion< T >::operator+( const vector< 3, T >& a ) const
706 {
707  return quaternion( x() + a.x(), y() + a.y(), z() + a.z(), w() );
708 }
709 
710 
711 
712 template < typename T >
713 quaternion< T >
714 quaternion< T >::operator-( const vector< 3, T >& a ) const
715 {
716  return quaternion( w(), x() - a.x(), y() - a.y(), z() - a.z() );
717 }
718 
719 
720 
721 template < typename T >
722 quaternion< T >
723 quaternion< T >::operator*( const vector< 3, T >& a ) const
724 {
725  return quaternion( -x() * a.x() - y() * a.y() - z() * a.z(),
726  w() * a.x() + y() * a.z() - z() * a.y(),
727  w() * a.y() + z() * a.x() - x() * a.z(),
728  w() * a.z() + x() * a.y() - y() * a.x() );
729 }
730 
731 
732 
733 template < typename T >
734 void quaternion< T >::operator+=( const vector< 3, T >& xyz )
735 {
736  x() += xyz.x();
737  y() += xyz.y();
738  y() += xyz.z();
739 }
740 
741 
742 
743 template < typename T >
744 void quaternion< T >::operator-=( const vector< 3, T >& xyz )
745 {
746  x() -= xyz.x();
747  y() -= xyz.y();
748  z() -= xyz.z();
749  return;
750 }
751 
752 
753 
754 template < typename T >
755 void quaternion< T >::operator*=(const vector< 3, T >& a )
756 {
757  T _x = x();
758  T _y = y();
759  T _z = z();
760  T _w = w();
761 
762  x() = _w * a.x() + _y * a.z() - _z * a.y();
763  y() = _w * a.y() + _z * a.x() - _x * a.z();
764  z() = _w * a.z() + _x * a.y() - _y * a.x();
765  w() = -_x * a.x() - _y * a.y() - _z * a.z();
766 }
767 
768 
769 
770 
771 template < typename T >
772 vector< 3, T > quaternion< T >::cross( const quaternion< T >& bb ) const
773 {
774  vector< 3, T > result;
775 
776  result.array[ 0 ] = y() * bb.z() - z() * bb.y();
777  result.array[ 1 ] = z() * bb.x() - x() * bb.z();
778  result.array[ 2 ] = x() * bb.y() - y() * bb.x();
779 
780  return result;
781 }
782 
783 
784 
785 template < typename T >
786 T quaternion< T >::dot( const quaternion< T >& q ) const
787 {
788  return w() * q.w() + x() * q.x() + y() * q.y() + z() * q.z();
789 }
790 
791 
792 
793 template < typename T >
794 T quaternion< T >::
795 dot( const quaternion< T >& p, const quaternion< T >& q )
796 {
797  return p.w() * q.w() + p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
798 }
799 
800 
801 
802 template < typename T >
803 void quaternion< T >::normal( const quaternion< T >& aa,
804  const quaternion< T >& bb,
805  const quaternion< T >& cc,
806  const quaternion< T >& dd )
807 {
808  //right hand system, CCW triangle
809  const quaternion< T > quat_t = bb - aa;
810  const quaternion< T > quat_u = cc - aa;
811  const quaternion< T > quat_v = dd - aa;
812  cross( quat_t );
813  cross( quat_u );
814  cross( quat_v );
815  normalize();
816 }
817 
818 
819 
820 template < typename T >
821 quaternion< T > quaternion< T >::normal( const quaternion< T >& aa,
822  const quaternion< T >& bb,
823  const quaternion< T >& cc )
824 {
825  quaternion< T > tmp;
826  tmp.normal( *this, aa, bb, cc );
827  return tmp;
828 }
829 
830 
831 // to combine two rotations, multiply the respective quaternions before using rotate
832 // instead of rotating twice for increased performance, but be aware of non-commutativity!
833 // (the first rotation quaternion has to be the first factor)
834 template< typename T >
835 quaternion< T > quaternion< T >::rotate( T theta, vector< 3, T >& axis,
836  const vector< 3, T >& a )
837 {
838  quaternion< T > p = a;
839  T alpha = theta / 2;
840  quaternion< T > q = std::cos( alpha ) + ( std::sin( alpha ) * axis.normalize() );
841  return q * p * q.invert();
842 }
843 
844 
845 
846 template< typename T >
847 quaternion< T > quaternion< T >::rotate_x( T theta, const vector< 3, T >& a )
848 {
849  quaternion< T > p = a;
850  T alpha = theta / 2;
851  quaternion< T > q = std::cos( alpha ) + ( std::sin( alpha ) * QUATERI );
852  return q * p * q.invert();
853 }
854 
855 
856 
857 template< typename T >
858 quaternion< T > quaternion< T >::rotate_y( T theta, const vector< 3, T >& a )
859 {
860  quaternion< T > p = a;
861  T alpha = theta / 2;
862  quaternion< T > q = std::cos( alpha ) + ( std::sin( alpha ) * QUATERJ );
863  return q * p * q.invert();
864 }
865 
866 
867 
868 template< typename T >
869 quaternion< T > quaternion< T >::rotate_z( T theta, const vector< 3, T >& a )
870 {
871  quaternion< T > p = a;
872  T alpha = theta / 2;
873  quaternion< T > q = std::cos( alpha ) + ( std::sin( alpha ) * QUATERK );
874  return q * p * q.invert();
875 }
876 
877 
878 
879 template < typename T >
880 matrix< 3, 3, T >
881 quaternion< T >::get_rotation_matrix() const
882 {
883  matrix< 3, 3, T > result;
884  get_rotation_matrix< 3 >( result );
885  return result;
886 }
887 
888 
889 
890 template < typename T > template< size_t D >
891 void quaternion< T >::get_rotation_matrix( matrix< D, D, T >& M ) const
892 {
893  T w2 = w() * w();
894  T x2 = x() * x();
895  T y2 = y() * y();
896  T z2 = z() * z();
897  T wx = w() * x();
898  T wy = w() * y();
899  T wz = w() * z();
900  T xy = x() * y();
901  T xz = x() * z();
902  T yz = y() * z();
903 
904  M( 0, 0 ) = w2 + x2 - y2 - z2;
905  M( 0, 1 ) = 2. * (xy - wz);
906  M( 0, 2 ) = 2. * (xz + wy);
907  M( 1, 0 ) = 2. * (xy + wz);
908  M( 1, 1 ) = w2 - x2 + y2 - z2;
909  M( 1, 2 ) = 2. * (yz - wx);
910  M( 2, 0 ) = 2. * (xz - wy);
911  M( 2, 1 ) = 2. * (yz + wx);
912  M( 2, 2 ) = w2 - x2 - y2 + z2;
913 
914 }
915 
916 template< typename T >
917 quaternion< T > quaternion< T >::
918 slerp( T a, const quaternion< T >& p, const quaternion< T >& q, const T epsilon )
919 {
920  quaternion< T > px = p.get_normalized();
921  quaternion< T > qx = q.get_normalized();
922 
923  T cosine = px.dot( qx );
924 
925  // check if inverted rotation is needed
926  if ( cosine < 0.0 )
927  {
928  cosine = -cosine;
929  qx = -qx;
930  }
931 
932  const T abs_cos = static_cast< T >( fabs(cosine) );
933  const T one_x = static_cast< T >( 1. - epsilon );
934  if( abs_cos < one_x )
935  {
936  // standard slerp
937  T sine = sqrt( 1. - ( cosine * cosine ) );
938  T angle = atan2( sine, cosine );
939  T coeff1 = std::sin( ( 1.0 - a ) * angle) / sine;
940  T coeff2 = std::sin( a * angle ) / sine;
941 
942  qx *= coeff2;
943  px *= coeff1;
944 
945  px += qx;
946  }
947  else
948  {
949  // linear interpolation for very small angles
950  px *= 1. - a;
951  qx *= a;
952  px += qx;
953  px.normalize();
954  }
955 
956  return px;
957 }
958 
959 
960 template < typename T >
961 quaternion< T >& quaternion< T >::operator=(const quaternion& other)
962 {
963  memcpy( array, other.array, 4 * sizeof( T ) );
964  return *this;
965 }
966 
967 
968 template < typename T >
969 const vector< 4, T >&
970 quaternion< T >::operator=( const vector< 4, T >& other )
971 {
972  memcpy( array, other.array, 4 * sizeof( T ) );
973  return other;
974 }
975 
976 
977 }
978 #endif