chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -0,0 +1,33 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>旋转矩形与轴对齐栅格的分离轴相交判定。</summary>
+internal static class OrientedRectangleCellIntersection
+{
+    /// <summary>任一投影轴没有严格分离时返回 true；擦边按相交处理。</summary>
+    public static bool Intersects(VehicleFootprint rectangle, double cellMinX, double cellMaxX, double cellMinY, double cellMaxY)
+    {
+        if (rectangle == null || cellMaxX < cellMinX || cellMaxY < cellMinY) return false;
+        double cellCenterX = (cellMinX + cellMaxX) / 2d;
+        double cellCenterY = (cellMinY + cellMaxY) / 2d;
+        double cellHalfX = (cellMaxX - cellMinX) / 2d;
+        double cellHalfY = (cellMaxY - cellMinY) / 2d;
+
+        return !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLongitudinalX, rectangle.AxisLongitudinalY) &&
+            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLateralX, rectangle.AxisLateralY) &&
+            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 1d, 0d) &&
+            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 0d, 1d);
+    }
+
+    private static bool HasStrictSeparation(VehicleFootprint rectangle, double cellCenterX, double cellCenterY, double cellHalfX, double cellHalfY,
+        double axisX, double axisY)
+    {
+        double rectangleCenter = rectangle.CenterX * axisX + rectangle.CenterY * axisY;
+        double cellCenter = cellCenterX * axisX + cellCenterY * axisY;
+        double rectangleRadius = rectangle.HalfLengthMeters * Math.Abs(rectangle.AxisLongitudinalX * axisX + rectangle.AxisLongitudinalY * axisY) +
+            rectangle.HalfWidthMeters * Math.Abs(rectangle.AxisLateralX * axisX + rectangle.AxisLateralY * axisY);
+        double cellRadius = cellHalfX * Math.Abs(axisX) + cellHalfY * Math.Abs(axisY);
+        return Math.Abs(rectangleCenter - cellCenter) > rectangleRadius + cellRadius;
+    }
+}