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);
}