Math3d.cs

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

public class Math3d
{
    public const double vectorPrecission = 1e-5d; //For Vector comparisons

    private static Transform tempChild = null;
    private static Transform tempParent = null;

    private static Vector3[] positionRegister;
    private static float[] posTimeRegister;
    private static int positionSamplesTaken = 0;

    private static Quaternion[] rotationRegister;
    private static float[] rotTimeRegister;
    private static int rotationSamplesTaken = 0;

    public static void Init()
    {

        tempChild = (new GameObject("Math3d_TempChild")).transform;
        tempParent = (new GameObject("Math3d_TempParent")).transform;

        tempChild.gameObject.hideFlags = HideFlags.HideAndDontSave;
        MonoBehaviour.DontDestroyOnLoad(tempChild.gameObject);

        tempParent.gameObject.hideFlags = HideFlags.HideAndDontSave;
        MonoBehaviour.DontDestroyOnLoad(tempParent.gameObject);

        //set the parent
        tempChild.parent = tempParent;
    }

    //Get a point on a Catmull-Rom spline.
    //The percentage is in range 0 to 1, which starts at the second control point and ends at the second last control point. 
    //The array cPoints should contain all control points. The minimum amount of control points should be 4. 
    //Source: https://forum.unity.com/threads/waypoints-and-constant-variable-speed-problems.32954/#post-213942
    public static Vector2 GetPointOnSpline(float percentage, Vector2[] cPoints)
    {

        //Minimum size is 4
        if (cPoints.Length >= 4)
        {

            //Convert the input range (0 to 1) to range (0 to numSections)
            int numSections = cPoints.Length - 3;
            int curPoint = Mathf.Min(Mathf.FloorToInt(percentage * (float)numSections), numSections - 1);
            float t = percentage * (float)numSections - (float)curPoint;

            //Get the 4 control points around the location to be sampled.
            Vector2 p0 = cPoints[curPoint];
            Vector2 p1 = cPoints[curPoint + 1];
            Vector2 p2 = cPoints[curPoint + 2];
            Vector2 p3 = cPoints[curPoint + 3];

            //The Catmull-Rom spline can be written as:
            // 0.5 * (2*P1 + (-P0 + P2) * t + (2*P0 - 5*P1 + 4*P2 - P3) * t^2 + (-P0 + 3*P1 - 3*P2 + P3) * t^3)
            //Variables P0 to P3 are the control points.
            //Variable t is the position on the spline, with a range of 0 to numSections.
            //C# way of writing the function. Note that f means float (to force precision).
            Vector2 result = .5f * (2f * p1 + (-p0 + p2) * t + (2f * p0 - 5f * p1 + 4f * p2 - p3) * (t * t) + (-p0 + 3f * p1 - 3f * p2 + p3) * (t * t * t));

            return new Vector2(result.x, result.y);
        }

        else
        {

            return new Vector2(0, 0);
        }
    }

