/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield 
 *
 * This library is open source and may be redistributed and/or modified under  
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or 
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 * 
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
 * OpenSceneGraph Public License for more details.
*/

#include <osg/Quat>
#include <osg/Notify>
#include <osg/Math>
#include <osg/Timer>

#include <osg/GL>

#include <stdlib.h>

using namespace osg;

#define SET_ROW(row, v1, v2, v3, v4 )    \
    _mat[(row)][0] = (v1); \
    _mat[(row)][1] = (v2); \
    _mat[(row)][2] = (v3); \
    _mat[(row)][3] = (v4);

#define INNER_PRODUCT(a,b,r,c) \
     ((a)._mat[r][0] * (b)._mat[0][c]) \
    +((a)._mat[r][1] * (b)._mat[1][c]) \
    +((a)._mat[r][2] * (b)._mat[2][c]) \
    +((a)._mat[r][3] * (b)._mat[3][c])


Matrix_implementation::Matrix_implementation( value_type a00, value_type a01, value_type a02, value_type a03,
                  value_type a10, value_type a11, value_type a12, value_type a13,
                  value_type a20, value_type a21, value_type a22, value_type a23,
                  value_type a30, value_type a31, value_type a32, value_type a33)
{
    SET_ROW(0, a00, a01, a02, a03 )
    SET_ROW(1, a10, a11, a12, a13 )
    SET_ROW(2, a20, a21, a22, a23 )
    SET_ROW(3, a30, a31, a32, a33 )
}

void Matrix_implementation::set( value_type a00, value_type a01, value_type a02, value_type a03,
                   value_type a10, value_type a11, value_type a12, value_type a13,
                   value_type a20, value_type a21, value_type a22, value_type a23,
                   value_type a30, value_type a31, value_type a32, value_type a33)
{
    SET_ROW(0, a00, a01, a02, a03 )
    SET_ROW(1, a10, a11, a12, a13 )
    SET_ROW(2, a20, a21, a22, a23 )
    SET_ROW(3, a30, a31, a32, a33 )
}

#define QX  q._v[0]
#define QY  q._v[1]
#define QZ  q._v[2]
#define QW  q._v[3]

void Matrix_implementation::set(const Quat& q_in)
{
    Quat q(q_in);
    double length2 = q.length2();
    if (length2!=1.0 && length2!=0)
    {
        // normalize quat if required.
        q /= sqrt(length2);
    }

    // Source: Gamasutra, Rotating Objects Using Quaternions
    //
    //http://www.gamasutra.com/features/19980703/quaternions_01.htm

    double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

    // calculate coefficients
    x2 = QX + QX;
    y2 = QY + QY;
    z2 = QZ + QZ;

    xx = QX * x2;
    xy = QX * y2;
    xz = QX * z2;

    yy = QY * y2;
    yz = QY * z2;
    zz = QZ * z2;

    wx = QW * x2;
    wy = QW * y2;
    wz = QW * z2;

    // Note.  Gamasutra gets the matrix assignments inverted, resulting
    // in left-handed rotations, which is contrary to OpenGL and OSG's 
    // methodology.  The matrix assignment has been altered in the next
    // few lines of code to do the right thing.
    // Don Burns - Oct 13, 2001
    _mat[0][0] = 1.0 - (yy + zz);
    _mat[1][0] = xy - wz;
    _mat[2][0] = xz + wy;
    _mat[3][0] = 0.0;

    _mat[0][1] = xy + wz;
    _mat[1][1] = 1.0 - (xx + zz);
    _mat[2][1] = yz - wx;
    _mat[3][1] = 0.0;

    _mat[0][2] = xz - wy;
    _mat[1][2] = yz + wx;
    _mat[2][2] = 1.0 - (xx + yy);
    _mat[3][2] = 0.0;

    _mat[0][3] = 0.0;
    _mat[1][3] = 0.0;
    _mat[2][3] = 0.0;
    _mat[3][3] = 1.0;
}

#if 1

// David Spillings implementation Mk 2
void Matrix_implementation::get( Quat& q ) const
{
    // From http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
    QW = 0.5 * sqrt( osg::maximum( 0.0, 1.0 + _mat[0][0] + _mat[1][1] + _mat[2][2] ) );
    QX = 0.5 * sqrt( osg::maximum( 0.0, 1.0 + _mat[0][0] - _mat[1][1] - _mat[2][2] ) );
    QY = 0.5 * sqrt( osg::maximum( 0.0, 1.0 - _mat[0][0] + _mat[1][1] - _mat[2][2] ) );
    QZ = 0.5 * sqrt( osg::maximum( 0.0, 1.0 - _mat[0][0] - _mat[1][1] + _mat[2][2] ) );

    QX = QX * osg::sign(  _mat[1][2] - _mat[2][1]) ;
    QY = QY * osg::sign(  _mat[2][0] - _mat[0][2]) ;
    QZ = QZ * osg::sign(  _mat[0][1] - _mat[1][0]) ;
}

#else

    #if 1
    // David Spillings implementation Mk 1
    void Matrix_implementation::get( Quat& q ) const
    {
        value_type s;
        value_type tq[4];
        int    i, j;

        // Use tq to store the largest trace
        tq[0] = 1 + _mat[0][0]+_mat[1][1]+_mat[2][2];
        tq[1] = 1 + _mat[0][0]-_mat[1][1]-_mat[2][2];
        tq[2] = 1 - _mat[0][0]+_mat[1][1]-_mat[2][2];
        tq[3] = 1 - _mat[0][0]-_mat[1][1]+_mat[2][2];

        // Find the maximum (could also use stacked if's later)
        j = 0;
        for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j;

        // check the diagonal
        if (j==0)
        {
            /* perform instant calculation */
            QW = tq[0];
            QX = _mat[1][2]-_mat[2][1]; 
            QY = _mat[2][0]-_mat[0][2]; 
            QZ = _mat[0][1]-_mat[1][0]; 
        }
        else if (j==1)
        {
            QW = _mat[1][2]-_mat[2][1]; 
            QX = tq[1];
            QY = _mat[0][1]+_mat[1][0]; 
            QZ = _mat[2][0]+_mat[0][2]; 
        }
        else if (j==2)
        {
            QW = _mat[2][0]-_mat[0][2]; 
            QX = _mat[0][1]+_mat[1][0]; 
            QY = tq[2];
            QZ = _mat[1][2]+_mat[2][1]; 
        }
        else /* if (j==3) */
        {
            QW = _mat[0][1]-_mat[1][0]; 
            QX = _mat[2][0]+_mat[0][2]; 
            QY = _mat[1][2]+_mat[2][1]; 
            QZ = tq[3];
        }

        s = sqrt(0.25/tq[j]);
        QW *= s;
        QX *= s;
        QY *= s;
        QZ *= s;

    }
    #else
    // Original implementation
    void Matrix_implementation::get( Quat& q ) const
    {
        // Source: Gamasutra, Rotating Objects Using Quaternions
        //
        //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm

        value_type  tr, s;
        value_type tq[4];
        int    i, j, k;

        int nxt[3] = {1, 2, 0};

        tr = _mat[0][0] + _mat[1][1] + _mat[2][2]+1.0;

        // check the diagonal
        if (tr > 0.0)
        {
            s = (value_type)sqrt (tr);
            QW = s / 2.0;
            s = 0.5 / s;
            QX = (_mat[1][2] - _mat[2][1]) * s;
            QY = (_mat[2][0] - _mat[0][2]) * s;
            QZ = (_mat[0][1] - _mat[1][0]) * s;
        }
        else
        {
            // diagonal is negative
            i = 0;
            if (_mat[1][1] > _mat[0][0])
                i = 1;
            if (_mat[2][2] > _mat[i][i])
                i = 2;
            j = nxt[i];
            k = nxt[j];

            s = (value_type)sqrt ((_mat[i][i] - (_mat[j][j] + _mat[k][k])) + 1.0);

            tq[i] = s * 0.5;

            if (s != 0.0)
                s = 0.5 / s;

            tq[3] = (_mat[j][k] - _mat[k][j]) * s;
            tq[j] = (_mat[i][j] + _mat[j][i]) * s;
            tq[k] = (_mat[i][k] + _mat[k][i]) * s;

            QX = tq[0];
            QY = tq[1];
            QZ = tq[2];
            QW = tq[3];
        }
    }
    #endif
#endif

int Matrix_implementation::compare(const Matrix_implementation& m) const
{
    const Matrix_implementation::value_type* lhs = reinterpret_cast<const Matrix_implementation::value_type*>(_mat);
    const Matrix_implementation::value_type* end_lhs = lhs+16;
    const Matrix_implementation::value_type* rhs = reinterpret_cast<const Matrix_implementation::value_type*>(m._mat);
    for(;lhs!=end_lhs;++lhs,++rhs)
    {
        if (*lhs < *rhs) return -1;
        if (*rhs < *lhs) return 1;
    }
    return 0;
}

void Matrix_implementation::setTrans( value_type tx, value_type ty, value_type tz )
{
    _mat[3][0] = tx;
    _mat[3][1] = ty;
    _mat[3][2] = tz;
}


void Matrix_implementation::setTrans( const Vec3f& v )
{
    _mat[3][0] = v[0];
    _mat[3][1] = v[1];
    _mat[3][2] = v[2];
}
void Matrix_implementation::setTrans( const Vec3d& v )
{
    _mat[3][0] = v[0];
    _mat[3][1] = v[1];
    _mat[3][2] = v[2];
}

void Matrix_implementation::makeIdentity()
{
    SET_ROW(0,    1, 0, 0, 0 )
    SET_ROW(1,    0, 1, 0, 0 )
    SET_ROW(2,    0, 0, 1, 0 )
    SET_ROW(3,    0, 0, 0, 1 )
}

void Matrix_implementation::makeScale( const Vec3f& v )
{
    makeScale(v[0], v[1], v[2] );
}

void Matrix_implementation::makeScale( const Vec3d& v )
{
    makeScale(v[0], v[1], v[2] );
}

void Matrix_implementation::makeScale( value_type x, value_type y, value_type z )
{
    SET_ROW(0,    x, 0, 0, 0 )
    SET_ROW(1,    0, y, 0, 0 )
    SET_ROW(2,    0, 0, z, 0 )
    SET_ROW(3,    0, 0, 0, 1 )
}

void Matrix_implementation::makeTranslate( const Vec3f& v )
{
    makeTranslate( v[0], v[1], v[2] );
}

void Matrix_implementation::makeTranslate( const Vec3d& v )
{
    makeTranslate( v[0], v[1], v[2] );
}

void Matrix_implementation::makeTranslate( value_type x, value_type y, value_type z )
{
    SET_ROW(0,    1, 0, 0, 0 )
    SET_ROW(1,    0, 1, 0, 0 )
    SET_ROW(2,    0, 0, 1, 0 )
    SET_ROW(3,    x, y, z, 1 )
}

void Matrix_implementation::makeRotate( const Vec3f& from, const Vec3f& to )
{
    Quat quat;
    quat.makeRotate(from,to);
    set(quat);
}
void Matrix_implementation::makeRotate( const Vec3d& from, const Vec3d& to )
{
    Quat quat;
    quat.makeRotate(from,to);
    set(quat);
}

void Matrix_implementation::makeRotate( value_type angle, const Vec3f& axis )
{
    Quat quat;
    quat.makeRotate( angle, axis);
    set(quat);
}
void Matrix_implementation::makeRotate( value_type angle, const Vec3d& axis )
{
    Quat quat;
    quat.makeRotate( angle, axis);
    set(quat);
}

void Matrix_implementation::makeRotate( value_type angle, value_type x, value_type y, value_type z ) 
{
    Quat quat;
    quat.makeRotate( angle, x, y, z);
    set(quat);
}

void Matrix_implementation::makeRotate( const Quat& quat )
{
    set(quat);
}

void Matrix_implementation::makeRotate( value_type angle1, const Vec3f& axis1, 
                         value_type angle2, const Vec3f& axis2,
                         value_type angle3, const Vec3f& axis3)
{
    Quat quat;
    quat.makeRotate(angle1, axis1, 
                    angle2, axis2,
                    angle3, axis3);
    set(quat);
}

void Matrix_implementation::makeRotate( value_type angle1, const Vec3d& axis1, 
                         value_type angle2, const Vec3d& axis2,
                         value_type angle3, const Vec3d& axis3)
{
    Quat quat;
    quat.makeRotate(angle1, axis1, 
                    angle2, axis2,
                    angle3, axis3);
    set(quat);
}

