Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- private static List<PathNode> FindPath (Node start, Node target)
- {
- float startTime = Time.realtimeSinceStartup;
- //The nodes on the calculated path.
- List<PathNode> path = new List<PathNode>();
- //The nodes which have been navigated.
- List<Node> closed = new List<Node>();
- //The nodes to be evaluated.
- List<Node> open = new List<Node>();
- //Cost from start node to the current node.
- Dictionary<Node, float> g_cost = new Dictionary<Node, float>();
- //Total movement cost from a node to the target node.
- Dictionary<Node, float> f_cost = new Dictionary<Node, float>();
- //Add the starting node to the open set.
- open.Add(start);
- f_cost.Add(start, start.DistanceTo(target));
- g_cost.Add(start, 0f);
- //How many times the loop has been ran.
- int iterations = 0;
- Node previous = start;
- //While the open set isn't empty;
- //while there are nodes to evaluate.
- while (open.Count > 0)
- {
- Node current = GetNextNode(open, f_cost);
- path.Add(new PathNode(previous, current));
- previous = current;
- Debug.Log("i: " + iterations);
- if (current == null)
- {
- Debug.Log("No current node!");
- break;
- }
- open.Remove(current);
- closed.Add(current);
- if(current == target)
- {
- Debug.Log("Found complete path!");
- break;
- }
- foreach(Node n in current.neighbours)
- {
- if (closed.Contains(n))
- continue;
- if(!open.Contains(n))
- {
- open.Add(n);
- float g = n.DistanceTo(n);
- g_cost.Add(n, g);
- f_cost.Add(n, n.DistanceTo(target) + g);
- //parent.put(n,p)
- }
- else
- {
- float g = g_cost[current] + n.DistanceTo(n);
- if(g < g_cost[n])
- {
- g_cost.Add(n, g);
- f_cost.Add(n, n.DistanceTo(target) + g);
- //parent.put(n,p)
- }
- }
- }
- //Exit the loop if we have iterated too many times.
- iterations++;
- if (iterations > _maxIterations)
- {
- if (current != target)
- Debug.LogWarning("Was unable to find a complete path to the target! Iterations exceeded!");
- break;
- }
- }
- if(open.Count == 0)
- {
- Debug.Log("Failed to find complete path! List of open nodes is empty!");
- }
- float endTime = Time.realtimeSinceStartup;
- Debug.Log("Generated path in " + (endTime - startTime).ToString("F5") + "ms");
- return path;
- }
- private static Node GetNextNode (List<Node> openList, Dictionary<Node, float> f_cost)
- {
- Node lowest = null;
- float score = Mathf.Infinity;
- Debug.Log(openList.Count);
- foreach(Node n in openList)
- {
- float f = f_cost[n] + n.weight;
- if(score > f_cost[n] && n.isWalkable)
- {
- lowest = n;
- score = f;
- }
- }
- return lowest;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement