Created
August 30, 2016 23:45
-
-
Save keithcollins/307c3335308fea62db2731265ab44c06 to your computer and use it in GitHub Desktop.
A* pathfinding implementation for Unity 5, C#
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
using UnityEngine; | |
using System.Collections.Generic; | |
using System.Collections; | |
// This script is adapted from these, | |
// but has been heavily modified in some areas: | |
// http://www.redblobgames.com/pathfinding/a-star/implementation.html#csharp | |
// https://gist.github.com/DanBrooker/1f8855367ae4add40792 | |
// I'm continuing to optimize and change things here. I would not use this | |
// in production as it is. | |
// Note that Floor, Forest and Wall are somewhat arbitrary, | |
// but also represent three differnt types of tiles, which are | |
// all handled differently by A*. Floors are Passable, walls are not, | |
// and forests are passable but with a higher movement cost. | |
public enum TileType { | |
Floor = 1, | |
Forest = 5, | |
Wall = System.Int32.MaxValue | |
} | |
public class Location { | |
public readonly int x, y; | |
public Location(int x, int y) { | |
this.x = x; | |
this.y = y; | |
} | |
public Location(float x, float y) { | |
this.x = Mathf.FloorToInt(x); | |
this.y = Mathf.FloorToInt(y); | |
} | |
public Location(Vector3 position) { | |
this.x = Mathf.RoundToInt(position.x); | |
this.y = Mathf.RoundToInt(position.y); | |
} | |
public Vector3 vector3() { | |
return new Vector3 (this.x, this.y, 0f); | |
} | |
public override bool Equals(System.Object obj) { | |
Location loc = obj as Location; | |
return this.x == loc.x && this.y == loc.y; | |
} | |
// This is creating collisions. How do I solve this? | |
public override int GetHashCode() { | |
return (x * 597) ^ (y * 1173); | |
} | |
} | |
public class SquareGrid { | |
// DIRS is directions | |
// I added diagonals to this but noticed it can create problems-- | |
// like the path will go through obstacles that are diagonal from each other. | |
public static readonly Location[] DIRS = new [] { | |
new Location(1, 0), // to right of tile | |
new Location(0, -1), // below tile | |
new Location(-1, 0), // to left of tile | |
new Location(0, 1), // above tile | |
new Location(1, 1), // diagonal top right | |
new Location(-1, 1), // diagonal top left | |
new Location(1, -1), // diagonal bottom right | |
new Location(-1, -1) // diagonal bottom left | |
}; | |
// The x and y here represent the grid's starting point, 0,0. | |
// And width and height are how many units wide and high the grid is. | |
// See how I use this in TileManager.cs to see how you can keep | |
// your Unity GameObjects in sync with this abstracted grid. | |
public SquareGrid(int x, int y, int width, int height) { | |
this.x = x; | |
this.y = y; | |
this.width = width; | |
this.height = height; | |
} | |
public int x, y, width, height; | |
// This is a 2d array that stores each tile's type and movement cost | |
// using the TileType enum defined above | |
public TileType[,] tiles = new TileType[width,height]; | |
// Check if a location is within the bounds of this grid. | |
public bool InBounds(Location id) { | |
return (x <= id.x) && (id.x < width) && (y <= id.y) && (id.y < height); | |
} | |
// Everything that isn't a Wall is Passable | |
public bool Passable(Location id) { | |
return (int)tiles[id.x,id.y] < System.Int32.MaxValue; | |
} | |
// If the heuristic = 2f, the movement is diagonal | |
public float Cost(Location a, Location b) { | |
if (AStarSearch.Heuristic(a,b) == 2f) { | |
return (float)(int)tiles[b.x,b.z] * Mathf.Sqrt(2f); | |
} | |
return (float)(int)tiles[b.x,b.z]; | |
} | |
// Check the tiles that are next to, above, below, or diagonal to | |
// this tile, and return them if they're within the game bounds and passable | |
public IEnumerable<Location> Neighbors(Location id) { | |
foreach (var dir in DIRS) { | |
Location next = new Location(id.x + dir.x, id.y + dir.y); | |
if (InBounds(next) && Passable(next)) { | |
yield return next; | |
} | |
} | |
} | |
} | |
public class PriorityQueue<T> { | |
// From Red Blob: I'm using an unsorted array for this example, but ideally this | |
// would be a binary heap. Find a binary heap class: | |
// * https://bitbucket.org/BlueRaja/high-speed-priority-queue-for-c/wiki/Home | |
// * http://visualstudiomagazine.com/articles/2012/11/01/priority-queues-with-c.aspx | |
// * http://xfleury.github.io/graphsearch.html | |
// * http://stackoverflow.com/questions/102398/priority-queue-in-net | |
private List<KeyValuePair<T, float>> elements = new List<KeyValuePair<T, float>>(); | |
public int Count { | |
get { return elements.Count; } | |
} | |
public void Enqueue(T item, float priority) { | |
elements.Add(new KeyValuePair<T, float>(item,priority)); | |
} | |
// Returns the Location that has the lowest priority | |
public T Dequeue() { | |
int bestIndex = 0; | |
for (int i = 0; i < elements.Count; i++) { | |
if (elements[i].Value < elements[bestIndex].Value) { | |
bestIndex = i; | |
} | |
} | |
T bestItem = elements[bestIndex].Key; | |
elements.RemoveAt(bestIndex); | |
return bestItem; | |
} | |
} | |
// Now that all of our classes are in place, we get get | |
// down to the business of actually finding a path. | |
public class AStarSearch { | |
// Someone suggested making this a 2d field. | |
// That will be worth looking at if you run into performance issues. | |
public Dictionary<Location, Location> cameFrom = new Dictionary<Location, Location>(); | |
public Dictionary<Location, float> costSoFar = new Dictionary<Location, float>(); | |
private Location start; | |
private Location goal; | |
static public float Heuristic(Location a, Location b) { | |
return Mathf.Abs(a.x - b.x) + Mathf.Abs(a.y - b.y); | |
} | |
// Conduct the A* search | |
public AStarSearch(SquareGrid graph, Location start, Location goal) { | |
// start is current sprite Location | |
this.start = start; | |
// goal is sprite destination eg tile user clicked on | |
this.goal = goal; | |
// add the cross product of the start to goal and tile to goal vectors | |
// Vector3 startToGoalV = Vector3.Cross(start.vector3,goal.vector3); | |
// Location startToGoal = new Location(startToGoalV); | |
// Vector3 neighborToGoalV = Vector3.Cross(neighbor.vector3,goal.vector3); | |
// frontier is a List of key-value pairs: | |
// Location, (float) priority | |
var frontier = new PriorityQueue<Location>(); | |
// Add the starting location to the frontier with a priority of 0 | |
frontier.Enqueue(start, 0f); | |
cameFrom.Add(start, start); // is set to start, None in example | |
costSoFar.Add(start, 0f); | |
while (frontier.Count > 0f) { | |
// Get the Location from the frontier that has the lowest | |
// priority, then remove that Location from the frontier | |
Location current = frontier.Dequeue(); | |
// If we're at the goal Location, stop looking. | |
if (current.Equals(goal)) break; | |
// Neighbors will return a List of valid tile Locations | |
// that are next to, diagonal to, above or below current | |
foreach (var neighbor in graph.Neighbors(current)) { | |
// If neighbor is diagonal to current, graph.Cost(current,neighbor) | |
// will return Sqrt(2). Otherwise it will return only the cost of | |
// the neighbor, which depends on its type, as set in the TileType enum. | |
// So if this is a normal floor tile (1) and it's neighbor is an | |
// adjacent (not diagonal) floor tile (1), newCost will be 2, | |
// or if the neighbor is diagonal, 1+Sqrt(2). And that will be the | |
// value assigned to costSoFar[neighbor] below. | |
float newCost = costSoFar[current] + graph.Cost(current, neighbor); | |
// If there's no cost assigned to the neighbor yet, or if the new | |
// cost is lower than the assigned one, add newCost for this neighbor | |
if (!costSoFar.ContainsKey(neighbor) || newCost < costSoFar[neighbor]) { | |
// If we're replacing the previous cost, remove it | |
if (costSoFar.ContainsKey(neighbor)) { | |
costSoFar.Remove(neighbor); | |
cameFrom.Remove(neighbor); | |
} | |
costSoFar.Add(neighbor, newCost); | |
cameFrom.Add(neighbor, current); | |
float priority = newCost + Heuristic(neighbor, goal); | |
frontier.Enqueue(neighbor, priority); | |
} | |
} | |
} | |
} | |
// Return a List of Locations representing the found path | |
public List<Location> FindPath() { | |
List<Location> path = new List<Location>(); | |
Location current = goal; | |
// path.Add(current); | |
while (!current.Equals(start)) { | |
if (!cameFrom.ContainsKey(current)) { | |
MonoBehaviour.print("cameFrom does not contain current."); | |
return new List<Location>(); | |
} | |
path.Add(current); | |
current = cameFrom[current]; | |
} | |
// path.Add(start); | |
path.Reverse(); | |
return path; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment