public List <Action> GetShortestPath(State fromState, State toState) { var frontier = new PriorityQueue <float, SearchNode <State, Action> >(); var exploredSet = new HashSet <State>(); var frontierMap = new Dictionary <State, SearchNode <State, Action> >(); var startNode = new SearchNode <State, Action>(null, 0, 0, fromState, default(Action)); frontier.Enqueue(startNode, 0); frontierMap.Add(fromState, startNode); while (!frontier.IsEmpty) { var node = frontier.Dequeue(); frontierMap.Remove(node.state); if (node.state.Equals(toState)) { return(BuildSolution(node)); } exploredSet.Add(node.state); // expand node and add to frontier var expand = info.Expand(node.state); if (expand == null) { return(BuildSolution(node)); } foreach (var action in expand) { var child = info.ApplyAction(node.state, action); SearchNode <State, Action> frontierNode = null; var isNodeInFrontier = frontierMap.TryGetValue(child, out frontierNode); if (!exploredSet.Contains(child) && !isNodeInFrontier) { var searchNode = CreateSearchNode(node, action, child, toState); frontier.Enqueue(searchNode, searchNode.f); frontierMap.Add(child, searchNode); } else if (isNodeInFrontier) { var searchNode = CreateSearchNode(node, action, child, toState); if (frontierNode.f > searchNode.f) { frontier.Replace(frontierNode, frontierNode.f, searchNode.f); } } } } return(null); }
public List <Action> GetShortestPath(State fromState, State toState) { SimplePriorityQueue <SearchNode <State, Action>, float> frontier = new SimplePriorityQueue <SearchNode <State, Action>, float>(); HashSet <State> exploredSet = new HashSet <State>(); Dictionary <State, SearchNode <State, Action> > frontierMap = new Dictionary <State, SearchNode <State, Action> >(); SearchNode <State, Action> startNode = new SearchNode <State, Action>(null, 0, 0, fromState, default(Action)); frontier.Enqueue(startNode, 0); frontierMap.Add(fromState, startNode); while (true) { if (frontier.Count == 0) { return(null); } SearchNode <State, Action> node = frontier.Dequeue(); if (node.state.Equals(toState)) { return(BuildSolution(node)); } exploredSet.Add(node.state); // expand node and add to frontier foreach (Action action in info.Expand(node.state)) { State child = info.ApplyAction(node.state, action); SearchNode <State, Action> frontierNode = null; bool isNodeInFrontier = frontierMap.TryGetValue(child, out frontierNode); if (!exploredSet.Contains(child) && !isNodeInFrontier) { SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState); frontier.Enqueue(searchNode, searchNode.f); exploredSet.Add(child); } else if (isNodeInFrontier) { SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState); if (frontierNode.f > searchNode.f) { //frontier.Replace(frontierNode, frontierNode.f, searchNode.f); frontier.UpdatePriority(frontierNode, searchNode.f); } } } } }
public List <Action> GetShortestPath(State fromState, State toState) { #if UNITY_IPHONE PriorityQueue <Float, SearchNode <State, Action> > frontier = new PriorityQueue <Float, SearchNode <State, Action> >(); #else PriorityQueue <float, SearchNode <State, Action> > frontier = new PriorityQueue <float, SearchNode <State, Action> >(); #endif HashSet <State> exploredSet = new HashSet <State>(); Dictionary <State, SearchNode <State, Action> > frontierMap = new Dictionary <State, SearchNode <State, Action> >(); SearchNode <State, Action> startNode = new SearchNode <State, Action>(null, 0, 0, fromState, default(Action)); #if UNITY_IPHONE frontier.Enqueue(startNode, new Float(0)); #else frontier.Enqueue(startNode, 0); #endif frontierMap.Add(fromState, startNode); int shortCircuit = 0; while (!frontier.IsEmpty) { shortCircuit++; SearchNode <State, Action> node = frontier.Dequeue(); frontierMap.Remove(node.state); if (node.state.Equals(toState)) { return(BuildSolution(node)); } exploredSet.Add(node.state); // expand node and add to frontier foreach (Action action in info.Expand(node.state)) { State child = info.ApplyAction(node.state, action); SearchNode <State, Action> frontierNode = null; bool isNodeInFrontier = frontierMap.TryGetValue(child, out frontierNode); if (!exploredSet.Contains(child) && !isNodeInFrontier) { SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState); #if UNITY_IPHONE frontier.Enqueue(searchNode, new Float(searchNode.f)); #else frontier.Enqueue(searchNode, searchNode.f); #endif frontierMap.Add(child, searchNode); } else if (isNodeInFrontier) { SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState); if (frontierNode.f > searchNode.f) { #if UNITY_IPHONE frontier.Replace(frontierNode, new Float(frontierNode.f), new Float(searchNode.f)); #else frontier.Replace(frontierNode, frontierNode.f, searchNode.f); #endif // reset f on the frontier node to made it's new position in the priority queue frontierNode.f = searchNode.f; } } } if (shortCircuit > 3000) { DebugLog.Log("Short circuiting pathfinder, took more than 3000 tries"); break; } } return(null); }
public List <Action> GetShortestPath(State fromState, State toState) { #if UNITY_IPHONE PriorityQueue <Float, SearchNode <State, Action> > frontier = new PriorityQueue <Float, SearchNode <State, Action> >(); #else PriorityQueue <float, SearchNode <State, Action> > frontier = new PriorityQueue <float, SearchNode <State, Action> >(); #endif HashSet <State> exploredSet = new HashSet <State>(); Dictionary <State, SearchNode <State, Action> > frontierMap = new Dictionary <State, SearchNode <State, Action> >(); SearchNode <State, Action> startNode = new SearchNode <State, Action>(null, 0, 0, fromState, default(Action)); #if UNITY_IPHONE frontier.Enqueue(startNode, new Float(0)); #else frontier.Enqueue(startNode, 0); #endif frontierMap.Add(fromState, startNode); while (!frontier.IsEmpty) { SearchNode <State, Action> node = frontier.Dequeue(); frontierMap.Remove(node.state); if (node.state.Equals(toState)) { return(BuildSolution(node)); } exploredSet.Add(node.state); // dirty hack to prevent jumping overhead if (exploredSet.Count > 2048) { return(null); } // expand node and add to frontier List <Action> exp = info.Expand(node.state); for (int i = 0; i < exp.Count; i++) { Action action = exp[i]; State child = info.ApplyAction(node.state, action); SearchNode <State, Action> frontierNode = null; bool isNodeInFrontier = frontierMap.TryGetValue(child, out frontierNode); if (!exploredSet.Contains(child) && !isNodeInFrontier) { SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState); #if UNITY_IPHONE frontier.Enqueue(searchNode, new Float(searchNode.f)); #else frontier.Enqueue(searchNode, searchNode.f); #endif frontierMap.Add(child, searchNode); } else if (isNodeInFrontier) { SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState); if (frontierNode.f > searchNode.f) { #if UNITY_IPHONE frontier.Replace(frontierNode, new Float(frontierNode.f), new Float(searchNode.f)); #else frontier.Replace(frontierNode, frontierNode.f, searchNode.f); #endif } } } } return(null); }