Пример #1
0
    public void FindPath(PathRequest3D request, Action <PathResult> callback)
    {
        Stopwatch sw = new Stopwatch();

        sw.Start();

        Vector3[] waypoints   = new Vector3[0];
        bool      pathSuccess = false;

        Node3D startNode  = grid.NodeFromWorldPoint(request.pathStart);
        Node3D targetNode = grid.NodeFromWorldPoint(request.pathEnd);

        startNode.parent = startNode;


        if (startNode.walkable && targetNode.walkable)
        {
            Heap3D <Node3D>  openSet   = new Heap3D <Node3D>(grid.MaxSize);
            HashSet <Node3D> closedSet = new HashSet <Node3D>();
            openSet.Add(startNode);

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

                if (currentNode == targetNode)
                {
                    sw.Stop();
                    //print ("Path found: " + sw.ElapsedMilliseconds + " ms");
                    pathSuccess = true;
                    break;
                }

                foreach (Node3D 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));
    }