BALL: matrix44.h Source File

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