BansheeEngine/MBansheeEngine/Math/Matrix3.cs

using System;
using System.Runtime.InteropServices;

namespace BansheeEngine
{
    [StructLayout(LayoutKind.Sequential), SerializeObject]
    public struct Matrix3
    {
        private struct EulerAngleOrderData
		{
            public EulerAngleOrderData(int a, int b, int c, float sign)
            {
                this.a = a;
                this.b = b;
                this.c = c;
                this.sign = sign;
            }

			public int a, b, c;
            public float sign;
		};

        private static EulerAngleOrderData[] EA_LOOKUP = 
		{ new EulerAngleOrderData(0, 1, 2, 1.0f), new EulerAngleOrderData(0, 2, 1, -1.0f), new EulerAngleOrderData(1, 0, 2, -1.0f),
		  new EulerAngleOrderData(1, 2, 0, 1.0f), new EulerAngleOrderData(2, 0, 1,  1.0f), new EulerAngleOrderData(2, 1, 0, -1.0f) };

        public static readonly Matrix3 zero = new Matrix3();
        public static readonly Matrix3 identity = new Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1);

        public float m00;
        public float m01;
        public float m02;
        public float m10;
        public float m11;
        public float m12;
        public float m20;
        public float m21;
        public float m22;

        public Matrix3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
        {
            this.m00 = m00;
            this.m01 = m01;
            this.m02 = m02;
            this.m10 = m10;
            this.m11 = m11;
            this.m12 = m12;
            this.m20 = m20;
            this.m21 = m21;
            this.m22 = m22;
        }

        public float this[int row, int column]
        {
            get
            {
                return this[row * 3 + column];
            }
            set
            {
                this[row * 3 + column] = value;
            }
        }

