71 lines
3.0 KiB
C#
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));
|
|
}
|
|
}
|