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 RotationMatrixPanoCommand::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 =  m[0][0] * ( m[1][1] * m[2][2] - m[2][1] * m[1][2] );
00137                 result -= m[1][0] * ( m[0][1] * m[2][2] - m[2][1] * m[0][2] );
00138                 result += m[2][0] * ( m[0][1] * m[1][2] - m[1][1] * m[0][2] );
00139                 return result;
00140         }
00141         
00143         inline Vector3 TransformVector(const Vector3 &V) const
00144         {
00145                 Vector3 Result;
00146                 Result.x = V.x * m[0][0] + V.y * m[1][0] + V.z * m[2][0];
00147                 Result.y = V.x * m[0][1] + V.y * m[1][1] + V.z * m[2][1];
00148                 Result.z = V.x * m[0][2] + V.y * m[1][2] + V.z * m[2][2];
00149                 return Result;
00150         }
00151 
00153         Matrix3 Inverse() const;
00154     
00156     void Print(std::ostream & o) const;
00157 
00158 };
00159 
00161 inline Matrix3 GetRotationAroundU(const Vector3& U, double Angle)
00162 {
00163         // is debugged and optimized
00164         Vector3 u = U.GetNormalized();
00165     if (u.Norm()<0.01) {
00166         Matrix3 r;
00167         r.SetIdentity();
00168         return r;
00169     }
00170         double cs, ss, ux2, uy2, uz2, uxy, uxz, uyz;
00171         
00172         cs = cos(Angle);
00173         ss = sin(Angle);
00174         ux2 = u.x*u.x;
00175         uy2 = u.y*u.y;
00176         uz2 = u.z*u.z;
00177         uxy = u.x*u.y;
00178         uxz = u.x*u.z;
00179         uyz = u.y*u.z;
00180     Matrix3 m;
00181     m.m[0][0] = ux2 + cs*(1-ux2);
00182     m.m[1][0] = uxy*(1-cs) - u.z*ss;
00183     m.m[2][0] = uxz*(1-cs) + u.y*ss;
00184     m.m[0][1] = uxy*(1-cs) + u.z*ss;
00185     m.m[1][1] = uy2 + cs*(1-uy2);
00186     m.m[2][1] = uyz*(1-cs)-u.x*ss;
00187     m.m[0][2] = uxz*(1-cs)-u.y*ss;
00188     m.m[1][2] = uyz*(1-cs)+u.x*ss;
00189     m.m[2][2] = uz2 + cs*(1-uz2);
00190     return m;
00191     /*
00192         return Matrix3( ux2 + cs*(1-ux2),
00193                                  uxy*(1-cs) - u.z*ss,
00194                                  uxz*(1-cs) + u.y*ss,
00195                                  uxy*(1-cs) + u.z*ss,
00196                                  uy2 + cs*(1-uy2),
00197                                  uyz*(1-cs)-u.x*ss,
00198                                  uxz*(1-cs)-u.y*ss,
00199                                  uyz*(1-cs)+u.x*ss,
00200                                  uz2 + cs*(1-uz2) );
00201     */
00202 }
00203 
00204 inline Matrix3 GetRotationAroundU(const Vector3& da)
00205 {
00206         return GetRotationAroundU(da, da.Norm());
00207 }
00208 
00209 /*// return the rotation matrix around X
00210 Matrix3 GetRotationX(double Ang)
00211 {
00212         double a = cos(Ang);
00213         double b = sin(Ang);
00214         return Matrix3(
00215                         1.0,    0.0,            0.0,
00216                         0.0,    a,                      b,
00217                         0.0,    -b,                     a );
00218 }
00219 
00220 //return the rotation matrix around Y
00221 Matrix3 GetRotationY(double Ang)
00222 {
00223         double a = cos(Ang);
00224         double b = -sin(Ang);
00225         return Matrix3(
00226                         a,              0.0,    b,
00227                         0.0,    1.0,    0.0,
00228                         -b,             0.0,    a );
00229 }
00230 
00231 //return the rotation matrix around Z
00232 Matrix3 GetRotationZ(double Ang)
00233 {
00234         double a = cos(Ang);
00235         double b = sin(Ang);
00236         return Matrix3(
00237                         a,              b,              0.0,
00238                         -b,             a,              0.0,
00239                         0.0,    0.0,    1.0);
00240 }
00241 */
00242 
00243 inline std::ostream & operator<<(std::ostream & s, const Matrix3 & m)
00244 {
00245     m.Print(s);
00246     return s;
00247 }
00248 
00249 #endif // _H

Generated on 28 Jul 2015 for Hugintrunk by  doxygen 1.4.7