JPS寻路

强迫邻居

定义：节点X的邻居节点有障碍物，且X的父节点P经过X到达N的距离代价，比不经过X到大N的任一路径的距离代价都小，则称N是X的强迫邻居。

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

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

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

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

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

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

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

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

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

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

获取强迫邻居在哪个方向

/// <param name="dir">搜索方向</param>
/// <param name="pos">节点X</param>
/// <param name="forceNeighborDirs">强制邻居所在方向</param>
private bool CheckForcedNeighborsLine(Vector2Int dir, Vector2Int pos, List<Vector2Int> forceNeighborDirs)
{
    if (dir.x != 0) // 水平方向
    {
        bool ret = false;
        if (CellType.Obstacle == GetCellType(pos + new Vector2Int(0, 1)) && CellType.Walkable == GetCellType(pos + new Vector2Int(dir.x, 1))) 
        {
            //下方为障碍: 往右时, 右下可走时为强迫邻居; 往左时, 左下可走为强迫邻居
            forceNeighborDirs.Add(new Vector2Int(dir.x, 1));
            ret = true;
        }

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

        if (CellType.Obstacle == GetCellType(pos + new Vector2Int(-1, 0)) && CellType.Walkable == GetCellType(pos + new Vector2Int(-1, dir.y)))
        {
            //左侧为障碍: 往上时, 左上可走为强迫邻居; 往下时, 左下可走为强迫邻居
            forceNeighborDirs.Add(new Vector2Int(-1, dir.y));
            ret = true;
        }
        return ret;
    }
}

斜向移动时强迫邻居的规定

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

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

3，右下移动时，左侧为障碍，左下可走则为强迫邻居

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

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

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

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

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

获取强迫邻居在哪个方向

private bool CheckForcedNeighborsDiagonal(Vector2Int dir, Vector2Int pos, List<Vector2Int> forceNeighborDirs)
{
    bool ret = false;
    if (CellType.Obstacle == GetCellType(pos + new Vector2Int(-dir.x, 0)) && CellType.Walkable == GetCellType(pos + new Vector2Int(-dir.x, dir.y)))
    {
        forceNeighborDirs.Add(new Vector2Int(-dir.x, dir.y));
        ret = true;
    }

    if (CellType.Obstacle == GetCellType(pos + new Vector2Int(0, -dir.y)) && CellType.Walkable == GetCellType(pos + new Vector2Int(dir.x, -dir.y)))
    {
        forceNeighborDirs.Add(new Vector2Int(dir.x, -dir.y));
        ret = true;
    }
    return ret;
}

没有强迫邻居的情况示例

2，3为可走节点，但为啥他们不能成为强迫邻居？

2可以直接由P斜向到达，不需要经过X

3可以通过P>2>3到达，也存在不经过X的路径；他和P>X>3这条路径是等价的，而JPS的核心思想就是排除冗余，避免评估等价路径，所以这种会出现等价路径的情况在JPS中是直接排除掉的。

跳点(Jump Point)

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

(1) 起点、终点是跳点

(2) 节点A至少有一个强迫邻居时是跳点

(3) 父节点在斜方向(斜向搜索)，节点A的水平或者垂直方向上有起点或终点、或有强迫邻居的节点。

下图中A的父节点P在斜方向，他的水平方向上节点2有强迫邻居N，所以A是跳点；节点2有一个强迫邻居，所以也是跳点。

跳点搜索规则

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

(2) 后续从某个跳点继续搜索，但不是8个方向都要搜索，根据父节点方向决定在哪些方向上搜索跳点，有强制邻居时还要对强制邻居方向进行搜索。

(2-1) 如果父节点在斜方向（红色箭头），则搜索方向为：(pdir.x, 0), (0, pdir.y), (pdir.x, pdir.y)这3个方向（蓝色箭头）

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

/// <summary>
/// 在水平或垂直方向上搜索跳点 
/// </summary>
/// <param name="pos">从该点开始搜索</param>
/// <param name="dir">搜索方向</param>
/// <param name="jumpPoint">搜索到的跳点</param>
/// <param name="forceNeighborDirs">有强迫邻居的节点作为跳点时, 存放强制邻居方向</param>
private bool FindJumpPointLine(Vector2Int pos, Vector2Int dir, out Vector2Int jumpPoint, List<Vector2Int> forceNeighborDirs)
{
    Vector2Int tempPos = pos;
    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, forceNeighborDirs)) // 如果该节点有强迫邻居, 则为跳点
        {
            jumpPoint = tempPos;
            return true;
        }
    }
}