void Matrix_implementation::mult( const Matrix_implementation& lhs, const Matrix_implementation& rhs )
{   
    if (&lhs==this)
    {
        postMult(rhs);
        return;
    }
    if (&rhs==this)
    {
        preMult(lhs);
        return;
    }

// PRECONDITION: We assume neither &lhs nor &rhs == this
// if it did, use preMult or postMult instead
    _mat[0][0] = INNER_PRODUCT(lhs, rhs, 0, 0);
    _mat[0][1] = INNER_PRODUCT(lhs, rhs, 0, 1);
    _mat[0][2] = INNER_PRODUCT(lhs, rhs, 0, 2);
    _mat[0][3] = INNER_PRODUCT(lhs, rhs, 0, 3);
    _mat[1][0] = INNER_PRODUCT(lhs, rhs, 1, 0);
    _mat[1][1] = INNER_PRODUCT(lhs, rhs, 1, 1);
    _mat[1][2] = INNER_PRODUCT(lhs, rhs, 1, 2);
    _mat[1][3] = INNER_PRODUCT(lhs, rhs, 1, 3);
    _mat[2][0] = INNER_PRODUCT(lhs, rhs, 2, 0);
    _mat[2][1] = INNER_PRODUCT(lhs, rhs, 2, 1);
    _mat[2][2] = INNER_PRODUCT(lhs, rhs, 2, 2);
    _mat[2][3] = INNER_PRODUCT(lhs, rhs, 2, 3);
    _mat[3][0] = INNER_PRODUCT(lhs, rhs, 3, 0);
    _mat[3][1] = INNER_PRODUCT(lhs, rhs, 3, 1);
    _mat[3][2] = INNER_PRODUCT(lhs, rhs, 3, 2);
    _mat[3][3] = INNER_PRODUCT(lhs, rhs, 3, 3);
}

void Matrix_implementation::preMult( const Matrix_implementation& other )
{
    // brute force method requiring a copy
    //Matrix_implementation tmp(other* *this);
    // *this = tmp;

    // more efficient method just use a value_type[4] for temporary storage.
    value_type t[4];
    for(int col=0; col<4; ++col) {
        t[0] = INNER_PRODUCT( other, *this, 0, col );
        t[1] = INNER_PRODUCT( other, *this, 1, col );
        t[2] = INNER_PRODUCT( other, *this, 2, col );
        t[3] = INNER_PRODUCT( other, *this, 3, col );
        _mat[0][col] = t[0];
        _mat[1][col] = t[1];
        _mat[2][col] = t[2];
        _mat[3][col] = t[3];
    }

}

void Matrix_implementation::postMult( const Matrix_implementation& other )
{
    // brute force method requiring a copy
    //Matrix_implementation tmp(*this * other);
    // *this = tmp;

    // more efficient method just use a value_type[4] for temporary storage.
    value_type t[4];
    for(int row=0; row<4; ++row)
    {
        t[0] = INNER_PRODUCT( *this, other, row, 0 );
        t[1] = INNER_PRODUCT( *this, other, row, 1 );
        t[2] = INNER_PRODUCT( *this, other, row, 2 );
        t[3] = INNER_PRODUCT( *this, other, row, 3 );
        SET_ROW(row, t[0], t[1], t[2], t[3] )
    }
}

#undef INNER_PRODUCT

