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

66 lines
3.7 KiB
C#

using System.Numerics;
namespace GamePort.Game;
/// <summary>412A90 four-child setup and467080 post-normal wheel positioning.
/// Child support is queried manually; children have no independent integrator.</summary>
public sealed class ScooterWheels
{
public const float Radius=23.769514083862305f;
public static readonly Vector3[] Offsets=[new(-3,1,73),new(3,1,73),new(-3,1,-30),new(3,1,-30)];
private readonly Vector3[] roots=new Vector3[4],angles=new Vector3[4];
private readonly float[] spins=new float[4];
private readonly bool[] supported=new bool[4];
public IReadOnlyList<Vector3> Roots=>roots;
public IReadOnlyList<Vector3> Rotations=>angles;
public int Front=>supported.Take(2).Count(v=>v);
public int Rear=>supported.Skip(2).Count(v=>v);
public void Reset(){Array.Clear(roots);Array.Clear(angles);Array.Clear(spins);Array.Clear(supported);}
public void Update(Vector3 root,Vector3 rotation,float steering,float speed,bool braking,bool crash,OriginalStaticCollision world)
{
var forward=RocketOrientation.Rotate(Vector3.UnitZ,rotation);
for(int i=0;i<4;i++)
{
var before=roots[i];var local=Offsets[i];var desired=root+RocketOrientation.Rotate(local,rotation);
if(!crash)
{
var result=OriginalBodySupport.Resolve(new(desired,before,Vector3.Zero,Vector3.Zero,Radius,1,PreventLowering:false),
supported[i],(a,b,s)=>world.TraceSupport(a,b,s));
supported[i]=result.Supported;
var anchor=result.Supported?result.SupportAnchor.Y:-25000;
local.Y=Math.Clamp(local.Y+anchor-desired.Y,Offsets[i].Y-15,Offsets[i].Y+15);
}
roots[i]=root+RocketOrientation.Rotate(local,rotation);
var delta=(roots[i]-before) with{Y=0};var distance=OriginalBodySupport.QuickMagnitude(delta);
if(distance!=0&&(speed<0||!braking))
spins[i]=RocketOrientation.AddAxis(spins[i],(float)(distance/Radius*57.295776f)*(Vector3.Dot(delta,forward)>0?1:-1));
angles[i]=i%2==0
?new(rotation.Z,RocketOrientation.AddAxis(rotation.Y,90),RocketOrientation.AddAxis(rotation.X,-spins[i]))
:new(RocketOrientation.AddAxis(0,-rotation.Z),RocketOrientation.AddAxis(rotation.Y,-90),RocketOrientation.AddAxis(rotation.X,spins[i]));
if(i<2)angles[i]=RocketOrientation.Add(angles[i],RocketOrientation.Rotate(new(0,steering,0),rotation));
}
}
public static Vector3 TerrainTarget(Vector3 normal,float yaw)
{
var(_,x)=OriginalHeading.Angles(new(normal.X,0,normal.Y));
var(_,z)=OriginalHeading.Angles(new(normal.Z,0,normal.Y));
var index=(int)((double)yaw*45.511112213134766)&16383;
var(sine,cosine)=RocketOrientation.Units(index);
//Source table cardinal endpoints are exact, rather than Math.Cos(pi/2)'s residue.
if((index&4095)==0){sine=index switch{4096=>1,12288=>-1,_=>0};cosine=index switch{0=>1,8192=>-1,_=>0};}
var components=new Vector3((float)(-x*(double)cosine-z*(double)sine),0,
(float)(-z*(double)cosine+x*(double)sine))*.02197265625f;
return new(RocketOrientation.AddAxis(0,-components.Z),yaw,RocketOrientation.AddAxis(0,components.X));
}
public static Vector3 Face(Vector3 rotation,Vector3 target,float rate,float dt)
{
float Axis(float current,float desired)
{
float delta=desired-current;
if(delta>=180)delta-=360;else if(delta< -180)delta+=360;
float gain=dt*rate;float step=delta*gain;
return MathF.Abs(step)<gain?desired:current+step;
}
return new(Axis(rotation.X,target.X),Axis(rotation.Y,target.Y),Axis(rotation.Z,target.Z));
}
}