Refactor ElkSharp hybrid routing and document speed path

src/__Libraries/StellaOps.ElkSharp/ElkEdgeRouterIterative.StrategyRepair.RepairPlan.Expanders.cs

@@ -0,0 +1,215 @@
+using System.Collections.Concurrent;
+using System.Diagnostics;
+using System.Globalization;
+
+namespace StellaOps.ElkSharp;
+
+internal static partial class ElkEdgeRouterIterative
+{
+    private static string[] ExpandRepeatCollectorRepairSet(
+        IReadOnlyCollection<string> selectedEdgeIds,
+        IReadOnlyCollection<ElkRoutedEdge> edges,
+        IReadOnlyCollection<ElkPositionedNode> nodes)
+    {
+        var selected = selectedEdgeIds.ToHashSet(StringComparer.Ordinal);
+        foreach (var group in ElkRepeatCollectorCorridors.DetectSharedLaneGroups(edges, nodes))
+        {
+            if (!group.EdgeIds.Any(selected.Contains))
+            {
+                continue;
+            }
+
+            foreach (var edgeId in group.EdgeIds)
+            {
+                selected.Add(edgeId);
+            }
+        }
+
+        return selected
+            .OrderBy(edgeId => edgeId, StringComparer.Ordinal)
+            .ToArray();
+    }
+
+    private static string[] ExpandTargetApproachJoinRepairSet(
+        IReadOnlyCollection<string> selectedEdgeIds,
+        IReadOnlyCollection<ElkRoutedEdge> edges,
+        IReadOnlyCollection<ElkPositionedNode> nodes,
+        double minLineClearance)
+    {
+        var selected = selectedEdgeIds.ToHashSet(StringComparer.Ordinal);
+        var nodesById = nodes.ToDictionary(node => node.Id, StringComparer.Ordinal);
+        var edgeArray = edges.ToArray();
+
+        foreach (var group in edgeArray.GroupBy(edge => edge.TargetNodeId ?? string.Empty, StringComparer.Ordinal))
+        {
+            if (string.IsNullOrWhiteSpace(group.Key)
+                || !nodesById.TryGetValue(group.Key, out var targetNode))
+            {
+                continue;
+            }
+
+            var targetEdges = group.ToArray();
+            for (var i = 0; i < targetEdges.Length; i++)
+            {
+                var leftEdge = targetEdges[i];
+                var leftPath = ExtractPath(leftEdge);
+                if (leftPath.Count < 2)
+                {
+                    continue;
+                }
+
+                var leftSide = ElkEdgeRoutingGeometry.ResolveBoundaryApproachSide(leftPath[^1], leftPath[^2], targetNode);
+                for (var j = i + 1; j < targetEdges.Length; j++)
+                {
+                    var rightEdge = targetEdges[j];
+                    var rightPath = ExtractPath(rightEdge);
+                    if (rightPath.Count < 2)
+                    {
+                        continue;
+                    }
+
+                    var rightSide = ElkEdgeRoutingGeometry.ResolveBoundaryApproachSide(rightPath[^1], rightPath[^2], targetNode);
+                    if (!string.Equals(leftSide, rightSide, StringComparison.Ordinal))
+                    {
+                        continue;
+                    }
+
+                    if (!HasTargetApproachJoinPair(leftPath, rightPath, minLineClearance))
+                    {
+                        continue;
+                    }
+
+                    selected.Add(leftEdge.Id);
+                    selected.Add(rightEdge.Id);
+                }
+            }
+        }
+
+        return selected
+            .OrderBy(edgeId => edgeId, StringComparer.Ordinal)
+            .ToArray();
+    }
+
+    private static string[] ExpandSharedLaneRepairSet(
+        IReadOnlyCollection<string> selectedEdgeIds,
+        IReadOnlyCollection<ElkRoutedEdge> edges,
+        IReadOnlyCollection<ElkPositionedNode> nodes)
+    {
+        var selected = selectedEdgeIds.ToHashSet(StringComparer.Ordinal);
+        foreach (var (leftEdgeId, rightEdgeId) in ElkEdgeRoutingScoring.DetectSharedLaneConflicts(edges, nodes))
+        {
+            if (!selected.Contains(leftEdgeId) && !selected.Contains(rightEdgeId))
+            {
+                continue;
+            }
+
+            selected.Add(leftEdgeId);
+            selected.Add(rightEdgeId);
+        }
+
+        return selected
+            .OrderBy(edgeId => edgeId, StringComparer.Ordinal)
+            .ToArray();
+    }
+
+    private static string[] ExpandUnderNodeRepairSet(
+        IReadOnlyCollection<string> selectedEdgeIds,
+        IReadOnlyCollection<ElkRoutedEdge> edges,
+        IReadOnlyCollection<ElkPositionedNode> nodes)
+    {
+        var selected = selectedEdgeIds.ToHashSet(StringComparer.Ordinal);
+        foreach (var edge in edges)
+        {
+            if (ElkEdgeRoutingScoring.CountUnderNodeViolations([edge], nodes) > 0)
+            {
+                selected.Add(edge.Id);
+            }
+        }
+
+        return selected
+            .OrderBy(edgeId => edgeId, StringComparer.Ordinal)
+            .ToArray();
+    }
+
+    private static List<ElkPoint> ExtractPath(ElkRoutedEdge edge)
+    {
+        var path = new List<ElkPoint>();
+        foreach (var section in edge.Sections)
+        {
+            if (path.Count == 0)
+            {
+                path.Add(section.StartPoint);
+            }
+
+            path.AddRange(section.BendPoints);
+            path.Add(section.EndPoint);
+        }
+
+        return path;
+    }
+
+    private static bool HasTargetApproachJoinPair(
+        IReadOnlyList<ElkPoint> leftPath,
+        IReadOnlyList<ElkPoint> rightPath,
+        double minLineClearance,
+        int maxSegmentsFromEnd = 3)
+    {
+        var leftSegments = FlattenSegmentsNearEnd(leftPath, maxSegmentsFromEnd);
+        var rightSegments = FlattenSegmentsNearEnd(rightPath, maxSegmentsFromEnd);
+
+        foreach (var leftSegment in leftSegments)
+        {
+            foreach (var rightSegment in rightSegments)
+            {
+                if (!ElkEdgeRoutingGeometry.AreParallelAndClose(
+                        leftSegment.Start,
+                        leftSegment.End,
+                        rightSegment.Start,
+                        rightSegment.End,
+                        minLineClearance))
+                {
+                    continue;
+                }
+
+                var overlap = ElkEdgeRoutingGeometry.ComputeSharedSegmentLength(
+                    leftSegment.Start,
+                    leftSegment.End,
+                    rightSegment.Start,
+                    rightSegment.End);
+                if (overlap > 8d)
+                {
+                    return true;
+                }
+
+                var leftLength = ElkEdgeRoutingGeometry.ComputeSegmentLength(leftSegment.Start, leftSegment.End);
+                var rightLength = ElkEdgeRoutingGeometry.ComputeSegmentLength(rightSegment.Start, rightSegment.End);
+                if (Math.Min(leftLength, rightLength) > 8d)
+                {
+                    return true;
+                }
+            }
+        }
+
+        return false;
+    }
+
+    private static IReadOnlyList<RoutedEdgeSegment> FlattenSegmentsNearEnd(
+        IReadOnlyList<ElkPoint> path,
+        int maxSegmentsFromEnd)
+    {
+        if (path.Count < 2 || maxSegmentsFromEnd <= 0)
+        {
+            return [];
+        }
+
+        var startIndex = Math.Max(0, path.Count - (maxSegmentsFromEnd + 1));
+        var segments = new List<RoutedEdgeSegment>();
+        for (var i = startIndex; i < path.Count - 1; i++)
+        {
+            segments.Add(new RoutedEdgeSegment(string.Empty, path[i], path[i + 1]));
+        }
+
+        return segments;
+    }
+
+}