Vector, Quaternion, Physics, Normals etc. utils

BezierUtils.cs

// <copyright file="BezierUtils.cs" company="vvrvvd">
// Copyright (c) vvrvvd. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.
// </copyright>

using UnityEngine;

namespace SplineEditor
{
	/// <summary>
	/// Utility class for general Bezier Curve calculations.
	/// </summary>
	public static class BezierUtils
	{
		/// <summary>
		/// Returns coordinates for point on a cubic bezier curve with given control points and t.
		/// </summary>
		/// <param name="p0">The first control point position for cubic bezier curve.</param>
		/// <param name="p1">The second control point position for a cubic bezier curve.</param>
		/// <param name="p2">The third control point position for a cubic bezier curve.</param>
		/// <param name="p3">The fourth control point position for a cubic bezier curve.</param>
		/// <param name="t">Parameter for point on a cubic bezier curve.</param>
		/// <returns>Point position on a cubic bezier curve for given parameters.</returns>
		public static Vector3 GetPoint(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float t)
		{
			t = Mathf.Clamp01(t);
			var oneMinusT = 1f - t;
			return
				(oneMinusT * oneMinusT * oneMinusT * p0) +
				(3f * oneMinusT * oneMinusT * t * p1) +
				(3f * oneMinusT * t * t * p2) +
				(t * t * t * p3);
		}

		/// <summary>
		/// Returns the first derivative of a cubic bezier curve for given control points and t.
		/// </summary>
		/// <param name="p0">The first control point position for cubic bezier curve.</param>
		/// <param name="p1">The second control point position for a cubic bezier curve.</param>
		/// <param name="p2">The third control point position for a cubic bezier curve.</param>
		/// <param name="p3">The fourth control point position for a cubic bezier curve.</param>
		/// <param name="t">Parameter for point on a cubic bezier curve.</param>
		/// <returns>The first derivative on a cubic bezier curve for given parameters.</returns>
		public static Vector3 GetTheFirstDerivative(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float t)
		{
			t = Mathf.Clamp01(t);
			float oneMinusT = 1f - t;
			return
				(3f * oneMinusT * oneMinusT * (p1 - p0)) +
				(6f * oneMinusT * t * (p2 - p1)) +
				(3f * t * t * (p3 - p2));
		}

		/// <summary>
		/// Cubic bezier curve length based on mid point quadratic approximation.
		/// </summary>
		/// <param name="p0">The first control point position for cubic bezier curve.</param>
		/// <param name="p1">The second control point position for a cubic bezier curve.</param>
		/// <param name="p2">The third control point position for a cubic bezier curve.</param>
		/// <param name="p3">The fourth control point position for a cubic bezier curve.</param>
		/// <returns>Total cubic bezier curve length using mid point quadratic approximation.</returns>
		public static float GetCubicLength(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3)
		{
			var midPoint = GetPoint(p0, p1, p2, p3, 0.5f);
			return GetQuadraticLength(p0, p1, midPoint) + GetQuadraticLength(midPoint, p2, p3);
		}

