diff --git a/Content.Server/Physics/Controllers/MoverController.cs b/Content.Server/Physics/Controllers/MoverController.cs index 3d79e112710..0c3ac07f969 100644 --- a/Content.Server/Physics/Controllers/MoverController.cs +++ b/Content.Server/Physics/Controllers/MoverController.cs @@ -270,45 +270,80 @@ private static void ApplyTick(PilotComponent component, float fraction) /// 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(); + 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; + } + + if (!TryComp(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(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(); 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; @@ -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); @@ -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); @@ -398,10 +584,6 @@ private void HandleShuttleMovement(float frameTime) _thruster.SetAngularThrust(shuttle, true); } } - else - { - _thruster.SetAngularThrust(shuttle, false); - } } }