xsi_rotationf.h Source
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__