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
		{

			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.