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

namespace VisiMath
{
    /// <summary>
    /// Matrix 3x3 used for rotation in 3D or for 2D homogenous transformations
    /// </summary>
    [Serializable]
    [StructLayout(LayoutKind.Sequential)]
    public struct Matrix3 : IEquatable<Matrix3>
    {
        /// <summary>
        /// Matrix fields
        /// </summary>        
        public float m00, m10, m20, m01, m11, m21, m02, m12, m22;

        #region Acessors


        #region Rows and Columns

        [XmlIgnore]
        public Vector3 Row0
        {
            get
            {
                return new Vector3(m00, m01, m02);
            }
            set
            {
                m00 = value.X;
                m01 = value.Y;
                m02 = value.Z;
            }
        }

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

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

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

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

        [XmlIgnore]
        public Vector3 Column2
        {
            get
            {
                return new Vector3(m02, m12, m22);
            }
            set
            {
                m00 = value.X;
                m10 = value.Y;
                m20 = value.Z;
            }
        }


        #endregion

        /// <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-2</exception>
        public float this[int index1, int index2]
        {
            get
            {
                if ((index1 < 0) || (index1 > 2))
                {
                    throw new ArgumentOutOfRangeException("index1", "Indexing out of the matrix4");
                }
                if ((index2 < 0) || (index2 > 2))
                {
                    throw new ArgumentOutOfRangeException("index2", "Indexing out of the matrix4");
                }

                return this[index2 * 3 + index1];
            }

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

        }

