A* pathfinding implementation for Unity3D, using raycasting and colliders for obstacles

AStar.cs

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;

	}

}