Skip to content
This repository has been archived by the owner on Nov 1, 2024. It is now read-only.

Commit

Permalink
Merge pull request #116 from Corvax-Frontier/revert-115-opimizon
Browse files Browse the repository at this point in the history 
Revert "Optimizon 2"
  • Loading branch information
@Vonsant
Vonsant authored May 4, 2024
2 parents 34dbb67 + 628b9d4 commit d2c2ab7
Showing 1 changed file with 232 additions and 50 deletions.
282 changes: 232 additions & 50 deletions Content.Server/Physics/Controllers/MoverController.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -270,45 +270,80 @@ private static void ApplyTick(PilotComponent component, float fraction)
/// </summary>
private Vector2 ObtainMaxVel(Vector2 vel, ShuttleComponent shuttle)
{
// Use LengthSquared to avoid unnecessary square root computation
if (vel.LengthSquared() == 0f)
if (vel.Length() == 0f)
return Vector2.Zero;

// Directly extract velocity components
float velX = vel.X;
float velY = vel.Y;
// this math could PROBABLY be simplified for performance
// probably
// __________________________________
// / / __ __ \2 / __ __ \2
// O = I : _ / |I * | 1/H | | + |I * | 0 | |
// V \ |_ 0 _| / \ |_1/V_| /

// Simplified index calculation based on the direction of velocity components
int horizIndex = velX > 0 ? 1 : 3;
int vertIndex = velY > 0 ? 2 : 0;
var horizIndex = vel.X > 0 ? 1 : 3; // east else west
var vertIndex = vel.Y > 0 ? 2 : 0; // north else south
var horizComp = vel.X != 0 ? MathF.Pow(Vector2.Dot(vel, new Vector2(shuttle.BaseLinearThrust[horizIndex] / shuttle.LinearThrust[horizIndex], 0f)), 2) : 0;
var vertComp = vel.Y != 0 ? MathF.Pow(Vector2.Dot(vel, new Vector2(0f, shuttle.BaseLinearThrust[vertIndex] / shuttle.LinearThrust[vertIndex])), 2) : 0;

// Calculate thrust factors only if the respective velocity component is non-zero
float horizThrustRatio = velX != 0 ? shuttle.BaseLinearThrust[horizIndex] / shuttle.LinearThrust[horizIndex] : 0;
float vertThrustRatio = velY != 0 ? shuttle.BaseLinearThrust[vertIndex] / shuttle.LinearThrust[vertIndex] : 0;

// Calculate normalized component values
float horizNormalized = velX * horizThrustRatio;
float vertNormalized = velY * vertThrustRatio;

// Calculate the normalization factor using the squared sum of the normalized components
float normalizationFactor = 1f / MathF.Sqrt(horizNormalized * horizNormalized + vertNormalized * vertNormalized);

// Apply the normalization factor to the entire vector and scale by maximum velocity
return new Vector2(velX * normalizationFactor, velY * normalizationFactor) * shuttle.BaseMaxLinearVelocity;
return shuttle.BaseMaxLinearVelocity * vel * MathF.ReciprocalSqrtEstimate(horizComp + vertComp);
}

private void HandleShuttleMovement(float frameTime)
{
var directions = new[] { DirectionFlag.South, DirectionFlag.East, DirectionFlag.North, DirectionFlag.West };
var xformQuery = GetEntityQuery<TransformComponent>();
var newPilots = new Dictionary<EntityUid, (ShuttleComponent Shuttle, HashSet<(EntityUid PilotUid, PilotComponent Pilot, InputMoverComponent Mover, TransformComponent ConsoleXform)>)>();

// We just mark off their movement and the shuttle itself does its own movement
var activePilotQuery = EntityQueryEnumerator<PilotComponent, InputMoverComponent>();
var shuttleQuery = GetEntityQuery<ShuttleComponent>();

while (activePilotQuery.MoveNext(out var uid, out var pilot, out var mover))
{
var consoleEnt = pilot.Console;

// TODO: This is terrible. Just make a new mover and also make it remote piloting + device networks
if (TryComp<DroneConsoleComponent>(consoleEnt, out var cargoConsole))
{
consoleEnt = cargoConsole.Entity;
}

if (!TryComp<TransformComponent>(consoleEnt, out var xform)) continue;

var gridId = xform.GridUid;
// This tries to see if the grid is a shuttle and if the console should work.
if (!TryComp<MapGridComponent>(gridId, out var _) ||
!shuttleQuery.TryGetComponent(gridId, out var shuttleComponent) ||
!shuttleComponent.Enabled)
continue;

if (!newPilots.TryGetValue(gridId!.Value, out var pilots))
{
pilots = (shuttleComponent, new HashSet<(EntityUid, PilotComponent, InputMoverComponent, TransformComponent)>());
newPilots[gridId.Value] = pilots;
}

pilots.Item2.Add((uid, pilot, mover, xform));
}

// Reset inputs for non-piloted shuttles.
foreach (var (shuttleUid, (shuttle, _)) in _shuttlePilots)
{
if (newPilots.ContainsKey(shuttleUid) || CanPilot(shuttleUid))
continue;

_thruster.DisableLinearThrusters(shuttle);
}

_shuttlePilots = newPilots;

// Collate all of the linear / angular velocites for a shuttle
// then do the movement input once for it.
var xformQuery = GetEntityQuery<TransformComponent>();
foreach (var (shuttleUid, (shuttle, pilots)) in _shuttlePilots)
{
if (Paused(shuttleUid) || CanPilot(shuttleUid) || !TryComp<PhysicsComponent>(shuttleUid, out var body))
continue;

var shuttleNorthAngle = _xformSystem.GetWorldRotation(shuttleUid, xformQuery);
var forceMul = frameTime * body.InvMass;

// Collate movement linear and angular inputs together
var linearInput = Vector2.Zero;
Expand All @@ -324,7 +359,7 @@ private void HandleShuttleMovement(float frameTime)
brakeInput += brakes;
}

if (strafe.LengthSquared() > 0f) // Use LengthSquared to avoid sqrt calculation
if (strafe.Length() > 0f)
{
var offsetRotation = consoleXform.LocalRotation;
linearInput += offsetRotation.RotateVec(strafe);
Expand All @@ -336,58 +371,209 @@ private void HandleShuttleMovement(float frameTime)
}
}

