Files
Neutron/src/GamePort/Game/RocketOrientation.cs
T

71 lines
3.0 KiB
C#

using System.Numerics;
namespace GamePort.Game;
/// <summary>472B10/472480/41ADF0. Lookup angles and the source's asymmetric
/// negative seam are intentional. Numeric original references accompany the
/// Rocketship tests; double intermediates approximate x87, not its full ABI.</summary>
public static class RocketOrientation
{
public static (float Sin, float Cos) Trig(float degrees)
{
if (!float.IsFinite(degrees) || Math.Abs(degrees) > 40000000)
throw new ArgumentOutOfRangeException(nameof(degrees));
var index = (int)((double)degrees * 45.511112213134766) & 16383;
return Units(index);
}
public static (float Sin, float Cos) Units(int index)
{
var radians = (index & 16383) * (Math.Tau / 16384);
return ((float)Math.Sin(radians), (float)Math.Cos(radians));
}
public static Vector3 Rotate(Vector3 vector, Vector3 euler)
{
Finite(vector); Finite(euler);
var (sx,cx)=Trig(euler.X); var (sy,cy)=Trig(euler.Y); var (sz,cz)=Trig(euler.Z);
var a=(float)((double)cz*vector.Y-(double)sz*vector.X);
var b=(double)cz*vector.X+(double)sz*vector.Y;
var c=(double)cx*vector.Z-(double)a*sx;
return new((float)(b*cy-c*sy),(float)((double)a*cx+(double)sx*vector.Z),(float)(c*cy+b*sy));
}
public static float AddAxis(float value, float delta)
{
var total=(double)value+delta;
if (!double.IsFinite(total) || Math.Abs(total)>int.MaxValue)
throw new ArgumentOutOfRangeException(nameof(delta));
if (total<0) total=360-Math.Abs(Math.Truncate(total));
else if (total>360) total-=360;
return (float)total;
}
public static Vector3 Add(Vector3 angles, Vector3 delta) =>
new(AddAxis(angles.X,delta.X),AddAxis(angles.Y,delta.Y),AddAxis(angles.Z,delta.Z));
public static Vector3 Face(Vector3 rotation, Vector3 destination, float seconds, bool barrelRolling=false)
{
Finite(rotation); Finite(destination);
if (!float.IsFinite(seconds) || seconds<=0 || seconds>=1)
throw new ArgumentOutOfRangeException(nameof(seconds));
if (barrelRolling) return rotation;
var difference=destination-rotation;
float Arc(float value,bool yaw=false) => value>180 ? value-360 : !yaw&&value< -180 ? value+360 : value;
return Add(rotation,new(Arc(difference.X)*seconds*2,Arc(difference.Y,true)*seconds*4,Arc(difference.Z)*seconds*3));
}
public static Matrix4x4 Model(Vector3 euler, Vector3 root, Vector3 scale)
{
Finite(root); Finite(scale);
var x=Rotate(Vector3.UnitX,euler);var y=Rotate(Vector3.UnitY,euler);var z=Rotate(Vector3.UnitZ,euler);
return Matrix4x4.CreateScale(scale)*new Matrix4x4(x.X,x.Y,x.Z,0,y.X,y.Y,y.Z,0,z.X,z.Y,z.Z,0,root.X,root.Y,root.Z,1);
}
private static void Finite(Vector3 value)
{
if (!float.IsFinite(value.X)||!float.IsFinite(value.Y)||!float.IsFinite(value.Z))
throw new ArgumentOutOfRangeException(nameof(value));
}
}