ParkingRobot/MultiWheelC/Fleet/FleetLayoutCapture.cs

using System;
using System.Collections.Generic;
using MyParking.Shared;
// 夹紧后只执行一次 → 建立固定布局
namespace MultiWheelC.Fleet
{
    // 建立编队时使用的一辆成员车世界位姿快照。
    public readonly struct FleetMemberPose
    {
        public FleetMemberPose(
            int vehicleId,
            Pose2D poseInWorld)
        {
            if (vehicleId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(vehicleId),
                    "编队成员车号必须大于零。");
            }

            NumericGuard.EnsureFinite(
                poseInWorld,
                nameof(poseInWorld));

            VehicleId = vehicleId;
            PoseInWorld = new Pose2D(
                poseInWorld.XMeters,
                poseInWorld.YMeters,
                AngleMath.NormalizeRadians(
                    poseInWorld.YawRadians));
        }

        public int VehicleId { get; }

        public Pose2D PoseInWorld { get; }
    }

    // 保存布局建立时的车队世界位姿和固定成员布局。
    public readonly struct FleetLayoutCaptureResult
    {
        public FleetLayoutCaptureResult(
            Pose2D fleetPoseInWorld,
            FleetLayout layout)
        {
            NumericGuard.EnsureFinite(
                fleetPoseInWorld,
                nameof(fleetPoseInWorld));

            FleetPoseInWorld = new Pose2D(
                fleetPoseInWorld.XMeters,
                fleetPoseInWorld.YMeters,
                AngleMath.NormalizeRadians(
                    fleetPoseInWorld.YawRadians));
            Layout = layout ??
                throw new ArgumentNullException(
                    nameof(layout));
        }

        public Pose2D FleetPoseInWorld { get; }

        public FleetLayout Layout { get; }
    }

    // 根据同一世界坐标系中的成员位姿建立车队几何中心和固定布局。
    public static class FleetLayoutCapture
    {
        public static FleetLayoutCaptureResult Capture(
            IReadOnlyList<FleetMemberPose> memberPoses,
            int leaderVehicleId)
        {
            if (memberPoses == null)
            {
                throw new ArgumentNullException(
                    nameof(memberPoses));
            }

            if (memberPoses.Count == 0)
            {
                throw new ArgumentException(
                    "建立编队布局至少需要一辆成员车。",
                    nameof(memberPoses));
            }

            if (leaderVehicleId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(leaderVehicleId),
                    "主车车号必须大于零。");
            }

            var vehicleIds = new HashSet<int>();
            var centerXMeters = 0.0;
            var centerYMeters = 0.0;
            var leaderFound = false;
            var leaderYawRadians = 0.0;

            for (var index = 0;
                 index < memberPoses.Count;
                 index++)
            {
                var memberPose = memberPoses[index];
                if (memberPose.VehicleId <= 0)
                {
                    throw new ArgumentOutOfRangeException(
                        nameof(memberPoses),
                        $"第{index}辆成员车的车号必须大于零。");
                }

                NumericGuard.EnsureFinite(
                    memberPose.PoseInWorld,
                    $"{nameof(memberPoses)}[{index}]." +
                    nameof(FleetMemberPose.PoseInWorld));

                if (!vehicleIds.Add(memberPose.VehicleId))
                {
                    throw new ArgumentException(
                        $"成员位姿包含重复车号{memberPose.VehicleId}。",
                        nameof(memberPoses));
                }

                centerXMeters +=
                    memberPose.PoseInWorld.XMeters;
                centerYMeters +=
                    memberPose.PoseInWorld.YMeters;

                if (memberPose.VehicleId == leaderVehicleId)
                {
                    leaderFound = true;
                    leaderYawRadians =
                        memberPose.PoseInWorld.YawRadians;
                }
            }

            if (!leaderFound)
            {
                throw new ArgumentException(
                    $"成员位姿中不存在主车{leaderVehicleId}。",
                    nameof(leaderVehicleId));
            }

            centerXMeters /= memberPoses.Count;
            centerYMeters /= memberPoses.Count;
            NumericGuard.EnsureFinite(
                centerXMeters,
                nameof(centerXMeters));
            NumericGuard.EnsureFinite(
                centerYMeters,
                nameof(centerYMeters));

            var fleetPoseInWorld = new Pose2D(
                centerXMeters,
                centerYMeters,
                AngleMath.NormalizeRadians(
                    leaderYawRadians));
            var worldPoseInFleet =
                FrameTransform2D.Inverse(
                    fleetPoseInWorld);
            var vehicleLayouts =
                new VehicleLayout[memberPoses.Count];

            for (var index = 0;
                 index < memberPoses.Count;
                 index++)
            {
                var memberPose = memberPoses[index];
                var poseInFleet =
                    FrameTransform2D.Compose(
                        worldPoseInFleet,
                        memberPose.PoseInWorld);
                vehicleLayouts[index] =
                    new VehicleLayout(
                        memberPose.VehicleId,
                        poseInFleet);
            }

            return new FleetLayoutCaptureResult(
                fleetPoseInWorld,
                new FleetLayout(vehicleLayouts));
        }
    }
}