Skip to content

Instantly share code, notes, and snippets.

@keithcollins
Created August 30, 2016 23:45
Show Gist options
  • Save keithcollins/307c3335308fea62db2731265ab44c06 to your computer and use it in GitHub Desktop.
Save keithcollins/307c3335308fea62db2731265ab44c06 to your computer and use it in GitHub Desktop.
A* pathfinding implementation for Unity 5, C#
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