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. All rights reserved.
CONFIDENTIAL