VoidwalkerMath

using SharpDX;
using System;
using VoidwalkerEngine.Framework.Logic;

namespace VoidwalkerEngine.Framework.Maths
{
    public static class VoidwalkerMath
    {
        /// <summary>
        /// Represents the mathematical constant e.
        /// </summary>
        public const float E = 2.718282f;
        /// <summary>
        /// Represents the log base two of e.
        /// </summary>
        public const float Log2E = 1.442695f;
        /// <summary>
        /// First Person Rotational Bounds Limits
        /// </summary>
        public const float PiOver2RadiusClamp = 1.560796F;
        /// <summary>
        /// Represents the log base ten of e.
        /// </summary>
        public const float Log10E = 0.4342945f;
        /// <summary>
        /// Represents the value of pi.
        /// </summary>
        public const float Pi = 3.141593f;
        /// <summary>
        /// Represents the value of pi times two.
        /// </summary>
        public const float TwoPi = 6.28318530718f;
        /// <summary>
        /// Represents the value of pi times two.
        /// </summary>
        public const float FourPi = 12.5663706144f;
        /// <summary>
        /// Represents the value of pi divided by two.
        /// </summary>
        public const float PiOver2 = 1.570796f;
        /// <summary>
        /// Represents the value of pi divided by four.
        /// </summary>
        public const float PiOver4 = 0.7853982f;
        /// <summary>
        ///
        /// </summary>
        public const float PiDividedBy180 = 0.01745329F;
        /// <summary>
        /// My lucky number 31.
        /// </summary>
        public const int Lucky31 = 31;
        /// <summary>
        /// 0.5f
        /// </summary>
        public const float ModifierHalf = 0.5f;
        /// <summary>
        /// 1.0f
        /// </summary>
        public const float ModifierEqual = 1.0f;
        /// <summary>
        /// 1.5f
        /// </summary>
        public const float ModifierSesqui = 1.5f;
        /// <summary>
        /// 2.0f
        /// </summary>
        public const float ModifierDouble = 2.0f;

        /// <summary>
        ///
        /// </summary>
        /// <param name="fieldOfView"></param>
        /// <param name="farZ"></param>
        /// <param name="nearZ"></param>
        /// <param name="viewportWidth"></param>
        /// <param name="viewportHeight"></param>
        /// <returns></returns>
        public static Matrix CreateInfiniteProjectionMatrix(float fieldOfView, float nearZ, float farZ, float viewportWidth, float viewportHeight)
        {
            float e = 1 / (float)Math.Tan(fieldOfView / 2);
            float a = viewportHeight / viewportWidth;
            float alg1 = -((farZ + nearZ) / (farZ - nearZ));
            float alg2 = -((2 * farZ * nearZ) / (farZ - nearZ));
            Matrix result = new Matrix(
                e, 0.0f, 0.0f, 0.0f,
                0.0f, e / a, 0.0f, 0.0f,
                0.0f, 0.0f, alg1, alg2,
                0.0f, 0.0f, -1.0f, 0.0f);
            return result;
        }

        //Matrix InfiniteReverseZProjection(float fovY_radians, float aspectWbyH, float zNear)
        //{
        //    float f = 1.0f / (float)System.Math.Tan(fovY_radians / 2.0f);
        //    Matrix Result = new Matrix(
        //        f / aspectWbyH, 0.0f, 0.0f, 0.0f,
        //                  0.0f, f, 0.0f, 0.0f,
        //                  0.0f, 0.0f, 0.0f, -1.0f,
        //                  0.0f, 0.0f, zNear, 0.0f);

        //    return Result;
        //}

        public static int ToNearest(int value, int power, bool allowZero = true)
        {
            int result = power * (int)Math.Round(value / (double)power);
            if (!allowZero)
            {
                if (result <= 1)
                {
                    return power;
                }
            }
            return result;
        }

        /// <summary>
        /// Inverts a given Integer.
        /// </summary>
        /// <param name="value"></param>
        /// <returns></returns>
        public static int Invert(int value)
        {
            return -value;
        }

