手撸一个A*+二叉堆优化（三）

添加了player之后就需要得到player真实坐标对应的网格，所以需要一个根据世界坐标返回数组内坐标的方法

 public Node GetFromPositon(Vector3 positon)
    {
        float percentX = (positon.x + gridSize.x / 2) / gridSize.x;
        float percentY = (positon.z+ gridSize.y / 2) / gridSize.y;
        percentX = Mathf.Clamp01(percentX);
        percentY = Mathf.Clamp01(percentY);

        int x = Mathf.RoundToInt((gridCntX - 1) * percentX);
        int y = Mathf.RoundToInt((gridCntY - 1) * percentY);

        return grid[x, y];
    }

FindPath类中开始搜索

public Transform player;
    public Transform target;
    private Grid _gird;

得到初始点和目标点以及我们创造的Grid

先写一个计算估值的方法

private int GetDistanceNodes(Node a, Node b)
    {
        int cntX = Mathf.Abs(a._gridX - b._gridX);
        int cntY = Mathf.Abs(a._gridY - b._gridY);
        if (cntX>cntY)
        {
            // return 10 * cntY + 10 * (cntX - cntY);
            return 14 * cntY + 10 * (cntX - cntY);
        }
        else
        {
            //  return 10* cntX + 10 * (cntY - cntX);
            return 14 * cntX + 10 * (cntY - cntX);
        }
    }

X大于Y就先向上走斜角再走水平直线，X小于Y就先向旁边就斜角再走垂直直线。

再写搜索的方法

void FindingPath(Vector3 StartPos,Vector3 EndPos)
    {
        Stopwatch sw = new Stopwatch();
        sw.Start();

        Node startNode = _gird.GetFromPositon(StartPos);
        Node endNode = _gird.GetFromPositon(EndPos);
        List<Node> openSet = new List<Node>()；
        HashSet<Node> closeSet = new HashSet<Node>();
        openSet.Add(startNode);//加入开启列表
//遍历开启列表
        while (openSet.Count()>0)
        {
            Node CurrentNode = openSet[0];

            for (int i = 0; i < openSet.Count; i++)
            {
                if (openSet[i].fCost<CurrentNode.fCost||
                    (openSet[i].fCost==CurrentNode.fCost&&openSet[i].hCost<CurrentNode.hCost))
                {
                    CurrentNode = openSet[i];
                }
            }
            openSet.Remove(CurrentNode);//移出开启列表
            closeSet.Add(CurrentNode);//加入关闭列表
//如果等于目标值就结束
            if (CurrentNode==endNode)
            {
                sw.Stop();
                print(sw.ElapsedMilliseconds);
                GeneratePaht(startNode,endNode);//生产路径点
            }
//每次中心点更换都要重新计算周围的格子
            foreach (var node in _gird.GetNerbourhood(CurrentNode))
            {
                if (!node._canwalk || closeSet.Contains(node)) continue;//障碍物或关闭列表跳过
                int newCost = CurrentNode.gCost + GetDistanceNodes(CurrentNode,node);//更新G，如果比之前小就更新估值和父节点
                if (newCost < node.gCost||!openSet.Contains(node))
                {
                    node.gCost = newCost;
                    node.hCost = GetDistanceNodes(node, endNode);
                    node.parent = CurrentNode;
                    if (!openSet.Contains(node))
                    {
                        openSet.Add(node);//如果是新格子就加入开启列表
                    }
                }
            }
        }
    }

至于关闭列表为什么要用HashSet，那肯定是为了提高查找效率，HashSet不包含重复元素,并且其中存储的元素没有特定的顺序,插入元素的操作非常快，不需要像List<T>类那样重排集合。

再写一个生成路径点的方法，也就是找父节点，依次加入path集合

private void GeneratePaht(Node startNode,Node endNode)
    {
        List<Node> path = new List<Node>();
        Node temp = endNode;
        while (temp!=startNode)
        {
            path.Add(temp);
            temp = temp.parent;
        }
        path.Reverse();//反转
        _gird.path = path;
    }

接着回到Grid,写寻找周围格子的方法

 public List<Node> GetNerbourhood(Node node)
    {
        List<Node> neibourhood = new List<Node>();

        for (int i = -1; i <= 1; i++)
        {
            for (int j = -1; j <= 1; j++)
            {
                //  if ((i == 0 && j == 0)||(i==-1&&(j==1||j==-1)||(i==1&&(j==1||j==-1)))) continue;
                if ((i == 0 && j == 0)) continue;
                int tempX = node._gridX + i;
                int tempY = node._gridY + j;
//不能超出格子范围
                if (tempX < gridCntX && tempX > 0 && tempY > 0 && tempY < gridCntY)
                {
                    neibourhood.Add(grid[tempX,tempY]);
                }
            }
        }
        return neibourhood;
    }

现在只要在FindPath的update里执行搜索就可以了

    void Update()
    {
        //if(Input.GetKeyDown(KeyCode.J))
        FindingPath(player.position,target.position);
    }

同样可以用OnDrawGizmos显示我们计算出来的路径点

 if (path != null)
{
                foreach (var node in path)
                {
                    Gizmos.color = Color.black;
                    Gizmos.DrawCube(node._worldPos, Vector3.one * (nodeDiameter - .1f));
                }
 }

在搜索的方法里写了Stopwatch，可以在console里看到消耗的时间，如果不明显，可以增加地图的大小，我的plane大小是10，格子半径1，也就是地图是50X50.

---

在下一篇里将会用二叉堆对搜索方法进行优化，主要是优化开启列表的遍历搜索