// Normalize pilot inputs by the number of pilots
int pilotCount = pilots.Count;
linearInput /= pilotCount;
angularInput /= pilotCount;
brakeInput /= pilotCount;
var count = pilots.Count;
linearInput /= count;
angularInput /= count;
brakeInput /= count;

// Handle shuttle movement
if (brakeInput > 0f)
{
// Handle braking logic (expanded upon separately)
if (body.LinearVelocity.Length() > 0f)
{
// Minimum brake velocity for a direction to show its thrust appearance.
const float appearanceThreshold = 0.1f;

// Get velocity relative to the shuttle so we know which thrusters to fire
var shuttleVelocity = (-shuttleNorthAngle).RotateVec(body.LinearVelocity);
var force = Vector2.Zero;

if (shuttleVelocity.X < 0f)
{
_thruster.DisableLinearThrustDirection(shuttle, DirectionFlag.West);

if (shuttleVelocity.X < -appearanceThreshold)
_thruster.EnableLinearThrustDirection(shuttle, DirectionFlag.East);

var index = (int) Math.Log2((int) DirectionFlag.East);
force.X += shuttle.LinearThrust[index];
}
else if (shuttleVelocity.X > 0f)
{
_thruster.DisableLinearThrustDirection(shuttle, DirectionFlag.East);

if (shuttleVelocity.X > appearanceThreshold)
_thruster.EnableLinearThrustDirection(shuttle, DirectionFlag.West);

var index = (int) Math.Log2((int) DirectionFlag.West);
force.X -= shuttle.LinearThrust[index];
}

if (shuttleVelocity.Y < 0f)
{
_thruster.DisableLinearThrustDirection(shuttle, DirectionFlag.South);

if (shuttleVelocity.Y < -appearanceThreshold)
_thruster.EnableLinearThrustDirection(shuttle, DirectionFlag.North);

var index = (int) Math.Log2((int) DirectionFlag.North);
force.Y += shuttle.LinearThrust[index];
}
else if (shuttleVelocity.Y > 0f)
{
_thruster.DisableLinearThrustDirection(shuttle, DirectionFlag.North);

if (shuttleVelocity.Y > appearanceThreshold)
_thruster.EnableLinearThrustDirection(shuttle, DirectionFlag.South);

var index = (int) Math.Log2((int) DirectionFlag.South);
force.Y -= shuttle.LinearThrust[index];
}

var impulse = force * brakeInput * ShuttleComponent.BrakeCoefficient;
impulse = shuttleNorthAngle.RotateVec(impulse);
var forceMul = frameTime * body.InvMass;
var maxVelocity = (-body.LinearVelocity).Length() / forceMul;

// Don't overshoot
if (impulse.Length() > maxVelocity)
impulse = impulse.Normalized() * maxVelocity;

PhysicsSystem.ApplyForce(shuttleUid, impulse, body: body);
}
else
{
_thruster.DisableLinearThrusters(shuttle);
}

if (body.AngularVelocity != 0f)
{
var torque = shuttle.AngularThrust * brakeInput * (body.AngularVelocity > 0f ? -1f : 1f) * ShuttleComponent.BrakeCoefficient;
var torqueMul = body.InvI * frameTime;

if (body.AngularVelocity > 0f)
{
torque = MathF.Max(-body.AngularVelocity / torqueMul, torque);
}
else
{
torque = MathF.Min(-body.AngularVelocity / torqueMul, torque);
}

if (!torque.Equals(0f))
{
PhysicsSystem.ApplyTorque(shuttleUid, torque, body: body);
_thruster.SetAngularThrust(shuttle, true);
}
}
else
{
_thruster.SetAngularThrust(shuttle, false);
}
}

