Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- using UnityEngine;
- using System.Collections;
- using System.Collections.Generic;
- namespace Pathfinding
- {
- public class Dijkstras{
- private Dictionary<INode, Node> dictionary = new Dictionary<INode, Node>();
- private INode seed;
- public Dijkstras(INode start, INode end = null)
- {
- seed = start;
- Node snode = new Node(seed, 0);
- dictionary.Add(seed, snode);
- Queue<INode> queue = new Queue<INode>();
- queue.Enqueue(seed);
- int overflow = 0;
- float maxDistanceToCover = float.MaxValue;
- while(queue.Count != 0 && overflow++ < 100000)
- {
- INode next = queue.Dequeue();
- Debug.Assert(dictionary.ContainsKey(next));
- Node nextNode = dictionary[next];
- if(nextNode == end && end != null)
- {
- maxDistanceToCover = nextNode.weightSoFar;
- }
- if (nextNode.weightSoFar > maxDistanceToCover)
- continue;
- List<INode> allAdjacents = next.AllAdjacents();
- foreach (INode adj in allAdjacents)
- {
- if(dictionary.ContainsKey(adj))
- {
- Node adjNode = dictionary[adj];
- if (adjNode.weightSoFar > next.Weight() + nextNode.weightSoFar)
- {
- adjNode.Link(nextNode, next.Weight() + nextNode.weightSoFar);
- queue.Enqueue(adj);
- }
- }
- else
- {
- if (adj.Passable())
- {
- Node adjNode = new Node(adj);
- adjNode.Link(nextNode, nextNode.weightSoFar);
- dictionary.Add(adj, adjNode);
- queue.Enqueue(adj);
- }
- }
- }
- }
- }
- public bool PathExistsTo(INode to)
- {
- return to != null && dictionary.ContainsKey(to);
- }
- public List<Node> PathFromSeed(INode to)
- {
- List<Node> result = new List<Node>();
- if(!dictionary.ContainsKey(to) || !PathExistsTo(to))
- return result;
- Node next = dictionary[to];
- while(next != null && next != seed)
- {
- result.Add(next);
- next = next.previous;
- }
- result.Reverse();
- return result;
- }
- public void PrintPathTo(INode end)
- {
- string s = "";
- foreach(Node n in PathFromSeed(end))
- {
- s += n.inode.Name() + "\n";
- }
- Debug.Log(s);
- }
- }
- public interface INode
- {
- List<INode> AllAdjacents();
- float Weight();
- bool Passable();
- Vector3 Location();
- string Name();
- }
- public class Node
- {
- public INode inode { get; private set; }
- public float weightSoFar { get; private set; }
- public Node previous { get; private set; }
- public Node(INode inode, float weightSoFar)
- {
- this.inode = inode;
- this.weightSoFar = weightSoFar;
- }
- public Node(INode inode)
- {
- this.inode = inode;
- this.weightSoFar = float.MaxValue;
- }
- public void Link(Node previous, float weight)
- {
- this.previous = previous;
- weightSoFar = weightSoFar;
- }
- }
- }
Add Comment
Please, Sign In to add comment