OSGMatrix.inl

Go to the documentation of this file.

/*---------------------------------------------------------------------------*\
 *                                OpenSG                                     *
 *                                                                           *
 *                                                                           *
 *             Copyright (C) 2000-2002 by the OpenSG Forum                   *
 *                                                                           *
 *                            www.opensg.org                                 *
 *                                                                           *
 *   contact: dirk@opensg.org, gerrit.voss@vossg.org, jbehr@zgdv.de          *
 *                                                                           *
\*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*\
 *                                License                                    *
 *                                                                           *
 * This library is free software; you can redistribute it and/or modify it   *
 * under the terms of the GNU Library General Public License as published    *
 * by the Free Software Foundation, version 2.                               *
 *                                                                           *
 * 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 GNU         *
 * Library General Public License for more details.                          *
 *                                                                           *
 * You should have received a copy of the GNU Library General Public         *
 * License along with this library; if not, write to the Free Software       *
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 *
 *                                                                           *
\*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*\
 *                                Changes                                    *
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *                                                                           *
\*---------------------------------------------------------------------------*/

#include <stdlib.h>
#include <stdio.h>

#include <iostream>
#include <iomanip>

#include <OSGLog.h>

OSG_BEGIN_NAMESPACE

#if defined(__hpux)
template<class ValueTypeT> 
const UInt32 TransformationMatrix<ValueTypeT>::JacobiRank;
#endif

template<class ValueTypeT>
TransformationMatrix<ValueTypeT> 
    TransformationMatrix<ValueTypeT>::_identityMatrix;

/*-------------------------------------------------------------------------*/
/*                            Class Get                                    */

template<class ValueTypeT> inline
const TransformationMatrix<ValueTypeT> &
    TransformationMatrix<ValueTypeT>::identity(void)
{
    return _identityMatrix;
}

/*-------------------------------------------------------------------------*/
/*                            Constructors                                 */

template<class ValueTypeT> inline
TransformationMatrix<ValueTypeT>::TransformationMatrix(void)
{
    for(UInt32 i = 0; i < 4; i++)
    {
        _matrix[i][i] = TypeTraits<ValueType>::getOneElement();
    }
}

template<class ValueTypeT> inline
TransformationMatrix<ValueTypeT>::TransformationMatrix(
    const TransformationMatrix &source)
{
    for(UInt32 i = 0; i < 4; i++)
    {
        _matrix[i] = source._matrix[i];
    }
}

template<class ValueTypeT> inline
TransformationMatrix<ValueTypeT>::TransformationMatrix(
    const VectorType3f &vector1,
    const VectorType3f &vector2,
    const VectorType3f &vector3)
{
    _matrix[0].setValue(vector1);
    _matrix[1].setValue(vector2);
    _matrix[2].setValue(vector3);
}

template<class ValueTypeT> inline
TransformationMatrix<ValueTypeT>::TransformationMatrix(
    const VectorType3f &vector1,
    const VectorType3f &vector2,
    const VectorType3f &vector3,
    const VectorType3f &vector4)
{
    _matrix[0].setValue(vector1);
    _matrix[1].setValue(vector2);
    _matrix[2].setValue(vector3);
    _matrix[3].setValue(vector4);
}

template<class ValueTypeT> inline
TransformationMatrix<ValueTypeT>::TransformationMatrix(
    const ValueTypeT rVal00,
    const ValueTypeT rVal10,
    const ValueTypeT rVal20,
    const ValueTypeT rVal30,

    const ValueTypeT rVal01,
    const ValueTypeT rVal11,
    const ValueTypeT rVal21,
    const ValueTypeT rVal31,

    const ValueTypeT rVal02,
    const ValueTypeT rVal12,
    const ValueTypeT rVal22,
    const ValueTypeT rVal32,

    const ValueTypeT rVal03,
    const ValueTypeT rVal13,
    const ValueTypeT rVal23,
    const ValueTypeT rVal33)
{
    _matrix[0].setValues(rVal00, rVal01, rVal02, rVal03);
    _matrix[1].setValues(rVal10, rVal11, rVal12, rVal13);
    _matrix[2].setValues(rVal20, rVal21, rVal22, rVal23);
    _matrix[3].setValues(rVal30, rVal31, rVal32, rVal33);
}

