Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
A* pathfinding implementation for Unity3D, using raycasting and colliders for obstacles
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
public static class AStar {
// A* pathfinding
/// <summary>
/// Returns estimated cost of moving from point A to point B.
/// Instead of using just basic distance between two points,
/// you can multiply it by some terrain effects modifier to calculate
/// easier/harder movement across terrain, comes in handy when making
/// strategy games with roads, forests, hills, swamps, you name it.
/// </summary>
/// <returns>Estimated cost of moving from point A to point B.</returns>
/// <param name="source">Source</param>
/// <param name="destination">Destination</param>
static int HeuristicCostEstimate (Vector3 source, Vector3 destination) {
return (int)Vector3.Distance (source, destination);
}
/// <summary>
/// Returns a list of unobstructed neighbouring nodes to a given point. Expand the list if you're calculating for heights
/// or want to get non-cross (more than 4 directions) movement without using SmoothPath() method.
/// </summary>
/// <returns>Neighbouring nodes list</returns>
/// <param name="point">Position</param>
static List<Vector3> NeighbouringNodes (Vector3 point) {
Vector3[] neighbours = new Vector3[4] {
new Vector3 (point.x + 1, point.y, point.z),
new Vector3 (point.x - 1, point.y, point.z),
new Vector3 (point.x, point.y, point.z + 1),
new Vector3 (point.x, point.y, point.z - 1)
};
List<Vector3> returnSet = new List<Vector3> ();
foreach (Vector3 n in neighbours) {
if (IsPointAnObstruction(n, point)) continue; // don't add obstructed points to returned set
returnSet.Add(n);
}
return returnSet;
}
/// <summary>
/// Determines if a point contains an obstruction to movement (this includes checking for game area borders).
/// Obstruction check is done by raycasting, make sure you have your obstacles on "Obstruction" layer
/// or remove layer check from spherecast call. You can also adjust spherecast radius value if your character scale is different.
/// </summary>
/// <returns><c>true</c> if is point contains an obstruction to movement; otherwise, <c>false</c>.</returns>
/// <param name="point">Destination point</param>
/// <param name="current">Current position</param>
static bool IsPointAnObstruction (Vector3 point, Vector3 current) {
// check for game area borders or otherwise limit the area of path search,
// this is important! can be a severe performance hit!
if (point.x < 0 || point.x > 20 || point.z < 0 || point.z > 20) return true;
Ray ray = new Ray(current, (point - current).normalized);
if (Physics.SphereCast(ray, 0.49f, Vector3.Distance(current, point), LayerMask.NameToLayer("Obstruction"))) return true;
return false;
}
/// <summary>
/// The actual A* pathfinding implementation. You don't have to change anything in here, it just works.
/// Will return <c>null</c> if there's no path.
/// </summary>
/// <returns>List of waypoints or <c>null</c> if path not found.</returns>
/// <param name="source">Source</param>
/// <param name="destination">Destination</param>
public static List<Vector3> FindPath(Vector3 source, Vector3 destination) {
List<Vector3> closedSet = new List<Vector3> ();
List<Vector3> openSet = new List<Vector3> () { source };
Dictionary <Vector3, Vector3> cameFrom = new Dictionary <Vector3, Vector3> ();
Dictionary <Vector3, int> gScores = new Dictionary <Vector3, int> ();
Dictionary <Vector3, int> hScores = new Dictionary <Vector3, int> ();
Dictionary <Vector3, int> fScores = new Dictionary <Vector3, int> ();
gScores.Add (source, 0);
hScores.Add (source, HeuristicCostEstimate (source, destination));
fScores.Add (source, 0 + HeuristicCostEstimate (source, destination)); // g[0] + h[0]
while (openSet.Count > 0) {
Vector3 current = Vector3.zero;
int minimumFScore = int.MaxValue;
foreach (Vector3 currentValue in openSet) {
int currentFScore = (int)fScores[currentValue];
minimumFScore = Mathf.Min (currentFScore, minimumFScore);
if (minimumFScore == currentFScore)
current = currentValue;
}
if (current == destination) {
Vector3 currentValue = current;
List<Vector3> returnValue = new List<Vector3> () { currentValue };
while (cameFrom.ContainsKey(currentValue)) {
currentValue = cameFrom[currentValue];
returnValue.Add(currentValue);
}
returnValue.Reverse();
return returnValue;
}
openSet.Remove(current);
closedSet.Add (current);
List<Vector3> neighbours = NeighbouringNodes(current);
foreach (Vector3 neighbour in neighbours) {
if (closedSet.Contains(neighbour)) continue;
int tentativeGScore = gScores.ContainsKey(neighbour) ? gScores[neighbour] : 0 + HeuristicCostEstimate(neighbour, destination);
bool tentativeIsBetter = false;
if (!openSet.Contains(neighbour)) {
openSet.Add(neighbour);
int hScoreNeighbour = HeuristicCostEstimate(current, neighbour);
hScores[neighbour] = hScoreNeighbour;
tentativeIsBetter = true;
} else if (tentativeGScore < gScores[neighbour]) {
tentativeIsBetter = true;
} else
tentativeIsBetter = false;
if (tentativeIsBetter) {
cameFrom[neighbour] = current;
gScores[neighbour] = tentativeGScore;
fScores[neighbour] = gScores[neighbour] + hScores[neighbour];
}
}
}
return null;
}
/// <summary>
/// Method for smoothing the path returned by FindPath() method. Removes middle points if there's a straight unobstructed path between two points.
/// Same as in IsPointAnObstruction() method, mind the spherecasting and the obstacle layer.
/// </summary>
/// <returns>Smoothed path</returns>
/// <param name="path">Path from FindPath() method</param>
public static List<Vector3> SmoothPath(List<Vector3> path) {
List<Vector3> smoothPath = new List<Vector3>();
smoothPath.Add((Vector3)path[0]);
path.RemoveAt(0);
path.Reverse(); // reverse to walk from last point
while (path.Count > 0) {
Vector3 lP = smoothPath[smoothPath.Count - 1]; // will be drawing from last point in smoothed path
Vector3 nP = path[path.Count - 1]; // next unsmoothed path point is the last in reversed array
foreach (Vector3 p in path) {
Ray ray = new Ray(lP, (p - lP).normalized);
if (Physics.SphereCast(ray, 0.49f, Vector3.Distance(p, lP), LayerMask.NameToLayer("Obstruction"))) continue; // walk to next if doesn't fit
nP = p;
break;
}
// nP can still be unchanged here,
// so next point is the same as in unsmoothed path
smoothPath.Add(nP);
int idx = path.IndexOf(nP);
path.RemoveRange(idx, path.Count - idx); // kill everything after (including) found point
}
return smoothPath;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment