quaternion.h ソースファイル

説明を見る。
 // 
 // Copyright (c) 2003-2011, MIST Project, Nagoya University
 // All rights reserved.
 // 
 // Redistribution and use in source and binary forms, with or without modification,
 // are permitted provided that the following conditions are met:
 // 
 // 1. Redistributions of source code must retain the above copyright notice,
 // this list of conditions and the following disclaimer.
 // 
 // 2. Redistributions in binary form must reproduce the above copyright notice,
 // this list of conditions and the following disclaimer in the documentation
 // and/or other materials provided with the distribution.
 // 
 // 3. Neither the name of the Nagoya University nor the names of its contributors
 // may be used to endorse or promote products derived from this software
 // without specific prior written permission.
 // 
 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
 // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 // IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
 // THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 // 
 
 #ifndef __INCLUDE_MIST_QUATERNION__
 #define __INCLUDE_MIST_QUATERNION__
 
 
 #ifndef __INCLUDE_MIST_CONF_H__
 #include "config/mist_conf.h"
 #endif
 
 #ifndef __INCLUDE_MIST_TYPE_TRAIT_H__
 #include "config/type_trait.h"
 #endif
 
 #ifndef __INCLUDE_MIST_VECTOR__
 #include "vector.h"
 #endif
 
 
 #include <cmath>
 
 // mist名前空間の始まり
 _MIST_BEGIN
 
 
 template < class T >
 class quaternion
 {
 public:
     typedef T value_type;                                       
     typedef size_t size_type;                                   
     typedef ptrdiff_t difference_type;                          
     typedef typename float_type< T >::value_type float_type;    
 
 public:
     value_type w;       
     value_type x;       
     value_type y;       
     value_type z;       
 
     quaternion( ) : w( 0 ), x( 0 ), y( 0 ), z( 0 ){ }
 
     quaternion( const value_type &ww, const value_type &xx, const value_type &yy, const value_type &zz ) : w( ww ), x( xx ), y( yy ), z( zz ){ }
 
     explicit quaternion( const value_type &ww ) : w( ww ), x( 0 ), y( 0 ), z( 0 ){ }
 
     template < class TT >
     quaternion( const quaternion< TT > &q ) : w( static_cast< value_type >( q.w ) ), x( static_cast< value_type >( q.x ) ), y( static_cast< value_type >( q.y ) ), z( static_cast< value_type >( q.z ) ){ }
 
     quaternion( const quaternion< T > &q ) : w( q.w ), x( q.x ), y( q.y ), z( q.z ){ }
 
 
     template < class TT >
     quaternion( value_type ww, const vector3< TT > &v ) : w( ww ), x( static_cast< value_type >( v.x ) ), y( static_cast< value_type >( v.y ) ), z( static_cast< value_type >( v.z ) ){ }
 
 
     template < class TT >
     quaternion( const vector3< TT > &axis, value_type theta )
     {
         double t = theta * 3.1415926535897932384626433832795 / 180.0 / 2.0;
         double c = std::cos( t );
         double s = std::sin( t );
         w = static_cast< value_type >( c );
         x = static_cast< value_type >( s * axis.x );
         y = static_cast< value_type >( s * axis.y );
         z = static_cast< value_type >( s * axis.z );
     }
 
 
     template < class TT >
     quaternion( vector3< TT > dir, vector3< TT > up )
     {
         // ワールド座標の単位ベクトル
         vector3< TT > e2( 0, 1, 0 );
         vector3< TT > e3( 0, 0, 1 );
 
         // 単位ベクトルにする
         dir = dir.unit( );
         up  = up.unit( );
 
         // 視線方向を合わせるクォータニオンを作成
         quaternion q1 = quaternion::rotate( e3, dir );
 
         // Y軸を回転させる
         e2 = q1.rotate( e2 );
 
         // 上向き方向を合わせるクォータニオンを作成
         quaternion q2 = quaternion::rotate( e2, up, dir );
 
         // 回転を合成する
         operator =( q2 * q1 );
     }
 
 
     template < class TT >
     const quaternion &operator =( const quaternion< TT > &q )
     {
         w = static_cast< value_type >( q.w );
         x = static_cast< value_type >( q.x );
         y = static_cast< value_type >( q.y );
         z = static_cast< value_type >( q.z );
         return ( *this );
     }
 
     const quaternion &operator =( const quaternion< T > &q )
     {
         if( &q != this )
         {
             w = q.w;
             x = q.x;
             y = q.y;
             z = q.z;
         }
         return ( *this );
     }
 
     quaternion operator -( ) const { return ( quaternion( -w, -x, -y, -z ) ); }
 
     template< class TT >
     const quaternion &operator +=( const quaternion< TT > &q )
     {
         w = static_cast< value_type >( w + q.w );
         x = static_cast< value_type >( x + q.x );
         y = static_cast< value_type >( y + q.y );
         z = static_cast< value_type >( z + q.z );
         return( *this );
     }
 
 #if defined( __MIST_MSVC__ ) && __MIST_MSVC__ < 7
     const quaternion &operator +=( const double &a )
 #else
     template < class TT >
     const quaternion &operator +=( const TT &a )
 #endif
     {
         w = static_cast< value_type >( w + a );
         return( *this );
     }
 
     template< class TT >
     const quaternion &operator -=( const quaternion< TT > &q )
     {
         w = static_cast< value_type >( w - q.w );
         x = static_cast< value_type >( x - q.x );
         y = static_cast< value_type >( y - q.y );
         z = static_cast< value_type >( z - q.z );
         return( *this );
     }
 
 #if defined( __MIST_MSVC__ ) && __MIST_MSVC__ < 7
     const quaternion &operator -=( const double &a )
 #else
     template < class TT >
     const quaternion &operator -=( const TT &a )
 #endif
     {
         w = static_cast< value_type >( w - a );
         return( *this );
     }
 
     template < class TT >
     const quaternion &operator *=( const quaternion< TT > &q )
     {
         value_type ww = static_cast< value_type >( w * q.w - x * q.x - y * q.y - z * q.z );
         value_type xx = static_cast< value_type >( w * q.x + x * q.w + y * q.z - z * q.y );
         value_type yy = static_cast< value_type >( w * q.y + y * q.w + z * q.x - x * q.z );
         value_type zz = static_cast< value_type >( w * q.z + z * q.w + x * q.y - y * q.x );
         w = ww;
         x = xx;
         y = yy;
         z = zz;
         return( *this );
     }
 
 #if defined( __MIST_MSVC__ ) && __MIST_MSVC__ < 7
     const quaternion &operator *=( const double &a )
 #else
     template < class TT >
     const quaternion &operator *=( const TT &a )
 #endif
     {
         w = static_cast< value_type >( w * a );
         x = static_cast< value_type >( x * a );
         y = static_cast< value_type >( y * a );
         z = static_cast< value_type >( z * a );
         return( *this );
     }
 
     template < class TT >
     const quaternion &operator /=( const quaternion< TT > &q )
     {
         return( this->operator *=( q.inv( ) ) );
     }
 
 #if defined( __MIST_MSVC__ ) && __MIST_MSVC__ < 7
     const quaternion &operator /=( const double &a )
 #else
     template < class TT >
     const quaternion &operator /=( const TT &a )
 #endif
     {
         w = static_cast< value_type >( w / a );
         x = static_cast< value_type >( x / a );
         y = static_cast< value_type >( y / a );
         z = static_cast< value_type >( z / a );
         return( *this );
     }
 
 
     bool operator ==( const quaternion &q ) const { return( w == q.w && x == q.x && y == q.y && z == q.z ); }
 
     bool operator !=( const quaternion &q ) const { return( !( *this == q ) ); }
 
     bool operator < ( const quaternion &q ) const
     {
         if( w == q.w )
         {
             if( x == q.x )
             {
                 if( y == q.y )
                 {
                     return( z < q.z );
                 }
                 else
                 {
                     return( y < q.y );
                 }
             }
             else
             {
                 return( x < q.x );
             }
         }
         else
         {
             return( w < q.w );
         }
     }
 
     bool operator <=( const quaternion &q ) const { return( q >= *this ); }
 
     bool operator > ( const quaternion &q ) const { return( q < *this ); }
 
     bool operator >=( const quaternion &q ) const { return( !( *this < q ) ); }
 
 
 public: // その他の関数
 
     const quaternion conjugate( ) const 
     {
         return( quaternion( w, -x, -y, -z ) );
     }
 
     const quaternion inv( ) const
     {
         float_type length_ = length( );
         return( conjugate( ) / ( length_ * length_ ) );
     }
 
     const quaternion unit( ) const
     {
         float_type length_ = length( );
         if( length_ > 0 )
         {
             return( quaternion( static_cast< value_type >( w / length_ ), static_cast< value_type >( x / length_ ), static_cast< value_type >( y / length_ ), static_cast< value_type >( z / length_ ) ) );
         }
         else
         {
             return( *this );
         }
     }
 
     template < class TT >
     value_type inner( const quaternion< TT > &q ) const
     {
         return( w * q.w + x * q.x + y * q.y + z * q.z );
     }
 
     float_type length( ) const { return ( static_cast< float_type >( std::sqrt( static_cast< double >( w * w + x * x + y * y + z * z ) ) ) ); }
 
 
     template < class TT >
     const vector3< TT > rotate( const vector3< TT > &v ) const
     {
         quaternion q = ( *this ) * quaternion( 0, static_cast< value_type >( v.x ), static_cast< value_type >( v.y ), static_cast< value_type >( v.z ) ) * inv( );
         return( vector3< TT >( static_cast< TT >( q.x ), static_cast< TT >( q.y ), static_cast< TT >( q.z ) ) );
     }
 
 
 
     template < class TT >
     static quaternion rotate( vector3< TT > v1, vector3< TT > v2 )
     {
         // 単位ベクトルにする
         v1 = v1.unit( );
         v2 = v2.unit( );
 
         // 回転角度を計算する
         double dot = v1.inner( v2 );
         if( dot < -1.0 )
         {
             return( quaternion( -1, 0, 0, 0 ) );
         }
 
         double c = std::sqrt( ( dot + 1.0 ) * 0.5 );
 
         if( std::abs( c - 1.0 ) < 1.0e-6 || c > 1.0 )
         {
             return( quaternion( 1, 0, 0, 0 ) );
         }
         else if( std::abs( c + 1.0 ) < 1.0e-6 || c < -1.0 )
         {
             return( quaternion( -1, 0, 0, 0 ) );
         }
 
         return( quaternion( value_type( c ), std::sqrt( 1.0 - c * c ) * v1.outer( v2 ).unit( ) ) );
     }
 
     template < class TT >
     static quaternion rotate( vector3< TT > v1, vector3< TT > v2, const vector3< TT > &axis )
     {
         // 単位ベクトルにする
         v1 = v1.unit( );
         v2 = v2.unit( );
 
         // 回転角度を計算する
         double dot = v1.inner( v2 );
         if( dot < -1.0 )
         {
             return( quaternion( -1, 0, 0, 0 ) );
         }
 
         double c = std::sqrt( ( dot + 1.0 ) * 0.5 );
 
         if( std::abs( c - 1.0 ) < 1.0e-6 || c > 1.0 )
         {
             return( quaternion( 1, 0, 0, 0 ) );
         }
         else if( std::abs( c + 1.0 ) < 1.0e-6 || c < -1.0 )
         {
             return( quaternion( -1, 0, 0, 0 ) );
         }
 
         double s = std::sqrt( 1.0 - c * c );
 
         if( axis.inner( v1.outer( v2 ) ) < 0.0 )
         {
             s = -s;
         }
 
         return( quaternion( value_type( c ), s * axis ) );
     }
 };
 
 // 型の昇格を行う演算の定義
 
 DEFINE_PROMOTE_BIND_OPERATOR1( quaternion, + )
 
 
 DEFINE_PROMOTE_BIND_OPERATOR2( quaternion, + )
 
 DEFINE_PROMOTE_BIND_OPERATOR3( quaternion, + )
 
 DEFINE_PROMOTE_BIND_OPERATOR1( quaternion, - )
 
 DEFINE_PROMOTE_BIND_OPERATOR2( quaternion, - )
 
 DEFINE_PROMOTE_BIND_OPERATOR4( quaternion, - )
 
 DEFINE_PROMOTE_BIND_OPERATOR1( quaternion, * )
 
 DEFINE_PROMOTE_BIND_OPERATOR2( quaternion, * )
 
 DEFINE_PROMOTE_BIND_OPERATOR3( quaternion, * )
 
 DEFINE_PROMOTE_BIND_OPERATOR1( quaternion, / )
 
 DEFINE_PROMOTE_BIND_OPERATOR2( quaternion, / )
 
 
 
 
 
 
 template < class T > inline std::ostream &operator <<( std::ostream &out, const quaternion< T > &q )
 {
     out << "( ";
     out << q.w << ", ";
     out << q.x << ", ";
     out << q.y << ", ";
     out << q.z << " )";
     return( out );
 }
 
 
 
 template < class T1, class T2 >
 const quaternion< double > interpolate( const quaternion< T1 > &q1, const quaternion< T2 > &q2, double t )
 {
     typedef quaternion< double > quaternion_type;
 
     quaternion_type Q1( q1.unit( ) );
     quaternion_type Q2( q2.unit( ) );
 
     double dot = Q1.inner( Q2 );
 
     if( std::abs( dot ) < 1.0e-6 )
     {
         return( Q1 );
     }
     else if( dot < 0.0 )
     {
         double theta = std::acos( dot );
 
         // 球面線形補間を行う
         return( quaternion_type( Q1 * std::sin( theta * ( 1.0 - t ) ) - Q2 * std::sin( theta * t ) ).unit( ) );
     }
     else
     {
         double theta = std::acos( dot );
 
         // 球面線形補間を行う
         return( quaternion_type( Q1 * std::sin( theta * ( 1.0 - t ) ) + Q2 * std::sin( theta * t ) ).unit( ) );
     }
 }
 
 
 // クォータニオンから行列へ変換する
 //template < class T >
 //matrix< T > rotate_matrix( const quaternion< T > &q )
 //{
 //  matrix< T > mat(  4, 4  );
 //  mat( 0, 0 ) = q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z;
 //  mat( 0, 1 ) = 2 * ( q.x * q.y - q.w * q.z );
 //  mat( 0, 2 ) = 2 * ( q.x * q.z + q.w * q.y );
 //  mat( 0, 3 ) = 0;
 //  mat( 1, 0 ) = 2 * ( q.x * q.y + q.w * q.z );
 //  mat( 1, 1 ) = q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z;
 //  mat( 1, 2 ) = 2 * ( q.y * q.z - q.w * q.x );
 //  mat( 1, 3 ) = 0;
 //  mat( 2, 0 ) = 2 * ( q.x * q.z - q.w * q.y );
 //  mat( 2, 1 ) = 2 * ( q.y * q.z + q.w * q.x );
 //  mat( 2, 2 ) = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z;
 //  mat( 2, 3 ) = 0;
 //  mat( 3, 0 ) = 0;
 //  mat( 3, 1 ) = 0;
 //  mat( 3, 2 ) = 0;
 //  mat( 3, 3 ) = 1;
 //  return( mat );
 //}
 
 // 
 // 
 //
 //
 
 
 template < class T >
 const quaternion< T > track_ball( const vector2< T > &p1, const vector2< T > &p2, const vector3< T > &axisX, const vector3< T > axisY, const vector3< T > axisZ, const typename vector3< T >::value_type &trackball_size )
 {
     typedef typename quaternion< T >::value_type value_type;
 
     if( p1 == p2 )
     {
         return( quaternion< T >( 1, 0, 0, 0 ) );
     }
 
     vector3< T > sp1( p1.x, p1.y, 0 ), sp2( p2.x, p2.y, 0 );
     value_type l, _2 = std::sqrt( value_type( 2.0 ) );
 
     // 点1の座標を仮想トラックボール上に投影
     l = p1.length( );
     if( l < trackball_size / _2 )
     {
         sp1.z = - std::sqrt( trackball_size * trackball_size - l * l );
     }
     else
     {
         sp1.z = - trackball_size * trackball_size / 2.0 / l;
     }
 
     // 点2の座標を仮想トラックボール上に投影
     l = p2.length( );
     if( l < trackball_size / _2 )
     {
         sp2.z = - std::sqrt( trackball_size * trackball_size - l * l );
     }
     else
     {
         sp2.z = - trackball_size * trackball_size / 2.0 / l;
     }
 
     //  sp1 = sp1.unit();
     //  sp2 = sp2.unit();
 
     // 右手系と左手系でここの外積の向きを反転させる
     //  Vector3<double> axis = (sp2 * sp1).unit();
     vector3< T > axis = ( sp1 * sp2 ).unit( );
     axis = ( axis.x * axisX + axis.y * axisY + axis.z * axisZ ).unit( );
 
     l = ( sp2 - sp1 ).length( ) / ( 2 * trackball_size );
     //  l = (l < -1.0)? -1.0: l;
     l = l > 1 ? 1: l;
 
     double phi = std::asin( l );
     //  fprintf(stdout, "axis(%.1f, %.1f, %.1f)   theta = %.1f\n", axis.x, axis.y, axis.z, phi * 180 / PAI);
     //  printf("%.1f\n", phi * 180 / PAI);
     return( quaternion< T >( std::cos( phi ), std::sin( phi ) * axis ) );
 }
 
 template < class T >
 inline const quaternion< T > track_ball( const vector2< T > &p1, const vector2< T > &p2, const vector3< T > &axisX, const vector3< T > axisY, const vector3< T > axisZ )
 {
     return( track_ball( p1, p2, axisX, axisY, axisZ, 0.8 ) );
 }
 
 
 template < class T >
 const quaternion< T > track_ball( const typename vector3< T >::value_type &x1, const typename vector3< T >::value_type &y1, const typename vector3< T >::value_type &x2, const typename vector3< T >::value_type &y2,
                                                 const vector3< T > &axisX, const vector3< T > &axisY, const vector3< T > &axisZ, const typename vector3< T >::value_type &trackball_size )
 {
     return( track_ball( vector2< T >( x1, y1 ), vector2< T >( x2, y2 ), axisX, axisY, axisZ, trackball_size ) );
 }
 
 
 template < class T >
 const quaternion< T > track_ball( const typename vector3< T >::value_type &x1, const typename vector3< T >::value_type &y1, const typename vector3< T >::value_type &x2,
                                             const typename vector3< T >::value_type &y2, const vector3< T > &axisX, const vector3< T > &axisY, const vector3< T > &axisZ )
 {
     return( track_ball( vector2< T >( x1, y1 ), vector2< T >( x2, y2 ), axisX, axisY, axisZ, 0.8 ) );
 }
 
 
 // mist名前空間の終わり
 _MIST_END
 
 #endif // __INCLUDE_MIST_QUATERNION__
 