C#工作流正交寻路避障算法

/// <summary>
/// 正交路由器：基于网格 A* 寻路算法，生成避开障碍物（节点）的正交折线路径。
/// 支持避开已有连线、减少拐弯数、保证所有线段严格水平/垂直。
/// </summary>
public class OrthogonalRouter
{
    private readonly int _cellSize;
    private readonly float _margin;
    private const float TurnPenalty = 1.5f;
    private const float RoutePenalty = 30f;

    public OrthogonalRouter(int cellSize = 10, float margin = 18f)
    {
        _cellSize = cellSize;
        _margin = margin;
    }

    public List<PointF> Route(
        PointF start, SizeF startDir,
        PointF end, SizeF endDir,
        List<RectangleF> obstacles,
        List<List<PointF>> existingRoutes = null)
    {
        float exitDist = _margin + _cellSize * 2;
        var startExit = new PointF(
            start.X + startDir.Width * exitDist,
            start.Y + startDir.Height * exitDist);
        var endEntry = new PointF(
            end.X + endDir.Width * exitDist,
            end.Y + endDir.Height * exitDist);

        // 两轮寻路：第一轮完全阻挡已有连线，第二轮使用高惩罚
        var gridPath = FindPath(startExit, startDir, endEntry, endDir, obstacles, existingRoutes, blockRoutes: true);
        if (gridPath == null || gridPath.Count == 0)
            gridPath = FindPath(startExit, startDir, endEntry, endDir, obstacles, existingRoutes, blockRoutes: false);

        var route = new List<PointF> { start, startExit };

        if (gridPath != null && gridPath.Count > 0)
        {
            if (!SharesAxis(startExit, gridPath[0]))
                route.Add(MakeBridge(startExit, gridPath[0], startDir));

            route.AddRange(gridPath);

            if (!SharesAxis(gridPath[^1], endEntry))
                route.Add(MakeBridge(gridPath[^1], endEntry, endDir));
        }
        else
        {
            float midX = (startExit.X + endEntry.X) / 2f;
            route.Add(new PointF(midX, startExit.Y));
            route.Add(new PointF(midX, endEntry.Y));
        }

        route.Add(endEntry);
        route.Add(end);

        route = EnforceOrthogonal(route);
        route = SimplifyRoute(route);
        route = SmoothPath(route, obstacles);
        route = SimplifyRoute(route);

        return route;
    }

    private static PointF MakeBridge(PointF exit, PointF gridPoint, SizeF dir)
    {
        bool horizontal = Math.Abs(dir.Width) > 0.5f;
        return horizontal
            ? new PointF(exit.X, gridPoint.Y)
            : new PointF(gridPoint.X, exit.Y);
    }

    private static bool SharesAxis(PointF a, PointF b)
        => Math.Abs(a.X - b.X) < 0.5f || Math.Abs(a.Y - b.Y) < 0.5f;

