JSP寻路 - 4向

直线移动时的强迫邻居规定

下面的图中字母的表示：P表示父节点，B表示障碍，X表示当前节点，N表示强迫邻居(Forced Neighbor)

1，向右移动时，左上为障碍时，上方可走则为强迫邻居

2，向右移动时，左下为障碍时，下方可走则为强迫邻居

3，向左移动时，右上为障碍时，上方可走则为强迫邻居

4，向左移动时，右下为障碍时，下方可走则为强迫邻居

5，向上移动时，左下为障碍时，左侧可走则为强迫邻居

6，向上移动时，右下为障碍时，右侧可走则为强迫邻居

7，向下移动时，左上为障碍时，左侧可走则为强迫邻居

8，向下移动时，右上为障碍时，右侧可走则为强迫邻居

/// <summary>
/// 检查直线方向上的强迫邻居
/// </summary>
/// <param name="dir">搜索方向</param>
/// <param name="pos">节点位置</param>
/// <param name="extraSearchDirs">强迫邻居所在方向</param>
private bool CheckForcedNeighborsLine(Vector2Int dir, Vector2Int pos, List<Vector2Int> extraSearchDirs)
{
    if (dir.x != 0) // 水平方向
    {
        bool ret = false;
        // 上方可走时, 为强迫邻居的情况:
        // 1) 向右时, 左上为障碍
        // 2) 向左时, 右上为障碍
        if (CellType.Walkable == GetCellType(pos + new Vector2Int(0, -1))
            && CellType.Obstacle == GetCellType(pos + new Vector2Int(-dir.x, -1)) )
        {
            extraSearchDirs.Add(new Vector2Int(0, -1));
            ret = true;
        }

        // 下方可走时, 为强迫邻居的情况:
        // 1) 向右时, 左下为障碍
        // 2) 向左时, 右下为障碍
        if (CellType.Walkable == GetCellType(pos + new Vector2Int(0, 1))
            && CellType.Obstacle == GetCellType(pos + new Vector2Int(-dir.x, 1)) )
        {
            extraSearchDirs.Add(new Vector2Int(0, 1));
            ret = true;
        }

        return ret;
    }
    else // 垂直方向
    {
        bool ret = false;
        // 左侧可走时, 为强迫邻居的情况:
        // 1) 向上时, 左下为障碍
        // 2) 向下时, 左上为障碍
        if (CellType.Walkable == GetCellType(pos + new Vector2Int(-1, 0))
            && CellType.Obstacle == GetCellType(pos + new Vector2Int(-1, -dir.y)) )
        {
            extraSearchDirs.Add(new Vector2Int(-1, 0));
            ret = true;
        }

        // 右侧可走时, 为强迫邻居的情况:
        // 1) 向上时, 右下为障碍
        // 2) 向下时, 右上为障碍
        if (CellType.Walkable == GetCellType(pos + new Vector2Int(1, 0))
            && CellType.Obstacle == GetCellType(pos + new Vector2Int(1, -dir.y)) )
        {
            extraSearchDirs.Add(new Vector2Int(1, 0));
            ret = true;
        }

        return ret;
    }
}

跳点(Jump Point)

什么样的节点可以作为跳点？

(1) 起点、终点是跳点

(2) 节点A至少有一个强迫邻居时是跳点（这边叫他强迫邻居跳点）。下图中A有强迫邻居N，所以A是跳点：

(3) 节点A的下一个位置是尽头时是跳点（这边叫他尽头跳点）。下图中A下一个位置是尽头，所以A是跳点：

跳点搜索规则

(1) 起点要在4个方向上搜索，每个方向在遇到跳点，障碍或超出边界时结束

(2) 后续从某个跳点继续搜索，但不是4个方向都要搜索，根据父节点方向决定在哪些方向上搜索跳点，还要根据跳点类型决定额外搜索方向。

(2-1) 父节点在直线方向（红色箭头），则搜索方向为：(pdir.x, 0), 因为A为强迫邻居跳点，所以还要加入强迫邻居所在方向（有几个强迫邻居就要加几个）