    //Finds the intersection points between a straight line and a spline. Solves a Cubic polynomial equation
    //The output is in the form of a percentage along the length of the spline (range 0 to 1).
    //The linePoints array should contain two points which form a straight line.
    //The cPoints array should contain all the control points of the spline.
    //Use case: create a gauge with a non-linear scale by defining an array with needle angles vs the number it should point at. The array creates a spline.
    //Driving the needle with a float in range 0 to 1 gives an unpredictable result. Instead, use the GetLineSplineIntersections() function to find the angle the
    //gauge needle should have for a given number it should point at. In this case, cPoints should contain x for angle and y for scale number.
    //Make a horizontal line at the given scale number (y) you want to find the needle angle for. The returned float is a percentage location on the spline (range 0 to 1). 
    //Plug this value into the GetPointOnSpline() function to get the x coordinate which represents the needle angle.
    //Source: https://medium.com/@csaba.apagyi/finding-catmull-rom-spline-and-line-intersection-part-2-mathematical-approach-dfb969019746
    public static float[] GetLineSplineIntersections(Vector2[] linePoints, Vector2[] cPoints)
    {

        List<float> list = new List<float>();
        float[] crossings;

        int numSections = cPoints.Length - 3;

        //The line spline intersection can only be calculated for one segment of a spline, meaning 4 control points,
        //with a spline segment between the middle two control points. So check all spline segments.
        for (int i = 0; i < numSections; i++)
        {

            //Get the 4 control points around the location to be sampled.
            Vector2 p0 = cPoints[i];
            Vector2 p1 = cPoints[i + 1];
            Vector2 p2 = cPoints[i + 2];
            Vector2 p3 = cPoints[i + 3];

            //The Catmull-Rom spline can be written as:
            // 0.5 * (2P1 + (-P0 + P2) * t + (2P0 - 5P1 + 4P2 - P3) * t^2 + (-P0 + 3P1 - 3P2 + P3) * t^3)
            //Variables P0 to P3 are the control points.
            //Notation: 2P1 means 2*controlPoint1
            //Variable t is the position on the spline, converted from a range of 0 to 1.
            //C# way of writing the function is below. Note that f means float (to force precision).
            //Vector2 result = .5f * (2f * p1 + (-p0 + p2) * t + (2f * p0 - 5f * p1 + 4f * p2 - p3) * (t * t) + (-p0 + 3f * p1 - 3f * p2 + p3) * (t * t * t));

            //The variable t is the only unknown, so the rest can be substituted:
            //a = 0.5 * (-p0 + 3*p1 - 3*p2 + p3)
            //b = 0.5 * (2*p0 - 5*p1 + 4*p2 - p3) 
            //c = 0.5 * (-p0 + p2)
            //d = 0.5 * (2*p1)

            //This gives rise to the following Cubic equation:
            //a * t^3 + b * t^2 + c * t + d = 0

            //The spline control points (p0-3) consist of two variables: the x and y coordinates. They are independent so we can handle them separately.
            //Below, a1 is substitution a where the x coordinate of each point is used, like so:  a1 = 0.5 * (-p0.x + 3*p1.x - 3*p2.x + p3.x)
            //Below, a2 is substitution a where the y coordinate of each point is used, like so:  a2 = 0.5 * (-p0.y + 3*p1.y - 3*p2.y + p3.y)
            //The same logic applies for substitutions b, c, and d.

            float a1 = 0.5f * (-p0.x + 3f * p1.x - 3f * p2.x + p3.x);
            float a2 = 0.5f * (-p0.y + 3f * p1.y - 3f * p2.y + p3.y);
            float b1 = 0.5f * (2f * p0.x - 5f * p1.x + 4f * p2.x - p3.x);
            float b2 = 0.5f * (2f * p0.y - 5f * p1.y + 4f * p2.y - p3.y);
            float c1 = 0.5f * (-p0.x + p2.x);
            float c2 = 0.5f * (-p0.y + p2.y);
            float d1 = 0.5f * (2f * p1.x);
            float d2 = 0.5f * (2f * p1.y);

            //We now have two Cubic functions. One for x and one for y.
            //Note that a, b, c, and d are not vector variables itself but substituted functions.
            //x = a1 * t^3 + b1 * t^2 + c1 * t + d1
            //y = a2 * t^3 + b2 * t^2 + c2 * t + d2

            //Line formula, standard form:
            //Ax + By + C = 0
            float A = linePoints[0].y - linePoints[1].y;
            float B = linePoints[1].x - linePoints[0].x;
            float C = (linePoints[0].x - linePoints[1].x) * linePoints[0].y + (linePoints[1].y - linePoints[0].y) * linePoints[0].x;

            //Substituting the values of x and y from the separated Spline formula into the Line formula, we get:
            //A * (a1 * t^3 + b1 * t^2 + c1 * t + d1) + B * (a2 * t^3 + b2 * t^2 + c2 * t + d2) + C = 0

            //Rearranged version:		
            //(A * a1 + B * a2) * t^3 + (A * b1 + B * b2) * t^2 + (A * c1 + B * c2) * t + (A * d1 + B * d2 + C) = 0

            //Substituting gives rise to a Cubic function:
            //a * t^3 + b * t^2 + c * t + d = 0
            float a = A * a1 + B * a2;
            float b = A * b1 + B * b2;
            float c = A * c1 + B * c2;
            float d = A * d1 + B * d2 + C;


            //This is again a Cubic equation, combined from the Line and the Spline equation. If you solve this you can get up to 3 line-spline cross points.
            //How to solve a Cubic equation is described here: 
            //https://www.cs.rit.edu/~ark/pj/lib/edu/rit/numeric/Cubic.shtml
            //https://www.codeproject.com/Articles/798474/To-Solve-a-Cubic-Equation

            int crossAmount;
            float cross1;
            float cross2;
            float cross3;
            float crossCorrected;

            //Two different implementations of solving a Cubic equation.
            //	SolveCubic2(out crossAmount, out cross1, out cross2, out cross3, a, b, c, d);
            SolveCubic(out crossAmount, out cross1, out cross2, out cross3, a, b, c, d);

            //Get the highest and lowest value (in range 0 to 1) of the current section and calculate the difference.
            float currentSectionLowest = (float)i / (float)numSections;
            float currentSectionHighest = ((float)i + 1f) / (float)numSections;
            float diff = currentSectionHighest - currentSectionLowest;

            //Only use the result if it is within range 0 to 1.
            //The range 0 to 1 is within the current segment. It has to be converted to the range of the entire spline,
            //which still uses a range of 0 to 1.
            if (cross1 >= 0 && cross1 <= 1)
            {

                //Map an intermediate range (0 to 1) to the lowest and highest section values.
                crossCorrected = (cross1 * diff) + currentSectionLowest;

                //Add the result to the list.
                list.Add(crossCorrected);
            }

            if (cross2 >= 0 && cross2 <= 1)
            {

                //Map an intermediate range (0 to 1) to the lowest and highest section values.
                crossCorrected = (cross2 * diff) + currentSectionLowest;

                //Add the result to the list.
                list.Add(crossCorrected);