    private List<PointF> FindPath(
        PointF start, SizeF startDir,
        PointF end, SizeF endDir,
        List<RectangleF> obstacles,
        List<List<PointF>> existingRoutes,
        bool blockRoutes)
    {
        float minX = Math.Min(start.X, end.X);
        float maxX = Math.Max(start.X, end.X);
        float minY = Math.Min(start.Y, end.Y);
        float maxY = Math.Max(start.Y, end.Y);
        foreach (var obs in obstacles)
        {
            minX = Math.Min(minX, obs.X);
            minY = Math.Min(minY, obs.Y);
            maxX = Math.Max(maxX, obs.Right);
            maxY = Math.Max(maxY, obs.Bottom);
        }

        float pad = _margin + _cellSize * 4;
        minX -= pad; minY -= pad;
        maxX += pad; maxY += pad;

        int cellSize = _cellSize;
        int cols = (int)Math.Ceiling((maxX - minX) / cellSize) + 1;
        int rows = (int)Math.Ceiling((maxY - minY) / cellSize) + 1;

        const int maxDim = 500;
        if (cols > maxDim || rows > maxDim)
        {
            float scale = Math.Max((float)cols / maxDim, (float)rows / maxDim);
            cellSize = (int)Math.Ceiling(cellSize * scale);
            cols = (int)Math.Ceiling((maxX - minX) / cellSize) + 1;
            rows = (int)Math.Ceiling((maxY - minY) / cellSize) + 1;
        }

        var blocked = new bool[cols, rows];
        foreach (var obs in obstacles)
        {
            var expanded = RectangleF.Inflate(obs, _margin, _margin);
            int c0 = Math.Max(0, (int)((expanded.X - minX) / cellSize));
            int r0 = Math.Max(0, (int)((expanded.Y - minY) / cellSize));
            int c1 = Math.Min(cols - 1, (int)((expanded.Right - minX) / cellSize));
            int r1 = Math.Min(rows - 1, (int)((expanded.Bottom - minY) / cellSize));
            for (int r = r0; r <= r1; r++)
                for (int c = c0; c <= c1; c++)
                    blocked[c, r] = true;
        }

        // 标记已有连线经过的网格单元
        var routeCells = new HashSet<(int, int)>();
        if (existingRoutes != null)
        {
            foreach (var route in existingRoutes)
            {
                for (int i = 0; i < route.Count - 1; i++)
                {
                    int c1 = Clamp((int)((route[i].X - minX) / cellSize), 0, cols - 1);
                    int r1 = Clamp((int)((route[i].Y - minY) / cellSize), 0, rows - 1);
                    int c2 = Clamp((int)((route[i + 1].X - minX) / cellSize), 0, cols - 1);
                    int r2 = Clamp((int)((route[i + 1].Y - minY) / cellSize), 0, rows - 1);
                    if (c1 == c2)
                    {
                        int rMin = Math.Min(r1, r2), rMax = Math.Max(r1, r2);
                        for (int r = rMin; r <= rMax; r++) routeCells.Add((c1, r));
                    }
                    else
                    {
                        int cMin = Math.Min(c1, c2), cMax = Math.Max(c1, c2);
                        for (int c = cMin; c <= cMax; c++) routeCells.Add((c, r1));
                    }
                }
            }
        }

        // 第一轮：将已有连线单元标记为完全阻挡
        if (blockRoutes)
        {
            foreach (var (c, r) in routeCells)
                blocked[c, r] = true;
        }

        int sc = Clamp((int)((start.X - minX) / cellSize), 0, cols - 1);
        int sr = Clamp((int)((start.Y - minY) / cellSize), 0, rows - 1);
        int ec = Clamp((int)((end.X - minX) / cellSize), 0, cols - 1);
        int er = Clamp((int)((end.Y - minY) / cellSize), 0, rows - 1);

        // 确保起点和终点不被阻挡
        blocked[sc, sr] = false;
        blocked[ec, er] = false;

        // 沿出/入射方向清理通道
        int sdc = Math.Sign(startDir.Width), sdr = Math.Sign(startDir.Height);
        for (int i = 1; i <= 3; i++)
        {
            int tc = sc + sdc * i, tr = sr + sdr * i;
            if (tc >= 0 && tc < cols && tr >= 0 && tr < rows) blocked[tc, tr] = false;
        }
        int edc = Math.Sign(endDir.Width), edr = Math.Sign(endDir.Height);
        for (int i = 1; i <= 3; i++)
        {
            int tc = ec + edc * i, tr = er + edr * i;
            if (tc >= 0 && tc < cols && tr >= 0 && tr < rows) blocked[tc, tr] = false;
        }

        // 方向编码: 0=上, 1=右, 2=下, 3=左
        int[] dc = { 0, 1, 0, -1 };
        int[] dr = { -1, 0, 1, 0 };

        int startDirIdx = Math.Abs(startDir.Width) > 0.5f
            ? (startDir.Width > 0 ? 1 : 3)
            : (startDir.Height > 0 ? 2 : 0);

        // 方向感知 A*：状态 = (col, row, direction)，拐弯时增加惩罚
        var gScore = new float[cols, rows, 4];
        var closed = new bool[cols, rows, 4];
        var cameFromC = new int[cols, rows, 4];
        var cameFromR = new int[cols, rows, 4];
        var cameFromD = new int[cols, rows, 4];
        for (int r = 0; r < rows; r++)
            for (int c = 0; c < cols; c++)
                for (int d = 0; d < 4; d++)
                {
                    gScore[c, r, d] = float.MaxValue;
                    cameFromC[c, r, d] = -1;
                    cameFromR[c, r, d] = -1;
                    cameFromD[c, r, d] = -1;
                }

        gScore[sc, sr, startDirIdx] = 0;

        var open = new PriorityQueue<(int c, int r, int d), float>();
        open.Enqueue((sc, sr, startDirIdx), Manhattan(sc, sr, ec, er));

        bool found = false;
        int bestEndDir = -1;

        while (open.TryDequeue(out var cur, out _))
        {
            if (closed[cur.c, cur.r, cur.d]) continue;
            closed[cur.c, cur.r, cur.d] = true;

            if (cur.c == ec && cur.r == er)
            {
                found = true;
                bestEndDir = cur.d;
                break;
            }

            for (int i = 0; i < 4; i++)
            {
                int nc = cur.c + dc[i];
                int nr = cur.r + dr[i];
                if (nc < 0 || nc >= cols || nr < 0 || nr >= rows) continue;
                if (blocked[nc, nr]) continue;

                float stepCost = 1f;
                if (i != cur.d) stepCost += TurnPenalty;
                if (!blockRoutes && routeCells.Contains((nc, nr))) stepCost += RoutePenalty;

                float tentativeG = gScore[cur.c, cur.r, cur.d] + stepCost;
                if (tentativeG < gScore[nc, nr, i])
                {
                    cameFromC[nc, nr, i] = cur.c;
                    cameFromR[nc, nr, i] = cur.r;
                    cameFromD[nc, nr, i] = cur.d;
                    gScore[nc, nr, i] = tentativeG;
                    float f = tentativeG + Manhattan(nc, nr, ec, er) * 1.001f;
                    open.Enqueue((nc, nr, i), f);
                }
            }
        }

        if (!found) return null;

        // 回溯路径
        var cells = new List<(int c, int r)>();
        int cc = ec, cr = er, cd = bestEndDir;
        while (cc != -1 && cr != -1 && cd != -1)
        {
            cells.Add((cc, cr));
            int pc = cameFromC[cc, cr, cd];
            int pr = cameFromR[cc, cr, cd];
            int pd = cameFromD[cc, cr, cd];
            if (pc == -1) break;
            cc = pc; cr = pr; cd = pd;
        }
        cells.Reverse();

        var worldPath = new List<PointF>(cells.Count);
        foreach (var (c, r) in cells)
        {
            worldPath.Add(new PointF(
                (float)Math.Round(minX + c * cellSize),
                (float)Math.Round(minY + r * cellSize)));
        }

        return worldPath;
    }

