PlayerMovment/Assets/Scripts/Util/MathExtras.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class MathExtras : MonoBehaviour
{
    //          TYPES
    /// <summary>
    /// Serializable version of Keyframe struct.
    /// </summary>
    [System.Serializable]
    public struct Keyframe
    {
        [Tooltip("Time and Value of keyframe.")]
        public Vector2 timeValue;

        [Tooltip("Tangents of keyframe.")]
        public Vector2 tangents;
    }


    /// <summary>
    /// Representation of an AABB volume.
    /// </summary>
    [System.Serializable]
    public struct AabbVolume
    {
        [Tooltip("Minimum corner of AABB volume.")]
        public Vector3 minCorner;

        [Tooltip("Maximum corner of AABB volume.")]
        public Vector3 maxCorner;

        public AabbVolume(Vector3 min, Vector3 max)
        {
            this.minCorner = min;
            this.maxCorner = max;

            this.Validate();
        }

        public void Validate()
        {
            Vector3 mn, mx;

            mn.x = Mathf.Min(this.minCorner.x, this.maxCorner.x);
            mn.y = Mathf.Min(this.minCorner.y, this.maxCorner.y);
            mn.z = Mathf.Min(this.minCorner.z, this.maxCorner.z);

            mx.x = Mathf.Max(this.minCorner.x, this.maxCorner.x);
            mx.y = Mathf.Max(this.minCorner.y, this.maxCorner.y);
            mx.z = Mathf.Max(this.minCorner.z, this.maxCorner.z);

            if (mn == mx)
                Debug.LogError($"Bad AABB Volume: {this.minCorner} -> {this.maxCorner}");

            this.minCorner = mn;
            this.maxCorner = mx;
        }

        public bool ContainsPoint(Vector3 pt)
        {
            return
                (
                    this.minCorner.x <= pt.x && this.maxCorner.x >= pt.x &&
                    this.minCorner.y <= pt.y && this.maxCorner.y >= pt.y &&
                    this.minCorner.z <= pt.z && this.maxCorner.z >= pt.z
                );
        }

        public Vector3 GetNormalizedPoint(Vector3 pt)
        {
            return new Vector3
                (
                    Mathf.InverseLerp(this.minCorner.x, this.maxCorner.x, pt.x),
                    Mathf.InverseLerp(this.minCorner.y, this.maxCorner.y, pt.y),
                    Mathf.InverseLerp(this.minCorner.z, this.maxCorner.z, pt.z)
                );
        }

        public Vector3 GetPoint(Vector3 normalized)
        {
            return new Vector3
                (
                    Mathf.Lerp(this.minCorner.x, this.maxCorner.x, normalized.x),
                    Mathf.Lerp(this.minCorner.y, this.maxCorner.y, normalized.y),
                    Mathf.Lerp(this.minCorner.z, this.maxCorner.z, normalized.z)
                );
        }

        public Vector3 Extents { get { return this.maxCorner - this.minCorner; } }
    }

    
    /// <summary>
    /// An "octohedral neighborhood" for bucketing data points to the 6 cardinal directions.
    /// </summary>
    public struct Neighborhood
    {
        //          STATIC
        private static Vector3[] _directions = new Vector3[]
        {
            new Vector3( 0f,  0f,  0f),

            new Vector3( 1f,  0f,  0f),
            new Vector3(-1f,  0f,  0f),
            new Vector3( 0f,  1f,  0f),
            new Vector3( 0f, -1f,  0f),
            new Vector3( 0f,  0f,  1f),
            new Vector3( 0f,  0f, -1f),
        };

        //          TYPES
        public enum Direction
        {
            INVALID     = 0,

            X_POS       = 1,
            X_NEG       = 2,
            Y_POS       = 3,
            Y_NEG       = 4,
            Z_POS       = 5,
            Z_NEG       = 6
        }


        //          INTERNALS
        private bool _init;
        private int[] _buckets;


        //          BEHAVIORS
        private void Init()
        {
            this._init = true;
            this._buckets = new int[_directions.Length];
        }

        public void Clear()
        {
            if (!this._init)
                this.Init();
            else
            {
                this._buckets[0] = 0;
                this._buckets[1] = 0;
                this._buckets[2] = 0;
                this._buckets[3] = 0;
                this._buckets[4] = 0;
                this._buckets[5] = 0;
                this._buckets[6] = 0;
            }
        }

        public Direction GetDir(Transform relative, Vector3 point)
        {
            return this.GetDir(relative.InverseTransformDirection(point));
        }

        public Direction GetDir(Vector3 point)
        {
            // Classify into direction
            float bestDot = float.MinValue;
            int bestBucket = 0;

            for (int i = 0; i < _directions.Length; i++)
            {
                float dot = Vector3.Dot(point, _directions[i]);

                if (dot > bestDot)
                {
                    bestDot = dot;
                    bestBucket = i;
                }
            }

            // Return
            return (Direction)bestBucket;
        }

        public void Add(Transform relative, Vector3 point)
        {
            this.Add(relative.InverseTransformDirection(point));
        }

        public void Add(Vector3 point)
        {
            // Guard; not init
            if (!this._init)
                this.Init();

            // Classify and increment
            this._buckets[(int)this.GetDir(point)]++;
        }

        public int GetCount(Direction dir)
        {
            if (!this._init)
                this.Init();

            return this._buckets[(int)dir];
        }
    }


    /// <summary>
    /// Implementation of simple Moving Average (https://en.wikipedia.org/wiki/Moving_average#Simple_moving_average)
    /// </summary>
    [System.Serializable]
    public struct SMA
    {
        //          PROPERTIES
        public float CurAvg { get { return this._curAvg; } }


        //          INTERNALS
        private bool _init;
        
        private float[] _buffer;
        private float _curAvg;
        private int _index;


        public SMA(int len, float fill)
        {
            this._init = true;
            
            this._curAvg = fill;
            this._buffer = new float[len];
            this._index = 0;
        }

        public void AddSample(float sample)
        {
            // Guard
            if (!this._init)
            {
                this._init = true;

                this._curAvg = 0f;
                this._buffer = new float[5];
                this._index = 0;
            }

            this._curAvg += (sample - this._buffer[this._index]) / this._buffer.Length;
            this._buffer[this._index] = sample;
            this._index = (this._index + 1) % this._buffer.Length;
        }
    }


    /// <summary>
    /// Implementation of simple Moving Average (https://en.wikipedia.org/wiki/Moving_average#Simple_moving_average)
    /// </summary>
    [System.Serializable]
    public struct SMA2
    {
        //          PROPERTIES
        public Vector2 CurAvg { get { return this._curAvg; } }


        //          INTERNALS
        private bool _init;

        private Vector2[] _buffer;
        private Vector2 _curAvg;
        private int _index;


        public SMA2(int len, Vector2 fill)
        {
            this._init = true;

            this._curAvg = fill;
            this._buffer = new Vector2[len];
            this._index = 0;
        }

        public void AddSample(Vector2 sample)
        {
            // Guard
            if (!this._init)
            {
                this._init = true;

                this._curAvg = Vector2.zero;
                this._buffer = new Vector2[5];
                this._index = 0;
            }

            this._curAvg += (sample - this._buffer[this._index]) / this._buffer.Length;
            this._buffer[this._index] = sample;
            this._index = (this._index + 1) % this._buffer.Length;
        }
    }


    /// <summary>
    /// Implementation of simple Moving Average (https://en.wikipedia.org/wiki/Moving_average#Simple_moving_average)
    /// </summary>
    [System.Serializable]
    public struct SMA3
    {
        //          PROPERTIES
        public Vector3 CurAvg { get { return this._curAvg; } }


        //          INTERNALS
        private bool _init;

        private Vector3[] _buffer;
        private Vector3 _curAvg;
        private int _index;


        public SMA3(int len, Vector3 fill)
        {
            this._init = true;

            this._curAvg = fill;
            this._buffer = new Vector3[len];
            this._index = 0;
        }

        public void AddSample(Vector3 sample)
        {
            // Guard
            if (!this._init)
            {
                this._init = true;

                this._curAvg = Vector3.zero;
                this._buffer = new Vector3[5];
                this._index = 0;
            }

            this._curAvg += (sample - this._buffer[this._index]) / this._buffer.Length;
            this._buffer[this._index] = sample;
            this._index = (this._index + 1) % this._buffer.Length;
        }
    }


    //          BEHAVIORS
    /// <summary>
    /// Returns a rotation vector whose elements are in the range (-180f, 180f).
    /// </summary>
    /// <param name="euler">Rotation vector to center</param>
    /// <returns>The rotation with each element in the range (-180f, 180f).</returns>
    public static Vector3 CenterEulerAngles(Vector3 euler)
    {
        return new Vector3
            (
                euler.x > 180f ? euler.x - 360f : euler.x,
                euler.y > 180f ? euler.y - 360f : euler.y,
                euler.z > 180f ? euler.z - 360f : euler.z
            );
    }

    /// <summary>
    /// Repeat then Inverse Lerp
    /// </summary>
    /// <param name="t">Input value</param>
    /// <param name="max">Maximum value</param>
    /// <returns>Remainder of (t % max), normalized to range [0, 1]</returns>
    public static float NormalizedRepeat(float t, float max)
    {
        return Mathf.InverseLerp(0f, max, Mathf.Repeat(t, max));
    }

    public static Vector3 Apply(Vector3 op, float w, Matrix4x4 mat)
    {
        Vector4 v = op;
        v.w = w;

        return mat * v;
    }

    public static float LerpSmooth(float a, float b, float dt, float smoothingTime)
    {
        // Thanks Freya
        return Mathf.Lerp(a, b, 1f - Mathf.Pow(2f, (-1f * dt / (-1f * Mathf.Clamp(smoothingTime, 0.01f, 5f) / -4.321928f))));           // Constant is Mathf.Log(0.05f, 2f) which is 'precision'
    }

    public static Vector2 LerpSmooth2(Vector2 a, Vector2 b, float dt, float smoothingTime)
    {
        return Vector2.Lerp(a, b, 1f - Mathf.Pow(2f, (-1f * dt / (-1f * Mathf.Clamp(smoothingTime, 0.01f, 5f) / -4.321928f))));         // Constant is Mathf.Log(0.05f, 2f) which is 'precision'
    }

    public static Vector3 LerpSmooth3(Vector3 a, Vector3 b, float dt, float smoothingTime)
    {
        return Vector3.Lerp(a, b, 1f - Mathf.Pow(2f, (-1f * dt / (-1f * Mathf.Clamp(smoothingTime, 0.01f, 5f) / -4.321928f))));         // Constant is Mathf.Log(0.05f, 2f) which is 'precision'
    }

    public static Quaternion LerpSmoothRot(Quaternion a, Quaternion b, float dt, float smoothingTime)
    {
        return Quaternion.Slerp(a, b, 1f - Mathf.Pow(2f, (-1f * dt / (-1f * Mathf.Clamp(smoothingTime, 0.01f, 5f) / -4.321928f))));     // Constant is Mathf.Log(0.05f, 2f) which is 'precision'
    }


    /// <summary>
    /// Calculates and returns the closest points on a pair of given lines.
    /// </summary>
    /// <param name="pos1">Point on line 1.</param>
    /// <param name="dir1">Direction of line 1.</param>
    /// <param name="pos2">Point on line 2.</param>
    /// <param name="dir2">Direction of line 2.</param>
    /// <param name="closest1">Closest point between lines on line 1, outptut.</param>
    /// <param name="closest2">Closest point between lines on line 2, outptut.</param>
    /// <returns>True if calculation successful, false if not.</returns>
    public static bool GetClosestPoints(Vector3 pos1, Vector3 dir1, Vector3 pos2, Vector3 dir2, out Vector3 closest1, out Vector3 closest2)
    {
        Vector3 n = Vector3.Cross(dir1, dir2);
        Vector3 n1 = Vector3.Cross(dir1, n);
        Vector3 n2 = Vector3.Cross(dir2, n);

        float d1n2 = Vector3.Dot(dir1, n2);
        float d2n1 = Vector3.Dot(dir2, n1);

        if (!Mathf.Approximately(d1n2, 0f) && !Mathf.Approximately(d2n1, 0f))
        {
            closest1 = pos1 + (Vector3.Dot(pos2 - pos1, n2) / (Vector3.Dot(dir1, n2))) * dir1;
            closest2 = pos2 + (Vector3.Dot(pos1 - pos2, n1) / (Vector3.Dot(dir2, n1))) * dir2;

            return true;
        }
        else
        {
            closest1 = Vector3.zero;
            closest2 = Vector3.zero;

            return false;
        }
    }

    /// <summary>
    /// Three calls to Mathf.PerlinNoise1D packaged for brevity.
    /// Caller expected to handle offsets between dimensions to avoid variance issues.
    /// </summary>
    /// <param name="coords"></param>
    /// <returns>Random Vector3 in range [0,1]^3</returns>
    public static Vector3 CheapPerlin3d(Vector3 coords)
    {
        return new Vector3
            (
                Mathf.PerlinNoise1D(coords.x),
                Mathf.PerlinNoise1D(coords.y),
                Mathf.PerlinNoise1D(coords.z)
            );
    }

    /// <summary>
    /// Clips the given float at the given absolute value.
    /// </summary>
    /// <param name="op">Float to clip</param>
    /// <param name="absMax">Maximum absolute value</param>
    /// <returns>Float clipped to given absolute value.</returns>
    public static float ClipFloat(float op, float absMax)
    {
        return (Mathf.Abs(op) > absMax) ? Mathf.Sign(op) * absMax : op;
    }

    /// <summary>
    /// Clips the given vector at the given magnitude.
    /// </summary>
    /// <param name="op">Vector to clip</param>
    /// <param name="max">Maximum magnitude</param>
    /// <returns>Vector clipped to magnitude.</returns>
    public static Vector3 ClipVec2(Vector2 op, float max)
    {
        return (op.sqrMagnitude > max * max) ? op.normalized * max : op;
    }

    /// <summary>
    /// Clips the given vector at the given magnitude.
    /// </summary>
    /// <param name="op">Vector to clip</param>
    /// <param name="max">Maximum magnitude</param>
    /// <returns>Vector clipped to magnitude.</returns>
    public static Vector3 ClipVec3(Vector3 op, float max)
    {
        return (op.sqrMagnitude > max * max) ? op.normalized * max : op;
    }

    /// <summary>
    /// Creates an animation curve from the provided keyframes.
    /// </summary>
    /// <param name="dkfs">Keyframes to create a curve from.</param>
    /// <returns>An animation curve from the given keyframes.</returns>
    public static AnimationCurve CurveFromKeyframes(Keyframe[] dkfs)
    {
        UnityEngine.Keyframe[] kfs = new UnityEngine.Keyframe[dkfs.Length];

        for (int i = 0; i < kfs.Length; i++)
            kfs[i] = new UnityEngine.Keyframe(dkfs[i].timeValue.x, dkfs[i].timeValue.y, dkfs[i].tangents.x, dkfs[i].tangents.y);

        return new AnimationCurve(kfs);
    }

    /// <summary>
    /// Samples the distribution specified by the given quantile function curve.
    /// </summary>
    /// <param name="quantileCurve">Quantile curve for the desired distribution.</param>
    /// <returns>Sample pulled from the distribution. [0,1] </returns>
    public static float SampleDistribution(AnimationCurve quantileCurve)
    {
        return quantileCurve.Evaluate(Random.Range(0f, 1f));
    }
}