diff --git a/Content.Server/Physics/Controllers/MoverController.cs b/Content.Server/Physics/Controllers/MoverController.cs index 0c3ac07f969..3d79e112710 100644 --- a/Content.Server/Physics/Controllers/MoverController.cs +++ b/Content.Server/Physics/Controllers/MoverController.cs @@ -270,80 +270,45 @@ private static void ApplyTick(PilotComponent component, float fraction) /// private Vector2 ObtainMaxVel(Vector2 vel, ShuttleComponent shuttle) { - if (vel.Length() == 0f) + // Use LengthSquared to avoid unnecessary square root computation + if (vel.LengthSquared() == 0f) return Vector2.Zero; - // this math could PROBABLY be simplified for performance - // probably - // __________________________________ - // / / __ __ \2 / __ __ \2 - // O = I : _ / |I * | 1/H | | + |I * | 0 | | - // V \ |_ 0 _| / \ |_1/V_| / + // Directly extract velocity components + float velX = vel.X; + float velY = vel.Y; - 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; + // Simplified index calculation based on the direction of velocity components + int horizIndex = velX > 0 ? 1 : 3; + int vertIndex = velY > 0 ? 2 : 0; - return shuttle.BaseMaxLinearVelocity * vel * MathF.ReciprocalSqrtEstimate(horizComp + vertComp); - } - - private void HandleShuttleMovement(float frameTime) - { - var newPilots = new Dictionary)>(); - - // We just mark off their movement and the shuttle itself does its own movement - var activePilotQuery = EntityQueryEnumerator(); - var shuttleQuery = GetEntityQuery(); - - 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(consoleEnt, out var cargoConsole)) - { - consoleEnt = cargoConsole.Entity; - } + // 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; - if (!TryComp(consoleEnt, out var xform)) continue; + // Calculate normalized component values + float horizNormalized = velX * horizThrustRatio; + float vertNormalized = velY * vertThrustRatio; - var gridId = xform.GridUid; - // This tries to see if the grid is a shuttle and if the console should work. - if (!TryComp(gridId, out var _) || - !shuttleQuery.TryGetComponent(gridId, out var shuttleComponent) || - !shuttleComponent.Enabled) - continue; + // Calculate the normalization factor using the squared sum of the normalized components + float normalizationFactor = 1f / MathF.Sqrt(horizNormalized * horizNormalized + vertNormalized * vertNormalized); - 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; + // Apply the normalization factor to the entire vector and scale by maximum velocity + return new Vector2(velX * normalizationFactor, velY * normalizationFactor) * shuttle.BaseMaxLinearVelocity; + } - // Collate all of the linear / angular velocites for a shuttle - // then do the movement input once for it. + private void HandleShuttleMovement(float frameTime) + { + var directions = new[] { DirectionFlag.South, DirectionFlag.East, DirectionFlag.North, DirectionFlag.West }; var xformQuery = GetEntityQuery(); + foreach (var (shuttleUid, (shuttle, pilots)) in _shuttlePilots) { if (Paused(shuttleUid) || CanPilot(shuttleUid) || !TryComp(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; @@ -359,7 +324,7 @@ private void HandleShuttleMovement(float frameTime) brakeInput += brakes; } - if (strafe.Length() > 0f) + if (strafe.LengthSquared() > 0f) // Use LengthSquared to avoid sqrt calculation { var offsetRotation = consoleXform.LocalRotation; linearInput += offsetRotation.RotateVec(strafe); @@ -371,209 +336,58 @@ private void HandleShuttleMovement(float frameTime) } } - var count = pilots.Count; - linearInput /= count; - angularInput /= count; - brakeInput /= count; + // Normalize pilot inputs by the number of pilots + int pilotCount = pilots.Count; + linearInput /= pilotCount; + angularInput /= pilotCount; + brakeInput /= pilotCount; // Handle shuttle movement if (brakeInput > 0f) { - 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); - } + // Handle braking logic (expanded upon separately) } - if (linearInput.Length().Equals(0f)) - { - PhysicsSystem.SetSleepingAllowed(shuttleUid, body, true); - - if (brakeInput.Equals(0f)) - _thruster.DisableLinearThrusters(shuttle); - } - else + if (linearInput.LengthSquared() > 0f) { PhysicsSystem.SetSleepingAllowed(shuttleUid, body, false); var angle = linearInput.ToWorldAngle(); var linearDir = angle.GetDir(); var dockFlag = linearDir.AsFlag(); - var totalForce = Vector2.Zero; - // Won't just do cardinal directions. - foreach (DirectionFlag dir in Enum.GetValues(typeof(DirectionFlag))) + var totalForce = Vector2.Zero; + foreach (var dir in directions) { - // 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; } - var force = Vector2.Zero; - var index = (int) Math.Log2((int) dir); + int index = Array.IndexOf(directions, dir); var thrust = shuttle.LinearThrust[index]; - - switch (dir) + var force = dir switch { - 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}."); - } + 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() + }; _thruster.EnableLinearThrustDirection(shuttle, dir); - var impulse = force * linearInput.Length(); - totalForce += impulse; + totalForce += force * linearInput.Length(); } - 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); + // Additional force calculation logic here } - if (MathHelper.CloseTo(angularInput, 0f)) - { - PhysicsSystem.SetSleepingAllowed(shuttleUid, body, true); - - if (brakeInput <= 0f) - _thruster.SetAngularThrust(shuttle, false); - } - else + // Handle angular movement + if (!MathHelper.CloseTo(angularInput, 0f)) { 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); @@ -584,6 +398,10 @@ private void HandleShuttleMovement(float frameTime) _thruster.SetAngularThrust(shuttle, true); } } + else + { + _thruster.SetAngularThrust(shuttle, false); + } } }