Math3d.cs

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

#pragma warning disable IDE0018 // Inline variable declaration
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
        {