BALL  1.4.2
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Groups Pages
matrix44.h
Go to the documentation of this file.
1 // -*- Mode: C++; tab-width: 2; -*-
2 // vi: set ts=2:
3 //
4 // $Id: matrix44.h,v 1.55.14.1 2007/03/25 21:23:45 oliver Exp $
5 //
6 
7 #ifndef BALL_MATHS_MATRIX44_H
8 #define BALL_MATHS_MATRIX44_H
9 
10 #ifndef BALL_COMMON_EXCEPTION_H
11 # include <BALL/COMMON/exception.h>
12 #endif
13 
14 #include <cmath>
15 #include <iostream>
16 #include <cstdlib>
17 
18 #ifndef BALL_MATHS_ANGLE_H
19 # include <BALL/MATHS/angle.h>
20 #endif
21 
22 #ifndef BALL_MATHS_VECTOR3_H
23 # include <BALL/MATHS/vector3.h>
24 #endif
25 
26 #ifndef BALL_MATHS_VECTOR4_H
27 # include <BALL/MATHS/vector4.h>
28 #endif
29 
30 namespace BALL
31 {
38 
39  template <typename T>
40  class TMatrix4x4;
41 
50  template <typename T>
51  std::istream& operator >> (std::istream& s, TMatrix4x4<T>& m)
52  ;
53 
59  template <typename T>
60  std::ostream& operator << (std::ostream& s, const TMatrix4x4<T>& m)
61  ;
63 
66  template <typename T>
67  class TMatrix4x4
68  {
69  public:
70 
71  BALL_CREATE(TMatrix4x4)
72 
73 
76 
81  TMatrix4x4()
82  ;
83 
90  TMatrix4x4(const T* ptr);
91 
98  TMatrix4x4(const T ptr[4][4]);
99 
105  TMatrix4x4(const TMatrix4x4& m)
106  ;
107 
116  TMatrix4x4
117  (const TVector4<T>& col1, const TVector4<T>& col2,
118  const TVector4<T>& col3, const TVector4<T>& col4)
119  ;
120 
125  TMatrix4x4
126  (const T& m11, const T& m12, const T& m13, const T& m14,
127  const T& m21, const T& m22, const T& m23, const T& m24,
128  const T& m31, const T& m32, const T& m33, const T& m34,
129  const T& m41, const T& m42, const T& m43, const T& m44)
130  ;
131 
136  virtual ~TMatrix4x4()
137 
138  {
139  }
140 
144  virtual void clear()
145  ;
146 
148 
151 
157  void set(const T* ptr);
158 
164  void set(const T ptr[4][4]);
165 
169  void set(const TMatrix4x4& m);
170 
177  void set
178  (const TVector4<T>& col1, const TVector4<T>& col2,
179  const TVector4<T>& col3, const TVector4<T>& col4);
180 
184  void set
185  (const T& m11, const T& m12, const T& m13, const T& m14,
186  const T& m21, const T& m22, const T& m23, const T& m24,
187  const T& m31, const T& m32, const T& m33, const T& m34,
188  const T& m41, const T& m42, const T& m43, const T& m44);
189 
195  TMatrix4x4& operator = (const T* ptr);
196 
202  TMatrix4x4& operator = (const T ptr[4][4]);
203 
208  TMatrix4x4& operator = (const TMatrix4x4& m);
209 
215  void get(T* ptr) const;
216 
222  void get(T ptr[4][4]) const;
223 
228  void get(TMatrix4x4& m) const;
229 
236  void get
237  (TVector4<T>& col1, TVector4<T>& col2,
238  TVector4<T>& col3, TVector4<T>& col4) const;
239 
243  void get
244  (T& m11, T& m12, T& m13, T& m14,
245  T& m21, T& m22, T& m23, T& m24,
246  T& m31, T& m32, T& m33, T& m34,
247  T& m41, T& m42, T& m43, T& m44) const;
248 
252  void swap(TMatrix4x4& m);
253 
255 
258 
263  T getTrace() const;
264 
268  static const TMatrix4x4& getZero();
269 
274  static const TMatrix4x4& getIdentity();
275 
280  void setIdentity();
281 
286  void set(const T& t = (T)1);
287 
292  void transpose();
293 
299  TVector4<T> getRow(Position row) const;
300 
306  TVector4<T> getColumn(Position col) const;
307 
313  void setRow(Position row, const TVector4<T>& row_value);
314 
320  void setColumn(Position col, const TVector4<T>& col_value);
321 
329  bool isEqual(const TMatrix4x4& m) const;
330 
334  TVector4<T> getDiagonal() const;
335 
342  T& operator () (Position row, Position col);
343 
350  const T& operator () (Position row, Position col) const;
351 
358  const T& operator [] (Position position) const;
359 
364  T& operator [] (Position position);
365 
368  TMatrix4x4 operator + () const;
369 
372  TMatrix4x4 operator - () const;
373 
379  TMatrix4x4 operator + (const TMatrix4x4& m) const;
380 
386  TMatrix4x4& operator += (const TMatrix4x4& m);
387 
393  TMatrix4x4 operator - (const TMatrix4x4& m) const;
394 
400  TMatrix4x4& operator -= (const TMatrix4x4& m);
401 
406  TMatrix4x4 operator * (const T& scalar) const;
407 
412  TMatrix4x4& operator *= (const T& scalar);
413 
419  TMatrix4x4 operator / (const T& scalar) const;
420 
426  TMatrix4x4& operator /= (const T& scalar);
427 
431  TMatrix4x4 operator * (const TMatrix4x4& m) const;
432 
436  TMatrix4x4& operator *= (const TMatrix4x4& m);
437 
441  TVector4<T> operator * (const TVector4<T>& vector) const;
442 
449  bool invert(TMatrix4x4& inverse) const;
450 
456  bool invert();
457 
461  T getDeterminant() const;
462 
468  void translate(const T &x, const T &y, const T &z);
469 
473  void translate(const TVector3<T>& v);
474 
480  void setTranslation(const T& x, const T& y, const T& z);
481 
485  void setTranslation(const TVector3<T>& v);
486 
492  void scale(const T& x_scale, const T& y_scale, const T& z_scale);
493 
497  void scale(const T& scale);
498 
502  void scale(const TVector3<T>& v);
503 
509  void setScale(const T& x_scale, const T& y_scale, const T& z_scale);
510 
514  void setScale(const T& scale);
515 
519  void setScale(const TVector3<T>& v);
520 
524  void rotateX(const TAngle<T>& phi);
525 
529  void setRotationX(const TAngle<T>& phi);
530 
534  void rotateY(const TAngle<T>& phi);
535 
539  void setRotationY(const TAngle<T>& phi);
540 
544  void rotateZ(const TAngle<T>& phi);
545 
549  void setRotationZ(const TAngle<T>& phi);
550 
557  void rotate(const TAngle<T>& phi, const T& axis_x, const T& axis_y, const T& axis_z);
558 
563  void rotate(const TAngle<T>& phi, const TVector3<T>& axis);
564 
569  void rotate(const TAngle<T>& phi, const TVector4<T>& axis);
570 
577  void setRotation(const TAngle<T>& phi, const T& axis_x, const T& axis_y, const T& axis_z);
578 
583  void setRotation(const TAngle<T>& phi, const TVector3<T>& axis);
584 
589  void setRotation(const TAngle<T>& phi, const TVector4<T>& axis);
591 
595 
601  bool operator == (const TMatrix4x4& m) const;
602 
608  bool operator != (const TMatrix4x4& m) const;
609 
614  bool isIdentity() const;
615 
619  bool isRegular() const;
620 
624  bool isSingular() const;
625 
630  bool isSymmetric() const;
631 
635  bool isLowerTriangular() const;
636 
640  bool isUpperTriangular() const;
641 
645  bool isDiagonal() const;
647 
651 
656  bool isValid() const;
657 
664  void dump(std::ostream& s = std::cout, Size depth = 0) const;
666 
670 
672  T m11;
673 
675  T m12;
676 
678  T m13;
679 
681  T m14;
682 
684  T m21;
685 
687  T m22;
688 
690  T m23;
691 
693  T m24;
694 
696  T m31;
697 
699  T m32;
700 
702  T m33;
703 
705  T m34;
706 
708  T m41;
709 
711  T m42;
712 
714  T m43;
715 
717  T m44;
719 
720  private:
721 
722  void initializeComponentPointers_()
723  {
724  T **ptr = (T **)comp_ptr_;
725 
726  *ptr++ = &m11; *ptr++ = &m12; *ptr++ = &m13; *ptr++ = &m14;
727  *ptr++ = &m21; *ptr++ = &m22; *ptr++ = &m23; *ptr++ = &m24;
728  *ptr++ = &m31; *ptr++ = &m32; *ptr++ = &m33; *ptr++ = &m34;
729  *ptr++ = &m41; *ptr++ = &m42; *ptr++ = &m43; *ptr = &m44;
730  }
731 
732  // pointers to the components of the matrix
733  T* comp_ptr_[16];
734  };
736 
737  template <typename T>
738  TMatrix4x4<T>::TMatrix4x4()
739  : m11(0), m12(0), m13(0), m14(0),
740  m21(0), m22(0), m23(0), m24(0),
741  m31(0), m32(0), m33(0), m34(0),
742  m41(0), m42(0), m43(0), m44(0)
743  {
744  initializeComponentPointers_();
745  }
746 
747  template <typename T>
748  TMatrix4x4<T>::TMatrix4x4( const T* ptr)
749  {
750  if (ptr == 0)
751  {
752  throw Exception::NullPointer(__FILE__, __LINE__);
753  }
754 
755  m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++;
756  m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++;
757  m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++;
758  m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr;
759 
760  initializeComponentPointers_();
761  }
762 
763  template <typename T>
764  TMatrix4x4<T>::TMatrix4x4(const T array_ptr[4][4])
765  {
766  if (array_ptr == 0)
767  {
768  throw Exception::NullPointer(__FILE__, __LINE__);
769  }
770 
771  const T *ptr = *array_ptr;
772 
773  m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++;
774  m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++;
775  m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++;
776  m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr;
777 
778  initializeComponentPointers_();
779  }
780 
781  template <typename T>
782  TMatrix4x4<T>::TMatrix4x4(const TMatrix4x4<T>& m)
783  : m11(m.m11), m12(m.m12), m13(m.m13), m14(m.m14),
784  m21(m.m21), m22(m.m22), m23(m.m23), m24(m.m24),
785  m31(m.m31), m32(m.m32), m33(m.m33), m34(m.m34),
786  m41(m.m41), m42(m.m42), m43(m.m43), m44(m.m44)
787  {
788  initializeComponentPointers_();
789  }
790 
791 
792  template <typename T>
793  TMatrix4x4<T>::TMatrix4x4
794  (const TVector4<T>& col1, const TVector4<T>& col2,
795  const TVector4<T>& col3,const TVector4<T>& col4)
796  : m11(col1.x), m12(col1.y), m13(col1.z), m14(col1.h),
797  m21(col2.x), m22(col2.y), m23(col2.z), m24(col2.h),
798  m31(col3.x), m32(col3.y), m33(col3.z), m34(col3.h),
799  m41(col4.x), m42(col4.y), m43(col4.z), m44(col4.h)
800  {
801  initializeComponentPointers_();
802  }
803 
804  template <typename T>
805  TMatrix4x4<T>::TMatrix4x4
806  (const T& m11, const T& m12, const T& m13, const T& m14,
807  const T& m21, const T& m22, const T& m23, const T& m24,
808  const T& m31, const T& m32, const T& m33, const T& m34,
809  const T& m41, const T& m42, const T& m43, const T& m44)
810  : m11(m11), m12(m12), m13(m13), m14(m14),
811  m21(m21), m22(m22), m23(m23), m24(m24),
812  m31(m31), m32(m32), m33(m33), m34(m34),
813  m41(m41), m42(m42), m43(m43), m44(m44)
814  {
815  initializeComponentPointers_();
816  }
817 
818  template <typename T>
819  void TMatrix4x4<T>::clear()
820  {
821  set((T)0);
822  }
823 
824  template <typename T>
825  void TMatrix4x4<T>::set(const T* ptr)
826  {
827  if (ptr == 0)
828  {
829  throw Exception::NullPointer(__FILE__, __LINE__);
830  }
831 
832  m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++;
833  m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++;
834  m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++;
835  m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr;
836  }
837 
838  template <typename T>
839  void TMatrix4x4<T>::set(const T array_ptr[4][4])
840  {
841  if (array_ptr == 0)
842  {
843  throw Exception::NullPointer(__FILE__, __LINE__);
844  }
845 
846  const T *ptr = *array_ptr;
847 
848  m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++;
849  m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++;
850  m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++;
851  m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr;
852  }
853 
854  template <typename T>
855  void TMatrix4x4<T>::set(const TMatrix4x4<T>& m)
856  {
857  m11 = m.m11; m12 = m.m12; m13 = m.m13; m14 = m.m14;
858  m21 = m.m21; m22 = m.m22; m23 = m.m23; m24 = m.m24;
859  m31 = m.m31; m32 = m.m32; m33 = m.m33; m34 = m.m34;
860  m41 = m.m41; m42 = m.m42; m43 = m.m43; m44 = m.m44;
861  }
862 
863  template <typename T>
864  void TMatrix4x4<T>::set
865  (const TVector4<T>& col1, const TVector4<T>& col2,
866  const TVector4<T>& col3, const TVector4<T>& col4)
867  {
868  m11 = col1.x; m12 = col1.y; m13 = col1.z; m14 = col1.h;
869  m21 = col2.x; m22 = col2.y; m23 = col2.z; m24 = col2.h;
870  m31 = col3.x; m32 = col3.y; m33 = col3.z; m34 = col3.h;
871  m41 = col4.x; m42 = col4.y; m43 = col4.z; m44 = col4.h;
872  }
873 
874  template <typename T>
875  void TMatrix4x4<T>::set
876  (const T& c11, const T& c12, const T& c13, const T& c14,
877  const T& c21, const T& c22, const T& c23, const T& c24,
878  const T& c31, const T& c32, const T& c33, const T& c34,
879  const T& c41, const T& c42, const T& c43, const T& c44)
880  {
881  m11 = c11; m12 = c12; m13 = c13; m14 = c14;
882  m21 = c21; m22 = c22; m23 = c23; m24 = c24;
883  m31 = c31; m32 = c32; m33 = c33; m34 = c34;
884  m41 = c41; m42 = c42; m43 = c43; m44 = c44;
885  }
886 
887  template <typename T>
888  BALL_INLINE
889  TMatrix4x4<T>& TMatrix4x4<T>::operator = (const T* ptr)
890  {
891  set(ptr);
892  return *this;
893  }
894 
895  template <typename T>
896  BALL_INLINE
897  TMatrix4x4<T>& TMatrix4x4<T>::operator = (const T array_ptr[4][4])
898  {
899  set(array_ptr);
900  return *this;
901  }
902 
903  template <typename T>
904  BALL_INLINE
905  TMatrix4x4<T>& TMatrix4x4<T>::operator = (const TMatrix4x4<T>& m)
906  {
907  set(m);
908  return *this;
909  }
910 
911  template <typename T>
912  void TMatrix4x4<T>::get(T* ptr) const
913  {
914  if (ptr == 0)
915  {
916  throw Exception::NullPointer(__FILE__, __LINE__);
917  }
918 
919  *ptr++ = m11; *ptr++ = m12; *ptr++ = m13; *ptr++ = m14;
920  *ptr++ = m21; *ptr++ = m22; *ptr++ = m23; *ptr++ = m24;
921  *ptr++ = m31; *ptr++ = m32; *ptr++ = m33; *ptr++ = m34;
922  *ptr++ = m41; *ptr++ = m42; *ptr++ = m43; *ptr = m44;
923  }
924 
925  template <typename T>
926  void TMatrix4x4<T>::get(T array_ptr[4][4]) const
927  {
928  if (array_ptr == 0)
929  {
930  throw Exception::NullPointer(__FILE__, __LINE__);
931  }
932 
933  T *ptr = *array_ptr;
934 
935  *ptr++ = m11; *ptr++ = m12; *ptr++ = m13; *ptr++ = m14;
936  *ptr++ = m21; *ptr++ = m22; *ptr++ = m23; *ptr++ = m24;
937  *ptr++ = m31; *ptr++ = m32; *ptr++ = m33; *ptr++ = m34;
938  *ptr++ = m41; *ptr++ = m42; *ptr++ = m43; *ptr = m44;
939  }
940 
941  template <typename T>
942  void TMatrix4x4<T>::get(TMatrix4x4<T>& m) const
943  {
944  m.set(*this);
945  }
946 
947  template <typename T>
948  void TMatrix4x4<T>::get
949  (TVector4<T>& col1, TVector4<T>& col2,
950  TVector4<T>& col3, TVector4<T>& col4) const
951  {
952  col1.x = m11; col1.y = m12; col1.z = m13; col1.h = m14;
953  col2.x = m21; col2.y = m22; col2.z = m23; col2.h = m24;
954  col3.x = m31; col3.y = m32; col3.z = m33; col3.h = m34;
955  col4.x = m41; col4.y = m42; col4.z = m43; col4.h = m44;
956  }
957 
958  template <typename T>
959  void TMatrix4x4<T>::get
960  (T& c11, T& c12, T& c13, T& c14,
961  T& c21, T& c22, T& c23, T& c24,
962  T& c31, T& c32, T& c33, T& c34,
963  T& c41, T& c42, T& c43, T& c44) const
964  {
965  c11 = m11; c12 = m12; c13 = m13; c14 = m14;
966  c21 = m21; c22 = m22; c23 = m23; c24 = m24;
967  c31 = m31; c32 = m32; c33 = m33; c34 = m34;
968  c41 = m41; c42 = m42; c43 = m43; c44 = m44;
969  }
970 
971  template <typename T>
972  BALL_INLINE
973  T TMatrix4x4<T>::getTrace() const
974  {
975  return (m11 + m22 + m33 + m44);
976  }
977 
978  template <typename T>
979  BALL_INLINE
980  const TMatrix4x4<T>& TMatrix4x4<T>::getZero()
981  {
982  static TMatrix4x4<T> null_matrix
983  (0, 0, 0, 0,
984  0, 0, 0, 0,
985  0, 0, 0, 0,
986  0, 0, 0, 0);
987 
988  return null_matrix;
989  }
990 
991 
992  template <typename T>
993  BALL_INLINE
994  void TMatrix4x4<T>::setIdentity()
995  {
996  m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0;
997  m11 = m22 = m33 = m44 = (T)1;
998  }
999  template <typename T>
1000  BALL_INLINE
1001  const TMatrix4x4<T>& TMatrix4x4<T>::getIdentity()
1002  {
1003  static TMatrix4x4<T> identity
1004  (1, 0, 0, 0,
1005  0, 1, 0, 0,
1006  0, 0, 1, 0,
1007  0, 0, 0, 1);
1008 
1009  return identity;
1010  }
1011 
1012  template <typename T>
1013  void TMatrix4x4<T>::set(const T& t)
1014  {
1015  m11 = m12 = m13 = m14
1016  = m21 = m22 = m23 = m24
1017  = m31 = m32 = m33 = m34
1018  = m41 = m42 = m43 = m44
1019  = t;
1020  }
1021 
1022  template <typename T>
1023  void TMatrix4x4<T>::swap(TMatrix4x4<T>& m)
1024  {
1025  T tmp = m11; m11 = m.m11; m.m11 = tmp;
1026  tmp = m12; m12 = m.m12; m.m12 = tmp;
1027  tmp = m13; m13 = m.m13; m.m13 = tmp;
1028  tmp = m14; m14 = m.m14; m.m14 = tmp;
1029  tmp = m21; m21 = m.m21; m.m21 = tmp;
1030  tmp = m22; m22 = m.m22; m.m22 = tmp;
1031  tmp = m23; m23 = m.m23; m.m23 = tmp;
1032  tmp = m24; m24 = m.m24; m.m24 = tmp;
1033  tmp = m31; m31 = m.m31; m.m31 = tmp;
1034  tmp = m32; m32 = m.m32; m.m32 = tmp;
1035  tmp = m33; m33 = m.m33; m.m33 = tmp;
1036  tmp = m34; m34 = m.m34; m.m34 = tmp;
1037  tmp = m41; m41 = m.m41; m.m41 = tmp;
1038  tmp = m42; m42 = m.m42; m.m42 = tmp;
1039  tmp = m43; m43 = m.m43; m.m43 = tmp;
1040  tmp = m44; m44 = m.m44; m.m44 = tmp;
1041  }
1042 
1043  template <typename T>
1044  void TMatrix4x4<T>::transpose()
1045  {
1046  T tmp = m12;
1047  m12 = m21;
1048  m21 = tmp;
1049 
1050  tmp = m13;
1051  m13 = m31;
1052  m31 = tmp;
1053 
1054  tmp = m14;
1055  m14 = m41;
1056  m41 = tmp;
1057 
1058  tmp = m23;
1059  m23 = m32;
1060  m32 = tmp;
1061 
1062  tmp = m24;
1063  m24 = m42;
1064  m42 = tmp;
1065 
1066  tmp = m34;
1067  m34 = m43;
1068  m43 = tmp;
1069  }
1070 
1071  template <typename T>
1072  TVector4<T> TMatrix4x4<T>::getRow(Position row) const
1073  {
1074  if (row > 3)
1075  {
1076  throw Exception::IndexOverflow(__FILE__, __LINE__, row, 3);
1077  }
1078 
1079  // calculate the start of the row in the array
1080  const T* ptr = comp_ptr_[4 * row];
1081  return TVector4<T> (ptr[0], ptr[1], ptr[2], ptr[3]);
1082  }
1083 
1084  template <typename T>
1085  TVector4<T> TMatrix4x4<T>::getColumn(Position col) const
1086  {
1087  if (col > 3)
1088  {
1089  throw Exception::IndexOverflow(__FILE__, __LINE__, col, 3);
1090  }
1091 
1092  const T* ptr = comp_ptr_[col];
1093 
1094  return TVector4<T> (ptr[0], ptr[4], ptr[8], ptr[12]);
1095  }
1096 
1097 
1098  template <typename T>
1099  void TMatrix4x4<T>::setRow(Position row, const TVector4<T>& row_value)
1100  {
1101  if (row > 3)
1102  {
1103  throw Exception::IndexOverflow(__FILE__, __LINE__, row, 3);
1104  }
1105 
1106  // calculate a pointer to the start of the row
1107  T* ptr = comp_ptr_[4 * row];
1108 
1109  ptr[0] = row_value.x;
1110  ptr[1] = row_value.y;
1111  ptr[2] = row_value.z;
1112  ptr[3] = row_value.h;
1113  }
1114 
1115  template <typename T>
1116  void TMatrix4x4<T>::setColumn(Position col, const TVector4<T>& col_value)
1117  {
1118  if (col > 3)
1119  {
1120  throw Exception::IndexOverflow(__FILE__, __LINE__, col, 3);
1121  }
1122 
1123  // calculate a pointer to the start of the column
1124  T* ptr = comp_ptr_[col];
1125 
1126  ptr[0] = col_value.x;
1127  ptr[4] = col_value.y;
1128  ptr[8] = col_value.z;
1129  ptr[12] = col_value.h;
1130  }
1131 
1132  template <typename T>
1133  bool TMatrix4x4<T>::isEqual(const TMatrix4x4<T>& m) const
1134  {
1135  // iterate over all component pointers
1136  // and compare the elements for approximate equality
1137  for (Position i = 0; i < 16; i++)
1138  {
1139  if (Maths::isEqual(*comp_ptr_[i], *m.comp_ptr_[i]) == false)
1140  {
1141  return false;
1142  }
1143  }
1144 
1145  return true;
1146  }
1147 
1148  template <typename T>
1149  TVector4<T>TMatrix4x4<T>::getDiagonal() const
1150  {
1151  return TVector4<T>(m11, m22, m33, m44);
1152  }
1153 
1154  template <typename T>
1155  BALL_INLINE
1156  T& TMatrix4x4<T>::operator () (Position row, Position col)
1157  {
1158  if ((row > 3) || (col > 3))
1159  {
1160  throw Exception::IndexOverflow(__FILE__, __LINE__, row + col, 3);
1161  }
1162 
1163  return *comp_ptr_[4 * row + col];
1164  }
1165 
1166  template <typename T>
1167  BALL_INLINE
1168  const T& TMatrix4x4<T>::operator () (Position row, Position col) const
1169  {
1170  if ((row > 3) || (col > 3))
1171  {
1172  throw Exception::IndexOverflow(__FILE__, __LINE__, row + col, 3);
1173  }
1174 
1175  return *comp_ptr_[4 * row + col];
1176  }
1177 
1178  template <typename T>
1179  BALL_INLINE
1180  const T& TMatrix4x4<T>::operator [] (Position position) const
1181  {
1182  if (position > 15)
1183  {
1184  throw Exception::IndexOverflow(__FILE__, __LINE__, position, 15);
1185  }
1186  return *comp_ptr_[position];
1187  }
1188 
1189  template <typename T>
1190  BALL_INLINE
1191  T& TMatrix4x4<T>::operator [] (Position position)
1192  {
1193  if (position > 15)
1194  {
1195  throw Exception::IndexOverflow(__FILE__, __LINE__, position, 15);
1196  }
1197  return *comp_ptr_[position];
1198  }
1199 
1200  template <typename T>
1201  BALL_INLINE
1202  TMatrix4x4<T> TMatrix4x4<T>::operator + () const
1203  {
1204  return *this;
1205  }
1206 
1207  template <typename T>
1208  BALL_INLINE TMatrix4x4<T>
1209  TMatrix4x4<T>::operator - () const
1210  {
1211  return TMatrix4x4<T>
1212  (-m11, -m12, -m13, -m14,
1213  -m21, -m22, -m23, -m24,
1214  -m31, -m32, -m33, -m34,
1215  -m41, -m42, -m43, -m44);
1216  }
1217 
1218  template <typename T>
1219  TMatrix4x4<T> TMatrix4x4<T>::operator + (const TMatrix4x4<T>& m) const
1220  {
1221  return TMatrix4x4<T>
1222  (m11 + m.m11, m12 + m.m12, m13 + m.m13, m14 + m.m14,
1223  m21 + m.m21, m22 + m.m22, m23 + m.m23, m24 + m.m24,
1224  m31 + m.m31, m32 + m.m32, m33 + m.m33, m34 + m.m34,
1225  m41 + m.m41, m42 + m.m42, m43 + m.m43, m44 + m.m44);
1226  }
1227 
1228  template <typename T>
1229  TMatrix4x4<T>& TMatrix4x4<T>::operator += (const TMatrix4x4<T>& m)
1230  {
1231  m11 += m.m11;
1232  m12 += m.m12;
1233  m13 += m.m13;
1234  m14 += m.m14;
1235  m21 += m.m21;
1236  m22 += m.m22;
1237  m23 += m.m23;
1238  m24 += m.m24;
1239  m31 += m.m31;
1240  m32 += m.m32;
1241  m33 += m.m33;
1242  m34 += m.m34;
1243  m41 += m.m41;
1244  m42 += m.m42;
1245  m43 += m.m43;
1246  m44 += m.m44;
1247 
1248  return *this;
1249  }
1250 
1251  template <typename T>
1252  TMatrix4x4<T> TMatrix4x4<T>::operator - (const TMatrix4x4<T>& m) const
1253  {
1254  return TMatrix4x4<T>
1255  (m11 - m.m11, m12 - m.m12, m13 - m.m13, m14 - m.m14,
1256  m21 - m.m21, m22 - m.m22, m23 - m.m23, m24 - m.m24,
1257  m31 - m.m31, m32 - m.m32, m33 - m.m33, m34 - m.m34,
1258  m41 - m.m41, m42 - m.m42, m43 - m.m43, m44 - m.m44);
1259  }
1260 
1261  template <typename T>
1262  TMatrix4x4<T>& TMatrix4x4<T>::operator -= (const TMatrix4x4<T>& m)
1263  {
1264  m11 -= m.m11;
1265  m12 -= m.m12;
1266  m13 -= m.m13;
1267  m14 -= m.m14;
1268  m21 -= m.m21;
1269  m22 -= m.m22;
1270  m23 -= m.m23;
1271  m24 -= m.m24;
1272  m31 -= m.m31;
1273  m32 -= m.m32;
1274  m33 -= m.m33;
1275  m34 -= m.m34;
1276  m41 -= m.m41;
1277  m42 -= m.m42;
1278  m43 -= m.m43;
1279  m44 -= m.m44;
1280 
1281  return *this;
1282  }
1283 
1284  template <typename T>
1285  TMatrix4x4<T> TMatrix4x4<T>::operator * (const T& scalar) const
1286  {
1287  return TMatrix4x4<T>
1288  (m11 * scalar, m12 * scalar, m13 * scalar, m14 * scalar,
1289  m21 * scalar, m22 * scalar, m23 * scalar, m24 * scalar,
1290  m31 * scalar, m32 * scalar, m33 * scalar, m34 * scalar,
1291  m41 * scalar, m42 * scalar, m43 * scalar, m44 * scalar);
1292  }
1293 
1294  template <typename T>
1295  TMatrix4x4<T> operator * (const T& scalar, const TMatrix4x4<T>& m)
1296  {
1297  return TMatrix4x4<T>
1298  (scalar * m.m11, scalar * m.m12, scalar * m.m13, scalar * m.m14,
1299  scalar * m.m21, scalar * m.m22, scalar * m.m23, scalar * m.m24,
1300  scalar * m.m31, scalar * m.m32, scalar * m.m33, scalar * m.m34,
1301  scalar * m.m41, scalar * m.m42, scalar * m.m43, scalar * m.m44);
1302  }
1303 
1304  template <typename T>
1305  TMatrix4x4<T>& TMatrix4x4<T>::operator *= (const T& scalar)
1306  {
1307  m11 *= scalar;
1308  m12 *= scalar;
1309  m13 *= scalar;
1310  m14 *= scalar;
1311  m21 *= scalar;
1312  m22 *= scalar;
1313  m23 *= scalar;
1314  m24 *= scalar;
1315  m31 *= scalar;
1316  m32 *= scalar;
1317  m33 *= scalar;
1318  m34 *= scalar;
1319  m41 *= scalar;
1320  m42 *= scalar;
1321  m43 *= scalar;
1322  m44 *= scalar;
1323 
1324  return *this;
1325  }
1326 
1327  template <typename T>
1328  TVector3<T> operator *(const TMatrix4x4<T>& matrix, const TVector3<T>& vector)
1329  {
1330  return TVector3<T>
1331  (matrix.m11 * vector.x + matrix.m12 * vector.y + matrix.m13 * vector.z + matrix.m14,
1332  matrix.m21 * vector.x + matrix.m22 * vector.y + matrix.m23 * vector.z + matrix.m24,
1333  matrix.m31 * vector.x + matrix.m32 * vector.y + matrix.m33 * vector.z + matrix.m34);
1334  }
1335 
1336  template <typename T>
1337  BALL_INLINE
1338  TMatrix4x4<T>TMatrix4x4<T>::operator / (const T& scalar) const
1339  {
1340  if (scalar == (T)0)
1341  {
1342  throw Exception::DivisionByZero(__FILE__, __LINE__);
1343  }
1344 
1345  return (*this * ((T)1 / scalar));
1346  }
1347 
1348  template <typename T>
1349  BALL_INLINE
1350  TMatrix4x4<T>& TMatrix4x4<T>::operator /= (const T& scalar)
1351  {
1352  if (scalar == (T)0)
1353  {
1354  throw Exception::DivisionByZero(__FILE__, __LINE__);
1355  }
1356 
1357  return (*this *= (T)1 / scalar);
1358  }
1359 
1360  template <typename T>
1361  TMatrix4x4<T> TMatrix4x4<T>::operator * (const TMatrix4x4<T>& m) const
1362  {
1363  return TMatrix4x4<T>
1364  (m11 * m.m11 + m12 * m.m21 + m13 * m.m31 + m14 * m.m41,
1365  m11 * m.m12 + m12 * m.m22 + m13 * m.m32 + m14 * m.m42,
1366  m11 * m.m13 + m12 * m.m23 + m13 * m.m33 + m14 * m.m43,
1367  m11 * m.m14 + m12 * m.m24 + m13 * m.m34 + m14 * m.m44,
1368 
1369  m21 * m.m11 + m22 * m.m21 + m23 * m.m31 + m24 * m.m41,
1370  m21 * m.m12 + m22 * m.m22 + m23 * m.m32 + m24 * m.m42,
1371  m21 * m.m13 + m22 * m.m23 + m23 * m.m33 + m24 * m.m43,
1372  m21 * m.m14 + m22 * m.m24 + m23 * m.m34 + m24 * m.m44,
1373 
1374  m31 * m.m11 + m32 * m.m21 + m33 * m.m31 + m34 * m.m41,
1375  m31 * m.m12 + m32 * m.m22 + m33 * m.m32 + m34 * m.m42,
1376  m31 * m.m13 + m32 * m.m23 + m33 * m.m33 + m34 * m.m43,
1377  m31 * m.m14 + m32 * m.m24 + m33 * m.m34 + m34 * m.m44,
1378 
1379  m41 * m.m11 + m42 * m.m21 + m43 * m.m31 + m44 * m.m41,
1380  m41 * m.m12 + m42 * m.m22 + m43 * m.m32 + m44 * m.m42,
1381  m41 * m.m13 + m42 * m.m23 + m43 * m.m33 + m44 * m.m43,
1382  m41 * m.m14 + m42 * m.m24 + m43 * m.m34 + m44 * m.m44);
1383  }
1384 
1385  template <typename T>
1386  TMatrix4x4<T>& TMatrix4x4<T>::operator *= (const TMatrix4x4<T>& m)
1387  {
1388  set(m11 * m.m11 + m12 * m.m21 + m13 * m.m31 + m14 * m.m41,
1389  m11 * m.m12 + m12 * m.m22 + m13 * m.m32 + m14 * m.m42,
1390  m11 * m.m13 + m12 * m.m23 + m13 * m.m33 + m14 * m.m43,
1391  m11 * m.m14 + m12 * m.m24 + m13 * m.m34 + m14 * m.m44,
1392 
1393  m21 * m.m11 + m22 * m.m21 + m23 * m.m31 + m24 * m.m41,
1394  m21 * m.m12 + m22 * m.m22 + m23 * m.m32 + m24 * m.m42,
1395  m21 * m.m13 + m22 * m.m23 + m23 * m.m33 + m24 * m.m43,
1396  m21 * m.m14 + m22 * m.m24 + m23 * m.m34 + m24 * m.m44,
1397 
1398  m31 * m.m11 + m32 * m.m21 + m33 * m.m31 + m34 * m.m41,
1399  m31 * m.m12 + m32 * m.m22 + m33 * m.m32 + m34 * m.m42,
1400  m31 * m.m13 + m32 * m.m23 + m33 * m.m33 + m34 * m.m43,
1401  m31 * m.m14 + m32 * m.m24 + m33 * m.m34 + m34 * m.m44,
1402 
1403  m41 * m.m11 + m42 * m.m21 + m43 * m.m31 + m44 * m.m41,
1404  m41 * m.m12 + m42 * m.m22 + m43 * m.m32 + m44 * m.m42,
1405  m41 * m.m13 + m42 * m.m23 + m43 * m.m33 + m44 * m.m43,
1406  m41 * m.m14 + m42 * m.m24 + m43 * m.m34 + m44 * m.m44);
1407 
1408  return *this;
1409  }
1410 
1411  template <typename T>
1412  TVector4<T> TMatrix4x4<T>::operator * (const TVector4<T>& v) const
1413  {
1414  return TVector4<T>
1415  (m11 * v.x + m12 * v.y + m13 * v.z + m14 * v.h,
1416  m21 * v.x + m22 * v.y + m23 * v.z + m24 * v.h,
1417  m31 * v.x + m32 * v.y + m33 * v.z + m34 * v.h,
1418  m41 * v.x + m42 * v.y + m43 * v.z + m44 * v.h);
1419  }
1420 
1421  template <typename T>
1422  bool TMatrix4x4<T>::invert(TMatrix4x4<T>& inverse) const
1423  {
1431  Index i, j, k;
1432 
1433  T a[4][4] = // holds the matrix we want to invert
1434  {
1435  { m11, m12, m13, m14 },
1436  { m21, m22, m23, m24 },
1437  { m31, m32, m33, m34 },
1438  { m41, m42, m43, m44 }
1439  };
1440 
1441  // holds the maximum in the part of A we still have to work with
1442  T scale, sum_of_squares, sigma, tau;
1443  T c[4], d[4];
1444  for (k=0; k<3; k++)
1445  {
1446  scale = (T)0;
1447  // find the maximum in a
1448  for (i=k; i<4; i++)
1449  scale = Maths::max((T)fabs(a[i][k]), scale);
1450 
1451  // is the matrix singular?
1452  if (scale == (T)0)
1453  return false;
1454 
1455  // nope. we can normalize the remaining rows
1456  for (i=k; i<4; i++)
1457  a[i][k] /= scale;
1458 
1459  sum_of_squares = (T)0;
1460  for (i=k; i<4; i++)
1461  sum_of_squares += a[i][k]*a[i][k];
1462 
1463  // shift the diagonal element
1464  sigma = (a[k][k] >= 0) ? sqrt(sum_of_squares) : -sqrt(sum_of_squares);
1465  a[k][k] += sigma;
1466 
1467  c[k] = sigma*a[k][k];
1468  d[k] = -scale*sigma;
1469 
1470  for (j = k+1; j<4; j++)
1471  {
1472  // store the scalar product of a_[k] and a_[j]
1473  sum_of_squares = (T)0;
1474  for (i = k; i<4; i++)
1475  sum_of_squares += a[i][k] * a[i][j];
1476 
1477  tau = sum_of_squares / c[k];
1478 
1479  // prepare the matrix
1480  for (i=k; i<4; i++)
1481  a[i][j] -= tau*a[i][k];
1482  }
1483  }
1484  d[3] = a[3][3];
1485 
1486  // is the matrix singular?
1487  if (d[3] == (T)0)
1488  return 1;
1489 
1490  // now we have the QR decomposition. The upper triangle of A contains
1491  // R, except for the diagonal elements, which are stored in d. c contains
1492  // the values needed to compute the Householder matrices Q, and the vectors
1493  // u needed for the determination of the Qs are stored in the lower triangle
1494  // of A
1495  //
1496  // now we need to solve four linear systems of equations, one for each column
1497  // of the resulting matrix
1498  T result[4][4];
1499  result[0][0] = 1; result[0][1] = 0; result[0][2] = 0; result[0][3] = 0;
1500  result[1][0] = 0; result[1][1] = 1; result[1][2] = 0; result[1][3] = 0;
1501  result[2][0] = 0; result[2][1] = 0; result[2][2] = 1; result[2][3] = 0;
1502  result[3][0] = 0; result[3][1] = 0; result[3][2] = 0; result[3][3] = 1;
1503 
1504  for (k=0; k<4; k++) // k generates the k-th column of the inverse
1505  {
1506  // form the vector Q^t * b, which is simple, since b = e_k
1507  for (j=0; j<3; j++)
1508  {
1509  sum_of_squares = (T)0;
1510  for (i=j; i<4; i++)
1511  sum_of_squares += a[i][j]*result[i][k];
1512 
1513  tau = sum_of_squares / c[j];
1514 
1515  for (i=j; i<4; i++)
1516  result[i][k] -= tau*a[i][j];
1517  }
1518 
1519  // and solve the resulting system
1520  result[3][k] /= d[3];
1521  for (i=2; i>=0; i--)
1522  {
1523  sum_of_squares = (T)0;
1524  for (j=i+1; j<4; j++)
1525  sum_of_squares += a[i][j] * result[j][k];
1526 
1527  result[i][k] = (result[i][k] - sum_of_squares) / d[i];
1528  }
1529  }
1530 
1531  T* k_ptr = *result;
1532  inverse.m11 = *k_ptr++;
1533  inverse.m12 = *k_ptr++;
1534  inverse.m13 = *k_ptr++;
1535  inverse.m14 = *k_ptr++;
1536  inverse.m21 = *k_ptr++;
1537  inverse.m22 = *k_ptr++;
1538  inverse.m23 = *k_ptr++;
1539  inverse.m24 = *k_ptr++;
1540  inverse.m31 = *k_ptr++;
1541  inverse.m32 = *k_ptr++;
1542  inverse.m33 = *k_ptr++;
1543  inverse.m34 = *k_ptr++;
1544  inverse.m41 = *k_ptr++;
1545  inverse.m42 = *k_ptr++;
1546  inverse.m43 = *k_ptr++;
1547  inverse.m44 = *k_ptr;
1548 
1549  return true;
1550  }
1551 
1552  template <typename T>
1553  BALL_INLINE bool TMatrix4x4<T>::invert()
1554  {
1555  return invert(*this);
1556  }
1557 
1558  template <typename T>
1559  T TMatrix4x4<T>::getDeterminant() const
1560  {
1561  Position i;
1562  Position j;
1563  Position k;
1564  T submatrix[3][3];
1565  T matrix[4][4] =
1566  {
1567  { m11, m12, m13, m14 },
1568  { m21, m22, m23, m24 },
1569  { m31, m32, m33, m34 },
1570  { m41, m42, m43, m44 }
1571  };
1572  T determinant = 0;
1573 
1574  for (i = 0; i < 4; i++)
1575  {
1576  for (j = 0; j < 3; j++)
1577  {
1578  for (k = 0; k < 3; k++)
1579  {
1580  submatrix[j][k] =
1581  matrix[j + 1][(k < i) ? k : k + 1];
1582  }
1583  }
1584 
1585  determinant += matrix[0][i] * (T)(i / 2.0 == (i >> 1) ? 1 : -1)
1586  * (submatrix[0][0] * submatrix[1][1] * submatrix[2][2]
1587  + submatrix[0][1] * submatrix[1][2] * submatrix[2][0]
1588  + submatrix[0][2] * submatrix[1][0] * submatrix[2][1]
1589  - submatrix[0][2] * submatrix[1][1] * submatrix[2][0]
1590  - submatrix[0][0] * submatrix[1][2] * submatrix[2][1]
1591  - submatrix[0][1] * submatrix[1][0] * submatrix[2][2]);
1592  }
1593 
1594  return determinant;
1595  }
1596 
1597  template <typename T>
1598  void TMatrix4x4<T>::translate(const T& x, const T& y, const T& z)
1599  {
1600  m14 += m11 * x + m12 * y + m13 * z;
1601  m24 += m21 * x + m22 * y + m23 * z;
1602  m34 += m31 * x + m32 * y + m33 * z;
1603  m44 += m41 * x + m42 * y + m43 * z;
1604  }
1605 
1606  template <typename T>
1607  void TMatrix4x4<T>::translate(const TVector3<T>& v)
1608  {
1609  m14 += m11 * v.x + m12 * v.y + m13 * v.z;
1610  m24 += m21 * v.x + m22 * v.y + m23 * v.z;
1611  m34 += m31 * v.x + m32 * v.y + m33 * v.z;
1612  m44 += m41 * v.x + m42 * v.y + m43 * v.z;
1613  }
1614 
1615  template <typename T>
1616  void TMatrix4x4<T>::setTranslation(const T& x, const T& y, const T& z)
1617  {
1618  m11 = m22 = m33 = m44 = 1;
1619 
1620  m12 = m13 =
1621  m21 = m23 =
1622  m31 = m32 =
1623  m41 = m42 = m43 = 0;
1624 
1625  m14 = x;
1626  m24 = y;
1627  m34 = z;
1628  }
1629 
1630  template <typename T>
1631  void TMatrix4x4<T>::setTranslation(const TVector3<T>& v)
1632  {
1633  m11 = m22 = m33 = m44 = 1;
1634 
1635  m12 = m13 =
1636  m21 = m23 =
1637  m31 = m32 =
1638  m41 = m42 = m43 = 0;
1639 
1640  m14 = v.x;
1641  m24 = v.y;
1642  m34 = v.z;
1643  }
1644 
1645  template <typename T>
1646  void TMatrix4x4<T>::scale(const T& x_scale, const T& y_scale, const T& z_scale)
1647  {
1648  m11 *= x_scale;
1649  m21 *= x_scale;
1650  m31 *= x_scale;
1651  m41 *= x_scale;
1652 
1653  m12 *= y_scale;
1654  m22 *= y_scale;
1655  m32 *= y_scale;
1656  m42 *= y_scale;
1657 
1658  m13 *= z_scale;
1659  m23 *= z_scale;
1660  m33 *= z_scale;
1661  m43 *= z_scale;
1662  }
1663 
1664  template <typename T>
1665  void TMatrix4x4<T>::scale(const T& scale)
1666  {
1667  m11 *= scale;
1668  m21 *= scale;
1669  m31 *= scale;
1670  m41 *= scale;
1671 
1672  m12 *= scale;
1673  m22 *= scale;
1674  m32 *= scale;
1675  m42 *= scale;
1676 
1677  m13 *= scale;
1678  m23 *= scale;
1679  m33 *= scale;
1680  m43 *= scale;
1681  }
1682 
1683  template <typename T>
1684  void TMatrix4x4<T>::scale(const TVector3<T>& v)
1685  {
1686  m11 *= v.x;
1687  m21 *= v.x;
1688  m31 *= v.x;
1689  m41 *= v.x;
1690 
1691  m12 *= v.y;
1692  m22 *= v.y;
1693  m32 *= v.y;
1694  m42 *= v.y;
1695 
1696  m13 *= v.z;
1697  m23 *= v.z;
1698  m33 *= v.z;
1699  m43 *= v.z;
1700  }
1701 
1702  template <typename T>
1703  void TMatrix4x4<T>::setScale(const T& x_scale, const T& y_scale, const T& z_scale)
1704  {
1705  m11 = x_scale;
1706  m22 = y_scale;
1707  m33 = z_scale;
1708  m44 = 1;
1709 
1710  m12 = m13 = m14 =
1711  m21 = m23 = m24 =
1712  m31 = m32 = m34 =
1713  m41 = m42 = m43 = 0;
1714  }
1715 
1716  template <typename T>
1717  void TMatrix4x4<T>::setScale(const T& scale)
1718  {
1719  m11 = scale;
1720  m22 = scale;
1721  m33 = scale;
1722  m44 = 1;
1723 
1724  m12 = m13 = m14 =
1725  m21 = m23 = m24 =
1726  m31 = m32 = m34 =
1727  m41 = m42 = m43 = 0;
1728  }
1729 
1730  template <typename T>
1731  void TMatrix4x4<T>::setScale(const TVector3<T>& v)
1732  {
1733  m11 = v.x;
1734  m22 = v.y;
1735  m33 = v.z;
1736  m44 = 1;
1737 
1738  m12 = m13 = m14 =
1739  m21 = m23 = m24 =
1740  m31 = m32 = m34 =
1741  m41 = m42 = m43 = 0;
1742  }
1743 
1744  template <typename T>
1745  BALL_INLINE
1746  void TMatrix4x4<T>::rotateX(const TAngle<T>& phi)
1747  {
1748  TMatrix4x4<T> rotation;
1749 
1750  rotation.setRotationX(phi);
1751  *this *= rotation;
1752  }
1753 
1754  template <typename T>
1755  void TMatrix4x4<T>::setRotationX(const TAngle<T>& phi)
1756  {
1757  m11 = m44 = 1;
1758 
1759  m12 = m13 = m14
1760  = m21 = m24
1761  = m31 = m34
1762  = m41 = m42 = m43
1763  = 0;
1764 
1765  m22 = m33 = cos(phi);
1766  m23 = -(m32 = sin(phi));
1767  }
1768 
1769  template <typename T>
1770  BALL_INLINE
1771  void TMatrix4x4<T>::rotateY(const TAngle<T>& phi)
1772  {
1773  TMatrix4x4<T> rotation;
1774 
1775  rotation.setRotationY(phi);
1776  *this *= rotation;
1777  }
1778 
1779  template <typename T>
1780  void TMatrix4x4<T>::setRotationY(const TAngle<T>& phi)
1781  {
1782  m22 = m44 = 1;
1783 
1784  m12 = m14
1785  = m21 = m23 = m24
1786  = m32 = m34
1787  = m41 = m42 = m43
1788  = 0;
1789 
1790  m11 = m33 = cos(phi);
1791  m31 = -(m13 = sin(phi));
1792  }
1793 
1794  template <typename T>
1795  BALL_INLINE
1796  void TMatrix4x4<T>::rotateZ(const TAngle<T>& phi)
1797  {
1798  TMatrix4x4<T> rotation;
1799 
1800  rotation.setRotationZ(phi);
1801  *this *= rotation;
1802  }
1803 
1804  template <typename T>
1805  void TMatrix4x4<T>::setRotationZ(const TAngle<T>& phi)
1806  {
1807  m33 = m44 = 1;
1808 
1809  m13 = m14 = m23 = m24 = m31 =
1810  m32 = m34 = m41 = m42 = m43 = 0;
1811 
1812  m11 = m22 = cos(phi);
1813  m12 = -(m21 = sin(phi));
1814  }
1815 
1816  template <typename T>
1817  BALL_INLINE
1818  void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const TVector3<T>& v)
1819  {
1820  rotate(phi, v.x, v.y, v.z);
1821  }
1822 
1823  template <typename T>
1824  BALL_INLINE
1825  void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const TVector4<T>& v)
1826  {
1827  rotate(phi, v.x, v.y, v.z);
1828  }
1829 
1830  //
1831  // Arbitrary axis rotation matrix.
1832  //
1833  // [Taken from the MESA-Library. But modified for additional Speed-Up.]
1834  //
1835  // This function was contributed by Erich Boleyn (erich@uruk.org).
1836  //
1837  // This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied
1838  // like so: Rz * Ry * T * Ry' * Rz'. T is the final rotation
1839  // (which is about the X-axis), and the two composite transforms
1840  // Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary
1841  // from the arbitrary axis to the X-axis then back. They are
1842  // all elementary rotations.
1843  //
1844  // Rz' is a rotation about the Z-axis, to bring the axis vector
1845  // into the x-z plane. Then Ry' is applied, rotating about the
1846  // Y-axis to bring the axis vector parallel with the X-axis. The
1847  // rotation about the X-axis is then performed. Ry and Rz are
1848  // simply the respective inverse transforms to bring the arbitrary
1849  // axis back to it's original orientation. The first transforms
1850  // Rz' and Ry' are considered inverses, since the data from the
1851  // arbitrary axis gives you info on how to get to it, not how
1852  // to get away from it, and an inverse must be applied.
1853  //
1854  // The basic calculation used is to recognize that the arbitrary
1855  // axis vector (x, y, z), since it is of unit length, actually
1856  // represents the sines and cosines of the angles to rotate the
1857  // X-axis to the same orientation, with theta being the angle about
1858  // Z and phi the angle about Y (in the order described above)
1859  // as follows:
1860  //
1861  // cos ( theta ) = x / sqrt ( 1 - z^2 )
1862  // sin ( theta ) = y / sqrt ( 1 - z^2 )
1863  //
1864  // cos ( phi ) = sqrt ( 1 - z^2 )
1865  // sin ( phi ) = z
1866  //
1867  // Note that cos ( phi ) can further be inserted to the above
1868  // formulas:
1869  //
1870  // cos ( theta ) = x / cos ( phi )
1871  // sin ( theta ) = y / sin ( phi )
1872  //
1873  // ...etc. Because of those relations and the standard trigonometric
1874  // relations, it is pssible to reduce the transforms down to what
1875  // is used below. It may be that any primary axis chosen will give the
1876  // same results (modulo a sign convention) using thie method.
1877  //
1878  // Particularly nice is to notice that all divisions that might
1879  // have caused trouble when parallel to certain planes or
1880  // axis go away with care paid to reducing the expressions.
1881  // After checking, it does perform correctly under all cases, since
1882  // in all the cases of division where the denominator would have
1883  // been zero, the numerator would have been zero as well, giving
1884  // the expected result.
1885 
1886  template <typename T>
1887  void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const T& ax, const T& ay, const T& az)
1888  {
1889  T xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
1890  T x = ax;
1891  T y = ay;
1892  T z = az;
1893 
1894  double sin_angle = sin(phi);
1895  double cos_angle = cos(phi);
1896 
1897  xx = x * x;
1898  yy = y * y;
1899  zz = z * z;
1900 
1901  T mag = sqrt(xx + yy + zz);
1902 
1903  if (mag == (T)0)
1904  {
1905  m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0;
1906  m11 = m22 = m33 = m44 = (T)1;
1907  }
1908 
1909  x /= mag;
1910  y /= mag;
1911  z /= mag;
1912 
1913  // we need to recalculate xx, yy, zz due to the
1914  // normalization. recalculation is probably faster
1915  // than normalizing xx, yy, zz
1916  xx = x*x;
1917  yy = y*y;
1918  zz = z*z;
1919 
1920  xy = x * y;
1921  yz = y * z;
1922  zx = z * x;
1923  xs = (T) (x * sin_angle);
1924  ys = (T) (y * sin_angle);
1925  zs = (T) (z * sin_angle);
1926  one_c = (T) (1 - cos_angle);
1927 
1928  m11 = (T)( (one_c * xx) + cos_angle );
1929  m12 = (one_c * xy) - zs;
1930  m13 = (one_c * zx) + ys;
1931  m14 = 0;
1932 
1933  m21 = (one_c * xy) + zs;
1934  m22 = (T) ((one_c * yy) + cos_angle);
1935  m23 = (one_c * yz) - xs;
1936  m24 = 0;
1937 
1938  m31 = (one_c * zx) - ys;
1939  m32 = (one_c * yz) + xs;
1940  m33 = (T) ((one_c * zz) + cos_angle);
1941  m34 = 0;
1942 
1943  m41 = 0;
1944  m42 = 0;
1945  m43 = 0;
1946  m44 = 1;
1947  }
1948 
1949  template <typename T>
1950  void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const T& x, const T& y, const T& z)
1951  {
1952  m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0;
1953  m11 = m22 = m33 = m44 = (T)1;
1954  rotate(phi, x, y, z);
1955  }
1956 
1957  template <typename T>
1958  BALL_INLINE
1959  void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const TVector3<T>& v)
1960  {
1961  m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0;
1962  m11 = m22 = m33 = m44 = (T)1;
1963  rotate(phi, v.x, v.y, v.z);
1964  }
1965 
1966  template <typename T>
1967  BALL_INLINE
1968  void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const TVector4<T>& v)
1969  {
1970  m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0;
1971  m11 = m22 = m33 = m44 = (T)1;
1972  rotate(phi, v.x, v.y, v.z);
1973  }
1974 
1975  template <typename T>
1976  bool TMatrix4x4<T>::operator == (const TMatrix4x4<T>& m) const
1977  {
1978  return
1979  ( m11 == m.m11
1980  && m12 == m.m12
1981  && m13 == m.m13
1982  && m14 == m.m14
1983  && m21 == m.m21
1984  && m22 == m.m22
1985  && m23 == m.m23
1986  && m24 == m.m24
1987  && m31 == m.m31
1988  && m32 == m.m32
1989  && m33 == m.m33
1990  && m34 == m.m34
1991  && m41 == m.m41
1992  && m42 == m.m42
1993  && m43 == m.m43
1994  && m44 == m.m44);
1995  }
1996 
1997  template <typename T>
1998  bool TMatrix4x4<T>::operator != (const TMatrix4x4<T>& m) const
1999  {
2000  return
2001  ( m11 != m.m11
2002  || m12 != m.m12
2003  || m13 != m.m13
2004  || m14 != m.m14
2005  || m21 != m.m21
2006  || m22 != m.m22
2007  || m23 != m.m23
2008  || m24 != m.m24
2009  || m31 != m.m31
2010  || m32 != m.m32
2011  || m33 != m.m33
2012  || m34 != m.m34
2013  || m41 != m.m41
2014  || m42 != m.m42
2015  || m43 != m.m43
2016  || m44 != m.m44);
2017  }
2018 
2019  template <typename T>
2020  bool TMatrix4x4<T>::isIdentity() const
2021  {
2022  return
2023  ( m11 == (T)1
2024  && m12 == (T)0
2025  && m13 == (T)0
2026  && m14 == (T)0
2027  && m21 == (T)0
2028  && m22 == (T)1
2029  && m23 == (T)0
2030  && m24 == (T)0
2031  && m31 == (T)0
2032  && m32 == (T)0
2033  && m33 == (T)1
2034  && m34 == (T)0
2035  && m41 == (T)0
2036  && m42 == (T)0
2037  && m43 == (T)0
2038  && m44 == (T)1);
2039  }
2040 
2041  template <typename T>
2042  BALL_INLINE
2043  bool TMatrix4x4<T>::isRegular() const
2044  {
2045  return (getDeterminant() != (T)0);
2046  }
2047 
2048  template <typename T>
2049  BALL_INLINE
2050  bool TMatrix4x4<T>::isSingular() const
2051  {
2052  return (getDeterminant() == (T)0);
2053  }
2054 
2055  template <typename T>
2056  bool TMatrix4x4<T>::isSymmetric() const
2057  {
2058  return ( m12 == m21 && m13 == m31
2059  && m14 == m41 && m23 == m32
2060  && m24 == m42 && m34 == m43);
2061  }
2062 
2063  template <typename T>
2064  bool TMatrix4x4<T>::isLowerTriangular() const
2065  {
2066  return ( m12 == (T)0
2067  && m13 == (T)0
2068  && m14 == (T)0
2069  && m23 == (T)0
2070  && m24 == (T)0
2071  && m34 == (T)0);
2072  }
2073 
2074  template <typename T>
2075  bool TMatrix4x4<T>::isUpperTriangular() const
2076  {
2077  return ( m21 == (T)0
2078  && m31 == (T)0
2079  && m32 == (T)0
2080  && m41 == (T)0
2081  && m42 == (T)0
2082  && m43 == (T)0);
2083  }
2084 
2085  template <typename T>
2086  BALL_INLINE
2087  bool TMatrix4x4<T>::isDiagonal() const
2088  {
2089  return ( m12 == (T)0
2090  && m13 == (T)0
2091  && m14 == (T)0
2092  && m21 == (T)0
2093  && m23 == (T)0
2094  && m24 == (T)0
2095  && m31 == (T)0
2096  && m32 == (T)0
2097  && m34 == (T)0
2098  && m41 == (T)0
2099  && m42 == (T)0
2100  && m43 == (T)0);
2101  }
2102 
2103  template <typename T>
2104  bool TMatrix4x4<T>::isValid() const
2105  {
2106  T **ptr = (T **)comp_ptr_;
2107 
2108  return ( *ptr++ == &m11
2109  && *ptr++ == &m12
2110  && *ptr++ == &m13
2111  && *ptr++ == &m14
2112  && *ptr++ == &m21
2113  && *ptr++ == &m22
2114  && *ptr++ == &m23
2115  && *ptr++ == &m24
2116  && *ptr++ == &m31
2117  && *ptr++ == &m32
2118  && *ptr++ == &m33
2119  && *ptr++ == &m34
2120  && *ptr++ == &m41
2121  && *ptr++ == &m42
2122  && *ptr++ == &m43
2123  && *ptr == &m44);
2124  }
2125 
2126  template <typename T>
2127  std::istream& operator >> (std::istream& s, TMatrix4x4<T>& m)
2128  {
2129  char c;
2130  s >> c
2131  >> m.m11 >> m.m12 >> m.m13 >> m.m14 >> c >> c
2132  >> m.m21 >> m.m22 >> m.m23 >> m.m24 >> c >> c
2133  >> m.m31 >> m.m32 >> m.m33 >> m.m34 >> c >> c
2134  >> m.m41 >> m.m42 >> m.m43 >> m.m44 >> c;
2135 
2136  return s;
2137  }
2138 
2139  template <typename T>
2140  std::ostream& operator << (std::ostream& s, const TMatrix4x4<T>& m)
2141  {
2142  s << '/' << std::setw(14) << m.m11 << ' ' << std::setw(14) << m.m12 << ' ' << std::setw(14) << m.m13 << ' ' << std::setw(14) << m.m14 << " \\" << std::endl
2143  << '|' << std::setw(14) << m.m21 << ' ' << std::setw(14) << m.m22 << ' ' << std::setw(14) << m.m23 << ' ' << std::setw(14) << m.m24 << " |" << std::endl
2144  << '|' << std::setw(14) << m.m31 << ' ' << std::setw(14) << m.m32 << ' ' << std::setw(14) << m.m33 << ' ' << std::setw(14) << m.m34 << " |" << std::endl
2145  << '\\' << std::setw(14) << m.m41 << ' ' << std::setw(14) << m.m42 << ' ' << std::setw(14) << m.m43 << ' ' << std::setw(14) << m.m44 << " /" << std::endl;
2146 
2147  return s;
2148  }
2149 
2150  template <typename T>
2151  void TMatrix4x4<T>::dump(std::ostream& s, Size depth) const
2152  {
2153  BALL_DUMP_STREAM_PREFIX(s);
2154 
2155  BALL_DUMP_HEADER(s, this, this);
2156 
2157  BALL_DUMP_DEPTH(s, depth);
2158  s << m11 << " " << m12 << " " << m13 << " " << m14 << std::endl;
2159 
2160  BALL_DUMP_DEPTH(s, depth);
2161  s << m21 << " " << m22 << " " << m23 << " " << m24 << std::endl;
2162 
2163  BALL_DUMP_DEPTH(s, depth);
2164  s << m31 << " " << m32 << " " << m33 << " " << m34 << std::endl;
2165 
2166  BALL_DUMP_DEPTH(s, depth);
2167  s << m41 << " " << m42 << " " << m43 << " " << m44 << std::endl;
2168 
2169  BALL_DUMP_STREAM_SUFFIX(s);
2170  }
2171 
2173  template <typename T>
2174  TMatrix4x4<T> operator * (const T& scalar, const TMatrix4x4<T>& m);
2175 
2177  template <typename T>
2178  TVector3<T> operator * (const TMatrix4x4<T>& matrix, const TVector3<T>& vector);
2179 
2184  typedef TMatrix4x4<float> Matrix4x4;
2185 
2186 } // namespace BALL
2187 
2188 #endif // BALL_MATHS_MATRIX44_H
Generated by   doxygen 1.8.3.1