VCRA-SZCZERBACKI-PointBasedRendering/ssMatrix.cpp

#include "ssDefs.hpp"
#include "ssVector.hpp"
#include "ssMatrix.hpp"

#include <cstring>

using namespace SS;

//===================================================================
//                                      Vector-Matrix functions
//===================================================================

Vector2 SS::operator* (const Vector2& v,  const Matrix2x2& m)
{
        Vector2 t = v;
        t *= m;
        return t;
}

Vector3 SS::operator* (const Vector3& v,  const Matrix3x3& m)
{
        Vector3 t = v;
        t *= m;
        return t;
}

Vector4 SS::operator* (const Vector4& v,  const Matrix4x4& m)
{
        Vector4 t = v;
        t *= m;
        return t;
}


//===================================================================
//                                      Binary Matrix Multiplications
//===================================================================

Matrix4x4       SS::operator* (const Matrix4x4& m1, const Matrix4x4& m2)
{
        Matrix4x4 m(m1);
        m*=m2;
        return m;
}

Matrix3x3       SS::operator* (const Matrix3x3& m1, const Matrix3x3& m2)
{
        Matrix3x3 m(m1);
        m*=m2;
        return m;
}

Matrix2x2       SS::operator* (const Matrix2x2& m1, const Matrix2x2& m2)
{
        Matrix2x2 m(m1);
        m*=m2;
        return m;
}

//===================================================================
//                                              Matrix3x4
//===================================================================

bool Matrix3x4::operator== (const Matrix3x4& n) const
{
        return ((*this)[0]==n[0] && (*this)[1]==n[1] && (*this)[2]==n[2]);
}

void Matrix3x4::clear(void)
{
        float* t = &m[0][0];
        for (int i = 0; i < 12; i++)
                t[i] = 0.0f;
}
                
void Matrix3x4::ident(void)
{
        m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f;
        m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f; m[1][3] = 0.0f;
        m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f; m[2][3] = 0.0f;
}

