ParkingRobot/Simulation/Core/VirtualSteerWheel.cs

namespace MyParking.Simulation.Core;

/// <summary>
/// 模拟单个舵轮的转向和驱动响应，不包含真实电机物理模型。
/// </summary>
public sealed class VirtualSteerWheel
{
    private const double AngleLowerLimitDegrees = -120.0;
    private const double AngleUpperLimitDegrees = 120.0;
    private const double MaximumSteeringRateDegreesPerSecond = 90.0;
    private const double MaximumDriveAccelerationMetersPerSecondSquared = 0.8;
    private const double AlignmentToleranceDegrees = 1.5;

    public VirtualSteerWheel(
        string name,
        double xMeters,
        double yMeters)
    {
        Name = name;
        XMeters = xMeters;
        YMeters = yMeters;
    }

    public string Name { get; }

    public double XMeters { get; }

    public double YMeters { get; }

    public double TargetAngleDegrees { get; private set; }

    public double ActualAngleDegrees { get; private set; }

    public double TargetSpeedMetersPerSecond { get; private set; }

    public double ActualSpeedMetersPerSecond { get; private set; }

    public bool IsAligned =>
        Math.Abs(TargetAngleDegrees - ActualAngleDegrees) <=
        AlignmentToleranceDegrees;

    /// <summary>
    /// 设置期望轮胎速度向量，并在机械限位内选择等价舵角。
    /// </summary>
    public bool SetVelocityVector(
        double vxMetersPerSecond,
        double vyMetersPerSecond)
    {
        var speed = Math.Sqrt(
            vxMetersPerSecond * vxMetersPerSecond +
            vyMetersPerSecond * vyMetersPerSecond);

        if (speed < 1e-6)
        {
            TargetSpeedMetersPerSecond = 0.0;
            return true;
        }

        var desiredAngleDegrees =
            Math.Atan2(vyMetersPerSecond, vxMetersPerSecond) *
            180.0 / Math.PI;

        return SetDirectionAndSpeed(
            desiredAngleDegrees,
            speed);
    }

    /// <summary>
    /// 停车时设置舵轮预对齐方向。
    /// </summary>
    public bool PrepareDirection(double targetAngleDegrees)
    {
        TargetSpeedMetersPerSecond = 0.0;
        return SetDirectionAndSpeed(
            targetAngleDegrees,
            0.0);
    }

    /// <summary>
    /// 将驱动目标设置为零，并保持当前舵轮方向。
    /// </summary>
    public void Stop()
    {
        TargetSpeedMetersPerSecond = 0.0;
    }

    /// <summary>
    /// 按固定转向速度和驱动加速度更新虚拟反馈。
    /// </summary>
    public void Step(double deltaTimeSeconds)
    {
        ActualAngleDegrees = MoveTowards(
            ActualAngleDegrees,
            TargetAngleDegrees,
            MaximumSteeringRateDegreesPerSecond *
            deltaTimeSeconds);

        var allowedTargetSpeed =
            IsAligned ? TargetSpeedMetersPerSecond : 0.0;

        ActualSpeedMetersPerSecond = MoveTowards(
            ActualSpeedMetersPerSecond,
            allowedTargetSpeed,
            MaximumDriveAccelerationMetersPerSecondSquared *
            deltaTimeSeconds);
    }

    /// <summary>
    /// 恢复舵轮初始状态。
    /// </summary>
    public void Reset()
    {
        TargetAngleDegrees = 0.0;
        ActualAngleDegrees = 0.0;
        TargetSpeedMetersPerSecond = 0.0;
        ActualSpeedMetersPerSecond = 0.0;
    }

    private bool SetDirectionAndSpeed(
        double desiredAngleDegrees,
        double desiredSpeedMetersPerSecond)
    {
        var candidates = new[]
        {
            (Angle: NormalizeDegrees(desiredAngleDegrees),
                Speed: desiredSpeedMetersPerSecond),
            (Angle: NormalizeDegrees(desiredAngleDegrees + 180.0),
                Speed: -desiredSpeedMetersPerSecond),
            (Angle: NormalizeDegrees(desiredAngleDegrees - 180.0),
                Speed: -desiredSpeedMetersPerSecond)
        };

        var validCandidates = candidates
            .Where(candidate =>
                candidate.Angle >= AngleLowerLimitDegrees &&
                candidate.Angle <= AngleUpperLimitDegrees)
            .OrderBy(candidate =>
                Math.Abs(candidate.Angle - ActualAngleDegrees))
            .ToArray();

        if (validCandidates.Length == 0)
        {
            TargetSpeedMetersPerSecond = 0.0;
            return false;
        }

        var selected = validCandidates[0];
        TargetAngleDegrees = selected.Angle;
        TargetSpeedMetersPerSecond = selected.Speed;
        return true;
    }

    private static double MoveTowards(
        double current,
        double target,
        double maximumChange)
    {
        var difference = target - current;
        if (Math.Abs(difference) <= maximumChange)
            return target;

        return current + Math.Sign(difference) * maximumChange;
    }

    private static double NormalizeDegrees(double angleDegrees)
    {
        angleDegrees %= 360.0;
        if (angleDegrees >= 180.0)
            angleDegrees -= 360.0;
        if (angleDegrees < -180.0)
            angleDegrees += 360.0;
        return angleDegrees;
    }
}