		/// <summary>
		/// Quadratic Bezier Curve Length.
		/// <remarks>
		/// Integral calculation by Dave Eberly.
		/// See: http://www.gamedev.net/topic/551455-length-of-a-generalized-quadratic-bezier-curve-in-3d/
		/// </remarks>
		/// </summary>
		/// <param name="p0">The first control point position for a quadratic bezier curve.</param>
		/// <param name="p1">The second control point position for a quadratic bezier curve.</param>
		/// <param name="p2">The third control point position for a quadratic bezier curve.</param>
		/// <returns>Total quadratic bezier curve length using integrals.</returns>
		public static float GetQuadraticLength(Vector3 p0, Vector3 p1, Vector3 p2)
		{
			if (p0 == p2)
			{
				if (p0 == p1)
				{
					return 0.0f;
				}

				return (p0 - p1).magnitude;
			}

			if (p1 == p0 || p1 == p2)
			{
				return (p0 - p2).magnitude;
			}

			var a0 = p1 - p0;
			var a1 = p0 - (2.0f * p1) + p2;
			if (Mathf.Approximately(a1.x, 0.0f) && Mathf.Approximately(a1.y, 0.0f) && Mathf.Approximately(a1.z, 0.0f))
			{
				var c = 4.0f * Vector3.Dot(a1, a1);
				var b = 8.0f * Vector3.Dot(a0, a1);
				var a = 4.0f * Vector3.Dot(a0, a0);
				var q = (4.0f * a * c) - (b * b);
				var twoCpB = (2.0f * c) + b;
				var sumCBA = c + b + a;
				var l0 = 0.25f / c * ((twoCpB * Mathf.Sqrt(sumCBA)) - (b * Mathf.Sqrt(a)));
				if (Mathf.Approximately(q, 0.0f))
				{
					return l0;
				}

				var l1 = q / (8.0f * Mathf.Pow(c, 1.5f)) * (Mathf.Log((2.0f * Mathf.Sqrt(c * sumCBA)) + twoCpB) - Mathf.Log((2.0f * Mathf.Sqrt(c * a)) + b));
				return l0 + l1;
			}
			else
			{
				return 2.0f * a0.magnitude;
			}
		}

		/// <summary>
		/// Returns coordinates of p1 for given control points and point on a cubic curve and t.
		/// </summary>
		/// <param name="p0">The first control point position for a cubic bezier curve.</param>
		/// <param name="p2">The third control point position for a cubic bezier curve.</param>
		/// <param name="p3">The fourth control point position for a cubic bezier curve.</param>
		/// <param name="pointOnCurve">Point on curve used for calculating the second control point on cubic bezier curve.</param>
		/// <param name="t">Parameter for point on a cubic bezier curve.</param>
		/// <returns>The second control point position for a cubic bezier curve.</returns>
		public static Vector3 GetInverseCubicPointP1(Vector3 p0, Vector3 p2, Vector3 p3, Vector3 pointOnCurve, float t)
		{
			t = Mathf.Clamp01(t);
			var oneMinusT = 1f - t;
			return
				(pointOnCurve -
				(oneMinusT * oneMinusT * oneMinusT * p0) -
				(3f * oneMinusT * t * t * p2) -
				(t * t * t * p3)) /
				(3f * oneMinusT * oneMinusT * t);
		}

		/// <summary>
		/// Returns coordinates of p2 for given control points and point on curve and t.
		/// </summary>
		/// <param name="p0">The first control point position for a cubic bezier curve.</param>
		/// <param name="p1">The second control point position for a cubic bezier curve.</param>
		/// <param name="p3">The fourth control point position for a cubic bezier curve.</param>
		/// <param name="pointOnCurve">Point on curve used for calculating the third control point on cubic bezier curve.</param>
		/// <param name="t">Parameter for point on a cubic bezier curve.</param>
		/// <returns>The third control point position for a cubic bezier curve.</returns>
		public static Vector3 GetInverseCubicPointP2(Vector3 p0, Vector3 p1, Vector3 p3, Vector3 pointOnCurve, float t)
		{
			t = Mathf.Clamp01(t);
			var oneMinusT = 1f - t;
			return
				(pointOnCurve -
				(oneMinusT * oneMinusT * oneMinusT * p0) -
				(3f * oneMinusT * oneMinusT * t * p1) -
				(t * t * t * p3)) /
				(3f * oneMinusT * t * t);
		}

