using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Runtime.InteropServices;
using System.Xml.Serialization;

namespace VisiMath
{
    /// <summary>
    /// Matrix 4x4 used for homogenous transformations
    /// </summary>
    [Serializable]
    [StructLayout(LayoutKind.Sequential)]
    public struct Matrix4 : IEquatable<Matrix4>
    {
        public float m00, m10, m20, m30, m01, m11, m21, m31, m02, m12, m22, m32, m03, m13, m23, m33;


        #region quick access

        /*
        public float m00
        {
            set { _m[0] = value; }
            get { return _m[0]; }
        }
        public float m10
        {
            set { _m[1] = value; }
            get { return _m[1]; }
        }
        public float m20
        {
            set { _m[2] = value; }
            get { return _m[2]; }
        }

        public float m30
        {
            set { _m[3] = value; }
            get { return _m[3]; }
        }

        public float m01
        {
            set { _m[4] = value; }
            get { return _m[4]; }
        }
        public float m11
        {
            set { _m[5] = value; }
            get { return _m[5]; }
        }
        public float m21
        {
            set { _m[6] = value; }
            get { return _m[6]; }
        }
        public float m31
        {
            set { _m[7] = value; }
            get { return _m[7]; }
        }
        public float m02
        {
            set { _m[8] = value; }
            get { return _m[8]; }
        }
        public float m12
        {
            set { _m[9] = value; }
            get { return _m[9]; }
        }
        public float m22
        {
            set { _m[10] = value; }
            get { return _m[10]; }
        }
        public float m32
        {
            set { _m[11] = value; }
            get { return _m[11]; }
        }

        public float m03
        {
            set { _m[12] = value; }
            get { return _m[12]; }
        }
        public float m13
        {
            set { _m[13] = value; }
            get { return _m[13]; }
        }
        public float m23
        {
            set { _m[14] = value; }
            get { return _m[14]; }
        }
        public float m33
        {
            set { _m[15] = value; }
            get { return _m[15]; }
        }

*/

        [XmlIgnore]
        public Vector4 Row0
        {
            get
            {
                return new Vector4(m00, m01, m02, m03);
            }

            set
            {
                m00 = value.X;
                m01 = value.Y;
                m02 = value.Z;
                m03 = value.W;
            }
        }

        [XmlIgnore]
        public Vector4 Row1
        {
            get
            {
                return new Vector4(m10, m11, m12, m13);
            }
            set
            {
                m10 = value.X;
                m11 = value.Y;
                m12 = value.Z;
                m13 = value.W;
            }
        }

        [XmlIgnore]
        public Vector4 Row2
        {
            get
            {
                return new Vector4(m20, m21, m22, m23);
            }
            set
            {
                m20 = value.X;
                m21 = value.Y;
                m22 = value.Z;
                m23 = value.W;
            }

        }

        [XmlIgnore]
        public Vector4 Row3
        {
            get
            {
                return new Vector4(m30, m31, m32, m33);
            }
            set
            {
                m30 = value.X;
                m31 = value.Y;
                m32 = value.Z;
                m33 = value.W;
            }

        }
        
        [XmlIgnore]
        public Vector4 Column0
        {
            get { return new Vector4(m00, m10, m20, m30); }
            set
            {
                m00 = value.X;
                m10 = value.Y;
                m20 = value.Z;
                m30 = value.W;
            }

        }

        [XmlIgnore]
        public Vector4 Column1
        {
            get { return new Vector4(m01, m11, m21, m31); }
            set
            {
                m01 = value.X;
                m11 = value.Y;
                m21 = value.Z;
                m31 = value.W;
            }
        }

        [XmlIgnore]
        public Vector4 Column2
        {
            get { return new Vector4(m02, m12, m22, m32); }
            set
            {
                m02 = value.X;
                m12 = value.Y;
                m22 = value.Z;
                m32 = value.W;
            }
        }

        [XmlIgnore]
        public Vector4 Column3
        {
            get { return new Vector4(m03, m13, m23, m33); }
            set
            {
                m03 = value.X;
                m13 = value.Y;
                m23 = value.Z;
                m33 = value.W;
            }
        }

        /// <summary>
        /// Index acessor, maps internal fields to [index1, index2]
        /// </summary>
        /// <param name="index1">Row index</param>
        /// <param name="index2">Column index</param>
        /// <exception cref="System.ArgumentOutOfRangeException">Thrown if index used is not 0-3</exception>
        public float this[int index1, int index2]
        {
            get
            {
                if ((index1 < 0) || (index1 > 3))
                {
                    throw new ArgumentOutOfRangeException("index1", "Indexing out of the matrix4");
                }
                if ((index2 < 0) || (index2 > 3))
                {
                    throw new ArgumentOutOfRangeException("index2", "Indexing out of the matrix4");
                }

                return this[index2 * 4 + index1];
            }

            set
            {
                if ((index1 < 0) || (index1 > 3))
                {
                    throw new ArgumentOutOfRangeException("index1", "Indexing out of the matrix4");
                }
                if ((index2 < 0) || (index2 > 3))
                {
                    throw new ArgumentOutOfRangeException("index2", "Indexing out of the matrix4");
                }
                this[index2 * 4 + index1] = value;    
            }
        }