    /// <summary>
    /// 路径平滑：尝试用两段正交线替代多段折线，减少拐点数量。
    /// 仅当能减少路径点数时才接受替换，避免无限循环。
    /// </summary>
    private List<PointF> SmoothPath(List<PointF> path, List<RectangleF> obstacles)
    {
        if (path.Count <= 3) return path;

        var result = new List<PointF>(path);
        int i = 0;
        int maxIterations = result.Count * result.Count + 10;
        int iterations = 0;
        while (i < result.Count - 2 && iterations < maxIterations)
        {
            iterations++;
            bool improved = false;
            // j > i + 2 确保至少替换2个中间点为1个拐角点，路径必定缩短
            for (int j = result.Count - 1; j > i + 2; j--)
            {
                // 尝试拐角点1：先水平后垂直
                var c1 = new PointF(result[j].X, result[i].Y);
                if (IsSegmentClear(result[i], c1, obstacles) && IsSegmentClear(c1, result[j], obstacles))
                {
                    result.RemoveRange(i + 1, j - i - 1);
                    result.Insert(i + 1, c1);
                    improved = true;
                    break;
                }

                // 尝试拐角点2：先垂直后水平
                var c2 = new PointF(result[i].X, result[j].Y);
                if (IsSegmentClear(result[i], c2, obstacles) && IsSegmentClear(c2, result[j], obstacles))
                {
                    result.RemoveRange(i + 1, j - i - 1);
                    result.Insert(i + 1, c2);
                    improved = true;
                    break;
                }
            }
            if (!improved) i++;
        }
        return result;
    }