/*-------------------------------------------------------------------------*/
/*                             Destructor                                  */

template<class ValueTypeT> inline
TransformationMatrix<ValueTypeT>::~TransformationMatrix(void)
{
}

/*-------------------------------------------------------------------------*/
/*                                Set                                      */

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setIdentity(void)
{
    for(UInt32 i = 0; i < 4; i++)
    {
        _matrix[i].setNull();
        _matrix[i][i] = TypeTraits<ValueType>::getOneElement();
    }
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setValue(
    const TransformationMatrix &mat)
{
    for(UInt32 i = 0; i < 4; i++)
    {
        _matrix[i] = mat._matrix[i];
    }
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setValue(const VectorType3f &vector1,
                                                const VectorType3f &vector2,
                                                const VectorType3f &vector3)
{
    _matrix[0].setValue(vector1);
    _matrix[1].setValue(vector2);
    _matrix[2].setValue(vector3);
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setValue(const VectorType3f &vector1,
                                                const VectorType3f &vector2,
                                                const VectorType3f &vector3,
                                                const VectorType3f &vector4)
{
    _matrix[0].setValue(vector1);
    _matrix[1].setValue(vector2);
    _matrix[2].setValue(vector3);
    _matrix[3].setValue(vector4);
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setValue(const ValueTypeT rVal00,
                                                const ValueTypeT rVal10,
                                                const ValueTypeT rVal20,
                                                const ValueTypeT rVal30,
                                                
                                                const ValueTypeT rVal01,
                                                const ValueTypeT rVal11,
                                                const ValueTypeT rVal21,
                                                const ValueTypeT rVal31,
                                                
                                                const ValueTypeT rVal02,
                                                const ValueTypeT rVal12,
                                                const ValueTypeT rVal22,
                                                const ValueTypeT rVal32,
                                                
                                                const ValueTypeT rVal03,
                                                const ValueTypeT rVal13,
                                                const ValueTypeT rVal23,
                                                const ValueTypeT rVal33)
{
    _matrix[0].setValues(rVal00, rVal01, rVal02, rVal03);
    _matrix[1].setValues(rVal10, rVal11, rVal12, rVal13);
    _matrix[2].setValues(rVal20, rVal21, rVal22, rVal23);
    _matrix[3].setValues(rVal30, rVal31, rVal32, rVal33);
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setValueTransposed(
    const ValueTypeT rVal00,
    const ValueTypeT rVal01,
    const ValueTypeT rVal02,
    const ValueTypeT rVal03,

    const ValueTypeT rVal10,
    const ValueTypeT rVal11,
    const ValueTypeT rVal12,
    const ValueTypeT rVal13,

    const ValueTypeT rVal20,
    const ValueTypeT rVal21,
    const ValueTypeT rVal22,
    const ValueTypeT rVal23,

    const ValueTypeT rVal30,
    const ValueTypeT rVal31,
    const ValueTypeT rVal32,
    const ValueTypeT rVal33)
{
    _matrix[0].setValues(rVal00, rVal01, rVal02, rVal03);
    _matrix[1].setValues(rVal10, rVal11, rVal12, rVal13);
    _matrix[2].setValues(rVal20, rVal21, rVal22, rVal23);
    _matrix[3].setValues(rVal30, rVal31, rVal32, rVal33);
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setValue(const ValueTypeT *pMat, 
                                                      bool        bTransposed)
{
    const ValueTypeT *pTmpMat = pMat;

    if(bTransposed == true)
    {
        for(UInt32 i = 0; i < 4; i++)
        {
            _matrix[i].setValue(pTmpMat);

            pTmpMat += 4;
        }
    }
    else
    {
        for(UInt32 i = 0; i < 4; i++)
        {
            for(UInt32 j = 0; j < 4; j++)
            {
                _matrix[i][j] = pTmpMat[j * 4 + i];
            }
        }
    }
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setValue(const VectorType *pMat)
{
    for(UInt32 i = 0; i < 4; i++)
    {
        _matrix[i] = pMat[i];
    }
}

#ifndef WIN32


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setValue(const VectorType3f *pMat)
{
    for(UInt32 i = 0; i < 4; i++)
    {
        _matrix[i].setValue(pMat[i]);
    }
}

#endif

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setValue(const Char8 *str,
                                                      bool   bTransposed)
{
    UInt32 i;
    UInt32 numOfToken = 16;
    
    Char8 *c = const_cast<char*>(str);

    Char8 *tokenC = 0;
    Char8  token[256];

    ValueTypeT vec[16];
    
    if( (str  == NULL) ||
        (*str == '\0') )
    {
        setIdentity();
        return;
    }
    
    for(i = 0; i < numOfToken; c++)
    {
        switch (*c)
        {
            case '\0':
                if (tokenC)
                {
                    *tokenC   = 0;
                     vec[i++] = TypeTraits<ValueTypeT>::getFromString(token);

                }

                while (i < numOfToken)
                {
                    vec[i++] = TypeTraits<ValueTypeT>::getZeroElement();
                }

                break;
            case ' ':
            case '\t':
            case '\n':
            case ',':
                if (tokenC)
                {
                    *tokenC   = 0;
                     vec[i++] = TypeTraits<ValueTypeT>::getFromString(token);
                     tokenC   = 0;
                }
                break;
            default:
                if (!tokenC)
                {
                    tokenC = token;
                }
                *tokenC++ = *c;
                break;
        }
    }

    if(bTransposed == true)
    {
        setValueTransposed(vec[0],  vec[1],  vec[2],  vec[3],
                           vec[4],  vec[5],  vec[6],  vec[7],
                           vec[8],  vec[9],  vec[10], vec[11],
                           vec[12], vec[13], vec[14], vec[15]);
    }
    else
    {
        setValue(vec[0],  vec[1],  vec[2],  vec[3],
                 vec[4],  vec[5],  vec[6],  vec[7],
                 vec[8],  vec[9],  vec[10], vec[11],
                 vec[12], vec[13], vec[14], vec[15]);
    }
}

/*-------------------------------------------------------------------------*/
/*                                Get                                      */


template<class ValueTypeT> inline
ValueTypeT *TransformationMatrix<ValueTypeT>::getValues(void)
{
    return _matrix[0].getValues();
}

template<class ValueTypeT> inline
const ValueTypeT *TransformationMatrix<ValueTypeT>::getValues(void) const
{
    return _matrix[0].getValues();
}

/*-------------------------------------------------------------------------*/
/*                       Set Transformation                                */


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setScale(const ValueTypeT s)
{
    _matrix[0][0] = s;
    _matrix[1][1] = s;
    _matrix[2][2] = s;
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setScale(const ValueTypeT sx,
                                                const ValueTypeT sy, 
                                                const ValueTypeT sz)
{
    _matrix[0][0] = sx;
    _matrix[1][1] = sy;
    _matrix[2][2] = sz;
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setScale(const VectorType3f &s)
{
    _matrix[0][0] = s[0];
    _matrix[1][1] = s[1];
    _matrix[2][2] = s[2];
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setTranslate(const ValueTypeT tx,
                                                    const ValueTypeT ty,
                                                    const ValueTypeT tz)
{
    _matrix[3][0] = tx;
    _matrix[3][1] = ty;
    _matrix[3][2] = tz;
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setTranslate(const VectorType3f &t)
{
    _matrix[3].setValue(t);
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setTranslate(const PointType3f &t)
{
    _matrix[3].setValue(t);
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setRotate(const QuaternionType &q)
{
    q.getValuesOnly(*this);
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f &t)
{
    setIdentity();

    _matrix[3][0] = t[0];
    _matrix[3][1] = t[1];
    _matrix[3][2] = t[2];

}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setTransform(const QuaternionType &r)
{
    // Calculate the 4x4 rotation matrix
    r.getValue(*this);
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f   &t,
                                                    const QuaternionType &r)
{
    r.getValuesOnly(*this);

    // Calculate the resulting transformation matrix
    _matrix[0][3] = 0.0;
    _matrix[1][3] = 0.0;
    _matrix[2][3] = 0.0;

    _matrix[3][0] = t[0];
    _matrix[3][1] = t[1];
    _matrix[3][2] = t[2];
    _matrix[3][3] = 1.0;
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f   &t,
                                                    const QuaternionType &r,
                                                    const VectorType3f   &s)
{
    // Calculate the 3x3 rotation matrix
    r.getValuesOnly(*this);

    // Calculate the resulting transformation matrix
    _matrix[0][0] *= s[0]; _matrix[0][1] *= s[0]; _matrix[0][2] *=s[0];
    _matrix[1][0] *= s[1]; _matrix[1][1] *= s[1]; _matrix[1][2] *=s[1];
    _matrix[2][0] *= s[2]; _matrix[2][1] *= s[2]; _matrix[2][2] *=s[2];

    _matrix[0][3] = 0.0;
    _matrix[1][3] = 0.0;
    _matrix[2][3] = 0.0;

    _matrix[3][0] = t[0];
    _matrix[3][1] = t[1];
    _matrix[3][2] = t[2];
    _matrix[3][3] = 1.0;
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setTransform(const VectorType3f   &t,
                                                    const QuaternionType &r,
                                                    const VectorType3f   &s,
                                                    const QuaternionType &so)
{
    Matrix tmpMat1;
    Matrix tmpMat2;

    // Concatenate the rotations r and so
    QuaternionType rg(r);
    rg.mult(so);

    // Calculate the inverse of so
    QuaternionType soi(so);
    soi.invert();

    // Calculate the 3x3 rotation matrix
    rg. getValue(tmpMat1);
    soi.getValue(tmpMat2);

    // Calculate the resulting transformation matrix
    tmpMat1[0][0] *= s[0]; tmpMat1[0][1] *= s[0]; tmpMat1[0][2] *=s[0];
    tmpMat1[1][0] *= s[1]; tmpMat1[1][1] *= s[1]; tmpMat1[1][2] *=s[1];
    tmpMat1[2][0] *= s[2]; tmpMat1[2][1] *= s[2]; tmpMat1[2][2] *=s[2];

    _matrix[0][0] =
        tmpMat2[0][0] * tmpMat1[0][0] +
        tmpMat2[0][1] * tmpMat1[1][0] +
        tmpMat2[0][2] * tmpMat1[2][0];

    _matrix[0][1] =
        tmpMat2[0][0] * tmpMat1[0][1] +
        tmpMat2[0][1] * tmpMat1[1][1] +
        tmpMat2[0][2] * tmpMat1[2][1];

    _matrix[0][2] =
        tmpMat2[0][0] * tmpMat1[0][2] +
        tmpMat2[0][1] * tmpMat1[1][2] +
        tmpMat2[0][2] * tmpMat1[2][2];

    _matrix[0][3] = 0.0;


    _matrix[1][0] =
        tmpMat2[1][0] * tmpMat1[0][0] +
        tmpMat2[1][1] * tmpMat1[1][0] +
        tmpMat2[1][2] * tmpMat1[2][0];

    _matrix[1][1] =
        tmpMat2[1][0] * tmpMat1[0][1] +
        tmpMat2[1][1] * tmpMat1[1][1] +
        tmpMat2[1][2] * tmpMat1[2][1];

    _matrix[1][2] =
        tmpMat2[1][0] * tmpMat1[0][2] +
        tmpMat2[1][1] * tmpMat1[1][2] +
        tmpMat2[1][2] * tmpMat1[2][2];

    _matrix[1][3] = 0.0;


    _matrix[2][0] =
        tmpMat2[2][0] * tmpMat1[0][0] +
        tmpMat2[2][1] * tmpMat1[1][0] +
        tmpMat2[2][2] * tmpMat1[2][0];

    _matrix[2][1] =
        tmpMat2[2][0] * tmpMat1[0][1] +
        tmpMat2[2][1] * tmpMat1[1][1] +
        tmpMat2[2][2] * tmpMat1[2][1];

    _matrix[2][2] =
        tmpMat2[2][0] * tmpMat1[0][2] +
        tmpMat2[2][1] * tmpMat1[1][2] +
        tmpMat2[2][2] * tmpMat1[2][2];

    _matrix[2][3] = 0.0;

    _matrix[3][0] = t[0];
    _matrix[3][1] = t[1];
    _matrix[3][2] = t[2];
    _matrix[3][3] = 1.0;
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::setTransform(
    const VectorType3f   &translation,
    const QuaternionType &rotation,
    const VectorType3f   &scaleFactor,
    const QuaternionType &scaleOrientation,
    const VectorType3f   &center)
{
    Matrix tmpMat1;
    Matrix tmpMat2;

    // Concatenate the translations t and c
    VectorType3f tg(translation);
    tg += center;

    // Concatenate the rotations r and so
    QuaternionType rg(rotation);
    rg *= scaleOrientation;

    // Calculate the inverse of so
    QuaternionType soi(scaleOrientation);
    soi.invert();

    // Calculate the 3x3 rotation matrix
    rg. getValue(tmpMat1);
    soi.getValue(tmpMat2);

    // Calculate the resulting transformation matrix

    tmpMat1[0][0] *= scaleFactor[0];
    tmpMat1[0][1] *= scaleFactor[0];
    tmpMat1[0][2] *= scaleFactor[0];

    tmpMat1[1][0] *= scaleFactor[1];
    tmpMat1[1][1] *= scaleFactor[1];
    tmpMat1[1][2] *= scaleFactor[1];

    tmpMat1[2][0] *= scaleFactor[2];
    tmpMat1[2][1] *= scaleFactor[2];
    tmpMat1[2][2] *= scaleFactor[2];

    _matrix[0][0] =
        tmpMat2[0][0] * tmpMat1[0][0] +
        tmpMat2[0][1] * tmpMat1[1][0] +
        tmpMat2[0][2] * tmpMat1[2][0];

    _matrix[0][1] =
        tmpMat2[0][0] * tmpMat1[0][1] +
        tmpMat2[0][1] * tmpMat1[1][1] +
        tmpMat2[0][2] * tmpMat1[2][1];

    _matrix[0][2] =
        tmpMat2[0][0] * tmpMat1[0][2] +
        tmpMat2[0][1] * tmpMat1[1][2] +
        tmpMat2[0][2] * tmpMat1[2][2];

    _matrix[0][3] = 0.0;


    _matrix[1][0] =
        tmpMat2[1][0] * tmpMat1[0][0] +
        tmpMat2[1][1] * tmpMat1[1][0] +
        tmpMat2[1][2] * tmpMat1[2][0];

    _matrix[1][1] =
        tmpMat2[1][0] * tmpMat1[0][1] +
        tmpMat2[1][1] * tmpMat1[1][1] +
        tmpMat2[1][2] * tmpMat1[2][1];

    _matrix[1][2] =
        tmpMat2[1][0] * tmpMat1[0][2] +
        tmpMat2[1][1] * tmpMat1[1][2] +
        tmpMat2[1][2] * tmpMat1[2][2];

    _matrix[1][3] = 0.0;


    _matrix[2][0] =
        tmpMat2[2][0] * tmpMat1[0][0] +
        tmpMat2[2][1] * tmpMat1[1][0] +
        tmpMat2[2][2] * tmpMat1[2][0];

    _matrix[2][1] =
        tmpMat2[2][0] * tmpMat1[0][1] +
        tmpMat2[2][1] * tmpMat1[1][1] +
        tmpMat2[2][2] * tmpMat1[2][1];

    _matrix[2][2] =
        tmpMat2[2][0] * tmpMat1[0][2] +
        tmpMat2[2][1] * tmpMat1[1][2] +
        tmpMat2[2][2] * tmpMat1[2][2];

    _matrix[2][3] = 0.0;


    _matrix[3][0] =
        _matrix[0][0] * -center[0] +
        _matrix[1][0] * -center[1] +
        _matrix[2][0] * -center[2] + tg[0];

    _matrix[3][1] =
        _matrix[0][1] * -center[0] +
        _matrix[1][1] * -center[1] +
        _matrix[2][1] * -center[2] + tg[1];

    _matrix[3][2] =
        _matrix[0][2] * -center[0] +
        _matrix[1][2] * -center[1] +
        _matrix[2][2] * -center[2] + tg[2];

    _matrix[3][3] = 1.0;

}

/*-------------------------------------------------------------------------*/
/*                           Get Transform                                 */

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::getTransform(
          VectorType3f   &translation,
          QuaternionType &rotation,
          VectorType3f   &scaleFactor,
          QuaternionType &scaleOrientation,
    const VectorType3f   &center          ) const
{
    TransformationMatrix m;
    TransformationMatrix c;

    m.setTranslate(-center);

    m.mult(*this);

    c.setTranslate(center);

    m.mult(c);

    m.getTransform(translation, rotation, scaleFactor, scaleOrientation);
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::getTransform(
    VectorType3f   &translation,
    QuaternionType &rotation,
    VectorType3f   &scaleFactor,
    QuaternionType &scaleOrientation) const
{
    TransformationMatrix so;
    TransformationMatrix rot;
    TransformationMatrix proj;

    this->factor(so, scaleFactor, rot, translation, proj);
    
    so.transpose();
    scaleOrientation.setValue(so);

    // gives us transpose of correct answer.
    rotation.setValue(rot);

}

template<class ValueTypeT> inline
bool TransformationMatrix<ValueTypeT>::factor(TransformationMatrix &r,
                                              VectorType3f         &s,
                                              TransformationMatrix &u,
                                              VectorType3f         &t,
                                              TransformationMatrix &proj) const
{
    Real64               det;       
    Real64               det_sign;   
    Real64               scratch;
    Int32                i;
    Int32                j;
    Int32                junk;
    TransformationMatrix a;
    TransformationMatrix aT;
    TransformationMatrix rT;
    TransformationMatrix b;
    TransformationMatrix si;
    ValueTypeT           evalues [3];
    VectorType3f         evectors[3];
    
    a = *this;

    proj.setIdentity();

    scratch = 1.0;
    
    for (i = 0; i < 3; i++) 
    {
        for (j = 0; j < 3; j++) 
        {
            a._matrix[i][j] *= ValueTypeT(scratch);
        }

        t[i] = _matrix[3][i] * ValueTypeT(scratch);

        a._matrix[3][i] = a._matrix[i][3] = 0.0;
    }

    a._matrix[3][3] = 1.0;
    
    /* (3) Compute det A. If negative, set sign = -1, else sign = 1 */

    det      = a.det3();

    det_sign = (det < 0.0 ? -1.0 : 1.0);

    if(det_sign * det < 1e-12)
        return false;      // singular
    
    /* (4) B = A * A^  (here A^ means A transpose) */
    
    aT.transposeFrom(a);
    b = a;
    b.mult(aT);
    
    b.jacobi(evalues, evectors, junk);
    
    r.setValue(evectors[0][0], evectors[0][1], evectors[0][2], 0.0, 
               evectors[1][0], evectors[1][1], evectors[1][2], 0.0, 
               evectors[2][0], evectors[2][1], evectors[2][2], 0.0, 
                          0.0,            0.0,            0.0, 1.0);
    
    /* Compute s = sqrt(evalues), with sign. Set si = s-inverse */

    si.setIdentity();

    for(i = 0; i < 3; i++) 
    {
        s[i] = ValueTypeT(det_sign * osgsqrt(evalues[i]));

        si._matrix[i][i] = 1.0f / s[i];
    }
    
    /* (5) Compute U = RT S! R A. */

    rT.transposeFrom(r);
    u = r;
    u.mult(si);
    u.mult(rT);
    u.mult(a);
    
    return true;
}

/*---------------------------- transform objects ---------------------------*/

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multMatrixPnt(
    const PointType3f &src,
          PointType3f &dst) const
{
    dst.setValues((src[0] * _matrix[0][0] +
                   src[1] * _matrix[1][0] +
                   src[2] * _matrix[2][0] +
                            _matrix[3][0]),
                  (src[0] * _matrix[0][1] +
                   src[1] * _matrix[1][1] +
                   src[2] * _matrix[2][1] +
                            _matrix[3][1]),
                  (src[0] * _matrix[0][2] +
                   src[1] * _matrix[1][2] +
                   src[2] * _matrix[2][2] +
                            _matrix[3][2]));
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multMatrixPnt(PointType3f &pnt) const
{
    multMatrixPnt(pnt, pnt);
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multFullMatrixPnt(
    const PointType3f &src,
          PointType3f &dst) const
{
    ValueTypeT w =  src[0] * _matrix[0][3] +
                    src[1] * _matrix[1][3] +
                    src[2] * _matrix[2][3] +
                             _matrix[3][3];

    if( w == 0.0f )
    {
        SINFO << "multFullMatrixPnt: w == 0.0f!" << std::endl;

        dst.setValues(0, 0, 0);

        return;
    }

    //CHECK
    w = ValueTypeT(1.)/w;
    dst.setValues((src[0] * _matrix[0][0] +
                   src[1] * _matrix[1][0] +
                   src[2] * _matrix[2][0] +
                            _matrix[3][0]) * w,
                  (src[0] * _matrix[0][1] +
                   src[1] * _matrix[1][1] +
                   src[2] * _matrix[2][1] +
                            _matrix[3][1]) * w,
                  (src[0] * _matrix[0][2] +
                   src[1] * _matrix[1][2] +
                   src[2] * _matrix[2][2] +
                            _matrix[3][2]) * w);
}

 
template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multFullMatrixPnt(PointType3f &pnt)const
{
    multFullMatrixPnt(pnt, pnt);
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multMatrixVec(
    const VectorType3f &src,
          VectorType3f &dst) const
{
    dst.setValues((src[0] * _matrix[0][0] +
                   src[1] * _matrix[1][0] +
                   src[2] * _matrix[2][0]),
                  (src[0] * _matrix[0][1] +
                   src[1] * _matrix[1][1] +
                   src[2] * _matrix[2][1]),
                  (src[0] * _matrix[0][2] +
                   src[1] * _matrix[1][2] +
                   src[2] * _matrix[2][2]));
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multMatrixVec(VectorType3f &vec) const
{
    multMatrixVec(vec, vec);
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::mult(const PointType3f &src,
                                                  PointType3f &dest) const
{
    multMatrixPnt(src, dest);
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::mult(PointType3f &pnt) const
{
    multMatrixPnt(pnt, pnt);
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::mult(const VectorType3f &src,
                                                  VectorType3f &dest) const
{
    multMatrixVec(src, dest);
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::mult(VectorType3f &vec) const
{
    multMatrixVec(vec, vec);
}



template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multPntMatrix(
    const PointType3f &src,
          PointType3f &dst) const
{
    dst.setValues((src[0] * _matrix[0][0] +
                   src[1] * _matrix[0][1] +
                   src[2] * _matrix[0][2] +
                            _matrix[0][3]),
                  (src[0] * _matrix[1][0] +
                   src[1] * _matrix[1][1] +
                   src[2] * _matrix[1][2] +
                            _matrix[1][3]),
                  (src[0] * _matrix[2][0] +
                   src[1] * _matrix[2][1] +
                   src[2] * _matrix[2][2] +
                            _matrix[2][3]));
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multPntMatrix(PointType3f &pnt) const
{
    multPntMatrix(pnt, pnt);
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multPntFullMatrix(
    const PointType3f &src,
          PointType3f &dst) const
{
    ValueTypeT w =  src[0] * _matrix[3][0] +
                    src[1] * _matrix[3][1] +
                    src[2] * _matrix[3][2] +
                             _matrix[3][3];

    if( w == 0.0f )
    {
        SINFO << "multFullMatrixPnt: w == 0.0f!" << std::endl;

        dst.setValues(0, 0, 0);

        return;
    }

    w = 1./w;

    dst.setValues((src[0] * _matrix[0][0] +
                   src[1] * _matrix[0][1] +
                   src[2] * _matrix[0][2] +
                            _matrix[0][3]) * w,
                  (src[0] * _matrix[1][0] +
                   src[1] * _matrix[1][1] +
                   src[2] * _matrix[1][2] +
                            _matrix[1][3]) * w,
                  (src[0] * _matrix[2][0] +
                   src[1] * _matrix[2][1] +
                   src[2] * _matrix[2][2] +
                            _matrix[2][3]) * w);
}

 
template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multPntFullMatrix(PointType3f &pnt)const
{
    multPntFullMatrix(pnt, pnt);
}

template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multVecMatrix(
    const VectorType3f &src,
          VectorType3f &dst) const
{
    dst.setValues((src[0] * _matrix[0][0] +
                   src[1] * _matrix[0][1] +
                   src[2] * _matrix[0][2]),
                  (src[0] * _matrix[1][0] +
                   src[1] * _matrix[1][1] +
                   src[2] * _matrix[1][2]),
                  (src[0] * _matrix[2][0] +
                   src[1] * _matrix[2][1] +
                   src[2] * _matrix[2][2]));
}


template<class ValueTypeT> inline
void TransformationMatrix<ValueTypeT>::multVecMatrix(VectorType3f &vec) const
{
    multVecMatrix(vec, vec);
}

/*---------------------------- simple math ---------------------------------*/

template<class ValueTypeT> inline
bool TransformationMatrix<ValueTypeT>::equals(
    const TransformationMatrix &matrix,
    const ValueType             tolerance) const
{
    UInt32 i;
    bool returnValue = true;

    for(i = 0; i < 4; i++)
    {
        returnValue &= _matrix[i].equals(matrix._matrix[i], tolerance);

        if(returnValue == false)
            break;
    }

    return returnValue;
}


template<class ValueTypeT> inline
ValueTypeT TransformationMatrix<ValueTypeT>::det3(void) const
{
    return (_matrix[0][0] * _matrix[1][1] * _matrix[2][2] +
            _matrix[0][1] * _matrix[1][2] * _matrix[2][0] +
            _matrix[0][2] * _matrix[1][0] * _matrix[2][1] -
            _matrix[0][2] * _matrix[1][1] * _matrix[2][0] -
            _matrix[0][1] * _matrix[1][0] * _matrix[2][2] -
            _matrix[0][0] * _matrix[1][2] * _matrix[2][1]);
}


template<class ValueTypeT> inline
ValueTypeT TransformationMatrix<ValueTypeT>::det (void) const
{
    ValueTypeT
        a1, a2, a3, a4,
        b1, b2, b3, b4,
        c1, c2, c3, c4,
        d1, d2, d3, d4;

    a1 = _matrix[0][0];
    b1 = _matrix[1][0];
    c1 = _matrix[2][0];
    d1 = _matrix[3][0];

    a2 = _matrix[0][1];
    b2 = _matrix[1][1];
    c2 = _matrix[2][1];
    d2 = _matrix[3][1];

    a3 = _matrix[0][2];
    b3 = _matrix[1][2];
    c3 = _matrix[2][2];
    d3 = _matrix[3][2];

    a4 = _matrix[0][3];
    b4 = _matrix[1][3];
    c4 = _matrix[2][3];
    d4 = _matrix[3][3];

    return(   a1 * det3(b2, b3, b4, c2, c3, c4, d2, d3, d4)
            - b1 * det3(a2, a3, a4, c2, c3, c4, d2, d3, d4)
            + c1 * det3(a2, a3, a4, b2, b3, b4, d2, d3, d4)
            - d1 * det3(a2, a3, a4, b2, b3, b4, c2, c3, c4));

}

// This inverse stuff should be grouped in a better way :-). It's just a
// Cut&Paste section. I will have a look at it lateron (GV)

template<class ValueTypeT> inline
bool TransformationMatrix<ValueTypeT>::inverse(
    TransformationMatrix &result) const
{
    ValueTypeT rDet;

    ValueTypeT
        a1, a2, a3, a4,
        b1, b2, b3, b4,
        c1, c2, c3, c4,
        d1, d2, d3, d4;

    a1 = _matrix[0][0];
    b1 = _matrix[1][0];
    c1 = _matrix[2][0];
    d1 = _matrix[3][0];

    a2 = _matrix[0][1];
    b2 = _matrix[1][1];
    c2 =