BALL
1.4.1
|
00001 // -*- Mode: C++; tab-width: 2; -*- 00002 // vi: set ts=2: 00003 // 00004 // $Id: matrix44.h,v 1.55.14.1 2007/03/25 21:23:45 oliver Exp $ 00005 // 00006 00007 #ifndef BALL_MATHS_MATRIX44_H 00008 #define BALL_MATHS_MATRIX44_H 00009 00010 #ifndef BALL_COMMON_EXCEPTION_H 00011 # include <BALL/COMMON/exception.h> 00012 #endif 00013 00014 #include <math.h> 00015 #include <iostream> 00016 #include <stdlib.h> 00017 00018 #ifndef BALL_MATHS_ANGLE_H 00019 # include <BALL/MATHS/angle.h> 00020 #endif 00021 00022 #ifndef BALL_MATHS_VECTOR3_H 00023 # include <BALL/MATHS/vector3.h> 00024 #endif 00025 00026 #ifndef BALL_MATHS_VECTOR4_H 00027 # include <BALL/MATHS/vector4.h> 00028 #endif 00029 00030 namespace BALL 00031 { 00038 00039 template <typename T> 00040 class TMatrix4x4; 00041 00050 template <typename T> 00051 std::istream& operator >> (std::istream& s, TMatrix4x4<T>& m) 00052 ; 00053 00059 template <typename T> 00060 std::ostream& operator << (std::ostream& s, const TMatrix4x4<T>& m) 00061 ; 00063 00066 template <typename T> 00067 class TMatrix4x4 00068 { 00069 public: 00070 00071 BALL_CREATE(TMatrix4x4) 00072 00073 00076 00081 TMatrix4x4() 00082 ; 00083 00090 TMatrix4x4(const T* ptr); 00091 00098 TMatrix4x4(const T ptr[4][4]); 00099 00105 TMatrix4x4(const TMatrix4x4& m) 00106 ; 00107 00116 TMatrix4x4 00117 (const TVector4<T>& col1, const TVector4<T>& col2, 00118 const TVector4<T>& col3, const TVector4<T>& col4) 00119 ; 00120 00125 TMatrix4x4 00126 (const T& m11, const T& m12, const T& m13, const T& m14, 00127 const T& m21, const T& m22, const T& m23, const T& m24, 00128 const T& m31, const T& m32, const T& m33, const T& m34, 00129 const T& m41, const T& m42, const T& m43, const T& m44) 00130 ; 00131 00136 virtual ~TMatrix4x4() 00137 00138 { 00139 } 00140 00144 virtual void clear() 00145 ; 00146 00148 00151 00157 void set(const T* ptr); 00158 00164 void set(const T ptr[4][4]); 00165 00169 void set(const TMatrix4x4& m); 00170 00177 void set 00178 (const TVector4<T>& col1, const TVector4<T>& col2, 00179 const TVector4<T>& col3, const TVector4<T>& col4); 00180 00184 void set 00185 (const T& m11, const T& m12, const T& m13, const T& m14, 00186 const T& m21, const T& m22, const T& m23, const T& m24, 00187 const T& m31, const T& m32, const T& m33, const T& m34, 00188 const T& m41, const T& m42, const T& m43, const T& m44); 00189 00195 TMatrix4x4& operator = (const T* ptr); 00196 00202 TMatrix4x4& operator = (const T ptr[4][4]); 00203 00208 TMatrix4x4& operator = (const TMatrix4x4& m); 00209 00215 void get(T* ptr) const; 00216 00222 void get(T ptr[4][4]) const; 00223 00228 void get(TMatrix4x4& m) const; 00229 00236 void get 00237 (TVector4<T>& col1, TVector4<T>& col2, 00238 TVector4<T>& col3, TVector4<T>& col4) const; 00239 00243 void get 00244 (T& m11, T& m12, T& m13, T& m14, 00245 T& m21, T& m22, T& m23, T& m24, 00246 T& m31, T& m32, T& m33, T& m34, 00247 T& m41, T& m42, T& m43, T& m44) const; 00248 00252 void swap(TMatrix4x4& m); 00253 00255 00258 00263 T getTrace() const; 00264 00268 static const TMatrix4x4& getZero(); 00269 00274 static const TMatrix4x4& getIdentity(); 00275 00280 void setIdentity(); 00281 00286 void set(const T& t = (T)1); 00287 00292 void transpose(); 00293 00299 TVector4<T> getRow(Position row) const; 00300 00306 TVector4<T> getColumn(Position col) const; 00307 00313 void setRow(Position row, const TVector4<T>& row_value); 00314 00320 void setColumn(Position col, const TVector4<T>& col_value); 00321 00329 bool isEqual(const TMatrix4x4& m) const; 00330 00334 TVector4<T> getDiagonal() const; 00335 00342 T& operator () (Position row, Position col); 00343 00350 const T& operator () (Position row, Position col) const; 00351 00358 const T& operator [] (Position position) const; 00359 00364 T& operator [] (Position position); 00365 00368 TMatrix4x4 operator + () const; 00369 00372 TMatrix4x4 operator - () const; 00373 00379 TMatrix4x4 operator + (const TMatrix4x4& m) const; 00380 00386 TMatrix4x4& operator += (const TMatrix4x4& m); 00387 00393 TMatrix4x4 operator - (const TMatrix4x4& m) const; 00394 00400 TMatrix4x4& operator -= (const TMatrix4x4& m); 00401 00406 TMatrix4x4 operator * (const T& scalar) const; 00407 00412 TMatrix4x4& operator *= (const T& scalar); 00413 00419 TMatrix4x4 operator / (const T& scalar) const; 00420 00426 TMatrix4x4& operator /= (const T& scalar); 00427 00431 TMatrix4x4 operator * (const TMatrix4x4& m) const; 00432 00436 TMatrix4x4& operator *= (const TMatrix4x4& m); 00437 00441 TVector4<T> operator * (const TVector4<T>& vector) const; 00442 00449 bool invert(TMatrix4x4& inverse) const; 00450 00456 bool invert(); 00457 00461 T getDeterminant() const; 00462 00468 void translate(const T &x, const T &y, const T &z); 00469 00473 void translate(const TVector3<T>& v); 00474 00480 void setTranslation(const T& x, const T& y, const T& z); 00481 00485 void setTranslation(const TVector3<T>& v); 00486 00492 void scale(const T& x_scale, const T& y_scale, const T& z_scale); 00493 00497 void scale(const T& scale); 00498 00502 void scale(const TVector3<T>& v); 00503 00509 void setScale(const T& x_scale, const T& y_scale, const T& z_scale); 00510 00514 void setScale(const T& scale); 00515 00519 void setScale(const TVector3<T>& v); 00520 00524 void rotateX(const TAngle<T>& phi); 00525 00529 void setRotationX(const TAngle<T>& phi); 00530 00534 void rotateY(const TAngle<T>& phi); 00535 00539 void setRotationY(const TAngle<T>& phi); 00540 00544 void rotateZ(const TAngle<T>& phi); 00545 00549 void setRotationZ(const TAngle<T>& phi); 00550 00557 void rotate(const TAngle<T>& phi, const T& axis_x, const T& axis_y, const T& axis_z); 00558 00563 void rotate(const TAngle<T>& phi, const TVector3<T>& axis); 00564 00569 void rotate(const TAngle<T>& phi, const TVector4<T>& axis); 00570 00577 void setRotation(const TAngle<T>& phi, const T& axis_x, const T& axis_y, const T& axis_z); 00578 00583 void setRotation(const TAngle<T>& phi, const TVector3<T>& axis); 00584 00589 void setRotation(const TAngle<T>& phi, const TVector4<T>& axis); 00591 00595 00601 bool operator == (const TMatrix4x4& m) const; 00602 00608 bool operator != (const TMatrix4x4& m) const; 00609 00614 bool isIdentity() const; 00615 00619 bool isRegular() const; 00620 00624 bool isSingular() const; 00625 00630 bool isSymmetric() const; 00631 00635 bool isLowerTriangular() const; 00636 00640 bool isUpperTriangular() const; 00641 00645 bool isDiagonal() const; 00647 00651 00656 bool isValid() const; 00657 00664 void dump(std::ostream& s = std::cout, Size depth = 0) const; 00666 00670 00672 T m11; 00673 00675 T m12; 00676 00678 T m13; 00679 00681 T m14; 00682 00684 T m21; 00685 00687 T m22; 00688 00690 T m23; 00691 00693 T m24; 00694 00696 T m31; 00697 00699 T m32; 00700 00702 T m33; 00703 00705 T m34; 00706 00708 T m41; 00709 00711 T m42; 00712 00714 T m43; 00715 00717 T m44; 00719 00720 private: 00721 00722 void initializeComponentPointers_() 00723 { 00724 T **ptr = (T **)comp_ptr_; 00725 00726 *ptr++ = &m11; *ptr++ = &m12; *ptr++ = &m13; *ptr++ = &m14; 00727 *ptr++ = &m21; *ptr++ = &m22; *ptr++ = &m23; *ptr++ = &m24; 00728 *ptr++ = &m31; *ptr++ = &m32; *ptr++ = &m33; *ptr++ = &m34; 00729 *ptr++ = &m41; *ptr++ = &m42; *ptr++ = &m43; *ptr = &m44; 00730 } 00731 00732 // pointers to the components of the matrix 00733 T* comp_ptr_[16]; 00734 }; 00736 00737 template <typename T> 00738 TMatrix4x4<T>::TMatrix4x4() 00739 : m11(0), m12(0), m13(0), m14(0), 00740 m21(0), m22(0), m23(0), m24(0), 00741 m31(0), m32(0), m33(0), m34(0), 00742 m41(0), m42(0), m43(0), m44(0) 00743 { 00744 initializeComponentPointers_(); 00745 } 00746 00747 template <typename T> 00748 TMatrix4x4<T>::TMatrix4x4( const T* ptr) 00749 { 00750 if (ptr == 0) 00751 { 00752 throw Exception::NullPointer(__FILE__, __LINE__); 00753 } 00754 00755 m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++; 00756 m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++; 00757 m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++; 00758 m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr; 00759 00760 initializeComponentPointers_(); 00761 } 00762 00763 template <typename T> 00764 TMatrix4x4<T>::TMatrix4x4(const T array_ptr[4][4]) 00765 { 00766 if (array_ptr == 0) 00767 { 00768 throw Exception::NullPointer(__FILE__, __LINE__); 00769 } 00770 00771 const T *ptr = *array_ptr; 00772 00773 m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++; 00774 m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++; 00775 m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++; 00776 m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr; 00777 00778 initializeComponentPointers_(); 00779 } 00780 00781 template <typename T> 00782 TMatrix4x4<T>::TMatrix4x4(const TMatrix4x4<T>& m) 00783 : m11(m.m11), m12(m.m12), m13(m.m13), m14(m.m14), 00784 m21(m.m21), m22(m.m22), m23(m.m23), m24(m.m24), 00785 m31(m.m31), m32(m.m32), m33(m.m33), m34(m.m34), 00786 m41(m.m41), m42(m.m42), m43(m.m43), m44(m.m44) 00787 { 00788 initializeComponentPointers_(); 00789 } 00790 00791 00792 template <typename T> 00793 TMatrix4x4<T>::TMatrix4x4 00794 (const TVector4<T>& col1, const TVector4<T>& col2, 00795 const TVector4<T>& col3,const TVector4<T>& col4) 00796 : m11(col1.x), m12(col1.y), m13(col1.z), m14(col1.h), 00797 m21(col2.x), m22(col2.y), m23(col2.z), m24(col2.h), 00798 m31(col3.x), m32(col3.y), m33(col3.z), m34(col3.h), 00799 m41(col4.x), m42(col4.y), m43(col4.z), m44(col4.h) 00800 { 00801 initializeComponentPointers_(); 00802 } 00803 00804 template <typename T> 00805 TMatrix4x4<T>::TMatrix4x4 00806 (const T& m11, const T& m12, const T& m13, const T& m14, 00807 const T& m21, const T& m22, const T& m23, const T& m24, 00808 const T& m31, const T& m32, const T& m33, const T& m34, 00809 const T& m41, const T& m42, const T& m43, const T& m44) 00810 : m11(m11), m12(m12), m13(m13), m14(m14), 00811 m21(m21), m22(m22), m23(m23), m24(m24), 00812 m31(m31), m32(m32), m33(m33), m34(m34), 00813 m41(m41), m42(m42), m43(m43), m44(m44) 00814 { 00815 initializeComponentPointers_(); 00816 } 00817 00818 template <typename T> 00819 void TMatrix4x4<T>::clear() 00820 { 00821 set((T)0); 00822 } 00823 00824 template <typename T> 00825 void TMatrix4x4<T>::set(const T* ptr) 00826 { 00827 if (ptr == 0) 00828 { 00829 throw Exception::NullPointer(__FILE__, __LINE__); 00830 } 00831 00832 m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++; 00833 m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++; 00834 m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++; 00835 m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr; 00836 } 00837 00838 template <typename T> 00839 void TMatrix4x4<T>::set(const T array_ptr[4][4]) 00840 { 00841 if (array_ptr == 0) 00842 { 00843 throw Exception::NullPointer(__FILE__, __LINE__); 00844 } 00845 00846 const T *ptr = *array_ptr; 00847 00848 m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++; 00849 m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++; 00850 m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++; 00851 m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr; 00852 } 00853 00854 template <typename T> 00855 void TMatrix4x4<T>::set(const TMatrix4x4<T>& m) 00856 { 00857 m11 = m.m11; m12 = m.m12; m13 = m.m13; m14 = m.m14; 00858 m21 = m.m21; m22 = m.m22; m23 = m.m23; m24 = m.m24; 00859 m31 = m.m31; m32 = m.m32; m33 = m.m33; m34 = m.m34; 00860 m41 = m.m41; m42 = m.m42; m43 = m.m43; m44 = m.m44; 00861 } 00862 00863 template <typename T> 00864 void TMatrix4x4<T>::set 00865 (const TVector4<T>& col1, const TVector4<T>& col2, 00866 const TVector4<T>& col3, const TVector4<T>& col4) 00867 { 00868 m11 = col1.x; m12 = col1.y; m13 = col1.z; m14 = col1.h; 00869 m21 = col2.x; m22 = col2.y; m23 = col2.z; m24 = col2.h; 00870 m31 = col3.x; m32 = col3.y; m33 = col3.z; m34 = col3.h; 00871 m41 = col4.x; m42 = col4.y; m43 = col4.z; m44 = col4.h; 00872 } 00873 00874 template <typename T> 00875 void TMatrix4x4<T>::set 00876 (const T& c11, const T& c12, const T& c13, const T& c14, 00877 const T& c21, const T& c22, const T& c23, const T& c24, 00878 const T& c31, const T& c32, const T& c33, const T& c34, 00879 const T& c41, const T& c42, const T& c43, const T& c44) 00880 { 00881 m11 = c11; m12 = c12; m13 = c13; m14 = c14; 00882 m21 = c21; m22 = c22; m23 = c23; m24 = c24; 00883 m31 = c31; m32 = c32; m33 = c33; m34 = c34; 00884 m41 = c41; m42 = c42; m43 = c43; m44 = c44; 00885 } 00886 00887 template <typename T> 00888 BALL_INLINE 00889 TMatrix4x4<T>& TMatrix4x4<T>::operator = (const T* ptr) 00890 { 00891 set(ptr); 00892 return *this; 00893 } 00894 00895 template <typename T> 00896 BALL_INLINE 00897 TMatrix4x4<T>& TMatrix4x4<T>::operator = (const T array_ptr[4][4]) 00898 { 00899 set(array_ptr); 00900 return *this; 00901 } 00902 00903 template <typename T> 00904 BALL_INLINE 00905 TMatrix4x4<T>& TMatrix4x4<T>::operator = (const TMatrix4x4<T>& m) 00906 { 00907 set(m); 00908 return *this; 00909 } 00910 00911 template <typename T> 00912 void TMatrix4x4<T>::get(T* ptr) const 00913 { 00914 if (ptr == 0) 00915 { 00916 throw Exception::NullPointer(__FILE__, __LINE__); 00917 } 00918 00919 *ptr++ = m11; *ptr++ = m12; *ptr++ = m13; *ptr++ = m14; 00920 *ptr++ = m21; *ptr++ = m22; *ptr++ = m23; *ptr++ = m24; 00921 *ptr++ = m31; *ptr++ = m32; *ptr++ = m33; *ptr++ = m34; 00922 *ptr++ = m41; *ptr++ = m42; *ptr++ = m43; *ptr = m44; 00923 } 00924 00925 template <typename T> 00926 void TMatrix4x4<T>::get(T array_ptr[4][4]) const 00927 { 00928 if (array_ptr == 0) 00929 { 00930 throw Exception::NullPointer(__FILE__, __LINE__); 00931 } 00932 00933 T *ptr = *array_ptr; 00934 00935 *ptr++ = m11; *ptr++ = m12; *ptr++ = m13; *ptr++ = m14; 00936 *ptr++ = m21; *ptr++ = m22; *ptr++ = m23; *ptr++ = m24; 00937 *ptr++ = m31; *ptr++ = m32; *ptr++ = m33; *ptr++ = m34; 00938 *ptr++ = m41; *ptr++ = m42; *ptr++ = m43; *ptr = m44; 00939 } 00940 00941 template <typename T> 00942 void TMatrix4x4<T>::get(TMatrix4x4<T>& m) const 00943 { 00944 m.set(*this); 00945 } 00946 00947 template <typename T> 00948 void TMatrix4x4<T>::get 00949 (TVector4<T>& col1, TVector4<T>& col2, 00950 TVector4<T>& col3, TVector4<T>& col4) const 00951 { 00952 col1.x = m11; col1.y = m12; col1.z = m13; col1.h = m14; 00953 col2.x = m21; col2.y = m22; col2.z = m23; col2.h = m24; 00954 col3.x = m31; col3.y = m32; col3.z = m33; col3.h = m34; 00955 col4.x = m41; col4.y = m42; col4.z = m43; col4.h = m44; 00956 } 00957 00958 template <typename T> 00959 void TMatrix4x4<T>::get 00960 (T& c11, T& c12, T& c13, T& c14, 00961 T& c21, T& c22, T& c23, T& c24, 00962 T& c31, T& c32, T& c33, T& c34, 00963 T& c41, T& c42, T& c43, T& c44) const 00964 { 00965 c11 = m11; c12 = m12; c13 = m13; c14 = m14; 00966 c21 = m21; c22 = m22; c23 = m23; c24 = m24; 00967 c31 = m31; c32 = m32; c33 = m33; c34 = m34; 00968 c41 = m41; c42 = m42; c43 = m43; c44 = m44; 00969 } 00970 00971 template <typename T> 00972 BALL_INLINE 00973 T TMatrix4x4<T>::getTrace() const 00974 { 00975 return (m11 + m22 + m33 + m44); 00976 } 00977 00978 template <typename T> 00979 BALL_INLINE 00980 const TMatrix4x4<T>& TMatrix4x4<T>::getZero() 00981 { 00982 static TMatrix4x4<T> null_matrix 00983 (0, 0, 0, 0, 00984 0, 0, 0, 0, 00985 0, 0, 0, 0, 00986 0, 0, 0, 0); 00987 00988 return null_matrix; 00989 } 00990 00991 00992 template <typename T> 00993 BALL_INLINE 00994 void TMatrix4x4<T>::setIdentity() 00995 { 00996 m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0; 00997 m11 = m22 = m33 = m44 = (T)1; 00998 } 00999 template <typename T> 01000 BALL_INLINE 01001 const TMatrix4x4<T>& TMatrix4x4<T>::getIdentity() 01002 { 01003 static TMatrix4x4<T> identity 01004 (1, 0, 0, 0, 01005 0, 1, 0, 0, 01006 0, 0, 1, 0, 01007 0, 0, 0, 1); 01008 01009 return identity; 01010 } 01011 01012 template <typename T> 01013 void TMatrix4x4<T>::set(const T& t) 01014 { 01015 m11 = m12 = m13 = m14 01016 = m21 = m22 = m23 = m24 01017 = m31 = m32 = m33 = m34 01018 = m41 = m42 = m43 = m44 01019 = t; 01020 } 01021 01022 template <typename T> 01023 void TMatrix4x4<T>::swap(TMatrix4x4<T>& m) 01024 { 01025 T tmp = m11; m11 = m.m11; m.m11 = tmp; 01026 tmp = m12; m12 = m.m12; m.m12 = tmp; 01027 tmp = m13; m13 = m.m13; m.m13 = tmp; 01028 tmp = m14; m14 = m.m14; m.m14 = tmp; 01029 tmp = m21; m21 = m.m21; m.m21 = tmp; 01030 tmp = m22; m22 = m.m22; m.m22 = tmp; 01031 tmp = m23; m23 = m.m23; m.m23 = tmp; 01032 tmp = m24; m24 = m.m24; m.m24 = tmp; 01033 tmp = m31; m31 = m.m31; m.m31 = tmp; 01034 tmp = m32; m32 = m.m32; m.m32 = tmp; 01035 tmp = m33; m33 = m.m33; m.m33 = tmp; 01036 tmp = m34; m34 = m.m34; m.m34 = tmp; 01037 tmp = m41; m41 = m.m41; m.m41 = tmp; 01038 tmp = m42; m42 = m.m42; m.m42 = tmp; 01039 tmp = m43; m43 = m.m43; m.m43 = tmp; 01040 tmp = m44; m44 = m.m44; m.m44 = tmp; 01041 } 01042 01043 template <typename T> 01044 void TMatrix4x4<T>::transpose() 01045 { 01046 T tmp = m12; 01047 m12 = m21; 01048 m21 = tmp; 01049 01050 tmp = m13; 01051 m13 = m31; 01052 m31 = tmp; 01053 01054 tmp = m14; 01055 m14 = m41; 01056 m41 = tmp; 01057 01058 tmp = m23; 01059 m23 = m32; 01060 m32 = tmp; 01061 01062 tmp = m24; 01063 m24 = m42; 01064 m42 = tmp; 01065 01066 tmp = m34; 01067 m34 = m43; 01068 m43 = tmp; 01069 } 01070 01071 template <typename T> 01072 TVector4<T> TMatrix4x4<T>::getRow(Position row) const 01073 { 01074 if (row > 3) 01075 { 01076 throw Exception::IndexOverflow(__FILE__, __LINE__, row, 3); 01077 } 01078 01079 // calculate the start of the row in the array 01080 const T* ptr = comp_ptr_[4 * row]; 01081 return TVector4<T> (ptr[0], ptr[1], ptr[2], ptr[3]); 01082 } 01083 01084 template <typename T> 01085 TVector4<T> TMatrix4x4<T>::getColumn(Position col) const 01086 { 01087 if (col > 3) 01088 { 01089 throw Exception::IndexOverflow(__FILE__, __LINE__, col, 3); 01090 } 01091 01092 const T* ptr = comp_ptr_[col]; 01093 01094 return TVector4<T> (ptr[0], ptr[4], ptr[8], ptr[12]); 01095 } 01096 01097 01098 template <typename T> 01099 void TMatrix4x4<T>::setRow(Position row, const TVector4<T>& row_value) 01100 { 01101 if (row > 3) 01102 { 01103 throw Exception::IndexOverflow(__FILE__, __LINE__, row, 3); 01104 } 01105 01106 // calculate a pointer to the start of the row 01107 T* ptr = comp_ptr_[4 * row]; 01108 01109 ptr[0] = row_value.x; 01110 ptr[1] = row_value.y; 01111 ptr[2] = row_value.z; 01112 ptr[3] = row_value.h; 01113 } 01114 01115 template <typename T> 01116 void TMatrix4x4<T>::setColumn(Position col, const TVector4<T>& col_value) 01117 { 01118 if (col > 3) 01119 { 01120 throw Exception::IndexOverflow(__FILE__, __LINE__, col, 3); 01121 } 01122 01123 // calculate a pointer to the start of the column 01124 T* ptr = comp_ptr_[col]; 01125 01126 ptr[0] = col_value.x; 01127 ptr[4] = col_value.y; 01128 ptr[8] = col_value.z; 01129 ptr[12] = col_value.h; 01130 } 01131 01132 template <typename T> 01133 bool TMatrix4x4<T>::isEqual(const TMatrix4x4<T>& m) const 01134 { 01135 // iterate over all component pointers 01136 // and compare the elements for approximate equality 01137 for (Position i = 0; i < 16; i++) 01138 { 01139 if (Maths::isEqual(*comp_ptr_[i], *m.comp_ptr_[i]) == false) 01140 { 01141 return false; 01142 } 01143 } 01144 01145 return true; 01146 } 01147 01148 template <typename T> 01149 TVector4<T>TMatrix4x4<T>::getDiagonal() const 01150 { 01151 return TVector4<T>(m11, m22, m33, m44); 01152 } 01153 01154 template <typename T> 01155 BALL_INLINE 01156 T& TMatrix4x4<T>::operator () (Position row, Position col) 01157 { 01158 if ((row > 3) || (col > 3)) 01159 { 01160 throw Exception::IndexOverflow(__FILE__, __LINE__, row + col, 3); 01161 } 01162 01163 return *comp_ptr_[4 * row + col]; 01164 } 01165 01166 template <typename T> 01167 BALL_INLINE 01168 const T& TMatrix4x4<T>::operator () (Position row, Position col) const 01169 { 01170 if ((row > 3) || (col > 3)) 01171 { 01172 throw Exception::IndexOverflow(__FILE__, __LINE__, row + col, 3); 01173 } 01174 01175 return *comp_ptr_[4 * row + col]; 01176 } 01177 01178 template <typename T> 01179 BALL_INLINE 01180 const T& TMatrix4x4<T>::operator [] (Position position) const 01181 { 01182 if (position > 15) 01183 { 01184 throw Exception::IndexOverflow(__FILE__, __LINE__, position, 15); 01185 } 01186 return *comp_ptr_[position]; 01187 } 01188 01189 template <typename T> 01190 BALL_INLINE 01191 T& TMatrix4x4<T>::operator [] (Position position) 01192 { 01193 if (position > 15) 01194 { 01195 throw Exception::IndexOverflow(__FILE__, __LINE__, position, 15); 01196 } 01197 return *comp_ptr_[position]; 01198 } 01199 01200 template <typename T> 01201 BALL_INLINE 01202 TMatrix4x4<T> TMatrix4x4<T>::operator + () const 01203 { 01204 return *this; 01205 } 01206 01207 template <typename T> 01208 BALL_INLINE TMatrix4x4<T> 01209 TMatrix4x4<T>::operator - () const 01210 { 01211 return TMatrix4x4<T> 01212 (-m11, -m12, -m13, -m14, 01213 -m21, -m22, -m23, -m24, 01214 -m31, -m32, -m33, -m34, 01215 -m41, -m42, -m43, -m44); 01216 } 01217 01218 template <typename T> 01219 TMatrix4x4<T> TMatrix4x4<T>::operator + (const TMatrix4x4<T>& m) const 01220 { 01221 return TMatrix4x4<T> 01222 (m11 + m.m11, m12 + m.m12, m13 + m.m13, m14 + m.m14, 01223 m21 + m.m21, m22 + m.m22, m23 + m.m23, m24 + m.m24, 01224 m31 + m.m31, m32 + m.m32, m33 + m.m33, m34 + m.m34, 01225 m41 + m.m41, m42 + m.m42, m43 + m.m43, m44 + m.m44); 01226 } 01227 01228 template <typename T> 01229 TMatrix4x4<T>& TMatrix4x4<T>::operator += (const TMatrix4x4<T>& m) 01230 { 01231 m11 += m.m11; 01232 m12 += m.m12; 01233 m13 += m.m13; 01234 m14 += m.m14; 01235 m21 += m.m21; 01236 m22 += m.m22; 01237 m23 += m.m23; 01238 m24 += m.m24; 01239 m31 += m.m31; 01240 m32 += m.m32; 01241 m33 += m.m33; 01242 m34 += m.m34; 01243 m41 += m.m41; 01244 m42 += m.m42; 01245 m43 += m.m43; 01246 m44 += m.m44; 01247 01248 return *this; 01249 } 01250 01251 template <typename T> 01252 TMatrix4x4<T> TMatrix4x4<T>::operator - (const TMatrix4x4<T>& m) const 01253 { 01254 return TMatrix4x4<T> 01255 (m11 - m.m11, m12 - m.m12, m13 - m.m13, m14 - m.m14, 01256 m21 - m.m21, m22 - m.m22, m23 - m.m23, m24 - m.m24, 01257 m31 - m.m31, m32 - m.m32, m33 - m.m33, m34 - m.m34, 01258 m41 - m.m41, m42 - m.m42, m43 - m.m43, m44 - m.m44); 01259 } 01260 01261 template <typename T> 01262 TMatrix4x4<T>& TMatrix4x4<T>::operator -= (const TMatrix4x4<T>& m) 01263 { 01264 m11 -= m.m11; 01265 m12 -= m.m12; 01266 m13 -= m.m13; 01267 m14 -= m.m14; 01268 m21 -= m.m21; 01269 m22 -= m.m22; 01270 m23 -= m.m23; 01271 m24 -= m.m24; 01272 m31 -= m.m31; 01273 m32 -= m.m32; 01274 m33 -= m.m33; 01275 m34 -= m.m34; 01276 m41 -= m.m41; 01277 m42 -= m.m42; 01278 m43 -= m.m43; 01279 m44 -= m.m44; 01280 01281 return *this; 01282 } 01283 01284 template <typename T> 01285 TMatrix4x4<T> TMatrix4x4<T>::operator * (const T& scalar) const 01286 { 01287 return TMatrix4x4<T> 01288 (m11 * scalar, m12 * scalar, m13 * scalar, m14 * scalar, 01289 m21 * scalar, m22 * scalar, m23 * scalar, m24 * scalar, 01290 m31 * scalar, m32 * scalar, m33 * scalar, m34 * scalar, 01291 m41 * scalar, m42 * scalar, m43 * scalar, m44 * scalar); 01292 } 01293 01294 template <typename T> 01295 TMatrix4x4<T> operator * (const T& scalar, const TMatrix4x4<T>& m) 01296 { 01297 return TMatrix4x4<T> 01298 (scalar * m.m11, scalar * m.m12, scalar * m.m13, scalar * m.m14, 01299 scalar * m.m21, scalar * m.m22, scalar * m.m23, scalar * m.m24, 01300 scalar * m.m31, scalar * m.m32, scalar * m.m33, scalar * m.m34, 01301 scalar * m.m41, scalar * m.m42, scalar * m.m43, scalar * m.m44); 01302 } 01303 01304 template <typename T> 01305 TMatrix4x4<T>& TMatrix4x4<T>::operator *= (const T& scalar) 01306 { 01307 m11 *= scalar; 01308 m12 *= scalar; 01309 m13 *= scalar; 01310 m14 *= scalar; 01311 m21 *= scalar; 01312 m22 *= scalar; 01313 m23 *= scalar; 01314 m24 *= scalar; 01315 m31 *= scalar; 01316 m32 *= scalar; 01317 m33 *= scalar; 01318 m34 *= scalar; 01319 m41 *= scalar; 01320 m42 *= scalar; 01321 m43 *= scalar; 01322 m44 *= scalar; 01323 01324 return *this; 01325 } 01326 01327 template <typename T> 01328 TVector3<T> operator *(const TMatrix4x4<T>& matrix, const TVector3<T>& vector) 01329 { 01330 return TVector3<T> 01331 (matrix.m11 * vector.x + matrix.m12 * vector.y + matrix.m13 * vector.z + matrix.m14, 01332 matrix.m21 * vector.x + matrix.m22 * vector.y + matrix.m23 * vector.z + matrix.m24, 01333 matrix.m31 * vector.x + matrix.m32 * vector.y + matrix.m33 * vector.z + matrix.m34); 01334 } 01335 01336 template <typename T> 01337 BALL_INLINE 01338 TMatrix4x4<T>TMatrix4x4<T>::operator / (const T& scalar) const 01339 { 01340 if (scalar == (T)0) 01341 { 01342 throw Exception::DivisionByZero(__FILE__, __LINE__); 01343 } 01344 01345 return (*this * ((T)1 / scalar)); 01346 } 01347 01348 template <typename T> 01349 BALL_INLINE 01350 TMatrix4x4<T>& TMatrix4x4<T>::operator /= (const T& scalar) 01351 { 01352 if (scalar == (T)0) 01353 { 01354 throw Exception::DivisionByZero(__FILE__, __LINE__); 01355 } 01356 01357 return (*this *= (T)1 / scalar); 01358 } 01359 01360 template <typename T> 01361 TMatrix4x4<T> TMatrix4x4<T>::operator * (const TMatrix4x4<T>& m) const 01362 { 01363 return TMatrix4x4<T> 01364 (m11 * m.m11 + m12 * m.m21 + m13 * m.m31 + m14 * m.m41, 01365 m11 * m.m12 + m12 * m.m22 + m13 * m.m32 + m14 * m.m42, 01366 m11 * m.m13 + m12 * m.m23 + m13 * m.m33 + m14 * m.m43, 01367 m11 * m.m14 + m12 * m.m24 + m13 * m.m34 + m14 * m.m44, 01368 01369 m21 * m.m11 + m22 * m.m21 + m23 * m.m31 + m24 * m.m41, 01370 m21 * m.m12 + m22 * m.m22 + m23 * m.m32 + m24 * m.m42, 01371 m21 * m.m13 + m22 * m.m23 + m23 * m.m33 + m24 * m.m43, 01372 m21 * m.m14 + m22 * m.m24 + m23 * m.m34 + m24 * m.m44, 01373 01374 m31 * m.m11 + m32 * m.m21 + m33 * m.m31 + m34 * m.m41, 01375 m31 * m.m12 + m32 * m.m22 + m33 * m.m32 + m34 * m.m42, 01376 m31 * m.m13 + m32 * m.m23 + m33 * m.m33 + m34 * m.m43, 01377 m31 * m.m14 + m32 * m.m24 + m33 * m.m34 + m34 * m.m44, 01378 01379 m41 * m.m11 + m42 * m.m21 + m43 * m.m31 + m44 * m.m41, 01380 m41 * m.m12 + m42 * m.m22 + m43 * m.m32 + m44 * m.m42, 01381 m41 * m.m13 + m42 * m.m23 + m43 * m.m33 + m44 * m.m43, 01382 m41 * m.m14 + m42 * m.m24 + m43 * m.m34 + m44 * m.m44); 01383 } 01384 01385 template <typename T> 01386 TMatrix4x4<T>& TMatrix4x4<T>::operator *= (const TMatrix4x4<T>& m) 01387 { 01388 set(m11 * m.m11 + m12 * m.m21 + m13 * m.m31 + m14 * m.m41, 01389 m11 * m.m12 + m12 * m.m22 + m13 * m.m32 + m14 * m.m42, 01390 m11 * m.m13 + m12 * m.m23 + m13 * m.m33 + m14 * m.m43, 01391 m11 * m.m14 + m12 * m.m24 + m13 * m.m34 + m14 * m.m44, 01392 01393 m21 * m.m11 + m22 * m.m21 + m23 * m.m31 + m24 * m.m41, 01394 m21 * m.m12 + m22 * m.m22 + m23 * m.m32 + m24 * m.m42, 01395 m21 * m.m13 + m22 * m.m23 + m23 * m.m33 + m24 * m.m43, 01396 m21 * m.m14 + m22 * m.m24 + m23 * m.m34 + m24 * m.m44, 01397 01398 m31 * m.m11 + m32 * m.m21 + m33 * m.m31 + m34 * m.m41, 01399 m31 * m.m12 + m32 * m.m22 + m33 * m.m32 + m34 * m.m42, 01400 m31 * m.m13 + m32 * m.m23 + m33 * m.m33 + m34 * m.m43, 01401 m31 * m.m14 + m32 * m.m24 + m33 * m.m34 + m34 * m.m44, 01402 01403 m41 * m.m11 + m42 * m.m21 + m43 * m.m31 + m44 * m.m41, 01404 m41 * m.m12 + m42 * m.m22 + m43 * m.m32 + m44 * m.m42, 01405 m41 * m.m13 + m42 * m.m23 + m43 * m.m33 + m44 * m.m43, 01406 m41 * m.m14 + m42 * m.m24 + m43 * m.m34 + m44 * m.m44); 01407 01408 return *this; 01409 } 01410 01411 template <typename T> 01412 TVector4<T> TMatrix4x4<T>::operator * (const TVector4<T>& v) const 01413 { 01414 return TVector4<T> 01415 (m11 * v.x + m12 * v.y + m13 * v.z + m14 * v.h, 01416 m21 * v.x + m22 * v.y + m23 * v.z + m24 * v.h, 01417 m31 * v.x + m32 * v.y + m33 * v.z + m34 * v.h, 01418 m41 * v.x + m42 * v.y + m43 * v.z + m44 * v.h); 01419 } 01420 01421 template <typename T> 01422 bool TMatrix4x4<T>::invert(TMatrix4x4<T>& inverse) const 01423 { 01431 Index i, j, k; 01432 01433 T a[4][4] = // holds the matrix we want to invert 01434 { 01435 { m11, m12, m13, m14 }, 01436 { m21, m22, m23, m24 }, 01437 { m31, m32, m33, m34 }, 01438 { m41, m42, m43, m44 } 01439 }; 01440 01441 // holds the maximum in the part of A we still have to work with 01442 T scale, sum_of_squares, sigma, tau; 01443 T c[4], d[4]; 01444 for (k=0; k<3; k++) 01445 { 01446 scale = (T)0; 01447 // find the maximum in a 01448 for (i=k; i<4; i++) 01449 scale = Maths::max((T)fabs(a[i][k]), scale); 01450 01451 // is the matrix singular? 01452 if (scale == (T)0) 01453 return false; 01454 01455 // nope. we can normalize the remaining rows 01456 for (i=k; i<4; i++) 01457 a[i][k] /= scale; 01458 01459 sum_of_squares = (T)0; 01460 for (i=k; i<4; i++) 01461 sum_of_squares += a[i][k]*a[i][k]; 01462 01463 // shift the diagonal element 01464 sigma = (a[k][k] >= 0) ? sqrt(sum_of_squares) : -sqrt(sum_of_squares); 01465 a[k][k] += sigma; 01466 01467 c[k] = sigma*a[k][k]; 01468 d[k] = -scale*sigma; 01469 01470 for (j = k+1; j<4; j++) 01471 { 01472 // store the scalar product of a_[k] and a_[j] 01473 sum_of_squares = (T)0; 01474 for (i = k; i<4; i++) 01475 sum_of_squares += a[i][k] * a[i][j]; 01476 01477 tau = sum_of_squares / c[k]; 01478 01479 // prepare the matrix 01480 for (i=k; i<4; i++) 01481 a[i][j] -= tau*a[i][k]; 01482 } 01483 } 01484 d[3] = a[3][3]; 01485 01486 // is the matrix singular? 01487 if (d[3] == (T)0) 01488 return 1; 01489 01490 // now we have the QR decomposition. The upper triangle of A contains 01491 // R, except for the diagonal elements, which are stored in d. c contains 01492 // the values needed to compute the Householder matrices Q, and the vectors 01493 // u needed for the determination of the Qs are stored in the lower triangle 01494 // of A 01495 // 01496 // now we need to solve four linear systems of equations, one for each column 01497 // of the resulting matrix 01498 T result[4][4]; 01499 result[0][0] = 1; result[0][1] = 0; result[0][2] = 0; result[0][3] = 0; 01500 result[1][0] = 0; result[1][1] = 1; result[1][2] = 0; result[1][3] = 0; 01501 result[2][0] = 0; result[2][1] = 0; result[2][2] = 1; result[2][3] = 0; 01502 result[3][0] = 0; result[3][1] = 0; result[3][2] = 0; result[3][3] = 1; 01503 01504 for (k=0; k<4; k++) // k generates the k-th column of the inverse 01505 { 01506 // form the vector Q^t * b, which is simple, since b = e_k 01507 for (j=0; j<3; j++) 01508 { 01509 sum_of_squares = (T)0; 01510 for (i=j; i<4; i++) 01511 sum_of_squares += a[i][j]*result[i][k]; 01512 01513 tau = sum_of_squares / c[j]; 01514 01515 for (i=j; i<4; i++) 01516 result[i][k] -= tau*a[i][j]; 01517 } 01518 01519 // and solve the resulting system 01520 result[3][k] /= d[3]; 01521 for (i=2; i>=0; i--) 01522 { 01523 sum_of_squares = (T)0; 01524 for (j=i+1; j<4; j++) 01525 sum_of_squares += a[i][j] * result[j][k]; 01526 01527 result[i][k] = (result[i][k] - sum_of_squares) / d[i]; 01528 } 01529 } 01530 01531 T* k_ptr = *result; 01532 inverse.m11 = *k_ptr++; 01533 inverse.m12 = *k_ptr++; 01534 inverse.m13 = *k_ptr++; 01535 inverse.m14 = *k_ptr++; 01536 inverse.m21 = *k_ptr++; 01537 inverse.m22 = *k_ptr++; 01538 inverse.m23 = *k_ptr++; 01539 inverse.m24 = *k_ptr++; 01540 inverse.m31 = *k_ptr++; 01541 inverse.m32 = *k_ptr++; 01542 inverse.m33 = *k_ptr++; 01543 inverse.m34 = *k_ptr++; 01544 inverse.m41 = *k_ptr++; 01545 inverse.m42 = *k_ptr++; 01546 inverse.m43 = *k_ptr++; 01547 inverse.m44 = *k_ptr; 01548 01549 return true; 01550 } 01551 01552 template <typename T> 01553 BALL_INLINE bool TMatrix4x4<T>::invert() 01554 { 01555 return invert(*this); 01556 } 01557 01558 template <typename T> 01559 T TMatrix4x4<T>::getDeterminant() const 01560 { 01561 Position i; 01562 Position j; 01563 Position k; 01564 T submatrix[3][3]; 01565 T matrix[4][4] = 01566 { 01567 { m11, m12, m13, m14 }, 01568 { m21, m22, m23, m24 }, 01569 { m31, m32, m33, m34 }, 01570 { m41, m42, m43, m44 } 01571 }; 01572 T determinant = 0; 01573 01574 for (i = 0; i < 4; i++) 01575 { 01576 for (j = 0; j < 3; j++) 01577 { 01578 for (k = 0; k < 3; k++) 01579 { 01580 submatrix[j][k] = 01581 matrix[j + 1][(k < i) ? k : k + 1]; 01582 } 01583 } 01584 01585 determinant += matrix[0][i] * (T)(i / 2.0 == (i >> 1) ? 1 : -1) 01586 * (submatrix[0][0] * submatrix[1][1] * submatrix[2][2] 01587 + submatrix[0][1] * submatrix[1][2] * submatrix[2][0] 01588 + submatrix[0][2] * submatrix[1][0] * submatrix[2][1] 01589 - submatrix[0][2] * submatrix[1][1] * submatrix[2][0] 01590 - submatrix[0][0] * submatrix[1][2] * submatrix[2][1] 01591 - submatrix[0][1] * submatrix[1][0] * submatrix[2][2]); 01592 } 01593 01594 return determinant; 01595 } 01596 01597 template <typename T> 01598 void TMatrix4x4<T>::translate(const T& x, const T& y, const T& z) 01599 { 01600 m14 += m11 * x + m12 * y + m13 * z; 01601 m24 += m21 * x + m22 * y + m23 * z; 01602 m34 += m31 * x + m32 * y + m33 * z; 01603 m44 += m41 * x + m42 * y + m43 * z; 01604 } 01605 01606 template <typename T> 01607 void TMatrix4x4<T>::translate(const TVector3<T>& v) 01608 { 01609 m14 += m11 * v.x + m12 * v.y + m13 * v.z; 01610 m24 += m21 * v.x + m22 * v.y + m23 * v.z; 01611 m34 += m31 * v.x + m32 * v.y + m33 * v.z; 01612 m44 += m41 * v.x + m42 * v.y + m43 * v.z; 01613 } 01614 01615 template <typename T> 01616 void TMatrix4x4<T>::setTranslation(const T& x, const T& y, const T& z) 01617 { 01618 m11 = m22 = m33 = m44 = 1; 01619 01620 m12 = m13 = 01621 m21 = m23 = 01622 m31 = m32 = 01623 m41 = m42 = m43 = 0; 01624 01625 m14 = x; 01626 m24 = y; 01627 m34 = z; 01628 } 01629 01630 template <typename T> 01631 void TMatrix4x4<T>::setTranslation(const TVector3<T>& v) 01632 { 01633 m11 = m22 = m33 = m44 = 1; 01634 01635 m12 = m13 = 01636 m21 = m23 = 01637 m31 = m32 = 01638 m41 = m42 = m43 = 0; 01639 01640 m14 = v.x; 01641 m24 = v.y; 01642 m34 = v.z; 01643 } 01644 01645 template <typename T> 01646 void TMatrix4x4<T>::scale(const T& x_scale, const T& y_scale, const T& z_scale) 01647 { 01648 m11 *= x_scale; 01649 m21 *= x_scale; 01650 m31 *= x_scale; 01651 m41 *= x_scale; 01652 01653 m12 *= y_scale; 01654 m22 *= y_scale; 01655 m32 *= y_scale; 01656 m42 *= y_scale; 01657 01658 m13 *= z_scale; 01659 m23 *= z_scale; 01660 m33 *= z_scale; 01661 m43 *= z_scale; 01662 } 01663 01664 template <typename T> 01665 void TMatrix4x4<T>::scale(const T& scale) 01666 { 01667 m11 *= scale; 01668 m21 *= scale; 01669 m31 *= scale; 01670 m41 *= scale; 01671 01672 m12 *= scale; 01673 m22 *= scale; 01674 m32 *= scale; 01675 m42 *= scale; 01676 01677 m13 *= scale; 01678 m23 *= scale; 01679 m33 *= scale; 01680 m43 *= scale; 01681 } 01682 01683 template <typename T> 01684 void TMatrix4x4<T>::scale(const TVector3<T>& v) 01685 { 01686 m11 *= v.x; 01687 m21 *= v.x; 01688 m31 *= v.x; 01689 m41 *= v.x; 01690 01691 m12 *= v.y; 01692 m22 *= v.y; 01693 m32 *= v.y; 01694 m42 *= v.y; 01695 01696 m13 *= v.z; 01697 m23 *= v.z; 01698 m33 *= v.z; 01699 m43 *= v.z; 01700 } 01701 01702 template <typename T> 01703 void TMatrix4x4<T>::setScale(const T& x_scale, const T& y_scale, const T& z_scale) 01704 { 01705 m11 = x_scale; 01706 m22 = y_scale; 01707 m33 = z_scale; 01708 m44 = 1; 01709 01710 m12 = m13 = m14 = 01711 m21 = m23 = m24 = 01712 m31 = m32 = m34 = 01713 m41 = m42 = m43 = 0; 01714 } 01715 01716 template <typename T> 01717 void TMatrix4x4<T>::setScale(const T& scale) 01718 { 01719 m11 = scale; 01720 m22 = scale; 01721 m33 = scale; 01722 m44 = 1; 01723 01724 m12 = m13 = m14 = 01725 m21 = m23 = m24 = 01726 m31 = m32 = m34 = 01727 m41 = m42 = m43 = 0; 01728 } 01729 01730 template <typename T> 01731 void TMatrix4x4<T>::setScale(const TVector3<T>& v) 01732 { 01733 m11 = v.x; 01734 m22 = v.y; 01735 m33 = v.z; 01736 m44 = 1; 01737 01738 m12 = m13 = m14 = 01739 m21 = m23 = m24 = 01740 m31 = m32 = m34 = 01741 m41 = m42 = m43 = 0; 01742 } 01743 01744 template <typename T> 01745 BALL_INLINE 01746 void TMatrix4x4<T>::rotateX(const TAngle<T>& phi) 01747 { 01748 TMatrix4x4<T> rotation; 01749 01750 rotation.setRotationX(phi); 01751 *this *= rotation; 01752 } 01753 01754 template <typename T> 01755 void TMatrix4x4<T>::setRotationX(const TAngle<T>& phi) 01756 { 01757 m11 = m44 = 1; 01758 01759 m12 = m13 = m14 01760 = m21 = m24 01761 = m31 = m34 01762 = m41 = m42 = m43 01763 = 0; 01764 01765 m22 = m33 = cos(phi); 01766 m23 = -(m32 = sin(phi)); 01767 } 01768 01769 template <typename T> 01770 BALL_INLINE 01771 void TMatrix4x4<T>::rotateY(const TAngle<T>& phi) 01772 { 01773 TMatrix4x4<T> rotation; 01774 01775 rotation.setRotationY(phi); 01776 *this *= rotation; 01777 } 01778 01779 template <typename T> 01780 void TMatrix4x4<T>::setRotationY(const TAngle<T>& phi) 01781 { 01782 m22 = m44 = 1; 01783 01784 m12 = m14 01785 = m21 = m23 = m24 01786 = m32 = m34 01787 = m41 = m42 = m43 01788 = 0; 01789 01790 m11 = m33 = cos(phi); 01791 m31 = -(m13 = sin(phi)); 01792 } 01793 01794 template <typename T> 01795 BALL_INLINE 01796 void TMatrix4x4<T>::rotateZ(const TAngle<T>& phi) 01797 { 01798 TMatrix4x4<T> rotation; 01799 01800 rotation.setRotationZ(phi); 01801 *this *= rotation; 01802 } 01803 01804 template <typename T> 01805 void TMatrix4x4<T>::setRotationZ(const TAngle<T>& phi) 01806 { 01807 m33 = m44 = 1; 01808 01809 m13 = m14 = m23 = m24 = m31 = 01810 m32 = m34 = m41 = m42 = m43 = 0; 01811 01812 m11 = m22 = cos(phi); 01813 m12 = -(m21 = sin(phi)); 01814 } 01815 01816 template <typename T> 01817 BALL_INLINE 01818 void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const TVector3<T>& v) 01819 { 01820 rotate(phi, v.x, v.y, v.z); 01821 } 01822 01823 template <typename T> 01824 BALL_INLINE 01825 void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const TVector4<T>& v) 01826 { 01827 rotate(phi, v.x, v.y, v.z); 01828 } 01829 01830 // 01831 // Arbitrary axis rotation matrix. 01832 // 01833 // [Taken from the MESA-Library. But modified for additional Speed-Up.] 01834 // 01835 // This function was contributed by Erich Boleyn (erich@uruk.org). 01836 // 01837 // This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied 01838 // like so: Rz * Ry * T * Ry' * Rz'. T is the final rotation 01839 // (which is about the X-axis), and the two composite transforms 01840 // Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary 01841 // from the arbitrary axis to the X-axis then back. They are 01842 // all elementary rotations. 01843 // 01844 // Rz' is a rotation about the Z-axis, to bring the axis vector 01845 // into the x-z plane. Then Ry' is applied, rotating about the 01846 // Y-axis to bring the axis vector parallel with the X-axis. The 01847 // rotation about the X-axis is then performed. Ry and Rz are 01848 // simply the respective inverse transforms to bring the arbitrary 01849 // axis back to it's original orientation. The first transforms 01850 // Rz' and Ry' are considered inverses, since the data from the 01851 // arbitrary axis gives you info on how to get to it, not how 01852 // to get away from it, and an inverse must be applied. 01853 // 01854 // The basic calculation used is to recognize that the arbitrary 01855 // axis vector (x, y, z), since it is of unit length, actually 01856 // represents the sines and cosines of the angles to rotate the 01857 // X-axis to the same orientation, with theta being the angle about 01858 // Z and phi the angle about Y (in the order described above) 01859 // as follows: 01860 // 01861 // cos ( theta ) = x / sqrt ( 1 - z^2 ) 01862 // sin ( theta ) = y / sqrt ( 1 - z^2 ) 01863 // 01864 // cos ( phi ) = sqrt ( 1 - z^2 ) 01865 // sin ( phi ) = z 01866 // 01867 // Note that cos ( phi ) can further be inserted to the above 01868 // formulas: 01869 // 01870 // cos ( theta ) = x / cos ( phi ) 01871 // sin ( theta ) = y / sin ( phi ) 01872 // 01873 // ...etc. Because of those relations and the standard trigonometric 01874 // relations, it is pssible to reduce the transforms down to what 01875 // is used below. It may be that any primary axis chosen will give the 01876 // same results (modulo a sign convention) using thie method. 01877 // 01878 // Particularly nice is to notice that all divisions that might 01879 // have caused trouble when parallel to certain planes or 01880 // axis go away with care paid to reducing the expressions. 01881 // After checking, it does perform correctly under all cases, since 01882 // in all the cases of division where the denominator would have 01883 // been zero, the numerator would have been zero as well, giving 01884 // the expected result. 01885 01886 template <typename T> 01887 void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const T& ax, const T& ay, const T& az) 01888 { 01889 T xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c; 01890 T x = ax; 01891 T y = ay; 01892 T z = az; 01893 01894 double sin_angle = sin(phi); 01895 double cos_angle = cos(phi); 01896 01897 xx = x * x; 01898 yy = y * y; 01899 zz = z * z; 01900 01901 T mag = sqrt(xx + yy + zz); 01902 01903 if (mag == (T)0) 01904 { 01905 m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0; 01906 m11 = m22 = m33 = m44 = (T)1; 01907 } 01908 01909 x /= mag; 01910 y /= mag; 01911 z /= mag; 01912 01913 // we need to recalculate xx, yy, zz due to the 01914 // normalization. recalculation is probably faster 01915 // than normalizing xx, yy, zz 01916 xx = x*x; 01917 yy = y*y; 01918 zz = z*z; 01919 01920 xy = x * y; 01921 yz = y * z; 01922 zx = z * x; 01923 xs = (T) (x * sin_angle); 01924 ys = (T) (y * sin_angle); 01925 zs = (T) (z * sin_angle); 01926 one_c = (T) (1 - cos_angle); 01927 01928 m11 = (T)( (one_c * xx) + cos_angle ); 01929 m12 = (one_c * xy) - zs; 01930 m13 = (one_c * zx) + ys; 01931 m14 = 0; 01932 01933 m21 = (one_c * xy) + zs; 01934 m22 = (T) ((one_c * yy) + cos_angle); 01935 m23 = (one_c * yz) - xs; 01936 m24 = 0; 01937 01938 m31 = (one_c * zx) - ys; 01939 m32 = (one_c * yz) + xs; 01940 m33 = (T) ((one_c * zz) + cos_angle); 01941 m34 = 0; 01942 01943 m41 = 0; 01944 m42 = 0; 01945 m43 = 0; 01946 m44 = 1; 01947 } 01948 01949 template <typename T> 01950 void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const T& x, const T& y, const T& z) 01951 { 01952 m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0; 01953 m11 = m22 = m33 = m44 = (T)1; 01954 rotate(phi, x, y, z); 01955 } 01956 01957 template <typename T> 01958 BALL_INLINE 01959 void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const TVector3<T>& v) 01960 { 01961 m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0; 01962 m11 = m22 = m33 = m44 = (T)1; 01963 rotate(phi, v.x, v.y, v.z); 01964 } 01965 01966 template <typename T> 01967 BALL_INLINE 01968 void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const TVector4<T>& v) 01969 { 01970 m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0; 01971 m11 = m22 = m33 = m44 = (T)1; 01972 rotate(phi, v.x, v.y, v.z); 01973 } 01974 01975 template <typename T> 01976 bool TMatrix4x4<T>::operator == (const TMatrix4x4<T>& m) const 01977 { 01978 return 01979 ( m11 == m.m11 01980 && m12 == m.m12 01981 && m13 == m.m13 01982 && m14 == m.m14 01983 && m21 == m.m21 01984 && m22 == m.m22 01985 && m23 == m.m23 01986 && m24 == m.m24 01987 && m31 == m.m31 01988 && m32 == m.m32 01989 && m33 == m.m33 01990 && m34 == m.m34 01991 && m41 == m.m41 01992 && m42 == m.m42 01993 && m43 == m.m43 01994 && m44 == m.m44); 01995 } 01996 01997 template <typename T> 01998 bool TMatrix4x4<T>::operator != (const TMatrix4x4<T>& m) const 01999 { 02000 return 02001 ( m11 != m.m11 02002 || m12 != m.m12 02003 || m13 != m.m13 02004 || m14 != m.m14 02005 || m21 != m.m21 02006 || m22 != m.m22 02007 || m23 != m.m23 02008 || m24 != m.m24 02009 || m31 != m.m31 02010 || m32 != m.m32 02011 || m33 != m.m33 02012 || m34 != m.m34 02013 || m41 != m.m41 02014 || m42 != m.m42 02015 || m43 != m.m43 02016 || m44 != m.m44); 02017 } 02018 02019 template <typename T> 02020 bool TMatrix4x4<T>::isIdentity() const 02021 { 02022 return 02023 ( m11 == (T)1 02024 && m12 == (T)0 02025 && m13 == (T)0 02026 && m14 == (T)0 02027 && m21 == (T)0 02028 && m22 == (T)1 02029 && m23 == (T)0 02030 && m24 == (T)0 02031 && m31 == (T)0 02032 && m32 == (T)0 02033 && m33 == (T)1 02034 && m34 == (T)0 02035 && m41 == (T)0 02036 && m42 == (T)0 02037 && m43 == (T)0 02038 && m44 == (T)1); 02039 } 02040 02041 template <typename T> 02042 BALL_INLINE 02043 bool TMatrix4x4<T>::isRegular() const 02044 { 02045 return (getDeterminant() != (T)0); 02046 } 02047 02048 template <typename T> 02049 BALL_INLINE 02050 bool TMatrix4x4<T>::isSingular() const 02051 { 02052 return (getDeterminant() == (T)0); 02053 } 02054 02055 template <typename T> 02056 bool TMatrix4x4<T>::isSymmetric() const 02057 { 02058 return ( m12 == m21 && m13 == m31 02059 && m14 == m41 && m23 == m32 02060 && m24 == m42 && m34 == m43); 02061 } 02062 02063 template <typename T> 02064 bool TMatrix4x4<T>::isLowerTriangular() const 02065 { 02066 return ( m12 == (T)0 02067 && m13 == (T)0 02068 && m14 == (T)0 02069 && m23 == (T)0 02070 && m24 == (T)0 02071 && m34 == (T)0); 02072 } 02073 02074 template <typename T> 02075 bool TMatrix4x4<T>::isUpperTriangular() const 02076 { 02077 return ( m21 == (T)0 02078 && m31 == (T)0 02079 && m32 == (T)0 02080 && m41 == (T)0 02081 && m42 == (T)0 02082 && m43 == (T)0); 02083 } 02084 02085 template <typename T> 02086 BALL_INLINE 02087 bool TMatrix4x4<T>::isDiagonal() const 02088 { 02089 return ( m12 == (T)0 02090 && m13 == (T)0 02091 && m14 == (T)0 02092 && m21 == (T)0 02093 && m23 == (T)0 02094 && m24 == (T)0 02095 && m31 == (T)0 02096 && m32 == (T)0 02097 && m34 == (T)0 02098 && m41 == (T)0 02099 && m42 == (T)0 02100 && m43 == (T)0); 02101 } 02102 02103 template <typename T> 02104 bool TMatrix4x4<T>::isValid() const 02105 { 02106 T **ptr = (T **)comp_ptr_; 02107 02108 return ( *ptr++ == &m11 02109 && *ptr++ == &m12 02110 && *ptr++ == &m13 02111 && *ptr++ == &m14 02112 && *ptr++ == &m21 02113 && *ptr++ == &m22 02114 && *ptr++ == &m23 02115 && *ptr++ == &m24 02116 && *ptr++ == &m31 02117 && *ptr++ == &m32 02118 && *ptr++ == &m33 02119 && *ptr++ == &m34 02120 && *ptr++ == &m41 02121 && *ptr++ == &m42 02122 && *ptr++ == &m43 02123 && *ptr == &m44); 02124 } 02125 02126 template <typename T> 02127 std::istream& operator >> (std::istream& s, TMatrix4x4<T>& m) 02128 { 02129 char c; 02130 s >> c 02131 >> m.m11 >> m.m12 >> m.m13 >> m.m14 >> c >> c 02132 >> m.m21 >> m.m22 >> m.m23 >> m.m24 >> c >> c 02133 >> m.m31 >> m.m32 >> m.m33 >> m.m34 >> c >> c 02134 >> m.m41 >> m.m42 >> m.m43 >> m.m44 >> c; 02135 02136 return s; 02137 } 02138 02139 template <typename T> 02140 std::ostream& operator << (std::ostream& s, const TMatrix4x4<T>& m) 02141 { 02142 s << '/' << std::setw(14) << m.m11 << ' ' << std::setw(14) << m.m12 << ' ' << std::setw(14) << m.m13 << ' ' << std::setw(14) << m.m14 << " \\" << std::endl 02143 << '|' << std::setw(14) << m.m21 << ' ' << std::setw(14) << m.m22 << ' ' << std::setw(14) << m.m23 << ' ' << std::setw(14) << m.m24 << " |" << std::endl 02144 << '|' << std::setw(14) << m.m31 << ' ' << std::setw(14) << m.m32 << ' ' << std::setw(14) << m.m33 << ' ' << std::setw(14) << m.m34 << " |" << std::endl 02145 << '\\' << std::setw(14) << m.m41 << ' ' << std::setw(14) << m.m42 << ' ' << std::setw(14) << m.m43 << ' ' << std::setw(14) << m.m44 << " /" << std::endl; 02146 02147 return s; 02148 } 02149 02150 template <typename T> 02151 void TMatrix4x4<T>::dump(std::ostream& s, Size depth) const 02152 { 02153 BALL_DUMP_STREAM_PREFIX(s); 02154 02155 BALL_DUMP_HEADER(s, this, this); 02156 02157 BALL_DUMP_DEPTH(s, depth); 02158 s << m11 << " " << m12 << " " << m13 << " " << m14 << std::endl; 02159 02160 BALL_DUMP_DEPTH(s, depth); 02161 s << m21 << " " << m22 << " " << m23 << " " << m24 << std::endl; 02162 02163 BALL_DUMP_DEPTH(s, depth); 02164 s << m31 << " " << m32 << " " << m33 << " " << m34 << std::endl; 02165 02166 BALL_DUMP_DEPTH(s, depth); 02167 s << m41 << " " << m42 << " " << m43 << " " << m44 << std::endl; 02168 02169 BALL_DUMP_STREAM_SUFFIX(s); 02170 } 02171 02173 template <typename T> 02174 TMatrix4x4<T> operator * (const T& scalar, const TMatrix4x4<T>& m); 02175 02177 template <typename T> 02178 TVector3<T> operator * (const TMatrix4x4<T>& matrix, const TVector3<T>& vector); 02179 02184 typedef TMatrix4x4<float> Matrix4x4; 02185 02186 } // namespace BALL 02187 02188 #endif // BALL_MATHS_MATRIX44_H