增加车队布局与刚体速度分配基础

Shared/Fleet/FleetKinematics.cs

@@ -1 +1,76 @@
-// 把车队整体速度分解为每辆车的局部速度
+using System;
+using System.Collections.Generic;
+
+namespace MyParking.Shared
+{
+    // 纯数学地把车队参考点速度分解成各成员车体系中的刚体速度。
+    public static class FleetKinematics
+    {
+        public static IReadOnlyList<FleetMemberCommand> Decompose(
+            FleetLayout layout,
+            FleetMotionCommand command)
+        {
+            if (layout == null)
+            {
+                throw new ArgumentNullException(nameof(layout));
+            }
+
+            EnsureCommandIsFinite(command);
+
+            var commands =
+                new FleetMemberCommand[layout.VehicleCount];
+            var referencePoint = command.ReferencePointInFleet;
+            var referenceTwist = command.TwistAtReferencePoint;
+
+            for (var index = 0;
+                 index < layout.VehicleCount;
+                 index++)
+            {
+                var vehicle = layout.Vehicles[index];
+                var offsetXMeters =
+                    vehicle.PoseInFleet.XMeters -
+                    referencePoint.XMeters;
+                var offsetYMeters =
+                    vehicle.PoseInFleet.YMeters -
+                    referencePoint.YMeters;
+
+                var twistAtVehicleInFleet = new Twist2D(
+                    referenceTwist.VxMetersPerSecond -
+                    referenceTwist.OmegaRadiansPerSecond *
+                    offsetYMeters,
+                    referenceTwist.VyMetersPerSecond +
+                    referenceTwist.OmegaRadiansPerSecond *
+                    offsetXMeters,
+                    referenceTwist.OmegaRadiansPerSecond);
+
+                var fleetPoseInVehicle =
+                    FrameTransform2D.Inverse(
+                        vehicle.PoseInFleet);
+                var twistInVehicleBody =
+                    FrameTransform2D.TransformTwistAtSamePoint(
+                        fleetPoseInVehicle,
+                        twistAtVehicleInFleet);
+
+                commands[index] = new FleetMemberCommand(
+                    vehicle.VehicleId,
+                    twistInVehicleBody);
+            }
+
+            return Array.AsReadOnly(commands);
+        }
+
+        private static void EnsureCommandIsFinite(
+            FleetMotionCommand command)
+        {
+            NumericGuard.EnsureFinite(
+                command.ReferencePointInFleet.XMeters,
+                nameof(command.ReferencePointInFleet));
+            NumericGuard.EnsureFinite(
+                command.ReferencePointInFleet.YMeters,
+                nameof(command.ReferencePointInFleet));
+            NumericGuard.EnsureFinite(
+                command.TwistAtReferencePoint,
+                nameof(command.TwistAtReferencePoint));
+        }
+    }
+}

Shared/Fleet/FleetLayout.cs

@@ -0,0 +1,76 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MyParking.Shared
+{
+    // 编队固定布局快照，车队坐标系原点就是编队参考中心。
+    public sealed class FleetLayout
+    {
+        private readonly ReadOnlyCollection<VehicleLayout> _vehicles;
+
+        public FleetLayout(IReadOnlyList<VehicleLayout> vehicles)
+        {
+            if (vehicles == null)
+            {
+                throw new ArgumentNullException(nameof(vehicles));
+            }
+
+            if (vehicles.Count == 0)
+            {
+                throw new ArgumentException(
+                    "编队布局至少需要包含一辆车。",
+                    nameof(vehicles));
+            }
+
+            var snapshot = new VehicleLayout[vehicles.Count];
+            var vehicleIds = new HashSet<int>();
+
+            for (var index = 0; index < vehicles.Count; index++)
+            {
+                var vehicle = vehicles[index];
+                if (vehicle.VehicleId <= 0)
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(vehicles),
+                        $"第{index}辆车的编号必须大于零。");
+                }
+
+                if (!vehicleIds.Add(vehicle.VehicleId))
+                {
+                    throw new ArgumentException(
+                        $"编队布局包含重复车号{vehicle.VehicleId}。",
+                        nameof(vehicles));
+                }
+
+                NumericGuard.EnsureFinite(
+                    vehicle.PoseInFleet,
+                    $"{nameof(vehicles)}[{index}].{nameof(VehicleLayout.PoseInFleet)}");
+                snapshot[index] = vehicle;
+            }
+
+            _vehicles = Array.AsReadOnly(snapshot);
+        }
+
+        public IReadOnlyList<VehicleLayout> Vehicles => _vehicles;
+
+        public int VehicleCount => _vehicles.Count;
+
+        public bool TryGetVehicle(
+            int vehicleId,
+            out VehicleLayout vehicle)
+        {
+            for (var index = 0; index < _vehicles.Count; index++)
+            {
+                if (_vehicles[index].VehicleId == vehicleId)
+                {
+                    vehicle = _vehicles[index];
+                    return true;
+                }
+            }
+
+            vehicle = default;
+            return false;
+        }
+    }
+}

Shared/Fleet/FleetModels.cs

@@ -0,0 +1,67 @@
+namespace MyParking.Shared
+{
+    // 单车车体系在车队坐标系中的固定位姿。
+    public readonly struct VehicleLayout
+    {
+        public VehicleLayout(int vehicleId, Pose2D poseInFleet)
+        {
+            VehicleId = vehicleId;
+            PoseInFleet = poseInFleet;
+        }
+
+        public int VehicleId { get; }
+
+        public Pose2D PoseInFleet { get; }
+    }
+
+    // 车队参考点及该点在车队坐标系中表达的刚体速度。
+    public readonly struct FleetMotionCommand
+    {
+        public FleetMotionCommand(
+            Point2D referencePointInFleet,
+            Twist2D twistAtReferencePoint)
+        {
+            ReferencePointInFleet = referencePointInFleet;
+            TwistAtReferencePoint = twistAtReferencePoint;
+        }
+
+        public Point2D ReferencePointInFleet { get; }
+
+        public Twist2D TwistAtReferencePoint { get; }
+
+        public static FleetMotionCommand RotateAround(
+            Point2D rotationCenterInFleet,
+            double omegaRadiansPerSecond)
+        {
+            return new FleetMotionCommand(
+                rotationCenterInFleet,
+                new Twist2D(
+                    0.0,
+                    0.0,
+                    omegaRadiansPerSecond));
+        }
+
+        public static FleetMotionCommand Stop()
+        {
+            return new FleetMotionCommand(
+                Point2D.Zero,
+                Twist2D.Zero);
+        }
+    }
+
+    // 分配给指定车辆、在该车车体系中表达的刚体速度。
+    public readonly struct FleetMemberCommand
+    {
+        public FleetMemberCommand(
+            int vehicleId,
+            Twist2D twistInVehicleBody)
+        {
+            VehicleId = vehicleId;
+            TwistInVehicleBody = twistInVehicleBody;
+        }
+
+        public int VehicleId { get; }
+
+        public Twist2D TwistInVehicleBody { get; }
+    }
+}

Shared/Fleet/FleetProtocol.cs

@@ -0,0 +1 @@
+// 只定义无线报文的数据结构、版本和字段