git.stella-ops.org/src/__Libraries/StellaOps.ElkSharp/ElkEdgePostProcessorAStar.cs

namespace StellaOps.ElkSharp;

internal static class ElkEdgePostProcessorAStar
{
    internal static List<ElkPoint>? RerouteWithGridAStar(
        ElkPoint start, ElkPoint end,
        (double Left, double Top, double Right, double Bottom, string Id)[] obstacles,
        string sourceId, string targetId,
        double margin)
    {
        return RerouteWithGridAStar(
            start,
            end,
            obstacles,
            sourceId,
            targetId,
            new OrthogonalAStarOptions(margin, 200d, 0d, 14d),
            [],
            CancellationToken.None);
    }

    internal static List<ElkPoint>? RerouteWithGridAStar(
        ElkPoint start, ElkPoint end,
        (double Left, double Top, double Right, double Bottom, string Id)[] obstacles,
        string sourceId, string targetId,
        OrthogonalAStarOptions options,
        IReadOnlyList<OrthogonalSoftObstacle> softObstacles,
        CancellationToken cancellationToken)
    {
        var xs = new SortedSet<double> { start.X, end.X };
        var ys = new SortedSet<double> { start.Y, end.Y };
        foreach (var ob in obstacles)
        {
            if (ob.Id == sourceId || ob.Id == targetId) continue;
            xs.Add(ob.Left - options.Margin);
            xs.Add(ob.Right + options.Margin);
            ys.Add(ob.Top - options.Margin);
            ys.Add(ob.Bottom + options.Margin);
        }

        var xArr = xs.ToArray();
        var yArr = ys.ToArray();
        var xCount = xArr.Length;
        var yCount = yArr.Length;
        if (xCount < 2 || yCount < 2) return null;

        var startIx = Array.BinarySearch(xArr, start.X);
        var startIy = Array.BinarySearch(yArr, start.Y);
        var endIx = Array.BinarySearch(xArr, end.X);
        var endIy = Array.BinarySearch(yArr, end.Y);
        if (startIx < 0 || startIy < 0 || endIx < 0 || endIy < 0) return null;

        bool IsBlocked(int ix1, int iy1, int ix2, int iy2)
        {
            var x1 = xArr[ix1]; var y1 = yArr[iy1];
            var x2 = xArr[ix2]; var y2 = yArr[iy2];
            foreach (var ob in obstacles)
            {
                if (ob.Id == sourceId || ob.Id == targetId) continue;
                if (ix1 == ix2)
                {
                    var segX = x1;
                    if (segX > ob.Left && segX < ob.Right)
                    {
                        var minY = Math.Min(y1, y2);
                        var maxY = Math.Max(y1, y2);
                        if (maxY > ob.Top && minY < ob.Bottom) return true;
                    }
                }
                else if (iy1 == iy2)
                {
                    var segY = y1;
                    if (segY > ob.Top && segY < ob.Bottom)
                    {
                        var minX = Math.Min(x1, x2);
                        var maxX = Math.Max(x1, x2);
                        if (maxX > ob.Left && minX < ob.Right) return true;
                    }
                }
            }

            return false;
        }

        // A* with (ix, iy, direction) state; direction: 0=none, 1=horizontal, 2=vertical
        var stateCount = xCount * yCount * 3;
        var gScore = new double[stateCount];
        Array.Fill(gScore, double.MaxValue);
        var cameFrom = new int[stateCount];
        Array.Fill(cameFrom, -1);

        int StateId(int ix, int iy, int dir) => (ix * yCount + iy) * 3 + dir;
        double Heuristic(int ix, int iy) =>
            Math.Abs(xArr[ix] - xArr[endIx]) + Math.Abs(yArr[iy] - yArr[endIy]);

        var startState = StateId(startIx, startIy, 0);
        gScore[startState] = 0d;
        var openSet = new PriorityQueue<int, double>();
        openSet.Enqueue(startState, Heuristic(startIx, startIy));

        var dx = new[] { 1, -1, 0, 0 };
        var dy = new[] { 0, 0, 1, -1 };
        var dirs = new[] { 1, 1, 2, 2 };

        var maxIterations = xCount * yCount * 6;
        var iterations = 0;
        var closed = new HashSet<int>();
        while (openSet.Count > 0 && iterations++ < maxIterations)
        {
            cancellationToken.ThrowIfCancellationRequested();

            var current = openSet.Dequeue();

            if (!closed.Add(current))
            {
                continue;
            }

            var curDir = current % 3;
            var curIy = (current / 3) % yCount;
            var curIx = (current / 3) / yCount;

            if (curIx == endIx && curIy == endIy)
            {
                var path = new List<ElkPoint>();
                var state = current;
                while (state >= 0)
                {
                    var sIy = (state / 3) % yCount;
                    var sIx = (state / 3) / yCount;
                    path.Add(new ElkPoint { X = xArr[sIx], Y = yArr[sIy] });
                    state = cameFrom[state];
                }

                path.Reverse();
                var simplified = new List<ElkPoint> { path[0] };
                for (var i = 1; i < path.Count - 1; i++)
                {
                    var prev = simplified[^1];
                    var next = path[i + 1];
                    if (Math.Abs(prev.X - path[i].X) > 0.5d || Math.Abs(path[i].X - next.X) > 0.5d)
                    {
                        if (Math.Abs(prev.Y - path[i].Y) > 0.5d || Math.Abs(path[i].Y - next.Y) > 0.5d)
                        {
                            simplified.Add(path[i]);
                        }
                    }
                }

                simplified.Add(path[^1]);
                return simplified;
            }

            for (var d = 0; d < 4; d++)
            {
                var nx = curIx + dx[d];
                var ny = curIy + dy[d];
                if (nx < 0 || nx >= xCount || ny < 0 || ny >= yCount) continue;
                if (IsBlocked(curIx, curIy, nx, ny)) continue;

                var newDir = dirs[d];
                var bend = (curDir != 0 && curDir != newDir) ? options.BendPenalty : 0d;
                var dist = Math.Abs(xArr[nx] - xArr[curIx]) + Math.Abs(yArr[ny] - yArr[curIy]);
                var softCost = ComputeSoftObstacleCost(
                    xArr[curIx],
                    yArr[curIy],
                    xArr[nx],
                    yArr[ny],
                    softObstacles,
                    options);
                var tentativeG = gScore[current] + dist + bend + softCost;
                var neighborState = StateId(nx, ny, newDir);

                if (tentativeG < gScore[neighborState])
                {
                    gScore[neighborState] = tentativeG;
                    cameFrom[neighborState] = current;
                    var f = tentativeG + Heuristic(nx, ny);
                    openSet.Enqueue(neighborState, f);
                }
            }
        }

        return null;
    }

    private static double ComputeSoftObstacleCost(
        double x1,
        double y1,
        double x2,
        double y2,
        IReadOnlyList<OrthogonalSoftObstacle> softObstacles,
        OrthogonalAStarOptions options)
    {
        if (options.SoftObstacleWeight <= 0d || softObstacles.Count == 0)
        {
            return 0d;
        }

        var candidateStart = new ElkPoint { X = x1, Y = y1 };
        var candidateEnd = new ElkPoint { X = x2, Y = y2 };
        var cost = 0d;

        foreach (var obstacle in softObstacles)
        {
            if (ElkEdgeRoutingGeometry.SegmentsIntersect(candidateStart, candidateEnd, obstacle.Start, obstacle.End))
            {
                cost += 120d * options.SoftObstacleWeight;
                continue;
            }

            if (ElkEdgeRoutingGeometry.AreParallelAndClose(
                    candidateStart,
                    candidateEnd,
                    obstacle.Start,
                    obstacle.End,
                    options.SoftObstacleClearance))
            {
                cost += 18d * options.SoftObstacleWeight;
            }
        }

        return cost;
    }
}