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

MultiWheelC.Tests/FleetKinematicsTests.cs

@@ -0,0 +1,139 @@
+using System;
+using System.Collections.Generic;
+using MyParking.Shared;
+
+namespace MultiWheelC.Tests
+{
+    internal static class FleetKinematicsTests
+    {
+        private const double Tolerance = 1e-9;
+
+        public static void Run()
+        {
+            var layout = CreateTailToTailLayout();
+
+            VerifyTranslation(layout);
+            VerifyRotationAroundFleetCenter(layout);
+            VerifyRotationAroundFirstVehicle(layout);
+            VerifyStop(layout);
+
+            Console.WriteLine(
+                "FleetKinematics刚体速度分配测试通过。共4个场景。");
+        }
+
+        private static FleetLayout CreateTailToTailLayout()
+        {
+            return new FleetLayout(
+                new[]
+                {
+                    new VehicleLayout(
+                        1,
+                        new Pose2D(1.0, 0.0, 0.0)),
+                    new VehicleLayout(
+                        2,
+                        new Pose2D(-1.0, 0.0, Math.PI))
+                });
+        }
+
+        private static void VerifyTranslation(FleetLayout layout)
+        {
+            var commands = FleetKinematics.Decompose(
+                layout,
+                new FleetMotionCommand(
+                    Point2D.Zero,
+                    new Twist2D(0.4, 0.0, 0.0)));
+
+            AssertTwist(Find(commands, 1), 0.4, 0.0, 0.0);
+            AssertTwist(Find(commands, 2), -0.4, 0.0, 0.0);
+        }
+
+        private static void VerifyRotationAroundFleetCenter(
+            FleetLayout layout)
+        {
+            var commands = FleetKinematics.Decompose(
+                layout,
+                FleetMotionCommand.RotateAround(
+                    Point2D.Zero,
+                    0.2));
+
+            AssertTwist(Find(commands, 1), 0.0, 0.2, 0.2);
+            AssertTwist(Find(commands, 2), 0.0, 0.2, 0.2);
+        }
+
+        private static void VerifyRotationAroundFirstVehicle(
+            FleetLayout layout)
+        {
+            var commands = FleetKinematics.Decompose(
+                layout,
+                FleetMotionCommand.RotateAround(
+                    new Point2D(1.0, 0.0),
+                    0.2));
+
+            AssertTwist(Find(commands, 1), 0.0, 0.0, 0.2);
+            AssertTwist(Find(commands, 2), 0.0, 0.4, 0.2);
+        }
+
+        private static void VerifyStop(FleetLayout layout)
+        {
+            var commands = FleetKinematics.Decompose(
+                layout,
+                FleetMotionCommand.Stop());
+
+            AssertTwist(Find(commands, 1), 0.0, 0.0, 0.0);
+            AssertTwist(Find(commands, 2), 0.0, 0.0, 0.0);
+        }
+
+        private static FleetMemberCommand Find(
+            IReadOnlyList<FleetMemberCommand> commands,
+            int vehicleId)
+        {
+            for (var index = 0; index < commands.Count; index++)
+            {
+                if (commands[index].VehicleId == vehicleId)
+                {
+                    return commands[index];
+                }
+            }
+
+            throw new InvalidOperationException(
+                $"没有找到车辆{vehicleId}的分配命令。");
+        }
+
+        private static void AssertTwist(
+            FleetMemberCommand command,
+            double expectedVx,
+            double expectedVy,
+            double expectedOmega)
+        {
+            AssertNear(
+                command.TwistInVehicleBody.VxMetersPerSecond,
+                expectedVx,
+                command.VehicleId,
+                "Vx");
+            AssertNear(
+                command.TwistInVehicleBody.VyMetersPerSecond,
+                expectedVy,
+                command.VehicleId,
+                "Vy");
+            AssertNear(
+                command.TwistInVehicleBody.OmegaRadiansPerSecond,
+                expectedOmega,
+                command.VehicleId,
+                "Omega");
+        }
+
+        private static void AssertNear(
+            double actual,
+            double expected,
+            int vehicleId,
+            string valueName)
+        {
+            if (Math.Abs(actual - expected) > Tolerance)
+            {
+                throw new InvalidOperationException(
+                    $"车辆{vehicleId}的{valueName}错误：" +
+                    $"actual={actual:F9}, expected={expected:F9}。");
+            }
+        }
+    }
+}

MultiWheelC.Tests/Program.cs

@@ -36,6 +36,7 @@ namespace MultiWheelC.Tests
 
             Console.WriteLine(
                 "Stanley前进/倒车横向符号测试通过。共8个场景。");
+            FleetKinematicsTests.Run();
         }
 
         /// <summary>