Path.cs

using System;
using System.Collections.Generic;
using UnityEngine;
#if UNITY_EDITOR
using UnityEditor;
#endif
namespace ca.HenrySoftware.Deko
{
	public class Path : Singleton<Path>
	{
		public bool Draw = true;
		public int Limit = 10;
		private PathNode[] _map;
		private PathNode[] _closed;
		private PathQueue _open;
		private PathNode _from;
		private PathNode _to;
		private PathNode _current;
		private GUIStyle _style = new GUIStyle();
		private void Start()
		{
			_style.normal.textColor = Color.red;
			_style.fontSize = 16;
			SetupMap();
		}
		public void SetupMap()
		{
			int count = Generator.Instance.Width * Generator.Instance.Height;
			_map = new PathNode[count];
			_closed = new PathNode[count];
			_open = new PathQueue(count);
			for (int y = 0; y < Generator.Instance.Height; y++)
			{
				for (int x = 0; x < Generator.Instance.Width; x++)
				{
					Vector2 p = new Vector2(x, y);
					bool walkable = true;
					int back = TileAt(p, MapLayer.Background);
					bool isFloor = Tile.IsFloor(back);
					if (!isFloor) walkable = false;
					int fore = TileAt(p, MapLayer.Foreground);
					bool isWall = Tile.IsWall(fore);
					if (isWall) walkable = false;
					_map[TileIndex(x, y)] = new PathNode(x, y, walkable, null);
				}
			}
		}
		public int TileAt(Vector2 p, MapLayer layer)
		{
			return Generator.Instance.Map.GetTileIdAtPosition(TilePosition(p), (int)layer);
		}
		public int TileIndex(Vector2 p)
		{
			return TileIndex((int)p.x, (int)p.y);
		}
		public int TileIndex(int x, int y)
		{
			return y * Generator.Instance.Width + x;
		}
		public Vector2 TilePosition(int index)
		{
			float y = index / Generator.Instance.Width;
			float x = index - Generator.Instance.Width * y;
			return new Vector2(x, y);
		}
		public Vector2 TilePosition(Vector2 p)
		{
			return TilePosition((int)p.x, (int)p.y);
		}
		public Vector2 TilePosition(int x, int y)
		{
			return new Vector2(Constants.TileSize * x, Constants.TileSize * y);
		}
		public bool InsideMap(Vector2 p)
		{
			return InsideMap((int)p.x, (int)p.y);
		}
		public bool InsideMap(int x, int y)
		{
			bool inside = false;
			if ((x >= 0) && (y >= 0) && (x < Generator.Instance.Width) && (y < Generator.Instance.Height))
			{
				inside = true;
			}
			return inside;
		}
		public bool InsideEdge(Vector2 p)
		{
			return InsideEdge((int)p.x, (int)p.y);
		}
		public bool InsideEdge(int x, int y)
		{
			bool inside = false;
			if ((x >= 1) && (y >= 1) && (x < Generator.Instance.Width - 1) && (y < Generator.Instance.Height - 1))
			{
				inside = true;
			}
			return inside;
		}
		public List<Vector2> FindPath(Vector2 from, Vector2 to)
		{
			List<Vector2> path = new List<Vector2>();
			SetFromAndTo(from, to);
			if (_from != null)
			{
				if (_to == null)
				{
					return path;
				}
				bool foundTo = to.Equals(_to);
				Array.Clear(_closed, 0, _closed.Length);
				_open.Clear();
				_map[_from.TileIndex].Parent = null;
				_open.Enqueue(_from);
				while (_open.Count > 0)
				{
					_current = _open.Dequeue();
					_closed[_current.TileIndex] = _current;
					if (_current.TileIndex == _to.TileIndex) break;
					NeighbourCheck();
				}
				while (_current.Parent != null)
				{
					path.Add(new Vector2(_current.X, _current.Y));
					_current = _current.Parent;
				}
				Vector2 fromPoint = new Vector2(_from.X, _from.Y);
				path.Add(fromPoint);
				path.Reverse();
				if (foundTo)
				{
					path.Add(new Vector2(_to.X, _to.Y));
				}
				if (foundTo && (path.Count > 2))
				{
					Vector2 last1 = path[path.Count - 1];
					Vector2 last2 = path[path.Count - 2];
					Vector2 last3 = path[path.Count - 3];
					// if the third last is closest then the second last to last, remove second last
					if (Vector2.Distance(last1, last3) < Vector2.Distance(last3, last2))
					{
						path.RemoveAt(path.Count - 2);
					}
					Vector2 first = path[0];
					Vector2 second = path[1];
					// if the second is closer to from than it is to first, remove first
					if (Vector2.Distance(second, fromPoint) < Vector2.Distance(first, second))
					{
						path.RemoveAt(0);
					}
				}
			}
			return path;
		}
		private void SetFromAndTo(Vector2 from, Vector2 to)
		{
			_from = FindClose(from, to);
			_to = FindClose(to, from);
		}
		private PathNode FindClose(Vector2 from, Vector2 to)
		{
			PathNode closestNode = null;
			PathNode fromNode = _map[TileIndex(from)];
			PathNode toNode = _map[TileIndex(to)];
			if (fromNode.Walkable && fromNode.Reachable)
			{
				return fromNode;
			}
			else
			{
				int radius = 1;
				List<PathNode> nodes = new List<PathNode>();
				while ((nodes.Count < 1) && (radius < Limit))
				{
					nodes = FindClosestCheck((int)from.x, (int)from.y, radius);
					radius++;
				}
				if (nodes.Count > 0)
				{
					float closest = float.MaxValue;
					Vector2 test0 = new Vector2(toNode.X, toNode.Y);
					foreach (PathNode node in nodes)
					{
						Vector2 test1 = new Vector2(node.X, node.Y);
						float distance = Vector2.Distance(test0, test1);
						if (distance < closest)
						{
							closest = distance;
							closestNode = node;
						}
					}
				}
			}
			return closestNode;
		}
		private List<PathNode> FindClosestCheck(int x, int y, int r)
		{
			List<PathNode> nodes = new List<PathNode>();
			for (int yy = y - r; yy <= y + r; yy++)
			{
				for (int xx = x - r; xx <= x + r; xx++)
				{
					if (((yy == y - r) || (xx == x - r) || (yy == y + r) || (xx == x + r)) && InsideMap(xx, yy))
					{
						PathNode node = _map[TileIndex(xx, yy)];
						if (node.Walkable && node.Reachable)
						{
							nodes.Add(node);
						}
					}
				}
			}
			return nodes;
		}
		public void ReachableClear()
		{
			foreach (PathNode node in _map)
			{
				node.Reachable = false;
			}
		}
		public void ReachableFrom(Vector2 p)
		{
			ReachableFrom((int)p.x, (int)p.y);
		}
		public void ReachableFrom(int x, int y)
		{
			Stack<Vector2> points = new Stack<Vector2>();
			points.Push(new Vector2(x, y));
			while (points.Count > 0)
			{
				PathNode current = _map[TileIndex(points.Pop())];
				current.Reachable = true;
				for (int yy = current.Y - 1; yy <= current.Y + 1; yy++)
				{
					for (int xx = current.X - 1; xx <= current.X + 1; xx++)
					{
						if ((yy != current.Y || xx != current.X) && InsideMap(xx, yy))
						{
							PathNode node = _map[TileIndex(xx, yy)];
							if (!node.Reachable && node.Walkable)
							{
								points.Push(new Vector2(xx, yy));
							}
						}
					}
				}
			}
		}
		private void NeighbourCheck()
		{
			for (int y = _current.Y - 1; y <= _current.Y + 1; y++)
			{
				for (int x = _current.X - 1; x <= _current.X + 1; x++)
				{
					if ((y != _current.Y || x != _current.X) && InsideMap(x, y))
					{
						PathNode node = _map[TileIndex(x, y)];
						if (node.Walkable && !IsClosed(node))
						{
							if (!_open.Contains(node))
							{
								PathNode addNode = node;
								addNode.H = GetHeuristics(node);
								addNode.G = GetMovementCost(node) + _current.G;
								addNode.F = addNode.H + addNode.G;
								addNode.Parent = _current;
								_open.Enqueue(addNode);
							}
							else
							{
								if (_current.G + GetMovementCost(node) < node.G)
								{
									node.Parent = _current;
									node.G = GetMovementCost(node) + _current.G;
									node.F = node.H + node.G;
									node.Parent = _current;
									_open.CascadeUp(node);
								}
							}
						}
					}
				}
			}
		}
		private bool IsClosed(PathNode node)
		{
			return (_closed[node.TileIndex] != null) ? true : false;
		}
		private int GetMovementCost(PathNode node)
		{
			return ((_current.X != node.X) && (_current.Y != node.Y)) ? 14 : 10;
		}
		private int GetHeuristics(PathNode node)
		{
			return GetHeuristics(node, _to);
		}
		private int GetHeuristics(PathNode from, PathNode to)
		{
			return (int)(Mathf.Abs(from.X - to.X) * 10f) + (int)(Mathf.Abs(from.Y - to.Y) * (10f));
		}
#if UNITY_EDITOR
		private void OnDrawGizmosSelected()
		{
			if (!Draw) return;
			if (_map != null)
			{
				const float tileSize = Constants.TileSize;
				const float half = tileSize * .5f;
				for (int y = 0; y < Generator.Instance.Height; y++)
				{
					for (int x = 0; x < Generator.Instance.Width; x++)
					{
						Vector2 p = TilePosition(x, y);
						PathNode node = _map[TileIndex(x, y)];
						Color color = Color.white;
						if (_open.Contains(node) || IsClosed(node))
						{
							//Vector2 fp = new Vector2(p.x - half, p.y);
							//Handles.Label(fp, "F: " + node.F.ToString(), _style);
							//Vector2 gp = new Vector2(p.x - half, p.y + half);
							//Handles.Label(gp, "G: " + node.G.ToString(), _style);
							//Vector2 hp = new Vector2(p.x, p.y + half);
							//Handles.Label(hp, "H: " + node.H.ToString(), _style);
							if (node.Parent != null)
							{
								Vector2 parent = TilePosition(new Vector2(node.Parent.X, node.Parent.Y));
								ArrowGizmo(p, parent, Color.black);
							}
						}
						if (_open.Contains(node))
							color = Utility.Alpha(Constants.ErrorBlue, .5f);
						else if (IsClosed(node))
							color = Utility.Alpha(Constants.ErrorYellow, .5f);
						else if (node.Walkable && node.Reachable)
							color = Utility.Alpha(Constants.ErrorGreen, .5f);
						else
							color = Utility.Alpha(Constants.ErrorRed, .5f);
						Gizmos.color = color;
						Gizmos.DrawCube(new Vector3(p.x, p.y, -(half)), new Vector3(tileSize, tileSize, tileSize));
					}
				}
			}
		}
		private void ArrowGizmo(Vector2 child, Vector2 parent, Color c)
		{
			const float length = .05f;
			const float angle = 20f;
			Gizmos.color = c;
			Vector2 d = (parent - child) * .5f;
			Gizmos.DrawRay(child, d);
			Vector3 right = Quaternion.LookRotation(d, Vector3.right) * Quaternion.Euler(180 + angle, 0f, 0f) * Vector3.forward;
			Vector3 left = Quaternion.LookRotation(d, Vector3.right) * Quaternion.Euler(180 - angle, 0f, 0f) * Vector3.forward;
			Gizmos.DrawRay(child + d, right * length);
			Gizmos.DrawRay(child + d, left * length);
		}
#endif
	}
}