// 斜向搜索跳点
private bool FindJumpPointDiagonal(Vector2Int pos, Vector2Int dir, out Vector2Int jumpPoint, List<Vector2Int> forceNeighborDirs)
{
    Vector2Int tempPos = pos;
    while (true) //不断沿dir方向搜索
    {
        tempPos += dir;

        if (CellType.Walkable != GetCellType(tempPos)) //超出边界或遇到障碍
        {
            jumpPoint = Vector2Int.zero;
            return false;
        }

        if (tempPos == m_EndPos) // 如果是终点, 则为跳点
        {
            jumpPoint = tempPos;
            return true;
        }

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

        if (FindJumpPointLine(tempPos, new Vector2Int(dir.x, 0), out var pt, null)) //斜向搜索时, 水平方向有跳点时, 自己则为跳点
        {
            jumpPoint = tempPos;
            return true;
        }

        if (FindJumpPointLine(tempPos, new Vector2Int(0, dir.y), out pt, null)) //斜向搜索时, 垂直方向有跳点时, 自己则为跳点
        {
            jumpPoint = tempPos;
            return true;
        }
    }
}

整体流程

和AStar有点类似

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

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

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

public class JPSPathFinder
{
    public char[,] m_MapData;

    private Vector2Int m_EndPos;
    private PriorityQueue<Node> m_OpenList = new PriorityQueue<Node>();
    private HashSet<Vector2Int> m_ClosedList = new HashSet<Vector2Int>();
    private List<Vector2Int> m_SearchDirs = new List<Vector2Int>();
    private List<Vector2Int> m_ForcedNeighborDirs = new List<Vector2Int>();
    private List<Vector2Int> m_Path = new List<Vector2Int>();

    // 核心寻路方法
    public bool FindPath(Vector2Int startPos, Vector2Int endPos)
    {
        Debug.Log($"FindPath: 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);

        //起点要在8个方向上搜索
        m_SearchDirs.Add(Vector2Int.up);
        m_SearchDirs.Add(Vector2Int.right);
        m_SearchDirs.Add(Vector2Int.down);
        m_SearchDirs.Add(Vector2Int.left);
        m_SearchDirs.Add(new Vector2Int(1, 1)); // 右上
        m_SearchDirs.Add(new Vector2Int(1, -1)); // 右下
        m_SearchDirs.Add(new Vector2Int(-1, 1)); // 左上
        m_SearchDirs.Add(new Vector2Int(-1, -1)); // 左下

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

            foreach (var searchDir in m_SearchDirs)
            {
                Vector2Int jumpPoint = Vector2Int.zero;
                if (searchDir.x * searchDir.y == 0)
                {
                    if (!FindJumpPointLine(curNode.pos, searchDir, out jumpPoint, m_ForcedNeighborDirs))
                    {
                        m_ForcedNeighborDirs.Clear();
                        continue;
                    }
                }
                else
                {
                    if (!FindJumpPointDiagonal(curNode.pos, searchDir, out jumpPoint, m_ForcedNeighborDirs))
                    {
                        m_ForcedNeighborDirs.Clear();
                        continue;
                    }
                }

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

                Node successor = new Node(jumpPoint);
                int gCost = curNode.g + CalcG(curNode.pos, jumpPoint);
                successor.g = gCost;
                int hCost = CalcH(jumpPoint, endPos);
                successor.f = gCost + hCost;
                successor.parent = curNode;
                successor.forcedNeighborDirs.AddRange(m_ForcedNeighborDirs);                m_OpenList.Enqueue(successor);
                m_ForcedNeighborDirs.Clear();
            }
            m_SearchDirs.Clear();

            if (m_OpenList.Count <= 0)
                break;

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

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

        FindFailCleanup();
        return false;
    }

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

    private void FindSuccCleanup()
    {
        m_OpenList.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 && dir.y != 0) //父节点在斜方向
        {
            searchDirs.Add(new Vector2Int(dir.x, 0));
            searchDirs.Add(new Vector2Int(0, dir.y));
            searchDirs.Add(dir);

            if (node.forcedNeighborDirs.Count > 0) //有强制邻居时, 强制邻居所在的方向也要搜索
            {
                foreach (var item in node.forcedNeighborDirs)
                {
                    if (item != dir)
                        searchDir.Add(item);
                }
            }
        }
        else
        {
            if (dir.x != 0) //父节点在水平方向
            {
                searchDirs.Add(new Vector2Int(dir.x, 0));
            }
            else if (dir.y != 0) //父节点在垂直方向
            {
                searchDirs.Add(new Vector2Int(0, dir.y));
            }

            if (node.forcedNeighborDirs.Count > 0) //有强制邻居时, 强制邻居所在的方向也要搜索
            {
                searchDirs.AddRange(node.forcedNeighborDirs);
            }
        }
    }

    //计算实际代价, 八进制距离优化版 -斜向和直线移动代价不同时
    private int CalcG(Vector2Int a, Vector2Int b)
    {
        int dx = Mathf.Abs(a.x - b.x);
        int dy = Mathf.Abs(a.y - b.y);
        if (dx == dy)
            return dx * 14;
        elsereturn Mathf.Min(dx, dy) * 14 + Mathf.Abs(dx - dy) * 10;
    }

    // 计算启发代价
    private int CalcH(Vector2Int a, Vector2Int b)
    {
        return CalcG(a, b); //斜向和直线移动代价不同, 所以使用和G一样的计算模型
    }

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

    //...

}

计算代价的几种方式

