BansheeEngine/CamelotUtility/Source/CmMatrix3.cpp

/*
-----------------------------------------------------------------------------
This source file is part of OGRE
    (Object-oriented Graphics Rendering Engine)
For the latest info, see http://www.ogre3d.org/

Copyright (c) 2000-2011 Torus Knot Software Ltd

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
-----------------------------------------------------------------------------
*/
#include "CmMatrix3.h"
#include "CmQuaternion.h"
#include "CmMath.h"

// Adapted from Matrix math by Wild Magic http://www.geometrictools.com/

namespace CamelotFramework
{
    const float Matrix3::EPSILON = 1e-06f;
    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);
    const float Matrix3::SVD_EPSILON = 1e-04f;
    const unsigned int Matrix3::SVD_MAX_ITERS = 32;
	const Matrix3::EulerAngleOrderData Matrix3::EA_LOOKUP[6] = 
		{ { 0, 1, 2, 1.0f}, { 0, 2, 1, -1.0f}, { 1, 0, 2, -1.0f},
		  { 1, 2, 0, 1.0f}, { 2, 0, 1,  1.0f}, { 2, 1, 0, -1.0f} };;


    Vector3 Matrix3::getColumn(size_t col) const
    {
        assert(0 <= col && col < 3);
        
		return Vector3(m[0][col],m[1][col], m[2][col]);
    }

    void Matrix3::setColumn(size_t col, const Vector3& vec)
    {
        assert(0 <= col && col < 3);

        m[0][col] = vec.x;
        m[1][col] = vec.y;
        m[2][col] = vec.z;
    }

    void Matrix3::fromAxes(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis)
    {
        setColumn(0, xAxis);
        setColumn(1, yAxis);
        setColumn(2, zAxis);
    }

    bool Matrix3::operator== (const Matrix3& rhs) const
    {
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
            {
                if (m[row][col] != rhs.m[row][col])
                    return false;
            }
        }

        return true;
    }

	bool Matrix3::operator!= (const Matrix3& rhs) const
	{
		return !operator==(rhs);
	}

    Matrix3 Matrix3::operator+ (const Matrix3& rhs) const
    {
        Matrix3 sum;
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
            {
                sum.m[row][col] = m[row][col] + rhs.m[row][col];
            }
        }

        return sum;
    }

    Matrix3 Matrix3::operator- (const Matrix3& rhs) const
    {
        Matrix3 diff;
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
            {
                diff.m[row][col] = m[row][col] -
                    rhs.m[row][col];
            }
        }

        return diff;
    }

    Matrix3 Matrix3::operator* (const Matrix3& rhs) const
    {
        Matrix3 prod;
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
            {
                prod.m[row][col] = m[row][0]*rhs.m[0][col] +
                    m[row][1]*rhs.m[1][col] + m[row][2]*rhs.m[2][col];
            }
        }

        return prod;
    }

    Matrix3 Matrix3::operator- () const
    {
        Matrix3 neg;
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
                neg[row][col] = -m[row][col];
        }

        return neg;
    }

    Matrix3 Matrix3::operator* (float rhs) const
    {
        Matrix3 prod;
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
                prod[row][col] = rhs*m[row][col];
        }

        return prod;
    }

    Matrix3 operator* (float lhs, const Matrix3& rhs)
    {
        Matrix3 prod;
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
                prod[row][col] = lhs*rhs.m[row][col];
        }

        return prod;
    }

	Vector3 Matrix3::transform(const Vector3& vec) const
	{
		Vector3 prod;
		for (size_t row = 0; row < 3; row++)
		{
			prod[row] =
				m[row][0]*vec[0] +
				m[row][1]*vec[1] +
				m[row][2]*vec[2];
		}

		return prod;
	}

    Matrix3 Matrix3::transpose () const
    {
        Matrix3 matTranspose;
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
                matTranspose[row][col] = m[col][row];
        }

        return matTranspose;
    }

    bool Matrix3::inverse (Matrix3& matInv, float tolerance) const
    {
        matInv[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];
        matInv[0][1] = m[0][2]*m[2][1] - m[0][1]*m[2][2];
        matInv[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];
        matInv[1][0] = m[1][2]*m[2][0] - m[1][0]*m[2][2];
        matInv[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];
        matInv[1][2] = m[0][2]*m[1][0] - m[0][0]*m[1][2];
        matInv[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];
        matInv[2][1] = m[0][1]*m[2][0] - m[0][0]*m[2][1];
        matInv[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0];

        float det = m[0][0]*matInv[0][0] + m[0][1]*matInv[1][0] + m[0][2]*matInv[2][0];

        if (Math::abs(det) <= tolerance)
            return false;

        float invDet = 1.0f/det;
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
                matInv[row][col] *= invDet;
        }

        return true;
    }

    Matrix3 Matrix3::inverse (float tolerance) const
    {
        Matrix3 matInv = Matrix3::ZERO;
        inverse(matInv, tolerance);
        return matInv;
    }

    float Matrix3::determinant() const
    {
        float cofactor00 = m[1][1]*m[2][2] - m[1][2]*m[2][1];
        float cofactor10 = m[1][2]*m[2][0] - m[1][0]*m[2][2];
        float cofactor20 = m[1][0]*m[2][1] - m[1][1]*m[2][0];

        float det = m[0][0]*cofactor00 + m[0][1]*cofactor10 + m[0][2]*cofactor20;

        return det;
    }

    void Matrix3::bidiagonalize (Matrix3& matA, Matrix3& matL, Matrix3& matR)
    {
        float v[3], w[3];
        float length, sign, t1, invT1, t2;
        bool bIdentity;

        // Map first column to (*,0,0)
        length = Math::sqrt(matA[0][0]*matA[0][0] + matA[1][0]*matA[1][0] + matA[2][0]*matA[2][0]);
        if (length > 0.0f)
        {
            sign = (matA[0][0] > 0.0f ? 1.0f : -1.0f);
            t1 = matA[0][0] + sign*length;
            invT1 = 1.0f/t1;
            v[1] = matA[1][0]*invT1;
            v[2] = matA[2][0]*invT1;

            t2 = -2.0f/(1.0f+v[1]*v[1]+v[2]*v[2]);
            w[0] = t2*(matA[0][0]+matA[1][0]*v[1]+matA[2][0]*v[2]);
            w[1] = t2*(matA[0][1]+matA[1][1]*v[1]+matA[2][1]*v[2]);
            w[2] = t2*(matA[0][2]+matA[1][2]*v[1]+matA[2][2]*v[2]);
            matA[0][0] += w[0];
            matA[0][1] += w[1];
            matA[0][2] += w[2];
            matA[1][1] += v[1]*w[1];
            matA[1][2] += v[1]*w[2];
            matA[2][1] += v[2]*w[1];
            matA[2][2] += v[2]*w[2];

            matL[0][0] = 1.0f+t2;
            matL[0][1] = matL[1][0] = t2*v[1];
            matL[0][2] = matL[2][0] = t2*v[2];
            matL[1][1] = 1.0f+t2*v[1]*v[1];
            matL[1][2] = matL[2][1] = t2*v[1]*v[2];
            matL[2][2] = 1.0f+t2*v[2]*v[2];
            bIdentity = false;
        }
        else
        {
            matL = Matrix3::IDENTITY;
            bIdentity = true;
        }

        // Map first row to (*,*,0)
        length = Math::sqrt(matA[0][1]*matA[0][1]+matA[0][2]*matA[0][2]);
        if ( length > 0.0 )
        {
            sign = (matA[0][1] > 0.0f ? 1.0f : -1.0f);
            t1 = matA[0][1] + sign*length;
            v[2] = matA[0][2]/t1;

            t2 = -2.0f/(1.0f+v[2]*v[2]);
            w[0] = t2*(matA[0][1]+matA[0][2]*v[2]);
            w[1] = t2*(matA[1][1]+matA[1][2]*v[2]);
            w[2] = t2*(matA[2][1]+matA[2][2]*v[2]);
            matA[0][1] += w[0];
            matA[1][1] += w[1];
            matA[1][2] += w[1]*v[2];
            matA[2][1] += w[2];
            matA[2][2] += w[2]*v[2];

            matR[0][0] = 1.0;
            matR[0][1] = matR[1][0] = 0.0;
            matR[0][2] = matR[2][0] = 0.0;
            matR[1][1] = 1.0f+t2;
            matR[1][2] = matR[2][1] = t2*v[2];
            matR[2][2] = 1.0f+t2*v[2]*v[2];
        }
        else
        {
            matR = Matrix3::IDENTITY;
        }

        // Map second column to (*,*,0)
        length = Math::sqrt(matA[1][1]*matA[1][1]+matA[2][1]*matA[2][1]);
        if ( length > 0.0 )
        {
            sign = (matA[1][1] > 0.0f ? 1.0f : -1.0f);
            t1 = matA[1][1] + sign*length;
            v[2] = matA[2][1]/t1;

            t2 = -2.0f/(1.0f+v[2]*v[2]);
            w[1] = t2*(matA[1][1]+matA[2][1]*v[2]);
            w[2] = t2*(matA[1][2]+matA[2][2]*v[2]);
            matA[1][1] += w[1];
            matA[1][2] += w[2];
            matA[2][2] += v[2]*w[2];

            float a = 1.0f+t2;
            float b = t2*v[2];
            float c = 1.0f+b*v[2];

            if (bIdentity)
            {
                matL[0][0] = 1.0;
                matL[0][1] = matL[1][0] = 0.0;
                matL[0][2] = matL[2][0] = 0.0;
                matL[1][1] = a;
                matL[1][2] = matL[2][1] = b;
                matL[2][2] = c;
            }
            else
            {
                for (int row = 0; row < 3; row++)
                {
                    float tmp0 = matL[row][1];
                    float tmp1 = matL[row][2];
                    matL[row][1] = a*tmp0+b*tmp1;
                    matL[row][2] = b*tmp0+c*tmp1;
                }
            }
        }
    }

    void Matrix3::golubKahanStep (Matrix3& matA, Matrix3& matL, Matrix3& matR)
    {
        float f11 = matA[0][1]*matA[0][1]+matA[1][1]*matA[1][1];
        float t22 = matA[1][2]*matA[1][2]+matA[2][2]*matA[2][2];
        float t12 = matA[1][1]*matA[1][2];
        float trace = f11+t22;
        float diff = f11-t22;
        float discr = Math::sqrt(diff*diff+4.0f*t12*t12);
        float root1 = 0.5f*(trace+discr);
        float root2 = 0.5f*(trace-discr);

        // Adjust right
        float y = matA[0][0] - (Math::abs(root1-t22) <= Math::abs(root2-t22) ? root1 : root2);
        float z = matA[0][1];
		float invLength = Math::invSqrt(y*y+z*z);
        float sin = z*invLength;
        float cos = -y*invLength;

        float tmp0 = matA[0][0];
        float tmp1 = matA[0][1];
        matA[0][0] = cos*tmp0-sin*tmp1;
        matA[0][1] = sin*tmp0+cos*tmp1;
        matA[1][0] = -sin*matA[1][1];
        matA[1][1] *= cos;

        size_t row;
        for (row = 0; row < 3; row++)
        {
            tmp0 = matR[0][row];
            tmp1 = matR[1][row];
            matR[0][row] = cos*tmp0-sin*tmp1;
            matR[1][row] = sin*tmp0+cos*tmp1;
        }

        // Adjust left
        y = matA[0][0];
        z = matA[1][0];
        invLength = Math::invSqrt(y*y+z*z);
        sin = z*invLength;
        cos = -y*invLength;

        matA[0][0] = cos*matA[0][0]-sin*matA[1][0];
        tmp0 = matA[0][1];
        tmp1 = matA[1][1];
        matA[0][1] = cos*tmp0-sin*tmp1;
        matA[1][1] = sin*tmp0+cos*tmp1;
        matA[0][2] = -sin*matA[1][2];
        matA[1][2] *= cos;

        size_t col;
        for (col = 0; col < 3; col++)
        {
            tmp0 = matL[col][0];
            tmp1 = matL[col][1];
            matL[col][0] = cos*tmp0-sin*tmp1;
            matL[col][1] = sin*tmp0+cos*tmp1;
        }

        // Adjust right
        y = matA[0][1];
        z = matA[0][2];
        invLength = Math::invSqrt(y*y+z*z);
        sin = z*invLength;
        cos = -y*invLength;

        matA[0][1] = cos*matA[0][1]-sin*matA[0][2];
        tmp0 = matA[1][1];
        tmp1 = matA[1][2];
        matA[1][1] = cos*tmp0-sin*tmp1;
        matA[1][2] = sin*tmp0+cos*tmp1;
        matA[2][1] = -sin*matA[2][2];
        matA[2][2] *= cos;

        for (row = 0; row < 3; row++)
        {
            tmp0 = matR[1][row];
            tmp1 = matR[2][row];
            matR[1][row] = cos*tmp0-sin*tmp1;
            matR[2][row] = sin*tmp0+cos*tmp1;
        }

        // Adjust left
        y = matA[1][1];
        z = matA[2][1];
        invLength = Math::invSqrt(y*y+z*z);
        sin = z*invLength;
        cos = -y*invLength;

        matA[1][1] = cos*matA[1][1]-sin*matA[2][1];
        tmp0 = matA[1][2];
        tmp1 = matA[2][2];
        matA[1][2] = cos*tmp0-sin*tmp1;
        matA[2][2] = sin*tmp0+cos*tmp1;

        for (col = 0; col < 3; col++)
        {
            tmp0 = matL[col][1];
            tmp1 = matL[col][2];
            matL[col][1] = cos*tmp0-sin*tmp1;
            matL[col][2] = sin*tmp0+cos*tmp1;
        }
    }

    void Matrix3::singularValueDecomposition(Matrix3& matL, Vector3& matS, Matrix3& matR) const
    {
		size_t row, col;

        Matrix3 mat = *this;
        bidiagonalize(mat, matL, matR);

        for (unsigned int i = 0; i < SVD_MAX_ITERS; i++)
        {
            float tmp, tmp0, tmp1;
            float sin0, cos0, tan0;
            float sin1, cos1, tan1;

            bool test1 = (Math::abs(mat[0][1]) <= SVD_EPSILON*(Math::abs(mat[0][0])+Math::abs(mat[1][1])));
            bool test2 = (Math::abs(mat[1][2]) <= SVD_EPSILON*(Math::abs(mat[1][1])+Math::abs(mat[2][2])));

            if (test1)
            {
                if (test2)
                {
                    matS[0] = mat[0][0];
                    matS[1] = mat[1][1];
                    matS[2] = mat[2][2];
                    break;
                }
                else
                {
                    // 2x2 closed form factorization
                    tmp = (mat[1][1]*mat[1][1] - mat[2][2]*mat[2][2] + mat[1][2]*mat[1][2])/(mat[1][2]*mat[2][2]);
                    tan0 = 0.5f*(tmp+Math::sqrt(tmp*tmp + 4.0f));
                    cos0 = Math::invSqrt(1.0f+tan0*tan0);
                    sin0 = tan0*cos0;

                    for (col = 0; col < 3; col++)
                    {
                        tmp0 = matL[col][1];
                        tmp1 = matL[col][2];
                        matL[col][1] = cos0*tmp0-sin0*tmp1;
                        matL[col][2] = sin0*tmp0+cos0*tmp1;
                    }

                    tan1 = (mat[1][2]-mat[2][2]*tan0)/mat[1][1];
                    cos1 = Math::invSqrt(1.0f+tan1*tan1);
                    sin1 = -tan1*cos1;

                    for (row = 0; row < 3; row++)
                    {
                        tmp0 = matR[1][row];
                        tmp1 = matR[2][row];
                        matR[1][row] = cos1*tmp0-sin1*tmp1;
                        matR[2][row] = sin1*tmp0+cos1*tmp1;
                    }

                    matS[0] = mat[0][0];
                    matS[1] = cos0*cos1*mat[1][1] - sin1*(cos0*mat[1][2]-sin0*mat[2][2]);
                    matS[2] = sin0*sin1*mat[1][1] + cos1*(sin0*mat[1][2]+cos0*mat[2][2]);
                    break;
                }
            }
            else
            {
                if (test2)
                {
                    // 2x2 closed form factorization
                    tmp = (mat[0][0]*mat[0][0] + mat[1][1]*mat[1][1] - mat[0][1]*mat[0][1])/(mat[0][1]*mat[1][1]);
                    tan0 = 0.5f*(-tmp+Math::sqrt(tmp*tmp + 4.0f));
                    cos0 = Math::invSqrt(1.0f+tan0*tan0);
                    sin0 = tan0*cos0;

                    for (col = 0; col < 3; col++)
                    {
                        tmp0 = matL[col][0];
                        tmp1 = matL[col][1];
                        matL[col][0] = cos0*tmp0-sin0*tmp1;
                        matL[col][1] = sin0*tmp0+cos0*tmp1;
                    }

                    tan1 = (mat[0][1]-mat[1][1]*tan0)/mat[0][0];
                    cos1 = Math::invSqrt(1.0f+tan1*tan1);
                    sin1 = -tan1*cos1;

                    for (row = 0; row < 3; row++)
                    {
                        tmp0 = matR[0][row];
                        tmp1 = matR[1][row];
                        matR[0][row] = cos1*tmp0-sin1*tmp1;
                        matR[1][row] = sin1*tmp0+cos1*tmp1;
                    }

                    matS[0] = cos0*cos1*mat[0][0] - sin1*(cos0*mat[0][1]-sin0*mat[1][1]);
                    matS[1] = sin0*sin1*mat[0][0] + cos1*(sin0*mat[0][1]+cos0*mat[1][1]);
                    matS[2] = mat[2][2];
                    break;
                }
                else
                {
                    golubKahanStep(mat, matL, matR);
                }
            }
        }

        // Positize diagonal
        for (row = 0; row < 3; row++)
        {
            if ( matS[row] < 0.0 )
            {
                matS[row] = -matS[row];
                for (col = 0; col < 3; col++)
                    matR[row][col] = -matR[row][col];
            }
        }
    }

    void Matrix3::orthonormalize()
    {
        // Compute q0
        float invLength = Math::invSqrt(m[0][0]*m[0][0]+ m[1][0]*m[1][0] + m[2][0]*m[2][0]);

        m[0][0] *= invLength;
        m[1][0] *= invLength;
        m[2][0] *= invLength;

        // Compute q1
        float dot0 = m[0][0]*m[0][1] + m[1][0]*m[1][1] + m[2][0]*m[2][1];

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

        invLength = Math::invSqrt(m[0][1]*m[0][1] + m[1][1]*m[1][1] + m[2][1]*m[2][1]);

        m[0][1] *= invLength;
        m[1][1] *= invLength;
        m[2][1] *= invLength;

        // Compute q2
        float dot1 = m[0][1]*m[0][2] + m[1][1]*m[1][2] + m[2][1]*m[2][2];
        dot0 = m[0][0]*m[0][2] + m[1][0]*m[1][2] + m[2][0]*m[2][2];

        m[0][2] -= dot0*m[0][0] + dot1*m[0][1];
        m[1][2] -= dot0*m[1][0] + dot1*m[1][1];
        m[2][2] -= dot0*m[2][0] + dot1*m[2][1];

        invLength = Math::invSqrt(m[0][2]*m[0][2] + m[1][2]*m[1][2] + m[2][2]*m[2][2]);

        m[0][2] *= invLength;
        m[1][2] *= invLength;
        m[2][2] *= invLength;
    }

    void Matrix3::QDUDecomposition(Matrix3& matQ, Vector3& vecD, Vector3& vecU) const
    {
        // Build orthogonal matrix Q
        float invLength = Math::invSqrt(m[0][0]*m[0][0] + m[1][0]*m[1][0] + m[2][0]*m[2][0]);
        matQ[0][0] = m[0][0]*invLength;
        matQ[1][0] = m[1][0]*invLength;
        matQ[2][0] = m[2][0]*invLength;

        float dot = matQ[0][0]*m[0][1] + matQ[1][0]*m[1][1] + matQ[2][0]*m[2][1];
        matQ[0][1] = m[0][1]-dot*matQ[0][0];
        matQ[1][1] = m[1][1]-dot*matQ[1][0];
        matQ[2][1] = m[2][1]-dot*matQ[2][0];

        invLength = Math::invSqrt(matQ[0][1]*matQ[0][1] + matQ[1][1]*matQ[1][1] + matQ[2][1]*matQ[2][1]);
        matQ[0][1] *= invLength;
        matQ[1][1] *= invLength;
        matQ[2][1] *= invLength;

        dot = matQ[0][0]*m[0][2] + matQ[1][0]*m[1][2] + matQ[2][0]*m[2][2];
        matQ[0][2] = m[0][2]-dot*matQ[0][0];
        matQ[1][2] = m[1][2]-dot*matQ[1][0];
        matQ[2][2] = m[2][2]-dot*matQ[2][0];

        dot = matQ[0][1]*m[0][2] + matQ[1][1]*m[1][2] + matQ[2][1]*m[2][2];
        matQ[0][2] -= dot*matQ[0][1];
        matQ[1][2] -= dot*matQ[1][1];
        matQ[2][2] -= dot*matQ[2][1];

        invLength = Math::invSqrt(matQ[0][2]*matQ[0][2] + matQ[1][2]*matQ[1][2] + matQ[2][2]*matQ[2][2]);
        matQ[0][2] *= invLength;
        matQ[1][2] *= invLength;
        matQ[2][2] *= invLength;

        // Guarantee that orthogonal matrix has determinant 1 (no reflections)
        float fDet = matQ[0][0]*matQ[1][1]*matQ[2][2] + matQ[0][1]*matQ[1][2]*matQ[2][0] +
            matQ[0][2]*matQ[1][0]*matQ[2][1] - matQ[0][2]*matQ[1][1]*matQ[2][0] -
            matQ[0][1]*matQ[1][0]*matQ[2][2] - matQ[0][0]*matQ[1][2]*matQ[2][1];

        if (fDet < 0.0f)
        {
            for (size_t row = 0; row < 3; row++)
                for (size_t col = 0; col < 3; col++)
                    matQ[row][col] = -matQ[row][col];
        }

        // Build "right" matrix R
        Matrix3 matRight;
        matRight[0][0] = matQ[0][0]*m[0][0] + matQ[1][0]*m[1][0] +
            matQ[2][0]*m[2][0];
        matRight[0][1] = matQ[0][0]*m[0][1] + matQ[1][0]*m[1][1] +
            matQ[2][0]*m[2][1];
        matRight[1][1] = matQ[0][1]*m[0][1] + matQ[1][1]*m[1][1] +
            matQ[2][1]*m[2][1];
        matRight[0][2] = matQ[0][0]*m[0][2] + matQ[1][0]*m[1][2] +
            matQ[2][0]*m[2][2];
        matRight[1][2] = matQ[0][1]*m[0][2] + matQ[1][1]*m[1][2] +
            matQ[2][1]*m[2][2];
        matRight[2][2] = matQ[0][2]*m[0][2] + matQ[1][2]*m[1][2] +
            matQ[2][2]*m[2][2];

        // The scaling component
        vecD[0] = matRight[0][0];
        vecD[1] = matRight[1][1];
        vecD[2] = matRight[2][2];

        // The shear component
        float invD0 = 1.0f/vecD[0];
        vecU[0] = matRight[0][1]*invD0;
        vecU[1] = matRight[0][2]*invD0;
        vecU[2] = matRight[1][2]/vecD[1];
    }

    void Matrix3::toAxisAngle(Vector3& axis, Radian& radians) const
    {
        float trace = m[0][0] + m[1][1] + m[2][2];
        float cos = 0.5f*(trace-1.0f);
        radians = Math::acos(cos);  // In [0, PI]

        if (radians > Radian(0.0f))
        {
            if (radians < Radian(Math::PI))
            {
                axis.x = m[2][1]-m[1][2];
                axis.y = m[0][2]-m[2][0];
                axis.z = m[1][0]-m[0][1];
                axis.normalize();
            }
            else
            {
                // Angle is PI
                float fHalfInverse;
                if (m[0][0] >= m[1][1])
                {
                    // r00 >= r11
                    if (m[0][0] >= m[2][2])
                    {
                        // r00 is maximum diagonal term
                        axis.x = 0.5f*Math::sqrt(m[0][0] - m[1][1] - m[2][2] + 1.0f);
                        fHalfInverse = 0.5f/axis.x;
                        axis.y = fHalfInverse*m[0][1];
                        axis.z = fHalfInverse*m[0][2];
                    }
                    else
                    {
                        // r22 is maximum diagonal term
                        axis.z = 0.5f*Math::sqrt(m[2][2] - m[0][0] - m[1][1] + 1.0f);
                        fHalfInverse = 0.5f/axis.z;
                        axis.x = fHalfInverse*m[0][2];
                        axis.y = fHalfInverse*m[1][2];
                    }
                }
                else
                {
                    // r11 > r00
                    if ( m[1][1] >= m[2][2] )
                    {
                        // r11 is maximum diagonal term
                        axis.y = 0.5f*Math::sqrt(m[1][1] - m[0][0] - m[2][2] + 1.0f);
                        fHalfInverse  = 0.5f/axis.y;
                        axis.x = fHalfInverse*m[0][1];
                        axis.z = fHalfInverse*m[1][2];
                    }
                    else
                    {
                        // r22 is maximum diagonal term
                        axis.z = 0.5f*Math::sqrt(m[2][2] - m[0][0] - m[1][1] + 1.0f);
                        fHalfInverse = 0.5f/axis.z;
                        axis.x = fHalfInverse*m[0][2];
                        axis.y = fHalfInverse*m[1][2];
                    }
                }
            }
        }
        else
        {
            // The angle is 0 and the matrix is the identity.  Any axis will
            // work, so just use the x-axis.
            axis.x = 1.0f;
            axis.y = 0.0f;
            axis.z = 0.0f;
        }
    }

    void Matrix3::fromAxisAngle(const Vector3& axis, const Radian& angle)
    {
        float cos = Math::cos(angle);
        float sin = Math::sin(angle);
        float oneMinusCos = 1.0f-cos;
        float x2 = axis.x*axis.x;
        float y2 = axis.y*axis.y;
        float z2 = axis.z*axis.z;
        float xym = axis.x*axis.y*oneMinusCos;
        float xzm = axis.x*axis.z*oneMinusCos;
        float yzm = axis.y*axis.z*oneMinusCos;
        float xSin = axis.x*sin;
        float ySin = axis.y*sin;
        float zSin = axis.z*sin;

        m[0][0] = x2*oneMinusCos+cos;
        m[0][1] = xym-zSin;
        m[0][2] = xzm+ySin;
        m[1][0] = xym+zSin;
        m[1][1] = y2*oneMinusCos+cos;
        m[1][2] = yzm-xSin;
        m[2][0] = xzm-ySin;
        m[2][1] = yzm+xSin;
        m[2][2] = z2*oneMinusCos+cos;
    }

	void Matrix3::toQuaternion(Quaternion& quat) const
	{
		quat.fromRotationMatrix(*this);
	}

    void Matrix3::fromQuaternion(const Quaternion& quat)
	{
		quat.toRotationMatrix(*this);
	}

    bool Matrix3::toEulerAngles(Radian& xAngle, Radian& yAngle, Radian& zAngle) const
    {
        xAngle = Radian(Math::asin(m[0][2]));
        if (xAngle < Radian(Math::HALF_PI))
        {
            if (xAngle > Radian(-Math::HALF_PI))
            {
                yAngle = Math::atan2(-m[1][2], m[2][2]);
                zAngle = Math::atan2(-m[0][1], m[0][0]);

                return true;
            }
            else
            {
                // WARNING.  Not a unique solution.
                Radian angle = Math::atan2(m[1][0],m[1][1]);
                zAngle = Radian(0.0f);  // Any angle works
                yAngle = zAngle - angle;

                return false;
            }
        }
        else
        {
            // WARNING.  Not a unique solution.
            Radian angle = Math::atan2(m[1][0],m[1][1]);
            zAngle = Radian(0.0f);  // Any angle works
            yAngle = angle - zAngle;

            return false;
        }
    }

	bool Matrix3::toEulerAngles(Radian& xAngle, Radian& yAngle, Radian& zAngle, EulerAngleOrder order) const
	{
		const EulerAngleOrderData& l = EA_LOOKUP[(int)order];

		xAngle = Radian(Math::asin(l.sign * m[l.a][l.c]));
		if (xAngle < Radian(Math::HALF_PI))
		{
			if (xAngle > Radian(-Math::HALF_PI))
			{
				yAngle = Math::atan2(-l.sign * m[l.b][l.c], m[l.c][l.c]);
				zAngle = Math::atan2(-l.sign * m[l.a][l.b], m[l.a][l.a]);

				return true;
			}
			else
			{
				// WARNING.  Not a unique solution.
				Radian angle = Math::atan2(l.sign * m[l.b][l.a], m[l.b][l.b]);
				zAngle = Radian(0.0f);  // Any angle works
				yAngle = zAngle - angle;

				return false;
			}
		}
		else
		{
			// WARNING.  Not a unique solution.
			Radian angle = Math::atan2(l.sign * m[l.b][l.a], m[l.b][l.b]);
			zAngle = Radian(0.0f);  // Any angle works
			yAngle = angle - zAngle;

			return false;
		}
	}

    void Matrix3::fromEulerAngles(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle)
    {
        float cos, sin;

        cos = Math::cos(yAngle);
        sin = Math::sin(yAngle);
        Matrix3 xMat(1.0f, 0.0f, 0.0f, 0.0f, cos, -sin, 0.0f, sin, cos);

        cos = Math::cos(xAngle);
        sin = Math::sin(xAngle);
        Matrix3 yMat(cos, 0.0f, sin, 0.0f, 1.0f, 0.0f, -sin, 0.0f, cos);

        cos = Math::cos(zAngle);
        sin = Math::sin(zAngle);
        Matrix3 zMat(cos,-sin, 0.0f, sin, cos, 0.0f, 0.0f, 0.0f, 1.0f);

        *this = xMat*(yMat*zMat);
    }

	void Matrix3::fromEulerAngles(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle, EulerAngleOrder order)
	{
		const EulerAngleOrderData& l = EA_LOOKUP[(int)order];

		Matrix3 mats[3];
		float cos, sin;

		cos = Math::cos(yAngle);
		sin = Math::sin(yAngle);
		mats[0] = Matrix3(1.0f, 0.0f, 0.0f, 0.0f, cos, -sin, 0.0f, sin, cos);

		cos = Math::cos(xAngle);
		sin = Math::sin(xAngle);
		mats[1] = Matrix3(cos, 0.0f, sin, 0.0f, 1.0f, 0.0f, -sin, 0.0f, cos);

		cos = Math::cos(zAngle);
		sin = Math::sin(zAngle);
		mats[2] = Matrix3(cos,-sin, 0.0f, sin, cos, 0.0f, 0.0f, 0.0f, 1.0f);
	
		*this = mats[l.a]*(mats[l.b]*mats[l.c]);
	}

    void Matrix3::tridiagonal(float diag[3], float subDiag[3])
    {
        // Householder reduction T = Q^t M Q
        //   Input:
        //     mat, symmetric 3x3 matrix M
        //   Output:
        //     mat, orthogonal matrix Q
        //     diag, diagonal entries of T
        //     subd, subdiagonal entries of T (T is symmetric)

        float fA = m[0][0];
        float fB = m[0][1];
        float fC = m[0][2];
        float fD = m[1][1];
        float fE = m[1][2];
        float fF = m[2][2];

        diag[0] = fA;
        subDiag[2] = 0.0;
        if (Math::abs(fC) >= EPSILON)
        {
            float length = Math::sqrt(fB*fB+fC*fC);
            float invLength = 1.0f/length;
            fB *= invLength;
            fC *= invLength;
            float fQ = 2.0f*fB*fE+fC*(fF-fD);
            diag[1] = fD+fC*fQ;
            diag[2] = fF-fC*fQ;
            subDiag[0] = length;
            subDiag[1] = fE-fB*fQ;
            m[0][0] = 1.0;
            m[0][1] = 0.0;
            m[0][2] = 0.0;
            m[1][0] = 0.0;
            m[1][1] = fB;
            m[1][2] = fC;
            m[2][0] = 0.0;
            m[2][1] = fC;
            m[2][2] = -fB;
        }
        else
        {
            diag[1] = fD;
            diag[2] = fF;
            subDiag[0] = fB;
            subDiag[1] = fE;
            m[0][0] = 1.0;
            m[0][1] = 0.0;
            m[0][2] = 0.0;
            m[1][0] = 0.0;
            m[1][1] = 1.0;
            m[1][2] = 0.0;
            m[2][0] = 0.0;
            m[2][1] = 0.0;
            m[2][2] = 1.0;
        }
    }

    bool Matrix3::QLAlgorithm(float diag[3], float subDiag[3])
    {
        // QL iteration with implicit shifting to reduce matrix from tridiagonal to diagonal

        for (int i = 0; i < 3; i++)
        {
            const unsigned int maxIter = 32;
            unsigned int iter;
            for (iter = 0; iter < maxIter; iter++)
            {
                int j;
                for (j = i; j <= 1; j++)
                {
                    float sum = Math::abs(diag[j]) + Math::abs(diag[j+1]);

                    if (Math::abs(subDiag[j]) + sum == sum)
                        break;
                }

                if (j == i)
                    break;

                float tmp0 = (diag[i+1]-diag[i])/(2.0f*subDiag[i]);
                float tmp1 = Math::sqrt(tmp0*tmp0+1.0f);

                if (tmp0 < 0.0f)
                    tmp0 = diag[j]-diag[i]+subDiag[i]/(tmp0-tmp1);
                else
                    tmp0 = diag[j]-diag[i]+subDiag[i]/(tmp0+tmp1);

                float sin = 1.0f;
                float cos = 1.0f;
                float tmp2 = 0.0f;
                for (int k = j-1; k >= i; k--)
                {
                    float tmp3 = sin*subDiag[k];
                    float tmp4 = cos*subDiag[k];

                    if (Math::abs(tmp3) >= Math::abs(tmp0))
                    {
                        cos = tmp0/tmp3;
                        tmp1 = Math::sqrt(cos*cos+1.0f);
                        subDiag[k+1] = tmp3*tmp1;
                        sin = 1.0f/tmp1;
                        cos *= sin;
                    }
                    else
                    {
                        sin = tmp3/tmp0;
                        tmp1 = Math::sqrt(sin*sin+1.0f);
                        subDiag[k+1] = tmp0*tmp1;
                        cos = 1.0f/tmp1;
                        sin *= cos;
                    }

                    tmp0 = diag[k+1]-tmp2;
                    tmp1 = (diag[k]-tmp0)*sin+2.0f*tmp4*cos;
                    tmp2 = sin*tmp1;
                    diag[k+1] = tmp0+tmp2;
                    tmp0 = cos*tmp1-tmp4;

                    for (int row = 0; row < 3; row++)
                    {
                        tmp3 = m[row][k+1];
                        m[row][k+1] = sin*m[row][k] + cos*tmp3;
                        m[row][k] = cos*m[row][k] - sin*tmp3;
                    }
                }

                diag[i] -= tmp2;
                subDiag[i] = tmp0;
                subDiag[j] = 0.0;
            }

            if (iter == maxIter)
            {
                // Should not get here under normal circumstances
                return false;
            }
        }

        return true;
    }

    void Matrix3::eigenSolveSymmetric(float eigenValues[3], Vector3 eigenVectors[3]) const
    {
        Matrix3 mat = *this;
        float subDiag[3];
        mat.tridiagonal(eigenValues, subDiag);
        mat.QLAlgorithm(eigenValues, subDiag);

        for (size_t i = 0; i < 3; i++)
        {
            eigenVectors[i][0] = mat[0][i];
            eigenVectors[i][1] = mat[1][i];
            eigenVectors[i][2] = mat[2][i];
        }

        // Make eigenvectors form a right--handed system
        Vector3 cross = eigenVectors[1].cross(eigenVectors[2]);
        float det = eigenVectors[0].dot(cross);
        if (det < 0.0f)
        {
            eigenVectors[2][0] = -eigenVectors[2][0];
            eigenVectors[2][1] = -eigenVectors[2][1];
            eigenVectors[2][2] = -eigenVectors[2][2];
        }
    }
}