Matrix3.cpp

Go to the documentation of this file.

// -*- c-basic-offset: 4 -*-
#include "Matrix3.h"

inline Matrix3 getIdentity()
{
    Matrix3 tmp;
    tmp.SetIdentity();
    return tmp;
}
Matrix3 Matrix3::Identity = getIdentity();


Matrix3::Matrix3() {
    for (int i=0; i<3; i++)
        for (int j=0; j<3; j++)
            m[i][j] = 0.0;
}

Matrix3::Matrix3(const Matrix3& ot)
{
    (*this) = ot; // call copy operator
}

void Matrix3::SetIdentity()
{
    m[0][0] = 1; m[0][1] = 0;  m[0][2] = 0;
    m[1][0] = 0; m[1][1] = 1;  m[1][2] = 0;
    m[2][0] = 0; m[2][1] = 0;  m[2][2] = 1;
}

void Matrix3::SetRotation( double Yaw, double Pitch, double Roll )
{
    double      SR      = sin(Roll),
            SP  = sin(Pitch),
            SY  = sin(Yaw),
            CR  = cos(Roll),
            CP  = cos(Pitch),
            CY  = cos(Yaw);

    m[0][0]     = CP * CY;
    m[0][1]     = CP * SY;
    m[0][2]     = SP;
    
    m[1][0]     = SR * SP * CY - CR * SY;
    m[1][1]     = SR * SP * SY + CR * CY;
    m[1][2]     = - SR * CP;
    
    m[2][0]     = -( CR * SP * CY + SR * SY );
    m[2][1]     = CY * SR - CR * SP * SY;
    m[2][2]     = CR * CP;
}

#if 0

Matrix3::void SetRotationPT( double Yaw, double Pitch, double Roll )
{
    double  SR  = sin(Roll),
    SP  = sin(Pitch),
    SY  = sin(Yaw),
    CR  = cos(Roll),
    CP  = cos(Pitch),
    CY  = cos(Yaw);

    m[0][0] = CP * CY;
    m[0][1] = CP * SY;
    m[0][2] = -SP;

    m[1][0] = SR * SP * CY - CR * SY;
    m[1][1] = SR * SP * SY + CR * CY;
    m[1][2] = SR * CP;

    m[2][0] = CR * SP * CY + SR * SY;
    m[2][1] = CR * SP * SY - CY * SR;
    m[2][2] = CR * CP;
}
#endif 


void Matrix3::SetRotationPT( double yaw, double pitch, double roll )
{
    double cosr = cos (roll);
    double sinr = sin (roll);
    double cosp = cos (pitch);
    double sinp = sin (0 - pitch);
    double cosy = cos (yaw);
    double siny = sin (0 - yaw);

    Matrix3 rollm;

    /*
    rollm[0][0] = new Math::Matrix ([        1,       0,       0 ],
            [        0,   cosr,-1*sinr ],
            [        0,   sinr,   cosr ]);
    */

    rollm.m[0][0] = 1.0;      rollm.m[0][1] = 0.0;      rollm.m[0][2] = 0.0;
    rollm.m[1][0] = 0.0;      rollm.m[1][1] = cosr;     rollm.m[1][2] = -sinr;
    rollm.m[2][0] = 0.0;      rollm.m[2][1] = sinr;     rollm.m[2][2] = cosr;

    /*
my pitchm = new Math::Matrix ([    cosp,       0,   sinp ],
                                    [        0,       1,       0 ],
                                    [ -1*sinp,       0,   cosp ]);
    */
    
    Matrix3 pitchm;
    pitchm.m[0][0] = cosp;   pitchm.m[0][1] =  0.0;  pitchm.m[0][2] = sinp;
    pitchm.m[1][0] =  0.0;   pitchm.m[1][1] =    1;  pitchm.m[1][2] = 0.0;
    pitchm.m[2][0] = -sinp;  pitchm.m[2][1] =  0.0;  pitchm.m[2][2] = cosp;

    /*
my yawm   = new Math::Matrix ([    cosy,-1*siny,       0 ],
                                    [    siny,   cosy,       0 ],
                                    [        0,       0,       1 ]);
    */
    Matrix3 yawm;
    yawm.m[0][0] = cosy;   yawm.m[0][1] = -siny;   yawm.m[0][2] = 0.0;
    yawm.m[1][0] = siny;   yawm.m[1][1] =  cosy;   yawm.m[1][2] = 0.0;
    yawm.m[2][0] = 0.0;    yawm.m[2][1] =   0.0;   yawm.m[2][2] = 1.0;


    *this = yawm * pitchm * rollm;
}