// orthoNormalize the 3x3 rotation matrix
void Matrix_implementation::orthoNormalize(const Matrix_implementation& rhs)
{
    value_type x_colMag = (rhs._mat[0][0] * rhs._mat[0][0]) + (rhs._mat[1][0] * rhs._mat[1][0]) + (rhs._mat[2][0] * rhs._mat[2][0]);
    value_type y_colMag = (rhs._mat[0][1] * rhs._mat[0][1]) + (rhs._mat[1][1] * rhs._mat[1][1]) + (rhs._mat[2][1] * rhs._mat[2][1]);
    value_type z_colMag = (rhs._mat[0][2] * rhs._mat[0][2]) + (rhs._mat[1][2] * rhs._mat[1][2]) + (rhs._mat[2][2] * rhs._mat[2][2]);
    
    if(!equivalent((double)x_colMag, 1.0) && !equivalent((double)x_colMag, 0.0))
    {
      x_colMag = sqrt(x_colMag);
      _mat[0][0] = rhs._mat[0][0] / x_colMag;
      _mat[1][0] = rhs._mat[1][0] / x_colMag;
      _mat[2][0] = rhs._mat[2][0] / x_colMag;
    }
    else
    {
      _mat[0][0] = rhs._mat[0][0];
      _mat[1][0] = rhs._mat[1][0];
      _mat[2][0] = rhs._mat[2][0];
    }

    if(!equivalent((double)y_colMag, 1.0) && !equivalent((double)y_colMag, 0.0))
    {
      y_colMag = sqrt(y_colMag);
      _mat[0][1] = rhs._mat[0][1] / y_colMag;
      _mat[1][1] = rhs._mat[1][1] / y_colMag;
      _mat[2][1] = rhs._mat[2][1] / y_colMag;
    }
    else
    {
      _mat[0][1] = rhs._mat[0][1];
      _mat[1][1] = rhs._mat[1][1];
      _mat[2][1] = rhs._mat[2][1];
    }

    if(!equivalent((double)z_colMag, 1.0) && !equivalent((double)z_colMag, 0.0))
    {
      z_colMag = sqrt(z_colMag);
      _mat[0][2] = rhs._mat[0][2] / z_colMag;
      _mat[1][2] = rhs._mat[1][2] / z_colMag;
      _mat[2][2] = rhs._mat[2][2] / z_colMag;
    }
    else
    {
      _mat[0][2] = rhs._mat[0][2];
      _mat[1][2] = rhs._mat[1][2];
      _mat[2][2] = rhs._mat[2][2];
    }

    _mat[3][0] = rhs._mat[3][0];
    _mat[3][1] = rhs._mat[3][1];
    _mat[3][2] = rhs._mat[3][2];

    _mat[0][3] = rhs._mat[0][3];
    _mat[1][3] = rhs._mat[1][3];
    _mat[2][3] = rhs._mat[2][3];
    _mat[3][3] = rhs._mat[3][3];

}

bool Matrix_implementation::invert( const Matrix_implementation& rhs)
{
#if 1
    return invert_4x4_new(rhs);
#else
    static const osg::Timer& timer = *Timer::instance();

    Matrix_implementation a;
    Matrix_implementation b;

    Timer_t t1 = timer.tick();

    a.invert_4x4_new(rhs);
    
    Timer_t t2 = timer.tick();

    b.invert_4x4_orig(rhs);
    
    Timer_t t3 = timer.tick();

    static double new_time = 0.0;
    static double orig_time = 0.0;
    static double count = 0.0;
    
    new_time += timer.delta_u(t1,t2);
    orig_time += timer.delta_u(t2,t3);
    ++count;
    
    std::cout<<"Average new="<<new_time/count<<"  orig = "<<orig_time/count<<std::endl;

    std::cout<<"new matrix invert time="<<timer.delta_u(t1,t2)<<" "<<a<<std::endl;
    std::cout<<"orig matrix invert time="<<timer.delta_u(t2,t3)<<" "<<b<<std::endl;
    
    set(b);
    
    return true;
#endif
}

/******************************************
  Matrix inversion technique:
Given a matrix mat, we want to invert it.
mat = [ r00 r01 r02 a
        r10 r11 r12 b
        r20 r21 r22 c
        tx  ty  tz  d ]
We note that this matrix can be split into three matrices.
mat = rot * trans * corr, where rot is rotation part, trans is translation part, and corr is the correction due to perspective (if any).
rot = [ r00 r01 r02 0
        r10 r11 r12 0
        r20 r21 r22 0
        0   0   0   1 ]
trans = [ 1  0  0  0
          0  1  0  0
          0  0  1  0
          tx ty tz 1 ]
corr = [ 1 0 0 px
         0 1 0 py
         0 0 1 pz
         0 0 0 s ]
where the elements of corr are obtained from linear combinations of the elements of rot, trans, and mat.
So the inverse is mat' = (trans * corr)' * rot', where rot' must be computed the traditional way, which is easy since it is only a 3x3 matrix.
This problem is simplified if [px py pz s] = [0 0 0 1], which will happen if mat was composed only of rotations, scales, and translations (which is common).  In this case, we can ignore corr entirely which saves on a lot of computations.
******************************************/