        /// <summary>
        /// Increases or decreases a given number by a multiplier. If the
        /// multiplier is greater than 0, the number will be increased. If
        /// the number is less than 0, the given number will be decreased
        /// by the multiplier.
        /// </summary>
        /// <param name="value"></param>
        /// <param name="multiplier"></param>
        /// <returns></returns>
        public static int ApplyMultiplier(int value, int multiplier)
        {
            return (int)(value * ToMultiplier(multiplier));
        }

        /// <summary>
        /// Converts an integer to a multiplier. If the value of 20
        /// is used, the output will be 1.20, if -40 is used, the output
        /// will be 0.60
        /// </summary>
        /// <param name="percent"></param>
        /// <returns></returns>
        public static double ToMultiplier(int percent)
        {
            return ((100 + percent) / 100D);
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="value"></param>
        /// <param name="percent"></param>
        /// <returns></returns>
        public static double PercentOf(int value, int percent)
        {
            return ((Clamp(percent, 0, 100) / 100d) * value);
        }

        /// <summary>
        /// Clamps the specified Number to between 0-360, and wraps
        /// back around if required.
        /// </summary>
        /// <param name="degrees"></param>
        /// <returns></returns>
        public static int ClampDegrees(int degrees)
        {
            return (degrees % 360) + (degrees < 0 ? 360 : 0);
        }

        public static double InversePow(double value, double power)
        {
            return Math.Pow(value, 1.0 / power);
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="f"></param>
        /// <returns></returns>
        public static float Sin(float f)
        {
            return (float)Math.Sin(f);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="f"></param>
        /// <returns></returns>
        public static float Cos(float f)
        {
            return (float)Math.Cos(f);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="f"></param>
        /// <returns></returns>
        public static float Tan(float f)
        {
            return (float)Math.Tan(f);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="f"></param>
        /// <returns></returns>
        public static float Asin(float f)
        {
            return (float)Math.Asin(f);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="f"></param>
        /// <returns></returns>
        public static float Acos(float f)
        {
            return (float)Math.Acos(f);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="f"></param>
        /// <returns></returns>
        public static float Atan(float f)
        {
            return (float)Math.Atan(f);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="y"></param>
        /// <param name="x"></param>
        /// <returns></returns>
        public static float Atan2(float y, float x)
        {
            return (float)Math.Atan2(y, x);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="f"></param>
        /// <returns></returns>
        public static float Sqrt(float f)
        {
            return (float)Math.Sqrt(f);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <returns></returns>
        public static float Pow(float x, float y)
        {
            return (float)Math.Pow(x, y);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <returns></returns>
        public static float Pow(double x, double y)
        {
            return (float)Math.Pow(x, y);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="f"></param>
        /// <returns></returns>
        public static float Abs(float f)
        {
            return Math.Abs(f);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="value"></param>
        /// <returns></returns>
        public static int Abs(int value)
        {
            return Math.Abs(value);
        }
        /// <summary>
        ///
        /// </summary>
        /// <param name="value"></param>
        /// <param name="percent"></param>
        /// <returns></returns>
        public static int MultiplyByPercent(int value, double percent)
        {
            return ((int)(value * percent));
        }

        public static string GetSign(int value)
        {
            if (value == 0)
            {
                return "";
            }
            if (value > 0)
            {
                return "+";
            }
            else
            {
                return "-";
            }
        }

        /// <summary>
        /// Clamps the specified value.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum.</param>
        /// <param name="max">The maximum.</param>
        /// <returns></returns>
        public static int Clamp(int value, int min, int max)
        {
            return (value < min) ? min : (value > max) ? max : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="value"></param>
        /// <param name="range"></param>
        /// <returns></returns>
        public static int Clamp(int value, Range range)
        {
            return Clamp(value, range.Minimum, range.Maximum);
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="min"></param>
        /// <param name="max"></param>
        public static void Clamp(int[] array, int min, int max)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Clamp(array[i], min, max);
            }
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="range"></param>
        public static void Clamp(int[] array, Range range)
        {
            Clamp(array, range.Minimum, range.Maximum);
        }

        /// <summary>
        /// Clamps the specified value.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum.</param>
        /// <param name="max">The maximum.</param>
        /// <returns></returns>
        public static float Clamp(float value, float min, float max)
        {
            return (value < min) ? min : (value > max) ? max : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="min"></param>
        /// <param name="max"></param>
        public static void Clamp(float[] array, float min, float max)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Clamp(array[i], min, max);
            }
        }

        /// <summary>
        /// Clamps the specified value.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum.</param>
        /// <param name="max">The maximum.</param>
        /// <returns></returns>
        public static double Clamp(double value, double min, double max)
        {
            return (value < min) ? min : (value > max) ? max : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="min"></param>
        /// <param name="max"></param>
        public static void Clamp(double[] array, double min, double max)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Clamp(array[i], min, max);
            }
        }

        /// <summary>
        /// Clamps the specified value.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum.</param>
        /// <param name="max">The maximum.</param>
        /// <returns></returns>
        public static long Clamp(long value, long min, long max)
        {
            return (value < min) ? min : (value > max) ? max : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="min"></param>
        /// <param name="max"></param>
        public static void Clamp(long[] array, long min, long max)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Clamp(array[i], min, max);
            }
        }

        /// <summary>
        /// Clamps the specified value.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum.</param>
        /// <param name="max">The maximum.</param>
        /// <returns></returns>
        public static byte Clamp(byte value, byte min, byte max)
        {
            return (value < min) ? min : (value > max) ? max : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="min"></param>
        /// <param name="max"></param>
        public static void Clamp(byte[] array, byte min, byte max)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Clamp(array[i], min, max);
            }
        }

        /// <summary>
        /// Clamps the specfied number to number desired maximum.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="max">The maximum.</param>
        /// <returns></returns>
        public static int ClampMaximum(int value, int max)
        {
            return value > max ? max : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="max"></param>
        public static void Max(int[] array, int max)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = ClampMaximum(array[i], max);
            }
        }

        /// <summary>
        /// Clamps the specfied number to number desired maximum.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="max">The maximum.</param>
        /// <returns></returns>
        public static double Max(double value, double max)
        {
            return value > max ? max : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="max"></param>
        public static void Max(double[] array, double max)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Max(array[i], max);
            }
        }

        /// <summary>
        /// Clamps the specfied number to number desired maximum.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="max">The maximum.</param>
        /// <returns></returns>
        public static byte Max(byte value, byte max)
        {
            return value > max ? max : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="max"></param>
        public static void Max(byte[] array, byte max)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Max(array[i], max);
            }
        }

        /// <summary>
        /// Clamps the specfied number to number desired maximum.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="max">The maximum.</param>
        /// <returns></returns>
        public static float Max(float value, float max)
        {
            return value > max ? max : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="max"></param>
        public static void Max(float[] array, float max)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Max(array[i], max);
            }
        }

        /// <summary>
        /// Clamps the specfied number to number desired minimum.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum.</param>
        /// <returns></returns>
        public static int ClampMinimum(int value, int min)
        {
            return value < min ? min : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="min"></param>
        public static void Min(int[] array, int min)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = ClampMinimum(array[i], min);
            }
        }

        /// <summary>
        /// Clamps the specfied number to number desired minimum.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum.</param>
        /// <returns></returns>
        public static double Min(double value, double min)
        {
            return value < min ? min : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="min"></param>
        public static void Min(double[] array, double min)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Min(array[i], min);
            }
        }

        /// <summary>
        /// Clamps the specified number to number desired minimum.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum.</param>
        /// <returns></returns>
        public static byte Min(byte value, byte min)
        {
            return value < min ? min : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="min"></param>
        public static void Min(byte[] array, byte min)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Min(array[i], min);
            }
        }

        /// <summary>
        /// Clamps the specified number to number desired minimum.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum.</param>
        /// <returns></returns>
        public static float Min(float value, float min)
        {
            return value < min ? min : value;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="array"></param>
        /// <param name="min"></param>
        public static void Min(float[] array, float min)
        {
            for (int i = 0; i < array.Length; i++)
            {
                array[i] = Min(array[i], min);
            }
        }


        /// <summary>
        /// Parses number positive or negative integer from number string using number highly optimized routine.
        /// </summary>
        /// <param name="data"></param>
        /// <returns></returns>
        public static int ParseInt32Fast(string data)
        {
            return data[0] == 45 ? ParseNegativeInt32Fast(data) : ParsePositiveInt32Fast(data);
        }

        /// <summary>
        /// Parses number positive integer from number string using number highly optimized routine.
        /// </summary>
        /// <param name="data"></param>
        /// <returns></returns>
        public static int ParsePositiveInt32Fast(string data)
        {
            int result = 0;
            for (int i = 0; i < data.Length; i++)
            {
                result = ((10 * result) + (data[i] - 48));
            }
            return result;
        }

        public static bool IsNumeric(string data, bool canBeNegative = true, bool canHaveDecimalPlace = false)
        {
            int negativeSignCount = 0;
            for (int i = 0; i < data.Length; i++)
            {
                if (data[i] == '-')
                {
                    if (!canBeNegative)
                    {
                        return false;
                    }
                    negativeSignCount++;
                    if (negativeSignCount > 1)
                    {
                        return false;
                    }
                    continue;
                }
                if (!char.IsDigit(data[i]))
                {
                    return false;
                }
            }
            return true;
        }

        /// <summary>
        /// Creates an Oriented Bounding Box
        /// </summary>
        /// <param name="min"></param>
        /// <param name="max"></param>
        /// <param name="transform"></param>
        /// <returns></returns>
        public static BoundingBox CreateOrientedBoundingBox(Vector3 min, Vector3 max, Matrix transform)
        {
            Vector3[] corners = new Vector3[]
            {
                Vector3.TransformCoordinate(new Vector3(min.X, max.Y, max.Z), transform),
                Vector3.TransformCoordinate(new Vector3(max.X, max.Y, max.Z), transform),
                Vector3.TransformCoordinate(new Vector3(max.X, min.Y, max.Z), transform),
                Vector3.TransformCoordinate(new Vector3(min.X, min.Y, max.Z), transform),
                Vector3.TransformCoordinate(new Vector3(min.X, max.Y, min.Z), transform),
                Vector3.TransformCoordinate(new Vector3(max.X, max.Y, min.Z), transform),
                Vector3.TransformCoordinate(new Vector3(max.X, min.Y, min.Z), transform),
                Vector3.TransformCoordinate(new Vector3(min.X, min.Y, min.Z), transform)
            };
            return BoundingBox.FromPoints(corners);
        }

        /// <summary>
        /// Creates a Billboard Matrix that is aligned on all axes.
        /// </summary>
        /// <param name="geometryLocation"></param>
        /// <param name="cameraLocation"></param>
        /// <param name="up"></param>
        /// <param name="forward"></param>
        /// <param name="applyTranslation"></param>
        /// <returns></returns>
        public static Matrix CreateBillboard(Vector3 geometryLocation, Vector3 cameraLocation, Vector3 up, Vector3 forward)
        {
            Vector3 difference = geometryLocation - cameraLocation;
            float lengthSquared = difference.LengthSquared();
            difference = MathUtil.IsZero(lengthSquared) ? -forward : difference * (float)(1.0 / Math.Sqrt(lengthSquared));
            Vector3.Cross(ref up, ref difference, out Vector3 crossProduct);
            crossProduct.Normalize();
            Vector3.Cross(ref difference, ref crossProduct, out Vector3 final);
            Matrix result = new Matrix
            {
                M11 = crossProduct.X,
                M12 = crossProduct.Y,
                M13 = crossProduct.Z,
                M21 = final.X,
                M22 = final.Y,
                M23 = final.Z,
                M31 = difference.X,
                M32 = difference.Y,
                M33 = difference.Z,
                M41 = geometryLocation.X,
                M42 = geometryLocation.Y,
                M43 = geometryLocation.Z,
                M44 = 1.0f
            };
            return result;
        }

        /// <summary>
        /// Creates a Billboard matrix that is aligned on the X and Z axis.
        /// </summary>
        /// <param name="geometryLocation"></param>
        /// <param name="cameraLocation"></param>
        /// <param name="up"></param>
        /// <param name="forward"></param>
        /// <param name="applyTranslation"></param>
        /// <returns></returns>
        public static Matrix CreateBillboardXZAxis(Vector3 geometryLocation, Vector3 cameraLocation, Vector3 up, Vector3 forward)
        {
            Vector3 difference = new Vector3(geometryLocation.X, 0, geometryLocation.Z) - new Vector3(cameraLocation.X, 0, cameraLocation.Z);
            float lengthSquared = difference.LengthSquared();
            difference = MathUtil.IsZero(lengthSquared) ? -forward : difference * (float)(1.0 / Math.Sqrt(lengthSquared));
            Vector3.Cross(ref up, ref difference, out Vector3 crossProduct);
            crossProduct.Normalize();
            Vector3.Cross(ref difference, ref crossProduct, out Vector3 final);
            Matrix result = new Matrix
            {
                M11 = crossProduct.X,
                M12 = crossProduct.Y,
                M13 = crossProduct.Z,
                M21 = final.X,
                M22 = final.Y,
                M23 = final.Z,
                M31 = difference.X,
                M32 = difference.Y,
                M33 = difference.Z,
                M41 = geometryLocation.X,
                M42 = geometryLocation.Y,
                M43 = geometryLocation.Z,
                M44 = 1.0f
            };
            return result;
        }

        /// <summary>
        /// Parses number negative integer from number string using number highly optimized routine.
        /// </summary>
        /// <param name="data"></param>
        /// <returns></returns>
        public static int ParseNegativeInt32Fast(string data)
        {
            int result = 0;
            for (int i = 1; i < data.Length; i++)
            {
                result = ((10 * result) + (data[i] - 48));
            }
            return -result;
        }

        /// <summary>
        /// Parses number floating point number from number string. This
        /// function uses an invariant culture, meaning the float
        /// can be either negative or positive.
        /// </summary>
        /// <param name="input">The input.</param>
        /// <returns></returns>
        public static float ParseFloatFast(string input)
        {
            float result = 0;
            int index = 0;
            float exp = 0.1f;
            int length = input.Length;
            char character = input[0];
            float sign = 1;
            if (character == 45)
            {
                sign = -1;
                ++index;
            }
            while (true)
            {
                if (index >= length)
                {
                    return sign * result;
                }
                character = input[index++];
                if (character < 48 || character > 57)
                {
                    break;
                }
                result = ((result * 10) + (character - 48));
            }
            while (index < length)
            {
                character = input[index++];
                result += (character - 48) * exp;
                exp *= 0.1f;
            }
            return sign * result;
        }

        public static double ParseDoubleFast(string input)
        {
            double result = 0;
            int index = 0;
            double exp = 0.1f;
            int length = input.Length;
            char character = input[0];
            double sign = 1;
            if (character == 45)
            {
                sign = -1;
                ++index;
            }
            while (true)
            {
                if (index >= length)
                {
                    return sign * result;
                }
                character = input[index++];
                if (character < 48 || character > 57)
                {
                    break;
                }
                result = ((result * 10) + (character - 48));
            }
            while (index < length)
            {
                character = input[index++];
                result += (character - 48) * exp;
                exp *= 0.1f;
            }
            return sign * result;
        }
        /// <summary>
        /// CubeRoot(number,power)
        /// </summary>
        /// <param name="number"></param>
        /// <param name="power"></param>
        /// <returns></returns>
        public static double CubeRoot(double number, int power)
        {
            double[] x = new double[2];
            x[0] = number;
            x[1] = number / power;
            while (Math.Abs(x[0] - x[1]) > 0.001D)
            {
                x[1] = x[0];
                x[0] = (1 / (double)power) * ((((double)power - 1) * x[1]) + (number / Math.Pow(x[1], (double)power - 1)));
            }
            return x[0];
        }

        /// <summary>
        /// Converts degrees to radians.
        /// </summary>
        /// <param name="degrees">The angle in degrees.</param>
        public static float ToRadians(float degrees)
        {
            return degrees / 360.0f * TwoPi;
        }

        /// <summary>
        /// Creates a rotation matrix using degrees.
        /// </summary>
        /// <param name="xDegrees"></param>
        /// <param name="yDegrees"></param>
        /// <param name="zDegrees"></param>
        /// <returns></returns>
        public static Matrix CreateRotationMatrix(float xDegrees, float yDegrees, float zDegrees)
        {
            return
                Matrix.RotationX(ToRadians(xDegrees)) *
                Matrix.RotationY(ToRadians(yDegrees)) *
                Matrix.RotationZ(ToRadians(zDegrees));
        }

        /// <summary>
        /// Converts a Vector3 of Degrees to a Vector3 of Radians
        /// </summary>
        /// <param name="degrees"></param>
        /// <returns></returns>
        public static Vector3 ToRadians(Vector3 degrees)
        {
            return ToRadians(degrees.X,degrees.Y,degrees.Z);
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="degreesX"></param>
        /// <param name="degreesY"></param>
        /// <param name="degreesZ"></param>
        /// <returns></returns>
        public static Vector3 ToRadians(float degreesX, float degreesY, float degreesZ)
        {
            return
                new Vector3(
                    ToRadians(degreesX),
                    ToRadians(degreesY),
                    ToRadians(degreesZ));
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="degrees"></param>
        public static void ToRadians(float[] degrees)
        {
            for(int i = 0; i < degrees.Length; i++)
            {
                degrees[i] = ToRadians(degrees[i]);
            }
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="degrees"></param>
        /// <returns></returns>
        public static Matrix CreateRotationMatrix(Vector3 degrees)
        {
            return CreateRotationMatrix(degrees.X, degrees.Y, degrees.Z);
        }

        /// <summary>
        /// Converts radians to degrees.
        /// </summary>
        /// <param name="radians">The angle in radians.</param>
        public static float ToDegrees(float radians)
        {
            return radians * 57.29578f;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="radians"></param>
        /// <returns></returns>
        public static double ToDegrees(double radians)
        {
            return radians * 57.29578;
        }

        /// <summary>
        /// Gets the nearest bigger power of two.
        /// </summary>
        /// <param name="v">The v.</param>
        /// <returns></returns>
        public static int GetNearestBiggerPowerOfTwo(int v)
        {
            --v;
            v |= v >> 1;
            v |= v >> 2;
            v |= v >> 4;
            v |= v >> 8;
            v |= v >> 16;
            ++v;
            return v;
        }

        /// <summary>
        /// Gets the nearest bigger power of two.
        /// </summary>
        /// <param name="v">The v.</param>
        /// <returns></returns>
        public static uint GetNearestBiggerPowerOfTwo(uint v)
        {
            --v;
            v |= v >> 1;
            v |= v >> 2;
            v |= v >> 4;
            v |= v >> 8;
            v |= v >> 16;
            ++v;
            return v;
        }

        /// <summary>
        /// Returns nearest bigger power of two
        /// </summary>
        /// <param name="f"></param>
        /// <returns></returns>
        public static int GetNearestBiggerPowerOfTwo(float f)
        {
            int x = 1;
            while (x < f)
            {
                x <<= 1;
            }
            return x;
        }

        /// <summary>
        /// Gets the nearest bigger power of two.
        /// </summary>
        /// <param name="f">The f.</param>
        /// <returns></returns>
        public static int GetNearestBiggerPowerOfTwo(double f)
        {
            int x = 1;
            while (x < f)
            {
                x <<= 1;
            }
            return x;
        }

        /// <summary>
        /// Returns true if value is power of two
        /// </summary>
        /// <param name="x"></param>
        /// <returns></returns>
        public static bool IsPowerOfTwo(int x)
        {
            return ((x > 0) && ((x & (x - 1)) == 0));
        }

        /// <summary>
        /// Determines whether the specified value is odd.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <returns></returns>
        public static bool IsOdd(int value)
        {
            return value % 2 != 0;
        }

        /// <summary>
        /// Determines whether the specified value is even.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <returns></returns>
        public static bool IsEven(int value)
        {
            return value % 2 == 0;
        }

        /// <summary>
        /// Barycentrics the specified value1.
        /// </summary>
        /// <param name="value1">The value1.</param>
        /// <param name="value2">The value2.</param>
        /// <param name="value3">The value3.</param>
        /// <param name="amount1">The amount1.</param>
        /// <param name="amount2">The amount2.</param>
        /// <returns></returns>
        public static float Barycentric(float value1, float value2, float value3, float amount1, float amount2)
        {
            return value1 + (value2 - value1) * amount1 + (value3 - value1) * amount2;
        }

        /// <summary>
        /// Catmulls the rom.
        /// </summary>
        /// <param name="value1">The value1.</param>
        /// <param name="value2">The value2.</param>
        /// <param name="value3">The value3.</param>
        /// <param name="value4">The value4.</param>
        /// <param name="amount">The amount.</param>
        /// <returns></returns>
        public static float CatmullRom(float value1, float value2, float value3, float value4, float amount)
        {
            double amountSquared = amount * amount;
            double amountCubed = amountSquared * amount;
            return (float)(0.5 * (2.0 * value2 +
                (value3 - value1) * amount +
                (2.0 * value1 - 5.0 * value2 + 4.0 * value3 - value4) * amountSquared +
                (3.0 * value2 - value1 - 3.0 * value3 + value4) * amountCubed));
        }

        /// <summary>
        /// Distances the specified left.
        /// </summary>
        /// <param name="left">The left.</param>
        /// <param name="right">The right.</param>
        /// <returns></returns>
        public static float Distance(float left, float right)
        {
            return Math.Abs(left - right);
        }

        /// <summary>
        /// Distances the specified left.
        /// </summary>
        /// <param name="left">The left.</param>
        /// <param name="right">The right.</param>
        /// <returns></returns>
        public static int Distance(int left, int right)
        {
            return Math.Abs(left - right);
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        /// <returns></returns>
        public static float Distance(Vector3 left, Vector3 right)
        {
            return Math.Abs((left - right).Length());
        }

        /// <summary>
        /// Hermites the specified value1.
        /// </summary>
        /// <param name="value1">The value1.</param>
        /// <param name="tangent1">The tangent1.</param>
        /// <param name="value2">The value2.</param>
        /// <param name="tangent2">The tangent2.</param>
        /// <param name="amount">The amount.</param>
        /// <returns></returns>
        public static float Hermite(float value1, float tangent1, float value2, float tangent2, float amount)
        {
            float v1 = value1, v2 = value2, t1 = tangent1, t2 = tangent2, s = amount, result;
            float sCubed = s * s * s;
            float sSquared = s * s;
            switch (amount)
            {
                case 0f:
                    result = value1;
                    break;
                case 1f:
                    result = value2;
                    break;
                default:
                    result = (2 * v1 - 2 * v2 + t2 + t1) * sCubed +
                             (3 * v2 - 3 * v1 - 2 * t1 - t2) * sSquared +
                             t1 * s +
                             v1;
                    break;
            }
            return result;
        }

        /// <summary>
        /// Linearly interpolates between two values.
        /// </summary>
        /// <param name="value1">Source value.</param>
        /// <param name="value2">Destination value.</param>
        /// <param name="amount">Value between 0 and 1 indicating the weight of value2.</param>
        /// <returns>Interpolated value.</returns>
        public static float Lerp(float value1, float value2, float amount)
        {
            return value1 + (value2 - value1) * amount;
        }

        /// <summary>
        /// Smoothes the step.
        /// </summary>
        /// <param name="value1">The value1.</param>
        /// <param name="value2">The value2.</param>
        /// <param name="amount">The amount.</param>
        /// <returns></returns>
        public static float SmoothStep(float value1, float value2, float amount)
        {
            return Hermite(value1, 0f, value2, 0f, Clamp(amount, 0f, 1f));
        }

        /// <summary>
        /// https://en.wikipedia.org/wiki/Pythagorean_theorem
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static float Pythagorean(float a, float b)
        {
            return (float)Math.Sqrt((a * a) + (b * b));
        }

        /// <summary>
        ///
        /// (Hypotenuse² = (Perpendicular² + Base²))
        ///
        /// https://en.wikipedia.org/wiki/Pythagorean_theorem
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static int Pythagorean(int a, int b)
        {
            return (int)Math.Sqrt((a * a) + (b * b));
        }

        /// <summary>
        ///
        /// (Hypotenuse² = (Perpendicular² + Base²))
        ///
        /// https://en.wikipedia.org/wiki/Pythagorean_theorem
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static double Pythagorean(double a, double b)
        {
            return Math.Sqrt((a * a) + (b * b));
        }

        /// <summary>
        /// Finds the center of a specified Rectangle.
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="width"></param>
        /// <param name="height"></param>
        /// <returns></returns>
        public static Point FindCenter(int x, int y, int width, int height)
        {
            return new Point(x + (width / 2), y + (height / 2));
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="width"></param>
        /// <param name="height"></param>
        /// <param name="resultX"></param>
        /// <param name="resultY"></param>
        public static void FindCenter(int x, int y, int width, int height, out int resultX, out int resultY)
        {
            Point p = FindCenter(x, y, width, height);
            resultX = p.X;
            resultY = p.Y;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="location"></param>
        /// <param name="quaternion"></param>
        /// <param name="scale"></param>
        /// <returns></returns>
        public static Matrix CreateEntityMatrix(Vector3 location, Quaternion quaternion, Vector3 scale)
        {
            return
                Matrix.Scaling(scale) *
                Matrix.RotationQuaternion(quaternion) *
                Matrix.Translation(location);
        }

        /// <summary>
        /// Converts a given bool to either 1 or 0.
        /// </summary>
        /// <param name="value"></param>
        /// <returns></returns>
        public static int BoolToInt(bool value)
        {
            return value ? 1 : 0;
        }

        /// <summary>
        /// Converts a given integer to a boolean value.
        /// if the value is zero or negative, this returns false.
        /// If the value is 1 or more, this returns true;
        /// </summary>
        /// <param name="integer"></param>
        /// <returns></returns>
        public static bool IntToBool(int integer)
        {
            return integer > 0;
        }



        /// <summary>
        /// Calculates a Cardinal Direction from a given degree of
        /// rotation. If no direction can be calculated, this function
        /// returns CardinalDirectionType.None.
        /// </summary>
        /// <param name="degrees"></param>
        /// <returns></returns>
        public static CardinalDirection GetCardinalDirection(float degrees)
        {
            float rot = degrees % 360;
            if (rot < 0f)
            {
                rot += 360.0f;
            }
            if (0f <= rot && rot < 22.5)
            {
                return CardinalDirection.North;
            }
            if (22.5f <= rot && rot < 67.5f)
            {
                return CardinalDirection.NorthEast;
            }
            if (67.5f <= rot && rot < 112.5f)
            {
                return CardinalDirection.East;
            }
            if (112.5f <= rot && rot < 157.5f)
            {
                return CardinalDirection.SouthEast;
            }
            if (157.5f <= rot && rot < 202.5f)
            {
                return CardinalDirection.South;
            }
            if (202.5f <= rot && rot < 247.5f)
            {
                return CardinalDirection.SouthWest;
            }
            if (247.5f <= rot && rot < 292.5f)
            {
                return CardinalDirection.West;
            }
            if (292.5f <= rot && rot < 337.5f)
            {
                return CardinalDirection.NorthWest;
            }
            if (337.5f <= rot && rot < 360.0f)
            {
                return CardinalDirection.North;
            }
            return CardinalDirection.None;
        }
    }
}