
#include <WFX/math/Matrix3.h>

using namespace WFX;
using namespace WFX::math;

// Adapted from Matrix math by Wild Magic http://www.magic-software.com

const Real Matrix3::EPSILON = 1e-06;
const Matrix3 Matrix3::ZERO(0,0,0,0,0,0,0,0,0);
const Matrix3 Matrix3::IDENTITY(1,0,0,0,1,0,0,0,1);

 
Matrix3::Matrix3 () {
    // For efficiency reasons, do not initialize matrix.
}
 
Matrix3::Matrix3 (const Real aafEntry[3][3]) {
    memcpy(m,aafEntry,9*sizeof(Real));
}
 
Matrix3::Matrix3 (const Matrix3& rkMatrix) {
    memcpy(m,rkMatrix.m,9*sizeof(Real));
}
 
Matrix3::Matrix3 (Real fEntry00, Real fEntry01, Real fEntry02,
    Real fEntry10, Real fEntry11, Real fEntry12, Real fEntry20,
    Real fEntry21, Real fEntry22) {
    m[0][0] = fEntry00;
    m[0][1] = fEntry01;
    m[0][2] = fEntry02;
    m[1][0] = fEntry10;
    m[1][1] = fEntry11;
    m[1][2] = fEntry12;
    m[2][0] = fEntry20;
    m[2][1] = fEntry21;
    m[2][2] = fEntry22;
}
 
Real* Matrix3::operator[] (int iRow) const {
    return (Real*)m[iRow];
}
 
Matrix3::operator Real* () {
    return (Real*)m[0];
}
 
Matrix3& Matrix3::operator= (const Matrix3& rkMatrix) {
    memcpy(m,rkMatrix.m,9*sizeof(Real));
    return *this;
}
 
bool Matrix3::operator== (const Matrix3& rkMatrix) const {
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
        {
            if ( m[iRow][iCol] != rkMatrix.m[iRow][iCol] )
                return false;
        }
    }

    return true;
}
 
bool Matrix3::operator!= (const Matrix3& rkMatrix) const {
    return !operator==(rkMatrix);
}
 
Matrix3 Matrix3::operator+ (const Matrix3& rkMatrix) const {
    Matrix3 kSum;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
        {
            kSum.m[iRow][iCol] = m[iRow][iCol] +
                rkMatrix.m[iRow][iCol];
        }
    }
    return kSum;
}
 
Matrix3 Matrix3::operator- (const Matrix3& rkMatrix) const {
    Matrix3 kDiff;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
        {
            kDiff.m[iRow][iCol] = m[iRow][iCol] -
                rkMatrix.m[iRow][iCol];
        }
    }
    return kDiff;
}
 
Matrix3 Matrix3::operator* (const Matrix3& rkMatrix) const {
    Matrix3 kProd;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
        {
            kProd.m[iRow][iCol] =
                m[iRow][0]*rkMatrix.m[0][iCol] +
                m[iRow][1]*rkMatrix.m[1][iCol] +
                m[iRow][2]*rkMatrix.m[2][iCol];
        }
    }
    return kProd;
}
 
Vector3 Matrix3::operator* (const Vector3& rkPoint) const {
    Vector3 kProd;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        kProd[iRow] =
            m[iRow][0]*rkPoint[0] +
            m[iRow][1]*rkPoint[1] +
            m[iRow][2]*rkPoint[2];
    }
    return kProd;
}
 
Matrix3 Matrix3::operator- () const {
    Matrix3 kNeg;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            kNeg[iRow][iCol] = -m[iRow][iCol];
    }
    return kNeg;
}
 
Matrix3 Matrix3::operator* (Real fScalar) const {
    Matrix3 kProd;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            kProd[iRow][iCol] = fScalar*m[iRow][iCol];
    }
    return kProd;
}
 
Matrix3 Matrix3::transpose () const {
    Matrix3 kTranspose;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            kTranspose[iRow][iCol] = m[iCol][iRow];
    }
    return kTranspose;
}
 
bool Matrix3::inverse (Matrix3& rkInverse, Real fTolerance) const {
    // Invert a 3x3 using cofactors.  This is about 8 times faster than
    // the Numerical Recipes code which uses Gaussian elimination.

    rkInverse[0][0] = m[1][1]*m[2][2] -
        m[1][2]*m[2][1];
    rkInverse[0][1] = m[0][2]*m[2][1] -
        m[0][1]*m[2][2];
    rkInverse[0][2] = m[0][1]*m[1][2] -
        m[0][2]*m[1][1];
    rkInverse[1][0] = m[1][2]*m[2][0] -
        m[1][0]*m[2][2];
    rkInverse[1][1] = m[0][0]*m[2][2] -
        m[0][2]*m[2][0];
    rkInverse[1][2] = m[0][2]*m[1][0] -
        m[0][0]*m[1][2];
    rkInverse[2][0] = m[1][0]*m[2][1] -
        m[1][1]*m[2][0];
    rkInverse[2][1] = m[0][1]*m[2][0] -
        m[0][0]*m[2][1];
    rkInverse[2][2] = m[0][0]*m[1][1] -
        m[0][1]*m[1][0];

    Real fDet =
        m[0][0]*rkInverse[0][0] +
        m[0][1]*rkInverse[1][0]+
        m[0][2]*rkInverse[2][0];

    if ( Math::abs(fDet) <= fTolerance )
        return false;

    Real fInvDet = 1.0/fDet;
    for (int iRow = 0; iRow < 3; iRow++)
    {
        for (int iCol = 0; iCol < 3; iCol++)
            rkInverse[iRow][iCol] *= fInvDet;
    }

    return true;
}
 
Matrix3 Matrix3::inverse (Real fTolerance) const {
    Matrix3 kInverse = Matrix3::ZERO;
    inverse(kInverse,fTolerance);
    return kInverse;
}
 
Real Matrix3::determinant () const {
    Real fCofactor00 = m[1][1]*m[2][2] -
        m[1][2]*m[2][1];
    Real fCofactor10 = m[1][2]*m[2][0] -
        m[1][0]*m[2][2];
    Real fCofactor20 = m[1][0]*m[2][1] -
        m[1][1]*m[2][0];

    Real fDet =
        m[0][0]*fCofactor00 +
        m[0][1]*fCofactor10 +
        m[0][2]*fCofactor20;

    return fDet;
}
 

