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

namespace VisiMath
{
    /// <summary>
    /// Vector3 with float components
    /// </summary>
    [Serializable]
    [StructLayout(LayoutKind.Sequential)]
    public struct Vector3 : IEquatable<Vector3>, IXmlSerializable
    {
        #region Fields
        /// <summary>
        /// X value of the vector
        /// </summary>
        public float X;
        /// <summary>
        /// Y value of the vector
        /// </summary>
        public float Y;
        /// <summary>
        /// Z value of the vector
        /// </summary>
        public float Z;
        #endregion

        #region Constants

        /// <summary>
        /// Defines a unit-length Vector3 that points towards the X-axis.
        /// </summary>
        public static readonly Vector3 UnitX = new Vector3(1, 0, 0);

        /// <summary>
        /// Defines a unit-length Vector3 that points towards the Y-axis.
        /// </summary>
        public static readonly Vector3 UnitY = new Vector3(0, 1, 0);

        /// <summary>
        /// /// Defines a unit-length Vector3 that points towards the Z-axis.
        /// </summary>
        public static readonly Vector3 UnitZ = new Vector3(0, 0, 1);

        /// <summary>
        /// Defines a zero-length Vector3.
        /// </summary>
        public static readonly Vector3 Zero = new Vector3(0, 0, 0);

        /// <summary>
        /// Defines an instance with all components set to 1.
        /// </summary>
        public static readonly Vector3 One = new Vector3(1, 1, 1);

        /// <summary>
        /// Invalid vector
        /// </summary>
        public static readonly Vector3 Invalid = new Vector3(float.NaN);

        /// <summary>
        /// Defines the size of the Vector3 struct in bytes.
        /// </summary>
        public static readonly int SizeInBytes = Marshal.SizeOf(new Vector3());

        #endregion

        #region Constructors

        /// <summary>
        /// Basic constructor, constructs vector3 with specified value for all fields
        /// </summary>
        public Vector3(float val)
        {
            X = val; Y = val; Z = val;
        }

        /// <summary>
        /// Constructs Vector3 from specified coordinates
        /// </summary>
        /// <param name="x">X coordinate</param>
        /// <param name="y">Y coordinate</param>
        /// <param name="z">Z coordinate</param>
        public Vector3(float x, float y, float z)
        {
            X = x; Y = y; Z = z;
        }

        /// <summary>
        /// Copy constructor
        /// </summary>
        /// <param name="vec">Source value</param>
        public Vector3(Vector3 vec)
        {
            X = vec.X; Y = vec.Y; Z = vec.Z;
        }

        /// <summary>
        /// Fast copy constructor
        /// </summary>
        /// <param name="vec">Source value</param>
        public Vector3(ref Vector3 vec)
        {
            X = vec.X; Y = vec.Y; Z = vec.Z;
        }

        /// <summary>
        /// Scaled copy constructo
        /// </summary>
        /// <param name="vec">Source value</param>
        public Vector3(Vector3 vec, float scale)
        {
            X = vec.X * scale; Y = vec.Y * scale; Z = vec.Z * scale;
        }

        /// <summary>
        /// Scaled copy constructo
        /// </summary>
        /// <param name="vec">Source value</param>
        public Vector3(ref Vector3 vec, float scale)
        {
            X = vec.X * scale; Y = vec.Y * scale; Z = vec.Z * scale;
        }


        /// <summary>
        /// Constructs Vector3 from float array
        /// </summary>
        /// <exception cref="System.ArgumentException">
        /// Thrown if argument doesn't have at least 3 components
        /// </exception>
        /// <param name="field">float array with values, must have at least 3 elements</param>
        public Vector3(float[] field)
        {
            if (field.Length < 3)
                throw new System.ArgumentException("Attempt to initialize Vector3 with float field with less then 3 elements");
            X = field[0];
            Y = field[1];
            Z = field[2];
        }

        /*
        public Vector3(System.Xml.XmlReader reader)
        {
            X = Y = Z = 0;
            ReadXml(reader);
        }
         */

        #endregion


        #region Assigning and modifications


        /// <summary>
        /// constructs vector3 with specified value for all fields
        /// </summary>
        /// <param name="val">value for all fields</param>
        public void Assign(float val)
        {
            X = val;
            Y = val;
            Z = val;
        }

        /// <summary>
        /// Sets vector to specified values
        /// </summary>
        /// <param name="x">X value</param>
        /// <param name="y">Y value</param>
        /// <param name="z">Z value</param>
        public void Assign(float x, float y, float z)
        {
            X = x; Y = y; Z = z;
        }