bool Matrix_implementation::invert_4x4_new( const Matrix_implementation& mat )
{
    if (&mat==this)
    {
       Matrix_implementation tm(mat);
       return invert_4x4_new(tm);
    }

    register value_type r00, r01, r02,
                        r10, r11, r12,
                        r20, r21, r22;
      // Copy rotation components directly into registers for speed
    r00 = mat._mat[0][0]; r01 = mat._mat[0][1]; r02 = mat._mat[0][2];
    r10 = mat._mat[1][0]; r11 = mat._mat[1][1]; r12 = mat._mat[1][2];
    r20 = mat._mat[2][0]; r21 = mat._mat[2][1]; r22 = mat._mat[2][2];

        // Partially compute inverse of rot
    _mat[0][0] = r11*r22 - r12*r21;
    _mat[0][1] = r02*r21 - r01*r22;
    _mat[0][2] = r01*r12 - r02*r11;

      // Compute determinant of rot from 3 elements just computed
    register value_type one_over_det = 1.0/(r00*_mat[0][0] + r10*_mat[0][1] + r20*_mat[0][2]);
    r00 *= one_over_det; r10 *= one_over_det; r20 *= one_over_det;  // Saves on later computations

      // Finish computing inverse of rot
    _mat[0][0] *= one_over_det;
    _mat[0][1] *= one_over_det;
    _mat[0][2] *= one_over_det;
    _mat[0][3] = 0.0;
    _mat[1][0] = r12*r20 - r10*r22; // Have already been divided by det
    _mat[1][1] = r00*r22 - r02*r20; // same
    _mat[1][2] = r02*r10 - r00*r12; // same
    _mat[1][3] = 0.0;
    _mat[2][0] = r10*r21 - r11*r20; // Have already been divided by det
    _mat[2][1] = r01*r20 - r00*r21; // same
    _mat[2][2] = r00*r11 - r01*r10; // same
    _mat[2][3] = 0.0;
    _mat[3][3] = 1.0;

// We no longer need the rxx or det variables anymore, so we can reuse them for whatever we want.  But we will still rename them for the sake of clarity.

#define d r22
    d  = mat._mat[3][3];

    if( osg::square(d-1.0) > 1.0e-6 )  // Involves perspective, so we must
    {                       // compute the full inverse
    
        Matrix_implementation TPinv;
        _mat[3][0] = _mat[3][1] = _mat[3][2] = 0.0;

#define px r00
#define py r01
#define pz r02
#define one_over_s  one_over_det
#define a  r10
#define b  r11
#define c  r12

        a  = mat._mat[0][3]; b  = mat._mat[1][3]; c  = mat._mat[2][3];
        px = _mat[0][0]*a + _mat[0][1]*b + _mat[0][2]*c;
        py = _mat[1][0]*a + _mat[1][1]*b + _mat[1][2]*c;
        pz = _mat[2][0]*a + _mat[2][1]*b + _mat[2][2]*c;

#undef a
#undef b
#undef c
#define tx r10
#define ty r11
#define tz r12

        tx = mat._mat[3][0]; ty = mat._mat[3][1]; tz = mat._mat[3][2];
        one_over_s  = 1.0/(d - (tx*px + ty*py + tz*pz));

        tx *= one_over_s; ty *= one_over_s; tz *= one_over_s;  // Reduces number of calculations later on

        // Compute inverse of trans*corr
        TPinv._mat[0][0] = tx*px + 1.0;
        TPinv._mat[0][1] = ty*px;
        TPinv._mat[0][2] = tz*px;
        TPinv._mat[0][3] = -px * one_over_s;
        TPinv._mat[1][0] = tx*py;
        TPinv._mat[1][1] = ty*py + 1.0;
        TPinv._mat[1][2] = tz*py;
        TPinv._mat[1][3] = -py * one_over_s;
        TPinv._mat[2][0] = tx*pz;
        TPinv._mat[2][1] = ty*pz;
        TPinv._mat[2][2] = tz*pz + 1.0;
        TPinv._mat[2][3] = -pz * one_over_s;
        TPinv._mat[3][0] = -tx;
        TPinv._mat[3][1] = -ty;
        TPinv._mat[3][2] = -tz;
        TPinv._mat[3][3] = one_over_s;

        preMult(TPinv); // Finish computing full inverse of mat

#undef px
#undef py
#undef pz
#undef one_over_s
#undef d
    }
    else // Rightmost column is [0; 0; 0; 1] so it can be ignored
    {
        tx = mat._mat[3][0]; ty = mat._mat[3][1]; tz = mat._mat[3][2];

        // Compute translation components of mat'
        _mat[3][0] = -(tx*_mat[0][0] + ty*_mat[1][0] + tz*_mat[2][0]);
        _mat[3][1] = -(tx*_mat[0][1] + ty*_mat[1][1] + tz*_mat[2][1]);
        _mat[3][2] = -(tx*_mat[0][2] + ty*_mat[1][2] + tz*_mat[2][2]);

#undef tx
#undef ty
#undef tz
    }

    return true;
}


