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

2026-08-20 17:43:02 +08:00
parent 0d5539e595
commit fea2265e2d
30 changed files with 404 additions and 121 deletions
@@ -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));
+        }
+    }
+}