Skip to content

Instantly share code, notes, and snippets.

@cart
Created February 1, 2018 06:34
Show Gist options
  • Save cart/cc095f4950c40c1fe25169be5ee78247 to your computer and use it in GitHub Desktop.
Save cart/cc095f4950c40c1fe25169be5ee78247 to your computer and use it in GitHub Desktop.
A collection of C# Math extensions for Godot
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];
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment