xsi_rotationf.h
Go to the documentation of this file.

    00001 //*****************************************************************************
    00011 //*****************************************************************************
    00012 #ifndef __XSIROTATIONF_H__
    00013 #define __XSIROTATIONF_H__
    00014
    00015 #include "sicppsdk.h"
    00016 #include "xsi_math.h"
    00017 #include "xsi_vector3f.h"
    00018 #include "xsi_vector4f.h"
    00019 #include "xsi_matrix3f.h"
    00020 #include "xsi_quaternionf.h"
    00021
    00022 namespace XSI {
    00023
    00024 namespace MATH {
    00025
    00026 //*****************************************************************************
    00036 //*****************************************************************************
    00037 class SICPPSDKDECL CRotationf
    00038 {
    00039 public:
    00042     enum RotationOrder
    00043     {
    00044         siXYZ = 0,
    00045         siXZY = 1,
    00046         siYXZ = 2,
    00047         siYZX = 3,
    00048         siZXY = 4,
    00049         siZYX = 5
    00050     };
    00051
    00054     enum RotationRep
    00055     {
    00056         siQuaternionRot = 0,
    00057         siEulerRot,
    00058         siAxisAngleRot
    00059     };
    00060
    00062     SICPPSDK_INLINE CRotationf();
    00063
    00067     SICPPSDK_INLINE CRotationf( const CRotationf& in_rotation);
    00068
    00074     SICPPSDK_INLINE CRotationf( const CVector3f &in_EulerAngles, const RotationOrder in_RotOrder = siXYZ);
    00075
    00083     SICPPSDK_INLINE CRotationf( const float in_X, const float in_Y, const float in_Z, const RotationOrder in_RotOrder = siXYZ);
    00084
    00089     SICPPSDK_INLINE CRotationf( const CQuaternionf &in_Quat );
    00090
    00096     SICPPSDK_INLINE CRotationf( const CVector3f &in_Axis, const float in_Angle );
    00097
    00099     SICPPSDK_INLINE ~CRotationf();
    00100
    00105     SICPPSDK_INLINE CRotationf & operator=( const CRotationf & in_rotation);
    00106
    00112     SICPPSDK_INLINE CVector3f GetXYZAngles(const RotationOrder in_Order) const;
    00113
    00120     SICPPSDK_INLINE void GetXYZAngles( float &io_X, float &io_Y, float &io_Z ) const;
    00121
    00126     SICPPSDK_INLINE CVector3f GetXYZAngles( ) const;
    00127
    00131     SICPPSDK_INLINE RotationOrder GetOrder() const;
    00132
    00137     SICPPSDK_INLINE void SetOrder( const RotationOrder in_rotOrder );
    00138
    00142     SICPPSDK_INLINE RotationRep GetRepresentation() const;
    00143
    00149     SICPPSDK_INLINE void SetRepresentation( const RotationRep in_rotRep, const RotationOrder in_order = siXYZ );
    00150
    00161     SICPPSDK_INLINE CRotationf& Set( const RotationOrder in_order, const bool in_bCompensate = false );
    00162
    00169     SICPPSDK_INLINE CRotationf& Set( const CVector3f &in_EulerAngles, const RotationOrder in_Order = siXYZ);
    00170
    00179     SICPPSDK_INLINE CRotationf& Set( const float in_X, const float in_Y, const float in_Z, const RotationOrder in_Order = siXYZ);
    00180
    00186     SICPPSDK_INLINE CRotationf& Set( const CQuaternionf &in_Quat);
    00187
    00192     SICPPSDK_INLINE CRotationf& Mul( const CRotationf &in_rot );
    00193
    00199     SICPPSDK_INLINE CRotationf& Mul( const CRotationf &in_rot1, const CRotationf &in_rot2 );
    00200
    00205     SICPPSDK_INLINE CRotationf& Mul( const float in_scalar );
    00206
    00211     SICPPSDK_INLINE CRotationf& LeftMul( const CRotationf &in_rot );
    00212
    00219     SICPPSDK_INLINE CRotationf& Interpolate( const CRotationf& in_rot1, const CRotationf& in_rot2, const float in_u );
    00220
    00226     SICPPSDK_INLINE CRotationf& Div ( const CRotationf& in_rot1, const float in_scalar );
    00227
    00232     SICPPSDK_INLINE CRotationf& Div ( const float in_scalar );
    00233
    00238     SICPPSDK_INLINE CRotationf& Div( const CRotationf& in_rot );
    00239
    00245     SICPPSDK_INLINE CRotationf& Div( const CRotationf& in_rot1, const CRotationf& in_rot2 );
    00246
    00250     SICPPSDK_INLINE CRotationf& Invert();
    00251
    00256     SICPPSDK_INLINE CRotationf& Invert( const CRotationf &in_rot );
    00257
    00262     SICPPSDK_INLINE CQuaternionf GetQuaternion() const;
    00263
    00269     SICPPSDK_INLINE CRotationf& operator = ( const CQuaternionf &in_Quat );
    00270
    00276     SICPPSDK_INLINE CVector3f GetAxisAngle( float &io_Angle) const;
    00277
    00284     SICPPSDK_INLINE CRotationf& Set( const CVector3f &in_Axis,  const float in_Angle );
    00285
    00288     SICPPSDK_INLINE void SetIdentity();
    00289
    00295     SICPPSDK_INLINE bool Equals( const CRotationf &in_rot ) const;
    00296
    00302     SICPPSDK_INLINE bool EpsilonEquals( const CRotationf& in_rot, const float in_fEpsilon ) const;
    00303
    00309     SICPPSDK_INLINE bool operator == ( const CRotationf &in_rot ) const;
    00310
    00316     SICPPSDK_INLINE bool operator !=(const CRotationf & in_rot ) const;
    00317
    00325     SICPPSDK_INLINE bool operator < ( const CRotationf& in_rot ) const;
    00326
    00327 private:
    00328     SICPPSDK_INLINE void QuatToEuler( const CQuaternionf& in_quat, CVector3f& out_vct, const RotationOrder in_eOrder );
    00329     SICPPSDK_INLINE void QuatToMat( const CQuaternionf& in_quat, CMatrix3f& out_mat );
    00330     SICPPSDK_INLINE void AxisAngleToQuat( const CVector3f& in_vctAxis, const float in_fAngle, CQuaternionf& out_quat);
    00331     SICPPSDK_INLINE void QuatToAxisAngle(const CQuaternionf& in_quat, CVector3f& out_vctAxis, float& out_fAngle );
    00332
    00333     SICPPSDK_INLINE RotationOrder GetRotOrder() const;
    00334
    00335     float m_X, m_Y, m_Z;
    00336     union
    00337     {
    00338         float           m_W;            // Quaternion W component
    00339         float           m_Angle;        // Angle for axis/angle
    00340         RotationOrder   m_rotOrder;     // Euler rotation order
    00341     };
    00342     unsigned char m_rotRep;             // Rotation representation
    00343 };
    00344
    00345 SICPPSDK_INLINE CRotationf::CRotationf()
    00346 {
    00347     m_X = m_Y = m_Z = 0.0;
    00348     m_W = 1.0;
    00349     m_rotRep = siQuaternionRot;
    00350 }
    00351
    00352 SICPPSDK_INLINE CRotationf::CRotationf( const CRotationf& in_rotation)
    00353 {
    00354     *this = in_rotation;
    00355 }
    00356
    00357 SICPPSDK_INLINE CRotationf::CRotationf( const CQuaternionf& in_quat )
    00358 {
    00359     Set( in_quat );
    00360 }
    00361
    00362 SICPPSDK_INLINE CRotationf::CRotationf( const CVector3f &in_Angles, const RotationOrder in_Order)
    00363 {
    00364     Set( in_Angles, in_Order );
    00365 }
    00366
    00367 SICPPSDK_INLINE CRotationf::CRotationf( const CVector3f &in_Axis, const float in_fAngle )
    00368 {
    00369     Set( in_Axis, in_fAngle );
    00370 }
    00371
    00372 SICPPSDK_INLINE CRotationf::CRotationf( float in_fX, float in_fY, float in_fZ, const RotationOrder in_Order )
    00373 {
    00374     Set( in_fX, in_fY, in_fZ, in_Order );
    00375 }
    00376
    00377 SICPPSDK_INLINE CRotationf::~CRotationf()
    00378 {
    00379 }
    00380
    00381 SICPPSDK_INLINE CRotationf& CRotationf::operator=( const CRotationf & in_rotation)
    00382 {
    00383     m_X = in_rotation.m_X;
    00384     m_Y = in_rotation.m_Y;
    00385     m_Z = in_rotation.m_Z;
    00386     m_W = in_rotation.m_W;
    00387     m_rotRep = in_rotation.m_rotRep;
    00388     return *this;
    00389 }
    00390
    00391 SICPPSDK_INLINE CRotationf& CRotationf::Set( const CVector3f &in_AngleValues, const RotationOrder in_Order )
    00392 {
    00393     return Set( in_AngleValues.GetX(), in_AngleValues.GetY(), in_AngleValues.GetZ(), in_Order );
    00394 }
    00395
    00396 SICPPSDK_INLINE CRotationf& CRotationf::Set
    00397 (
    00398     const float in_fX,
    00399     const float in_fY,
    00400     const float in_fZ,
    00401     const RotationOrder in_Order
    00402 )
    00403 {
    00404     m_rotRep = siEulerRot;
    00405     m_rotOrder = in_Order;
    00406     m_X = in_fX; m_Y = in_fY; m_Z = in_fZ;
    00407
    00408     return *this;
    00409 }
    00410
    00411 SICPPSDK_INLINE CRotationf& CRotationf::Set( const RotationOrder in_order, const bool in_bCompensate )
    00412 {
    00413     if ( m_rotRep != siEulerRot )
    00414     {
    00415         SetRepresentation( siEulerRot, in_bCompensate ? in_order : siXYZ );
    00416     }
    00417
    00418     if ( in_order != m_rotRep )
    00419     {
    00420         if ( in_bCompensate )
    00421         {
    00422             CMatrix3f vRotTmp;
    00423             CVector3f vEuler( m_X, m_Y, m_Z );
    00424             XSI::MATH::EulerToMat( vEuler, vRotTmp, (int)m_rotOrder );
    00425
    00426             vEuler.SetNull( );
    00427             XSI::MATH::MatToEuler( vRotTmp, vEuler, in_order );
    00428             vEuler.Get( m_X, m_Y, m_Z );
    00429         }
    00430         m_rotOrder = in_order;
    00431     }
    00432     return *this;
    00433 }
    00434
    00435 SICPPSDK_INLINE CRotationf& CRotationf::Set( const CQuaternionf &in_Quat )
    00436 {
    00437     m_rotRep = siQuaternionRot;
    00438
    00439     m_W = in_Quat.GetW();
    00440     m_X = in_Quat.GetX();
    00441     m_Y = in_Quat.GetY();
    00442     m_Z = in_Quat.GetZ();
    00443
    00444     return *this;
    00445 }
    00446
    00447 SICPPSDK_INLINE CRotationf::RotationOrder CRotationf::GetRotOrder() const
    00448 {
    00449     return ( m_rotRep == siEulerRot ? m_rotOrder : siXYZ );
    00450 }
    00451
    00452 SICPPSDK_INLINE CVector3f CRotationf::GetXYZAngles(const RotationOrder in_Order) const
    00453 {
    00454     CVector3f outV;
    00455     if ( m_rotRep == siEulerRot && m_rotOrder == in_Order )
    00456     {
    00457         outV.Set( m_X, m_Y, m_Z );
    00458     }
    00459     else
    00460     {
    00461         CRotationf rot( *this );
    00462         rot.Set( in_Order, true );
    00463         outV.Set( rot.m_X, rot.m_Y, rot.m_Z );
    00464     }
    00465     return outV;
    00466 }
    00467
    00468 SICPPSDK_INLINE void CRotationf::GetXYZAngles( float &io_fX, float &io_fY, float &io_fZ ) const
    00469 {
    00470     CVector3f vEuler = GetXYZAngles( GetRotOrder() );
    00471     vEuler.Get( io_fX, io_fY, io_fZ );
    00472 }
    00473
    00474 SICPPSDK_INLINE CVector3f CRotationf::GetXYZAngles() const
    00475 {
    00476     return GetXYZAngles( GetRotOrder() );
    00477 }
    00478
    00479 SICPPSDK_INLINE CRotationf& CRotationf::Mul( const CRotationf &in_rot )
    00480 {
    00481     return Mul( *this, in_rot );
    00482 }
    00483
    00484 SICPPSDK_INLINE CRotationf& CRotationf::Mul( const CRotationf &in_rot1, const CRotationf &in_rot2 )
    00485 {
    00486     CQuaternionf q1 = in_rot1.GetQuaternion();
    00487     CQuaternionf q2 = in_rot2.GetQuaternion();
    00488     q2.Mul( q1 ).NormalizeInPlace();
    00489     return Set( q2 );
    00490 }
    00491
    00492 SICPPSDK_INLINE CRotationf& CRotationf::Mul( const float in_scalar )
    00493 {
    00494     switch (m_rotRep)
    00495     {
    00496         case siQuaternionRot:
    00497         case siEulerRot:
    00498         {
    00499             CRotationf identity;
    00500             Interpolate( identity, *this, in_scalar );
    00501         }
    00502         break;
    00503
    00504         case siAxisAngleRot:
    00505         {
    00506             m_Angle *= in_scalar;
    00507         }
    00508         break;
    00509     }
    00510     return *this;
    00511 }
    00512
    00513 SICPPSDK_INLINE CRotationf& CRotationf::Div( const CRotationf &in_rot )
    00514 {
    00515     return Div( *this, in_rot );
    00516 }
    00517
    00518 SICPPSDK_INLINE CRotationf& CRotationf::Div( const float in_scalar )
    00519 {
    00520     return Mul( 1.0f/in_scalar );
    00521 }
    00522
    00523 SICPPSDK_INLINE CRotationf& CRotationf::Div( const CRotationf &in_rot1, const CRotationf &in_rot2 )
    00524 {
    00525     CRotationf rotInv;
    00526     return Mul( in_rot1, rotInv.Invert(in_rot2) );
    00527 }
    00528
    00529 SICPPSDK_INLINE CRotationf& CRotationf::LeftMul( const CRotationf &in_rot )
    00530 {
    00531     return Mul( in_rot, *this );
    00532 }
    00533
    00534 SICPPSDK_INLINE CRotationf& CRotationf::Interpolate ( const CRotationf& in_rot1, const CRotationf& in_rot2, const float in_u )
    00535 {
    00536     CQuaternionf q1 = in_rot1.GetQuaternion();
    00537     CQuaternionf q2 = in_rot2.GetQuaternion();
    00538     CQuaternionf q;
    00539     q.Slerp( q1, q2, in_u );
    00540     return Set( q );
    00541 }
    00542
    00543 SICPPSDK_INLINE CRotationf& CRotationf::Invert( )
    00544 {
    00545     static RotationOrder InvRotOrder[] = { siZYX, siYZX, siZXY, siXZY, siYXZ, siXYZ };
    00546
    00547     switch (m_rotRep)
    00548     {
    00549         case siQuaternionRot:
    00550             m_X = -m_X; m_Y = -m_Y; m_Z = -m_Z;
    00551             break;
    00552         case siEulerRot:
    00553             m_rotOrder = InvRotOrder[m_rotOrder];
    00554             m_X = -m_X; m_Y = -m_Y; m_Z = -m_Z;
    00555             break;
    00556         case siAxisAngleRot:
    00557             m_Angle = -m_Angle;
    00558             break;
    00559     }
    00560     return *this;
    00561 }
    00562
    00563 SICPPSDK_INLINE CRotationf& CRotationf::Invert( const CRotationf &in_rot )
    00564 {
    00565     *this = in_rot;
    00566     return Invert( );
    00567 }
    00568
    00569 SICPPSDK_INLINE CVector3f CRotationf::GetAxisAngle( float &io_fAngle ) const
    00570 {
    00571     CVector3f out_axis;
    00572     if ( m_rotRep == siAxisAngleRot )
    00573     {
    00574         out_axis.Set( m_X, m_Y, m_Z );
    00575         io_fAngle = m_Angle;
    00576     }
    00577     else
    00578     {
    00579         CRotationf rot( *this );
    00580         rot.SetRepresentation( siAxisAngleRot );
    00581         out_axis.Set( rot.m_X, rot.m_Y, rot.m_Z );
    00582         io_fAngle = rot.m_Angle;
    00583     }
    00584     return out_axis;
    00585 }
    00586
    00587 SICPPSDK_INLINE CRotationf& CRotationf::Set( const CVector3f &in_axis, const float in_fAngle )
    00588 {
    00589     m_rotRep = siAxisAngleRot;
    00590     m_Angle = in_fAngle;
    00591     in_axis.Get( m_X, m_Y, m_Z );
    00592
    00593     return *this;
    00594 }
    00595
    00596 SICPPSDK_INLINE void CRotationf::SetIdentity()
    00597 {
    00598     switch (m_rotRep)
    00599     {
    00600         case siQuaternionRot:
    00601             m_W = 1.0; m_X = m_Y = m_Z = 0.0;
    00602             break;
    00603         case siEulerRot:
    00604             m_X = m_Y = m_Z = 0.0; m_rotOrder = siXYZ;
    00605             break;
    00606         case siAxisAngleRot:
    00607             m_Angle = 0.0; m_X = 1.0; m_Y = m_Z = 0.0;
    00608             break;
    00609     }
    00610 }
    00611
    00612 SICPPSDK_INLINE CRotationf::RotationOrder CRotationf::GetOrder() const
    00613 {
    00614     return m_rotOrder;
    00615 }
    00616
    00617 SICPPSDK_INLINE CRotationf::RotationRep CRotationf::GetRepresentation() const
    00618 {
    00619     return (CRotationf::RotationRep)m_rotRep;
    00620 }
    00621
    00622 void CRotationf::SetRepresentation( const RotationRep in_rep, const RotationOrder in_order )
    00623 {
    00624     if ( m_rotRep != in_rep )
    00625     {
    00626         switch( in_rep )
    00627         {
    00628             case siQuaternionRot:
    00629             {
    00630                 CQuaternionf l_quat;
    00631                 switch ( m_rotRep )
    00632                 {
    00633                     case siEulerRot:
    00634                     {
    00635                         CVector3f l_euler( m_X, m_Y, m_Z );
    00636                         XSI::MATH::EulerToQuat( l_euler, l_quat, m_rotOrder );
    00637                     }
    00638                     break;
    00639                     case siAxisAngleRot:
    00640                     {
    00641                         CVector3f l_axis( m_X, m_Y, m_Z );
    00642                         if ( l_axis.GetLength() > MicroEPS )
    00643                         {
    00644                             l_axis.NormalizeInPlace();
    00645                             AxisAngleToQuat( l_axis, m_Angle, l_quat );
    00646                         }
    00647                     }
    00648                     break;
    00649                 }
    00650                 *this = l_quat;
    00651             }
    00652             break;
    00653
    00654             case siEulerRot:
    00655             {
    00656                 CVector3f l_euler;
    00657                 switch ( m_rotRep )
    00658                 {
    00659                     case siQuaternionRot:
    00660                     {
    00661                         CQuaternionf l_quat( m_W, m_X, m_Y, m_Z );
    00662                         l_quat.NormalizeInPlace();
    00663                         QuatToEuler( l_quat, l_euler, in_order );
    00664                     }
    00665                     break;
    00666                     case siAxisAngleRot:
    00667                     {
    00668                         CQuaternionf l_quat;
    00669                         CVector3f l_axis( m_X, m_Y, m_Z );
    00670                         if ( l_axis.GetLength() > MicroEPS )
    00671                         {
    00672                             l_axis.NormalizeInPlace();
    00673                             AxisAngleToQuat( l_axis, m_Angle, l_quat );
    00674                         }
    00675                         QuatToEuler( l_quat, l_euler, in_order );
    00676                     }
    00677                     break;
    00678                 }
    00679                 Set( l_euler, in_order );
    00680             }
    00681             break;
    00682             case siAxisAngleRot:
    00683             {
    00684                 float l_angle = 0;
    00685                 CVector3f l_axis;
    00686                 switch ( m_rotRep )
    00687                 {
    00688                     case siQuaternionRot:
    00689                     {
    00690                         CQuaternionf l_quat( m_W, m_X, m_Y, m_Z );
    00691                         l_quat.NormalizeInPlace();
    00692                         QuatToAxisAngle( l_quat, l_axis, l_angle );
    00693                     }
    00694                     break;
    00695
    00696                     case siEulerRot:
    00697                     {
    00698                         CQuaternionf l_quat;
    00699                         CVector3f l_euler( m_X, m_Y, m_Z );
    00700                         XSI::MATH::EulerToQuat( l_euler, l_quat, m_rotOrder );
    00701                         QuatToAxisAngle( l_quat, l_axis, l_angle );
    00702                     }
    00703                     break;
    00704                 }
    00705
    00706                 if ( l_axis.IsNull() )
    00707                 {
    00708                     l_axis.Set(1.0, 0.0, 0.0); // If axis is undefined set it to a valid axis
    00709                 }
    00710                 Set( l_axis, l_angle );
    00711             }
    00712             break;
    00713         }
    00714     }
    00715 }
    00716
    00717 SICPPSDK_INLINE CQuaternionf CRotationf::GetQuaternion() const
    00718 {
    00719     CQuaternionf out_quat;
    00720     if ( m_rotRep == siQuaternionRot )
    00721     {
    00722         out_quat.Set( m_W, m_X, m_Y, m_Z );
    00723     }
    00724     else
    00725     {
    00726         CRotationf rot( *this );
    00727         rot.SetRepresentation( siQuaternionRot );
    00728         out_quat.Set( rot.m_W, rot.m_X, rot.m_Y, rot.m_Z );
    00729     }
    00730     return out_quat;
    00731 }
    00732
    00733 SICPPSDK_INLINE CRotationf& CRotationf::operator = ( const CQuaternionf &in_Quat )
    00734 {
    00735     return Set( in_Quat );
    00736 }
    00737
    00738 SICPPSDK_INLINE bool CRotationf::Equals( const CRotationf &in_rot ) const
    00739 {
    00740     if (this == &in_rot) return true;
    00741
    00742     CQuaternionf q1 = GetQuaternion();
    00743     CQuaternionf q2 = in_rot.GetQuaternion();
    00744
    00745     return (true == ( q1 == q2) );
    00746 }
    00747
    00748 SICPPSDK_INLINE bool CRotationf::EpsilonEquals( const CRotationf& in_rot, const float in_fEpsilon ) const
    00749 {
    00750     CRotationf r1( *this );
    00751     CRotationf r2( in_rot );
    00752     if ( r1.GetRepresentation() != r2.GetRepresentation() )
    00753     {
    00754         r2.SetRepresentation( r1.GetRepresentation() );
    00755     }
    00756     switch (r1.GetRepresentation())
    00757     {
    00758         case siQuaternionRot:
    00759         {
    00760             return r1.GetQuaternion().EpsilonEquals( r2.GetQuaternion(), in_fEpsilon );
    00761         }
    00762         case siEulerRot:
    00763         {
    00764             return ( r1.GetOrder() == r2.GetOrder() && r1.GetXYZAngles().EpsilonEquals( r2.GetXYZAngles(), in_fEpsilon ) );
    00765         }
    00766         case siAxisAngleRot:
    00767         {
    00768             float a1, a2;
    00769             CVector3f v1 = r1.GetAxisAngle( a1 );
    00770             CVector3f v2 = r2.GetAxisAngle( a2 );
    00771             return ( fabs( a1 - a2 ) < in_fEpsilon && v1.EpsilonEquals( v2, in_fEpsilon ) );
    00772         }
    00773         default:
    00774             break;
    00775     }
    00776     return false;
    00777 }
    00778
    00779 SICPPSDK_INLINE bool CRotationf::operator == ( const CRotationf &in_rot ) const
    00780 {
    00781     return Equals( in_rot );
    00782 }
    00783
    00784 SICPPSDK_INLINE bool CRotationf::operator != ( const CRotationf &in_rot ) const
    00785 {
    00786     return ! Equals( in_rot );
    00787 }
    00788
    00789 SICPPSDK_INLINE bool CRotationf::operator < ( const CRotationf& in_rot) const
    00790 {
    00791     if ( m_W != in_rot.m_W ) return m_W < in_rot.m_W;
    00792     if ( m_Z != in_rot.m_Z ) return m_Z < in_rot.m_Z;
    00793     if ( m_Y != in_rot.m_Y ) return m_Y < in_rot.m_Y;
    00794     if ( m_X != in_rot.m_X ) return m_X < in_rot.m_X;
    00795     if ( m_rotRep != in_rot.m_rotRep ) return m_rotRep < in_rot.m_rotRep;
    00796
    00797     return false;
    00798 }
    00799
    00800 SICPPSDK_INLINE void CRotationf::QuatToMat( const CQuaternionf& in_quat, CMatrix3f& out_mat )
    00801 {
    00802     float d2X, d2Y, d2Z, d2X2, d2Y2, d2Z2, d2WX, d2WY, d2WZ, d2XY, d2XZ, d2YZ;
    00803
    00804     d2X = (float)2.0 * in_quat.GetX();
    00805     d2Y = (float)2.0 * in_quat.GetY();
    00806     d2Z = (float)2.0 * in_quat.GetZ();
    00807
    00808     d2X2 = d2X * in_quat.GetX();
    00809     d2Y2 = d2Y * in_quat.GetY();
    00810     d2Z2 = d2Z * in_quat.GetZ();
    00811
    00812     d2WX = d2X * in_quat.GetW();
    00813     d2WY = d2Y * in_quat.GetW();
    00814     d2WZ = d2Z * in_quat.GetW();
    00815     d2XY = d2X * in_quat.GetY();
    00816     d2YZ = d2Y * in_quat.GetZ();
    00817     d2XZ = d2Z * in_quat.GetX();
    00818
    00819     out_mat.SetValue( 0, 0, (float)1.0 - d2Y2 - d2Z2 );
    00820     out_mat.SetValue( 1, 1, (float)1.0 - d2X2 - d2Z2 );
    00821     out_mat.SetValue( 2, 2, (float)1.0 - d2X2 - d2Y2 );
    00822     out_mat.SetValue( 0, 1, d2XY + d2WZ );
    00823     out_mat.SetValue( 1, 0, d2XY - d2WZ );
    00824     out_mat.SetValue( 0, 2, d2XZ - d2WY );
    00825     out_mat.SetValue( 2, 0, d2XZ + d2WY );
    00826     out_mat.SetValue( 1, 2, d2YZ + d2WX );
    00827     out_mat.SetValue( 2, 1, d2YZ - d2WX );
    00828 }
    00829
    00830 SICPPSDK_INLINE void CRotationf::QuatToEuler( const CQuaternionf& in_quat, CVector3f& out_vct, const RotationOrder in_eOrder )
    00831 {
    00832     CMatrix3f mat;
    00833     QuatToMat( in_quat, mat );
    00834     XSI::MATH::MatToEuler( mat, out_vct, in_eOrder);
    00835 }
    00836
    00837 SICPPSDK_INLINE void CRotationf::AxisAngleToQuat( const CVector3f& in_vctAxis, const float in_fAngle, CQuaternionf& out_quat)
    00838 {
    00839     float fCos, fSin;
    00840     CVector3f vct(in_vctAxis);
    00841
    00842     fCos = cos( in_fAngle * 0.5f );
    00843     fSin = sin( in_fAngle * 0.5f );
    00844
    00845     vct.SetLength( fSin );
    00846     out_quat.Set( fCos, vct.GetX(), vct.GetY(), vct.GetZ() );
    00847 }
    00848
    00849 SICPPSDK_INLINE void CRotationf::QuatToAxisAngle(const CQuaternionf& in_quat, CVector3f& out_vctAxis, float& out_fAngle )
    00850 {
    00851     out_fAngle = (float)(2.0 * ::acos( in_quat.GetW() ));
    00852
    00853     //from C3DRotation::GetAxisAngle();
    00854     float fSin = (float)sin( 0.5f * out_fAngle );
    00855
    00856     if( ::fabs( fSin ) < MicroEPS )
    00857     {
    00858         // If angle is almost zero, then axis of rotation is undefined.
    00859         out_vctAxis.SetNull();
    00860     }
    00861     else
    00862     {
    00863         //Retrieve the axis of rotation
    00864         fSin = 1.0f / fSin;
    00865         out_vctAxis.PutX( in_quat.GetX() * fSin );
    00866         out_vctAxis.PutY( in_quat.GetY() * fSin );
    00867         out_vctAxis.PutZ( in_quat.GetZ() * fSin );
    00868     }
    00869 }
    00870
    00871 };
    00872 };
    00873
    00874 #endif // __XSIROTATIONF_H__