LCOV - code coverage report
Current view: top level - vmmlib - quaternion.hpp (source / functions) Hit Total Coverage
Test: vmmlib Lines: 73 91 80.2 %
Date: 2017-05-16 00:10:45 Functions: 8 13 61.5 %

          Line data    Source code
       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:
      52             :     /** Construct an identity quaternion */
      53           2 :     Quaternion() : array() { array[3] = 1.; }
      54             :     Quaternion( T x, T y, T z, T w );
      55             : 
      56             :     /** Construct a rotation quaternion */
      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             : 
      64             :     /** @name Data Access */
      65             :     //@{
      66             :     /** @return true if the two quaternion are similar. */
      67             :     bool equals( const Quaternion& other,
      68             :                  T tolerance = std::numeric_limits< T >::epsilon( )) const;
      69             : 
      70           0 :     T x() const { return array[0]; }
      71           0 :     T y() const { return array[1]; }
      72           0 :     T z() const { return array[2]; }
      73           0 :     T w() const { return array[3]; }
      74             : 
      75             :     /** @return true if both quaternions are equal. */
      76             :     bool operator==( const Quaternion& a ) const;
      77             : 
      78             :     /** @return true if both quaternions are not equal. */
      79             :     bool operator!=( const Quaternion& a ) const;
      80             : 
      81             :     /** @return the negated quaternion of this quaternion. */
      82             :     Quaternion operator-() const;
      83             : 
      84             :     /** @return multiplicative inverse quaternion */
      85             :     Quaternion inverse() const;
      86             : 
      87             :     Quaternion getConjugate() const;
      88             : 
      89             :     T abs() const;
      90             :     T absSquare() const;
      91             : 
      92             :     /** @return the corresponding 3x3 rotation matrix. */
      93             :     Matrix< 3, 3, T > getRotationMatrix() const;
      94             :     //@}
      95             : 
      96             :     void normalize();
      97             : 
      98             :     Quaternion& operator=(const Quaternion& other);
      99             : 
     100             :     /** @name quaternion/quaternion operations */
     101             :     //@{
     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 );
     112             :     //@}
     113             : 
     114             :     /** @name quaternion/scalar operations */
     115             :     //@{
     116             :     Quaternion operator*( T a ) const;
     117             :     Quaternion operator/( T a ) const;
     118             : 
     119             :     void operator*=( T a );
     120             :     void operator/=( T a );
     121             :     //@}
     122             : 
     123           0 :     friend std::ostream& operator<< ( std::ostream& os, const Quaternion& q )
     124             :     {
     125           0 :         const std::ios::fmtflags flags = os.flags();
     126           0 :         const int                prec  = os.precision();
     127             : 
     128           0 :         os.setf( std::ios::right, std::ios::adjustfield );
     129           0 :         os.precision( 5 );
     130             : 
     131           0 :         os << "( "
     132           0 :            << std::setw(10) << q.x() << " "
     133           0 :            << std::setw(10) << q.y() << " "
     134           0 :            << std::setw(10) << q.z() << " "
     135           0 :            << std::setw(10) << q.w() << " "
     136           0 :            << ")";
     137             : 
     138           0 :         os.precision( prec );
     139           0 :         os.setf( flags );
     140           0 :         return os;
     141             :     }
     142             : 
     143             : private:
     144             :     T array[4];
     145             : };
     146             : }
     147             : 
     148             : #include <vmmlib/matrix.hpp>
     149             : 
     150             : namespace vmml
     151             : {
     152             : /** @name Free quaternion functions */
     153             : //@{
     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             : }
     168             : //@}
     169             : 
     170           3 : template < typename T > Quaternion< T >::Quaternion( T x_, T y_, T z_, T w_ )
     171             : {
     172           3 :     array[0] = x_;
     173           3 :     array[1] = y_;
     174           3 :     array[2] = z_;
     175           3 :     array[3] = w_;
     176           3 : }
     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 >
     261           4 : Quaternion< T >::Quaternion( const T angle, vector< 3, T > axis )
     262             : {
     263           4 :     axis.normalize();
     264           4 :     const T sinAngle2 = std::sin( angle * 0.5 );
     265           4 :     array[0] = axis.x() * sinAngle2;
     266           4 :     array[1] = axis.y() * sinAngle2;
     267           4 :     array[2] = axis.z() * sinAngle2;
     268           4 :     array[3] = std::cos( angle * 0.5 );
     269           4 : }
     270             : 
     271             : template< typename T >
     272           2 : bool Quaternion< T >::equals( const Quaternion& other, const T tolerance ) const
     273             : {
     274           4 :     return std::abs( array[0] - other.array[0] ) <= tolerance &&
     275           4 :            std::abs( array[1] - other.array[1] ) <= tolerance &&
     276           6 :            std::abs( array[2] - other.array[2] ) <= tolerance &&
     277           4 :            std::abs( array[3] - other.array[3] ) <= tolerance;
     278             : }
     279             : 
     280             : template < typename T >
     281           3 : bool Quaternion< T >::operator==( const Quaternion& rhs ) const
     282             : {
     283           9 :     return array[0] == rhs.array[0] && array[1] == rhs.array[1] &&
     284           9 :            array[2] == rhs.array[2] && array[3] == rhs.array[3];
     285             : }
     286             : 
     287             : template < typename T >
     288             : bool Quaternion< T >::operator!=( const Quaternion& a ) const
     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           1 : Quaternion< T > Quaternion< T >::operator*( const Quaternion< T >& rhs ) const
     346             : {
     347           1 :     Quaternion< T > ret( *this );
     348           1 :     ret *= rhs;
     349           1 :     return ret;
     350             : }
     351             : 
     352             : // Grassmann product
     353             : template < typename T >
     354           1 : 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           1 :     const T& a_ = array[ 3 ];
     359           1 :     const T& b_ = array[ 0 ];
     360           1 :     const T& c = array[ 1 ];
     361           1 :     const T& d = array[ 2 ];
     362           1 :     const T& _x = q.array[ 3 ];
     363           1 :     const T& _y = q.array[ 0 ];
     364           1 :     const T& _z = q.array[ 1 ];
     365           1 :     const T& _w = q.array[ 2 ];
     366             : 
     367           1 :     const T tmp_00 = (d - c) * (_z - _w);
     368           1 :     const T tmp_01 = (a_ + b_) * (_x + _y);
     369           1 :     const T tmp_02 = (a_ - b_) * (_z + _w);
     370           1 :     const T tmp_03 = (c + d) * (_x - _y);
     371           1 :     const T tmp_04 = (d - b_) * (_y - _z);
     372           1 :     const T tmp_05 = (d + b_) * (_y + _z);
     373           1 :     const T tmp_06 = (a_ + c) * (_x - _w);
     374           1 :     const T tmp_07 = (a_ - c) * (_x + _w);
     375           1 :     const T tmp_08 = tmp_05 + tmp_06 + tmp_07;
     376           1 :     const T tmp_09 = 0.5 * (tmp_04 + tmp_08);
     377             : 
     378           1 :     array[ 0 ] = tmp_01 + tmp_09 - tmp_08;
     379           1 :     array[ 1 ] = tmp_02 + tmp_09 - tmp_07;
     380           1 :     array[ 2 ] = tmp_03 + tmp_09 - tmp_06;
     381           1 :     array[ 3 ] = tmp_00 + tmp_09 - tmp_05;
     382           1 : }
     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 >
     390             : void Quaternion< T >::operator+=( const Quaternion< T >& q )
     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 >
     447           1 : Matrix< 3, 3, T > Quaternion< T >::getRotationMatrix() const
     448             : {
     449           1 :     const T w2 = array[3] * array[3];
     450           1 :     const T x2 = array[0] * array[0];
     451           1 :     const T y2 = array[1] * array[1];
     452           1 :     const T z2 = array[2] * array[2];
     453           1 :     const T wx = array[3] * array[0];
     454           1 :     const T wy = array[3] * array[1];
     455           1 :     const T wz = array[3] * array[2];
     456           1 :     const T xy = array[0] * array[1];
     457           1 :     const T xz = array[0] * array[2];
     458           1 :     const T yz = array[1] * array[2];
     459             : 
     460           1 :     Matrix< 3, 3, T > matrix;
     461           1 :     matrix( 0, 0 ) = w2 + x2 - y2 - z2;
     462           1 :     matrix( 0, 1 ) = 2. * (xy - wz);
     463           1 :     matrix( 0, 2 ) = 2. * (xz + wy);
     464           1 :     matrix( 1, 0 ) = 2. * (xy + wz);
     465           1 :     matrix( 1, 1 ) = w2 - x2 + y2 - z2;
     466           1 :     matrix( 1, 2 ) = 2. * (yz - wx);
     467           1 :     matrix( 2, 0 ) = 2. * (xz - wy);
     468           1 :     matrix( 2, 1 ) = 2. * (yz + wx);
     469           1 :     matrix( 2, 2 ) = w2 - x2 - y2 + z2;
     470           1 :     return matrix;
     471             : }
     472             : 
     473             : template < typename T >
     474             : Quaternion< T >& Quaternion< T >::operator=(const Quaternion& other)
     475             : {
     476             :     ::memcpy( array, other.array, 4 * sizeof( T ) );
     477             :     return *this;
     478             : }
     479             : 
     480             : }
     481             : #endif

Generated by: LCOV version 1.11