PathNode.cs

using System;
namespace ca.HenrySoftware.Deko
{
	public class PathNode : IComparable<PathNode>
	{
		public int X;
		public int Y;
		public bool Walkable = true;
		public bool Reachable = false;
		public PathNode Parent = null;
		public int TileIndex = 0;
		public int SortedIndex = 0;
		public int F = 0; // f = g + h
		public int G = 0; // real cost from start to here
		public int H = 0; // estimated cost from here to goal
		public float Cost = 1f;
		public PathNode(int x, int y, bool walkable, PathNode parent)
		{
			X = x;
			Y = y;
			TileIndex = Path.Instance.TileIndex(x, y);
			Walkable = walkable;
			Parent = parent;
		}
		public int CompareTo(PathNode b)
		{
			int compare = F.CompareTo(b.F);
			if (compare == 0) compare = H.CompareTo(b.H);
			return compare;
		}
		public override string ToString()
		{
			return TileIndex + " (" + X + ", " + Y + ") F:" + F + " H:" + H + " W:" + Walkable + " R:" + Reachable;
		}
	}
}

PathQueue.cs

namespace ca.HenrySoftware.Deko
{
	public sealed class PathQueue
	{
		public int Count { get; set; }
		private readonly PathNode[] _nodes;
		public PathQueue(int count)
		{
			Count = 0;
			_nodes = new PathNode[count];
		}
		public void Clear()
		{
			for (int i = 1; i < _nodes.Length; i++)
				_nodes[i] = null;
			Count = 0;
		}
		public bool Contains(PathNode node)
		{
			return _nodes[node.SortedIndex] == node;
		}
		private void Swap(PathNode node1, PathNode node2)
		{
			_nodes[node1.SortedIndex] = node2;
			_nodes[node2.SortedIndex] = node1;
			int temp = node1.SortedIndex;
			node1.SortedIndex = node2.SortedIndex;
			node2.SortedIndex = temp;
		}
		public void Enqueue(PathNode node)
		{
			Count++;
			_nodes[Count] = node;
			node.SortedIndex = Count;
			CascadeUp(Count);
		}
		public PathNode Dequeue()
		{
			PathNode node = _nodes[1];
			if (Count == 1)
			{
				Count--;
			}
			else
			{
				_nodes[1] = _nodes[Count];
				_nodes[1].SortedIndex = 1;
				Count--;
				CascadeDown(1);
			}
			return node;
		}
		public void CascadeUp(PathNode node)
		{
			CascadeUp(node.SortedIndex);
		}
		private void CascadeUp(int v)
		{
			while (v != 1)
			{
				PathNode node = _nodes[v];
				PathNode parent = _nodes[v / 2];
				if (node.CompareTo(parent) < 0)
				{
					Swap(node, parent);
					v = v / 2;
				}
				else break;
			}
		}
		private void CascadeDown(int v)
		{
			while (true)
			{
				int u = v;
				int rightIndex = 2 * u;
				int leftIndex = 2 * u + 1;
				if (leftIndex <= Count) // both
				{
					if (_nodes[u].CompareTo(_nodes[rightIndex]) >= 0)
						v = rightIndex;
					if (_nodes[v].CompareTo(_nodes[leftIndex]) >= 0)
						v = leftIndex;
				}
				else if (rightIndex <= Count) // one
				{
					PathNode rightNode = _nodes[rightIndex];
					if (_nodes[u].CompareTo(rightNode) >= 0)
						v = rightIndex;
				}
				if (u == v) // neither
					break;
				Swap(_nodes[u], _nodes[v]);
			}
		}
	}
}