        /// <summary>
        /// Sets vector as copy of source vector
        /// </summary>
        /// <param name="vec">source vector</param>
        public void Assign(ref Vector3 vec)
        {
            X = vec.X; Y = vec.Y; Z = vec.Z;
        }

        /// <summary>
        /// Sets vector as scaled copy of source vector
        /// </summary>
        /// <param name="vec">source vector</param>
        /// <param name="scale">scale</param>
        public void Assign(ref Vector3 vec, float scale)
        {
            X = scale * vec.X; Y = scale * vec.Y; Z = scale * vec.Z;
        }

        /// <summary>
        /// Sets vector to values from float field
        /// </summary>
        /// <exception cref="System.ArgumentException">
        /// Thrown if argument doesn't have at least 3 components
        /// </exception>
        /// <param name="field">Source values, must have at least 3 elements</param>
        public void Assign(float[] field)
        {
            if (field.Length < 3)
                throw new System.ArgumentException("Attempt to initialize Vector3 with float field with less then 3 elements");
            X = field[0];
            Y = field[1];
            Z = field[2];
        }

        /// <summary>
        /// Makes Vector invalid by setting X component to float.NotANumber
        /// </summary>
        public void MakeInvalid()
        {
            X = float.NaN;
        }

        /// <summary>
        /// Inverts vector
        /// </summary>
        public void Invert()
        {
            X = -X;
            Y = -Y;
            Z = -Z;
        }

        /// <summary>
        /// Adds specified vector to this vector - using ref for faster computing
        /// </summary>
        /// <param name="vec">operand</param>
        public void Add(ref Vector3 vec)
        {
            X += vec.X;
            Y += vec.Y;
            Z += vec.Z;
        }

        /// <summary>
        /// Adds specified vector to this vector
        /// </summary>
        /// <param name="vec">operand</param>
        public void Add(Vector3 vec)
        {
            X += vec.X;
            Y += vec.Y;
            Z += vec.Z;
        }

        /// <summary>
        /// Adds x y z values to this vector
        /// </summary>
        public void Add(float x, float y, float z)
        {
            X += x;
            Y += y;
            Z += z;
        }

        /// <summary>
        /// Adds scaled vector to this vector
        /// </summary>
        /// <param name="vec">Vector</param>
        /// <param name="scale">Scale</param>
        public void AddScaled(Vector3 vec, float scale)
        {
            AddScaled(ref vec, scale);
        }

        /// <summary>
        /// Adds scaled vector to this vector
        /// </summary>
        /// <param name="vec">Vector</param>
        /// <param name="scale">Scale</param>
        public void AddScaled(ref Vector3 vec, float scale)
        {
            X += vec.X * scale;
            Y += vec.Y * scale;
            Z += vec.Z * scale;
        }

        /// <summary>
        /// Substracts x y z values from this vector
        /// </summary>
        public void Substract(float x, float y, float z)
        {
            X -= x;
            Y -= y;
            Z -= z;
        }

        /// <summary>
        /// Substracts specified vector from this vector - using ref for faster computing
        /// </summary>
        /// <param name="vec">operand</param>
        public void Substract(ref Vector3 vec)
        {
            X -= vec.X;
            Y -= vec.Y;
            Z -= vec.Z;            
        }

        /// <summary>
        /// Substracts specified vector from this vector
        /// </summary>
        /// <param name="vec">operand</param>
        public void Substract(Vector3 vec)
        {
            X -= vec.X;
            Y -= vec.Y;
            Z -= vec.Z;
        }

        /// <summary>
        /// Multiplies vector by scalar value
        /// </summary>
        /// <param name="newscale">Scalar value</param>
        public void Mul(float newscale)
        {
            X *= newscale;
            Y *= newscale;
            Z *= newscale;
        }

        /// <summary>
        /// Divides vector by the scalar value
        /// </summary>
        /// <param name="div">scalar value</param>
        public void Divide(float div)
        {
            X /= div;
            Y /= div;
            Z /= div;
        }

        /// <summary>
        /// Normalizes vector. If length of the vector is zero, doesn't do anything.
        /// </summary>
        public void Normalize()
        {
            float l = Length;
            if (l != 0.0f)
            {
                Mul(1.0f / Length);
            }            
        }

        /// <summary>
        /// Fast normalization (less precission, usable for some operations
        /// </summary>
        public void NormalizeFast()
        {
            float newscale = MathTools.InverseSqrFast(X*X + Y*Y + Z*Z);
            Mul(newscale);
        }


        /// <summary>
        /// Transforms vector by rotational matrix
        /// </summary>
        /// <param name="mat">Rotational matrix</param>
        public void Transform(ref Matrix3 mat)
        {
            mat.Transform(ref this);
        }

        /// <summary>
        /// Transforms vector by homogenous matrix 4x4
        /// </summary>
        /// <param name="mat">Matrix</param>
        public void Transform(ref Matrix4 mat)
        {
            mat.Transform(ref this);
        }


        /// <summary>
        /// Clamps values of the vector 
        /// </summary>
        /// <param name="min">Minimal possible value</param>
        /// <param name="max">Maximal possible value</param>
        public void Clamp(ref Vector3 min, ref Vector3 max)
        {
            X = X < min.X ? min.X : X > max.X ? max.X : X;
            Y = Y < min.Y ? min.Y : Y > max.Y ? max.Y : Y;
            Z = Z < min.Z ? min.Z : Z > max.Z ? max.Z : Z;            
        }

        /// <summary>
        /// Clamps values of the vector to minimal possible value
        /// </summary>
        /// <param name="min"></param>
        public void ClampMin(ref Vector3 min)
        {
            if (X < min.X) X = min.X;
            if (Y < min.Y) Y = min.Y;
            if (Z < min.Z) Z = min.Z;
        }

        /// <summary>
        /// Clamps values of the vector to maximal possible value
        /// </summary>
        /// <param name="max"></param>
        public void ClampMax(ref Vector3 max)
        {
            if (X > max.X) X = max.X;
            if (Y > max.Y) Y = max.Y;
            if (Z > max.Z) Z = max.Z;
        }


        /// <summary>
        /// Lineary interpolates vector to target vector
        /// </summary>
        /// <param name="target">Target vector</param>
        /// <param name="inter">Interpolation value, assumes (0-1)</param>
        public void Interpolate(ref Vector3 target, float inter)
        {
            X += inter * (target.X - X);
            Y += inter * (target.Y - Y);
            Z += inter * (target.Z - Z);
        }


        #endregion

        #region Methods

        /// <summary>
        /// Property for the vector as an float array
        /// </summary>
        /// <exception cref="System.ArgumentException">
        /// Thrown if argument doesnt have at least three components</exception>
        [XmlIgnore]
        [Browsable(false)]
        public float[] Array
        {
            get { return new float[] { X, Y, Z }; }
            set
            {
                if (value.Length >= 3)
                {
                    Assign(value);
                }
                else
                {
                    throw new ArgumentException("Vector3: Float array must contain at least 3 elements");
                }
            }
        }

