69 lines
2.1 KiB
C#
69 lines
2.1 KiB
C#
using System.Numerics;
|
|
|
|
namespace Nerfed.Runtime;
|
|
|
|
public static class MathEx
|
|
{
|
|
public static float ToNearestMultiple(float value, float multiple) {
|
|
return MathF.Round(value / multiple, MidpointRounding.AwayFromZero) * multiple;
|
|
}
|
|
|
|
public static float Normalize(float value, float oldMin, float oldMax) {
|
|
return Remap(value, oldMin, oldMax, 0f, 1f);
|
|
}
|
|
|
|
public static float Denormalize(float value, float newMin, float newMax) {
|
|
return Remap(value, 0f, 1f, newMin, newMax);
|
|
}
|
|
|
|
public static float Remap(float value, float oldMin, float oldMax, float newMin, float newMax) {
|
|
return (value - oldMin) / (oldMax - oldMin) * (newMax - newMin) + newMin;
|
|
}
|
|
|
|
// https://stackoverflow.com/questions/70462758/c-sharp-how-to-convert-quaternions-to-euler-angles-xyz
|
|
public static Quaternion ToQuaternion(Vector3 v)
|
|
{
|
|
float cy = (float)Math.Cos(v.Z * 0.5);
|
|
float sy = (float)Math.Sin(v.Z * 0.5);
|
|
float cp = (float)Math.Cos(v.Y * 0.5);
|
|
float sp = (float)Math.Sin(v.Y * 0.5);
|
|
float cr = (float)Math.Cos(v.X * 0.5);
|
|
float sr = (float)Math.Sin(v.X * 0.5);
|
|
|
|
return new Quaternion
|
|
{
|
|
W = (cr * cp * cy + sr * sp * sy),
|
|
X = (sr * cp * cy - cr * sp * sy),
|
|
Y = (cr * sp * cy + sr * cp * sy),
|
|
Z = (cr * cp * sy - sr * sp * cy),
|
|
};
|
|
}
|
|
|
|
public static Vector3 ToEulerAngles(Quaternion q)
|
|
{
|
|
Vector3 angles = new();
|
|
|
|
// roll / x
|
|
double sinrCosp = 2 * (q.W * q.X + q.Y * q.Z);
|
|
double cosrCosp = 1 - 2 * (q.X * q.X + q.Y * q.Y);
|
|
angles.X = (float)Math.Atan2(sinrCosp, cosrCosp);
|
|
|
|
// pitch / y
|
|
double sinp = 2 * (q.W * q.Y - q.Z * q.X);
|
|
if (Math.Abs(sinp) >= 1)
|
|
{
|
|
angles.Y = (float)Math.CopySign(Math.PI / 2, sinp);
|
|
}
|
|
else
|
|
{
|
|
angles.Y = (float)Math.Asin(sinp);
|
|
}
|
|
|
|
// yaw / z
|
|
double sinyCosp = 2 * (q.W * q.Z + q.X * q.Y);
|
|
double cosyCosp = 1 - 2 * (q.Y * q.Y + q.Z * q.Z);
|
|
angles.Z = (float)Math.Atan2(sinyCosp, cosyCosp);
|
|
|
|
return angles;
|
|
}
|
|
} |