コード例 #1
0
    public List <Vector3> FindPath(Node deptNode, Node destNode)
    {
        NodeHeap       openHeap  = new NodeHeap();
        HashSet <Node> closedSet = new HashSet <Node>();

        openHeap.Add(deptNode);
        deptNode.fromCost = deptNode.toCost = 0;
        deptNode.from     = destNode.from = null;

        while (openHeap.Count > 0)
        {
            Node nodeToVisit = openHeap.Pop();
            closedSet.Add(nodeToVisit);

            if (nodeToVisit == destNode)
            {
                break;
            }

            foreach (var adjacent in GetAdjacents(nodeToVisit))
            {
                if (closedSet.Contains(adjacent))
                {
                    continue;
                }

                Node fromNode = null;
                if (nodeToVisit.from != null && nodeToVisit.from.IsLineOfSight(adjacent))
                {
                    fromNode = nodeToVisit.from;
                }
                else
                {
                    fromNode = nodeToVisit;
                }

                int toAdjacentCost = fromNode.toCost + PredictDistanceCost(fromNode, adjacent);
                if (!openHeap.Contains(adjacent) || toAdjacentCost < adjacent.toCost)
                {
                    adjacent.toCost   = toAdjacentCost;
                    adjacent.fromCost = PredictDistanceCost(adjacent, destNode);
                    adjacent.from     = fromNode;

                    if (openHeap.Contains(adjacent))
                    {
                        openHeap.UpdateItem(adjacent);
                    }
                    else
                    {
                        openHeap.Add(adjacent);
                    }
                }
            }
        }

        List <Node> path = new List <Node>();

        if (destNode.from != null)
        {
            Node currentNode = destNode;

            while (currentNode != null)
            {
                path.Add(currentNode);
                currentNode = currentNode.from;
            }
            path.Reverse();
        }

        // PS
        // SmoothingPath(path);

        List <Vector3> positionPath = new List <Vector3>();

        // Visit departure node looks wired when path is changed
        // Skip first(departure) node
        for (int i = 1; i < path.Count; i++)
        {
            positionPath.Add(path[i].tile.transform.position);
        }
        return(positionPath);
    }
コード例 #2
0
ファイル: Pathfinding.cs プロジェクト: GamingDF/ISA
    IEnumerator FindPath(Vector3 startPos, Vector3 targetPos, NodeGrid grid)
    {
        Vector3[] waypoints  = new Vector3[0];
        bool      pathSucces = false;

        if (grid != null)
        {
            Node startNode  = grid.GetNodeFromWorldPoint(startPos);
            Node targetNode = grid.GetNodeFromWorldPoint(targetPos);

            if (/*startNode.walkable && */ targetNode.walkable)
            {
                NodeHeap       openSet   = new NodeHeap(grid.MaxSize);
                HashSet <Node> closedSet = new HashSet <Node>();
                openSet.Add(startNode);

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

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

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

                        int newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);

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

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

            yield return(null);

            if (pathSucces)
            {
                waypoints  = RetracePath(startNode, targetNode, grid);
                pathSucces = (waypoints.Length > 0);
            }

            requestManager.FinishProcessingPath(waypoints, pathSucces);
        }
    }