void Matrix3::GetRotationPT( double & Yaw, double & Pitch, double & Roll )
{
    /*
    my $matrix = shift;
    my $roll = atan2 ($matrix->[2]->[1], $matrix->[2]->[2]);
    my $pitch = -1 * asin (-1 * $matrix->[2]->[0]);
    my $yaw = atan2 (-1 * $matrix->[1]->[0], $matrix->[0]->[0]);
    return ($roll, $pitch, $yaw);

    */
    Roll = atan2 (m[2][1], m[2][2]);
    Pitch = - asin (- m[2][0]);
    Yaw = atan2 (- m[1][0], m[0][0]);
}

void Matrix3::SetRotationX( double a )
{
    m[0][0] = 1.0;              m[0][1] = 0.0;          m[0][2] = 0.0;
    m[1][0] = 0.0;              m[1][1] = cos(a);       m[1][2] = sin(a);
    m[2][0] = 0.0;              m[2][1] =-m[1][2];      m[2][2] = m[1][1];
}

void Matrix3::SetRotationY( double a )
{
    m[0][0] = cos(a);   m[0][1] = 0.0;          m[0][2] =-sin(a);
    m[1][0] = 0.0;              m[1][1] = 1.0;          m[1][2] = 0.0;
    m[2][0] =-m[0][2];  m[2][1] = 0.0;          m[2][2] = m[0][0];
}

void Matrix3::SetRotationZ( double a )
{
    m[0][0] = cos(a);   m[0][1] = sin(a);       m[0][2] = 0.0;
    m[1][0] =-m[0][1];  m[1][1] = m[0][0];      m[1][2] = 0.0;
    m[2][0] = 0.0;              m[2][1] = 0.0;          m[2][2] = 1.0;
}

Matrix3& Matrix3::operator= (const Matrix3& ot)
{
    for (int i=0; i<3; i++)
        for (int j=0; j<3; j++)
            m[i][j] = ot.m[i][j];
    return *this;
}

Matrix3 Matrix3::operator*(const Matrix3& ot) const
{
    Matrix3     Result;
    Result.m[0][0] = m[0][0] * ot.m[0][0] + m[0][1] * ot.m[1][0] + m[0][2] * ot.m[2][0];
    Result.m[0][1] = m[0][0] * ot.m[0][1] + m[0][1] * ot.m[1][1] + m[0][2] * ot.m[2][1];
    Result.m[0][2] = m[0][0] * ot.m[0][2] + m[0][1] * ot.m[1][2] + m[0][2] * ot.m[2][2];
    
    Result.m[1][0] = m[1][0] * ot.m[0][0] + m[1][1] * ot.m[1][0] + m[1][2] * ot.m[2][0];
    Result.m[1][1] = m[1][0] * ot.m[0][1] + m[1][1] * ot.m[1][1] + m[1][2] * ot.m[2][1];
    Result.m[1][2] = m[1][0] * ot.m[0][2] + m[1][1] * ot.m[1][2] + m[1][2] * ot.m[2][2];
    
    Result.m[2][0] = m[2][0] * ot.m[0][0] + m[2][1] * ot.m[1][0] + m[2][2] * ot.m[2][0];
    Result.m[2][1] = m[2][0] * ot.m[0][1] + m[2][1] * ot.m[1][1] + m[2][2] * ot.m[2][1];
    Result.m[2][2] = m[2][0] * ot.m[0][2] + m[2][1] * ot.m[1][2] + m[2][2] * ot.m[2][2];
    return Result;
}