    /// <summary>
    /// 检查正交线段是否不与任何障碍物相交。
    /// </summary>
    private bool IsSegmentClear(PointF a, PointF b, List<RectangleF> obstacles)
    {
        float margin = _margin;

        if (Math.Abs(a.X - b.X) < 0.01f)
        {
            // 垂直线段
            float x = a.X;
            float yMin = Math.Min(a.Y, b.Y);
            float yMax = Math.Max(a.Y, b.Y);
            foreach (var obs in obstacles)
            {
                if (x > obs.Left - margin && x < obs.Right + margin &&
                    yMax > obs.Top - margin && yMin < obs.Bottom + margin)
                    return false;
            }
            return true;
        }
        else
        {
            // 水平线段
            float y = a.Y;
            float xMin = Math.Min(a.X, b.X);
            float xMax = Math.Max(a.X, b.X);
            foreach (var obs in obstacles)
            {
                if (y > obs.Top - margin && y < obs.Bottom + margin &&
                    xMax > obs.Left - margin && xMin < obs.Right + margin)
                    return false;
            }
            return true;
        }
    }

    /// <summary>
    /// 强制正交：检查所有相邻点对，若非水平/垂直则插入桥接点。
    /// </summary>
    private static List<PointF> EnforceOrthogonal(List<PointF> route)
    {
        if (route.Count <= 1) return route;

        var result = new List<PointF> { route[0] };
        for (int i = 1; i < route.Count; i++)
        {
            var prev = result[^1];
            var curr = route[i];

            if (Math.Abs(prev.X - curr.X) > 0.01f && Math.Abs(prev.Y - curr.Y) > 0.01f)
            {
                // 非正交，需要插入桥接点
                if (result.Count >= 2)
                {
                    var prevPrev = result[^2];
                    bool prevWasVertical = Math.Abs(prevPrev.X - prev.X) < 0.01f;
                    if (prevWasVertical)
                        result.Add(new PointF(prev.X, curr.Y));
                    else
                        result.Add(new PointF(curr.X, prev.Y));
                }
                else
                {
                    result.Add(new PointF(curr.X, prev.Y));
                }
            }
            result.Add(curr);
        }
        return result;
    }

    private static float Manhattan(int c1, int r1, int c2, int r2)
        => Math.Abs(c1 - c2) + Math.Abs(r1 - r2);

    private static int Clamp(int v, int min, int max)
        => v < min ? min : (v > max ? max : v);

    private static List<PointF> SimplifyRoute(List<PointF> route)
    {
        if (route.Count <= 2) return route;

        var deduped = new List<PointF> { route[0] };
        for (int i = 1; i < route.Count; i++)
        {
            var last = deduped[^1];
            if (Math.Abs(route[i].X - last.X) > 0.5f || Math.Abs(route[i].Y - last.Y) > 0.5f)
                deduped.Add(route[i]);
        }

        if (deduped.Count <= 2) return deduped;

        var result = new List<PointF> { deduped[0] };
        for (int i = 1; i < deduped.Count - 1; i++)
        {
            var prev = deduped[i - 1];
            var curr = deduped[i];
            var next = deduped[i + 1];
            if (!IsCollinear(prev, curr, next))
                result.Add(curr);
        }
        result.Add(deduped[^1]);

        return result;
    }

    private static bool IsCollinear(PointF a, PointF b, PointF c)
    {
        const float eps = 0.01f;
        return (Math.Abs(a.X - b.X) < eps && Math.Abs(b.X - c.X) < eps) ||
               (Math.Abs(a.Y - b.Y) < eps && Math.Abs(b.Y - c.Y) < eps);
    }
}