        public float this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                        return m00;
                    case 1:
                        return m01;
                    case 2:
                        return m02;
                    case 3:
                        return m10;
                    case 4:
                        return m11;
                    case 5:
                        return m12;
                    case 6:
                        return m20;
                    case 7:
                        return m21;
                    case 8:
                        return m22;
                    default:
                        throw new IndexOutOfRangeException("Invalid matrix index.");
                }

            }
            set
            {
                switch (index)
                {
                    case 0:
                        m00 = value;
                        break;
                    case 1:
                        m01 = value;
                        break;
                    case 2:
                        m02 = value;
                        break;
                    case 3:
                        m10 = value;
                        break;
                    case 4:
                        m11 = value;
                        break;
                    case 5:
                        m12 = value;
                        break;
                    case 6:
                        m20 = value;
                        break;
                    case 7:
                        m21 = value;
                        break;
                    case 8:
                        m22 = value;
                        break;
                    default:
                        throw new IndexOutOfRangeException("Invalid matrix index.");
                }
            }
        }

        public static Matrix3 operator *(Matrix3 lhs, Matrix3 rhs)
        {
            return new Matrix3()
            {
                m00 = lhs.m00 * rhs.m00 + lhs.m01 * rhs.m10 + lhs.m02 * rhs.m20,
                m01 = lhs.m00 * rhs.m01 + lhs.m01 * rhs.m11 + lhs.m02 * rhs.m21,
                m02 = lhs.m00 * rhs.m02 + lhs.m01 * rhs.m12 + lhs.m02 * rhs.m22,
                m10 = lhs.m10 * rhs.m00 + lhs.m11 * rhs.m10 + lhs.m12 * rhs.m20,
                m11 = lhs.m10 * rhs.m01 + lhs.m11 * rhs.m11 + lhs.m12 * rhs.m21,
                m12 = lhs.m10 * rhs.m02 + lhs.m11 * rhs.m12 + lhs.m12 * rhs.m22,
                m20 = lhs.m20 * rhs.m00 + lhs.m21 * rhs.m10 + lhs.m22 * rhs.m20,
                m21 = lhs.m20 * rhs.m01 + lhs.m21 * rhs.m11 + lhs.m22 * rhs.m21,
                m22 = lhs.m20 * rhs.m02 + lhs.m21 * rhs.m12 + lhs.m22 * rhs.m22,
            };
        }

        public static bool operator== (Matrix3 lhs, Matrix3 rhs)
        {
            if (lhs.m00 == rhs.m00 && lhs.m01 == rhs.m01 && lhs.m02 == rhs.m02 && 
                lhs.m10 == rhs.m10 && lhs.m11 == rhs.m11 && lhs.m12 == rhs.m12 &&
                lhs.m20 == rhs.m20 && lhs.m21 == rhs.m21 && lhs.m22 == rhs.m22)
                return true;
            else
                return false;
        }

        public static bool operator !=(Matrix3 lhs, Matrix3 rhs)
        {
            return !(lhs == rhs);
        }

        public override int GetHashCode()
        {
            float hash1 = m00.GetHashCode() ^ m10.GetHashCode() << 2 ^ m20.GetHashCode() >> 2;
            float hash2 = m01.GetHashCode() ^ m11.GetHashCode() << 2 ^ m21.GetHashCode() >> 2;
            float hash3 = m02.GetHashCode() ^ m12.GetHashCode() << 2 ^ m22.GetHashCode() >> 2;

            return hash1.GetHashCode() ^ hash2.GetHashCode() << 2 ^ hash3.GetHashCode() >> 2;
        }

        public override bool Equals(object other)
        {
            if (!(other is Matrix3))
                return false;

            Matrix3 mat = (Matrix3)other;
            if (m00 == mat.m00 && m01 == mat.m01 && m02 == mat.m02 &&
                m10 == mat.m10 && m11 == mat.m11 && m12 == mat.m12 &&
                m20 == mat.m20 && m21 == mat.m21 && m22 == mat.m22)
                return true;
            else
                return false;
        }

        public void Invert()
        {
            float[,] invVals = new float[3,3];

            invVals[0, 0] = m11 * m22 - m12 * m21;
            invVals[1, 0] = m12 * m20 - m10 * m22;
            invVals[2, 0] = m10 * m21 - m11 * m20;

            float det = m00 * invVals[0, 0] + m01 * invVals[1, 0] + m02 * invVals[2, 0];

            if (MathEx.Abs(det) <= 1e-06f)
                throw new DivideByZeroException("Matrix determinant is zero. Cannot invert.");

            invVals[0, 1] = m02 * m21 - m01 * m22;
            invVals[0, 2] = m01 * m12 - m02 * m11;

            invVals[1, 1] = m00 * m22 - m02 * m20;
            invVals[1, 2] = m02 * m10 - m00 * m12;

            invVals[2, 1] = m01 * m20 - m00 * m21;
            invVals[2, 2] = m00 * m11 - m01 * m10;

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

        public void Transpose()
        {
            float tmp = m10;
            m10 = m01;
            m01 = tmp;

            tmp = m20;
            m20 = m02;
            m02 = tmp;

            tmp = m12;
            m12 = m21;
            m21 = tmp;
        }

        public float Determinant()
        {
            float cofactor00 = m11 * m22 - m12 * m21;
            float cofactor10 = m12 * m20 - m10 * m22;
            float cofactor20 = m10 * m21 - m11 * m20;

            float det = m00 * cofactor00 + m01 * cofactor10 + m02 * cofactor20;

            return det;
        }

        public Vector3 Transform(Vector3 vec)
        {
            Vector3 outVec;
            outVec.x = m00 * vec.x + m01 * vec.y + m02 * vec.z;
            outVec.y = m10 * vec.x + m11 * vec.y + m12 * vec.z;
            outVec.z = m20 * vec.x + m21 * vec.y + m22 * vec.z;
            return outVec;
        }

        public void QDUDecomposition(out Matrix3 matQ, out Vector3 vecD, out Vector3 vecU)
        {
            matQ = new Matrix3();
            vecD = new Vector3();
            vecU = new Vector3();

            // Build orthogonal matrix Q
            float invLength = MathEx.InvSqrt(m00*m00 + m10*m10 + m20*m20);
            matQ.m00 = m00*invLength;
            matQ.m10 = m10*invLength;
            matQ.m20 = m20*invLength;

            float dot = matQ.m00*m01 + matQ.m10*m11 + matQ.m20*m21;
            matQ.m01 = m01-dot*matQ.m00;
            matQ.m11 = m11-dot*matQ.m10;
            matQ.m21 = m21-dot*matQ.m20;

            invLength = MathEx.InvSqrt(matQ.m01*matQ.m01 + matQ.m11*matQ.m11 + matQ.m21*matQ.m21);
            matQ.m01 *= invLength;
            matQ.m11 *= invLength;
            matQ.m21 *= invLength;

            dot = matQ.m00*m02 + matQ.m10*m12 + matQ.m20*m22;
            matQ.m02 = m02-dot*matQ.m00;
            matQ.m12 = m12-dot*matQ.m10;
            matQ.m22 = m22-dot*matQ.m20;

            dot = matQ.m01*m02 + matQ.m11*m12 + matQ.m21*m22;
            matQ.m02 -= dot*matQ.m01;
            matQ.m12 -= dot*matQ.m11;
            matQ.m22 -= dot*matQ.m21;

            invLength = MathEx.InvSqrt(matQ.m02*matQ.m02 + matQ.m12*matQ.m12 + matQ.m22*matQ.m22);
            matQ.m02 *= invLength;
            matQ.m12 *= invLength;
            matQ.m22 *= invLength;

            // Guarantee that orthogonal matrix has determinant 1 (no reflections)
            float fDet = matQ.m00*matQ.m11*matQ.m22 + matQ.m01*matQ.m12*matQ.m20 +
                matQ.m02*matQ.m10*matQ.m21 - matQ.m02*matQ.m11*matQ.m20 -
                matQ.m01*matQ.m10*matQ.m22 - matQ.m00*matQ.m12*matQ.m21;

            if (fDet < 0.0f)
            {
                matQ.m00 = -matQ.m00;
                matQ.m01 = -matQ.m01;
                matQ.m02 = -matQ.m02;
                matQ.m10 = -matQ.m10;
                matQ.m11 = -matQ.m11;
                matQ.m12 = -matQ.m12;
                matQ.m20 = -matQ.m20;
                matQ.m21 = -matQ.m21;
                matQ.m21 = -matQ.m22;
            }

            // Build "right" matrix R
            Matrix3 matRight = new Matrix3();
            matRight.m00 = matQ.m00 * m00 + matQ.m10 * m10 + matQ.m20 * m20;
            matRight.m01 = matQ.m00 * m01 + matQ.m10 * m11 + matQ.m20 * m21;
            matRight.m11 = matQ.m01 * m01 + matQ.m11 * m11 + matQ.m21 * m21;
            matRight.m02 = matQ.m00 * m02 + matQ.m10 * m12 + matQ.m20 * m22;
            matRight.m12 = matQ.m01 * m02 + matQ.m11 * m12 + matQ.m21 * m22;
            matRight.m22 = matQ.m02 * m02 + matQ.m12 * m12 + matQ.m22 * m22;

            // The scaling component
            vecD[0] = matRight.m00;
            vecD[1] = matRight.m11;
            vecD[2] = matRight.m22;

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

        /**
         * @note    Returns angles in degrees.
         */
        public Vector3 ToEulerAngles(EulerAngleOrder order = EulerAngleOrder.XYZ)
        {
            EulerAngleOrderData l = EA_LOOKUP[(int)order];

		    float xAngle = MathEx.Asin(l.sign * this[l.a, l.c]);
		    if (xAngle < MathEx.HalfPi)
		    {
			    if (xAngle > -MathEx.HalfPi)
			    {
				    float yAngle = MathEx.Atan2(-l.sign * this[l.b, l.c], this[l.c, l.c]);
				    float zAngle = MathEx.Atan2(-l.sign * this[l.a, l.b], this[l.a, l.a]);

                    return new Vector3(xAngle * MathEx.Rad2Deg, yAngle * MathEx.Rad2Deg, zAngle * MathEx.Rad2Deg);
			    }
			    else
			    {
				    // WARNING.  Not a unique solution.
				    float angle = MathEx.Atan2(l.sign * this[l.b, l.a], this[l.b, l.b]);
				    float zAngle = 0.0f;  // Any angle works
				    float yAngle = zAngle - angle;

                    return new Vector3(xAngle * MathEx.Rad2Deg, yAngle * MathEx.Rad2Deg, zAngle * MathEx.Rad2Deg);
			    }
		    }
		    else
		    {
			    // WARNING.  Not a unique solution.
			    float angle = MathEx.Atan2(l.sign * this[l.b, l.a], this[l.b, l.b]);
                float zAngle = 0.0f; // Any angle works
			    float yAngle = angle - zAngle;

                return new Vector3(xAngle * MathEx.Rad2Deg, yAngle * MathEx.Rad2Deg, zAngle * MathEx.Rad2Deg);
		    }
        }

        public Quaternion ToQuaternion()
        {
            return Quaternion.FromRotationMatrix(this);
        }

        public void ToAxisAngle(out Vector3 axis, out Degree angle)
        {
            float trace = m00 + m11 + m22;
            float cos = 0.5f*(trace-1.0f);
            Radian radians = MathEx.Acos(cos);  // In [0, PI]
            angle = radians.GetDegrees();

            if (radians > 0.0f)
            {
                if (radians < MathEx.Pi)
                {
                    axis.x = m21 - m12;
                    axis.y = m02 - m20;
                    axis.z = m10 - m01;

                    axis.Normalize();
                }
                else
                {
                    // Angle is PI
                    float halfInverse;
                    if (m00 >= m11)
                    {
                        // r00 >= r11
                        if (m00 >= m22)
                        {
                            // r00 is maximum diagonal term
                            axis.x = 0.5f*MathEx.Sqrt(m00 - m11 - m22 + 1.0f);
                            halfInverse = 0.5f/axis.x;
                            axis.y = halfInverse*m01;
                            axis.z = halfInverse*m02;
                        }
                        else
                        {
                            // r22 is maximum diagonal term
                            axis.z = 0.5f*MathEx.Sqrt(m22 - m00 - m11 + 1.0f);
                            halfInverse = 0.5f/axis.z;
                            axis.x = halfInverse*m02;
                            axis.y = halfInverse*m12;
                        }
                    }
                    else
                    {
                        // r11 > r00
                        if (m11 >= m22)
                        {
                            // r11 is maximum diagonal term
                            axis.y = 0.5f*MathEx.Sqrt(m11 - m00 - m22 + 1.0f);
                            halfInverse  = 0.5f/axis.y;
                            axis.x = halfInverse*m01;
                            axis.z = halfInverse*m12;
                        }
                        else
                        {
                            // r22 is maximum diagonal term
                            axis.z = 0.5f*MathEx.Sqrt(m22 - m00 - m11 + 1.0f);
                            halfInverse = 0.5f/axis.z;
                            axis.x = halfInverse*m02;
                            axis.y = halfInverse*m12;
                        }
                    }
                }
            }
            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;
            }
        }

        public static Matrix3 Inverse(Matrix3 mat)
        {
            Matrix3 copy = mat;
            copy.Invert();
            return copy;
        }

        public static Matrix3 Transpose(Matrix3 mat)
        {
            Matrix3 copy = mat;
            copy.Transpose();
            return copy;
        }

        public static Matrix3 FromEuler(Vector3 eulerDeg, EulerAngleOrder order)
        {
            EulerAngleOrderData l = EA_LOOKUP[(int)order];

		    Matrix3[] mats = new Matrix3[3];
		    float cos, sin;

		    cos = MathEx.Cos(eulerDeg.y * MathEx.Deg2Rad);
            sin = MathEx.Sin(eulerDeg.y * MathEx.Deg2Rad);
		    mats[0] = new Matrix3(1.0f, 0.0f, 0.0f, 0.0f, cos, -sin, 0.0f, sin, cos);

            cos = MathEx.Cos(eulerDeg.x * MathEx.Deg2Rad);
            sin = MathEx.Sin(eulerDeg.x * MathEx.Deg2Rad);
		    mats[1] = new Matrix3(cos, 0.0f, sin, 0.0f, 1.0f, 0.0f, -sin, 0.0f, cos);

            cos = MathEx.Cos(eulerDeg.z * MathEx.Deg2Rad);
            sin = MathEx.Sin(eulerDeg.z * MathEx.Deg2Rad);
		    mats[2] = new Matrix3(cos,-sin, 0.0f, sin, cos, 0.0f, 0.0f, 0.0f, 1.0f);
	
		    return mats[l.a]*(mats[l.b]*mats[l.c]);
        }

        public static Matrix3 FromAxisAngle(Vector3 axis, Degree angle)
        {
            Matrix3 mat;

            float cos = MathEx.Cos(angle.GetRadians());
            float sin = MathEx.Sin(angle.GetRadians());
            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;

            mat.m00 = x2 * oneMinusCos + cos;
            mat.m01 = xym - zSin;
            mat.m02 = xzm + ySin;
            mat.m10 = xym + zSin;
            mat.m11 = y2 * oneMinusCos + cos;
            mat.m12 = yzm - xSin;
            mat.m20 = xzm - ySin;
            mat.m21 = yzm + xSin;
            mat.m22 = z2 * oneMinusCos + cos;

            return mat;
        }

        public static Matrix3 FromQuaternion(Quaternion quat)
        {
            return quat.ToRotationMatrix();
        }

        public override string ToString()
        {
            return String.Format("({0}, {1}, {2},\n{3}, {4}, {5}\n{6}, {7}, {8})",
                m00, m01, m02, m10, m11, m12, m20, m21, m22);
        }
    }
}