Matrix3.h

Go to the documentation of this file.

// -*- c-basic-offset: 4 -*-
#ifndef _HUGIN_MATH_MATRIX3_H_
#define _HUGIN_MATH_MATRIX3_H_

#include <hugin_shared.h>
#include <math.h>
#include <hugin_math/Vector3.h>


class IMPEX Matrix3
{
public:
        double m[3][3];

        static Matrix3 Identity;
        
public:
    Matrix3();
        
        Matrix3(const Matrix3& ot);

        void SetIdentity();

        void SetRotation( double Yaw, double Pitch, double Roll );

#if 0

    void SetRotationPT( double Yaw, double Pitch, double Roll );
#endif 

    
    // [Ippei note]: Why the hell is this a method of general Matrix3 class?
    //  Should be subclassed or externally provided
    //  eg. static Matrix3 RotationMatrixPanoCommand::makeRotationMatrixPT(double yaw, double pitch, double roll)

    void SetRotationPT( double yaw, double pitch, double roll );

    void GetRotationPT( double & Yaw, double & Pitch, double & Roll );

    
        void SetRotationX( double a );

        void SetRotationY( double a );
        
        void SetRotationZ( double a );

        Matrix3& operator= (const Matrix3& ot);
        
        Matrix3 operator*(const Matrix3& ot) const;

    void operator/=(double s);

    void operator*=(double s);

        void operator*=(Matrix3 ot);

        inline bool operator==(Matrix3& ot) const
        {
                for(int i=0; i<3; i++)
                        for(int j=0; j<3; j++)
                                if(m[i][j] != ot.m[i][j])
                                        return false;
                return true;
        }

        inline bool operator!=(Matrix3& ot) const
        {
                return !(*this == ot);
        }

        inline Matrix3 Transpose()
        {
                Matrix3 Result;
                Result.m[0][0] = m[0][0];
                Result.m[0][1] = m[1][0];
                Result.m[0][2] = m[2][0];
                Result.m[1][0] = m[0][1];
                Result.m[1][1] = m[1][1];
                Result.m[1][2] = m[2][1];
                Result.m[2][0] = m[0][2];
                Result.m[2][1] = m[1][2];
                Result.m[2][2] = m[2][2];
                return Result;
        }

        inline double Determinant() const
        {
                double result =  m[0][0] * ( m[1][1] * m[2][2] - m[2][1] * m[1][2] );
                result -= m[1][0] * ( m[0][1] * m[2][2] - m[2][1] * m[0][2] );
                result += m[2][0] * ( m[0][1] * m[1][2] - m[1][1] * m[0][2] );
                return result;
        }
        
        inline Vector3 TransformVector(const Vector3 &V) const
        {
                Vector3 Result;
                Result.x = V.x * m[0][0] + V.y * m[1][0] + V.z * m[2][0];
                Result.y = V.x * m[0][1] + V.y * m[1][1] + V.z * m[2][1];
                Result.z = V.x * m[0][2] + V.y * m[1][2] + V.z * m[2][2];
                return Result;
        }

        Matrix3 Inverse() const;
    
    void Print(std::ostream & o) const;

};

inline Matrix3 GetRotationAroundU(const Vector3& U, double Angle)
{
        // is debugged and optimized
        Vector3 u = U.GetNormalized();
    if (u.Norm()<0.01) {
        Matrix3 r;
        r.SetIdentity();
        return r;
    }
        double cs, ss, ux2, uy2, uz2, uxy, uxz, uyz;
        
        cs = cos(Angle);
        ss = sin(Angle);
        ux2 = u.x*u.x;
        uy2 = u.y*u.y;
        uz2 = u.z*u.z;
        uxy = u.x*u.y;
        uxz = u.x*u.z;
        uyz = u.y*u.z;
    Matrix3 m;
    m.m[0][0] = ux2 + cs*(1-ux2);
    m.m[1][0] = uxy*(1-cs) - u.z*ss;
    m.m[2][0] = uxz*(1-cs) + u.y*ss;
    m.m[0][1] = uxy*(1-cs) + u.z*ss;
    m.m[1][1] = uy2 + cs*(1-uy2);
    m.m[2][1] = uyz*(1-cs)-u.x*ss;
    m.m[0][2] = uxz*(1-cs)-u.y*ss;
    m.m[1][2] = uyz*(1-cs)+u.x*ss;
    m.m[2][2] = uz2 + cs*(1-uz2);
    return m;
    /*
        return Matrix3( ux2 + cs*(1-ux2),
                                 uxy*(1-cs) - u.z*ss,
                                 uxz*(1-cs) + u.y*ss,
                                 uxy*(1-cs) + u.z*ss,
                                 uy2 + cs*(1-uy2),
                                 uyz*(1-cs)-u.x*ss,
                                 uxz*(1-cs)-u.y*ss,
                                 uyz*(1-cs)+u.x*ss,
                                 uz2 + cs*(1-uz2) );
    */
}

inline Matrix3 GetRotationAroundU(const Vector3& da)
{
        return GetRotationAroundU(da, da.Norm());
}

/*// return the rotation matrix around X
Matrix3 GetRotationX(double Ang)
{
        double a = cos(Ang);
        double b = sin(Ang);
        return Matrix3(
                        1.0,    0.0,            0.0,
                        0.0,    a,                      b,
                        0.0,    -b,                     a );
}

//return the rotation matrix around Y
Matrix3 GetRotationY(double Ang)
{
        double a = cos(Ang);
        double b = -sin(Ang);
        return Matrix3(
                        a,              0.0,    b,
                        0.0,    1.0,    0.0,
                        -b,             0.0,    a );
}

//return the rotation matrix around Z
Matrix3 GetRotationZ(double Ang)
{
        double a = cos(Ang);
        double b = sin(Ang);
        return Matrix3(
                        a,              b,              0.0,
                        -b,             a,              0.0,
                        0.0,    0.0,    1.0);
}
*/

inline std::ostream & operator<<(std::ostream & s, const Matrix3 & m)
{
    m.Print(s);
    return s;
}

#endif // _H