        public float this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                        return m00;
                    case 1:
                        return m10;
                    case 2:
                        return m20;
                    case 3:
                        return m01;
                    case 4:
                        return m11;
                    case 5:
                        return m21;
                    case 6:
                        return m02;
                    case 7:
                        return m12;
                    case 8:
                        return m22;
                    default:
                        throw new ArgumentOutOfRangeException("index", "Indexing out of the matrix3");
                }
            }

            set
            {
                switch (index)
                {
                    case 0:
                        m00= value; break;
                    case 1:
                        m10= value; break;
                    case 2:
                        m20= value; break;
                    case 3:
                        m01= value; break;
                    case 4:
                        m11= value; break;
                    case 5:
                        m21= value; break;
                    case 6:
                        m02= value; break;
                    case 7:
                        m12= value; break;
                    case 8:
                        m22= value; break;
                    default:
                        throw new ArgumentOutOfRangeException("index", "Indexing out of the matrix3");
                }
            }
        }

        #endregion


        #region Constructors

        /// <summary>
        /// Constructs matrix from the float values
        /// </summary>
        public Matrix3(float f00, float f01, float f02, float f10, float f11, float f12, float f20, float f21, float f22)
        {
            m00 = f00;
            m01 = f01;
            m02 = f02;
            m10 = f10;
            m11 = f11;
            m12 = f12;
            m20 = f20;
            m21 = f21;
            m22 = f22;
        }

        /// <summary>
        /// Constructs matrix with all fields set to specified value
        /// </summary>
        /// <param name="val">Value to set all fields to</param>
        public Matrix3(float val)
        {
            m00 = m01 = m02 = m10 = m11 = m12 = m20 = m21 = m22 = val;
        }

        /// <summary>
        /// Copies content of the float[] to matrix
        /// </summary>
        /// <param name="data">Initial values</param>
        public Matrix3(float[] data)
        {
            if (data.Length == 9)
            {
                m00 = data[0];
                m10 = data[1];
                m20 = data[2];
                m01 = data[3];
                m11 = data[4];
                m21 = data[5];
                m02 = data[6];
                m12 = data[7];
                m22 = data[8];
            }            
            else
            {
                throw new ArgumentException("Wrong size of array for matrix initialization", "data");
            }
        }

        /// <summary>
        /// Construct rotation matrix from the rotation quaternion
        /// </summary>
        /// <param name="rot">Rotation quaternion</param>
        public Matrix3(Quaternion rot)
        {
            rot.Normalize();

            float xx = rot.X * rot.X;
            float yy = rot.Y * rot.Y;
            float zz = rot.Z * rot.Z;
            float xy = rot.X * rot.Y;
            float xz = rot.X * rot.Z;
            float yz = rot.Y * rot.Z;
            float wx = rot.W * rot.X;
            float wy = rot.W * rot.Y;
            float wz = rot.W * rot.Z;
            
            m00 = 1 - 2 * (yy + zz);
            m01 = 2 * (xy - wz);
            m02 = 2 * (xz + wy);

            m10 = 2 * (xy + wz);
            m11 = 1 - 2 * (xx + zz);
            m12 = 2 * (yz - wx);

            m20 = 2 * (xz - wy);
            m21 = 2 * (yz + wx);
            m22 = 1 - 2 * (xx + yy);                        
        }

        /// <summary>
        /// Construct matrix from for rows of vector4
        /// </summary>
        public Matrix3(Vector3 row0, Vector3 row1, Vector3 row2)
        {
            m00 = row0.X; m01 = row0.Y; m02 = row0.Z;
            m10 = row1.X; m11 = row1.Y; m12 = row1.Z;
            m20 = row2.X; m21 = row2.Y; m22 = row2.Z;
        }

        #endregion

        #region Constants
        
        public static Matrix3 Identitity = new Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1);

        #endregion

        #region Properties

        /// <summary>
        /// Determinant of the matrix
        /// </summary>
        [XmlIgnore]
        public float Determinant
        {
            get
            {
                return (m00 * m11 * m22) - (m00 * m12 * m21) - (m01 * m10 * m22) + 
                    (m02 * m10 * m21) + (m01 * m12 * m20) - (m02 * m11 * m20);
            }
        }

        #endregion

        #region Methods


        /// <summary>
        /// assigns values to matrix
        /// </summary>
        public void Assign(float f00, float f01, float f02, float f10, float f11, float f12, float f20, float f21, float f22)
        {
            m00 = f00;
            m01 = f01;
            m02 = f02;
            m10 = f10;
            m11 = f11;
            m12 = f12;
            m20 = f20;
            m21 = f21;
            m22 = f22;
        }

        /// <summary>
        /// Set matrix to diagonal
        /// </summary>
        /// <param name="f00"></param>
        /// <param name="f11"></param>
        /// <param name="f22"></param>
        public void SetToDiagonal(float f00, float f11, float f22)
        {
            Assign(f00, 0, 0, 0, f11, 0, 0, 0, f22);
        }

        /// <summary>
        /// Sets matrix to identity
        /// </summary>
        public void SetToIdentity()
        {
            Assign(1, 0, 0, 0, 1, 0, 0, 0, 1);
        }

        /// <summary>
        /// Sets matrix to inertial tensor
        /// </summary>
        /// <param name="ix"></param>
        /// <param name="iy"></param>
        /// <param name="iz"></param>
        /// <param name="ixy"></param>
        /// <param name="ixz"></param>
        /// <param name="iyz"></param>
        public void SetInertialTensor(float ix, float iy, float iz, float ixy = 0, float ixz = 0, float iyz = 0)
        {
            m00 = ix;
            m01 = m10 = -ixy;
            m02 = m21 = -ixz;
            m11 = iy;
            m12 = m21 = -iyz;
            m22 = iz;
        }
        
        /// <summary>
        /// Transposes matrix
        /// </summary>
        public void Transpose()
        {
            MathTools.Swap(ref m01, ref m10);
            MathTools.Swap(ref m02, ref m20);
            MathTools.Swap(ref m12, ref m21);
        }

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

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

        /// <summary>
        /// Substracts another matrix from this one
        /// </summary>
        /// <param name="other">right operand</param>
        public void Substract(ref Matrix3 other)
        {
            for (int i = 0; i < 9; 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 < 9; i++)
            {
                this[i] *= scale;
            }
        }

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

        /// <summary>
        /// Multiplies this matrix by another and returns this*other)
        /// </summary>
        /// <param name="other">right operand</param>
        public Matrix3 Multiply(ref Matrix3 other)
        {
            return Matrix3.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(ref Vector3 vec)
        {
            return new Vector3(
                (m00 * vec.X + m01 * vec.Y + m02 * vec.Z),
                (m10 * vec.X + m11 * vec.Y + m12 * vec.Z),
                (m20 * vec.X + m21 * vec.Y + m22 * vec.Z)
            );
        }

        /// <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),
                (m10 * vec.X + m11 * vec.Y + m12 * vec.Z),
                (m20 * vec.X + m21 * vec.Y + m22 * vec.Z), 1
            );
        }

        /// <summary>
        /// Transforms specified vector by the matrix
        /// </summary>
        /// <param name="vec">Vector to be transformed</param>
        public void Transform(ref Vector3 vec)
        {
            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 transposed matrix
        /// </summary>
        /// <param name="vec">Vector to be transformed</param>
        public void TransformTranspose(ref Vector3 vec)
        {
            float X = (m00 * vec.X + m10 * vec.Y + m20 * vec.Z);
            float Y = (m01 * vec.X + m11 * vec.Y + m21 * vec.Z);
            float Z = (m02 * vec.X + m12 * 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);
            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;
            vec.W = 1;
        }

        /// <summary>
        /// Transforms specified vector by the matrix
        /// </summary>
        /// <param name="vector2">Vector to transform</param>
        public void Transform(ref Vector2 vector2)
        {
            Vector3 vec = new Vector3(vector2.X, vector2.Y, 1);
            Transform(ref vec);
            vector2.X = vec.X;
            vector2.Y = vec.Y;
        }

        /// <summary>
        /// Deconstructs rotation/scaling matrix to scale and rotation
        /// 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>
        public void Deconstruct(out float uniformscale, out Vector3 scale, out Quaternion q)
        {
            Vector3 c0 = Column0;
            Vector3 c1 = Column1;
            Vector3 c2 = Column2;

            float cx = c0.Length;
            float cy = c1.Length;
            float cz = c2.Length;

            c0.Divide(cx);
            c1.Divide(cy);
            c2.Divide(cz);

            if (MathTools.AlmostEqual(cx, cy, 0.0001f) && MathTools.AlmostEqual(cy, cz, 0.0001f))
            {
                uniformscale = cx;
            }
            else uniformscale = 0;

            scale = new Vector3(cx, cy, cz);

            Matrix3 rmat = new Matrix3(
                c0.X, c1.X, c2.X,
                c0.Y, c1.Y, c2.Y,
                c0.Z, c1.Z, c2.Z);
            
            q = Quaternion.CreateFromMatrix(ref rmat);            
        }

        /// <summary>
        /// Tries to extract uniform scale from the matrix
        /// </summary>
        /// <returns>Uniform scale if present or zero</returns>
        public float GetUniformScale()
        {
            float f;
            Vector3 sc;
            GetScale(out f, out sc);
            return f;
        }

        /// <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)
        {
            Vector3 c0 = Column0;
            Vector3 c1 = Column1;
            Vector3 c2 = Column2;

            float cx = c0.Length;
            float cy = c1.Length;
            float cz = c2.Length;

            if (MathTools.AlmostEqual(cx, cy, 0.0001f) && MathTools.AlmostEqual(cy, cz, 0.0001f))
            {
                uniform = cx;
            }
            else uniform = 0;

            scale = new Vector3(cx, cy, cz);
        }



        #endregion


        #region Static methods


        #region Math functions

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

        /// <summary>
        /// Calculates Inverse of the matrix
        /// </summary>
        /// <param name="matrix3">Matrix to be inverted</param>
        /// <returns>Inverted matrix</returns>
        /// <exception cref="System.ArgumentException">Thrown if matrix is singular</exception>
        public static Matrix3 Invert(ref Matrix3 matrix3)
        {
            float newm00 = matrix3.m11*matrix3.m22 - matrix3.m12*matrix3.m21;
            float newm01 = matrix3.m02*matrix3.m21 - matrix3.m01*matrix3.m22;
            float newm02 = matrix3.m01*matrix3.m12 - matrix3.m02*matrix3.m11;
            float newm10 = matrix3.m12*matrix3.m20 - matrix3.m10*matrix3.m22;
            float newm11 = matrix3.m00*matrix3.m22 - matrix3.m02*matrix3.m20;
            float newm12 = matrix3.m02*matrix3.m10 - matrix3.m00*matrix3.m12;
            float newm20 = matrix3.m10*matrix3.m21 - matrix3.m11*matrix3.m20;
            float newm21 = matrix3.m01*matrix3.m20 - matrix3.m00*matrix3.m21;
            float newm22 = matrix3.m00*matrix3.m11 - matrix3.m01*matrix3.m10;

            float det = matrix3.m00*newm00 + matrix3.m01*newm10+ matrix3.m02*newm20;

            if (MathTools.IsAlmostZero(det))
            {
                throw new ArgumentException("Cannot invert singular matrix");
            }

            det = 1.0f/det;

            Matrix3 retval = new Matrix3(newm00, newm01, newm02, newm10, newm11, newm12, newm20, newm21, newm22);
            
            retval.Multiply(det);            
            return retval;
        }

        /// <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 Matrix3 Multiply(ref Matrix3 left, ref Matrix3 right)
        {
            return new Matrix3(
                    left.m00 * right.m00 + left.m01 * right.m10 + left.m02 * right.m20,
                    left.m00 * right.m01 + left.m01 * right.m11 + left.m02 * right.m21,
                    left.m00 * right.m02 + left.m01 * right.m12 + left.m02 * right.m22,
                    left.m10 * right.m00 + left.m11 * right.m10 + left.m12 * right.m20,
                    left.m10 * right.m01 + left.m11 * right.m11 + left.m12 * right.m21,
                    left.m10 * right.m02 + left.m11 * right.m12 + left.m12 * right.m22,
                    left.m20 * right.m00 + left.m21 * right.m10 + left.m22 * right.m20,
                    left.m20 * right.m01 + left.m21 * right.m11 + left.m22 * right.m21,
                    left.m20 * right.m02 + left.m21 * right.m12 + left.m22 * right.m22
                    );
        }

        /// <summary>
        /// Returns transposed left * right
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        /// <returns>transposed(left) * right</returns>
        public static Matrix3 TransposeMultiply(ref Matrix3 left, ref Matrix3 right)
        {
            //return Transpose(ref left).Multiply(ref right);

            return new Matrix3(
                    left.m00 * right.m00 + left.m10 * right.m10 + left.m20 * right.m20,
                    left.m00 * right.m01 + left.m10 * right.m11 + left.m20 * right.m21,
                    left.m00 * right.m02 + left.m10 * right.m12 + left.m20 * right.m22,
                    left.m01 * right.m00 + left.m11 * right.m10 + left.m21 * right.m20,
                    left.m01 * right.m01 + left.m11 * right.m11 + left.m21 * right.m21,
                    left.m01 * right.m02 + left.m11 * right.m12 + left.m21 * right.m22,
                    left.m02 * right.m00 + left.m12 * right.m10 + left.m22 * right.m20,
                    left.m02 * right.m01 + left.m12 * right.m11 + left.m22 * right.m21,
                    left.m02 * right.m02 + left.m12 * right.m12 + left.m22 * right.m22
                    );
        }


        #endregion


        #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 Matrix3 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 Matrix3 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 Matrix3 CreateScale(float x, float y, float z)
        {
            return new Matrix3(
                x, 0, 0, 
                0, y, 0,
                0, 0, z);
        }

        #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 Matrix3 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</param>
        /// <param name="angle">Angle in radians</param>
        /// <returns>Rotation transformation</returns>
        public static Matrix3 CreateRotationAxisAngle(Vector3 axis, float angle)
        {
            float cos = (float)System.Math.Cos(-angle);
            float sin = (float)System.Math.Sin(-angle);
            float t = 1.0f - cos;

            axis.Normalize();

            return new Matrix3(  
                    t * axis.X * axis.X + cos, t * axis.X * axis.Y - sin * axis.Z, t * axis.X * axis.Z + sin * axis.Y,
                    t * axis.X * axis.Y + sin * axis.Z, t * axis.Y * axis.Y + cos, t * axis.Y * axis.Z - sin * axis.X,
                    t * axis.X * axis.Z - sin * axis.Y, t * axis.Y * axis.Z + sin * axis.X, t * axis.Z * axis.Z + cos);
        }

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

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

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

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

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

            return new Matrix3(
                cos, sin, 0,
                -sin, cos, 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 Matrix3 CreateRotationRPY(ref Vector3 rpy)
        {
            return CreateRotationRPY(rpy.X, rpy.Y, rpy.Z);
        }

        /// <summary>
        /// Creates rotation transformation from the pitch yaw roll 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 Matrix3 CreateRotationRPY(float roll, float pitch, float yaw)
        {
            //NOTE!!! check if works after refactoring

            /*
            ///old version
			float c1=(float)Math.Cos(pitch);
			float c2=(float)Math.Cos(yaw);
			float c3=(float)Math.Cos(roll);
			float s1=(float)Math.Cos(pitch);
			float s2=(float)Math.Cos(yaw);
			float s3=(float)Math.Cos(roll);
			return new Matrix3(
                c1*c2, c1*s2*s3-s1*c3, c1*s2*c3+s1*s3,
				s1*c2, s1*s2*s3+c1*c3, s1*s2*c3-c1*s3,
				-s2, c2*s3, c2*c1);            
             
             */

            Quaternion q = Quaternion.CreateFromRPY(roll, pitch, yaw);

            Matrix3 quatern = q.GetRotationMatrix();
            
            float cosyaw = (float)Math.Cos(yaw);
            float cospitch = (float)Math.Cos(pitch);
            float cosroll = (float)Math.Cos(roll);
            float sinyaw = (float)Math.Sin(yaw);
            float sinpitch = (float)Math.Sin(pitch);
            float sinroll = (float)Math.Sin(roll);


            Matrix3 rval2 = new Matrix3(
                cosyaw*cospitch, sinyaw*cosroll+cosyaw*sinpitch*sinroll, sinyaw*sinroll-cosyaw*sinpitch*cosroll,
                -sinyaw*cospitch, cosyaw*cosroll-sinyaw*sinpitch*sinroll, cosyaw*sinroll+sinyaw*sinpitch*cosroll,
                sinpitch,-cospitch*sinroll,cospitch*cosroll
            );

            return rval2;


            /*

			Matrix3 rval = new Matrix3(
                cosyaw*cospitch, cosyaw*sinpitch*sinroll - sinyaw*cosroll, cosyaw*sinpitch*cosroll + sinyaw*sinroll,
                sinyaw*cospitch, sinyaw*sinpitch*sinroll + cosyaw*cosroll, sinyaw*sinpitch*cosroll-cosyaw*sinroll,
                -sinpitch, cospitch*sinroll, cospitch*cosroll);
            
            
            float cosa = (float)Math.Cos(yaw);
            float cosb = (float)Math.Cos(pitch);
            float cosg = (float)Math.Cos(roll);
            float sina = (float)Math.Sin(yaw);
            float sinb = (float)Math.Sin(pitch);
            float sing = (float)Math.Sin(roll);


			Matrix3 rval = new Matrix3(
                cosa*cosb, cosa*sinb*sing - sina*cosg, cosa*sinb*cosg + sina*sing,
                sina*cosb, sina*sinb*sing + cosa*cosg, sina*sinb*cosg-cosa*sing,
                -sinb, cosb*sing, cosb*cosg);
             */
        }

        /// <summary>
        /// Create rotattional matrix from quaternion
        /// </summary>
        /// <param name="q">Rotation quaternion</param>
        /// <returns>Rotation</returns>
        public static Matrix3 CreateRotation(Quaternion q)
        {
            return q.GetRotationMatrix();
        }

        /// <summary>
        /// Creates skew symetric matrix from associated vector (inifinitesimal rotation)
        /// </summary>
        /// <param name="pos">Vector</param>
        /// <returns>Skew symetric rotation</returns>
        public static Matrix3 CreateSkewSymetric(ref Vector3 pos)
        {
            return new Matrix3(0, -pos.Z, pos.Y, pos.Z, 0, -pos.X, -pos.Y, pos.X, 0); //from cyclone
            //return new Matrix3(0, pos.Z, -pos.Y, -pos.Z, 0, pos.X, pos.Y, -pos.X, 0);
        }

        
        #endregion 

        #region 2D transformations

        /// <summary>
        /// Creates 2D scale transformation
        /// </summary>
        /// <param name="scale">scale factor</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DScale(float scale)
        {
            return Create2DScale(scale, scale);
        }


        /// <summary>
        /// Creates 2D scale transformation
        /// </summary>
        /// <param name="scalex">scale factor in x</param>
        /// <param name="scaley">scale factor in y</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DScale(float scalex, float scaley)
        {
            return new Matrix3(scalex, 0, 0, 0, scaley, 0, 0, 0, 1);
        }

        /// <summary>
        /// Creates 2D scale transformation
        /// </summary>
        /// <param name="scale">scale factor vector</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DScale(ref Vector2 scale)
        {
            return Create2DScale(scale.X, scale.Y);
        }

        /// <summary>
        /// Creates 2d rotation transformation
        /// </summary>
        /// <param name="angle">angle in radians</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DRotation(float angle)
        {
            float cos = (float)Math.Cos(angle);
            float sin = (float)Math.Sin(angle);
            return new Matrix3(cos, -sin, 0, sin, cos, 0, 0, 0, 1);
        }

        /// <summary>
        /// Creates 2d rotation transformation
        /// </summary>
        /// <param name="rotm">2x2 rotational matrix</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DRotation(ref Matrix2 rotm)
        {
            return new Matrix3(rotm.m00, rotm.m01, 0, rotm.m10, rotm.m11, 0, 0, 0, 1);
        }

        /// <summary>
        /// Creates 2d translation transformation
        /// </summary>
        /// <param name="trans">translation vector</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DTranslation(ref Vector2 trans)
        {
            return new Matrix3(1, 0, trans.X, 0, 1, trans.Y, 0, 0, 1);
        }

        /// <summary>
        /// Creates new 2d transfomation matrix
        /// </summary>
        /// <param name="scale">scale factor</param>
        /// <param name="angle">rotation angle in radians</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DTransformation(float scale, float angle)
        {
            Matrix3 rval = Create2DScale(scale);
            Matrix3 rot = Create2DRotation(angle);
            rval.MultiplyBy(ref rot);
            return rval;
        }

        /// <summary>
        /// Creates new 2d transfomation matrix
        /// </summary>
        /// <param name="scale">scale factor</param>
        /// <param name="angle">rotation angle in radians</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DTransformation(ref Vector2 scale, float angle)
        {
            Matrix3 rval = Create2DScale(ref scale);
            Matrix3 rot = Create2DRotation(angle);
            rval.MultiplyBy(ref rot);
            return rval;
        }

        /// <summary>
        /// Creates new 2d transfomation matrix
        /// </summary>
        /// <param name="trans">translation vector</param>
        /// <param name="angle">rotation angle in radians</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DTransformation(float angle, ref Vector2 trans)
        {
            Matrix3 rot = Create2DRotation(angle);
            rot.m02 = trans.X;
            rot.m12 = trans.Y;
            return rot;
        }

        /// <summary>
        /// Creates new 2d transfomation matrix
        /// </summary>
        /// <param name="scale">scale factor</param>
        /// <param name="trans">translation vector</param>
        /// <param name="angle">rotation angle in radians</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DTransformation(ref Vector2 scale, float angle, ref Vector2 trans)
        {
            Matrix3 rval = Create2DTransformation(ref scale, angle);
            rval.m02 = trans.X;
            rval.m12 = trans.Y;
            return rval;
        }

        /// <summary>
        /// Creates new 2d transfomation matrix
        /// </summary>
        /// <param name="rotm">rotational 2x2 matrix</param>
        /// <param name="trans">translation vector</param>
        /// <returns>New 2d transformation</returns>
        public static Matrix3 Create2DTransformation(ref Matrix2 rotm, ref Vector2 trans)
        {
            Matrix3 rval = Create2DRotation(ref rotm);
            rval.m02 = trans.X;
            rval.m12 = trans.Y;
            return rval;            
        }






        #endregion


        #region Inertia Tensors

        /// <summary>
        /// Creates inertia tensor for 3d axis aligned box with uniform mass distribution
        /// </summary>
        /// <param name="sizex">length in X</param>
        /// <param name="sizey">length in Y</param>
        /// <param name="sizez">length in Z</param>
        /// <param name="mass">mass of the object</param>
        /// <returns>Inertia tensor</returns>
        public static Matrix3 CreateInertiaCuboid(float sizex, float sizey, float sizez, float mass)
        {
            float m = mass/12;
            float dx = sizex*sizex;
            float dy = sizey*sizey;
            float dz = sizez*sizez;

            return new Matrix3(
                m * (dy + dz), 0, 0,
                0, m * (dx + dz), 0,
                0, 0, m * (dx + dz));
        }

        /// <summary>
        /// Creates inertia tensor for 3d axis aligned box with uniform mass distribution
        /// </summary>
        /// <param name="size">lengths in all 3 axes</param>
        /// <param name="mass">mass of the object</param>
        /// <returns>Inertia tensor</returns>
        public static Matrix3 CreateInertiaCuboid(Vector3 size, float mass)
        {
            return CreateInertiaCuboid(size.X, size.Y, size.Z, mass);
        }


        /// <summary>
        /// Creates inertia tensor for sphere with uniform density
        /// </summary>
        /// <param name="radius">Sphere radius</param>
        /// <param name="mass">mass of the object</param>
        /// <returns>Inertia tensor</returns>
        public static Matrix3 CreateInertiaSphere(float radius, float mass)
        {
            float m = 2*radius*radius*mass / 5;
            return new Matrix3(
                m, 0, 0,
                0, m, 0,
                0, 0, m);
        }

        /// <summary>
        /// Creates inertia tensor for sphere shell - all mass is on the surface
        /// </summary>
        /// <param name="radius">Sphere radius</param>
        /// <param name="mass">mass of the object</param>
        /// <returns>Inertia tensor</returns>
        public static Matrix3 CreateInertiaSphereShell(float radius, float mass)
        {
            float m = 2 * radius * radius * mass / 3;
            return new Matrix3(
                m, 0, 0,
                0, m, 0,
                0, 0, m);
        }

        #endregion


        #endregion

        #endregion


        #region operators

        /// <summary>
        /// Matrix multiplication
        /// </summary>
        /// <param name="left">left operand</param>
        /// <param name="right">right operand</param>
        /// <returns>New Matrix34 with result</returns>
        public static Matrix3 operator *(Matrix3 left, Matrix3 right)
        {
            return Matrix3.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 Matrix3 operator *(Matrix3 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 Matrix3 operator *(float left, Matrix3 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 Matrix3 operator -(Matrix3 left, Matrix3 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 Matrix3 operator +(Matrix3 left, Matrix3 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 *(Matrix3 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 *(Matrix3 left, Vector3 right)
        {
            return left.Multiply(ref 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 ==(Matrix3 left, Matrix3 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 !=(Matrix3 left, Matrix3 right)
        {
            return !left.Equals(right);
        }

        #endregion

        #region overrides

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

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

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

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

        #endregion

    }
}