// NOTE: used to be productFromLeft()
void Matrix3x4::operator*= (const Matrix3x4& m2)
{
        SS_ASSERT(&m2 != this);

        Matrix3x4 m1 = *this;
        m[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
        m[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
        m[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
        m[0][3] = m1[0][0]*m2[0][3] + m1[0][1]*m2[1][3] + m1[0][2]*m2[2][3] + m1[0][3];
        m[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
        m[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
        m[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];
        m[1][3] = m1[1][0]*m2[0][3] + m1[1][1]*m2[1][3] + m1[1][2]*m2[2][3] + m1[1][3];
        m[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
        m[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
        m[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];
        m[2][3] = m1[2][0]*m2[0][3] + m1[2][1]*m2[1][3] + m1[2][2]*m2[2][3] + m1[2][3];
}

void Matrix3x4::operator*= (float f)
{
        for (int j = 0; j < 3; j++)
        for (int i = 0; i < 4; i++)
                m[j][i] *= f;
}

void Matrix3x4::flushToZero (void)                      
{
        const double EPSILON = 1e-15;

        for (int j = 0; j < 3; j++)
        for (int i = 0; i < 4; i++)
        if (fabs(m[j][i]) <= EPSILON)
                m[j][i] = 0.0f;
}


/******************************************************************************
 *
 * Function:            SS::Matrx4x3::invert()
 *
 * Description:         
 *
 * Notes:                       TIME-CRITICAL
 *
 *                                      m may be equal to src...
 *
 ******************************************************************************/

bool Matrix3x4::invert  ()
{
        Matrix3x4 src = *this;

        float a1 = src[0][0]; 
        float b1 = src[0][1];
        float c1 = src[0][2]; 
        float d1 = src[0][3];
        float a2 = src[1][0]; 
        float b2 = src[1][1]; 
        float c2 = src[1][2]; 
        float d2 = src[1][3];
        float a3 = src[2][0]; 
        float b3 = src[2][1]; 
        float c3 = src[2][2]; 
        float d3 = src[2][3];

        float b2c3_b3c2 = b2 * c3 - b3 * c2;
        float a3c2_a2c3 = a3 * c2 - a2 * c3;
        float a2b3_a3b2 = a2 * b3 - a3 * b2;

        float rDet      = (a1 * (b2c3_b3c2) + b1 * (a3c2_a2c3) + c1 * (a2b3_a3b2));
        if(rDet == 0.0f) return false;
        rDet = 1.0f / rDet;

        b2c3_b3c2       *= rDet;
        a3c2_a2c3       *= rDet;
        a2b3_a3b2       *= rDet;
        a1                      *= rDet;
        b1                      *= rDet;
        c1                      *= rDet;

        float c1b3_b1c3 = c1 * b3 - b1 * c3;
        float b1c2_c1b2 = b1 * c2 - c1 * b2;

        m[0][0] = b2c3_b3c2;
        m[0][1] = c1b3_b1c3;
        m[0][2] = b1c2_c1b2;
        m[0][3] =-(d1 * (b2c3_b3c2) + d2 * (c1b3_b1c3) + d3 * (b1c2_c1b2));

        float c1a2_a1c2 = c1 * a2 - a1 * c2;
        float a1c3_c1a3 = a1 * c3 - c1 * a3;

        m[1][0] = a3c2_a2c3;
        m[1][1] = a1c3_c1a3;
        m[1][2] = c1a2_a1c2;
        m[1][3] =-(d1 * (a3c2_a2c3) + d2 * (a1c3_c1a3) + d3 * (c1a2_a1c2));

        float b1a3_a1b3 = b1 * a3 - a1 * b3;
        float a1b2_b1a2 = a1 * b2 - b1 * a2;

        m[2][0] = a2b3_a3b2;
        m[2][1] = b1a3_a1b3;
        m[2][2] = a1b2_b1a2;
        m[2][3] =-(d1 * (a2b3_a3b2) + d2 * (b1a3_a1b3) + d3 * (a1b2_b1a2));

        return true;
}

//===================================================================
//                                              Matrix4x4
//===================================================================

// avoid underflow in any of the components...
void Matrix4x4::flushToZero (void)                      
{
        const double EPSILON = 1e-15;

        for (int j = 0; j < 4; j++)
        for (int i = 0; i < 4; i++)
        if (fabs(m[j][i]) <= EPSILON)
                m[j][i] = 0.0f;
}

void Matrix4x4::transpose (const Matrix4x4& src)
{
        if (&src == this)
        {
                this->transpose();
                return;
        }

        // note that transposition is _not_ the same as matrix inversion
        // with generic 4x4 matrices (see Matrix4x4::invert)

        m[0][0] = src[0][0];
        m[0][1] = src[1][0];
        m[0][2] = src[2][0];
        m[0][3] = src[3][0];
        m[1][0] = src[0][1];
        m[1][1] = src[1][1];
        m[1][2] = src[2][1];
        m[1][3] = src[3][1];
        m[2][0] = src[0][2];
        m[2][1] = src[1][2];
        m[2][2] = src[2][2];
        m[2][3] = src[3][2];
        m[3][0] = src[0][3];
        m[3][1] = src[1][3];
        m[3][2] = src[2][3];
        m[3][3] = src[3][3];
}

void Matrix4x4::transpose (void)
{
        float tmp;
        tmp = m[0][1]; m[0][1] = m[1][0]; m[1][0] = tmp;
        tmp = m[0][2]; m[0][2] = m[2][0]; m[2][0] = tmp;
        tmp = m[0][3]; m[0][3] = m[3][0]; m[3][0] = tmp;
        tmp = m[1][2]; m[1][2] = m[2][1]; m[2][1] = tmp;
        tmp = m[1][3]; m[1][3] = m[3][1]; m[3][1] = tmp;
        tmp = m[2][3]; m[2][3] = m[3][2]; m[3][2] = tmp;
}

float Matrix4x4::det2x2( float a, float b, float c, float d ) const
{
        return a * d - b * c;
}

float Matrix4x4::det3x3( float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2, float c3 ) const
{
        return a1 * det2x2 (b2, b3, c2, c3) - b1 * det2x2 (a2, a3, c2, c3) + c1 * det2x2 (a2, a3, b2, b3);
}

float Matrix4x4::det() const
{
        return m[0][0] * det3x3( m[1][1], m[2][1], m[3][1], m[1][2], m[2][2], m[3][2], m[1][3], m[2][3], m[3][3] ) -
                   m[0][1] * det3x3( m[1][0], m[2][0], m[3][0], m[1][2], m[2][2], m[3][2], m[1][3], m[2][3], m[3][3] ) +
                   m[0][2] * det3x3( m[1][0], m[2][0], m[3][0], m[1][1], m[2][1], m[3][1], m[1][3], m[2][3], m[3][3] ) -
                   m[0][3] * det3x3( m[1][0], m[2][0], m[3][0], m[1][1], m[2][1], m[3][1], m[1][2], m[2][2], m[3][2] );
}

bool Matrix4x4::operator== (const Matrix4x4& n) const
{
        const float* d = (const float*)&(m[0][0]);
        const float* s = (const float*)&(n[0][0]);
        for (int i = 0; i < 16; i++)
        if (d[i]!=s[i])
                return false;
        return true;
}

void Matrix4x4::operator*= (const Matrix4x4& m2)
{
        SS_ASSERT(&m2 != this);

        Matrix4x4 m1 = *this;
        m[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0] + m1[0][3]*m2[3][0];
        m[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1] + m1[0][3]*m2[3][1];
        m[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2] + m1[0][3]*m2[3][2];
        m[0][3] = m1[0][0]*m2[0][3] + m1[0][1]*m2[1][3] + m1[0][2]*m2[2][3] + m1[0][3]*m2[3][3];
        m[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0] + m1[1][3]*m2[3][0];
        m[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1] + m1[1][3]*m2[3][1];
        m[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2] + m1[1][3]*m2[3][2];
        m[1][3] = m1[1][0]*m2[0][3] + m1[1][1]*m2[1][3] + m1[1][2]*m2[2][3] + m1[1][3]*m2[3][3];
        m[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0] + m1[2][3]*m2[3][0];
        m[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1] + m1[2][3]*m2[3][1];
        m[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2] + m1[2][3]*m2[3][2];
        m[2][3] = m1[2][0]*m2[0][3] + m1[2][1]*m2[1][3] + m1[2][2]*m2[2][3] + m1[2][3]*m2[3][3];
        m[3][0] = m1[3][0]*m2[0][0] + m1[3][1]*m2[1][0] + m1[3][2]*m2[2][0] + m1[3][3]*m2[3][0];
        m[3][1] = m1[3][0]*m2[0][1] + m1[3][1]*m2[1][1] + m1[3][2]*m2[2][1] + m1[3][3]*m2[3][1];
        m[3][2] = m1[3][0]*m2[0][2] + m1[3][1]*m2[1][2] + m1[3][2]*m2[2][2] + m1[3][3]*m2[3][2];
        m[3][3] = m1[3][0]*m2[0][3] + m1[3][1]*m2[1][3] + m1[3][2]*m2[2][3] + m1[3][3]*m2[3][3];
}

void Matrix4x4::operator*= (float f)
{
        for (int j = 0; j < 4; j++)
        for (int i = 0; i < 4; i++)
                m[j][i] *= f;
}

void Matrix4x4::clear (void)
{
        memset (this, 0, sizeof(Matrix4x4));
}

void Matrix4x4::ident (void)
{
        m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f;
        m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f; m[1][3] = 0.0f;
        m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f; m[2][3] = 0.0f;
        m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f;
}

/******************************************************************************
 *
 * Function:            
 *
 * Description:         Matrix4x4 inversion code
 *
 * Notes:                       this == &src is supported
 *
 ******************************************************************************/

bool Matrix4x4::invert  ()
{
        // perform 4x3 inversion if possible
        if(is3x4Matrix())
        {
                Matrix3x4& m = reinterpret_cast<Matrix3x4&>(*this);             // consider 4x3
                bool invertible = m.invert();                   // invert 4x3
                this->m[3][0] = 0;                                                                              // clear 4th row
                this->m[3][1] = 0;
                this->m[3][2] = 0;
                this->m[3][3] = 1.0f;
                return invertible;
        }

        // perform full 4x4 inversion
        Matrix4x4 src = *this;

        const float a4 = src[3][0]; 
        const float b4 = src[3][1]; 
        const float c4 = src[3][2]; 
        const float d4 = src[3][3];
        const float a1 = src[0][0]; 
        const float b1 = src[0][1]; 
        const float c1 = src[0][2]; 
        const float d1 = src[0][3];
        const float a2 = src[1][0]; 
        const float b2 = src[1][1]; 
        const float c2 = src[1][2]; 
        const float d2 = src[1][3];
        const float a3 = src[2][0];
        const float b3 = src[2][1]; 
        const float c3 = src[2][2]; 
        const float d3 = src[2][3];

        float a3b4_a4b3 = a3 * b4 - a4 * b3 ;
        float a3c4_a4c3 = a3 * c4 - a4 * c3 ;
        float a3d4_a4d3 = a3 * d4 - a4 * d3 ;
        float b3c4_b4c3 = b3 * c4 - b4 * c3 ;
        float b3d4_b4d3 = b3 * d4 - b4 * d3 ;
        float c3d4_c4d3 = c3 * d4 - c4 * d3 ;
        
        m[0][0]                 = (b2 * c3d4_c4d3 - c2 * b3d4_b4d3 + d2 * b3c4_b4c3);
        m[1][0]                 =-(a2 * c3d4_c4d3 - c2 * a3d4_a4d3 + d2 * a3c4_a4c3);
        m[2][0]                 = (a2 * b3d4_b4d3 - b2 * a3d4_a4d3 + d2 * a3b4_a4b3);
        m[3][0]                 =-(a2 * b3c4_b4c3 - b2 * a3c4_a4c3 + c2 * a3b4_a4b3);
        m[0][1]                 =-(b1 * c3d4_c4d3 - c1 * b3d4_b4d3 + d1 * b3c4_b4c3);
        m[1][1]                 = (a1 * c3d4_c4d3 - c1 * a3d4_a4d3 + d1 * a3c4_a4c3);
        m[2][1]                 =-(a1 * b3d4_b4d3 - b1 * a3d4_a4d3 + d1 * a3b4_a4b3);
        m[3][1]                 = (a1 * b3c4_b4c3 - b1 * a3c4_a4c3 + c1 * a3b4_a4b3);

        float a2d4_a4d2 = a2 * d4 - a4 * d2;
        float a2b4_a4b2 = a2 * b4 - a4 * b2;
        float a2c4_a4c2 = a2 * c4 - a4 * c2;
        float b2c4_b4c2 = b2 * c4 - b4 * c2;
        float b2d4_b4d2 = b2 * d4 - b4 * d2;
        float c2d4_c4d2 = c2 * d4 - c4 * d2;

        m[0][2]                 = (b1 * c2d4_c4d2 - c1 * b2d4_b4d2 + d1 * b2c4_b4c2);
        m[1][2]                 =-(a1 * c2d4_c4d2 - c1 * a2d4_a4d2 + d1 * a2c4_a4c2);
        m[2][2]                 = (a1 * b2d4_b4d2 - b1 * a2d4_a4d2 + d1 * a2b4_a4b2);
        m[3][2]                 =-(a1 * b2c4_b4c2 - b1 * a2c4_a4c2 + c1 * a2b4_a4b2);

        float a2b3_a3b2 = a2 * b3 - a3 * b2;
        float a2c3_a3c2 = a2 * c3 - a3 * c2;
        float a2d3_a3d2 = a2 * d3 - a3 * d2;
        float b2c3_b3c2 = b2 * c3 - b3 * c2;
        float b2d3_b3d2 = b2 * d3 - b3 * d2;
        float c2d3_c3d2 = c2 * d3 - c3 * d2;

        m[0][3]                 =-(b1 * c2d3_c3d2 - c1 * b2d3_b3d2 + d1 * b2c3_b3c2);
        m[1][3]                 = (a1 * c2d3_c3d2 - c1 * a2d3_a3d2 + d1 * a2c3_a3c2);
        m[2][3]                 =-(a1 * b2d3_b3d2 - b1 * a2d3_a3d2 + d1 * a2b3_a3b2);
        m[3][3]                 = (a1 * b2c3_b3c2 - b1 * a2c3_a3c2 + c1 * a2b3_a3b2);

        float  det              = a1 * m[0][0] + b1 * m[1][0] + c1 * m[2][0] + d1 * m[3][0];
        if (det == 0.0f) return false;

        if (det != 1.0f)
        {
                det = 1.0f/det;
                m[0][0] = m[0][0]*det;
                m[0][1] = m[0][1]*det;
                m[0][2] = m[0][2]*det;
                m[0][3] = m[0][3]*det;
                m[1][0] = m[1][0]*det;
                m[1][1] = m[1][1]*det;
                m[1][2] = m[1][2]*det;
                m[1][3] = m[1][3]*det;
                m[2][0] = m[2][0]*det;
                m[2][1] = m[2][1]*det;
                m[2][2] = m[2][2]*det;
                m[2][3] = m[2][3]*det;
                m[3][0] = m[3][0]*det;
                m[3][1] = m[3][1]*det;
                m[3][2] = m[3][2]*det;
                m[3][3] = m[3][3]*det;
        } 

        return true;
}


//===================================================================
//                                              Matrix3x3
//===================================================================

// avoid underflow in any of the components...
void Matrix3x3::flushToZero (void)                      
{
        const double EPSILON = 1e-15;

        for (int j = 0; j < 3; j++)
        for (int i = 0; i < 3; i++)
        if (fabs(m[j][i]) <= EPSILON)
                m[j][i] = 0.0f;
}

void Matrix3x3::transpose (const Matrix3x3& src)
{
        if (&src == this)
        {
                this->transpose();
                return;
        }

        // note that transposition is _not_ the same as matrix inversion
        // with generic 3x3 matrices (see Matrix3x3::invert)

        m[0][0] = src[0][0];
        m[0][1] = src[1][0];
        m[0][2] = src[2][0];
        m[1][0] = src[0][1];
        m[1][1] = src[1][1];
        m[1][2] = src[2][1];
        m[2][0] = src[0][2];
        m[2][1] = src[1][2];
        m[2][2] = src[2][2];
}

void Matrix3x3::transpose (void)
{
        float tmp;
        tmp = m[0][1]; m[0][1] = m[1][0]; m[1][0] = tmp;
        tmp = m[0][2]; m[0][2] = m[2][0]; m[2][0] = tmp;
        tmp = m[1][2]; m[1][2] = m[2][1]; m[2][1] = tmp;
}

float Matrix3x3::det (void)
{
    return m[0][0] * (m[1][1]*m[2][2] - m[2][1]*m[1][2]) +
           m[0][1] * (m[2][0]*m[1][2] - m[1][0]*m[2][2]) +
           m[0][2] * (m[1][0]*m[2][1] - m[2][0]*m[1][1]);
}


bool Matrix3x3::operator== (const Matrix3x3& n) const
{
        const float* d = (const float*)&(m[0][0]);
        const float* s = (const float*)&(n[0][0]);
        for (int i = 0; i < 9; i++)
        if (d[i]!=s[i])
                return false;
        return true;
}

void Matrix3x3::operator*= (const Matrix3x3& m2)
{
        SS_ASSERT(&m2 != this);

        Matrix3x3 m1 = *this;
        m[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
        m[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
        m[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
        m[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
        m[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
        m[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];
        m[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
        m[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
        m[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];
}

void Matrix3x3::operator*= (float f)
{
        for (int j = 0; j < 3; j++)
        for (int i = 0; i < 3; i++)
                m[j][i] *= f;
}

void Matrix3x3::clear (void)
{
        memset (this, 0, sizeof(Matrix3x3));
}

void Matrix3x3::ident (void)
{
        m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f;
        m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f;
        m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f;
}


/******************************************************************************
 *
 * Function:            
 *
 * Description:         Matrix3x3 inversion code
 *
 * Notes:                       this == &src is supported
 *
 ******************************************************************************/

bool Matrix3x3::invert  ()
{
        Matrix3x3 t = *this;

        float t1122_2112, t2012_1022, t1021_2011;
        float det;

        t1122_2112 = t[1][1]*t[2][2] - t[2][1]*t[1][2];
        t2012_1022 = t[2][0]*t[1][2] - t[1][0]*t[2][2];
        t1021_2011 = t[1][0]*t[2][1] - t[2][0]*t[1][1];

        det = t[0][0]*t1122_2112 + t[0][1]*t2012_1022 + t[0][2]*t1021_2011;
        if( det == 0.0f ) return false;

        det = 1.0f / det;
        m[0][0] = det * t1122_2112;
        m[1][0] = det * t2012_1022;
        m[2][0] = det * t1021_2011;
        m[0][1] = det * (t[2][1]*t[0][2] - t[0][1]*t[2][2]);
        m[1][1] = det * (t[0][0]*t[2][2] - t[2][0]*t[0][2]);
        m[2][1] = det * (t[2][0]*t[0][1] - t[0][0]*t[2][1]);
        m[0][2] = det * (t[0][1]*t[1][2] - t[1][1]*t[0][2]);
        m[1][2] = det * (t[1][0]*t[0][2] - t[0][0]*t[1][2]);
        m[2][2] = det * (t[0][0]*t[1][1] - t[1][0]*t[0][1]);

        return true;
}


//===================================================================
//                                              Matrix3x3
//===================================================================

// avoid underflow in any of the components...
void Matrix2x2::flushToZero (void)                      
{
        const double EPSILON = 1e-15;

        for (int j = 0; j < 2; j++)
        for (int i = 0; i < 2; i++)
        if (fabs(m[j][i]) <= EPSILON)
                m[j][i] = 0.0f;
}

void Matrix2x2::transpose (const Matrix2x2& src)
{
        if (&src == this)
        {
                this->transpose();
                return;
        }

        // note that transposition is _not_ the same as matrix inversion
        // with generic 2x2 matrices (see Matrix2x2::invert)

        m[0][0] = src[0][0];
        m[0][1] = src[1][0];
        m[1][0] = src[0][1];
        m[1][1] = src[1][1];
}

void Matrix2x2::transpose (void)
{
        float tmp;
        tmp = m[0][1]; m[0][1] = m[1][0]; m[1][0] = tmp;
}

float Matrix2x2::det (void)
{
    return m[0][0] * m[1][1] - m[0][1]*m[1][0];
}

bool Matrix2x2::operator== (const Matrix2x2& n) const
{
        const float* d = (const float*)&(m[0][0]);
        const float* s = (const float*)&(n[0][0]);
        for (int i = 0; i < 4; i++)
        if (d[i]!=s[i])
                return false;
        return true;
}

void Matrix2x2::operator*= (const Matrix2x2& m2)
{
        SS_ASSERT(&m2 != this);

        Matrix2x2 m1 = *this;
        m[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[1][0];
        m[0][1] = m1[0][0] * m2[0][1] + m1[0][1] * m2[1][1];
        m[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[1][0];
        m[1][1] = m1[1][0] * m2[0][1] + m1[1][1] * m2[1][1];
}

void Matrix2x2::operator*= (float f)
{
        for (int j = 0; j < 2; j++)
        for (int i = 0; i < 2; i++)
                m[j][i] *= f;
}

void Matrix2x2::clear (void)
{
        memset (this, 0, sizeof(Matrix2x2));
}

void Matrix2x2::ident (void)
{
        m[0][0] = 1.0f; m[0][1] = 0.0f;
        m[1][0] = 0.0f; m[1][1] = 1.0f;
}

/******************************************************************************
 *
 * Function:            
 *
 * Description:         Matrix2x2 inversion code
 *
 * Notes:                       this == &src is supported
 *
 ******************************************************************************/

bool Matrix2x2::invert  ()
{
        Matrix2x2 t = *this;

        float d = t.det();
        if( d == 0.0f ) return false;
        d = 1.0f / d;

        m[0][0] = t[1][1] * d;
        m[0][1] = -t[0][1] * d;
        m[1][0] = -t[1][0] * d;
        m[1][1] = t[0][0] * d;

        return true;
}

---