		/// <summary>
		/// Calculates controls points p1 and p2 for given p0, p3 and two points on curve with given u, v for them.
		/// </summary>
		/// <remarks>
		/// See: https://www.ijser.org/researchpaper/INVERSE-POINT-SOLUTION-OF-BEZIER-CURVE.pdf.
		/// </remarks>
		/// <param name="p0">The first control point for a cubic curve.</param>
		/// <param name="p3">The fourth control point for a cubic curve.</param>
		/// <param name="f">Point on the cubic curve for given u value.</param>
		/// <param name="g">Point on the cubic curve for given v value.</param>
		/// <param name="u">value of t parameter for given point f on the cubic curve.</param>
		/// <param name="v">value of t parameter for given point g on the cubic curve.</param>
		/// <param name="p1">The second control point for a cubic bezier curve.</param>
		/// <param name="p2">The third control point for a cubic bezier curve.</param>
		public static void GetInverseControlPoints(Vector3 p0, Vector3 p3, Vector3 f, Vector3 g, float u, float v, out Vector3 p1, out Vector3 p2)
		{
			p1 = Vector3.zero;
			p2 = Vector3.zero;

			var oneMinusU = 1f - u;
			var c =
				f -
				(oneMinusU * oneMinusU * oneMinusU * p0) -
				(u * u * u * p3);

			var oneMinusV = 1f - v;
			var d =
				g -
				(oneMinusV * oneMinusV * oneMinusV * p0) -
				(v * v * v * p3);

			var det =
					(3f * oneMinusU * oneMinusU * u * 3f * oneMinusV * v * v) -
					(3f * oneMinusU * u * u * 3f * oneMinusV * oneMinusV * v);

			var m0 = (3f * oneMinusV * v * v) / det;
			var m1 = (-3f * oneMinusU * u * u) / det;
			var m2 = (-3f * oneMinusV * oneMinusV * v) / det;
			var m3 = (3f * oneMinusU * oneMinusU * u) / det;

			var a = new float[,]
			{
				{ m0, m1 },
				{ m2, m3 }

				// | m0 m1 |
				// | m2 m3 |
			};

			var b = new float[,]
			{
				{ c.x, c.y, c.z },
				{ d.x, d.y, d.z }

				// | c.x d.x |
				// | c.y d.y |
				// | c.z d.z |
			};

			var solution = MultiplyMatrices(a, b);
			p1.x = solution[0, 0];
			p1.y = solution[0, 1];
			p1.z = solution[0, 2];

			p2.x = solution[1, 0];
			p2.y = solution[1, 1];
			p2.z = solution[1, 2];
		}

		private static float[,] MultiplyMatrices(float[,] a, float[,] b)
		{
			int m = a.GetLength(0);
			int n = a.GetLength(1);
			int p = b.GetLength(0);
			int q = b.GetLength(1);

			if (n != p)
			{
				Debug.LogError($"Matrix multiplication not possible due to sizes not matching {n} != {p}");
			}

			float[,] c = new float[m, q];

			for (var i = 0; i < m; i++)
			{
				for (var j = 0; j < q; j++)
				{
					c[i, j] = 0;
					for (int k = 0; k < n; k++)
					{
						c[i, j] += a[i, k] * b[k, j];
					}
				}
			}

			return c;
		}
	}
}

NormalsUtils.cs

// <copyright file="NormalsUtils.cs" company="vvrvvd">
// Copyright (c) vvrvvd. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.
// </copyright>

using UnityEngine;

namespace SplineEditor
{
	/// <summary>
	/// Utility class used for general normal vectors calculations.
	/// </summary>
	public static class NormalsUtils
	{
		/// <summary>
		/// Calculates normal based on rotation axis, two points and two tangents on curve.
		/// </summary>
		/// <param name="rotationAxis">Reference to the rotation axis that is used for normals calculations.</param>
		/// <param name="prevPoint">Previous point position.</param>
		/// <param name="currentPoint">Current point position.</param>
		/// <param name="prevTan">Previous tangent at point.</param>
		/// <param name="currenTan">Current tangent at point.</param>
		/// <returns>Normal vector calculated based on given parameters.</returns>
		public static Vector3 CalculateNormal(ref Vector3 rotationAxis, Vector3 prevPoint, Vector3 currentPoint, Vector3 prevTan, Vector3 currenTan)
		{
			// First reflection
			var offset = currentPoint - prevPoint;
			var sqrDst = offset.sqrMagnitude;
			var rot = rotationAxis - (offset * 2 / sqrDst * Vector3.Dot(offset, rotationAxis));
			var tan = prevTan - (offset * 2 / sqrDst * Vector3.Dot(offset, prevTan));

			// Second reflection
			var v2 = currenTan - tan;
			var c2 = Vector3.Dot(v2, v2);

			var finalRot = rot - (v2 * 2 / c2 * Vector3.Dot(v2, rot));
			var n = Vector3.Cross(finalRot, currenTan).normalized;
			rotationAxis = finalRot;
			return n;
		}
	}
}