if (linearInput.LengthSquared() > 0f)
if (linearInput.Length().Equals(0f))
{
PhysicsSystem.SetSleepingAllowed(shuttleUid, body, true);

if (brakeInput.Equals(0f))
_thruster.DisableLinearThrusters(shuttle);
}
else
{
PhysicsSystem.SetSleepingAllowed(shuttleUid, body, false);
var angle = linearInput.ToWorldAngle();
var linearDir = angle.GetDir();
var dockFlag = linearDir.AsFlag();

var totalForce = Vector2.Zero;
foreach (var dir in directions)

// Won't just do cardinal directions.
foreach (DirectionFlag dir in Enum.GetValues(typeof(DirectionFlag)))
{
// Brain no worky but I just want cardinals
switch (dir)
{
case DirectionFlag.South:
case DirectionFlag.East:
case DirectionFlag.North:
case DirectionFlag.West:
break;
default:
continue;
}

if ((dir & dockFlag) == 0x0)
{
_thruster.DisableLinearThrustDirection(shuttle, dir);
continue;
}

int index = Array.IndexOf(directions, dir);
var force = Vector2.Zero;
var index = (int) Math.Log2((int) dir);
var thrust = shuttle.LinearThrust[index];
var force = dir switch

switch (dir)
{
DirectionFlag.North => new Vector2(0, thrust),
DirectionFlag.South => new Vector2(0, -thrust),
DirectionFlag.East => new Vector2(thrust, 0),
DirectionFlag.West => new Vector2(-thrust, 0),
_ => throw new ArgumentOutOfRangeException()
};
case DirectionFlag.North:
force.Y += thrust;
break;
case DirectionFlag.South:
force.Y -= thrust;
break;
case DirectionFlag.East:
force.X += thrust;
break;
case DirectionFlag.West:
force.X -= thrust;
break;
default:
throw new ArgumentOutOfRangeException($"Attempted to apply thrust to shuttle {shuttleUid} along invalid dir {dir}.");
}

_thruster.EnableLinearThrustDirection(shuttle, dir);
totalForce += force * linearInput.Length();
var impulse = force * linearInput.Length();
totalForce += impulse;
}

// Additional force calculation logic here
var forceMul = frameTime * body.InvMass;

var localVel = (-shuttleNorthAngle).RotateVec(body.LinearVelocity);
var maxVelocity = ObtainMaxVel(localVel, shuttle); // max for current travel dir
var maxWishVelocity = ObtainMaxVel(totalForce, shuttle);
var properAccel = (maxWishVelocity - localVel) / forceMul;

var finalForce = Vector2Dot(totalForce, properAccel.Normalized()) * properAccel.Normalized();

if (localVel.Length() >= maxVelocity.Length() && Vector2.Dot(totalForce, localVel) > 0f)
finalForce = Vector2.Zero; // burn would be faster if used as such

if (finalForce.Length() > properAccel.Length())
finalForce = properAccel; // don't overshoot

//Log.Info($"shuttle: maxVelocity {maxVelocity} totalForce {totalForce} finalForce {finalForce} forceMul {forceMul} properAccel {properAccel}");

finalForce = shuttleNorthAngle.RotateVec(finalForce);

if (finalForce.Length() > 0f)
PhysicsSystem.ApplyForce(shuttleUid, finalForce, body: body);
}

// Handle angular movement
if (!MathHelper.CloseTo(angularInput, 0f))
if (MathHelper.CloseTo(angularInput, 0f))
{
PhysicsSystem.SetSleepingAllowed(shuttleUid, body, true);

if (brakeInput <= 0f)
_thruster.SetAngularThrust(shuttle, false);
}
else
{
PhysicsSystem.SetSleepingAllowed(shuttleUid, body, false);
var torque = shuttle.AngularThrust * -angularInput;

// Need to cap the velocity if 1 tick of input brings us over cap so we don't continuously
// edge onto the cap over and over.
var torqueMul = body.InvI * frameTime;

torque = Math.Clamp(torque,
(-ShuttleComponent.MaxAngularVelocity - body.AngularVelocity) / torqueMul,
(ShuttleComponent.MaxAngularVelocity - body.AngularVelocity) / torqueMul);
Expand All @@ -398,10 +584,6 @@ private void HandleShuttleMovement(float frameTime)
_thruster.SetAngularThrust(shuttle, true);
}
}
else
{
_thruster.SetAngularThrust(shuttle, false);
}
}
}

Expand Down

0 comments on commit d2c2ab7

Please sign in to comment.