Esempio n. 1
0
 public Goal(Predicate <NodeType> generalGoal)
 {
     SpecificGoal = new OptionalVal <NodeType>();
     GeneralGoal  = generalGoal;
 }
Esempio n. 2
0
 public Goal(NodeType specificGoal, Predicate <NodeType> generalGoal)
 {
     SpecificGoal = specificGoal;
     GeneralGoal  = generalGoal;
 }
Esempio n. 3
0
 public bool Equals(OptionalVal <T> o)
 {
     return((o.HasValue == HasValue) &&
            (!o.HasValue || o.val.Equals(val)));
 }
Esempio n. 4
0
        /// <summary>
        /// Finds the shortest path from the given start to a node that satisfies the given goal.
        /// Returns whether a full path to an acceptable goal was found.
        /// </summary>
        /// <param name="outPath">
        /// After this method is called, this list contains the path from start to end,
        ///     NOT including the start node itself.
        /// </param>
        /// <param name="maxPathLength">
        /// The maximum path length this method can search from the start node.
        /// "Path length" is the sum of the lengths of each edge in a path.
        /// </param>
        /// <param name="tryMyBest">
        /// If this is true, and a path to an actual goal can't be found,
        ///     this method will do extra work to attempt to find the closest path possible.
        /// </param>
        public bool FindPath(NodeType start, Goal <NodeType> goal, float maxPathLength,
                             bool tryMyBest, List <NodeType> outPath)
        {
            //Note: edge length is referred to as "search" cost,
            //    while edge length + A* heuristics is referred to as "traversal" cost.

            //Clear out the collections for this run.
            pathTree.Clear();
            nodesToSearch.Clear();
            cost_traversal.Clear();
            cost_search.Clear();
            considered.Clear();
            connections.Clear();

            //The final stopping place of this path.
            OptionalVal <NodeType> finalDestination = new OptionalVal <NodeType>();


            //Start searching from the source node.
            considered.Add(start);
            pathTree.Add(start, new OptionalVal <NodeType>());
            cost_traversal.Add(start, 0.0f);
            cost_search.Add(start, 0.0f);
            //Start the search frontier with a phony edge that "ends" at the start node.
            nodesToSearch.Push(new Edge <NodeType>(default(NodeType), start), 0.0f);

            //This is used in case we can't find the actual end node.
            NodeType lastNodeChecked = default(NodeType);

            //As long as there are more edges to search, keep checking them out.
            while (!nodesToSearch.IsEmpty)
            {
                //Get the edge/node being checked out.
                KeyValuePair <float, Edge <NodeType> > edgeToCheck = nodesToSearch.Pop();
                NodeType edgeDestination = edgeToCheck.Value.End;
                lastNodeChecked = edgeDestination;

                float totalTraversalCost = edgeToCheck.Key;
                float totalSearchCost    = cost_search[edgeDestination];


                //Put this edge into the path tree.
                //Note that if it was already in the path,
                //    then a shorter route to it has already been found.
                if (!pathTree.ContainsKey(edgeDestination))
                {
                    pathTree.Add(edgeDestination, edgeToCheck.Value.Start);
                }

                //If a goal has been found, stop here.
                if (goal.IsValidEnd(edgeDestination))
                {
                    finalDestination = edgeDestination;
                    break;
                }

                //If we've searched too far, discard this edge.
                if (totalSearchCost >= maxPathLength)
                {
                    continue;
                }


                //Now process all the edges coming out of this node.

                connections.Clear();
                Graph.GetConnections(edgeDestination, connections);
                foreach (Edge <NodeType> connection in connections)
                {
                    //Get the total cost of traversing/searching to this node.
                    float edgeLength, heuristic;
                    CalcCosts(goal, connection, out edgeLength, out heuristic);
                    float costToEdgeEnd_search    = edgeLength + totalSearchCost,
                          costToEdgeEnd_traversal = edgeLength + heuristic + totalTraversalCost;

                    //Only check out this node if it's not too far away.
                    if (costToEdgeEnd_search <= maxPathLength)
                    {
                        //If this node hasn't been found yet, add it to the list of nodes to search.
                        if (!considered.Contains(connection.End))
                        {
                            //Add the edge to the search space.
                            considered.Add(connection.End);
                            cost_traversal.Add(connection.End, costToEdgeEnd_traversal);
                            cost_search.Add(connection.End, costToEdgeEnd_search);

                            nodesToSearch.Push(connection, costToEdgeEnd_traversal);
                        }
                        //If it HAS been found already, this must be a longer path.
                        else                         //if (traversalCostToEdgeEnd < costToMoveToNode[connection.End])
                        {
                            UnityEngine.Assertions.Assert.IsTrue(costToEdgeEnd_traversal >=
                                                                 cost_traversal[connection.End]);
                            //Update the path tree.
                            //CostToMoveToNode[connections[i].End] = tempCost;
                            //getToNodeSearchCost[connections[i].End] = tempSearchCost;
                            //PathTree[connections[i].End] = connections[i].Start;
                        }
                    }
                }
            }


            //Now generate the actual path.

            //If we didn't find a valid goal node, pick the closest thing to it.
            if (!finalDestination.HasValue)
            {
                if (!tryMyBest || pathTree.Count == 0)
                {
                    return(false);
                }

                //If we can use heuristics to search towards a goal,
                //    we can abuse those heuristics to find something close to the goal.
                if (goal.SpecificGoal.HasValue)
                {
                    OptionalVal <NodeType> bestEnd = new OptionalVal <NodeType>();
                    float bestHeuristic            = float.PositiveInfinity;

                    foreach (NodeType n in pathTree.Values)
                    {
                        float edgeLength, heuristic;
                        CalcCosts(goal, new Edge <NodeType>(n, goal.SpecificGoal.Value),
                                  out edgeLength, out heuristic);
                        if (heuristic < bestHeuristic)
                        {
                            bestEnd       = n;
                            bestHeuristic = heuristic;
                        }
                    }

                    finalDestination = bestEnd.Value;
                }
                //Otherwise, there's no way to know how close we are,
                //    so just use the last node we checked out.
                else
                {
                    finalDestination = lastNodeChecked;
                }
            }

            //Build the path using the path tree.
            outPath.Clear();
            NodeType counter = finalDestination.Value;

            while (!counter.Equals(start))
            {
                outPath.Add(counter);
                counter = pathTree[counter];
            }
            outPath.Reverse();

            return(goal.IsValidEnd(finalDestination.Value));
        }