PhysicsUtils.cs

// <copyright file="PhysicsUtils.cs" company="vvrvvd">
// Copyright (c) vvrvvd. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.
// </copyright>

using UnityEngine;

namespace SplineEditor
{
	/// <summary>
	/// Utility class used for general physics calculations.
	/// </summary>
	public static class PhysicsUtils
	{
		/// <summary>
		/// Calculates casted point in the direction and returns if the point exists.
		/// </summary>
		/// <param name="point">Raycast origin position.</param>
		/// <param name="direction">Raycast direction position.</param>
		/// <param name="castedPoint">Reference to casted point Vector.</param>
		/// <returns>If a casted point was found or not.</returns>
		public static bool TryCastPoint(Vector3 point, Vector3 direction, out Vector3 castedPoint)
		{
			var isCorrectPosition = Physics.Raycast(point, direction, out var hit, Mathf.Infinity, Physics.AllLayers);

			castedPoint = isCorrectPosition ? hit.point : point;
			return isCorrectPosition;
		}
	}
}

QuaternionUtils.cs

// <copyright file="QuaternionUtils.cs" company="vvrvvd">
// Copyright (c) vvrvvd. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.
// </copyright>

using UnityEngine;

namespace SplineEditor
{
	/// <summary>
	/// Utility class used for general Quaternion calculations.
	/// </summary>
	public static class QuaternionUtils
	{
		/// <summary>
		/// Returns signed angle between two quaterion on given axis.
		/// </summary>
		/// <remarks>
		/// See: https://answers.unity.com/questions/599393/angles-from-quaternionvector-problem.html.
		/// </remarks>
		/// <param name="a">The first quaterion.</param>
		/// <param name="b">The second quaternion.</param>
		/// <param name="axis">Rotation axis between quaterions used for signed angle calculation.</param>
		/// <returns>Signed angle between given quaternions on the given axis.</returns>
		public static float GetSignedAngle(Quaternion a, Quaternion b, Vector3 axis)
		{
			var angle = 0f;
			var angleAxis = Vector3.zero;
			(b * Quaternion.Inverse(a)).ToAngleAxis(out angle, out angleAxis);
			if (Vector3.Angle(axis, angleAxis) > 90f)
			{
				angle = -angle;
			}

			return Mathf.DeltaAngle(0f, angle);
		}
	}
}

VectorUtils.cs

// <copyright file="VectorUtils.cs" company="vvrvvd">
// Copyright (c) vvrvvd. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.
// </copyright>

using UnityEngine;

namespace SplineEditor
{
	/// <summary>
	/// Utility class used for general Vector calculations.
	/// </summary>
	public static class VectorUtils
	{
		/// <summary>
		/// Rotates target point arount pivot point by given rotation.
		/// </summary>
		/// <param name="targetPoint">Position to rotated.</param>
		/// <param name="pivotPoint">Pivot position to be rotated around.</param>
		/// <param name="rotation">Quaternion to rotate target point around pivot point.</param>
		/// <returns>Target point position rotated around pivot point position by given quaternion rotation.</returns>
		public static Vector3 RotateAround(Vector3 targetPoint, Vector3 pivotPoint, Quaternion rotation)
		{
			return (rotation * (targetPoint - pivotPoint)) + pivotPoint;
		}

		/// <summary>
		/// Returns inversed lerp value t for given start, end and lerped point positions.
		/// </summary>
		/// <param name="startPoint">Start position.</param>
		/// <param name="endPoint">End position.</param>
		/// <param name="lerpedPoint">Lerped position between start and end positions.</param>
		/// <returns>Value of parameter t that'd give lerped point position between start point and end point positions (inversed lerp).</returns>
		public static float InverseLerp(Vector3 startPoint, Vector3 endPoint, Vector3 lerpedPoint)
		{
			Vector3 aB = endPoint - startPoint;
			Vector3 aV = lerpedPoint - startPoint;
			return Vector3.Dot(aV, aB) / Vector3.Dot(aB, aB);
		}
	}
}