        /// <summary>
        /// An index accessor 
        /// Maps index [0] -> X, [1] -> Y and [2] -> Z.
        /// </summary>
        /// <param name="index">The array index referring to a component within the vector (i.e. x, y, z)</param>
        /// <exception cref="System.ArgumentException">
        /// Thrown if the array argument isn't between 0 and 2 
        /// </exception>
        public float this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0: { return X; }
                    case 1: { return Y; }
                    case 2: { return Z; }
                    default: throw new ArgumentException("Indexing out of the vector", "index");
                }
            }
            set
            {
                switch (index)
                {
                    case 0: { X = value; break; }
                    case 1: { Y = value; break; }
                    case 2: { Z = value; break; }
                    default: throw new ArgumentException("Indexing out of the vector", "index");
                }
            }
        }

        /// <summary>
        /// Checks validity of the vector
        /// </summary>
        /// <returns>True if vector.X is not notanumber</returns>
        public bool IsValid()
        {
            if (float.IsNaN(X))
                return false;
            return true;
        }

        /// <summary>
        /// Returns true if vector is normalized
        /// </summary>
        /// <returns></returns>
        public bool IsNormalized()
        {
            if (LengthSquared == 1)
                return true;
            return false;
        }

        /// <summary>
        /// Vector length
        /// </summary>
        /// <exception cref="System.ArgumentOutOfRangeException">
        /// Thrown if new desired length is below zero
        /// </exception>
        /// <exception cref="System.ArgumentException">
        /// Thrown if vector is zero vector.
        /// </exception>
        [XmlIgnore]
        [ReadOnly(true)]
        public float Length
        {
            get
            {
                return (float)System.Math.Sqrt(LengthSquared);
            }

            set
            {
                if (value < 0)
                    throw new ArgumentOutOfRangeException("value", value, "Cannot set negative lenght to vector3");
                if (this.Equals(Vector3.Zero))
                {
                    throw new ArgumentException("Cannot set length to zero vector");
                }
                Mul(value / Length);
            }
        }

        /// <summary>
        /// Fast but imprecisse vector length
        /// </summary>
        [XmlIgnore]
        [Browsable(false)]
        public float LengthFast
        {
            get
            {
                return 1.0f / MathTools.InverseSqrFast(X * X + Y * Y + Z * Z);
            }
        }

        /// <summary>
        /// Squared lenght for fast distance comparison
        /// </summary>
        [XmlIgnore]
        [Browsable(false)]
        public float LengthSquared
        {
            get
            {
                return (X * X + Y * Y + Z * Z);
            }
        }


        /// <summary>
        /// Returns distance to other vector
        /// </summary>
        /// <param name="vec2">Vector to measure the distance</param>
        /// <returns>Distance between this and vec2</returns>
        public float Distance(Vector3 vec2)
        {
            return Distance(ref vec2);
        }

        /// <summary>
        /// Returns distance to other vector
        /// </summary>
        /// <param name="vec2">Vector to measure the distance</param>
        /// <returns>Distance between this and vec2</returns>
        public float Distance(ref Vector3 vec2)
        {
            float retval = (X - vec2.X) * (X - vec2.X)
                         + (Y - vec2.Y) * (Y - vec2.Y)
                         + (Z - vec2.Z) * (Z - vec2.Z);
            return (float)Math.Sqrt(retval);
        }

        /// <summary>
        /// Returns distance to other vector, fast but unacurrate version (good enough for distance checking)
        /// </summary>
        /// <param name="vec2">Vector to measure the distance</param>
        /// <returns>Distance between this and vec2</returns>
        public float DistanceFast(Vector3 vec2)
        {
            return DistanceFast(ref vec2);
        }

        /// <summary>
        /// Returns distance to other vector, fast but unacurrate version (good enough for distance checking)
        /// </summary>
        /// <param name="vec2">Vector to measure the distance</param>
        /// <returns>Distance between this and vec2</returns>
        public float DistanceFast(ref Vector3 vec2)
        {
            float retval = (X - vec2.X) * (X - vec2.X)
                         + (Y - vec2.Y) * (Y - vec2.Y)
                         + (Z - vec2.Z) * (Z - vec2.Z);
            return 1.0f / MathTools.InverseSqrFast(retval);
        }

        /// <summary>
        /// Returns squared distance to other vector, fast, for distance comparison
        /// </summary>
        /// <param name="vec2">Vector to measure the distance</param>
        /// <returns>Distance between this and vec2</returns>
        public float DistanceSquared(Vector3 vec2)
        {
            return DistanceSquared(ref vec2);
        }

        /// <summary>
        /// Returns squared distance to other vector, fast, for distance comparison
        /// </summary>
        /// <param name="vec2">Vector to measure the distance</param>
        /// <returns>Distance between this and vec2</returns>
        public float DistanceSquared(ref Vector3 vec2)
        {
            return (X - vec2.X) * (X - vec2.X)
                         + (Y - vec2.Y) * (Y - vec2.Y)
                         + (Z - vec2.Z) * (Z - vec2.Z);
        }


        /// <summary>
        /// Dot product with another vector
        /// </summary>
        /// <param name="vec2">Second vector</param>
        /// <returns>Dot product</returns>
        public float Dot(ref Vector3 vec2)
        {
            return this.X * vec2.X + this.Y * vec2.Y + this.Z * vec2.Z;
        }

        /// <summary>
        /// Dot product with another vector
        /// </summary>
        /// <param name="vec2">Second vector</param>
        /// <returns>Dot product</returns>
        public float Dot(Vector3 vec2)
        {
            return this.X * vec2.X + this.Y * vec2.Y + this.Z * vec2.Z;
        }

        /// <summary>
        /// Cross product with another vector
        /// </summary>
        /// <param name="vec2">Second vector</param>
        /// <returns>New Vector3 containing cross product of vectors</returns>
        public Vector3 Cross(ref Vector3 vec2)
        {
            return new Vector3(this.Y * vec2.Z - this.Z * vec2.Y,
                this.Z * vec2.X - this.X * vec2.Z,
                this.X * vec2.Y - this.Y * vec2.X);
        }

        /// <summary>
        /// Cross product with another vector
        /// </summary>
        /// <param name="vec2">Second vector</param>
        /// <returns>New Vector3 containing cross product of vectors</returns>
        public Vector3 Cross(Vector3 vec2)
        {
            return Cross(ref vec2);
        }

        /// <summary>
        /// Mixed product with two other vectors
        /// </summary>
        /// <param name="v2">second operand</param>
        /// <param name="v3">third operand</param>
        /// <returns>Mixed product</returns>
        public float Mixed(ref Vector3 v2, ref Vector3 v3)
        {
            Vector3 v = this.Cross(ref v2);
            return v.Dot(ref v3);
        }

        /// <summary>
        /// Returns normalized copy of this vector
        /// </summary>
        /// <returns>Normalized copy</returns>
        public Vector3 NormalizedCopy()
        {
            Vector3 rval = new Vector3(this);
            rval.Normalize();
            return rval;
        }

        /// <summary>
        /// Computes angle between vectors
        /// </summary>
        /// <param name="v2">second operand</param>
        /// <returns>Angle between this and v2 in radians</returns>
        public float Angle(Vector3 v2)
        {
            Vector3 v1 = NormalizedCopy();
            v2.Normalize();
            return (float)Math.Acos(v1.Dot(ref v2));
        }

        /// <summary>
        /// Computes angle between vectors, expects both vectors to be normalized!
        /// </summary>
        /// <param name="v2">second operand</param>
        /// <returns>Angle between this and v2 (both must be normalized vectors) in radians</returns>
        public float AngleNorm(ref Vector3 v2)
        {
            return (float)Math.Acos(this.Dot(ref v2));
        }


        #endregion

        #region operators

        public static Vector3 operator +(Vector3 left, Vector3 right)
        {
            left.Add(ref right);
            return left;
        }

        public static Vector3 operator -(Vector3 left, Vector3 right)
        {
            left.Substract(ref right);
            return left;
        }

        public static Vector3 operator -(Vector3 vec)
        {
            vec.Invert();
            return vec;
        }

        public static Vector3 operator *(Vector3 vec, float scale)
        {
            vec.Mul(scale);
            return vec;
        }

        public static Vector3 operator *(float scale, Vector3 vec)
        {
            vec.Mul(scale);
            return vec;
        }

        public static Vector3 operator /(Vector3 vec, float scale)
        {
            vec.Divide(scale);
            return vec;
        }

        public static bool operator ==(Vector3 left, Vector3 right)
        {
            return left.Equals(right);
        }

        public static bool operator !=(Vector3 left, Vector3 right)
        {
            return !left.Equals(right);
        }

        /// <summary>
        /// Compares magnitude of the vectors
        /// </summary>
        /// <param name="left">left operand</param>
        /// <param name="right">right operand</param>
        /// <returns>True if left operand's lenght is smaller than right</returns>
        public static bool operator <(Vector3 left, Vector3 right)
        {
            return left.LengthSquared < right.LengthSquared;
        }

        /// <summary>
        /// Compares magnitude of the vectors
        /// </summary>
        /// <param name="left">left operand</param>
        /// <param name="right">right operand</param>
        /// <returns>True if left operand's lenght is bigger than right</returns>
        public static bool operator >(Vector3 left, Vector3 right)
        {
            return left.LengthSquared > right.LengthSquared;
        }

        /// <summary>
        /// Compares magnitude of the vectors
        /// </summary>
        /// <param name="left">left operand</param>
        /// <param name="right">right operand</param>
        /// <returns>True if left operand's lenght is equal or smaller than right</returns>
        public static bool operator <=(Vector3 left, Vector3 right)
        {
            return left.LengthSquared <= right.LengthSquared;
        }

        /// <summary>
        /// Compares magnitude of the vectors
        /// </summary>
        /// <param name="left">left operand</param>
        /// <param name="right">right operand</param>
        /// <returns>True if left operand's lenght is equal or bigger than right</returns>
        public static bool operator >=(Vector3 left, Vector3 right)
        {
            return left.LengthSquared >= right.LengthSquared;
        }


        #endregion

        #region Static functions


        #region Products

        /// <summary>
        /// Dot product of two vectors
        /// </summary>
        /// <param name="v1">first operand</param>
        /// <param name="v2">second operand</param>
        /// <returns>Dot product of the vectors</returns>
        public static float Dot(ref Vector3 v1, ref Vector3 v2)
        {
            return v1.Dot(ref v2);
        }

        /// <summary>
        /// Cross product of two vectors
        /// </summary>
        /// <param name="v1">first operand</param>
        /// <param name="v2">second operand</param>
        /// <returns>Cross product</returns>
        public static Vector3 Cross(ref Vector3 v1, ref Vector3 v2)
        {
            return v1.Cross(ref v2);
        }

        public static Vector3 Cross(Vector3 v1, Vector3 v2)
        {
            return Cross(ref v1, ref v2);
        }
        
        /// <summary>
        /// Mixed product of three vectors
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <param name="v3"></param>
        /// <returns></returns>
        public static float MixedProduct(ref Vector3 v1, ref Vector3 v2, ref Vector3 v3)
        {
            return v1.Mixed(ref v2, ref v3);
        }


        #endregion

        /// <summary>
        /// Returns normalized copy of the vector
        /// </summary>
        /// <param name="v">operand</param>
        /// <returns>Normalized copy of v</returns>
        public static Vector3 Normalize(ref Vector3 v)
        {
            return v.NormalizedCopy();
        }

        public static Vector3 Normalize(Vector3 v)
        {
            return v.NormalizedCopy();
        }


        #region math operations

        /// <summary>
        /// Adds two vectors and sets res with result
        /// </summary>
        /// <param name="v1">first operand</param>
        /// <param name="v2">second operand</param>
        /// <param name="res">vector to save the result</param>
        public static void Add(ref Vector3 v1, ref Vector3 v2, out Vector3 res)
        {
            res.X = v1.X + v2.X;
            res.Y = v1.Y + v2.Y;
            res.Z = v1.Z + v2.Z;
        }

        /// <summary>
        /// Adds two vectors and returns result
        /// </summary>
        /// <param name="v1">first operand</param>
        /// <param name="v2">second operand</param>
        /// <returns>v1+v2</returns>
        public static Vector3 Add(ref Vector3 v1, ref Vector3 v2)
        {
            return new Vector3(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
        }

        /// <summary>
        /// Substracts two vectors and sets res with result
        /// </summary>
        /// <param name="v1">first operand</param>
        /// <param name="v2">second operand</param>
        /// <param name="res">vector to save the result</param>
        public static void Substract(ref Vector3 v1, ref Vector3 v2, out Vector3 res)
        {
            res.X = v1.X - v2.X;
            res.Y = v1.Y - v2.Y;
            res.Z = v1.Z - v2.Z;
        }

        /// <summary>
        /// Substracts two vectors and returns result
        /// </summary>
        /// <param name="v1">first operand</param>
        /// <param name="v2">second operand</param>
        /// <param name="res">v1-v2</param>
        /// <returns></returns>
        public static Vector3 Substract(ref Vector3 v1, ref Vector3 v2)
        {
            return new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
        }

        /// <summary>
        /// Multiplies vector by scalar value and sets result to res
        /// </summary>
        /// <param name="v1">vector</param>
        /// <param name="newscale">scalar</param>
        /// <param name="res">Vector to save result to</param>
        public static void Mul(ref Vector3 v1, float newscale, out Vector3 res)
        {
            res.X = v1.X * newscale;
            res.Y = v1.Y * newscale;
            res.Z = v1.Z * newscale;
        }

        /// <summary>
        /// Multiplies vector by scalar value and returns result
        /// </summary>
        /// <param name="v1">vector</param>
        /// <param name="newscale">scalar</param>
        /// <returns>V1*newscale</returns>
        public static Vector3 Mul(ref Vector3 v1, float newscale)
        {
            return new Vector3(v1.X * newscale, v1.Y * newscale, v1.Z * newscale);
        }

        /// <summary>
        /// Divides vector by scalar value and returns result
        /// </summary>
        /// <param name="v1">Vector</param>
        /// <param name="div">scalar</param>
        /// <returns>v1/div</returns>
        public static Vector3 Divide(ref Vector3 v1, float div)
        {
            Vector3 rval = new Vector3(v1);
            rval.Divide(div);
            return rval;
        }

        /// <summary>
        /// Divides vector by scalar value and returns result
        /// </summary>
        /// <param name="v1">Vector</param>
        /// <param name="div">scalar</param>
        /// <param name="res">Vector to save the result to</param>
        public static void Divide(ref Vector3 v1, float div, out Vector3 res)
        {
            res = new Vector3(v1);
            res.Divide(div);
        }

        /// <summary>
        /// Utility function for calculating AAboxes, will return vector with smallest values of parameters
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <param name="v3"></param>
        /// <returns>Minimal vector</returns>
        public static Vector3 Minimum(ref Vector3 v1, ref Vector3 v2, ref Vector3 v3)
        {
            float x, y, z;
            x = MathTools.Min(v1.X, float.MaxValue); x = MathTools.Min(v2.X, x); x = MathTools.Min(v3.X, x);
            y = MathTools.Min(v1.Y, float.MaxValue); y = MathTools.Min(v2.Y, y); y = MathTools.Min(v3.Y, y);
            z = MathTools.Min(v1.Z, float.MaxValue); z = MathTools.Min(v2.Z, z); z = MathTools.Min(v3.Z, z);
            return new Vector3(x, y, z);
        }

        /// <summary>
        /// Utility function for calculating AAboxes, will return vector with largest values of parameters
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <param name="v3"></param>
        /// <returns>Minimal vector</returns>
        public static Vector3 Maximum(ref Vector3 v1, ref Vector3 v2, ref Vector3 v3)
        {
            float x, y, z;
            x = MathTools.Max(v1.X, float.MinValue); x = MathTools.Max(v2.X, x); x = MathTools.Max(v3.X, x);
            y = MathTools.Max(v1.Y, float.MinValue); y = MathTools.Max(v2.Y, y); y = MathTools.Max(v3.Y, y);
            z = MathTools.Max(v1.Z, float.MinValue); z = MathTools.Max(v2.Z, z); z = MathTools.Max(v3.Z, z);
            return new Vector3(x, y, z);
        }

        #endregion


        #region Clamping

        /// <summary>
        /// Clamps value of the vector to minimum and maximum
        /// </summary>
        /// <param name="v1">Vector to be clamped</param>
        /// <param name="min">Minimal allowed values</param>
        /// <param name="max">Maximal allowed values</param>
        /// <param name="res">Ouput vector</param>
        public static void Clamp(ref Vector3 v1, ref Vector3 min, ref Vector3 max, out Vector3 res)
        {
            res = new Vector3(v1);
            res.Clamp(ref min, ref max);
        }

        /// <summary>
        /// Clamps value of the vector to minimum
        /// </summary>
        /// <param name="v1">Vector to be clamped</param>
        /// <param name="min">Minimal allowed values</param>
        /// <param name="res">Ouput vector</param>
        public static void ClampMin(ref Vector3 v1, ref Vector3 min, out Vector3 res)
        {
            res = new Vector3(v1);
            res.ClampMin(ref min);
        }

        /// <summary>
        /// Clamps value of the vector to maximum
        /// </summary>
        /// <param name="v1">Vector to be clamped</param>
        /// <param name="max">Maximal allowed values</param>
        /// <param name="res">Ouput vector</param>
        public static void ClampMax(ref Vector3 v1, ref Vector3 max, out Vector3 res)
        {
            res = new Vector3(v1);
            res.ClampMax(ref max);
        }

        /// <summary>
        /// Clamps value of the vector to minimum and maximum
        /// </summary>
        /// <param name="v1">Vector to be clamped</param>
        /// <param name="min">Minimal allowed values</param>
        /// <param name="max">Maximal allowed values</param>
        /// <returns>Clamped vector</returns>
        public static Vector3 Clamp(ref Vector3 v1, ref Vector3 min, ref Vector3 max)
        {
            Vector3 res = new Vector3(v1);
            res.Clamp(ref min, ref max);
            return res;
        }

        /// <summary>
        /// Clamps value of the vector to minimum
        /// </summary>
        /// <param name="v1">Vector to be clamped</param>
        /// <param name="min">Minimal allowed values</param>
        /// <returns>Clamped vector</returns>
        public static Vector3 ClampMin(Vector3 v1, ref Vector3 min)
        {
            v1.ClampMin(ref min);
            return v1;
        }

        /// <summary>
        /// Clamps value of the vector to maximum
        /// </summary>
        /// <param name="v1">Vector to be clamped</param>
        /// <param name="max">Maximal allowed values</param>
        /// <returns>Clamped vector</returns>
        public static Vector3 ClampMax(Vector3 v1, ref Vector3 max)
        {
            v1.ClampMax(ref max);
            return v1;
        }

        #endregion



        #region Interpolation

        /// <summary>
        /// Lineary interpolates vector to target vector
        /// </summary>
        /// <param name="source">Source vector</param>
        /// <param name="target">Target vector</param>
        /// <param name="inter">Interpolation value, assumes (0-1)</param>
        /// <param name="result">Vector to save result to</param>
        public static void Interpolate(ref Vector3 source, ref Vector3 target, float inter, out Vector3 result)
        {
            result.X = inter * (target.X - source.X) + source.X;
            result.Y = inter * (target.Y - source.Y) + source.Y;
            result.Z = inter * (target.Z - source.Z) + source.Z;
        }


        /// <summary>
        /// Lineary interpolates vector to target vector
        /// </summary>
        /// <param name="source">Source vector</param>
        /// <param name="target">Target vector</param>
        /// <param name="inter">Interpolation value, assumes (0-1)</param>
        /// <returns></returns>
        public static Vector3 Interpolate(ref Vector3 source, ref Vector3 target, float inter)
        {
            Vector3 result = new Vector3(source);
            result.Interpolate(ref target, inter);
            return result;
        }

        #endregion


        /// <summary>
        /// Moves Vector in speficied direction to specified distance
        /// </summary>
        /// <param name="origin">Origin of the movement</param>
        /// <param name="direction">Direction normal</param>
        /// <param name="distance">Required distance</param>
        /// <returns>Moved vector</returns>
        public static Vector3 MoveInDirection(ref Vector3 origin, ref Vector3 direction, float distance)
        {
            Vector3 rval;
            MoveInDirection(ref origin, ref direction, distance, out rval);
            return rval;
        }

        /// <summary>
        /// Moves Vector in speficied direction to specified distance
        /// </summary>
        /// <param name="origin">Origin of the movement</param>
        /// <param name="direction">Direction normal</param>
        /// <param name="distance">Required distance</param>
        /// <param name="newpos">Vector for new position</param>
        public static void MoveInDirection(ref Vector3 origin, ref Vector3 direction, float distance, out Vector3 newpos)
        {
            newpos = direction;
            newpos.Mul(distance);
            newpos.Add(origin);
        }




        #endregion


        #region overrides

        public override string ToString()
        {
            return String.Format("({0}, {1}, {2})", X, Y, Z);
        }

        public override int GetHashCode()
        {
            return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
        }

        public override bool Equals(object obj)
        {
            if (!(obj is Vector3))
                return false;

            return this.Equals((Vector3)obj);
        }

        #endregion


        #region IEquatable<Vector3> Members

        public bool Equals(Vector3 other)
        {
            return
                X == other.X &&
                Y == other.Y &&
                Z == other.Z;
        }

        public bool Equals(ref Vector3 other)
        {
            return
                X == other.X &&
                Y == other.Y &&
                Z == other.Z;
        }

        #endregion

        public void ClampRPY()
        {
            X = MathTools.ClampRotation(X);
            Y = MathTools.ClampRotation(Y);
            Z = MathTools.ClampRotation(Z);
        }

        #region IXmlSerializable Members

        /// <summary>
        /// Reqired by IXMLserializable
        /// </summary>
        /// <returns>Null</returns>
        public System.Xml.Schema.XmlSchema GetSchema()
        {
            return null;
        }

        /// <summary>
        /// Deserializes object from XML
        /// </summary>
        /// <param name="reader">XML reader</param>
        public void ReadXml(System.Xml.XmlReader reader)
        {            
            XMLHelper.FloatFromString(reader["X"], out X);
            XMLHelper.FloatFromString(reader["Y"], out Y);
            XMLHelper.FloatFromString(reader["Z"], out Z);
        }

        /// <summary>
        /// Serializes object to XML
        /// </summary>
        /// <param name="writer">XML writer to save values</param>
        public void WriteXml(System.Xml.XmlWriter writer)
        {
            XMLHelper.WriteFloatAttribute(writer, "X", X);
            XMLHelper.WriteFloatAttribute(writer, "Y", Y);
            XMLHelper.WriteFloatAttribute(writer, "Z", Z);
        }

        /// <summary>
        /// Serializes object to XML
        /// </summary>
        /// <param name="writer">XML writer to save values</param>
        /// <param name="rootname">Node to save value to</param>
        public void WriteXml(System.Xml.XmlWriter writer, string rootname)
        {
            writer.WriteStartElement(rootname);
            WriteXml(writer);
            writer.WriteEndElement();
        }

        #endregion
    }
}
