teleias

Dijkstra's [Easily Implementable Unity3D C#]

Dec 29th, 2015
96
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 2.76 KB | None | 0 0
  1. using UnityEngine;
  2. using System.Collections;
  3. using System.Collections.Generic;
  4. namespace Pathfinding
  5. {
  6.     public class Dijkstras{
  7.         private Dictionary<INode, Node> dictionary = new Dictionary<INode, Node>();
  8.         private INode seed;
  9.         public Dijkstras(INode start, INode end = null)
  10.         {
  11.             seed = start;
  12.             Node snode = new Node(seed, 0);
  13.             dictionary.Add(seed, snode);
  14.  
  15.             Queue<INode> queue = new Queue<INode>();
  16.             queue.Enqueue(seed);
  17.             int overflow = 0;
  18.  
  19.             float maxDistanceToCover = float.MaxValue;
  20.  
  21.             while(queue.Count != 0 && overflow++ < 100000)
  22.             {
  23.                 INode next = queue.Dequeue();
  24.                 Debug.Assert(dictionary.ContainsKey(next));
  25.                 Node nextNode = dictionary[next];
  26.  
  27.                 if(nextNode == end && end != null)
  28.                 {
  29.                     maxDistanceToCover = nextNode.weightSoFar;
  30.                 }
  31.  
  32.                 if (nextNode.weightSoFar > maxDistanceToCover)
  33.                     continue;
  34.  
  35.                 List<INode> allAdjacents = next.AllAdjacents();
  36.  
  37.                 foreach (INode adj in allAdjacents)
  38.                 {
  39.                     if(dictionary.ContainsKey(adj))
  40.                     {
  41.                         Node adjNode = dictionary[adj];
  42.                         if (adjNode.weightSoFar > next.Weight() + nextNode.weightSoFar)
  43.                         {
  44.                             adjNode.Link(nextNode, next.Weight() + nextNode.weightSoFar);
  45.                             queue.Enqueue(adj);
  46.                         }
  47.                     }
  48.                     else
  49.                     {
  50.                         if (adj.Passable())
  51.                         {
  52.                             Node adjNode = new Node(adj);
  53.                             adjNode.Link(nextNode, nextNode.weightSoFar);
  54.                             dictionary.Add(adj, adjNode);
  55.                             queue.Enqueue(adj);
  56.                         }
  57.                     }
  58.                 }
  59.             }
  60.         }
  61.         public bool PathExistsTo(INode to)
  62.         {
  63.             return to != null && dictionary.ContainsKey(to);
  64.         }
  65.         public List<Node> PathFromSeed(INode to)
  66.         {
  67.             List<Node> result = new List<Node>();
  68.             if(!dictionary.ContainsKey(to) || !PathExistsTo(to))
  69.                 return result;
  70.  
  71.             Node next = dictionary[to];
  72.             while(next != null && next != seed)
  73.             {
  74.                 result.Add(next);
  75.                 next = next.previous;
  76.             }
  77.             result.Reverse();
  78.             return result;
  79.         }
  80.         public void PrintPathTo(INode end)
  81.         {
  82.             string s = "";
  83.             foreach(Node n in PathFromSeed(end))
  84.             {
  85.                 s += n.inode.Name() + "\n";
  86.             }
  87.             Debug.Log(s);
  88.         }
  89.     }
  90.     public interface INode
  91.     {
  92.         List<INode> AllAdjacents();
  93.         float Weight();
  94.         bool Passable();
  95.         Vector3 Location();
  96.         string Name();
  97.     }
  98.     public class Node
  99.     {
  100.         public INode inode { get; private set; }
  101.         public float weightSoFar { get; private set; }
  102.         public Node previous { get; private set; }
  103.         public Node(INode inode, float weightSoFar)
  104.         {
  105.             this.inode = inode;
  106.             this.weightSoFar = weightSoFar;
  107.         }
  108.         public Node(INode inode)
  109.         {
  110.             this.inode = inode;
  111.             this.weightSoFar = float.MaxValue;
  112.         }
  113.         public void Link(Node previous, float weight)
  114.         {
  115.             this.previous = previous;
  116.             weightSoFar = weightSoFar;
  117.         }
  118.     }
  119. }
Add Comment
Please, Sign In to add comment