// 计算实际代价, 八进制距离标准版 - 斜向和直线移动代价不同时
private float CalcG(Vector2Int a, Vector2Int b)
{
    int dx = Mathf.Abs(a.x - b.x);
    int dy = Mathf.Abs(a.y - b.y);
    const float SQRT2 = 1.41421356f;
    return dx + dy + (SQRT2 - 2) * Mathf.Min(dx, dy);
}

// 计算实际代价, 欧几里得距离
private float CalcG(Vector2Int a, Vector2Int b)
{
    return Vector2Int.Distance(curNode.pos, jumpPoint);
}

// 计算启发代价, 曼哈顿距离 - 斜向和直线移动代价相同时
private int CalcH(Vector2Int a, Vector2Int b)
{
    return Mathf.Abs(a.x - b.x) + Mathf.Abs(a.y - b.y);
}

地图数据相关代码

public enum CellType
{
    Walkable = 0,
    Obstacle,
    Outside,
}

// 判断节点类型
private CellType GetCellType(Vector2Int pos)
{
    if (pos.y < 0 || pos.y >= m_MapData.GetLength(0))
        return CellType.Outside; //超出边界
    if (pos.x < 0 || pos.x >= m_MapData.GetLength(1))
        return CellType.Outside; //超出边界

    char c = m_MapData[pos.y, pos.x];
    if (c == '1')
        return CellType.Obstacle;
    return CellType.Walkable;
}

节点类

public class Node
{
    public Vector2Int pos; // 节点位置
    public int g; // g: 起点到当前节点的实际代价, h: 当前节点到终点的估计代价
    public int f; // 总代价（G + H）
    public Node parent;
    public List<Vector2Int> forcedNeighborDirs = new List<Vector2Int>(); //强制邻居

    public Node(Vector2Int pos)
    {
        this.pos = pos;
    }
}

用列表模拟的优先队列，生产环境一定要改成用堆(Heap)实现

public class PriorityQueue<T> where T : JPSPathFinder.Node
{
    private readonly List<T> elements = new List<T>();

    public int Count => elements.Count;

    public void Enqueue(T item)
    {
        elements.Add(item);
        elements.Sort((a, b) => a.f.CompareTo(b.f)); // 按F值排序
    }

    public T Dequeue()
    {
        T item = elements[0];
        elements.RemoveAt(0);
        return item;
    }

    public void Clear()
    {
        elements.Clear();
    }

}

使用示例

public class JpsPathFindMain : MonoBehaviour
{

    private static void ReadMapData(out char[,] mapData, out Vector2Int start, out Vector2Int end)
    {
        start = Vector2Int.zero;
        end = Vector2Int.zero;

        var mapDataAsset = AssetDatabase.LoadAssetAtPath<TextAsset>("Assets/Jps/MapData.txt");
        StringReader sr = new StringReader(mapDataAsset.text);
        string line = null;
        var lineDatas = new List<char[]>();
        int cols = 0;
        int y = 0;
        int lineNum = 0;
        while ((line = sr.ReadLine()) != null)
        {
            lineNum++;
            if (line == "" || line.StartsWith('#')) continue;

            var splitArr = line.Split(',');
            if (cols == 0)
            {
                cols = splitArr.Length;
            }
            else if (cols != splitArr.Length)
            {
                throw new Exception($"数据列数不一致: lineNum: {lineNum}, cols: {cols}");
            }

            var lineData = new char[splitArr.Length];
            lineDatas.Add(lineData);

            int x = 0;
            foreach (var item in splitArr)
            {
                if (item == "s")
                {
                    start = new Vector2Int(x, y);
                }
                else if (item == "e")
                {
                    end = new Vector2Int(x, y);
                }
                lineData[x++] = item[0];
            }

            y++;
        }

        mapData = new char[lineDatas.Count, cols];
        for (y = 0; y < mapData.GetLength(0); y++)
        {
            for (int x = 0; x < mapData.GetLength(1); x++)
            {
                mapData[y, x] = lineDatas[y][x];
            }
        }
    }

    void Start()
    {
        var jpsPathFind = new JPSPathFinder();
        ReadMapData(out var mapData, out var start, out var end);
        jpsPathFind.m_MapData = mapData;

        // 执行JPS寻路
        if (jpsPathFind.FindPath(start, end))
        {
            Debug.Log("Path found:");
            var path = jpsPathFind.Path;
            foreach (var node in path)
            {
                //Debug.Log(node);
                char c = mapData[node.y, node.x];
                if (c == 's' || c == 'e')
                {
                    //忽略
                }
                else
                {
                    mapData[node.y, node.x] = '*';
                }
            }

            var sb = new StringBuilder();
            sb.Append("\n");
            for (int y = 0; y < mapData.GetLength(0); ++y)
            {
                for (int x = 0; x < mapData.GetLength(1); ++x)
                {
                    if (x > 0)
                        sb.Append(",");
                    sb.Append($"{mapData[y, x]}");
                }
                sb.Append("\n");
            }
            Debug.Log($"{sb.ToString()}");
        }
        else
        {
            Debug.Log("No path found.");
        }
    }

}