        /// <summary>
        /// Index acessor, maps internal fields to one dimensional array
        /// </summary>
        /// <param name="index">index</param>
        /// <exception cref="System.ArgumentOutOfRangeException">Thrown if index used is not 0-15</exception>
        public float this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                        return m00;
                    case 1:
                        return m10;
                    case 2:
                        return m20;
                    case 3:
                        return m30;
                    case 4:
                        return m01;
                    case 5:
                        return m11;
                    case 6:
                        return m21;
                    case 7:
                        return m31;
                    case 8:
                        return m02;
                    case 9:
                        return m12;
                    case 10:
                        return m22;
                    case 11:
                        return m32;
                    case 12:
                        return m03;
                    case 13:
                        return m13;
                    case 14:
                        return m23;
                    case 15:
                        return m33;
                    default:
                        throw new ArgumentOutOfRangeException("index", "Indexing out of the matrix4");
                }
            }
            set
            {
                switch (index)
                {
                    case 0:
                        m00 = value; break;
                    case 1:
                        m10 = value; break;
                    case 2:
                        m20 = value; break;
                    case 3:
                        m30 = value; break;
                    case 4:
                        m01 = value; break;
                    case 5:
                        m11 = value; break;
                    case 6:
                        m21 = value; break;
                    case 7:
                        m31 = value; break;
                    case 8:
                        m02 = value; break;
                    case 9:
                        m12 = value; break;
                    case 10:
                        m22 = value; break;
                    case 11:
                        m32 = value; break;
                    case 12:
                        m03 = value; break;
                    case 13:
                        m13 = value; break;
                    case 14:
                        m23 = value; break;
                    case 15:
                        m33 = value; break;
                    default:
                        throw new ArgumentOutOfRangeException("index", "Indexing out of the matrix4");
                }
            }


        }


        #endregion

        #region Constructors

        public Matrix4(ref Matrix4 mat)
        {
            this.m00 = mat.m00;
            this.m01 = mat.m01;
            this.m02 = mat.m02;
            this.m03 = mat.m03;
            this.m10 = mat.m10;
            this.m11 = mat.m11;
            this.m12 = mat.m12;
            this.m13 = mat.m13;
            this.m20 = mat.m20;
            this.m21 = mat.m21;
            this.m22 = mat.m22;
            this.m23 = mat.m23;
            this.m30 = mat.m30;
            this.m31 = mat.m31;
            this.m32 = mat.m32;
            this.m33 = mat.m33;
        }

        /// <summary>
        /// Initializes matrix with all values set to val
        /// </summary>
        /// <param name="val">Initial value</param>
        public Matrix4(float val)
        {
            this.m00 = val;
            this.m01 = val;
            this.m02 = val;
            this.m03 = val;
            this.m10 = val;
            this.m11 = val;
            this.m12 = val;
            this.m13 = val;
            this.m20 = val;
            this.m21 = val;
            this.m22 = val;
            this.m23 = val;
            this.m30 = val;
            this.m31 = val;
            this.m32 = val;
            this.m33 = val;
        }

        /// <summary>
        /// Copies content of the float[] to matrix
        /// </summary>
        /// <param name="data">Initial values</param>
        public Matrix4(float[] data) : this()
        {
            if (data.Length == 16)
            {
                for (int i = 0; i < 15; i ++)
                    this[i] = data[i];
            }            
            else
            {
                throw new ArgumentException("Wrong size of array for matrix initialization", "data");
            }
        }

        /// <summary>
        /// Initialize matrix with all specified values
        /// </summary>
        public Matrix4(float n00, float n01, float n02, float n03, float n10, float n11, float n12, float n13,
            float n20, float n21, float n22, float n23, float n30, float n31, float n32, float n33)
        {
            m00 = n00;
            m01 = n01;
            m02 = n02;
            m03 = n03;
            m10 = n10;
            m11 = n11;
            m12 = n12;
            m13 = n13;
            m20 = n20;
            m21 = n21;
            m22 = n22;
            m23 = n23;
            m30 = n30;
            m31 = n31;
            m32 = n32;
            m33 = n33;
        }

        /// <summary>
        /// Creates homogenous Matrix4 from rotation matrix with zero translation
        /// </summary>
        /// <param name="rotation">rotation matrix</param>
        public Matrix4(Matrix3 rotation)
        {
            m00 = rotation.m00;
            m10 = rotation.m10;
            m20 = rotation.m20;
            m30 = 0;
            m01 = rotation.m01;
            m11 = rotation.m11;
            m21 = rotation.m21;
            m31 = 0;
            m02 = rotation.m02;
            m12 = rotation.m12;
            m22 = rotation.m22;
            m32 = 0;
            m03 = m13 = m23 = 0;
            m33 = 1;
        }

        /// <summary>
        /// Creates homogenous transformation matrix from rotation/scale matrix and translation vector
        /// </summary>
        /// <param name="rotation">Rotation matrix</param>
        /// <param name="translation">Translation vector</param>
        public Matrix4(Matrix3 rotation, Vector3 translation) : this(rotation)
        {
            m03 = translation.X;
            m13 = translation.Y;
            m23 = translation.Z;
        }

        /// <summary>
        /// Creates new homogenous transformation matrix from rotation quaternion and translation vector
        /// </summary>
        /// <param name="rot">Rotation</param>
        /// <param name="translation">Translation</param>
        public Matrix4(Quaternion rot, Vector3 translation) : this(rot)
        {
            m03 = translation.X;
            m13 = translation.Y;
            m23 = translation.Z;
        }

        /// <summary>
        /// Creates new homogenous transformation matrix from rotation quaternion
        /// </summary>
        /// <param name="rot">Rotation</param>
        public Matrix4(Quaternion rot)
        {
            Matrix3 rotation = rot.GetRotationMatrix();
            m00 = rotation.m00;
            m10 = rotation.m10;
            m20 = rotation.m20;
            m30 = 0;
            m01 = rotation.m01;
            m11 = rotation.m11;
            m21 = rotation.m21;
            m31 = 0;
            m02 = rotation.m02;
            m12 = rotation.m12;
            m22 = rotation.m22;
            m32 = 0;
            m03 = 0;
            m13 = 0;
            m23 = 0;
            m33 = 1;
        }

        /// <summary>
        /// Construct matrix from for rows of vector4
        /// </summary>
        public Matrix4(Vector4 row0, Vector4 row1, Vector4 row2, Vector4 row3) : this()
        {
            Row0 = row0;
            Row1 = row1;
            Row2 = row2;
            Row3 = row3;
        }

        #endregion


        #region Constants

        /// <summary>
        /// Identity matrix
        /// </summary>
        public static Matrix4 Identitity = new Matrix4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);

        /// <summary>
        /// Static rotation 90 degrees around the X axis
        /// </summary>
        public static Matrix4 RotationX90 = new Matrix4(
            1, 0, 0, 0,
            0, 0, -1, 0,
            0, 1, 0, 0,
            0, 0, 0, 1);

        public static Matrix4 RotationX180 = new Matrix4(
            1, 0, 0, 0,
            0, -1, 0, 0,
            0, 0, -1, 0,
            0, 0, 0, 1);

        public static Matrix4 RotationX270 = new Matrix4(
            1, 0, 0, 0,
            0, 0, 1, 0,
            0, -1, 0, 0,
            0, 0, 0, 1);

        /// <summary>
        /// Static rotation 90 degrees around the Y axis
        /// </summary>
        public static Matrix4 RotationY90 = new Matrix4(
            0, 0, 0, 1,
            0, 1, 0, 0,
            -1, 0, 0, 0,
            0, 0, 0, 1);

        /// <summary>
        /// Static rotation 90 degrees around the Z axis
        /// </summary>
        public static Matrix4 RotationZ90 = new Matrix4(
            0, -1, 0, 0,
            1, 0, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1);

        public static Matrix4 RotationZ180 = new Matrix4(
            -1, 0, 0, 0,
            0, -1, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1);

        public static Matrix4 RotationZ270 = new Matrix4(
            0, 1, 0, 0,
            -1, 0, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1);

        


        #endregion

        #region properties

        /// <summary>
        /// Rotational matrix
        /// </summary>
        [XmlIgnore]
        public Matrix3 Rotation
        {
            get
            {
                return new Matrix3(m00, m01, m02, m10, m11, m12, m20, m21, m22);
            }

            set
            {
                m00 = value.m00;
                m10 = value.m10;
                m20 = value.m20;
                m01 = value.m01;
                m11 = value.m11;
                m21 = value.m21;
                m02 = value.m02;
                m12 = value.m12;
                m22 = value.m22;                
            }
        }

        /// <summary>
        /// Translation vector
        /// </summary>
        [XmlIgnore]
        public Vector3 Translation
        {
            get
            {
                return new Vector3(m03, m13, m23);
            }

            set
            {
                m03 = value.X;
                m13 = value.Y;
                m23 = value.Z;
            }
        }

        [XmlIgnore]
        public float Determinant
        {
            get
            {
                return 
                    m00 * m11 * m22 * m33 - m00 * m11 * m23 * m32 + m00 * m12 * m23 * m31 - m00 * m12 * m21 * m33
                  + m00 * m13 * m21 * m32 - m00 * m13 * m22 * m31 - m01 * m12 * m23 * m30 + m01 * m12 * m20 * m33
                  - m01 * m13 * m20 * m32 + m01 * m13 * m22 * m30 - m01 * m11 * m22 * m33 + m01 * m11 * m23 * m32
                  + m02 * m13 * m20 * m31 - m02 * m13 * m21 * m30 + m02 * m11 * m21 * m33 - m02 * m11 * m23 * m31
                  + m02 * m11 * m23 * m30 - m02 * m11 * m20 * m33 - m03 * m11 * m21 * m32 + m03 * m11 * m22 * m31
                  - m03 * m11 * m22 * m30 + m03 * m11 * m20 * m32 - m03 * m12 * m20 * m31 + m03 * m12 * m21 * m30;
            }
        }


        #endregion


        #region Methods


        /// <summary>
        /// Assigns new values to the matrix
        /// </summary>
        public void Assign(float n00, float n01, float n02, float n03, float n10, float n11, float n12, float n13,
            float n20, float n21, float n22, float n23, float n30, float n31, float n32, float n33)
        {
            m00 = n00;
            m01 = n01;
            m02 = n02;
            m03 = n03;
            m10 = n10;
            m11 = n11;
            m12 = n12;
            m13 = n13;
            m20 = n20;
            m21 = n21;
            m22 = n22;
            m23 = n23;
            m30 = n30;
            m31 = n31;
            m32 = n32;
            m33 = n33;
        }

        /// <summary>
        /// Sets matrix to identity
        /// </summary>
        public void SetToIdentity()
        {
            for (int i = 0; i < 15; i++)
                this[i] = 0;
            m00 = m11 = m22 = m33 = 1;
        }


        /// <summary>
        /// Transposes matrix
        /// </summary>
        public void Transpose()
        {
            this = Matrix4.Transpose(ref this);
        }

        /// <summary>
        /// Inverts matrix
        /// </summary>
        public void Invert()
        {
            this = Matrix4.Invert(ref this);
        }

        /// <summary>
        /// Adds another matrix to this one
        /// </summary>
        /// <param name="other">right operand</param>
        public void Add(ref Matrix4 other)
        {
            for (int i = 0; i < 15; i++)
            {
                this[i] += other[i];
            }
        }

        /// <summary>
        /// Substracts another matrix from this one
        /// </summary>
        /// <param name="other">right operand</param>
        public void Substract(ref Matrix4 other)
        {
            for (int i = 0; i < 15; i++)
            {
                this[i] -= other[i];
            }
        }

        /// <summary>
        /// Scalar multiplication
        /// </summary>
        /// <param name="scale">Scalar operand</param>
        public void Multiply(float scale)
        {
            for (int i = 0; i < 15; i++)
            {
                this[i] *= scale;
            }
        }

        /// <summary>
        /// Multiplies this matrix by another (this = this*other)
        /// </summary>
        /// <param name="other">right operand</param>
        public void MultiplyBy(ref Matrix4 other)
        {
            this = Matrix4.Multiply(ref this, ref other);
        }

        /// <summary>
        /// Multiplies this matrix by another (this = this*other)
        /// </summary>
        /// <param name="other">right operand</param>
        public void MultiplyBy(Matrix4 other)
        {
            this = Matrix4.Multiply(ref this, ref other);
        }

        /// <summary>
        /// Multiplies this matrix by another and returns this*other)
        /// </summary>
        /// <param name="other">right operand</param>
        public Matrix4 Multiply(ref Matrix4 other)
        {
            return Matrix4.Multiply(ref this, ref other);
        }


        /// <summary>
        /// Multiplies vector by the matrix and returns result
        /// </summary>
        /// <param name="vec">Vector to be multiplied</param>
        /// <returns>Result vector</returns>
        public Vector3 Multiply(Vector3 vec)
        {
            return Multiply(ref vec);
        }

        /// <summary>
        /// Fast multiplication (ignoring 4th row) for regular transformations
        /// </summary>
        /// <param name="vec"></param>
        /// <returns>Transformed vector</returns>
        public Vector3 MultiplyFast(Vector3 vec)
        {
            TransformFast(ref vec);
            return vec;
        }

        /// <summary>
        /// Multiplies vector by the matrix and returns result
        /// </summary>
        /// <param name="vec">Vector to be multiplied</param>
        /// <returns>Result vector</returns>
        public Vector3 Multiply(ref Vector3 vec)
        {

            float d = 1.0f / (m30 * vec.X + m31 * vec.Y + m32 * vec.Z + m33);
            return new Vector3(
                (m00 * vec.X + m01 * vec.Y + m02 * vec.Z + m03) * d, 
                (m10 * vec.X + m11 * vec.Y + m12 * vec.Z + m13) * d,
                (m20 * vec.X + m21 * vec.Y + m22 * vec.Z + m23) * d
            );
        }

        /// <summary>
        /// Fast multiplication (ignoring 4th row) for regular transformations
        /// </summary>
        /// <param name="vec"></param>
        /// <returns>Transformed vector</returns>
        public Vector3 MultiplyFast(ref Vector3 vec)
        {
            return new Vector3(
                (m00 * vec.X + m01 * vec.Y + m02 * vec.Z + m03),
                (m10 * vec.X + m11 * vec.Y + m12 * vec.Z + m13),
                (m20 * vec.X + m21 * vec.Y + m22 * vec.Z + m23)
            );
        }

        /// <summary>
        /// Multiplies vector by the matrix and returns result
        /// </summary>
        /// <param name="vec">Vector to be multiplied</param>
        /// <returns>Result vector</returns>
        public Vector4 Multiply(ref Vector4 vec)
        {
            return new Vector4(
                (m00 * vec.X + m01 * vec.Y + m02 * vec.Z + m03* vec.W),
                (m10 * vec.X + m11 * vec.Y + m12 * vec.Z + m13 * vec.W),
                (m20 * vec.X + m21 * vec.Y + m22 * vec.Z + m23 * vec.W),
                (m30 * vec.X + m31 * vec.Y + m32 * vec.Z + m33 * vec.W)
            );
        }

        
        /// <summary>
        /// Transforms specified vector by the matrix
        /// </summary>
        /// <param name="vec">Vector to be transformed</param>
        public void Transform(ref Vector3 vec)
        {
            float d = 1.0f / (m30 * vec.X + m31 * vec.Y + m32 * vec.Z + m33);
            float X = (m00 * vec.X + m01 * vec.Y + m02 * vec.Z + m03) * d;
            float Y = (m10 * vec.X + m11 * vec.Y + m12 * vec.Z + m13) * d;
            float Z = (m20 * vec.X + m21 * vec.Y + m22 * vec.Z + m23) * d;

            vec.X = X;
            vec.Y = Y;
            vec.Z = Z;            
        }

        /// <summary>
        /// Faster transformation of the vector for matrices with fourth row 0 0 0 1 (normal transformation matrices)
        /// </summary>
        /// <param name="vec"></param>
        public void TransformFast(ref Vector3 vec)
        {
            float X = (m00 * vec.X + m01 * vec.Y + m02 * vec.Z + m03);
            float Y = (m10 * vec.X + m11 * vec.Y + m12 * vec.Z + m13);
            float Z = (m20 * vec.X + m21 * vec.Y + m22 * vec.Z + m23);

            vec.X = X;
            vec.Y = Y;
            vec.Z = Z;
        }
        
        /// <summary>
        /// Inverse transformation of the vector. Works ONLY for matrices with rotation and translation
        /// </summary>
        /// <param name="vec"></param>
        public void TransformInverted(ref Vector3 vec)
        {
            vec.X -= m03;
            vec.Y -= m13;
            vec.Z -= m23;

            float X = (m00 * vec.X + m10 * vec.Y + m20 * vec.Z + m03);
            float Y = (m01 * vec.X + m11 * vec.Y + m21 * vec.Z + m13);
            float Z = (m02 * vec.X + m12 * vec.Y + m22 * vec.Z + m23);

            vec.X = X;
            vec.Y = Y;
            vec.Z = Z;
        }


        /// <summary>
        /// Transforms all vectors in array by the matrix
        /// </summary>
        /// <param name="array">Array of vectors</param>
        public void Transform(Vector3[] array)
        {
            for (int i = 0; i < array.Length; i++)
            {
                Transform(ref array[i]);
            }
        }

        /// <summary>
        /// Transforms all vectors in array by the matrix, ignoring the 4th row for normal transformations without perspective change
        /// </summary>
        /// <param name="array">Array of vectors</param>
        public void TransformFast(Vector3[] array)
        {
            for (int i = 0; i < array.Length; i++)
            {
                TransformFast(ref array[i]);
            }
        }

        
        /// <summary>
        /// Transforms all positions and normals in array of vertex by the matrix
        /// </summary>
        /// <param name="array">Array of vectors</param>
        public void Transform(IVertex[] array)
        {
            for (int i = 0; i < array.Length; i++)
            {
                IVertex vert = array[i];
                vert.Position = Multiply(vert.Position);
                Vector3 norm = vert.Normal;
                TransformNormal(ref norm);
                vert.Normal = norm;
                array[i] = vert;
            }
        }

        /// <summary>
        /// Transforms all positions and normals in array of vertex by the matrix, fast version not using 4 th row
        /// </summary>
        /// <param name="array">Array of vectors</param>
        public void TransformFast(IVertex[] array)
        {
            for (int i = 0; i < array.Length; i++)
            {
                IVertex vert = array[i];
                vert.Position = MultiplyFast(vert.Position);
                Vector3 norm = vert.Normal;
                TransformNormalFast(ref norm);
                vert.Normal = norm;
                array[i] = vert;
            }
        }

        /// <summary>
        /// Transforms all positions and normals in array of vertex by the matrix
        /// </summary>
        /// <param name="array">Array of vectors</param>
        public void Transform(IVertexArray array)
        {
            for (int i = 0; i < array.Count; i++)
            {
                IVertex vert = array[i];
                vert.Position = Multiply(vert.Position);
                Vector3 norm = vert.Normal;
                TransformNormal(ref norm);
                vert.Normal = norm;
                array[i] = vert;
            }
        }


        /// <summary>
        /// Transforms all positions and normals in array of vertex by the matrix
        /// </summary>
        /// <param name="array">Array of vectors</param>
        public void TransformFast(IVertexArray array)
        {
            for (int i = 0; i < array.Count; i++)
            {
                IVertex vert = array[i];
                vert.Position = MultiplyFast(vert.Position);
                Vector3 norm = vert.Normal;
                TransformNormalFast(ref norm);
                vert.Normal = norm;
                array[i] = vert;
            }
        }

        /// <summary>
        /// Transforms Normal, uses slow invert.transpose.transform of rotation matrix.
        /// For transformations with uniform scaling use TransformNormalFast or store transformation matrix by yourself.
        /// </summary>
        /// <param name="vec"></param>
        public void TransformNormal(ref Vector3 vec)
        {
            Matrix3 m33 = Rotation;
            m33.Invert();
            m33.Transpose();
            m33.Transform(ref vec);
            vec.Normalize();
        }

        /// <summary>
        /// rotates normal - note, works correctly only for transformations with uniform scaling!
        /// Use TransformNormal for (slow) correct computation
        /// </summary>
        /// <param name="vec">Normal to be rotated</param>
        public void TransformNormalFast(ref Vector3 vec)
        {
            //Code duplicity, but faster then using rotation part
            float X = (m00 * vec.X + m01 * vec.Y + m02 * vec.Z);
            float Y = (m10 * vec.X + m11 * vec.Y + m12 * vec.Z);
            float Z = (m20 * vec.X + m21 * vec.Y + m22 * vec.Z);

            vec.X = X;
            vec.Y = Y;
            vec.Z = Z;
        }

        /// <summary>
        /// Transforms specified vector by the matrix
        /// </summary>
        /// <param name="vec">Vector to be transformed</param>
        public void Transform(ref Vector4 vec)
        {
            float X = m00 * vec.X + m01 * vec.Y + m02 * vec.Z + m03 * vec.W;
            float Y = m10 * vec.X + m11 * vec.Y + m12 * vec.Z + m13 * vec.W;
            float Z = m20 * vec.X + m21 * vec.Y + m22 * vec.Z + m23 * vec.W;
            float W = m30 * vec.X + m31 * vec.Y + m32 * vec.Z + m33 * vec.W;
            vec.X = X;
            vec.Y = Y;
            vec.Z = Z;
            vec.W = W;
        }

        /// <summary>
        /// Deconstructs homogenous transformation matrix to scale, rotation and translation.
        /// NOTE: if scale is uniform, uniformscale will be set to it, otherwise it will be set to zero
        /// </summary>
        /// <param name="uniformscale">Will be set to uniform scale if used, othewise to zero </param>
        /// <param name="scale">Will be set with scale vector</param>
        /// <param name="q">will be set with rotation quaternion</param>
        /// <param name="trans">Will be set with the translation vector</param>
        public void Deconstruct(out float uniformscale, out Vector3 scale, out Quaternion q, out Vector3 trans)
        {
            Matrix3 rmat = Rotation;
            rmat.Deconstruct(out uniformscale, out scale, out q);
            trans = Translation;
        }

        /// <summary>
        /// Deconstructs homogenous transformation matrix to scale, rotation and translation.
        /// NOTE: if scale is uniform, uniformscale will be set to it, otherwise it will be set to zero
        /// </summary>
        /// <param name="uniformscale">Will be set to uniform scale if used, othewise to zero </param>
        /// <param name="scale">Will be set with scale vector</param>
        /// <param name="rpy">will be set with rotation in roll pitch yaw format</param>
        /// <param name="trans">Will be set with the translation vector</param>
        public void Deconstruct(out float uniformscale, out Vector3 scale, out Vector3 rpy, out Vector3 trans)
        {
            Quaternion q;
            Deconstruct(out uniformscale, out scale, out q, out trans);
            rpy = q.GetRPY();
        }

        /// <summary>
        /// Tries to extract uniform scale from the matrix
        /// </summary>
        /// <returns>Uniform scale if present or zero</returns>
        public float GetUniformScale()
        {
            return Rotation.GetUniformScale();
        }

        /// <summary>
        /// Tries to extract scale from the matrix. If scale is not uniform, uniform parameter will be zero.
        /// </summary>
        /// <param name="uniform">Uniform scale if used otherwise zero</param>
        /// <param name="scale">Scale vector</param>
        public void GetScale(out float uniform, out Vector3 scale)
        {
            Rotation.GetScale(out uniform, out scale);
        }



        #endregion


        #region Static methods


        #region Matrix creation

        #region Scale matrices


        /// <summary>
        /// Creates uniform scale matrix
        /// </summary>
        /// <param name="scale">scale value for all coordinates</param>
        /// <returns>new scale matrix</returns>
        public static Matrix4 CreateScale(float scale)
        {
            return CreateScale(scale, scale, scale);
        }

        /// <summary>
        /// Creates non-uniform scale transformation
        /// </summary>
        /// <param name="scale">vector with scale for x, y, z</param>
        /// <returns>new scale matrix</returns>
        public static Matrix4 CreateScale(ref Vector3 scale)
        {
            return CreateScale(scale.X, scale.Y, scale.Z);
        }

        /// <summary>
        /// Creates non-uniform scale transformation
        /// </summary>
        /// <param name="x">x scale</param>
        /// <param name="y">y scale</param>
        /// <param name="z">z scale</param>
        /// <returns>new scale matrix</returns>
        public static Matrix4 CreateScale(float x, float y, float z)
        {
            return new Matrix4(
                x, 0, 0, 0,
                0, y, 0, 0,
                0, 0, z, 0,
                0, 0, 0, 1);
        }

        #endregion

        #region Rotation matrices

        /// <summary>
        /// Creates rotational matrix based on original and new normal vector)
        /// </summary>
        /// <param name="normal">Original normal</param>
        /// <param name="newnormal">New normal</param>
        /// <returns>Rotational matrix</returns>
        public static Matrix4 CreateRotationNormal(Vector3 normal, Vector3 newnormal)
        {
            return CreateRotationAxisAngle(normal.Cross(ref newnormal), -MathTools.ACos(normal.Dot(ref newnormal)));            
        }

        /// <summary>
        /// Creates rotation transformation from axis and angle
        /// </summary>
        /// <param name="axis">Rotation axis - must be normalized!</param>
        /// <param name="angle">Angle in radians</param>
        /// <returns>Rotation transformation</returns>
        public static Matrix4 CreateRotationAxisAngle(Vector3 axis, float angle)
        {
            float cos = (float)System.Math.Cos(-angle);
            float sin = (float)System.Math.Sin(-angle);
            float oneminus = 1.0f - cos;

            float x2 = axis.X * axis.X;
            float y2 = axis.Y * axis.Y;
            float z2 = axis.Z * axis.Z;
            float xy = axis.X*axis.Y;
            float xz = axis.X*axis.Z;
            float yz = axis.X*axis.Z;



            return new Matrix4(  
                    oneminus * x2 + cos, oneminus * xy - sin * axis.Z, oneminus * xz + sin * axis.Y, 0,
                    oneminus * xy + sin * axis.Z, oneminus * y2 + cos, oneminus * yz - sin * axis.X, 0,
                    oneminus * xz - sin * axis.Y, oneminus * yz + sin * axis.X, oneminus * z2 + cos, 0,
                    0, 0, 0, 1);
        }

        /// <summary>
        /// Creates rotation transformation around the x axis
        /// </summary>
        /// <param name="angle">Angle in radians</param>
        /// <returns>Rotation transformation</returns>
        public static Matrix4 CreateRotationX(float angle)
        {
            float cos = (float)System.Math.Cos(angle);
            float sin = (float)System.Math.Sin(angle);

            return new Matrix4(
                1, 0, 0, 0,
                0, cos, -sin, 0,
                0, sin, cos, 0,
                0, 0, 0, 1);
        }

        /// <summary>
        /// Creates rotation transformation around the y axis
        /// </summary>
        /// <param name="angle">Angle in radians</param>
        /// <returns>Rotation transformation</returns>
        public static Matrix4 CreateRotationY(float angle)
        {
            float cos = (float)System.Math.Cos(angle);
            float sin = (float)System.Math.Sin(angle);

            return new Matrix4(
                cos, 0, sin, 0,
                0, 1, 0, 0,
                -sin, 0, cos, 0,
                0, 0, 0, 1);
        }

        /// <summary>
        /// Creates rotation transformation around the z axis
        /// </summary>
        /// <param name="angle">Angle in radians</param>
        /// <returns>Rotation transformation</returns>
        public static Matrix4 CreateRotationZ(float angle)
        {
            float cos = (float)System.Math.Cos(angle);
            float sin = (float)System.Math.Sin(angle);

            return new Matrix4(
                cos, -sin, 0, 0,
                sin, cos, 0, 0,
                0, 0, 1, 0,
                0, 0, 0, 1);
        }

        /// <summary>
        /// Creates rotation transformation from the roll pitch yaw angles
        /// </summary>
        /// <param name="rpy">Vector holding roll/pitch/yaw</param>
        /// <returns>Rotation matrix</returns>
        public static Matrix4 CreateRotationRPY(ref Vector3 rpy)
        {
            //return CreateRotationRPY(rpy.Z, rpy.X, rpy.Y);
            return CreateRotationRPY(rpy.X, rpy.Y, rpy.Z);
        }

        /// <summary>
        /// Creates rotation transformation from the roll pitch yaw angles
        /// </summary>
        /// <param name="roll">roll angle</param>
        /// <param name="pitch">pitch angle</param>
        /// <param name="yaw">yaw angle</param>
        /// <returns>Rotation matrix</returns>
        public static Matrix4 CreateRotationRPY(float roll, float pitch, float yaw)
        {
            return new Matrix4(Matrix3.CreateRotationRPY(roll, pitch, yaw));
        }

        /// <summary>
        /// Creates rotation from the quaternion
        /// </summary>
        /// <param name="q">rotation quaternion</param>
        /// <returns>Rotation matrix</returns>
        public static Matrix4 CreateRotation(ref Quaternion q)
        {
            return new Matrix4(q.GetRotationMatrix());
        }
        
        #endregion 

        #region Translation matrix

        /// <summary>
        /// Creates translation matrix
        /// </summary>
        /// <param name="vec">translation vector</param>
        /// <returns>Translation matrix</returns>
        public static Matrix4 CreateTranslation(ref Vector3 vec)
        {
            return new Matrix4(
                1, 0, 0, vec.X,
                0, 1, 0, vec.Y,
                0, 0, 1, vec.Z,
                0, 0, 0, 1);
        }

        /// <summary>
        /// Creates translation matrix
        /// </summary>
        /// <param name="vec">translation vector</param>
        /// <returns>Translation matrix</returns>
        public static Matrix4 CreateTranslation(Vector3 vec)
        {
            return CreateTranslation(ref vec);
        }
        
        #endregion

        #region Transformation matrix

        /// <summary>
        /// Creates transformation from the rotation matrix3x3 and translation vector
        /// </summary>
        /// <param name="rot">Rotation matrix</param>
        /// <param name="trans">Translation vector</param>
        /// <returns>Finished tranformation</returns>
        public static Matrix4 CreateTransformation(ref Matrix3 rot, ref Vector3 trans)
        {
            return new Matrix4(rot, trans);
        }
        

        /// <summary>
        /// Creates transformation from the rotation quaternion and translation vector
        /// </summary>
        /// <param name="q">rotation</param>
        /// <param name="trans">translation</param>
        /// <returns>Finished transformation</returns>
        public static Matrix4 CreateTransformation(ref Quaternion q, ref Vector3 trans)
        {
            Matrix4 mat = CreateRotation(ref q);
            mat.Translation = trans;
            return mat;
        }

        /// <summary>
        /// Creates transformation from the rotation in RPY and translation vector
        /// </summary>
        /// <param name="rpy">Rotation in Roll Pitch Yaw format</param>
        /// <param name="trans">Translation vector</param>
        /// <returns>Finished translation</returns>
        public static Matrix4 CreateTransformation(ref Vector3 rpy, ref Vector3 trans)
        {
            //Note!!! maybe optimalize and do direct calculation...
            Matrix4 mat = CreateRotationRPY(ref rpy);
            mat.Translation = trans;
            return mat;
        }

        /// <summary>
        /// Creates transformation from the scale vector, rotation in RPY and translation vector
        /// </summary>
        /// <param name="scale">Scale vector</param>
        /// <param name="rpy">Rotation in Roll Pitch Yaw format</param>
        /// <param name="trans">Translation vector</param>
        /// <returns>Finished translation</returns>
        public static Matrix4 CreateTransformation(ref Vector3 scale, ref Vector3 rpy, ref Vector3 trans)
        {
            //Note!!! maybe optimalize and do direct calculation...
            Matrix4 mat = CreateScale(ref scale);
            Matrix4 rot = CreateRotationRPY(ref rpy);
            mat.MultiplyBy(ref rot);
            mat.Translation = trans;
            return mat;            
        }

        /// <summary>
        /// Creates complete transformation from scale vector, rotation quaternion and translation vector
        /// </summary>
        /// <param name="scale">Nonuniform scale</param>
        /// <param name="q">Rotation</param>
        /// <param name="trans">Translation</param>
        /// <returns>Finished transformation</returns>
        public static Matrix4 CreateTransformation(ref Vector3 scale, ref Quaternion q, ref Vector3 trans)
        {
            //Note!!! maybe optimalize and do direct calculation...
            Matrix4 mat = CreateScale(ref scale);
            Matrix4 rot = new Matrix4(q);
            mat.MultiplyBy(ref rot);
            mat.Translation = trans;
            return mat;
        }

        #endregion

        #region Projection matrices

        /// <summary>
        /// Creates ortographic projection matrix
        /// </summary>
        /// <param name="left">Left edge</param>
        /// <param name="right">Right edge</param>
        /// <param name="top">Top edge</param>
        /// <param name="bottom">Bottom edge</param>
        /// <param name="zNear">Near edge</param>
        /// <param name="zFar">Far edge</param>
        /// <returns>Projection matrix</returns>
        public static Matrix4 CreateOrtographic(float left, float right, float top, float bottom, float zNear, float zFar)
        {
            float invrl = 1 / (right - left);
            float invtb = 1 / (top - bottom);
            float invz = 1 / (zFar - zNear);

            return new Matrix4(
                2*invrl, 0, 0, -(right + left) *invrl,
                0, 2 * invtb, 0, -(top + bottom) * invtb,
                0, 0, -2 * invz, -(zFar + zNear) * invz,
                0, 0, 0, 1
                );
        }

        /// <summary>
        /// Creates ortographic projection matrix
        /// </summary>
        /// <param name="left">Left edge</param>
        /// <param name="right">Right edge</param>
        /// <param name="top">Top edge</param>
        /// <param name="bottom">Bottom edge</param>
        /// <param name="zNear">Near edge</param>
        /// <param name="zFar">Far edge</param>
        /// <param name="mat">Projection matrix</param>
        public static void CreateOrtographic(float left, float right, float top, float bottom, float zNear, float zFar, ref Matrix4 mat)
        {
            float invrl = 1 / (right - left);
            float invtb = 1 / (top - bottom);
            float invz = 1 / (zFar - zNear);

            mat.Assign(
                2 * invrl, 0, 0, -(right + left) * invrl,
                0, 2 * invtb, 0, -(top + bottom) * invtb,
                0, 0, -2 * invz, -(zFar + zNear) * invz,
                0, 0, 0, 1
                );
        }

        /// <summary>
        /// Creates ortographic projection matrix
        /// </summary>
        /// <param name="width">Width of volume</param>
        /// <param name="height">Height of volume</param>
        /// <param name="zNear">Near edge</param>
        /// <param name="zFar">Far edge</param>
        /// <returns>Projection matrix</returns>
        public static Matrix4 CreateOrtographic(float width, float height, float zNear, float zFar)
        {
            float wh = width/2;
            float hh = height /2;
            return CreateOrtographic(-wh, wh, - hh, hh, zNear, zFar);
        }

        /// <summary>
        /// Creates ortographic projection matrix
        /// </summary>
        /// <param name="width">Width of volume</param>
        /// <param name="height">Height of volume</param>
        /// <param name="zNear">Near edge</param>
        /// <param name="zFar">Far edge</param>
        /// <param name="mat">Projection matrix</param>
        public static void CreateOrtographic(float width, float height, float zNear, float zFar, ref Matrix4 mat)
        {
            float wh = width / 2;
            float hh = height / 2;
            CreateOrtographic(-wh, wh, -hh, hh, zNear, zFar, ref mat);
        }


        /// <summary>
        /// Creates perspective projection matrix
        /// </summary>
        /// <param name="left">Left edge</param>
        /// <param name="right">Right edge</param>
        /// <param name="bottom">Bottom edge</param>
        /// <param name="top">Top edge</param>
        /// <param name="zNear">Near edge</param>
        /// <param name="zFar">Far edge</param>
        /// <returns>Projection matrix</returns>
        /// <exception cref="System.ArgumentOutOfRangeException"> Thrown if zNear or zFar less or equal to zero or if zNear is bigger then zFar
        /// </exception>
        public static Matrix4 CreatePerspective(float left, float right, float top, float bottom, float zNear, float zFar)
        {
            Matrix4 rval = new Matrix4();
            CreatePerspective(left, right, top, bottom, zNear, zFar, ref rval);
            return rval;
        }

        /// <summary>
        /// Creates perspective projection matrix
        /// </summary>
        /// <param name="left">Left edge</param>
        /// <param name="right">Right edge</param>
        /// <param name="bottom">Bottom edge</param>
        /// <param name="top">Top edge</param>
        /// <param name="zNear">Near edge</param>
        /// <param name="zFar">Far edge</param>
        /// <param name="mat">Projection matrix</param>
        /// <exception cref="System.ArgumentOutOfRangeException"> Thrown if if zNear is bigger then zFar
        /// </exception>
        public static void CreatePerspective(float left, float right, float top, float bottom, float zNear, float zFar, ref Matrix4 mat)
        {
            if (zNear >= zFar)
                throw new ArgumentOutOfRangeException("zNear", "Matrix4:CreatePerspective - zNear > zFar");



            float x = (2.0f * zNear) / (right - left);
            float y = (2.0f * zNear) / (top - bottom);
            float a = -(right + left) / (right - left);
            float b = -(top + bottom) / (top - bottom);
            float c = (zFar + zNear) / (zFar - zNear);
            float d = -2.0f*(zFar * zNear) / (zFar - zNear);

            //side
            //up
            //forward   
            //This is matrix for X to right, Y down and Z forward
            mat.Assign(x, 0, a, 0,
                       0, y, b, 0,
                       0, 0, c, d,
                       0, 0, 1, 0);
            
            //x right, y down, z backward
            /*
            mat.Assign(x, 0, -a, 0,
                       0, y, -b, 0,
                       0, 0, -c, d,
                       0, 0, -1, 0);           
             */
 
        }

        /// <summary>
        /// Creates perspective projection matrix
        /// </summary>
        /// <param name="fov">Field of vision in radians</param>
        /// <param name="aspect">Aspect ratio of the screen</param>
        /// <param name="zNear">Near edge</param>
        /// <param name="zFar">Far edge</param>
        /// <returns>Projection matrix</returns>
        /// <exception cref="System.ArgumentOutOfRangeException"> Thrown if zNear or zFar less or equal to zero or if zNear is bigger then zFar, 
        /// or for wrong values of FOV or Aspect ration
        /// </exception>
        public static Matrix4 CreatePerspective(float fov, float aspect, float zNear, float zFar)
        {
            Matrix4 rval = new Matrix4();

            CreatePerspective(fov, aspect, zNear, zFar, ref rval);
            return rval;
        }


        /// <summary>
        /// Creates perspective projection matrix
        /// </summary>
        /// <param name="fov">Field of vision in radians</param>
        /// <param name="aspect">Aspect ratio of the screen</param>
        /// <param name="zNear">Near edge</param>
        /// <param name="zFar">Far edge</param>
        /// <param name="mat">Matrix where the result will be saved</param> 
        /// <exception cref="System.ArgumentOutOfRangeException"> Thrown if zNear is bigger then zFar, 
        /// or for wrong values of FOV or Aspect ration
        /// </exception>
        public static void CreatePerspective(float fov, float aspect, float zNear, float zFar, ref Matrix4 mat)
        {
            if (fov <= 0 || fov > MathTools.Pi)
                throw new ArgumentOutOfRangeException("fov", "Matrix4:CreatePerspective - Wrong field of vision");
            if (aspect <= 0)
                throw new ArgumentOutOfRangeException("aspect", "Matrix4:CreatePerspective - aspect ratio less then zero");

            float yMax = zNear * (float)System.Math.Tan(0.5f * fov);
            float yMin = -yMax;
            float xMin = yMin * aspect;
            float xMax = yMax * aspect;

            CreatePerspective(xMin, xMax, yMin, yMax, zNear, zFar, ref mat);
        }


        #endregion

        #region Camera matrices


        /// <summary>
        /// Creates camera transformation
        /// </summary>
        /// <param name="eye">Camera position</param>
        /// <param name="target">Target position</param>
        /// <param name="up">Up vector of the camera</param>
        /// <returns>Finished transformation</returns>
        public static Matrix4 LookAt(ref Vector3 eye, ref Vector3 target, ref Vector3 up)
        {
            Matrix4 rval = new Matrix4();
            LookAt(ref eye, ref target, ref up, ref rval);
            return rval;
        }

        /// <summary>
        /// Creates camera transformation
        /// </summary>
        /// <param name="eye">Camera position</param>
        /// <param name="target">Target position</param>
        /// <param name="up">Up vector of the camera</param>
        /// <returns>Finished transformation</returns>
        public static void LookAt(ref Vector3 eye, ref Vector3 target, ref Vector3 up, ref Matrix4 mat)
        {
            Vector3 forward = Vector3.Substract(ref eye, ref target);
            forward.Normalize();

            Vector3 side = Vector3.Normalize(up.Cross(ref forward));
            Vector3 myup = Vector3.Normalize(forward.Cross(ref side));

            //right
            //up
            //-forward

            /*
            Vector3 z = Vector3.Normalize(eye - target);
            Vector3 x = Vector3.Normalize(Vector3.Cross(up, z));
            Vector3 y = Vector3.Normalize(Vector3.Cross(z, x));
            */

            mat.Assign(
                -side.X, -side.Y, -side.Z, 0,
                -myup.X, -myup.Y, -myup.Z, 0,
                -forward.X, -forward.Y, -forward.Z, 0,
                0, 0, 0, 1);
            
            mat.MultiplyBy(Matrix4.CreateTranslation(-eye));
        }
            
        /// <summary>
        /// Constructs Camera matrix (inverted normal transformation and rotated so camera faces X axis, not Z as by default)
        /// </summary>
        /// <param name="eye">Position of the camera</param>
        /// <param name="rpy">Rotation of the camera</param>
        /// <param name="mat">Matrix to save camera to</param>
        public static void OrientedCamera(ref Vector3 eye, ref Vector3 rpy, ref Matrix4 mat)
        {
            //NOTE!!! adept for optimalization
            Matrix4 matx = CreateTransformation(ref rpy, ref eye);
            matx.ConvertToCamera(ref mat);
        }

        /// <summary>
        /// Constructs Camera matrix (inverted normal transformation and rotated so camera faces X axis, not Z as by default)
        /// </summary>
        /// <param name="eye">Position of the camera</param>
        /// <param name="rpy">Rotation of the camera</param>
        /// <returns>Camera matrix</returns>
        public static Matrix4 OrientedCamera(ref Vector3 eye, ref Vector3 rpy)
        {
            Matrix4 mat = new Matrix4();
            OrientedCamera(ref eye, ref rpy, ref mat);
            return mat;
        }
        

        /// <summary>
        /// Constructs camera matrix from this matrix (inverted and rotated so camera faces X axis, not Z as by default)
        /// </summary>
        /// <returns>Camera matrix</returns>
        public Matrix4 ConvertToCamera()
        {
            Matrix4 mat = new Matrix4();
            ConvertToCamera(ref mat);
            return mat;
        }

        //flipy(z270*x270)
        static Matrix4 camrot = new Matrix4(
            0, 0, 1, 0,
            1, 0, 0, 0,
            0, -1, 0, 0,
            0, 0, 0, 1);


        /// <summary>
        /// Constructs camera matrix from this matrix (inverted and rotated so camera faces X axis, not Z as by default)
        /// </summary>
        public void ConvertToCamera(ref Matrix4 other)
        {
            //other = Multiply(ref RotationX270);
            //other = RotationX270.Multiply(ref this);

            //other = Multiply(ref camrot);
            other = camrot.Multiply(ref this);
            other.Invert();



            //other = Matrix4.Invert(ref this);
        }


        #endregion



        #endregion

        #region math functions
        /// <summary>
        /// Calculates Inverse of the matrix
        /// </summary>
        /// <param name="matrix4">Matrix to be inverted</param>
        /// <returns>Inverted matrix</returns>
        /// <exception cref="System.ArgumentException">Thrown if matrix is singular</exception>
        public static Matrix4 Invert(ref Matrix4 matrix4)
        {
            float v0 = matrix4.m20 * matrix4.m31 - matrix4.m21 * matrix4.m30;
            float v1 = matrix4.m20 * matrix4.m32 - matrix4.m22 * matrix4.m30;
            float v2 = matrix4.m20 * matrix4.m33 - matrix4.m23 * matrix4.m30;
            float v3 = matrix4.m21 * matrix4.m32 - matrix4.m22 * matrix4.m31;
            float v4 = matrix4.m21 * matrix4.m33 - matrix4.m23 * matrix4.m31;
            float v5 = matrix4.m22 * matrix4.m33 - matrix4.m23 * matrix4.m32;

            float t00 = +(v5 * matrix4.m11 - v4 * matrix4.m12 + v3 * matrix4.m13);
            float t10 = -(v5 * matrix4.m10 - v2 * matrix4.m12 + v1 * matrix4.m13);
            float t20 = +(v4 * matrix4.m10 - v2 * matrix4.m11 + v0 * matrix4.m13);
            float t30 = -(v3 * matrix4.m10 - v1 * matrix4.m11 + v0 * matrix4.m12);

            float det = (t00 * matrix4.m00 + t10 * matrix4.m01 + t20 * matrix4.m02 + t30 * matrix4.m03);
            if (MathTools.Abs(det) < 0.00000000001)
            {
                throw new ArgumentException("Cannot invert singular matrix");
            }

            float invDet = 1 / det;

            float d00 = t00 * invDet;
            float d10 = t10 * invDet;
            float d20 = t20 * invDet;
            float d30 = t30 * invDet;

            float d01 = -(v5 * matrix4.m01 - v4 * matrix4.m02 + v3 * matrix4.m03) * invDet;
            float d11 = +(v5 * matrix4.m00 - v2 * matrix4.m02 + v1 * matrix4.m03) * invDet;
            float d21 = -(v4 * matrix4.m00 - v2 * matrix4.m01 + v0 * matrix4.m03) * invDet;
            float d31 = +(v3 * matrix4.m00 - v1 * matrix4.m01 + v0 * matrix4.m02) * invDet;

            v0 = matrix4.m10 * matrix4.m31 - matrix4.m11 * matrix4.m30;
            v1 = matrix4.m10 * matrix4.m32 - matrix4.m12 * matrix4.m30;
            v2 = matrix4.m10 * matrix4.m33 - matrix4.m13 * matrix4.m30;
            v3 = matrix4.m11 * matrix4.m32 - matrix4.m12 * matrix4.m31;
            v4 = matrix4.m11 * matrix4.m33 - matrix4.m13 * matrix4.m31;
            v5 = matrix4.m12 * matrix4.m33 - matrix4.m13 * matrix4.m32;

            float d02 = +(v5 * matrix4.m01 - v4 * matrix4.m02 + v3 * matrix4.m03) * invDet;
            float d12 = -(v5 * matrix4.m00 - v2 * matrix4.m02 + v1 * matrix4.m03) * invDet;
            float d22 = +(v4 * matrix4.m00 - v2 * matrix4.m01 + v0 * matrix4.m03) * invDet;
            float d32 = -(v3 * matrix4.m00 - v1 * matrix4.m01 + v0 * matrix4.m02) * invDet;

            v0 = matrix4.m21 * matrix4.m10 - matrix4.m20 * matrix4.m11;
            v1 = matrix4.m22 * matrix4.m10 - matrix4.m20 * matrix4.m12;
            v2 = matrix4.m23 * matrix4.m10 - matrix4.m20 * matrix4.m13;
            v3 = matrix4.m22 * matrix4.m11 - matrix4.m21 * matrix4.m12;
            v4 = matrix4.m23 * matrix4.m11 - matrix4.m21 * matrix4.m13;
            v5 = matrix4.m23 * matrix4.m12 - matrix4.m22 * matrix4.m13;

            float d03 = -(v5 * matrix4.m01 - v4 * matrix4.m02 + v3 * matrix4.m03) * invDet;
            float d13 = +(v5 * matrix4.m00 - v2 * matrix4.m02 + v1 * matrix4.m03) * invDet;
            float d23 = -(v4 * matrix4.m00 - v2 * matrix4.m01 + v0 * matrix4.m03) * invDet;
            float d33 = +(v3 * matrix4.m00 - v1 * matrix4.m01 + v0 * matrix4.m02) * invDet;

            return new Matrix4(d00, d01, d02, d03, d10, d11, d12, d13, d20, d21, d22, d23, d30, d31, d32, d33);            
        }

        /// <summary>
        /// Returns transposed matrix
        /// </summary>
        /// <param name="mat">Matrix to transpose</param>
        /// <returns>Transposed matrix</returns>
        public static Matrix4 Transpose(ref Matrix4 mat)
        {
            return new Matrix4(mat.Column0, mat.Column1, mat.Column2, mat.Column3);
        }


        /// <summary>
        /// Multiplies two matrices and returns result
        /// </summary>
        /// <param name="left">left operand</param>
        /// <param name="right">right operand</param>
        /// <returns>left*right</returns>
        public static Matrix4 Multiply(ref Matrix4 left, ref Matrix4 right)
        {
            return new Matrix4(
                    left.m00 * right.m00 + left.m01 * right.m10 + left.m02 * right.m20 + left.m03 * right.m30,
                    left.m00 * right.m01 + left.m01 * right.m11 + left.m02 * right.m21 + left.m03 * right.m31,
                    left.m00 * right.m02 + left.m01 * right.m12 + left.m02 * right.m22 + left.m03 * right.m32,
                    left.m00 * right.m03 + left.m01 * right.m13 + left.m02 * right.m23 + left.m03 * right.m33,
                    left.m10 * right.m00 + left.m11 * right.m10 + left.m12 * right.m20 + left.m13 * right.m30,
                    left.m10 * right.m01 + left.m11 * right.m11 + left.m12 * right.m21 + left.m13 * right.m31,
                    left.m10 * right.m02 + left.m11 * right.m12 + left.m12 * right.m22 + left.m13 * right.m32,
                    left.m10 * right.m03 + left.m11 * right.m13 + left.m12 * right.m23 + left.m13 * right.m33,
                    left.m20 * right.m00 + left.m21 * right.m10 + left.m22 * right.m20 + left.m23 * right.m30,
                    left.m20 * right.m01 + left.m21 * right.m11 + left.m22 * right.m21 + left.m23 * right.m31,
                    left.m20 * right.m02 + left.m21 * right.m12 + left.m22 * right.m22 + left.m23 * right.m32,
                    left.m20 * right.m03 + left.m21 * right.m13 + left.m22 * right.m23 + left.m23 * right.m33,
                    left.m30 * right.m00 + left.m31 * right.m10 + left.m32 * right.m20 + left.m33 * right.m30,
                    left.m30 * right.m01 + left.m31 * right.m11 + left.m32 * right.m21 + left.m33 * right.m31,
                    left.m30 * right.m02 + left.m31 * right.m12 + left.m32 * right.m22 + left.m33 * right.m32,
                    left.m30 * right.m03 + left.m31 * right.m13 + left.m32 * right.m23 + left.m33 * right.m33);
        }

        /// <summary>
        /// returns inverted multiplication (mat1.inverted*mat2)
        /// </summary>
        /// <param name="mat1">left operand</param>
        /// <param name="mat2">right operand</param>
        /// <returns>(mat1.inverted*mat2)</returns>
        public static Matrix4 InverseMul(ref Matrix4 mat1, ref Matrix4 mat2)
        {
            return Invert(ref mat1).Multiply(ref mat2); //OPTIMIZE!!!!
        }


        #endregion




        #endregion


        #region operators

        /// <summary>
        /// Matrix multiplication
        /// </summary>
        /// <param name="left">left operand</param>
        /// <param name="right">right operand</param>
        /// <returns>New Matrix44 with result</returns>
        public static Matrix4 operator *(Matrix4 left, Matrix4 right)
        {
            return Matrix4.Multiply(ref left, ref right);
        }

        /// <summary>
        /// Scalar multiplication
        /// </summary>
        /// <param name="left">Matrix</param>
        /// <param name="right">scalar</param>
        /// <returns>Matrix*scalar</returns>
        public static Matrix4 operator *(Matrix4 left, float right)
        {
            left.Multiply(right);
            return left;
        }

        /// <summary>
        /// Scalar multiplication
        /// </summary>
        /// <param name="left">scalar</param>
        /// <param name="right">Matrix</param>
        /// <returns>Matrix*scalar</returns>
        public static Matrix4 operator *(float left, Matrix4 right)
        {
            right.Multiply(left);
            return right;
        }

        /// <summary>
        /// Substraction of two matrices
        /// </summary>
        /// <param name="left">left operand</param>
        /// <param name="right">right operand</param>
        /// <returns>left - right</returns>
        public static Matrix4 operator -(Matrix4 left, Matrix4 right)
        {
            left.Substract(ref right);
            return left;
        }

        /// <summary>
        /// Addition of two matrices
        /// </summary>
        /// <param name="left">left operand</param>
        /// <param name="right">right operand</param>
        /// <returns>left + right</returns>
        public static Matrix4 operator +(Matrix4 left, Matrix4 right)
        {
            left.Add(ref right);
            return left;
        }

        /// <summary>
        /// Multiplication of vector by matrix
        /// </summary>
        /// <param name="left">Matrix</param>
        /// <param name="right">vector</param>
        /// <returns>Transformed vector</returns>
        public static Vector4 operator *(Matrix4 left, Vector4 right)
        {
            left.Multiply(ref right);
            return right;
        }

        /// <summary>
        /// Multiplication of vector by matrix
        /// </summary>
        /// <param name="left">Matrix</param>
        /// <param name="right">vector</param>
        /// <returns>Transformed vector</returns>
        public static Vector3 operator *(Matrix4 left, Vector3 right)
        {
            left.Multiply(ref right);
            return right;        
        }


        /// <summary>
        /// Compares two instances
        /// </summary>
        /// <param name="left">first operand.</param>
        /// <param name="right">second operand.</param>
        /// <returns>True if left  == right, otherwise false.</returns>
        public static bool operator ==(Matrix4 left, Matrix4 right)
        {
            return left.Equals(right);
        }

        /// <summary>
        /// Compares two instances
        /// </summary>
        /// <param name="left">first operand.</param>
        /// <param name="right">second operand.</param>
        /// <returns>True if left  != right, otherwise false.</returns>
        public static bool operator !=(Matrix4 left, Matrix4 right)
        {
            return !left.Equals(right);
        }

        #endregion


        #region overrides

        public override int GetHashCode()
        {
            return Row0.GetHashCode() ^ Row1.GetHashCode() ^ Row2.GetHashCode() ^ Row3.GetHashCode();
        }

        public override string ToString()
        {
            return String.Format("{0}\n{1}\n{2}\n{3}", Row0, Row1, Row2, Row3);
        }

        public override bool Equals(object obj)
        {
            if (obj is Matrix4)
            {
                return this.Equals((Matrix4)obj);
            }
            return false;
        }

        public bool Equals(Matrix4 other)
        {
            for (int i = 0; i < 15; i++)
            {
                if (this[i] != other[i]) return false;
            }
            return true;
        }

        #endregion


    }
}