template <class T>
inline T SGL_ABS(T a)
{
   return (a >= 0 ? a : -a);
}

#ifndef SGL_SWAP
#define SGL_SWAP(a,b,temp) ((temp)=(a),(a)=(b),(b)=(temp))
#endif

bool Matrix_implementation::invert_4x4_orig( const Matrix_implementation& mat )
{
    if (&mat==this) {
       Matrix_implementation tm(mat);
       return invert_4x4_orig(tm);
    }

    unsigned int indxc[4], indxr[4], ipiv[4];
    unsigned int i,j,k,l,ll;
    unsigned int icol = 0;
    unsigned int irow = 0;
    double temp, pivinv, dum, big;

    // copy in place this may be unnecessary
    *this = mat;

    for (j=0; j<4; j++) ipiv[j]=0;

    for(i=0;i<4;i++)
    {
       big=0.0;
       for (j=0; j<4; j++)
          if (ipiv[j] != 1)
             for (k=0; k<4; k++)
             {
                if (ipiv[k] == 0)
                {
                   if (SGL_ABS(operator()(j,k)) >= big)
                   {
                      big = SGL_ABS(operator()(j,k));
                      irow=j;
                      icol=k;
                   }
                }
                else if (ipiv[k] > 1)
                   return false;
             }
       ++(ipiv[icol]);
       if (irow != icol)
          for (l=0; l<4; l++) SGL_SWAP(operator()(irow,l),
                                       operator()(icol,l),
                                       temp);

       indxr[i]=irow;
       indxc[i]=icol;
       if (operator()(icol,icol) == 0)
          return false;

       pivinv = 1.0/operator()(icol,icol);
       operator()(icol,icol) = 1;
       for (l=0; l<4; l++) operator()(icol,l) *= pivinv;
       for (ll=0; ll<4; ll++)
          if (ll != icol)
          {
             dum=operator()(ll,icol);
             operator()(ll,icol) = 0;
             for (l=0; l<4; l++) operator()(ll,l) -= operator()(icol,l)*dum;
          }
    }
    for (int lx=4; lx>0; --lx)
    {
       if (indxr[lx-1] != indxc[lx-1])
          for (k=0; k<4; k++) SGL_SWAP(operator()(k,indxr[lx-1]),
                                       operator()(k,indxc[lx-1]),temp);
    }

    return true;
}

void Matrix_implementation::makeOrtho(double left, double right,
                       double bottom, double top,
                       double zNear, double zFar)
{
    // note transpose of Matrix_implementation wr.t OpenGL documentation, since the OSG use post multiplication rather than pre.
    double tx = -(right+left)/(right-left);
    double ty = -(top+bottom)/(top-bottom);
    double tz = -(zFar+zNear)/(zFar-zNear);
    SET_ROW(0, 2.0/(right-left),               0.0,               0.0, 0.0 )
    SET_ROW(1,              0.0,  2.0/(top-bottom),               0.0, 0.0 )
    SET_ROW(2,              0.0,               0.0,  -2.0/(zFar-zNear), 0.0 )
    SET_ROW(3,               tx,                ty,                 tz, 1.0 )
}

bool Matrix_implementation::getOrtho(double& left, double& right,
                      double& bottom, double& top,
                      double& zNear, double& zFar) const
{
    if (_mat[0][3]!=0.0 || _mat[1][3]!=0.0 || _mat[2][3]!=0.0 || _mat[3][3]!=1.0) return false;

    zNear = (_mat[3][2]+1.0) / _mat[2][2];
    zFar = (_mat[3][2]-1.0) / _mat[2][2];
    
    left = -(1.0+_mat[3][0]) / _mat[0][0];
    right = (1.0-_mat[3][0]) / _mat[0][0];

    bottom = -(1.0+_mat[3][1]) / _mat[1][1];
    top = (1.0-_mat[3][1]) / _mat[1][1];
    
    return true;
}            