(2-2) 父节点在直线方向（红色箭头），则搜索方向为：(pdir.x, 0), 因为A为尽头跳点，所以还要加入与前进方向垂直的两个方向(0, -1), (0, 1)

/// <summary>
/// 在水平或垂直方向上搜索跳点
/// </summary>
/// <param name="pos">从该点开始搜索</param>
/// <param name="dir">搜索方向</param>
/// <param name="jumpPoint">搜索到的跳点</param>
/// <param name="extraSearchDirs">有强迫邻居的节点作为跳点时, 存放强迫邻居方向</param>
private bool FindJumpPointLine(Vector2Int pos, Vector2Int dir, out Vector2Int jumpPoint, List<Vector2Int> extraSearchDirs)
{
    Vector2Int tempPos = pos;
    Vector2Int lastWalkable = tempPos;
    
    while (true) // 不断沿dir方向搜索
    {
        tempPos += dir;

        if (CellType.Walkable != GetCellType(tempPos)) // 超出边界
        {
            jumpPoint = Vector2Int.zero;
            return false;
        }
        
        if (tempPos == m_EndPos) // 如果是终点, 则为跳点
        {
            jumpPoint = tempPos;
            return true;
        }

        if (CheckForcedNeighborsLine(dir, tempPos, extraSearchDirs)) // 如果该节点有强迫邻居, 则为跳点
        {
            jumpPoint = tempPos;
            return true;
        }

        Vector2Int aheadPos = tempPos + dir;
        if (CellType.Walkable != GetCellType(aheadPos)) //下一步是尽头(障碍或边界), 则当前节点就是(尽头)跳点
        {
            //遇到尽头是跳点, 必须转向
            if (dir.x != 0) // 水平方向
            {
                extraSearchDirs.Add(new Vector2Int(0, -1));
                extraSearchDirs.Add(new Vector2Int(0, 1));
            }
            else
            {
                extraSearchDirs.Add(new Vector2Int(-1, 0));
                extraSearchDirs.Add(new Vector2Int(1, 0));
            }
            jumpPoint = tempPos;
            return true;
        }

        lastWalkable = tempPos;
    }
}

整体流程

和AStar有点类似

1) 从起点开始，作为跳点加入OpenList队列

2) 然后不断的找跳点，加入OpenList队列，每次从OpenList找一个代价最小的跳点继续找跳点

3) 直到找到终点或OpenList队列为空

public class JPSPathFinder4Dir
{
    public char[,] m_MapData;

    private Vector2Int m_EndPos;
    private PriorityQueue<Node> m_OpenList = new PriorityQueue<Node>();
    //用于不同的路径找到相同的跳点时, 根据代价选择更优路径
    private Dictionary<Vector2Int, Node> m_OpenDict = new Dictionary<Vector2Int, Node>(); 
    private HashSet<Vector2Int> m_ClosedList = new HashSet<Vector2Int>();
    private List<Vector2Int> m_SearchDirs = new List<Vector2Int>();
    private List<Vector2Int> m_ExtraSearchDirs = new List<Vector2Int>();
    private List<Vector2Int> m_Path = new List<Vector2Int>();

