00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #pragma once
00013
00015 #include "nvsgcommon.h"
00016
00017 #include "nvmath/nvmath.h"
00018 #include "nvmath/Vec4f.h"
00019 #include "nvmath/Vec3f.h"
00020 #include "nvmath/Mat33f.h"
00021
00022 namespace nvmath
00023 {
00025
00031 class Quatf
00032 {
00033 public:
00035
00036 NVSG_API Quatf( void );
00037
00039
00040 NVSG_API Quatf( const Vec4f& v
00041 );
00042
00044
00047 NVSG_API Quatf( float f0
00048 , float f1
00049 , float f2
00050 , float f3
00051 );
00052
00054
00055 NVSG_API Quatf( const Vec3f& axis
00056 , float angle
00057 );
00058
00060
00061 NVSG_API Quatf( const Vec3f& vec0
00062 , const Vec3f& vec1
00063 );
00064
00066
00067 NVSG_API float& operator[]( size_t i
00068 );
00069
00071
00072 NVSG_API const float& operator[]( size_t i
00073 ) const;
00074
00076
00077 NVSG_API Quatf& operator=( const Quatf& q
00078 );
00079
00081
00082 NVSG_API bool operator==( const Quatf& q
00083 ) const;
00084
00086
00087 NVSG_API bool operator!=( const Quatf& q
00088 ) const;
00089
00091
00092 NVSG_API Quatf& operator+=( const Quatf& q
00093 );
00094
00096
00097 NVSG_API Quatf& operator-=( const Quatf& q
00098 );
00099
00101
00102 NVSG_API Quatf operator-( const Quatf& q
00103 ) const;
00104
00106
00107 NVSG_API Quatf operator-( void ) const;
00108
00110
00111 NVSG_API Quatf& operator*=( float f
00112 );
00113
00115
00116 NVSG_API Quatf operator*( float f
00117 ) const;
00118
00120
00121 NVSG_API Quatf& operator*=( const Quatf& q
00122 );
00123
00125
00126 NVSG_API Quatf operator*( const Quatf& q
00127 ) const;
00128
00130
00131 NVSG_API Quatf& operator/=( float f
00132 );
00133
00135
00136 NVSG_API Quatf operator/( float f
00137 ) const;
00138
00140
00141 NVSG_API Quatf& operator/=( const Quatf& q
00142 );
00143
00145
00146 NVSG_API Quatf operator/( const Quatf& q
00147 ) const;
00148
00150
00152 NVSG_API Quatf operator~( void ) const;
00153
00155
00156 NVSG_API void invert( void );
00157
00159 NVSG_API Quatf& normalize( void );
00160
00161 private:
00162 NVSG_API void _byAxisAndAngle( const Vec3f& axis, float angle );
00163
00164 private:
00165 float m_quat[4];
00166 };
00167
00168
00169
00170
00173 inline void decompose( const Quatf &q
00174 , Vec3f &axis
00175 , float &angle
00176 )
00177 {
00178 angle = 2.0f * acosf( q[3] );
00179 if ( angle < FLT_EPSILON )
00180 {
00181
00182 axis[0] = 0.0f;
00183 axis[1] = 0.0f;
00184 axis[2] = 1.0f;
00185 }
00186 else
00187 {
00188 float dummy = 1.0f / sinf( 0.5f * angle );
00189 axis[0] = q[0] * dummy;
00190 axis[1] = q[1] * dummy;
00191 axis[2] = q[2] * dummy;
00192 axis.normalize();
00193 }
00194 }
00195
00199 inline float distance( const Quatf &q0
00200 , const Quatf &q1
00201 )
00202 {
00203 return( sqrtf( square( q0[0] - q1[0] ) + square( q0[1] - q1[1] ) + square( q0[2] - q1[2] ) + square( q0[3] - q1[3] ) ) );
00204 }
00205
00209 inline float magnitude( const Quatf &q
00210 )
00211 {
00212 return( sqrtf( square( q[0] ) + square( q[1] ) + square( q[2] ) + square( q[3] ) ) );
00213 }
00214
00219 inline bool isNormalized( const Quatf &q
00220 )
00221 {
00222 return( fabs( magnitude( q ) - 1.0f ) <= FLT_EPSILON );
00223 }
00224
00228 inline bool isNull( const Quatf &q
00229 )
00230 {
00231 return( magnitude( q ) <= FLT_EPSILON );
00232 }
00233
00237 inline Quatf operator*( float f
00238 , const Quatf &q
00239 )
00240 {
00241 return( q * f );
00242 }
00243
00248 inline Vec3f operator*( const Quatf &q, const Vec3f &v )
00249 {
00250 return Mat33f(q) * v;
00251 }
00252
00256 template<>
00257 NVSG_API Quatf lerp<Quatf>( float alpha
00258 , const Quatf &q0
00259 , const Quatf &q1
00260 );
00261
00262
00263
00264
00265 inline Quatf::Quatf( void )
00266 {
00267 }
00268
00269 inline Quatf::Quatf( float f0, float f1, float f2, float f3 )
00270 {
00271 m_quat[0] = f0;
00272 m_quat[1] = f1;
00273 m_quat[2] = f2;
00274 m_quat[3] = f3;
00275 }
00276
00277 inline Quatf::Quatf( const Vec4f& v )
00278 {
00279 m_quat[0] = v[0];
00280 m_quat[1] = v[1];
00281 m_quat[2] = v[2];
00282 m_quat[3] = v[3];
00283 __ASSERT( isNormalized( *this ) );
00284 }
00285
00286 inline void Quatf::_byAxisAndAngle( const Vec3f& axis, float angle )
00287 {
00288 __ASSERT( isNormalized( axis ) );
00289 float dummy = sinf( 0.5f * angle );
00290 m_quat[0] = axis[0] * dummy;
00291 m_quat[1] = axis[1] * dummy;
00292 m_quat[2] = axis[2] * dummy;
00293 m_quat[3] = cosf( 0.5f * angle );
00294 __ASSERT( isNormalized( *this ) );
00295 }
00296
00297 inline Quatf::Quatf( const Vec3f& axis, float angle )
00298 {
00299 _byAxisAndAngle( axis, angle );
00300 }
00301
00302 inline Quatf::Quatf( const Vec3f& vec0, const Vec3f& vec1 )
00303 {
00304 __ASSERT( isNormalized( vec0 ) && isNormalized( vec1 ) );
00305 if ( areCollinear( vec0, vec1 ) )
00306 {
00307 (*this) = Quatf( 0.0f, 0.0f, 0.0f, 1.0f );
00308 }
00309 else
00310 {
00311 _byAxisAndAngle( nvmath::normalize( vec0 ^ vec1 ), acosf( vec0 * vec1 ) );
00312 }
00313 }
00314
00315 inline float& Quatf::operator[]( size_t i )
00316 {
00317 __ASSERT( 0 <= i && i <= 3 );
00318 return( m_quat[i] );
00319 }
00320
00321 inline const float& Quatf::operator[]( size_t i ) const
00322 {
00323 __ASSERT( 0 <= i && i <= 3 );
00324 return( m_quat[i] );
00325 }
00326
00327 inline Quatf& Quatf::operator=( const Quatf& q )
00328 {
00329 m_quat[0] = q[0];
00330 m_quat[1] = q[1];
00331 m_quat[2] = q[2];
00332 m_quat[3] = q[3];
00333 return( *this );
00334 }
00335
00336 inline bool Quatf::operator==( const Quatf& q ) const
00337 {
00338 return( m_quat[0] == q[0]
00339 && m_quat[1] == q[1]
00340 && m_quat[2] == q[2]
00341 && m_quat[3] == q[3]
00342 );
00343 }
00344
00345 inline bool Quatf::operator!=( const Quatf& q ) const
00346 {
00347 return( m_quat[0] != q[0]
00348 || m_quat[1] != q[1]
00349 || m_quat[2] != q[2]
00350 || m_quat[3] != q[3]
00351 );
00352 }
00353
00354 inline Quatf Quatf::operator-( void ) const
00355 {
00356 return( Quatf( m_quat[0], m_quat[1], m_quat[2], -m_quat[3] ) );
00357 }
00358
00359 inline Quatf& Quatf::operator*=( const Quatf & q )
00360 {
00361 __ASSERT( isNormalized( *this ) && isNormalized( q ) );
00362 *this = Quatf( m_quat[1]*q[2] - m_quat[2]*q[1] + m_quat[3]*q[0] + m_quat[0]*q[3],
00363 m_quat[2]*q[0] - m_quat[0]*q[2] + m_quat[3]*q[1] + m_quat[1]*q[3],
00364 m_quat[0]*q[1] - m_quat[1]*q[0] + m_quat[3]*q[2] + m_quat[2]*q[3],
00365 m_quat[3]*q[3] - m_quat[0]*q[0] - m_quat[1]*q[1] - m_quat[2]*q[2] );
00366 normalize();
00367 return( *this );
00368 }
00369
00370 inline Quatf Quatf::operator*( const Quatf & q ) const
00371 {
00372 __ASSERT( isNormalized( *this ) && isNormalized( q ) );
00373 Quatf r(*this);
00374 r *= q;
00375 __ASSERT( isNormalized( r ) );
00376 return( r );
00377 }
00378
00379 inline Quatf& Quatf::operator/=( const Quatf & q )
00380 {
00381 Quatf tmp = *this / q;
00382 *this = tmp;
00383 return( *this );
00384 }
00385
00386 inline Quatf Quatf::operator/( const Quatf & q ) const
00387 {
00388 __ASSERT( fabsf( magnitude( q ) - 1.0f ) < FLT_EPSILON );
00389 return( *this * ~q );
00390 }
00391
00392 inline Quatf Quatf::operator~( void ) const
00393 {
00394 return( Quatf( -m_quat[0], -m_quat[1], -m_quat[2], m_quat[3] ) );
00395 }
00396
00397 inline void Quatf::invert( void )
00398 {
00399 __ASSERT( fabsf( magnitude( *this ) - 1.0f ) < FLT_EPSILON );
00400 *this = ~(*this);
00401 }
00402
00407 inline Quatf& Quatf::normalize( void )
00408 {
00409 float mag = magnitude( *this );
00410 __ASSERT( mag > FLT_EPSILON );
00411 for (int i = 0; i < 4; i++)
00412 {
00413 m_quat[i] /= mag;
00414 }
00415 return( *this );
00416 }
00417 }