using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace VisiMath
{
    /// <summary>
    /// Various float math tools
    /// </summary>
    public static class MathTools
    {
        #region Misc
        
        /// <summary>
        /// Returns nearest higher number then parameter which is power of 2
        /// </summary>
        /// <param name="value"></param>
        /// <returns></returns>
        public static int NearestPower2(int value)
        {
            int x = 2;

            while (x < value)
            {
                x = x * 2;
            }
            return x;
        }

        /// <summary>
        /// Changes size of the texture to nearest power of two
        /// </summary>
        /// <param name="width">Width</param>
        /// <param name="height">Height</param>
        public static void NearestTextureSize(ref int width, ref int height)
        {
            width = NearestPower2(width);
            height = NearestPower2(height);
        }

        #endregion


        #region float math

        /// <summary>
        /// Pi / 2
        /// </summary>
        public static float HalfPi = (float)Math.PI / 2;
        /// <summary>
        /// Pi / 4
        /// </summary>
        public static float QuarterPi = (float)Math.PI / 4;
        /// <summary>
        /// Pi
        /// </summary>
        public static float Pi = (float)Math.PI;
        /// <summary>
        /// Pi * 2
        /// </summary>
        public static float DoublePi = (float)(Math.PI * 2);
        //public static float Epsilon = float.Epsilon;
        /// <summary>
        /// Epsilon value
        /// </summary>
        public static float Epsilon = 0.000001f;

        /// <summary>
        /// Float square root
        /// </summary>
        /// <param name="val">value for square root</param>
        /// <returns>SQRT(val)</returns>
        public static float Sqrt(float val)
        {
            return (float)Math.Sqrt(val);
        }

        /// <summary>
        /// Computes fast inverse sqr
        /// </summary>
        /// <remarks>Based on http://www.codemaestro.com/reviews/review00000105.html </remarks>
        /// <param name="x">value</param>
        /// <returns>Aproximate inverse sqr of value</returns>
        public static float InverseSqrFast(float x)
        {
            unsafe
            {
                float xhalf = 0.5f * x;
                int i = *(int*)&x;              
                i = 0x5f375a86 - (i >> 1);      
                x = *(float*)&i;                
                x = x * (1.5f - xhalf * x * x); 
                return x;
            }
        }

        /// <summary>
        /// Returns a ^ b
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns>a^b</returns>
        public static float Pow(float a, float b)
        {
            return (float)Math.Pow(a, b);
        }

        /// <summary>
        /// returns true if number is almost zero (defined by Epsilon)
        /// </summary>
        /// <param name="val">Float value</param>
        /// <returns>True if almost zero</returns>
        public static bool IsAlmostZero(float val)
        {
            if (Abs(val) < Epsilon)
                return true;
            return false;
        }

        /// <summary>
        /// Returns absolute value
        /// </summary>
        /// <param name="val">float value</param>
        /// <returns>ABS(val)</returns>
        public static float Abs(float val)
        {
            if (val < 0) return -val;
            return val;
        }

        /// <summary>
        /// Returns smaller value
        /// </summary>
        /// <param name="val1">first value</param>
        /// <param name="val2">second value</param>
        /// <returns>Smaller of val1 and val2</returns>
        public static float Min(float val1, float val2)
        {
            if (val1 < val2)
                return val1;
            return val2;
        }

        /// <summary>
        /// Returns smaller value
        /// </summary>
        /// <param name="val1">first value</param>
        /// <param name="val2">second value</param>
        /// <param name="val3">third value</param>
        /// <returns>Smaller of val1 and val2</returns>
        public static float Min(float val1, float val2, float val3)
        {
            if (val1 < val2)
                if (val1 < val3)
                    return val1;
                else 
                return val3;
            if (val2 < val3)
                return val2;
            return val3;
        }

        /// <summary>
        /// returns minimal value inside vector3 
        /// </summary>
        /// <param name="vec">Vector3</param>
        /// <returns>Lowest value from axes</returns>
        public static float Min(Vector3 vec)
        {
            return Min(vec.X, vec.Y, vec.Z);
        }

        /// <summary>
        /// Creates minimal vector from two vectors. result has lowest values from both vectors)
        /// </summary>
        /// <param name="vec1">First vector</param>
        /// <param name="vec2">Second vectr</param>
        /// <returns>Vector with lowest values</returns>
        public static Vector3 Min(ref Vector3 vec1, ref Vector3 vec2)
        {
            return new Vector3(
                Min(vec1.X, vec2.X),
                Min(vec1.Y, vec2.Y),
                Min(vec1.Z, vec2.Z));
        }

        /// <summary>
        /// Creates maximal vector from two vectors. result has maximal values from both vectors)
        /// </summary>
        /// <param name="vec1">First vector</param>
        /// <param name="vec2">Second vectr</param>
        /// <returns>Vector with maximal values</returns>
        public static Vector3 Max(ref Vector3 vec1, ref Vector3 vec2)
        {
            return new Vector3(
                Max(vec1.X, vec2.X),
                Max(vec1.Y, vec2.Y),
                Max(vec1.Z, vec2.Z));
        }

        /// <summary>
        /// Returns bigger value
        /// </summary>
        /// <param name="val1">first value</param>
        /// <param name="val2">second value</param>
        /// <returns>Bigger of val1 and val2</returns>
        public static float Max(float val1, float val2)
        {
            if (val1 > val2)
                return val1;
            return val2;
        }

        /// <summary>
        /// Returns bigger value
        /// </summary>
        /// <param name="val1">first value</param>
        /// <param name="val2">second value</param>
        /// <param name="val3">third value</param>
        /// <returns>Bigger of val1 and val2</returns>
        public static float Max(float val1, float val2, float val3)
        {
            if (val1 > val2)
                if (val1 > val3)
                    return val1;
                else
                    return val3;
            else
                if (val2 > val3)
                    return val2;
                else
                    return val3;
        }

        /// <summary>
        /// returns Maximal value inside vector3 
        /// </summary>
        /// <param name="vec">Vector3</param>
        /// <returns>Max value from axes</returns>
        public static float Max(Vector3 vec)
        {
            return Max(vec.X, vec.Y, vec.Z);
        }

        /// <summary>
        /// Clamps float value to unit - 0 to 1
        /// </summary>
        /// <param name="val">Unit to be clamped</param>
        public static void ClampToUnit(ref float val)
        {
            Clamp(ref val, 0.0f, 1.0f);
        }

        /// <summary>
        /// Clamps float value to unit - 0 to 1
        /// </summary>
        /// <param name="val">Unit to be clamped</param>
        /// <returns>Clamped value</returns>
        public static float ClampToUnit(float val)
        {
            return Clamp(val, 0.0f, 1.0f);
        }

        /// <summary>
        /// Returns clamped float value
        /// </summary>
        /// <param name="val">value to be clamped</param>
        /// <param name="min">minimal possible value</param>
        /// <param name="max">maximal possible value</param>
        /// <returns>Clamped float value</returns>
        public static float Clamp(float val, float min, float max)
        {
            if (val < min) return min;
            if (val > max) return max;
            return val;
        }

        /// <summary>
        /// Clamps float value
        /// </summary>
        /// <param name="val">value to be clamped</param>
        /// <param name="min">minimal possible value</param>
        /// <param name="max">maximal possible value</param>
        public static void Clamp(ref float val, float min, float max)
        {
            if (val < min)
            {
                val = min;
                return;
            }
            if (val > max)
            {
                val = max;
            }
        }

        /// <summary>
        /// Returns clamped int value
        /// </summary>
        /// <param name="val">value to be clamped</param>
        /// <param name="min">minimal possible value</param>
        /// <param name="max">maximal possible value</param>
        /// <returns>Clamped int value</returns>
        public static int Clamp(int val, int min, int max)
        {
            if (val < min) return min;
            if (val > max) return max;
            return val;
        }

        /// <summary>
        /// Returns clamped int value
        /// </summary>
        /// <param name="val">value to be clamped</param>
        /// <param name="min">minimal possible value</param>
        /// <returns>Clamped int value</returns>
        public static int ClampMin(int val, int min)
        {
            if (val < min) return min;
            return val;
        }

        /// <summary>
        /// Returns clamped int value
        /// </summary>
        /// <param name="val">value to be clamped</param>
        /// <param name="max">maximal possible value</param>
        /// <returns>Clamped int value</returns>
        public static int ClampMax(int val, int max)
        {
            if (val > max) return max;
            return val;
        }

        /// <summary>
        /// Returns float value clamped to minimal allowed
        /// </summary>
        /// <param name="val">value to be clamped</param>
        /// <param name="min">minimal possible value</param>
        /// <returns>Clamped float value</returns>
        public static float ClampMin(float val, float min)
        {
            ClampMin(ref val, min);
            return val;
        }

        /// <summary>
        /// Returns float value clamped to minimal allowed
        /// </summary>
        /// <param name="val">value to be clamped</param>
        /// <param name="min">minimal possible value</param>
        public static void ClampMin(ref float val, float min)
        {
            if (val < min) val = min;
        }

        /// <summary>
        /// Returns float value clamped to maximum allowed
        /// </summary>
        /// <param name="val">value to be clamped</param>
        /// <param name="max">maximal possible value</param>
        public static void ClampMax(ref float val, float max)
        {
            if (val > max) val = max;
        }

        /// <summary>
        /// Returns float value clamped to maximum allowed
        /// </summary>
        /// <param name="val">value to be clamped</param>
        /// <param name="max">maximal possible value</param>
        /// <returns>Clamped float value</returns>
        public static float ClampMax(float val, float max)
        {
            ClampMax(ref val, max);
            return val;
        }

        /// <summary>
        /// Returns largest integer less or equal to val
        /// </summary>
        /// <param name="val">Float value</param>
        /// <returns>Floored value</returns>
        public static float Floor(float val)
        {
            return (float)Math.Floor(val);
        }
        
        /// <summary>
        /// Floors (changes to larges integer less or equal) val
        /// </summary>
        /// <param name="val">Float value</param>
        public static void Floor(ref float val)
        {
            val = (float)Math.Floor(val);
        }

        /// <summary>
        /// returns fractional part of the value
        /// </summary>
        /// <param name="val">Float value</param>
        /// <returns>Fractional part</returns>
        public static float FractionalPart(float val)
        {
            return val - Floor(val);
        }

        /// <summary>
        /// Swaps two float values
        /// </summary>
        /// <param name="val1">first value</param>
        /// <param name="val2">second value</param>
        public static void Swap(ref float val1, ref float val2)
        {
            float temp = val2;
            val2 = val1;
            val1 = temp;
        }

        /// <summary>
        /// Swaps two integer values
        /// </summary>
        /// <param name="val1">first value</param>
        /// <param name="val2">second value</param>
        public static void Swap(ref int val1, ref int val2)
        {
            int temp = val2;
            val2 = val1;
            val1 = temp;
        }

        /// <summary>
        /// Checks two float values for equality with epsilon
        /// </summary>
        /// <param name="val1">first value</param>
        /// <param name="val2">second value</param>
        /// <param name="epsilon">required epsilon</param>
        /// <returns>True of values are almost same</returns>
        public static bool AlmostEqual(float val1, float val2, float epsilon = 0.00001f)
        {
            if (Abs(val1 - val2) < epsilon)
                return true;
            return false;
        }

        /// <summary>
        /// Returns float sinus
        /// </summary>
        /// <param name="angle">angle in radians</param>
        /// <returns>sin(angle)</returns>
        public static float Sin(float angle)
        {
            return (float)Math.Sin((double)angle);
        }

        /// <summary>
        /// Returns float cosinus
        /// </summary>
        /// <param name="angle">angle in radians</param>
        /// <returns>cos(angle)</returns>
        public static float Cos(float angle)
        {
            return (float)Math.Cos((double)angle);
        }

        /// <summary>
        /// Arcus sinus
        /// </summary>
        /// <param name="value">value</param>
        /// <returns>ASIN(value)</returns>
        public static float ASin(float value)
        {
            return (float)Math.Asin(value);
        }

        /// <summary>
        /// Arcus cosinus
        /// </summary>
        /// <param name="value">value</param>
        /// <returns>Acos(value)</returns>
        public static float ACos(float value)
        {
            return (float)Math.Acos(value);
        }

        /// <summary>
        /// Arcus Tangens
        /// </summary>
        /// <param name="value">Value</param>
        /// <returns>Atan</returns>
        public static float ATan(float value)
        {
            return (float)Math.Atan(value);
        }

        /// <summary>
        /// Arcus Tangens 2
        /// </summary>
        /// <param name="value1">first value</param>
        /// <param name="value2">second value</param>
        /// <returns>Atan</returns>
        public static float ATan2(float value1, float value2)
        {
            return (float)Math.Atan2(value1, value2);
        }

        /// <summary>
        /// Converts degrees to radians
        /// </summary>
        /// <param name="radians">radians</param>
        /// <param name="clamp">if true, value will be clamped</param>
        /// <returns>degrees</returns>
        public static float RadToDeg(float radians, bool clamp = false)
        {
            if (clamp)
                ClampRotation(ref radians);
            return 180 * (radians / Pi);
        }
        
        /// <summary>
        /// converts radians to degrees
        /// </summary>
        /// <param name="degrees">degrees</param>
        /// <param name="clamp">if true, value will be clamped</param>
        /// <returns>radians</returns>
        public static float DegToRad(float degrees, bool clamp = false)
        {
            float rval = Pi * (degrees / 180.0f);
            if (clamp)
                ClampRotation(ref rval);
            return rval;
        }

        /// <summary>
        /// Converts rotational vector in degrees to radians
        /// </summary>
        /// <param name="rot">Rotational vector in degrees</param>
        /// <returns>Rotational vector in radians</returns>
        public static Vector3 DegToRad(Vector3 rot)
        {
            DegToRad(ref rot);
            return rot;
        }

        /// <summary>
        /// Converts rotational vector in degrees to radians
        /// </summary>
        /// <param name="rot">Rotational vector in degrees</param>
        public static void DegToRad(ref Vector3 rot)
        {
            rot.X = DegToRad(rot.X, true);
            rot.Y = DegToRad(rot.Y, true);
            rot.Z = DegToRad(rot.Z, true);
        }

        /// <summary>
        /// Converts rotational vector in radians to degrees
        /// </summary>
        /// <param name="rot">Rotational vector in radians</param>
        /// <returns>Rotational vector in degrees</returns>
        public static Vector3 RadToDeg(Vector3 rot)
        {
            RadToDeg(ref rot);
            return rot;
        }

        /// <summary>
        /// Converts rotational vector in radians to degrees
        /// </summary>
        /// <param name="rot">Rotational vector in radians</param>
        public static void RadToDeg(ref Vector3 rot)
        {
            rot.X = RadToDeg(rot.X, true);
            rot.Y = RadToDeg(rot.Y, true);
            rot.Z = RadToDeg(rot.Z, true);
        }


        /// <summary>
        /// Clamps rotation to -2pi to 2pi
        /// </summary>
        /// <param name="angle">angle</param>
        /// <returns>clamped angle</returns>
        public static float ClampRotation(float angle)
        {
            ClampRotation(ref angle);
            return angle;

        }

        /// <summary>
        /// Clamps rotation to -2pi to 2pi
        /// </summary>
        /// <param name="angle">angle</param>
        public static void ClampRotation(ref float angle)
        {
            while (angle > DoublePi)
                angle -= DoublePi;
            while (angle < -DoublePi)
                angle += DoublePi;
        }

        /// <summary>
        /// Clamps rotation RPY vector to -2pi to 2pi 
        /// </summary>
        /// <param name="rot">rotation vector</param>
        /// <returns>Clamped version</returns>
        public static Vector3 ClampRotation(Vector3 rot)
        {
            ClampRotation(ref rot);
            return rot;
        }

        /// <summary>
        /// Clamps rotation RPY vector to -2pi to 2pi 
        /// </summary>
        /// <param name="rot">rotation vector</param>
        public static void ClampRotation(ref Vector3 rot)
        {
            rot.X = ClampRotation(rot.X);
            rot.Y = ClampRotation(rot.Y);
            rot.Z = ClampRotation(rot.Z);
        }

        /// <summary>
        /// Calculates bias
        /// </summary>
        /// <param name="val">Value</param>
        /// <returns>Bias of value</returns>
        public static float Bias(float val)
        {
            return (float)Math.Pow(val, Math.Log10(val) / (-0.30102999566398119521373889472449)); //log(0.5)
        }

        /// <summary>
        /// Calculates gain
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static float Gain(float a, float b)
        {
	        double p = Math.Log10(1.0 - b) / (-0.30102999566398119521373889472449); //log(0.5)

	        if (a < .001)
		        return 0;
	        else if (a > .999)
		        return 1;
	        if (a < 0.5f)
		        return (float)(Math.Pow(2 * a, p) / 2);
	        else
		        return 1.0f - (float)Math.Pow(2 * (1.0 - a), p) / 2;
        }

        /// <summary>
        /// Calculates linear interpolation from a to b
        /// </summary>
        /// <param name="t">interpolation parameter</param>
        /// <param name="a">A point</param>
        /// <param name="b">B point</param>
        /// <returns>Interpolated value</returns>
        public static float Lerp(float t, float a, float b)
        {
            return a + t * (b - a);
        }

        /// <summary>
        /// Interpolation
        /// </summary>
        /// <param name="t">Interpolation parameter</param>
        /// <param name="a">First value</param>
        /// <param name="b">Second value</param>
        /// <returns>Interpolated value</returns>
        public static float Interp(float t, float a, float b)
        {
            float tt = t * t * (3 - 2 * t);
            return Lerp(tt, a, b);
        }


        /// <summary>
        /// Bicubic interpolation
        /// </summary>
        /// <param name="tx"></param>
        /// <param name="ty"></param>
        /// <param name="v0"></param>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <param name="v3"></param>
        /// <returns></returns>
        public static float BicubInterp(float tx, float ty, float v0, float v1, float v2, float v3)
        {
            float wx = tx * tx * (3 - 2 * tx);
            float a = Lerp(wx, v0, v1);
            float b = Lerp(wx, v2, v3);
            return Interp(ty, a, b);
        }

        /// <summary>
        /// Tricubic interpolation
        /// </summary>
        /// <param name="tx"></param>
        /// <param name="ty"></param>
        /// <param name="tz"></param>
        /// <param name="v0"></param>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <param name="v3"></param>
        /// <param name="v4"></param>
        /// <param name="v5"></param>
        /// <param name="v6"></param>
        /// <param name="v7"></param>
        /// <returns></returns>
        public static float TricubInterp(float tx, float ty, float tz,
            float v0, float v1, float v2, float v3, float v4, float v5, float v6, float v7)
        {
            float a = BicubInterp(tx, ty, v0, v1, v2, v3);
            float b = BicubInterp(tx, ty, v4, v5, v6, v7);
            return Interp(tz, a, b);
        }

        /// <summary>
        /// Calculates cosine interpolation from a to b
        /// </summary>
        /// <param name="t">interpolation parameter</param>
        /// <param name="a">A point</param>
        /// <param name="b">B point</param>
        /// <returns>Interpolated value</returns>
        public static float CosInterp(float t, float a, float b)
        {
            float ft = t * Pi;
            float f = (1 - Cos(ft)) * 0.5f;
            return a * (1 - ft) + b * f;
        }

        /// <summary>
        /// Returns cubic interpolation between v1 and v2 from four values
        /// </summary>
        /// <param name="t">interpolation parameter (0-1)</param>
        /// <param name="v0"></param>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <param name="v3"></param>
        /// <returns>Interpolated value</returns>
        public static float CubicInterp(float t, float v0, float v1, float v2, float v3)
        {
            float p = (v3 - v2) - (v0 - v1);
            float q = (v0 - v1) - p;
            float r = v2 - v0;

            return p * t * t * t + q * t * t + r * t + v1;
        }

        #endregion


        #region ContainsPoint

        /// <summary>
        /// Checks if point lies on the inifinite line and sets t to line parameter.
        /// Parameter can be used to decide if the point is in some part of the line boundry
        /// </summary>
        /// <param name="l1">Start of the line</param>
        /// <param name="l2">End of the line</param>
        /// <param name="pos">Vector to check</param>
        /// <param name="t">Line parameter (valid only if function returns true</param>
        /// <returns>True if point is on the line</returns>
        public static bool PointOnLine(ref Vector3 l1, ref Vector3 l2, ref Vector3 pos, out float t)
        {
            Vector3 dir = l2;
            dir.Substract(ref l1);
                        
            Vector3 v2 = pos;
            v2.Substract(ref l1);

            //Point is on the line if dir * t = pos

            t = pos.X / dir.X;
            if ((pos.Y / dir.Y) == t)
            {
                if ((pos.Z / dir.Z) == t)
                {
                    return true;
                }
            }
            return false;
        }

        /// <summary>
        /// Checks if point lies on the inifinite line
        /// </summary>
        /// <param name="l1">Start of the line</param>
        /// <param name="l2">End of the line</param>
        /// <param name="pos">Vector to check</param>
        /// <returns>True if point is on the line</returns>
        public static bool PointOnLine(ref Vector3 l1, ref Vector3 l2, ref Vector3 pos)
        {
            float t;
            return PointOnLine(ref l1, ref l2, ref pos, out t);
        }

        /// <summary>
        /// Checks if point lies on the linesegment
        /// </summary>
        /// <param name="l1">Start of the line</param>
        /// <param name="l2">End of the line</param>
        /// <param name="pos">Vector to check</param>
        /// <returns>True if point is on the line</returns>
        public static bool PointOnLineSegment(Vector3 l1, Vector3 l2, Vector3 pos)
        {
            float t;
            if (PointOnLine(ref l1, ref l2, ref pos, out t))
            {
                if ((t < 0) || (t > 1))
                    return false;
                return true;
            }
            return false;
        }

        /// <summary>
        /// Checks if point lies on the linesegment
        /// </summary>
        /// <param name="l1">Start of the line</param>
        /// <param name="l2">End of the line</param>
        /// <param name="pos">Vector to check</param>
        /// <returns>True if point is on the line</returns>
        public static bool PointOnLineSegment(ref Vector3 l1, ref Vector3 l2, ref Vector3 pos)
        {
            float t;
            if (PointOnLine(ref l1, ref l2, ref pos, out t))
            {
                if ((t < 0) || (t > 1))
                    return false;
                return true;
            }
            return false;
        }


        /// <summary>
        /// Returns true if point is inside the triangle
        /// </summary>
        /// <param name="v1">First corner</param>
        /// <param name="v2">Second corner</param>
        /// <param name="v3">Third corner</param>
        /// <param name="p">Point to check</param>
        /// <returns>True if point is inside the triangle</returns>
        /// <remarks>Uses barycentric technique (faster then sameside checking)</remarks>
        public static bool PointInTriangle(Vector3 v1, Vector3 v2, Vector3 v3, Vector3 p)
        {
            return PointInTriangle(ref v1, ref v2, ref v3, ref p);
        }

        /// <summary>
        /// Returns true if point is inside the triangle
        /// </summary>
        /// <param name="v1">First corner</param>
        /// <param name="v2">Second corner</param>
        /// <param name="v3">Third corner</param>
        /// <param name="p">Point to check</param>
        /// <returns>True if point is inside the triangle</returns>
        /// <remarks>Uses barycentric technique (faster then sameside checking)</remarks>
        public static bool PointInTriangle(ref Vector3 v1, ref Vector3 v2, ref Vector3 v3, ref Vector3 p)
        {
            Vector3 edge0 = v3 - v1;
            Vector3 edge1 = v2 - v1;
            Vector3 edge2 = p - v1;

            float dot00 = edge0.Dot(ref edge0);
            float dot01 = edge0.Dot(ref edge1);
            float dot02 = edge0.Dot(ref edge2);
            float dot11 = edge1.Dot(ref edge1);
            float dot12 = edge1.Dot(ref edge2);

            float invDenominator =  1.0f / (dot00 * dot11 - dot01 * dot01);
            float u = (dot11 * dot02 - dot01 * dot12) * invDenominator;
            float v = (dot00 * dot12 - dot01 * dot02) * invDenominator;
            
            if (((u > 0) && (v > 0) && (u + v < 1)))
                return true;
            return false;
        }

        #endregion

        #region Triangle normals

        /// <summary>
        /// Calculates normal of the triangle (position must be in clockwise order)
        /// </summary>
        /// <param name="v1">First corner</param>
        /// <param name="v2">Second corner</param>
        /// <param name="v3">Third corner</param>
        /// <param name="normalize">if set to false, normal won't be normalized after calculation! (usable for some applications)</param>
        /// <param name="norm">Normal will be saved here</param>
        public static void CalcTriNormal(ref Vector3 v1, ref Vector3 v2, ref Vector3 v3, out Vector3 norm, bool normalize = true)
        {
            Vector3 vec1 = Vector3.Substract(ref v2, ref v1);
            Vector3 vec2 = Vector3.Substract(ref v3, ref v1);   //NOTE, this is counter clocwise order???
            norm = vec1.Cross(ref vec2);
            norm.Normalize();            
        }


        /// <summary>
        /// Calculates normal of the triangle (position must be in clockwise order)
        /// </summary>
        /// <param name="v1">First corner</param>
        /// <param name="v2">Second corner</param>
        /// <param name="v3">Third corner</param>
        /// <param name="normalize">if set to false, normal won't be normalized after calculation! (usable for some applications)</param>
        /// <returns>Normal of the triangle face</returns>
        public static Vector3 CalcTriNormal(ref Vector3 v1, ref Vector3 v2, ref Vector3 v3, bool normalize = true)
        {
            Vector3 norm;
            CalcTriNormal(ref v1, ref v2, ref v3, out norm, normalize);
            return norm;
        }

        /// <summary>
        /// Calculates normal of the triangle (position must be in clockwise order)
        /// </summary>
        /// <param name="v1">First corner</param>
        /// <param name="v2">Second corner</param>
        /// <param name="v3">Third corner</param>
        /// <param name="normalize">if set to false, normal won't be normalized after calculation! (usable for some applications)</param>
        /// <returns>Normal of the triangle face</returns>
        public static Vector3 CalcTriNormal(Vector3 v1, Vector3 v2, Vector3 v3, bool normalize = true)
        {
            return CalcTriNormal(ref v1, ref v2, ref v3, normalize);
        }

        #endregion


        #region Distances

        #region Distance Vector to...

        /// <summary>
        /// Returns distance of the position from the linesegment
        /// </summary>
        /// <param name="l1">First end of the line</param>
        /// <param name="l2">Second end of the line</param>
        /// <param name="pos">Position vector</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorLineSegment(Vector3 l1, Vector3 l2, Vector3 pos)
        {
            return DistanceVectorLineSegment(ref l1, ref l2, ref pos);
        }


        /// <summary>
        /// Returns distance of the position from the linesegment
        /// </summary>
        /// <param name="l1">First end of the line</param>
        /// <param name="l2">Second end of the line</param>
        /// <param name="pos">Position vector</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorLineSegment(ref Vector3 l1, ref Vector3 l2, ref Vector3 pos)
        {
            float r;
            return DistanceVectorLineSegment(ref l1, ref l2, ref pos, out r);
        }

        /// <summary>
        /// Returns distance of the position from the linesegment
        /// </summary>
        /// <param name="l1">First end of the line</param>
        /// <param name="l2">Second end of the line</param>
        /// <param name="pos">Position vector</param>
        /// <param name="r">Will be set to parameter on the line</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorLineSegment(ref Vector3 l1, ref Vector3 l2, ref Vector3 pos, out float r)
        {
            Vector3 dir = l2;
            dir.Substract(ref l1);
            Vector3 v2 = pos;
            v2.Substract(ref l1);
            float lsq = dir.LengthSquared;
            r = dir.Dot(ref v2) / lsq;


            if ((r < 0.0f) || (r > 1.0f))
            {
                //point is not inside the segment boundary
                float dst1 = pos.Distance(ref l1);
                float dst2 = pos.Distance(ref l2);
                if (dst1 < dst2)
                    return dst1;
                return dst2;
            }
            return r * Sqrt(lsq);            
        }

        /// <summary>
        /// Returns distance of the position from the infinite line
        /// </summary>
        /// <param name="l1">One point on the line</param>
        /// <param name="l2">Second point of the line</param>
        /// <param name="pos">Position vector</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorLine(ref Vector3 l1, ref Vector3 l2, ref Vector3 pos)
        {
            Vector3 dir = l2;
            dir.Substract(ref l1);
            Vector3 v2 = pos;
            v2.Substract(ref l1);

            return dir.Dot(ref v2) * dir.Length;
        }

        /// <summary>
        /// Returns distance of the position from the ray
        /// </summary>
        /// <param name="orig">Origin of the ray</param>
        /// <param name="dir">Normalized direction of the ray</param>
        /// <param name="pos">Position vector</param>
        /// <param name="frontOnly">Check only front of the ray</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorRay(ref Vector3 orig, ref Vector3 dir, ref Vector3 pos, bool frontOnly = true)
        {
            Vector3 v2 = pos;
            v2.Substract(ref orig);
            float r = dir.Dot(ref v2);
            if (frontOnly && (r < 0.0f))
            {
                //point is behind the ray
                return pos.DistanceSquared(ref orig);
            }
            return r;
        }

        /// <summary>
        /// Calculates distance of the vector from the triangle
        /// </summary>
        /// <param name="first">First corner</param>
        /// <param name="second">Second corner</param>
        /// <param name="third">Third corner</param>
        /// <param name="vec">Position</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorTriangle(Vector3 first, Vector3 second, Vector3 third, Vector3 vec)
        {
            return DistanceVectorTriangle(ref first, ref second, ref third, ref vec);
        }


        /// <summary>
        /// Calculates distance of the vector from the triangle
        /// </summary>
        /// <param name="first">First corner</param>
        /// <param name="second">Second corner</param>
        /// <param name="third">Third corner</param>
        /// <param name="vec">Position</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorTriangle(ref Vector3 first, ref Vector3 second, ref Vector3 third, ref Vector3 vec)
        {
            float dst1 = vec.DistanceSquared(ref first);
            float dst2 = vec.DistanceSquared(ref second);
            float dst3 = vec.DistanceSquared(ref third);

            float len = second.DistanceSquared(ref first);
            if ((len > dst1) && (len > dst2))
            {
                len = third.DistanceSquared(ref second);
                if ((len > dst2) && (len > dst3))
                {
                    len = first.DistanceSquared(ref third);
                    if ((len > dst1) && (len > dst3))
                    {
                        Vector3 normal = CalcTriNormal(ref first, ref second, ref third, true);
                        normal.Mul(-1);
                        float d = normal.Dot(ref first);
                        return Math.Abs(normal.Dot(ref vec) - d);		//distance from the plane
                    }
                }
            }
            if (dst1 < dst2)
                return dst1 < dst3 ? Sqrt(dst1) : Sqrt(dst3);
            return dst2 < dst3 ? Sqrt(dst2) : Sqrt(dst3);
        }

        /// <summary>
        /// Calculates distance of the vector from the quad
        /// </summary>
        /// <param name="first">First corner</param>
        /// <param name="second">Second corner</param>
        /// <param name="third">Third corner</param>
        /// <param name="fourth">Fourth corner</param>
        /// <param name="vec">Position</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorQuad(ref Vector3 first, ref Vector3 second, ref Vector3 third, ref Vector3 fourth, ref Vector3 vec)
        {
            //optimize!!!
            float dst1 = DistanceVectorTriangle(ref first, ref second, ref third, ref vec);
            float dst2 = DistanceVectorTriangle(ref third, ref fourth, ref first, ref vec);
            return (dst1 < dst2) ? dst1 : dst2;
        }

        /// <summary>
        /// Calculates distance of the vector from the quad
        /// </summary>
        /// <param name="first">First corner</param>
        /// <param name="second">Second corner</param>
        /// <param name="third">Third corner</param>
        /// <param name="fourth">Fourth corner</param>
        /// <param name="vec">Position</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorQuad(Vector3 first, Vector3 second, Vector3 third, Vector3 fourth, Vector3 vec)
        {
            return DistanceVectorQuad(ref first, ref second, ref third, ref fourth, ref vec);
        }


        /// <summary>
        /// Calculates distance between point and sphere
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius of the sphere</param>
        /// <param name="pos">Vector</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorSphere(ref Vector3 center, float radius, ref Vector3 pos)
        {
            float dst = center.Distance(ref pos);
            dst -= radius;
            ClampMin(ref dst, 0);
            return dst;
        }

        /// <summary>
        /// Calculates distance between point and sphere
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius of the sphere</param>
        /// <param name="pos">Vector</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorSphere(Vector3 center, float radius, Vector3 pos)
        {
            return DistanceVectorSphere(ref center, radius, ref pos);
        }

        /// <summary>
        /// Returns distance between vector and plane
        /// </summary>
        /// <param name="point">Point on the plane</param>
        /// <param name="normal">NORMALIZED Plane normal</param>
        /// <param name="pos">Vector</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorPlane(ref Vector3 point, ref Vector3 normal, ref Vector3 pos)
        {
            float t = normal.Dot(ref pos) - normal.Dot(ref point);
            t = t / normal.Dot(ref normal);
            Vector3 closest = Vector3.MoveInDirection(ref pos, ref normal, t);
            return closest.Distance(ref point);
        }


        /// <summary>
        /// Returns distance between vector and plane
        /// </summary>
        /// <param name="point">Point on the plane</param>
        /// <param name="normal">Plane normal</param>
        /// <param name="pos">Vector</param>
        /// <returns>Distance</returns>
        public static float DistanceVectorPlane(Vector3 point, Vector3 normal, Vector3 pos)
        {
            return DistanceVectorPlane(ref point, ref normal, ref pos);
        }



        #endregion

        #region Distance ray to...

        /// <summary>
        /// Calculates distance of the ray to the triangle (0 if intersection)
        /// </summary>
        /// <param name="orig">Origin of ray</param>
        /// <param name="dir">Normalized direction of ray</param>
        /// <param name="first">First triangle vertice </param>
        /// <param name="second">Second triangle vertice </param>
        /// <param name="third">Third triangle vertice </param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public static float DistanceRayTriangle(Vector3 orig, Vector3 dir, Vector3 first, Vector3 second, Vector3 third, bool frontOnly = true)
        {
            return DistanceRayTriangle(ref orig, ref dir, ref first, ref second, ref third, frontOnly);
        }
        
        /// <summary>
        /// Calculates distance of the ray to the triangle (0 if intersection)
        /// </summary>
        /// <param name="orig">Origin of ray</param>
        /// <param name="dir">Normalized direction of ray</param>
        /// <param name="first">First triangle vertice </param>
        /// <param name="second">Second triangle vertice </param>
        /// <param name="third">Third triangle vertice </param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public static float DistanceRayTriangle(ref Vector3 orig, ref Vector3 dir, ref Vector3 first, ref Vector3 second, ref Vector3 third, bool frontOnly = true)
        {
            Vector3 inters = IntersectionRayTriangle(ref orig, ref dir, ref first, ref second, ref third, false);
            if (inters.IsValid())
                return 0;

            float dst1 = DistanceVectorRay(ref orig, ref dir, ref first, frontOnly);
            float dst2 = DistanceVectorRay(ref orig, ref dir, ref second, frontOnly);
            float dst3 = DistanceVectorRay(ref orig, ref dir, ref third, frontOnly);
            if (dst1 < dst2)
                return dst1 < dst3 ? dst1 : dst3;
            return dst2 < dst3 ? dst2 : dst3;
            //Optimize, doesn't check for intersections?!!!
        }

        /// <summary>
        /// Computes distance of the ray to the line segment
        /// </summary>
        /// <param name="orig">Origin of the ray</param>
        /// <param name="dir">Normalized direction</param>
        /// <param name="l1">First point of the line</param>
        /// <param name="l2">Second point of the line</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public static float DistanceRayToLineSegment(Vector3 orig, Vector3 dir, Vector3 l1, Vector3 l2, bool frontOnly = true)
        {
            return DistanceRayToLineSegment(ref orig, ref dir, ref l1, ref l2, frontOnly);
        }


                /// <summary>
        /// Computes distance of the ray to the line segment
        /// </summary>
        /// <param name="orig1">Origin of the ray</param>
        /// <param name="norm1">Normalized direction</param>
        /// <param name="orig2">First point of the line</param>
        /// <param name="targ2">Second point of the line</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public static float DistanceRayToLineSegment(ref Vector3 orig1, ref Vector3 norm1, ref Vector3 orig2, ref Vector3 targ2, bool frontOnly = true)
        {
            float t;
            return DistanceRayToLineSegment(ref orig1, ref norm1, ref orig2, ref targ2, out t, frontOnly);
        }

        /// <summary>
        /// Computes distance of the ray to the line segment
        /// </summary>
        /// <param name="orig1">Origin of the ray</param>
        /// <param name="norm1">Normalized direction</param>
        /// <param name="orig2">First point of the line</param>
        /// <param name="targ2">Second point of the line</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <param name="t">t parameter of the ray</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public static float DistanceRayToLineSegment(ref Vector3 orig1, ref Vector3 norm1, ref Vector3 orig2, ref Vector3 targ2, out float t, bool frontOnly = true)
        {
            Vector3 r = orig1 - orig2;
            Vector3 norm2 = targ2 - orig2;

            float s;

            t = 0;

            float a = norm1.Dot(ref norm1);
            float e = norm2.Dot(ref norm2);
            float c = norm1.Dot(ref r);
            float f = norm2.Dot(ref r);

            if (IsAlmostZero(e))
            {
                //second line is degenerated
                t = 0;
                s = -c / a;

                if ((s < 0) && frontOnly)
                    return float.NaN;
            }
            else
            {
                float b = norm1.Dot(ref norm2);
                float d = (a * e - b * b);

                if (IsAlmostZero(d))
                {
                    //lines are paralel!
                    s = 0.0f;
                }
                else
                {
                    s = (b * f - c * e) / d;
                    if ((s < 0) && frontOnly)
                        return float.NaN;
                }

                t = ClampToUnit((b * s + f) / e);
            }

            //this probably doesn't work!
            Vector3 L1 = Vector3.MoveInDirection(ref orig1, ref norm1, s);
            Vector3 L2 = Vector3.MoveInDirection(ref orig2, ref norm2, t);

            return L1.Distance(ref L2);
        }

        /// <summary>
        /// Computes distance of two rays
        /// </summary>
        /// <param name="orig1">Origin of the ray</param>
        /// <param name="norm1">Normalized direction</param>
        /// <param name="orig2">Origin of the ray 2</param>
        /// <param name="norm2">Normalized direction of the ray 2</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public static float DistanceTwoRays(Vector3 orig1, Vector3 norm1, Vector3 orig2, Vector3 norm2, bool frontOnly = true)
        {
            return DistanceTwoRays(ref orig1, ref norm1, ref orig2, ref norm2, frontOnly);
        }


        /// <summary>
        /// Computes distance of two rays
        /// </summary>
        /// <param name="orig1">Origin of the ray</param>
        /// <param name="norm1">Normalized direction</param>
        /// <param name="orig2">Origin of the ray 2</param>
        /// <param name="norm2">Normalized direction of the ray 2</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public static float DistanceTwoRays(ref Vector3 orig1, ref Vector3 norm1, ref Vector3 orig2, ref Vector3 norm2, bool frontOnly = true)
        {
            float s;
            return DistanceTwoRays(ref orig1, ref norm1, ref orig2, ref norm2, out s, frontOnly);
        }

        /// <summary>
        /// Computes distance of two rays
        /// </summary>
        /// <param name="orig1">Origin of the ray</param>
        /// <param name="norm1">Normalized direction</param>
        /// <param name="orig2">Origin of the ray 2</param>
        /// <param name="norm2">Normalized direction of the ray 2</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <param name="s">Parameter for the closest point of the first ray</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public static float DistanceTwoRays(ref Vector3 orig1, ref Vector3 norm1, ref Vector3 orig2, ref Vector3 norm2, out float s, bool frontOnly = true)
        {
            Vector3 r = orig1 - orig2;
            float t;
            float a = norm1.Dot(ref norm1);
            float e = norm2.Dot(ref norm2);
            float c = norm1.Dot(ref r);
            float f = norm2.Dot(ref r);
            float b = norm1.Dot(ref norm2);
            float d = (a * e - b * b);
            s = (b * f - c * e) / d;
            if ((s < 0) && frontOnly)
                return float.NaN;
            t = (b * s + f) / e;
            ClampMin(ref t, 0);
            Vector3 L1 = Vector3.MoveInDirection(ref orig1, ref norm1, s);
            Vector3 L2 = Vector3.MoveInDirection(ref orig2, ref norm2, t);

            return L1.Distance(ref L2);
        }


        /// <summary>
        /// Returns distance of the ray to quad
        /// </summary>
        /// <param name="orig1">Origin of the ray</param>
        /// <param name="norm1">Normalized direction</param>
        /// <param name="p1">corner of the quad</param>
        /// <param name="p2">corner of the quad</param>
        /// <param name="p3">corner of the quad</param>
        /// <param name="p4">corner of the quad</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public static float DistanceRayQuad(ref Vector3 orig1, ref Vector3 norm1, ref Vector3 p1, ref Vector3 p2, ref Vector3 p3, ref Vector3 p4, bool frontOnly = true)
        {
            //NOTE!!! optimize!
            float d1 = DistanceRayTriangle(ref orig1, ref norm1, ref p1, ref p2, ref p3, frontOnly);
            float d2 = DistanceRayTriangle(ref orig1, ref norm1, ref p3, ref p4, ref p1, frontOnly);

            if (float.IsNaN(d1))
                return d2;
            if (float.IsNaN(d2))
                return d1;

            return (d1 < d2) ? d1 : d2;
        }



        #endregion

        #region Distance lines

        /// <summary>
        /// Calculates distance between two lines
        /// </summary>
        /// <param name="orig1">Origin of the first line</param>
        /// <param name="dir1">Direction of the first line</param>
        /// <param name="orig2">Origin of the second line </param>
        /// <param name="dir2">Direction of the second line</param>
        /// <returns>Distance between lines</returns>
        public static float DistanceTwoLines(ref Vector3 orig1, ref Vector3 dir1, ref Vector3 orig2, ref Vector3 dir2)
        {
            float s, t;

            return DistanceTwoLines(ref orig1, ref dir1, ref orig2, ref dir2, out s, out t);
        }

        /// <summary>
        /// Calculates distance of two lines
        /// </summary>
        /// <param name="orig1">Origin of the first line</param>
        /// <param name="norm1">Normal of the first line</param>
        /// <param name="orig2">Origin of the second line</param>
        /// <param name="norm2">Normal of the second line</param>
        /// <param name="s">Will be filled with parameter for first line</param>
        /// <param name="t">Will be filled with parameter for second line</param>
        /// <returns>Distance of lines</returns>
        public static float DistanceTwoLines(ref Vector3 orig1, ref Vector3 norm1, ref Vector3 orig2, ref Vector3 norm2, out float s, out float t)
        {
            Vector3 r = orig1 - orig2;

            float a = norm1.Dot(ref norm1);
            float b = norm1.Dot(ref norm2);
            float c = norm1.Dot(ref r);
            float e = norm2.Dot(ref norm2);
            float f = norm2.Dot(ref r);

            float d = (a * e - b * b);

            if (IsAlmostZero(d))
            {
                //lines are paralel!
                s = 0.0f;
            }
            else
            {
                s = (b * f - c * e) / d;
            }

            t = (b * s + f) / e;

            Vector3 L1 = Vector3.MoveInDirection(ref orig1, ref norm1, s);
            Vector3 L2 = Vector3.MoveInDirection(ref orig2, ref norm2, t);

            return L1.Distance(ref L2);
        }

        /// <summary>
        /// Calculates distance of two line segments
        /// </summary>
        /// <param name="orig1">Origin of the first line</param>
        /// <param name="targ1">End point of the first line</param>
        /// <param name="orig2">Origin of the second line</param>
        /// <param name="targ2">End point  of the second line</param>
        /// <returns>Distance of lines</returns>
        public static float DistanceTwoLineSegments(ref Vector3 orig1, ref Vector3 targ1, ref Vector3 orig2, ref Vector3 targ2)
        {
            float s, t;
            return DistanceTwoLineSegments(ref orig1, ref targ1, ref orig2, ref targ2, out s, out t);
        }

        /// <summary>
        /// Calculates distance of two line segments
        /// </summary>
        /// <param name="orig1">Origin of the first line</param>
        /// <param name="targ1">End point of the first line</param>
        /// <param name="orig2">Origin of the second line</param>
        /// <param name="targ2">End point  of the second line</param>
        /// <returns>Distance of lines</returns>
        public static float DistanceTwoLineSegments(Vector3 orig1, Vector3 targ1, Vector3 orig2, Vector3 targ2)
        {
            float s, t;
            return DistanceTwoLineSegments(ref orig1, ref targ1, ref orig2, ref targ2, out s, out t);
        }


        /// <summary>
        /// Calculates distance of two line segments
        /// </summary>
        /// <param name="orig1">Origin of the first line</param>
        /// <param name="targ1">End point of the first line</param>
        /// <param name="orig2">Origin of the second line</param>
        /// <param name="targ2">End point  of the second line</param>
        /// <param name="s">Will be filled with parameter for first line</param>
        /// <param name="t">Will be filled with parameter for second line</param>
        /// <returns>Distance of lines</returns>
        public static float DistanceTwoLineSegments(ref Vector3 orig1, ref Vector3 targ1, ref Vector3 orig2, ref Vector3 targ2, out float s, out float t)
        {
            Vector3 r = orig1 - orig2;
            Vector3 norm1 = targ1 - orig1;
            Vector3 norm2 = targ2 - orig2;

            float a = norm1.Dot(ref norm1);
            float e = norm2.Dot(ref norm2);
            float f = norm2.Dot(ref r);

            if (IsAlmostZero(a) && IsAlmostZero(e))
            {
                //both line segments degenerate to points;
                s = t = 0;
                return orig1.Distance(ref orig2);
            }

            if (IsAlmostZero(a))
            {
                //first line is degenerated
                s = 0;
                t = f / e;
                ClampToUnit(ref t);
            }
            else
            {
                float c = norm1.Dot(ref r);
                if (IsAlmostZero(e))
                {
                    //second line is degenerated
                    t = 0;
                    s = -c / a;
                    ClampToUnit(ref s);
                }
                else
                {
                    float b = norm1.Dot(ref norm2);
                    float d = (a * e - b * b);

                    if (IsAlmostZero(d))
                    {
                        //lines are paralel!
                        s = 0.0f;
                    }
                    else
                    {
                        s = ClampToUnit((b * f - c * e) / d);
                    }

                    t = (b * s + f) / e;

                    if (t < 0.0f)
                    {
                        t = 0;
                        s = ClampToUnit(-c / a);
                    }
                    else if (t > 1.0f)
                    {
                        t = 1;
                        s = ClampToUnit((b - c) / a);
                    }
                }
            }

            Vector3 L1 = Vector3.MoveInDirection(ref orig1, ref norm1, s);
            Vector3 L2 = Vector3.MoveInDirection(ref orig2, ref norm2, t);

            return L1.Distance(ref L2);
        }


        /// <summary>
        /// Returns distance between Line segment and triangle
        /// </summary>
        /// <param name="l1">Start of the line</param>
        /// <param name="l2">End of the line</param>
        /// <param name="p1">First corner</param>
        /// <param name="p2">Second corner</param>
        /// <param name="p3">Third corner</param>
        /// <returns>Closest distance</returns>
        public static float DistanceLineSegmentTriangle(Vector3 l1, Vector3 l2, Vector3 p1, Vector3 p2, Vector3 p3)
        {
            return DistanceLineSegmentTriangle(ref l1, ref l2, ref p1, ref p2, ref p3);
        }

        /// <summary>
        /// Returns distance between Line segment and triangle
        /// </summary>
        /// <param name="l1">Start of the line</param>
        /// <param name="l2">End of the line</param>
        /// <param name="p1">First corner</param>
        /// <param name="p2">Second corner</param>
        /// <param name="p3">Third corner</param>
        /// <returns>Closest distance</returns>
        public static float DistanceLineSegmentTriangle(ref Vector3 l1, ref Vector3 l2, ref Vector3 p1, ref Vector3 p2, ref Vector3 p3)
        {
            //edge checking
            float dst = DistanceTwoLineSegments(ref l1, ref l2, ref p1, ref p2);
            float newdst = DistanceTwoLineSegments(ref l1, ref l2, ref p2, ref p3);
            if (newdst < dst)
                dst = newdst;
            newdst = DistanceTwoLineSegments(ref l1, ref l2, ref p3, ref p1);
            if (newdst < dst)
                dst = newdst;

            //projecting inside the triangle
            

            Vector3 dir = l2; dir.Substract(ref l1); dir.Normalize();
            Vector3 inters;
            float distance;
            if (IntersectionLineTriangle(ref l1, ref dir, ref p1, ref p2, ref p3, out distance, out inters))
            {
                Vector3 normal = CalcTriNormal(ref p1, ref p2, ref p3);
                if (distance < 0)
                {
                    newdst = DistanceVectorPlane(ref p1, ref normal, ref l1);
                }
                else
                {
                    newdst = DistanceVectorPlane(ref p1, ref normal, ref l2);
                }
                if (newdst < dst)
                    dst = newdst;
            }
            return dst;
        }

        /// <summary>
        /// Returns distance between Line segment and quad
        /// </summary>
        /// <param name="l1">Start of the line</param>
        /// <param name="l2">End of the line</param>
        /// <param name="p1">First corner</param>
        /// <param name="p2">Second corner</param>
        /// <param name="p3">Third corner</param>
        /// <param name="p4">Fourth corner</param>
        /// <returns>Closest distance</returns>
        public static float DistanceLineSegmentQuad(ref Vector3 l1, ref Vector3 l2, ref Vector3 p1, ref Vector3 p2, ref Vector3 p3, ref Vector3 p4)
        {
            //Optimize!!!

            float dst1 = DistanceLineSegmentTriangle(ref l1, ref l2, ref p1, ref p2, ref p3);
            float dst2 = DistanceLineSegmentTriangle(ref l1, ref l2, ref p3, ref p4, ref p1);
            return (dst1 < dst2) ? dst1 : dst2;
        }


        /// <summary>
        /// Returns distance between Line segment and quad
        /// </summary>
        /// <param name="l1">Start of the line</param>
        /// <param name="l2">End of the line</param>
        /// <param name="p1">First corner</param>
        /// <param name="p2">Second corner</param>
        /// <param name="p3">Third corner</param>
        /// <param name="p4">Fourth corner</param>
        /// <returns>Closest distance</returns>
        public static float DistanceLineSegmentQuad(Vector3 l1, Vector3 l2, Vector3 p1, Vector3 p2, Vector3 p3, Vector3 p4)
        {
            return DistanceLineSegmentQuad(ref l1, ref l2, ref p1, ref p2, ref p3, ref p4);
        }

        /// <summary>
        /// Returns distance between Line segment and triangle
        /// </summary>
        /// <param name="l1">Start of the line</param>
        /// <param name="l2">End of the line</param>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius of the sphere</param>
        /// <returns>Closest distance</returns>
        public static float DistanceLineSegmentSphere(Vector3 l1, Vector3 l2, Vector3 center, float radius)
        {
            return DistanceLineSegmentSphere(ref l1, ref l2, ref center, radius);
        }

        /// <summary>
        /// Returns distance between Line segment and triangle
        /// </summary>
        /// <param name="l1">Start of the line</param>
        /// <param name="l2">End of the line</param>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius of the sphere</param>
        /// <returns>Closest distance</returns>
        public static float DistanceLineSegmentSphere(ref Vector3 l1, ref Vector3 l2, ref Vector3 center, float radius)
        {
            return ClampMin(DistanceVectorLineSegment(ref l1, ref l2, ref center) - radius, 0);            
        }


        #endregion

        #region Distance Triangles and quads


        /// <summary>
        /// Returns closest distance between two triangles
        /// </summary>
        /// <param name="p1">Triangle 1 corner 1</param>
        /// <param name="p2">Triangle 1 corner 2</param>
        /// <param name="p3">Triangle 1 corner 3</param>
        /// <param name="q1">Triangle 2 corner 1</param>
        /// <param name="q2">Triangle 2 corner 2</param>
        /// <param name="q3">Triangle 2 corner 3</param>
        /// <returns>Closest distance</returns>
        public static float DistanceTwoTriangles(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 q1, Vector3 q2, Vector3 q3)
        {
            return DistanceTwoTriangles(ref p1, ref p2, ref p3, ref q1, ref q2, ref q3);
        }

        /// <summary>
        /// Returns closest distance between two triangles
        /// </summary>
        /// <param name="p1">Triangle 1 corner 1</param>
        /// <param name="p2">Triangle 1 corner 2</param>
        /// <param name="p3">Triangle 1 corner 3</param>
        /// <param name="q1">Triangle 2 corner 1</param>
        /// <param name="q2">Triangle 2 corner 2</param>
        /// <param name="q3">Triangle 2 corner 3</param>
        /// <returns>Closest distance</returns>
        public static float DistanceTwoTriangles(ref Vector3 p1, ref Vector3 p2, ref Vector3 p3, ref Vector3 q1, ref Vector3 q2, ref Vector3 q3)
        {
            //note - CHECK FOR INTERSECTING TRIANGLES!!!
            //should be optimalizable quite a lot (but much less readable)

            //edge detection

            float dist = DistanceTwoLineSegments(ref p1, ref p2, ref q1, ref q2);
            float newdist = DistanceTwoLineSegments(ref p1, ref p2, ref q2, ref q3);
            if (newdist < dist)
                dist = newdist;
            newdist = DistanceTwoLineSegments(ref p1, ref p2, ref q1, ref q3);
            if (newdist < dist)
                dist = newdist;
            newdist = DistanceTwoLineSegments(ref p2, ref p3, ref q1, ref q2);
            if (newdist < dist)
                dist = newdist;
            newdist = DistanceTwoLineSegments(ref p2, ref p3, ref q2, ref q3);
            if (newdist < dist)
                dist = newdist;
            newdist = DistanceTwoLineSegments(ref p2, ref p3, ref q1, ref q3);
            if (newdist < dist)
                dist = newdist;
            newdist = DistanceTwoLineSegments(ref p1, ref p3, ref q1, ref q2);
            if (newdist < dist)
                dist = newdist;
            newdist = DistanceTwoLineSegments(ref p1, ref p3, ref q2, ref q3);
            if (newdist < dist)
                dist = newdist;
            newdist = DistanceTwoLineSegments(ref p1, ref p3, ref q1, ref q3);
            if (newdist < dist)
                dist = newdist;

            //inside detection
            newdist = DistanceVectorTriangle(ref p1, ref p2, ref p3, ref q1); if (newdist < dist) dist = newdist;
            newdist = DistanceVectorTriangle(ref p1, ref p2, ref p3, ref q2); if (newdist < dist) dist = newdist;
            newdist = DistanceVectorTriangle(ref p1, ref p2, ref p3, ref q3); if (newdist < dist) dist = newdist;
            newdist = DistanceVectorTriangle(ref q1, ref q2, ref q3, ref p1); if (newdist < dist) dist = newdist;
            newdist = DistanceVectorTriangle(ref q1, ref q2, ref q3, ref p2); if (newdist < dist) dist = newdist;
            newdist = DistanceVectorTriangle(ref q1, ref q2, ref q3, ref p3); if (newdist < dist) dist = newdist;

            return newdist;
        }

        /// <summary>
        /// Returns closest distance between triangle and quad
        /// </summary>
        /// <param name="p1">Triangle 1 corner 1</param>
        /// <param name="p2">Triangle 1 corner 2</param>
        /// <param name="p3">Triangle 1 corner 3</param>
        /// <param name="q1">quad corner 1</param>
        /// <param name="q2">quad corner 2</param>
        /// <param name="q3">quad corner 3</param>
        /// <param name="q4">quad corner 4</param>
        /// <returns>Closest distance</returns>
        public static float DistanceTriangleQuad(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 q1, Vector3 q2, Vector3 q3, Vector3 q4)
        {
            return DistanceTriangleQuad(ref p1, ref p2, ref p3, ref q1, ref q2, ref q3, ref q4);
        }

        /// <summary>
        /// Returns closest distance between triangle and quad
        /// </summary>
        /// <param name="p1">Triangle 1 corner 1</param>
        /// <param name="p2">Triangle 1 corner 2</param>
        /// <param name="p3">Triangle 1 corner 3</param>
        /// <param name="q1">quad corner 1</param>
        /// <param name="q2">quad corner 2</param>
        /// <param name="q3">quad corner 3</param>
        /// <param name="q4">quad corner 4</param>
        /// <returns>Closest distance</returns>
        public static float DistanceTriangleQuad(ref Vector3 p1, ref Vector3 p2, ref Vector3 p3, ref Vector3 q1, ref Vector3 q2, ref Vector3 q3, ref Vector3 q4)
        {
            float dst1 = DistanceTwoTriangles(ref p1, ref p2, ref p3, ref q1, ref q2, ref q3);
            float dst2 = DistanceTwoTriangles(ref p1, ref p2, ref p3, ref q3, ref q4, ref q1);
            return (dst1 < dst2) ? dst1 : dst2;
        }

        #endregion


        #region Distance sphere

        /// <summary>
        /// Returns distance between sphere and triangle
        /// </summary>
        /// <param name="center">center of the sphere</param>
        /// <param name="radius">Radius</param>
        /// <param name="p1">Corner of the triangle</param>
        /// <param name="p2">Corner of the triangle</param>
        /// <param name="p3">Corner of the triangle</param>
        /// <returns>closest distance</returns>
        public static float DistanceSphereTriangle(ref Vector3 center, float radius, ref Vector3 p1, ref Vector3 p2, ref Vector3 p3)
        {
            return ClampMin(DistanceVectorTriangle(ref p1, ref p2, ref p3, ref center) - radius, 0);            
        }

        /// <summary>
        /// Returns distance between sphere and triangle
        /// </summary>
        /// <param name="center">center of the sphere</param>
        /// <param name="radius">Radius</param>
        /// <param name="p1">Corner of the triangle</param>
        /// <param name="p2">Corner of the triangle</param>
        /// <param name="p3">Corner of the triangle</param>
        /// <returns>closest distance</returns>
        public static float DistanceSphereTriangle(Vector3 center, float radius, Vector3 p1, Vector3 p2, Vector3 p3)
        {
            return DistanceSphereTriangle(ref center, radius, ref p1, ref p2, ref p3);
        }

        /// <summary>
        /// Returns distance between sphere and quad
        /// </summary>
        /// <param name="center">center of the sphere</param>
        /// <param name="radius">Radius</param>
        /// <param name="p1">Corner of the quad</param>
        /// <param name="p2">Corner of the quad</param>
        /// <param name="p3">Corner of the quad</param>
        /// <param name="p4">Corner of the quad</param>
        /// <returns>closest distance</returns>
        public static float DistanceSphereQuad(ref Vector3 center, float radius, ref Vector3 p1, ref Vector3 p2, ref Vector3 p3, ref Vector3 p4)
        {
            //optimize!!!
            float dst1 = DistanceSphereTriangle(ref center, radius, ref p1, ref p2, ref p3);
            float dst2 = DistanceSphereTriangle(ref center, radius, ref p3, ref p4, ref p1);

            return (dst1 < dst2) ? dst1 : dst2;
        }

        /// <summary>
        /// Returns distance between sphere and quad
        /// </summary>
        /// <param name="center">center of the sphere</param>
        /// <param name="radius">Radius</param>
        /// <param name="p1">Corner of the quad</param>
        /// <param name="p2">Corner of the quad</param>
        /// <param name="p3">Corner of the quad</param>
        /// <param name="p4">Corner of the quad</param>
        /// <returns>closest distance</returns>
        public static float DistanceSphereQuad(Vector3 center, float radius, Vector3 p1, Vector3 p2, Vector3 p3, Vector3 p4)
        {
            return DistanceSphereQuad(ref center, radius, ref p1, ref p2, ref p3, ref p4);
        }

        /// <summary>
        /// Returns distance between two spheres
        /// </summary>
        /// <param name="center1">Center of sphere 1</param>
        /// <param name="radius1">Radius of sphere 1</param>
        /// <param name="center2">Center of sphere 2</param>
        /// <param name="radius2">Radius of sphere 2</param>
        /// <returns>Distance between spheres</returns>
        public static float DistanceTwoSpheres(ref Vector3 center1, float radius1, ref Vector3 center2, float radius2)
        {
            return ClampMin(center1.Distance(ref center2) - radius1 - radius2, 0);
        }

        /// <summary>
        /// Returns distance between two spheres
        /// </summary>
        /// <param name="center1">Center of sphere 1</param>
        /// <param name="radius1">Radius of sphere 1</param>
        /// <param name="center2">Center of sphere 2</param>
        /// <param name="radius2">Radius of sphere 2</param>
        /// <returns>Distance between spheres</returns>
        public static float DistanceTwoSpheres(Vector3 center1, float radius1, Vector3 center2, float radius2)
        {
            return DistanceTwoSpheres(ref center1, radius1, ref center2, radius2);
        }


        #endregion


        #endregion

        #region Intersect quick check

        /// <summary>
        /// Quick check if sphere and AA box are in contact
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius of the sphere</param>
        /// <param name="min">Minimal corner</param>
        /// <param name="max">Maximal corner</param>
        /// <returns>True if intersection</returns>
        public static bool Intersects(ref Vector3 center, float radius, ref Vector3 min, ref Vector3 max)
        {
            //from graphic gems 1
            //note!!! adapt for distance too
            float dmin = 0;

            for (int i = 0; i < 3; i++)
            {
                if (center[i] > max[i])
                    dmin = dmin+(center[i]-max[i]);
                else if (center[i]<min[i])
                    dmin=dmin + center[i]-min[i];
            }
            if (dmin <= radius * radius)
                return true;
            return false;
        }

        /// <summary>
        /// Quick check if sphere and regular AA box are in contact
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius of the sphere</param>
        /// <param name="min">Minimal corner of the aabox</param>
        /// <param name="size">Size of the box</param>
        /// <returns>True if intersection</returns>
        public static bool Intersects(ref Vector3 center, float radius, ref Vector3 min, float size)
        {
            Vector3 max = min;
            max.Add(size, size, size);
            return Intersects(ref center, radius, ref min, ref max);
        }


        #endregion


        #region intersections


        /// <summary>
        /// Checks for intersection between line segment and cylinder
        /// </summary>
        /// <param name="line1">Line origin</param>
        /// <param name="line2">Line end</param>
        /// <param name="caps1">Cylinder origin</param>
        /// <param name="caps2">Cylinder end</param>
        /// <param name="radius">Radius</param>
        /// <param name="t">will be set to distance from origin</param>
        /// <returns>true if intersection exists</returns>
        public static bool IntersectionSegmentCylinder(ref Vector3 line1, ref Vector3 line2, ref Vector3 caps1, ref Vector3 caps2, float radius, out float t)
        {
            Vector3 d = caps2 - caps1;
            Vector3 m = line1 - caps1;
            Vector3 n = line2 - line1;

            float dd = d.Dot(ref d);
            float md = m.Dot(ref d);
            float nd = n.Dot(ref d);

            t = float.MaxValue;

            if ((md < 0) && (md + nd < 0))
                return false;   //segment outside caps1
            if ((md > dd) && (md + nd > dd))
                return false; //segment outside caps2

            float nn = n.Dot(ref n);
            float mn = m.Dot(ref n);

            float a = dd * nn - nd * nd;
            float k = m.Dot(ref m) - radius * radius;
            float c = dd * k - md * md;

            if (MathTools.IsAlmostZero(a))
            {
                //segment paralel to axis
                if (c > 0)
                    return false;
                //intersects:
                if (md < 0) t = -mn / nn;
                else if (md > dd) t = (nd - mn) / nn;
                else t = 0;
                return true;
            }

            float b = dd * mn - nd * md;
            float D = b * b - a * c;
            if (D < 0)
                return false; //imaginary number, no result

            t = (-b - MathTools.Sqrt(D)) / a;

            if ((t < 0) || (t > 1))
                return false;

            if (md + t * nd < 0)
            {
                //intersection outside caps1
                if (nd <= 0)
                    return false;
                t = -md / nd;
                if ((k + 2 * t * (mn + t * nn)) <= 0)
                    return true;
                return false;
            }

            if (md + t * nd > dd)
            {
                //intersection outside caps2
                if (nd >= 0)
                    return false;
                t = (dd - md) / nd;
                if ((k + dd - 2 * md * t * (2 * (mn - nd) + t * nn)) <= 0)
                    return true;
                return false;
            }
            
            // intersection between caps
            return true;
        }

        /// <summary>
        /// Checks for intersection between line segment and capsue
        /// </summary>
        /// <param name="line1">Line origin</param>
        /// <param name="line2">Line end</param>
        /// <param name="caps1">Capsule origin</param>
        /// <param name="caps2">Capsule end</param>
        /// <param name="radius">Radius</param>
        /// <param name="t">will be set to distance from origin</param>
        /// <returns>true if intersection exists</returns>
        public static bool IntersectionSegmentCapsule(ref Vector3 line1, ref Vector3 line2, ref Vector3 caps1, ref Vector3 caps2, float radius, out float t)
        {
            //OPTIMIZE!!!

            if (IntersectionSegmentCylinder(ref line1, ref line2, ref caps1, ref caps2, radius, out t))
                return true;

            if (IntersectionSegmentSphere(ref line1, ref line2, ref caps1, radius, out t))
                return true;

            if (IntersectionSegmentSphere(ref line1, ref line2, ref caps2, radius, out t))
                return true;
            return false;
        }


        /// <summary>
        /// Checks for intersection between triangle and capsue
        /// </summary>
        /// <param name="l1">Triangle 1</param>
        /// <param name="l2">Triangle 2</param>
        /// <param name="l3">Triangle 3</param>
        /// <param name="caps1">Capsule origin</param>
        /// <param name="caps2">Capsule end</param>
        /// <param name="radius">Radius</param>
        /// <param name="t">will be set to distance from origin</param>
        /// <returns>true if intersection exists</returns>
        public static bool IntersectionTriangleCapsule(ref Vector3 l1, ref Vector3 l2, ref Vector3 l3, ref Vector3 caps1, ref Vector3 caps2, float radius, out float t)
        {
            //OPTIMIZE!!!

            Vector3 normal = caps1 - caps2;
            normal.Normalize();

            //try to hit the triangle first

            Vector3 inters;

            if (IntersectionRayTriangle(ref caps1, ref normal, ref l1, ref l2, ref l3, out t, out inters, false))
            {
                t = ClampMin(t - radius, 0);
                return true;
            }

            //convert triangle edges to capsules and try to hit those

            if (IntersectionSegmentCapsule(ref caps1, ref caps2, ref l1, ref l2, radius, out t))
            {
                //recalculate t
                Vector3 norm = l2 - l1;
                float length = norm.Length;
                norm.Divide(length);
                Vector3 onedge = Vector3.MoveInDirection(ref l1, ref norm, t);

                if (DistanceVectorLineSegment(ref caps1, ref caps2, ref onedge, out t) < radius)
                {
                    t = t * length;
                    return true;
                }                
            }

            if (IntersectionSegmentCapsule(ref caps1, ref caps2, ref l2, ref l3, radius, out t))
            {
                //recalculate t
                Vector3 norm = l3 - l2;
                float length = norm.Length;
                norm.Divide(length);
                Vector3 onedge = Vector3.MoveInDirection(ref l2, ref norm, t);

                if (DistanceVectorLineSegment(ref caps1, ref caps2, ref onedge, out t) < radius)
                {
                    t = t * length;
                    return true;
                }
            }

            if (IntersectionSegmentCapsule(ref caps1, ref caps2, ref l3, ref l1, radius, out t))
            {
                //recalculate t
                Vector3 norm = l1 - l3;
                float length = norm.Length;
                norm.Divide(length);
                Vector3 onedge = Vector3.MoveInDirection(ref l3, ref norm, t);

                if (DistanceVectorLineSegment(ref caps1, ref caps2, ref onedge, out t) < radius)
                {
                    t = t * length;
                    return true;
                }
            }

            return false;
        }


        /// <summary>
        /// Checks for intersection between line segment and sphere
        /// </summary>
        /// <param name="line1">Line origin</param>
        /// <param name="line2">Line end</param>
        /// <param name="center">Sphere center</param>
        /// <param name="radius">Sphere radius</param>
        /// <param name="t">will be set to distance from origin</param>
        /// <returns>true if intersection exists</returns>
        public static bool IntersectionSegmentSphere(ref Vector3 line1, ref Vector3 line2, ref Vector3 center, float radius, out float t)
        {
            Vector3 dir = line2;
            dir.Substract(ref line1);
            Vector3 v2 = center;
            v2.Substract(ref line1);
            float lsq = dir.LengthSquared;
            t = dir.Dot(ref v2) / lsq;

            if ((t >= 0) && (t <= 1.0f))
            {
                if (t * Sqrt(lsq) < radius)
                    return true;
            }
            return false;
        }

        /// <summary>
        /// Intersection of ray and plane
        /// </summary>
        /// <param name="orig">Origin of ray</param>
        /// <param name="dir">Normalized direction of ray</param>
        /// <param name="point">Point on the plane</param>
        /// <param name="normal">Normal of the plane</param>
        /// <returns>Intersection point (invalid if no intersection)</returns>
        public static Vector3 IntersectionRayPlane(ref Vector3 orig, ref Vector3 dir, ref Vector3 point, ref Vector3 normal)
        {
            float t;
            Vector3 interspoint;

            if (IntersectionRayPlane(ref orig, ref dir, ref point, ref normal, out t, out interspoint))
            {
                return interspoint;
            }
            return Vector3.Invalid;
        }

        /// <summary>
        /// Intersection of ray and plane
        /// </summary>
        /// <param name="orig">Origin of ray</param>
        /// <param name="dir">Normalized direction of ray</param>
        /// <param name="point">Point on the plane</param>
        /// <param name="normal">Normal of the plane</param>
        /// <param name="interspoint">Will be filled with intersection point</param>
        /// <param name="t">Parameter of the ray (distance from origin)</param>
        /// <returns>True, if intersection exists</returns>
        public static bool IntersectionRayPlane(ref Vector3 orig, ref Vector3 dir, ref Vector3 point, ref Vector3 normal, out float t, out Vector3 interspoint)
        {
            interspoint = orig;

            float dst = normal.Dot(ref point); //distance from the origin

            t = (dst - normal.Dot(ref orig)) / normal.Dot(ref dir);

            if (t < 0)
                return false;

            interspoint.X += dir.X * t;
            interspoint.Y += dir.Y * t;
            interspoint.Z += dir.Z * t;
            return true;
        }

        /// <summary>
        /// Intersection of infinite line and plane
        /// </summary>
        /// <param name="orig">Origin of line</param>
        /// <param name="dir">Normalized direction of line</param>
        /// <param name="point">Point on the plane</param>
        /// <param name="normal">Normal of the plane</param>
        /// <param name="interspoint">Will be filled with intersection point</param>
        /// <param name="t">Parameter of the ray (distance from origin)</param>
        /// <returns>True, if intersection exists</returns>
        public static bool IntersectionLinePlane(ref Vector3 orig, ref Vector3 dir, ref Vector3 point, ref Vector3 normal, out float t, out Vector3 interspoint)
        {
            interspoint = orig;

            float dst = normal.Dot(ref point); //distance from the origin

            t = (dst - normal.Dot(ref orig)) / normal.Dot(ref dir);

            interspoint.X += dir.X * t;
            interspoint.Y += dir.Y * t;
            interspoint.Z += dir.Z * t;
            return true;
        }


        /// <summary>
        /// Returns intersection point of ray and triangle
        /// </summary>
        /// <param name="orig">Origin of ray</param>
        /// <param name="dir">Normalized direction of ray</param>
        /// <param name="first">First triangle vertice </param>
        /// <param name="second">Second triangle vertice </param>
        /// <param name="third">Third triangle vertice </param>
        /// <param name="frontFace">Return intersection only if triangle normal faces ray</param>
        /// <returns>Intersection point</returns>
        public static Vector3 IntersectionRayTriangle(ref Vector3 orig, ref Vector3 dir, ref Vector3 first, ref Vector3 second, ref Vector3 third, bool frontFace = false)
        {
            //Vector3 norm = CalcTriNormal(ref first, ref second, ref third, false);
            //Vector3 inters = IntersectionRayPlane(ref orig, ref dir, ref first, ref norm);

            float dist;
            Vector3 rval;

            if (IntersectionRayTriangle(ref orig, ref dir, ref first, ref second, ref third, out dist, out rval, frontFace))
            {
                return rval;
            }
            return Vector3.Invalid;
        }

        /// <summary>
        /// Returns intersection point of ray and triangle and distance
        /// </summary>
        /// <param name="orig">Origin of ray</param>
        /// <param name="dir">Normalized direction of ray</param>
        /// <param name="first">First triangle vertice </param>
        /// <param name="second">Second triangle vertice </param>
        /// <param name="third">Third triangle vertice </param>
        /// <param name="frontFace">Return intersection only if triangle normal faces ray</param>        
        /// <param name="distance">Will be set to distance</param>
        /// <param name="inters">Will be set to intersection point</param>
        /// <returns>True if intersection exists</returns>
        public static bool IntersectionRayTriangle( Vector3 orig,  Vector3 dir,  Vector3 first,  Vector3 second,  Vector3 third,
            out float distance, out Vector3 inters, bool frontFace = false)
        {
            return IntersectionRayTriangle(ref orig, ref dir, ref first, ref second, ref third, out distance, out inters, frontFace);
        }

        /// <summary>
        /// Returns intersection point of ray and triangle and distance
        /// </summary>
        /// <param name="orig">Origin of ray</param>
        /// <param name="dir">Normalized direction of ray</param>
        /// <param name="first">First triangle vertice </param>
        /// <param name="second">Second triangle vertice </param>
        /// <param name="third">Third triangle vertice </param>
        /// <param name="frontFace">Return intersection only if triangle normal faces ray</param>        
        /// <param name="distance">Will be set to distance</param>
        /// <param name="inters">Will be set to intersection point</param>
        /// <returns>True if intersection exists</returns>
        public static bool IntersectionRayTriangle(ref Vector3 orig, ref Vector3 dir, ref Vector3 first, ref Vector3 second, ref Vector3 third,
            out float distance, out Vector3 inters, bool frontFace = false)
        {
            Vector3 edge1 = Vector3.Substract(ref second, ref first);
            Vector3 edge2 = Vector3.Substract(ref third, ref first);
            Vector3 norm = edge1.Cross(ref edge2);
            distance = float.NaN;
            inters = orig;
            
            if (frontFace)
            {
                if (norm.Dot(ref dir) < 0)
                    return false;
                //triangle is not a front face
            }

            Vector3 vec = dir.Cross(ref edge2);

            float det = edge1.Dot(ref vec);
            if (IsAlmostZero(det))
            {
                return false;
            }

            float inverted = 1 / det;

            Vector3 vec2 = orig;
            vec2.Substract(ref first);

            float u = vec2.Dot(ref vec) * inverted;
            if ((u < 0) || (u > 1))
                return false;
            Vector3 vec3 = vec2.Cross(ref edge1);
            float v = dir.Dot(ref vec3) * inverted;
            if ((v < 0) || ((u + v) > 1))
                return false;
            
            distance = edge2.Dot(ref vec3) * inverted;

            inters.X += dir.X * distance;
            inters.Y += dir.Y * distance;
            inters.Z += dir.Z * distance;
            return true;
        }

        /// <summary>
        /// Returns intersection point of ray and triangle and distance
        /// </summary>
        /// <param name="orig">Origin of ray</param>
        /// <param name="dir">Normalized direction of line</param>
        /// <param name="first">First triangle vertice </param>
        /// <param name="second">Second triangle vertice </param>
        /// <param name="third">Third triangle vertice </param>
        /// <param name="distance">Will be set to distance</param>
        /// <param name="inters">Will be set to intersection point</param>
        /// <returns>True if intersection exists</returns>
        public static bool IntersectionLineTriangle(ref Vector3 orig, ref Vector3 dir, ref Vector3 first, ref Vector3 second, ref Vector3 third,
            out float distance, out Vector3 inters)
        {
            Vector3 normal = CalcTriNormal(ref first, ref second, ref third);
            
            if (!IntersectionLinePlane(ref orig, ref dir, ref first, ref normal, out distance, out inters))
                return false;

            if (PointInTriangle(ref first, ref second, ref third, ref inters))
                return true;
            return false;
        }






        /// <summary>
        /// Calculates intersection of ray and line segment
        /// </summary>
        /// <param name="orig">Origin of the ray</param>
        /// <param name="dir">Normalized direction</param>
        /// <param name="l1">First point of the line</param>
        /// <param name="l2">Second point of the line</param>
        /// <param name="distance">Sets to distance from the origin</param>
        /// <param name="point">Sets to intersection point</param>
        /// <returns>True if intersection exists</returns>
        public static bool IntersectionRayLineSegment(ref Vector3 orig, ref Vector3 dir, ref Vector3 l1, ref Vector3 l2, out float distance, out Vector3 point)
        {
            float s, t;

            Vector3 dir2 = l2; dir2.Substract(ref l1);
            Vector3 r = orig; r.Substract(ref l1);
            
            // ray = orig + dir*s (s >0)
            // ls = l1 + dir2*t (t 0..1)


            distance = float.NaN;
            point = new Vector3();

            float a = dir.Dot(ref dir);
            float e = dir2.Dot(ref dir2);
            float c = dir.Dot(ref r);
            float f = dir2.Dot(ref r);

            if (IsAlmostZero(e))
            {
                //second line is degenerated
                t = 0;
                s = -c / a;

                if (s < 0)
                {
                    distance = float.NaN;
                    return false;
                }
            }
            else
            {
                float b = dir.Dot(ref dir2);
                float d = (a * e - b * b);

                if (IsAlmostZero(d))
                {
                    //lines are paralel!
                    return false;
                }


                s = (b * f - c * e) / d;
                if (s < 0)
                    return false;

                t = (b * s + f) / e;

                if (!AlmostEqual(s, t, 0.0001f) || (t > 1.0f))
                    return false;
            }

            distance = s;

            point = Vector3.MoveInDirection(ref orig, ref dir, s);
            return true;
        }

        /// <summary>
        /// Calculates intersection of ray and line segment
        /// </summary>
        /// <param name="orig">Origin of the ray</param>
        /// <param name="dir">Normalized direction</param>
        /// <param name="l1">First point of the line</param>
        /// <param name="l2">Second point of the line</param>
        /// <param name="distance">Sets to distance from the origin</param>
        /// <param name="point">Sets to intersection point</param>
        /// <returns>True if intersection exists</returns>
        public static bool IntersectionRayLineSegment(Vector3 orig, Vector3 dir, Vector3 l1, Vector3 l2, out float distance, out Vector3 point)
        {
            return IntersectionRayLineSegment(ref orig, ref dir, ref l1, ref l2, out distance, out point);
        }


        /// <summary>
        /// Returns intersection of two rays if it exists
        /// </summary>
        /// <param name="orig1">Origin of the ray 1</param>
        /// <param name="dir1">Normalized direction of ray 1</param>
        /// <param name="orig2">Origin of the ray 2</param>
        /// <param name="dir2">Normalized direction of ray 2</param>
        /// <param name="distance">Sets to distance from the origin</param>
        /// <param name="point">Sets to intersection point</param>
        /// <returns>True if intersection exists</returns>
        public static bool IntersectionTwoRays(ref Vector3 orig1, ref Vector3 dir1, ref Vector3 orig2, ref Vector3 dir2, out float distance, out Vector3 point)
        {
            float s, t;
            distance = float.MaxValue;
            point = orig1;

            if (DistanceTwoLines(ref orig1, ref dir1, ref orig2, ref dir2, out s, out t)!= 0)
                return false;

            if (s < 0)
                return false;

            distance = s;
            point = Vector3.MoveInDirection(ref orig1, ref dir1, distance);
            return true;
        }


        /// <summary>
        /// Returns intersection of ray and Axis alligned box
        /// </summary>
        /// <param name="orig1">Origin of the ray 1</param>
        /// <param name="dir1">Normalized direction of ray 1</param>
        /// <param name="min">Minimal corner of AA</param>
        /// <param name="max">Maximal corner of AA</param>
        /// <param name="distance">Distance to intersection</param>
        /// <param name="point">Intersection point</param>
        /// <returns>True if intersection exists</returns>
        public static bool IntersectionRayAABox(ref Vector3 orig1, ref Vector3 dir1, ref Vector3 min, ref Vector3 max, out float distance, out Vector3 point)
        {
            float tmax = float.MaxValue;
            distance = 0;
            point = orig1;
            for (int i = 0; i < 3; i++)
            {
                if (IsAlmostZero(dir1[i]))
                {
                    // ray paralel to side of the box
                    if ((orig1[i] < min[i]) || (orig1[i] > max[i]))
                        return false;
                }
                else
                {
                    float ood = 1.0f / dir1[i];     //note, possible optimalization, store inverted direction in ray!!!
                    float t1 = (min[i] - orig1[i]) * ood;
                    float t2 = (max[i] - orig1[i]) * ood;
                    if (t1 > t2) 
                        Swap(ref t1, ref t2);
                    if (t1 > distance)
                        distance = t1;
                    if (t2 < tmax)
                        tmax = t2;
                    if (distance > tmax)
                        return false;                    
                }
            }
            point.Add(dir1.X * distance, dir1.Y * distance, dir1.Z * distance);
            return true;
        }

        /// <summary>
        /// Calculates intersection of the sphere and triangle - creates ColTriangle, it's faster to use it directly!
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius</param>
        /// <param name="p1">Triangle point 1</param>
        /// <param name="p2">Triangle point 2</param>
        /// <param name="p3">Triangle point 3</param>
        /// <param name="normal">Triangle normal</param>
        /// <returns>True if intersection</returns>
        public static bool IntersectionSphereTriangle(ref Vector3 center, float radius, 
            ref Vector3 p1, ref Vector3 p2, ref Vector3 p3)
        {
            Geometry.ColTriangle tri = new Geometry.ColTriangle();
            tri.Construct(ref p1, ref p2, ref p3);
            Vector3 point;
            float pen;
            return tri.IntersectionSphere(ref center, radius, out point, out pen);
        }


        #endregion


        #region Box Intersections and contains

        /// <summary>
        /// Returns true if bounding box 1 fully contains box 2
        /// </summary>
        /// <param name="min1">Minimum corner of bb1</param>
        /// <param name="max1">Maximum corner of bb1</param>
        /// <param name="min2">Minimum corner of bb2</param>
        /// <param name="max2">Maximum corner of bb2</param>
        /// <returns></returns>
        public static bool ContainsFully(ref Vector3 min1, ref Vector3 max1, ref Vector3 min2, ref Vector3 max2)
        {
            for (int i = 0; i < 3; i++)
            {
                if ((min1[i] > min2[i]) || (max1[i] < max2[i]))
                    return false;
            }
            return true;
        }

        /// <summary>
        /// Returns true if regular bounding box 1 fully contains box 2
        /// </summary>
        /// <param name="corner">Minimum corner of regular bb1</param>
        /// <param name="size">Size of bb1</param>
        /// <param name="min2">Minimum corner of bb2</param>
        /// <param name="max2">Maximum corner of bb2</param>
        /// <returns></returns>
        public static bool ContainsFully(ref Vector3 corner, float size, ref Vector3 min2, ref Vector3 max2)
        {
            for (int i = 0; i < 3; i++)
            {
                if ((corner[i] > min2[i]) || ((corner[i] + size) < max2[i]))
                    return false;
            }
            return true;
        }


        #endregion
    }
}