    // 核心寻路方法
    public bool FindPath(Vector2Int startPos, Vector2Int endPos)
    {
        Debug.Log($"FindPath4Dir: start:{startPos}, end:{endPos}");
        m_EndPos = endPos;

        m_Path.Clear();

        if (startPos == endPos)
        {
            m_Path.Add(startPos);
            return true;
        }

        if (CellType.Walkable != GetCellType(startPos) || CellType.Walkable != GetCellType(endPos))
        {
            Debug.Log($"start or end is not walkable");
            return false;
        }

        var curNode = new Node(startPos);
        curNode.g = 0;
        curNode.f = CalcH(startPos, endPos);

        // 起点在4个方向上搜索
        m_SearchDirs.Add(Vector2Int.up);
        m_SearchDirs.Add(Vector2Int.right);
        m_SearchDirs.Add(Vector2Int.down);
        m_SearchDirs.Add(Vector2Int.left);

        do
        {
            Debug.Log($"best: {curNode.pos}");
            m_ClosedList.Add(curNode.pos);

            foreach (var searchDir in m_SearchDirs)
            {
                if (!FindJumpPointLine(curNode.pos, searchDir, out var jumpPoint, m_ExtraSearchDirs))
                {
                    m_ExtraSearchDirs.Clear();
                    continue;
                }

                if (m_ClosedList.Contains(jumpPoint))
                {
                    m_ExtraSearchDirs.Clear();
                    continue;
                }
                Debug.Log($"{curNode.pos} -> 在方向{searchDir}找到跳点 -> {jumpPoint}");

                int gCost = curNode.g + CalcG(curNode.pos, jumpPoint);
                if (m_OpenDict.TryGetValue(jumpPoint, out var node))
                {
                    if (gCost < node.g)
                    {
                        node.g = gCost;
                        node.parent = curNode;
                        m_OpenList.Update();
                        node.extraSearchDirs.Clear();
                        node.extraSearchDirs.AddRange(m_ExtraSearchDirs);
                    }
                }
                else
                {
                    Node successor = new Node(jumpPoint);
                    successor.g = gCost;
                    int hCost = CalcH(jumpPoint, endPos);
                    successor.f = gCost + hCost;
                    successor.parent = curNode;
                    successor.extraSearchDirs.AddRange(m_ExtraSearchDirs);

                    m_OpenList.Enqueue(successor);
                    m_OpenDict.Add(jumpPoint, successor);
                }

                m_ExtraSearchDirs.Clear();
            }
            m_SearchDirs.Clear();

            if (m_OpenList.Count <= 0)
                break;

            curNode = m_OpenList.Dequeue();
            m_OpenDict.Remove(curNode.pos);
            if (curNode.pos == endPos) // 如果找到终点，则返回路径
            {
                RetracePath(curNode);
                FindSuccCleanup();
                return true;
            }

            UpdateSearchDirs(curNode, m_SearchDirs);
        } while (true);

        FindFailCleanup();
        Debug.Log($"path not found");
        return false;
    }

    private void FindFailCleanup()
    {
        m_ClosedList.Clear();
    }

    private void FindSuccCleanup()
    {
        m_OpenList.Clear();
        m_OpenDict.Clear();
        m_ClosedList.Clear();
    }

    // 根据父节点以及跳点类型, 确定要搜索的方向
    private void UpdateSearchDirs(Node node, List<Vector2Int> searchDirs)
    {
        var dir = node.pos - node.parent.pos; // 上一个位置到这个位置的方向
        dir.x = Mathf.Clamp(dir.x, -1, 1);
        dir.y = Mathf.Clamp(dir.y, -1, 1);

        if (dir.x != 0) //父节点在水平方向
        {
            AddSearchDirs(searchDirs, new Vector2Int(dir.x, 0), node.extraSearchDirs);
        }
        else if (dir.y != 0) //父节点在垂直方向
        {
            AddSearchDirs(searchDirs, new Vector2Int(0, dir.y), node.extraSearchDirs);
        }
    }

    private void AddSearchDirs(List<Vector2Int> searchDirs, Vector2Int dir, List<Vector2Int> extraSearchDirs)
    {
        searchDirs.Add(dir);

        foreach (var item in extraSearchDirs)
        {
            if (dir != item)
            {
                searchDirs.Add(item);
            }
        }
    }

    // 计算实际代价（曼哈顿距离）
    private int CalcG(Vector2Int a, Vector2Int b)
    {
        return Mathf.Abs(a.x - b.x) + Mathf.Abs(a.y - b.y);
    }

    // 计算启发代价
    private int CalcH(Vector2Int a, Vector2Int b)
    {
        return CalcG(a, b);
    }

    // 回溯路径
    private void RetracePath(Node node)
    {
        while (node != null)
        {
            m_Path.Add(node.pos);
            node = node.parent;
        }
        m_Path.Reverse();
    }

    public IReadOnlyList<Vector2Int> Path
    {
        get { return m_Path; }
    }

    //...

}

参考

JPS寻路 - 8向

pathfinding/finder/jps_never.go at master · actfuns/pathfinding

posted @ 2026-06-20 20:49 yanghui01 阅读(4) 评论(0) 收藏举报

刷新页面返回顶部

就当笔记吧