Matrix3.h

Go to the documentation of this file.
00001 // -*- c-basic-offset: 4 -*-
00024 #ifndef _HUGIN_MATH_MATRIX3_H_
00025 #define _HUGIN_MATH_MATRIX3_H_
00026 
00027 #include <hugin_shared.h>
00028 #include <math.h>
00029 #include <hugin_math/Vector3.h>
00030 
00031 
00037 class IMPEX Matrix3
00038 {
00039 public:
00041         double m[3][3];
00042 
00043         static Matrix3 Identity;
00044         
00045 public:
00047     Matrix3();
00048         
00050         Matrix3(const Matrix3& ot);
00051 
00053         void SetIdentity();
00054 
00056         void SetRotation( double Yaw, double Pitch, double Roll );
00057 
00058 #if 0
00059 
00062     void SetRotationPT( double Yaw, double Pitch, double Roll );
00063 #endif 
00064 
00065     
00066     // [Ippei note]: Why the hell is this a method of general Matrix3 class?
00067     //  Should be subclassed or externally provided
00068     //  eg. static Matrix3 RotationMatrixPT::makeRotationMatrixPT(double yaw, double pitch, double roll)
00069 
00073     void SetRotationPT( double yaw, double pitch, double roll );
00074 
00076     void GetRotationPT( double & Yaw, double & Pitch, double & Roll );
00077 
00078     
00080         void SetRotationX( double a );
00081 
00082         void SetRotationY( double a );
00083         
00084         void SetRotationZ( double a );
00085 
00087         Matrix3& operator= (const Matrix3& ot);
00088         
00090         Matrix3 operator*(const Matrix3& ot) const;
00091 
00093     void operator/=(double s);
00094 
00096     void operator*=(double s);
00097 
00099         void operator*=(Matrix3 ot);
00100 
00102         inline bool operator==(Matrix3& ot) const
00103         {
00104                 for(int i=0; i<3; i++)
00105                         for(int j=0; j<3; j++)
00106                                 if(m[i][j] != ot.m[i][j])
00107                                         return false;
00108                 return true;
00109         }
00110 
00112         inline bool operator!=(Matrix3& ot) const
00113         {
00114                 return !(*this == ot);
00115         }
00116 
00118         inline Matrix3 Transpose()
00119         {
00120                 Matrix3 Result;
00121                 Result.m[0][0] = m[0][0];
00122                 Result.m[0][1] = m[1][0];
00123                 Result.m[0][2] = m[2][0];
00124                 Result.m[1][0] = m[0][1];
00125                 Result.m[1][1] = m[1][1];
00126                 Result.m[1][2] = m[2][1];
00127                 Result.m[2][0] = m[0][2];
00128                 Result.m[2][1] = m[1][2];
00129                 Result.m[2][2] = m[2][2];
00130                 return Result;
00131         }
00132 
00134         inline double Determinant() const
00135         {
00136                 double result = 0.0;
00137                 result  = m[0][0] * ( m[1][1] * m[2][2] - m[2][1] * m[1][2] );
00138                 result -= m[1][0] * ( m[0][1] * m[2][2] - m[2][1] * m[0][2] );
00139                 result += m[2][0] * ( m[0][1] * m[1][2] - m[1][1] * m[0][2] );
00140                 return result;
00141         }
00142         
00144         inline Vector3 TransformVector(const Vector3 &V) const
00145         {
00146                 Vector3 Result;
00147                 Result.x = V.x * m[0][0] + V.y * m[1][0] + V.z * m[2][0];
00148                 Result.y = V.x * m[0][1] + V.y * m[1][1] + V.z * m[2][1];
00149                 Result.z = V.x * m[0][2] + V.y * m[1][2] + V.z * m[2][2];
00150                 return Result;
00151         }
00152 
00154         Matrix3 Inverse() const;
00155     
00157     void Print(std::ostream & o) const;
00158 
00159 };
00160 
00162 inline Matrix3 GetRotationAroundU(const Vector3& U, double Angle)
00163 {
00164         // is debugged and optimized
00165         Vector3 u = U.GetNormalized();
00166     if (u.Norm()<0.01) {
00167         Matrix3 r;
00168         r.SetIdentity();
00169         return r;
00170     }
00171         double cs, ss, ux2, uy2, uz2, uxy, uxz, uyz;
00172         
00173         cs = cos(Angle);
00174         ss = sin(Angle);
00175         ux2 = u.x*u.x;
00176         uy2 = u.y*u.y;
00177         uz2 = u.z*u.z;
00178         uxy = u.x*u.y;
00179         uxz = u.x*u.z;
00180         uyz = u.y*u.z;
00181     Matrix3 m;
00182     m.m[0][0] = ux2 + cs*(1-ux2);
00183     m.m[1][0] = uxy*(1-cs) - u.z*ss;
00184     m.m[2][0] = uxz*(1-cs) + u.y*ss;
00185     m.m[0][1] = uxy*(1-cs) + u.z*ss;
00186     m.m[1][1] = uy2 + cs*(1-uy2);
00187     m.m[2][1] = uyz*(1-cs)-u.x*ss;
00188     m.m[0][2] = uxz*(1-cs)-u.y*ss;
00189     m.m[1][2] = uyz*(1-cs)+u.x*ss;
00190     m.m[2][2] = uz2 + cs*(1-uz2);
00191     return m;
00192     /*
00193         return Matrix3( ux2 + cs*(1-ux2),
00194                                  uxy*(1-cs) - u.z*ss,
00195                                  uxz*(1-cs) + u.y*ss,
00196                                  uxy*(1-cs) + u.z*ss,
00197                                  uy2 + cs*(1-uy2),
00198                                  uyz*(1-cs)-u.x*ss,
00199                                  uxz*(1-cs)-u.y*ss,
00200                                  uyz*(1-cs)+u.x*ss,
00201                                  uz2 + cs*(1-uz2) );
00202     */
00203 }
00204 
00205 inline Matrix3 GetRotationAroundU(const Vector3& da)
00206 {
00207         return GetRotationAroundU(da, da.Norm());
00208 }
00209 
00210 /*// return the rotation matrix around X
00211 Matrix3 GetRotationX(double Ang)
00212 {
00213         double a = cos(Ang);
00214         double b = sin(Ang);
00215         return Matrix3(
00216                         1.0,    0.0,            0.0,
00217                         0.0,    a,                      b,
00218                         0.0,    -b,                     a );
00219 }
00220 
00221 //return the rotation matrix around Y
00222 Matrix3 GetRotationY(double Ang)
00223 {
00224         double a = cos(Ang);
00225         double b = -sin(Ang);
00226         return Matrix3(
00227                         a,              0.0,    b,
00228                         0.0,    1.0,    0.0,
00229                         -b,             0.0,    a );
00230 }
00231 
00232 //return the rotation matrix around Z
00233 Matrix3 GetRotationZ(double Ang)
00234 {
00235         double a = cos(Ang);
00236         double b = sin(Ang);
00237         return Matrix3(
00238                         a,              b,              0.0,
00239                         -b,             a,              0.0,
00240                         0.0,    0.0,    1.0);
00241 }
00242 */
00243 
00244 inline std::ostream & operator<<(std::ostream & s, const Matrix3 & m)
00245 {
00246     m.Print(s);
00247     return s;
00248 }
00249 
00250 #endif // _H

Generated on 31 Oct 2014 for Hugintrunk by  doxygen 1.4.7