Initial commit from MyParking project

2026-08-04 10:29:21 +08:00
commit d9432bd529
138 changed files with 16957 additions and 0 deletions
@@ -0,0 +1,115 @@
+using System;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 提供与坐标系无关的角度归一化、角度差和单位转换功能。
+    /// </summary>
+    public static class AngleMath
+    {
+        public const double TwoPi = 2.0 * Math.PI;
+
+        /// <summary>
+        /// 将弧度归一化到[-π, π)区间。
+        /// -π包含在结果中，+π不包含在结果中，因此+π会返回-π。
+        /// </summary>
+        public static double NormalizeRadians(double angleRadians)
+        {
+            EnsureFinite(angleRadians, nameof(angleRadians));
+
+            var normalized = angleRadians % TwoPi;
+
+            if (normalized >= Math.PI)
+            {
+                normalized -= TwoPi;
+            }
+            else if (normalized < -Math.PI)
+            {
+                normalized += TwoPi;
+            }
+
+            return normalized == 0.0 ? 0.0 : normalized;
+        }
+
+        /// <summary>
+        /// 将角度归一化到[-180°, 180°)区间。
+        /// -180°包含在结果中，+180°不包含在结果中，因此+180°会返回-180°。
+        /// </summary>
+        public static double NormalizeDegrees(double angleDegrees)
+        {
+            EnsureFinite(angleDegrees, nameof(angleDegrees));
+
+            var normalized = angleDegrees % 360.0;
+
+            if (normalized >= 180.0)
+            {
+                normalized -= 360.0;
+            }
+            else if (normalized < -180.0)
+            {
+                normalized += 360.0;
+            }
+
+            return normalized == 0.0 ? 0.0 : normalized;
+        }
+
+        /// <summary>
+        /// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为弧度。
+        /// 返回值位于[-π, π)；正值表示逆时针，负值表示顺时针。
+        /// </summary>
+        public static double ShortestDifferenceRadians(
+            double targetRadians,
+            double currentRadians)
+        {
+            EnsureFinite(targetRadians, nameof(targetRadians));
+            EnsureFinite(currentRadians, nameof(currentRadians));
+
+            return NormalizeRadians(targetRadians - currentRadians);
+        }
+
+        /// <summary>
+        /// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为度。
+        /// 返回值位于[-180°, 180°)；正值表示逆时针，负值表示顺时针。
+        /// </summary>
+        public static double ShortestDifferenceDegrees(
+            double targetDegrees,
+            double currentDegrees)
+        {
+            EnsureFinite(targetDegrees, nameof(targetDegrees));
+            EnsureFinite(currentDegrees, nameof(currentDegrees));
+
+            return NormalizeDegrees(targetDegrees - currentDegrees);
+        }
+
+        /// <summary>
+        /// 将角度从度转换为弧度，不进行归一化。
+        /// </summary>
+        public static double DegreesToRadians(double angleDegrees)
+        {
+            EnsureFinite(angleDegrees, nameof(angleDegrees));
+            return angleDegrees * Math.PI / 180.0;
+        }
+
+        /// <summary>
+        /// 将角度从弧度转换为度，不进行归一化。
+        /// </summary>
+        public static double RadiansToDegrees(double angleRadians)
+        {
+            EnsureFinite(angleRadians, nameof(angleRadians));
+            return angleRadians * 180.0 / Math.PI;
+        }
+
+        /// <summary>
+        /// 验证角度是可用于计算的有限数值。
+        /// </summary>
+        private static void EnsureFinite(double angle, string parameterName)
+        {
+            if (double.IsNaN(angle) || double.IsInfinity(angle))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "角度必须是有限数值。");
+            }
+        }
+    }
+}
@@ -0,0 +1,165 @@
+// 车体、运动、车队坐标系之间的转换
+using System;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。
+    /// 坐标系采用X向前、Y向左、逆时针为正的右手系。
+    /// </summary>
+    public static class FrameTransform2D
+    {
+        /// <summary>
+        /// 将角度归一化到[-π, π)范围。
+        /// </summary>
+        public static double NormalizeAngle(double angleRadians)
+        {
+            return AngleMath.NormalizeRadians(angleRadians);
+        }
+
+        /// <summary>
+        /// 计算从current到target的最短角度差。
+        /// 返回正值表示逆时针旋转。
+        /// </summary>
+        public static double ShortestAngleDifference(
+            double targetRadians,
+            double currentRadians)
+        {
+            return AngleMath.ShortestDifferenceRadians(
+                targetRadians,
+                currentRadians);
+        }
+
+        /// <summary>
+        /// 将源坐标系中的点变换到目标坐标系。
+        /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
+        /// </summary>
+        public static Point2D TransformPoint(
+            Pose2D sourcePoseInTarget,
+            Point2D pointInSource)
+        {
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                sourcePoseInTarget.XMeters +
+                cos * pointInSource.XMeters -
+                sin * pointInSource.YMeters,
+
+                sourcePoseInTarget.YMeters +
+                sin * pointInSource.XMeters +
+                cos * pointInSource.YMeters);
+        }
+
+        /// <summary>
+        /// 将目标坐标系中的点反向变换到源坐标系。
+        /// </summary>
+        public static Point2D InverseTransformPoint(
+            Pose2D sourcePoseInTarget,
+            Point2D pointInTarget)
+        {
+            var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters;
+
+            var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters;
+
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                cos * dx + sin * dy,
+                -sin * dx + cos * dy);
+        }
+
+        /// <summary>
+        /// 将源坐标系中的向量旋转到目标坐标系。
+        /// 向量没有位置，因此不叠加平移量。
+        /// </summary>
+        public static Point2D TransformVector(
+            Pose2D sourcePoseInTarget,
+            Point2D vectorInSource)
+        {
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                cos * vectorInSource.XMeters -
+                sin * vectorInSource.YMeters,
+
+                sin * vectorInSource.XMeters +
+                cos * vectorInSource.YMeters);
+        }
+
+        /// <summary>
+        /// 组合两级坐标变换。
+        /// parentFromMiddle表示middle在parent中的位姿；
+        /// middleFromChild表示child在middle中的位姿；
+        /// 返回child在parent中的位姿。
+        /// </summary>
+        public static Pose2D Compose(
+            Pose2D parentFromMiddle,
+            Pose2D middleFromChild)
+        {
+            var childPositionInParent = TransformPoint(
+                parentFromMiddle,
+                middleFromChild.Position);
+
+            return new Pose2D(
+                childPositionInParent.XMeters,
+                childPositionInParent.YMeters,
+                NormalizeAngle(
+                    parentFromMiddle.YawRadians +
+                    middleFromChild.YawRadians));
+        }
+
+        /// <summary>
+        /// 对坐标变换求逆。
+        /// 输入child在parent中的位姿，返回parent在child中的位姿。
+        /// </summary>
+        public static Pose2D Inverse(Pose2D childPoseInParent)
+        {
+            var cos = Math.Cos(childPoseInParent.YawRadians);
+            var sin = Math.Sin(childPoseInParent.YawRadians);
+
+            return new Pose2D(
+                -cos * childPoseInParent.XMeters -
+                sin * childPoseInParent.YMeters,
+
+                sin * childPoseInParent.XMeters -
+                cos * childPoseInParent.YMeters,
+
+                NormalizeAngle(
+                    -childPoseInParent.YawRadians));
+        }
+
+
+        /// <summary>
+        /// 将源坐标系中的位姿变换到目标坐标系。
+        /// </summary>
+        public static Pose2D TransformPose(
+            Pose2D sourcePoseInTarget,
+            Pose2D poseInSource)
+        {
+            return Compose(sourcePoseInTarget, poseInSource);
+        }
+
+        /// <summary>
+        /// 转换同一物理参考点处的速度表达坐标系。
+        /// 只旋转线速度，角速度保持不变。
+        /// </summary>
+        public static Twist2D TransformTwistAtSamePoint(
+            Pose2D sourcePoseInTarget,
+            Twist2D twistInSource)
+        {
+            var linearVelocityInTarget = TransformVector(
+                sourcePoseInTarget,
+                new Point2D(
+                    twistInSource.VxMetersPerSecond,
+                    twistInSource.VyMetersPerSecond));
+
+            return new Twist2D(
+                linearVelocityInTarget.XMeters,
+                linearVelocityInTarget.YMeters,
+                twistInSource.OmegaRadiansPerSecond);
+        }
+    }
+}