• メインページ
  • クラス
  • ファイル
  • ファイル一覧
  • ファイルメンバ

narQuat.h

00001 /*-----------------------------------------------------------------------*
00002   Project:  Nintendo Augmented Reality Library.
00003   File:     narQuat.h
00004 
00005   Copyright (C)2011-2012 Nintendo Co., Ltd.  All rights reserved.
00006 
00007   These coded instructions, statements, and computer programs contain
00008   proprietary information of Nintendo and/or its licensed developers
00009   and are protected by national and international copyright laws. They
00010   may not be disclosed to third parties or copied or duplicated in any
00011   form, in whole or in part, without the prior written consent of
00012   Nintendo.
00013   The content herein is highly confidential and should be handled
00014   accordingly.
00015 *-----------------------------------------------------------------------*/
00016 
00018 
00023 #ifndef NAR_QUAT_H__
00024 #define NAR_QUAT_H__
00025 
00026 #include "narConfig.h"
00027 #include "narMath.h"
00028 
00029 namespace mw { namespace nar
00030 {
00034     struct Quat_st
00035     {
00036         Quat_st()
00037         {}
00038         
00039         Quat_st( const Vec3F_st & cr_Xa, const Vec3F_st & cr_Ya, const Vec3F_st & cr_Za )
00040         {
00041             SetFromAxises( cr_Xa, cr_Ya, cr_Za );
00042         }
00043         
00046         void Set( f32 w, f32 x, f32 y, f32 z )
00047         {
00048              W() = w; X() = x; Y() = y; Z() = z;
00049         }
00050 
00053         void SetAll( f32 v )
00054         {
00055             Set( v, v, v, v );
00056         }
00057         
00060         void SetFromAxises( const Vec3F_st & cr_Xa, const Vec3F_st & cr_Ya, const Vec3F_st & cr_Za )
00061         {
00062             f32 tr = cr_Xa.X() + cr_Ya.Y() + cr_Za.Z();
00063             f32 s;
00064             
00065             if ( tr > 0.f )
00066             {
00067                 s = Sqrt( tr + 1.f );
00068                 W() = s * 0.5f;
00069                 s   = 0.5f / s;
00070                 X() = ( cr_Ya.Z() - cr_Za.Y() ) * s;
00071                 Y() = ( cr_Za.X() - cr_Xa.Z() ) * s;
00072                 Z() = ( cr_Xa.Y() - cr_Ya.X() ) * s;
00073             }
00074             else
00075             {
00076                 int i = 0;
00077                 if ( cr_Ya.Y() > cr_Xa.X() )
00078                 {
00079                     i = 1;
00080                     if ( cr_Za.Z() > cr_Ya.Y() ) i = 2;
00081                 }
00082                 else
00083                 {
00084                     if ( cr_Za.Z() > cr_Xa.X() ) i = 2;
00085                 }
00086 
00087                 switch ( i )
00088                 {
00089                 case 0: //  x
00090                     s   = Sqrt( ( cr_Xa.X() - ( cr_Ya.Y() + cr_Za.Z() ) ) + 1.f );
00091                     X() = s * 0.5f;
00092                     s = 0.5f / s;
00093                     Y() = ( cr_Xa.Y() + cr_Ya.X() ) * s;
00094                     Z() = ( cr_Za.X() + cr_Xa.Z() ) * s;
00095                     W() = ( cr_Ya.Z() - cr_Za.Y() ) * s;
00096                     break;
00097                 case 1: //  y
00098                     s   = Sqrt( ( cr_Ya.Y() - ( cr_Za.Z() + cr_Xa.X() ) ) + 1.f );
00099                     Y() = s * 0.5f;
00100                     s = 0.5f / s;
00101                     X() = ( cr_Xa.Y() + cr_Ya.X() ) * s;
00102                     Z() = ( cr_Ya.Z() + cr_Za.Y() ) * s;
00103                     W() = ( cr_Za.X() - cr_Xa.Z() ) * s;
00104                     break;
00105                 case 2: //  z
00106                     s   = Sqrt( ( cr_Za.Z() - ( cr_Xa.X() + cr_Ya.Y() ) ) + 1.f );
00107                     Z() = s * 0.5f;
00108                     s = 0.5f / s;
00109                     X() = ( cr_Za.X() + cr_Xa.Z() ) * s;
00110                     Y() = ( cr_Ya.Z() + cr_Za.Y() ) * s;
00111                     W() = ( cr_Xa.Y() - cr_Ya.X() ) * s;
00112                     break;
00113                 }
00114             }
00115         }
00116 
00119         void GetAxises( Vec3F_st & r_Xa, Vec3F_st & r_Ya, Vec3F_st & r_Za ) const
00120         {
00121             f32 s,xs,ys,zs,wx,wy,wz,xx,xy,xz,yy,yz,zz;
00122             s = 2.f / Dot( *this );
00123 
00124             xs = X() *  s;     ys = Y() *  s;  zs = Z() *  s;
00125             wx = W() * xs;     wy = W() * ys;  wz = W() * zs;
00126             xx = X() * xs;     xy = X() * ys;  xz = X() * zs;
00127             yy = Y() * ys;     yz = Y() * zs;  zz = Z() * zs;
00128 
00129             r_Xa.Set( 1.f - ( yy + zz ), xy + wz, xz - wy );
00130             r_Ya.Set( xy - wz, 1.f - ( xx + zz ), yz + wx );
00131             r_Za.Set( xz + wy, yz - wx, 1.f - ( xx + yy ) );
00132         }
00133 
00136         f32 Dot( const Quat_st & cr ) const
00137         {
00138             return X() * cr.X() + Y() * cr.Y() + Z() * cr.Z() + W() * cr.W();
00139         }
00140 
00143         void Scale( f32 scale )
00144         {
00145             W() *= scale;
00146             X() *= scale;
00147             Y() *= scale;
00148             Z() *= scale;
00149         }
00150 
00151 
00154         f32 SquareNorm() const
00155         {
00156             return Dot( *this );
00157         }
00158 
00159         
00162         void Normalize()
00163         {
00164             f32 mag = SquareNorm();
00165 
00166             if ( mag >= c_Epsilon )
00167             {
00168                 mag = 1.f / Sqrt( mag );
00169 
00170                 Scale( mag );
00171             }
00172             else
00173             {
00174                 SetAll( 0.f );
00175             }
00176         }
00177         
00183         void Slerp( const Quat_st & cr_P, const Quat_st & cr_Q, f32 t )
00184         {
00185             f32 cosTh = cr_P.Dot( cr_Q );
00186             f32 tp;
00187             f32 tq = 1.f;
00188             
00189             if ( cosTh < 0.f )
00190             {
00191                 cosTh = -cosTh;
00192                 tq    = -tq;
00193             }
00194             
00195             if ( cosTh <= 1.f - c_Epsilon )
00196             {
00197                 f32 theta = ACos( cosTh );
00198                 f32 sinTh = Sin( theta );
00199                 tp  = Sin( ( 1.f - t ) * theta ) / sinTh;
00200                 tq *= Sin( t * theta ) / sinTh;
00201             }
00202             else
00203             {
00204                 tp  = 1.f - t;
00205                 tq *= t;
00206             }
00207 
00208             W() = tp * cr_P.W() + tq * cr_Q.W();
00209             X() = tp * cr_P.X() + tq * cr_Q.X();
00210             Y() = tp * cr_P.Y() + tq * cr_Q.Y();
00211             Z() = tp * cr_P.Z() + tq * cr_Q.Z();
00212         }
00213 
00219         f32 GetRotate( Vec3F_st & r_Vec )
00220         {
00221             f32 theta_2     = ACos( W() );
00222             f32 sinTheta_2  = Sin( theta_2 );
00223 
00224             r_Vec.Set( X(), Y(), Z() );
00225             if ( sinTheta_2 != 0.f )
00226                 r_Vec.Div( sinTheta_2 );
00227 
00228             return 2.f * theta_2;
00229         }
00230 
00233         void Mult( const Quat_st & cr )
00234         {
00235             Set( W() * cr.W() - X() * cr.X() - Y() * cr.Y() - Z() * cr.Z(),
00236                  W() * cr.X() + X() * cr.W() + Y() * cr.Z() - Z() * cr.Y(),
00237                  W() * cr.Y() + Y() * cr.W() + Z() * cr.X() - X() * cr.Z(),
00238                  W() * cr.Z() + Z() * cr.W() + X() * cr.Y() - Y() * cr.X() );
00239         }
00240 
00243         void SetConjugate( const Quat_st & cr )
00244         {
00245             Set( cr.W(), -cr.X(), -cr.Y(), -cr.Z() );
00246         }
00247 
00250         void SetInverse( const Quat_st & cr )
00251         {
00252             f32 sqNorm = cr.SquareNorm();
00253             if ( sqNorm > c_Epsilon )
00254             {
00255                 f32 sqNormInv = 1.f / sqNorm;
00256                 W() =   cr.W() * sqNormInv;
00257                 X() = - cr.X() * sqNormInv;
00258                 Y() = - cr.Y() * sqNormInv;
00259                 Z() = - cr.Z() * sqNormInv;
00260             }
00261             else
00262             {
00263                 SetConjugate( cr );
00264             }
00265         }
00266         
00267         f32 & W() { return ma_[ 0 ]; }
00268         f32 & X() { return ma_[ 1 ]; }
00269         f32 & Y() { return ma_[ 2 ]; }
00270         f32 & Z() { return ma_[ 3 ]; }
00271         const f32 & W() const { return ma_[ 0 ]; }
00272         const f32 & X() const { return ma_[ 1 ]; }
00273         const f32 & Y() const { return ma_[ 2 ]; }
00274         const f32 & Z() const { return ma_[ 3 ]; }
00275 
00276         f32 ma_[ 4 ];
00277     };
00278 }
00279 }
00280 #endif
00281 

© 2011-2012 Nintendo Co., Ltd. All rights reserved.