Math3d.cs

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

public class Math3d
{
	public const float vectorPrecission = 1e-5f; //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
		{