添加单车底盘仿真平台并完善运动控制与夹臂功能

Simulation/Core/SimulationWorld.cs

@@ -0,0 +1,205 @@
+using MyParking.Shared;
+using MyParking.Simulation.Models;
+
+namespace MyParking.Simulation.Core;
+
+/// <summary>
+/// 管理离线仿真车辆、车队中心和成员布局。
+/// </summary>
+public sealed class SimulationWorld
+{
+    private readonly object _syncRoot = new();
+    private Dictionary<int, SimulationVehicle> _vehicles = new();
+    private SimulationConfigurationDto _configuration;
+
+    public SimulationWorld()
+    {
+        _configuration = CreateDefaultConfiguration();
+        ApplyConfigurationCore(_configuration);
+    }
+
+    public T WithVehicle<T>(
+        int vehicleId,
+        Func<SimulationVehicle, T> action)
+    {
+        lock (_syncRoot)
+        {
+            if (!_vehicles.TryGetValue(
+                    vehicleId,
+                    out var vehicle))
+            {
+                throw new KeyNotFoundException(
+                    $"不存在车辆{vehicleId}。");
+            }
+
+            return action(vehicle);
+        }
+    }
+
+    public void Step(double deltaTimeSeconds)
+    {
+        lock (_syncRoot)
+        {
+            foreach (var vehicle in _vehicles.Values)
+                vehicle.Step(deltaTimeSeconds);
+        }
+    }
+
+    public IReadOnlyList<VehicleStateDto> GetSnapshot()
+    {
+        lock (_syncRoot)
+        {
+            return _vehicles.Values
+                .OrderBy(vehicle => vehicle.VehicleId)
+                .Select(vehicle => vehicle.GetSnapshot())
+                .ToArray();
+        }
+    }
+
+    public SimulationConfigurationDto GetConfiguration()
+    {
+        lock (_syncRoot)
+        {
+            return CloneConfiguration(_configuration);
+        }
+    }
+
+    public void ApplyConfiguration(
+        SimulationConfigurationDto configuration)
+    {
+        ValidateConfiguration(configuration);
+
+        lock (_syncRoot)
+        {
+            _configuration = CloneConfiguration(configuration);
+            ApplyConfigurationCore(_configuration);
+        }
+    }
+
+    public void Reset()
+    {
+        lock (_syncRoot)
+        {
+            ApplyConfigurationCore(_configuration);
+        }
+    }
+
+    private void ApplyConfigurationCore(
+        SimulationConfigurationDto configuration)
+    {
+        var fleetPoseInWorld = new Pose2D(
+            configuration.FleetCenter.XMeters,
+            configuration.FleetCenter.YMeters,
+            configuration.FleetCenter.YawRadians);
+
+        _vehicles = configuration.Vehicles
+            .Take(configuration.VehicleCount)
+            .Select(layout =>
+            {
+                var bodyPoseInFleet = new Pose2D(
+                    layout.XMeters,
+                    layout.YMeters,
+                    layout.YawRadians);
+
+                var bodyPoseInWorld =
+                    FrameTransform2D.Compose(
+                        fleetPoseInWorld,
+                        bodyPoseInFleet);
+
+                return new SimulationVehicle(
+                    layout.VehicleId,
+                    bodyPoseInWorld.XMeters,
+                    bodyPoseInWorld.YMeters,
+                    bodyPoseInWorld.YawRadians);
+            })
+            .ToDictionary(vehicle => vehicle.VehicleId);
+    }
+
+    private static void ValidateConfiguration(
+        SimulationConfigurationDto configuration)
+    {
+        if (configuration.VehicleCount is < 1 or > 8)
+        {
+            throw new ArgumentOutOfRangeException(
+                nameof(configuration.VehicleCount),
+                "仿真车辆数量必须在1到8之间。");
+        }
+
+        if (configuration.FleetCenter == null)
+        {
+            throw new ArgumentException(
+                "必须提供车队中心位姿。",
+                nameof(configuration));
+        }
+
+        if (configuration.Vehicles == null ||
+            configuration.Vehicles.Count <
+            configuration.VehicleCount)
+        {
+            throw new ArgumentException(
+                "成员布局数量不能少于车辆数量。",
+                nameof(configuration));
+        }
+
+        var selectedLayouts = configuration.Vehicles
+            .Take(configuration.VehicleCount)
+            .ToArray();
+
+        if (selectedLayouts.Any(layout =>
+                layout.VehicleId <= 0) ||
+            selectedLayouts
+                .Select(layout => layout.VehicleId)
+                .Distinct()
+                .Count() != selectedLayouts.Length)
+        {
+            throw new ArgumentException(
+                "车辆编号必须大于零且不能重复。",
+                nameof(configuration));
+        }
+
+        var values = new[]
+        {
+            configuration.FleetCenter.XMeters,
+            configuration.FleetCenter.YMeters,
+            configuration.FleetCenter.YawRadians
+        }.Concat(selectedLayouts.SelectMany(layout => new[]
+        {
+            layout.XMeters,
+            layout.YMeters,
+            layout.YawRadians
+        }));
+
+        if (values.Any(value =>
+                double.IsNaN(value) ||
+                double.IsInfinity(value)))
+        {
+            throw new ArgumentException(
+                "车队和车辆布局不能包含NaN或无穷大。",
+                nameof(configuration));
+        }
+    }
+
+    private static SimulationConfigurationDto
+        CreateDefaultConfiguration()
+    {
+        return new SimulationConfigurationDto(
+            1,
+            new FleetCenterDto(0.0, 0.0, 0.0),
+            new[]
+            {
+                new VehicleLayoutDto(1, 0.0, 0.0, 0.0)
+            });
+    }
+
+    private static SimulationConfigurationDto
+        CloneConfiguration(
+            SimulationConfigurationDto configuration)
+    {
+        return new SimulationConfigurationDto(
+            configuration.VehicleCount,
+            configuration.FleetCenter with { },
+            configuration.Vehicles
+                .Select(layout => layout with { })
+                .ToArray());
+    }
+}