void Matrix3::operator/=(double s)
{
    for (int i=0; i<3; i++)
        for (int j=0; j<3; j++)
            m[i][j] /= s;
}

void Matrix3::operator*=(double s)
{
    for (int i=0; i<3; i++)
        for (int j=0; j<3; j++)
            m[i][j] *= s;
}

void Matrix3::operator*=(Matrix3 ot)
{
    Matrix3 Result;
    Result.m[0][0] = m[0][0] * ot.m[0][0] + m[0][1] * ot.m[1][0] + m[0][2] * ot.m[2][0];
    Result.m[0][1] = m[0][0] * ot.m[0][1] + m[0][1] * ot.m[1][1] + m[0][2] * ot.m[2][1];
    Result.m[0][2] = m[0][0] * ot.m[0][2] + m[0][1] * ot.m[1][2] + m[0][2] * ot.m[2][2];
    
    Result.m[1][0] = m[1][0] * ot.m[0][0] + m[1][1] * ot.m[1][0] + m[1][2] * ot.m[2][0];
    Result.m[1][1] = m[1][0] * ot.m[0][1] + m[1][1] * ot.m[1][1] + m[1][2] * ot.m[2][1];
    Result.m[1][2] = m[1][0] * ot.m[0][2] + m[1][1] * ot.m[1][2] + m[1][2] * ot.m[2][2];
    
    Result.m[2][0] = m[2][0] * ot.m[0][0] + m[2][1] * ot.m[1][0] + m[2][2] * ot.m[2][0];
    Result.m[2][1] = m[2][0] * ot.m[0][1] + m[2][1] * ot.m[1][1] + m[2][2] * ot.m[2][1];
    Result.m[2][2] = m[2][0] * ot.m[0][2] + m[2][1] * ot.m[1][2] + m[2][2] * ot.m[2][2];
    *this = Result;
}

Matrix3 Matrix3::Inverse() const
{
    Matrix3 Result;
    double Det = Determinant();

    if (Det == 0.0f)
        return Matrix3::Identity;

    double      invDet = 1.0f / Det;

    Result.m[0][0] =  invDet * ( m[1][1] * m[2][2] - m[2][1] * m[1][2] );
    Result.m[0][1] = -invDet * ( m[0][1] * m[2][2] - m[2][1] * m[0][2] );
    Result.m[0][2] =  invDet * ( m[0][1] * m[1][2] - m[1][1] * m[0][2] );
    
    Result.m[1][0] = -invDet * ( m[1][0] * m[2][2] - m[2][0] * m[1][2] );
    Result.m[1][1] =  invDet * ( m[0][0] * m[2][2] - m[2][0] * m[0][2] );
    Result.m[1][2] = -invDet * ( m[0][0] * m[1][2] - m[1][0] * m[0][2] );
    
    Result.m[2][0] =  invDet * ( m[1][0] * m[2][1] - m[2][0] * m[1][1] );
    Result.m[2][1] = -invDet * ( m[0][0] * m[2][1] - m[2][0] * m[0][1] );
    Result.m[2][2] =  invDet * ( m[0][0] * m[1][1] - m[1][0] * m[0][1] );
    return Result;
}

void Matrix3::Print(std::ostream & o) const
{
    o << "[ " << m[0][0] << "\t" << m[0][1] << "\t" <<  m[0][2] << std::endl
      << "  " << m[1][0] << "\t" << m[1][1] << "\t" <<  m[1][2] << std::endl
      << "  " << m[2][0] << "\t" << m[2][1] << "\t" <<  m[2][2] << std::endl;
}


/*// 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);
}
*/

---