Beispiel #1
0
    public void FindPath(PathRequest request, Action <PathResult> callBack)
    {
        lock (this)
        {
            Vector3[] wayPoints   = new Vector3[0];
            bool      pathSuccess = false;

            Node startNode  = grid.NodeFromWorldPoint(request.PathStart);
            Node targetNode = grid.NodeFromWorldPoint(request.PathEnd);
            if (targetNode.walkable)
            {
                Heap <Node>    openSet   = new Heap <Node>(grid.GetMaxGridSize);
                HashSet <Node> closedSet = new HashSet <Node>();
                openSet.Add(startNode);

                while (openSet.Count > 0)
                {
                    Node currentNode = openSet.RemoveFirst();
                    closedSet.Add(currentNode);

                    if (currentNode == targetNode)
                    {
                        pathSuccess = true;
                        break;
                    }

                    foreach (Node neighbour in grid.GetNeighbours(currentNode))
                    {
                        if (!neighbour.walkable || closedSet.Contains(neighbour))
                        {
                            continue;
                        }

                        int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
                        if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
                        {
                            neighbour.gCost  = newMovementCostToNeighbour;
                            neighbour.hCost  = GetDistance(neighbour, targetNode);
                            neighbour.parent = currentNode;

                            if (!openSet.Contains(neighbour))
                            {
                                openSet.Add(neighbour);
                            }
                            else
                            {
                                openSet.UpdateItem(neighbour);
                            }
                        }
                    }
                }
            }

            if (pathSuccess)
            {
                wayPoints   = RetracePath(startNode, targetNode);
                pathSuccess = wayPoints.Length > 0;
            }
            callBack(new PathResult(wayPoints, pathSuccess, request.CallBack));
        }
    }