protected override void ThreadFunction()
{
PriorityQueue<Node> open = new PriorityQueue<Node> (PathPlanningDataStructures.graph.Size ());
Dictionary<Node, Node> came_from = new Dictionary<Node, Node> ();
Dictionary<Node, float> cost_so_far = new Dictionary<Node, float> ();
came_from.Add (startNode, null);
cost_so_far.Add (startNode, 0);
open.queue (PathPlanningDataStructures.heuristic.Estimate (startNode, useItemReduction),
startNode);
while (open.getSize() > 0) {
Node current = open.dequeue ();
if (current.Equals (PathPlanningDataStructures.graph.endNode) ||
Node.distanceBetweenNodes (startNode, current) >= pathLength) {
if (came_from[current] == null) {
came_from[current] = startNode;
}
destinationNode = current;
break;
}
foreach (Node n in current.neighbors) {
float graph_cost = cost_so_far [current] + Node.distanceBetweenNodes (current, n);
if ((cost_so_far.ContainsKey (n) == false || graph_cost < cost_so_far [n]) &&
closedNodes.Contains(n.position) == false) {
cost_so_far [n] = graph_cost;
float priority = graph_cost +
PathPlanningDataStructures.heuristic.Estimate (n, useItemReduction);
open.queue (priority, n);
came_from [n] = current;
}
}
}
//Put nodes of the path into the list
pathWayPoints = new List<Vector3> ();
Node currentNode = destinationNode;
pathWayPoints.Add (currentNode.position);
lock (PathPlanningDataStructures.globalLock) {
PathPlanningDataStructures.nodeToCount [currentNode.position] += 1;
}
while (currentNode.Equals(startNode) == false) {
currentNode = came_from [currentNode];
pathWayPoints.Add (currentNode.position);
lock (PathPlanningDataStructures.globalLock) {
PathPlanningDataStructures.nodeToCount [currentNode.position] += 1;
}
}
pathWayPoints.Reverse ();
}