void Matrix_implementation::makeFrustum(double left, double right,
                         double bottom, double top,
                         double zNear, double zFar)
{
    // note transpose of Matrix_implementation wr.t OpenGL documentation, since the OSG use post multiplication rather than pre.
    double A = (right+left)/(right-left);
    double B = (top+bottom)/(top-bottom);
    double C = -(zFar+zNear)/(zFar-zNear);
    double D = -2.0*zFar*zNear/(zFar-zNear);
    SET_ROW(0, 2.0*zNear/(right-left),                    0.0, 0.0,  0.0 )
    SET_ROW(1,                    0.0, 2.0*zNear/(top-bottom), 0.0,  0.0 )
    SET_ROW(2,                      A,                      B,   C, -1.0 )
    SET_ROW(3,                    0.0,                    0.0,   D,  0.0 )
}

bool Matrix_implementation::getFrustum(double& left, double& right,
                                       double& bottom, double& top,
                                       double& zNear, double& zFar) const
{
    if (_mat[0][3]!=0.0 || _mat[1][3]!=0.0 || _mat[2][3]!=-1.0 || _mat[3][3]!=0.0) return false;


    zNear = _mat[3][2] / (_mat[2][2]-1.0);
    zFar = _mat[3][2] / (1.0+_mat[2][2]);
    
    left = zNear * (_mat[2][0]-1.0) / _mat[0][0];
    right = zNear * (1.0+_mat[2][0]) / _mat[0][0];

    top = zNear * (1.0+_mat[2][1]) / _mat[1][1];
    bottom = zNear * (_mat[2][1]-1.0) / _mat[1][1];
    
    return true;
}                 


void Matrix_implementation::makePerspective(double fovy,double aspectRatio,
                                            double zNear, double zFar)
{
    // calculate the appropriate left, right etc.
    double tan_fovy = tan(DegreesToRadians(fovy*0.5));
    double right  =  tan_fovy * aspectRatio * zNear;
    double left   = -right;
    double top    =  tan_fovy * zNear;
    double bottom =  -top;
    makeFrustum(left,right,bottom,top,zNear,zFar);
}

bool Matrix_implementation::getPerspective(double& fovy,double& aspectRatio,
                                           double& zNear, double& zFar) const
{
    double right  =  0.0;
    double left   =  0.0;
    double top    =  0.0;
    double bottom =  0.0;
    if (getFrustum(left,right,bottom,top,zNear,zFar))
    {
        fovy = RadiansToDegrees(atan(top/zNear)-atan(bottom/zNear));
        aspectRatio = (right-left)/(top-bottom);
        return true;
    }
    return false;
}

void Matrix_implementation::makeLookAt(const Vec3d& eye,const Vec3d& center,const Vec3d& up)
{
    Vec3d f(center-eye);
    f.normalize();
    Vec3d s(f^up);
    s.normalize();
    Vec3d u(s^f);
    u.normalize();

    set(
        s[0],     u[0],     -f[0],     0.0,
        s[1],     u[1],     -f[1],     0.0,
        s[2],     u[2],     -f[2],     0.0,
        0.0,     0.0,     0.0,      1.0);

    preMult(Matrix_implementation::translate(-eye));
}


void Matrix_implementation::getLookAt(Vec3f& eye,Vec3f& center,Vec3f& up,value_type lookDistance) const
{
    Matrix_implementation inv;
    inv.invert(*this);
    eye = osg::Vec3f(0.0,0.0,0.0)*inv;
    up = transform3x3(*this,osg::Vec3f(0.0,1.0,0.0));
    center = transform3x3(*this,osg::Vec3f(0.0,0.0,-1));
    center.normalize();
    center = eye + center*lookDistance;
}

void Matrix_implementation::getLookAt(Vec3d& eye,Vec3d& center,Vec3d& up,value_type lookDistance) const
{
    Matrix_implementation inv;
    inv.invert(*this);
    eye = osg::Vec3d(0.0,0.0,0.0)*inv;
    up = transform3x3(*this,osg::Vec3d(0.0,1.0,0.0));
    center = transform3x3(*this,osg::Vec3d(0.0,0.0,-1));
    center.normalize();
    center = eye + center*lookDistance;
}

#undef SET_ROW