Ejemplo n.º 1
0
    public bool FindPath(Vector2Int gridstart, Vector2Int gridend, ref List <Node> pathref)
    {
        // create open set
        List <Node> open_set = new List <Node>();

        Node start_node = grid_.GetNode(gridstart.x, gridstart.y);
        Node end_node   = grid_.GetNode(gridend.x, gridend.y);

        open_set.Add(start_node);
        start_node.inOpen_ = true;
        nodes_to_reset_.Add(start_node);

        while (open_set.Count > 0)
        {
            Node current_node = open_set[GetLowestFNode(open_set)];

            // erase current node from open set
            open_set.Remove(current_node);
            // remove current node from open set
            current_node.inOpen_ = false;
            // add current node to close set by flag
            current_node.inClosed_ = true;

            // if current node equals to end node, we have reached destination
            if (current_node == end_node)
            {
                pathref = RetracePath(start_node, end_node);
                return(true);
            }

            // else continue search
            foreach (Node n in grid_.GetNeighbours(current_node))
            {
                // if not traversable or in closed set, skip
                if (!n.isWalkable_ || n.inClosed_)
                {
                    continue;
                }
                // if new g cost < g cost (need updating), or if not in open set, update/calculate f cost, add to open set
                // calculate new g cost of neighbour relative to current node
                int new_g_cost = current_node.gCost_ + GetDistanceBetweenNodes(current_node, n);
                if (new_g_cost < n.gCost_ || !n.inOpen_)
                {
                    n.gCost_  = new_g_cost;
                    n.hCost_  = GetDistanceBetweenNodes(n, end_node);
                    n.parent_ = current_node;
                    if (!n.inOpen_)
                    {
                        open_set.Add(n);
                        n.inOpen_ = true;
                    }
                    nodes_to_reset_.Add(n);
                }
            }
        }
        return(false);
    }