A collection of C# Math extensions for Godot

gistfile1.txt

using Godot;

namespace HighHat.MathHelpers
{
	public static class MathExtensions
	{
		// TODO: expose Godot's Transform::interpolate_with to scripting.  This is just a C# port
		public static Transform InterpolateWith(this Transform transform, Transform targetTransform, float percent, bool includeScale = true)
		{
			var rotation = transform.basis.Orthonormalized().ToQuat();
			var position = transform.origin;

			var targetRotation = targetTransform.basis.ToQuat();
			var targetPosition = targetTransform.origin;

			var basis = rotation.Slerp(targetRotation, percent).Normalized();
			var origin = position.LinearInterpolate(targetPosition, percent);
			var result = new Transform(basis, origin);
			
			if (includeScale)
			{
				var scale = transform.basis.GetSignedScale(); // try with scale and see if it works
				var targetScale = targetTransform.basis.GetSignedScale();
				var scaleResult = scale.LinearInterpolate(targetScale, percent);
				result = result.Scaled(scaleResult);
			}

			return result;
		}

		public static Vector3 GetSignedScale(this Basis basis)
		{
			var determinantSign = basis.Determinant() > 0 ? 1 : -1;
			return determinantSign * basis.Scale;
		}

		public static Quat ToQuat(this Basis basis)
		{
			var xAxis = basis.GetAxis(0); // [0][0] [1][0] [2][0]
			var yAxis = basis.GetAxis(1); // [0][1] [1][1] [2][1]
			var zAxis = basis.GetAxis(2); // [0][2] [1][2] [2][2]
			var trace =  xAxis.x + yAxis.y + zAxis.z;
			var temp = new float[4];

			if (trace > 0.0f)
			{
				var s = Mathf.Sqrt(trace + 1.0f);
				temp[3] = s * 0.5f;
				s = 0.5f / s;

				temp[0] = (yAxis.z - zAxis.y) * s;
				temp[1] = (zAxis.x - xAxis.z) * s;
				temp[2] = (xAxis.y - yAxis.x) * s;
			}
			else
			{
				var i = xAxis.x < yAxis.y ?
					(yAxis.y < zAxis.z ? 2 : 1) :
					(xAxis.x < zAxis.z ? 2: 0);
				
				var j = (i + 1) % 3;
				var k = (i + 2) % 3;

				var s = Mathf.Sqrt(basis.GetElement(i, i) - basis.GetElement(j, j) - basis.GetElement(k, k) + 1.0f);
				temp[i] = s * 0.5f;
				s = 0.5f / s;

				temp[3] = (basis.GetElement(k, j) - basis.GetElement(j, k)) * s;
				temp[j] = (basis.GetElement(j, i) + basis.GetElement(i, j)) * s;
				temp[k] = (basis.GetElement(k, i) + basis.GetElement(i, k)) * s;
			}

			return new Quat(temp[0], temp[1], temp[2], temp[3]);
		}

		public static float GetElement(this Basis basis, int i, int j)
		{
			return basis.GetAxis(j)[i];
		}
	}
}