Pathfinding Algorithm (C# / Unity3d)
This is a custom variant of A* (A-Star) pathfinding, designed to move an entity from point A to B in the most efficient path possible on a two dimentional grid with obstacles. (The wiki entry on A* is certainly worth a read for those not familiar)
A* by itself would have "worked" efficiently - it's fairly easy to allow or diasllow diagonal movement in general through restricting input as needed. The issue I encountered in GoneViral's development was how to allow for diagonal movement in the open but disallow it when the character might diagonal-move directly through a wall corner. I suppose there were a multitude of options, but I chose to modify A* to basically run without diagonal movement allowed, but after a best path is chosen, optimize and reconfigure that path to make sure there were no wall issues. Not only did this fix the wall issues, but it created a more interesting path in a number of cases. After a lot of polish, I'm quite proud of the results.
The added element here is the creation of a "Trinode" class (a group of 3 nodes in succession, with a 4th node used to compare). The method creates trinodes as a path is created, then we look for "L" shapes and test against the same obstacle grid that was used for the pathfinding. If there is not an obstacle to prevent it, the middle node is eliminated from the path and the calculation continues.
halfway point along the white path.
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Linq;
namespace PathFinder {
public static class PathFindScript {
// Custom implementation of the A* pathfinding algorithm, with many additional functions for path smoothing, distance checking, error
//avoidance, visual queues, and availability (reachability) of given objects or locations
//Positions are exclusively grid potitions (not world positions) and are decoded by the player object as it walks the path.
public static void WalkPath(GameObject player, Vector2 endPos, LevelGenerator lg) {
//This script can be fired to move a player to a chosen, valid location.
//It sends a list of Vector2s to the player script, and the player will
//Walk each of them in sequence.
bool validPath = CheckPath(player, endPos, lg);
if ( validPath ) {
GameObject playerObj = player;
PlayerScript s = player.GetComponent();
if ( s.path.Count == 0 ) {
Vector2 playerLoc = FindGridLoc(player, lg);
List path = CreatePath(playerLoc, endPos, lg);
s.path = path;
} else {
throw new System.Exception("Not creating new path till old one is done.");
}
} else {
Debug.Log("Invalid path attempt");
}
}
public static bool CheckPath(GameObject player, Vector2 endPos, LevelGenerator lg) {
//This function combines a number of the smaller functions to simply return whether or not
//the attempted path will be valid. There are a number of checks that will return false for
//varying reasons.
bool validPath = false;
List path = new List();
GameObject playerObj = player;
PlayerScript s = player.GetComponent();
if ( s.path.Count == 0 ) {
Vector2 playerLoc = FindGridLoc(player, lg);
List nodeList = new List();
Node startingNode = new Node();
Node endingNode = new Node();
//Create Nodes for each location possible, set location vector2 as grid location from chosen tile
for ( int x = 1; x < lg.roomWidth - 1; x++ ) {
for ( int y = 1; y < lg.roomHeight - 1; y++ ) {
Node n = new Node();
nodeList.Add(n);
GameObject tile = lg.world_blocks[x, y].tile_object;
terrainBlockScript script = tile.GetComponent();
Vector2 tileV2 = new Vector2(script.gridLocX, script.gridLocY);
n.location = tileV2;
}
}
//Set start and end positions
foreach ( Node n in nodeList ) {
if ( n.location == playerLoc ) {
startingNode = n;
}
if ( n.location == endPos ) {
endingNode = n;
}
}
//Check/Set Validity for each node
foreach ( Node n in nodeList ) {
int isFree = lg.world_blocks[(int) n.location.x, (int) n.location.y].movementAllowance;
if ( isFree == 1 ) {
n.valid = true;
} else {
n.valid = false;
}
}
//Throw error if ending node is not valid.
if ( !endingNode.valid ) {
return validPath;
}
List openList = new List();
List closedList = new List();
bool pathFinished = false;
int max = 5000;
int iteration = 0;
openList.Add(startingNode);
while ( !pathFinished && iteration < max ) {
iteration++;
if ( iteration == max ) {
Debug.Log("Maximum iterations reached, path not found.");
return validPath;
}
Node currentNode;
List sortedOpen = new List();
sortedOpen = openList;
if ( sortedOpen.Count > 1 ) {
sortedOpen.Sort((n1, n2) => n1.cost.CompareTo(n2.cost));
}
//Check if path is valid
if ( sortedOpen.Count == 0 ) {
return false;
}
if ( iteration == 1 ) {
currentNode = startingNode;
} else {
currentNode = sortedOpen[0];
}
openList.Remove(currentNode);
closedList.Add(currentNode);
if ( currentNode == endingNode ) {
pathFinished = true;
break;
}
//Figure out neighbors, if valid
List adjacentNodes = new List();
foreach ( Node n in nodeList ) {
if ( n.location.x == currentNode.location.x && n.location.y == currentNode.location.y - 1 ) {
if ( n.valid && !closedList.Contains(n) ) {
adjacentNodes.Add(n);
}
}
if ( n.location.x == currentNode.location.x && n.location.y == currentNode.location.y + 1 ) {
if ( n.valid && !closedList.Contains(n) ) {
adjacentNodes.Add(n);
}
}
if ( n.location.x == currentNode.location.x + 1 && n.location.y == currentNode.location.y ) {
if ( n.valid && !closedList.Contains(n) ) {
adjacentNodes.Add(n);
}
}
if ( n.location.x == currentNode.location.x - 1 && n.location.y == currentNode.location.y ) {
if ( n.valid && !closedList.Contains(n) ) {
adjacentNodes.Add(n);
}
}
}
//Establish Costs
foreach ( Node n in adjacentNodes ) {
n.hcost = Vector2.Distance(n.location, endingNode.location);
n.gcost = Vector2.Distance(n.location, startingNode.location);
n.cost = n.hcost + n.gcost;
}
foreach ( Node n in adjacentNodes ) {
float newCost = currentNode.gcost + n.gcost;
if ( newCost > n.gcost || !openList.Contains(n) ) {
n.gcost = newCost;
n.hcost = Vector2.Distance(n.location, endingNode.location);
n.parent = currentNode;
if ( !openList.Contains(n) ) {
openList.Add(n);
}
}
}
}
if ( pathFinished ) {
validPath = true;
return true;
}
}
return validPath;
}
public static bool CheckIfBlockIsDiggable(GameObject player, GameObject obj, LevelGenerator lg) {
//Determine if block is reachable and ready to be dug once marked for dig
bool isDiggable = false;
terrainBlockScript tScript = obj.GetComponent();
Vector2 gridLoc = new Vector2(tScript.gridLocX, tScript.gridLocY);
int numEmptyBlocks = NumberOfSurroundingFreeBlocks(gridLoc, lg);
List freeLocations = new List();
if ( numEmptyBlocks > 0 ) {
freeLocations = GetSurroundingFreeBlocksList(gridLoc, lg);
if ( freeLocations.Count > 0 ) {
foreach ( Vector2 loc in freeLocations ) {
isDiggable = CheckPath(player, loc, lg);
if ( isDiggable ) {
return isDiggable;
}
}
}
} else {
return false;
}
return isDiggable;
}
public static List GetSurroundingFreeBlocksList(Vector2 gridLoc, LevelGenerator lg) {
//Return a list of locations around a target grid location
List freeLocations = new List();
if ( lg.world_blocks[(int) gridLoc.x + 1, (int) gridLoc.y].movementAllowance == 1 ) {
freeLocations.Add(new Vector2((int) gridLoc.x + 1, (int) gridLoc.y));
}
if ( lg.world_blocks[(int) gridLoc.x - 1, (int) gridLoc.y].movementAllowance == 1 ) {
freeLocations.Add(new Vector2((int) gridLoc.x - 1, (int) gridLoc.y));
}
if ( lg.world_blocks[(int) gridLoc.x, (int) gridLoc.y + 1].movementAllowance == 1 ) {
freeLocations.Add(new Vector2((int) gridLoc.x, (int) gridLoc.y + 1));
}
if ( lg.world_blocks[(int) gridLoc.x, (int) gridLoc.y - 1].movementAllowance == 1 ) {
freeLocations.Add(new Vector2((int) gridLoc.x, (int) gridLoc.y - 1));
}
return freeLocations;
}
public static int NumberOfSurroundingFreeBlocks(Vector2 gridLoc, LevelGenerator lg) {
//Return number of free-movement blocks surrounding a location
int num = 0;
if ( lg.world_blocks[(int) gridLoc.x + 1, (int) gridLoc.y].movementAllowance == 1 ) {
num++;
}
if ( lg.world_blocks[(int) gridLoc.x - 1, (int) gridLoc.y].movementAllowance == 1 ) {
num++;
}
if ( lg.world_blocks[(int) gridLoc.x, (int) gridLoc.y + 1].movementAllowance == 1 ) {
num++;
}
if ( lg.world_blocks[(int) gridLoc.x, (int) gridLoc.y - 1].movementAllowance == 1 ) {
num++;
}
return num;
}
public static List FindBreathPath(GameObject player, LevelGenerator lg) {
//This script will check surrounding blocks moving outward until it finds a block that
//meets sufficient oxygen requirements, then return a path to that block
Debug.Log("Running Find Breath Path");
List path = new List();
int minOxygenLevel = 20;
List nodeList = new List();
Node startingNode = new Node();
Node endingNode = new Node();
//Create Nodes for each location possible, set location vector2 as grid location from chosen tile
for ( int x = 1; x < lg.roomWidth - 1; x++ ) {
for ( int y = 1; y < lg.roomHeight - 1; y++ ) {
Node n = new Node();
nodeList.Add(n);
Vector2 tileV2 = new Vector2(lg.world_blocks[x, y].gridLocX, lg.world_blocks[x, y].gridLocY);
n.location = tileV2;
}
}
//Get Start and end positions for breath pathing
Vector2 startPos = new Vector2(player.GetComponent().gridLocX, player.GetComponent().gridLocY);
Vector2 endPos = FindOxygen(startPos, lg.roomWidth * 2, minOxygenLevel, player, lg);
//Set start and end positions
foreach ( Node n in nodeList ) {
if ( n.location == startPos ) {
startingNode = n;
}
if ( n.location == endPos ) {
endingNode = n;
}
}
//Check/Set Validity for each node
foreach ( Node n in nodeList ) {
int isFree = lg.world_blocks[(int) n.location.x, (int) n.location.y].movementAllowance;
if ( isFree == 1 ) {
n.valid = true;
} else {
n.valid = false;
}
}
//Throw error if ending node is not valid.
if ( !endingNode.valid ) {
throw new System.Exception("Ending location is not valid.");
}
FindBestPath(startingNode, nodeList, endingNode);
path = RetracePath(startingNode, endingNode);
if ( path.Count > 2 ) {
path = CleanPath(path, endingNode, nodeList, lg);
}
return path;
}
public static Vector2 FindOxygen(Vector2 startLoc, int MAXdistanceToSearch, int minOxyLevel, GameObject player, LevelGenerator lg) {
Vector2 returnLoc = new Vector2(-8000, -8000);
//First check the block we're standing in. If so, just return that and be done with it.
if ( lg.world_blocks[(int) startLoc.x, (int) startLoc.y].oxygenLevel > minOxyLevel ) {
return new Vector2((int) startLoc.x, (int) startLoc.y);
}
//Otherwise, build outward from the given location
for ( int distanceToSearch = 1; distanceToSearch < MAXdistanceToSearch; distanceToSearch++ ) {
for ( int x = -distanceToSearch; x < distanceToSearch; x++ ) {
for ( int y = -distanceToSearch; y < distanceToSearch; y++ ) {
//Ensure index not out of range
if ( x + (int) startLoc.x > 1
&& x + (int) startLoc.x < lg.roomWidth - 1
&& y + (int) startLoc.y > 1
&& y + (int) startLoc.y < lg.roomHeight - 1 ) {
//Begin outward search. Once found, check if path is reachable and return if so
if ( lg.world_blocks[x + (int) startLoc.x, y + (int) startLoc.y].oxygenLevel > minOxyLevel ) {
//Debug.Log("Found oxygen at: " + new Vector2(x + (int) startLoc.x, y + (int) startLoc.y));
Vector2 airLoc = new Vector2(x + (int) startLoc.x, y + (int) startLoc.y);
bool isvalid = CheckPath(player, airLoc, lg);
if ( isvalid ) {
return airLoc;
}
}
}
}
}
}
Debug.Log("Did not find oxygen");
return returnLoc;
}
public static List CreatePath(Vector2 startPos, Vector2 endPos, LevelGenerator lg) {
//CreatePath uses the initial location of the player and the ending Vec2
//To send to FindBestPath, then returns the formatted Vector2 List
List nodeList = new List();
List path = new List();
Node startingNode = new Node();
Node endingNode = new Node();
//Create Nodes for each location possible, set location vector2 as grid location from chosen tile
for ( int x = 1; x < lg.roomWidth - 1; x++ ) {
for ( int y = 1; y < lg.roomHeight - 1; y++ ) {
Node n = new Node();
nodeList.Add(n);
GameObject tile = lg.world_blocks[x, y].tile_object;
terrainBlockScript script = tile.GetComponent();
Vector2 tileV2 = new Vector2(script.gridLocX, script.gridLocY);
n.location = tileV2;
}
}
//Set start and end positions
foreach ( Node n in nodeList ) {
if ( n.location == startPos ) {
startingNode = n;
}
if ( n.location == endPos ) {
endingNode = n;
}
}
//Check/Set Validity for each node
foreach ( Node n in nodeList ) {
int isFree = lg.world_blocks[(int) n.location.x, (int) n.location.y].movementAllowance;
if ( isFree == 1 ) {
n.valid = true;
} else {
n.valid = false;
}
}
//Throw error if ending node is not valid.
if ( !endingNode.valid ) {
throw new System.Exception("Ending location is not valid.");
}
FindBestPath(startingNode, nodeList, endingNode);
path = RetracePath(startingNode, endingNode);
if ( path.Count > 2 ) {
path = CleanPath(path, endingNode, nodeList, lg);
if ( path.Count > 2 ) {
//ensure we reach the final destination, no matter what
if ( path[path.Count - 1] != endingNode.location ) {
path.Add(endingNode.location);
}
//fix wobble at path end
if ( path[path.Count - 3] == endingNode.location ) {
path.RemoveAt(path.Count - 1);
path.RemoveAt(path.Count - 1);
}
}
}
return path;
}
public static List CleanPath(List path, Node endingNode, List nodeList, LevelGenerator lg) {
//Cleanpath uses a list of Trinodes to reformat a valid path and smooth areas when possible via diagonal lines
//This is used in addition (instead of during path creation) to prevent a player from clipping diagonally though a block
List tList = CreateTrinodeList(path, nodeList); //create initial trinode list with D locations
List alteredPath = new List();
int numDeletable = 0;
//Mark nodes as deletable via CreateTrinodeList output
foreach ( TriNode t in tList ) {
if ( t.d.valid ) {
if ( lg.world_blocks[(int) t.d.location.x, (int) t.d.location.y].movementAllowance == 1 ) {
t.isDeletable = true;
numDeletable++;
}
}
}
//Remove nodes marked as deletable
foreach ( TriNode t in tList ) {
if ( t.isDeletable ) {
alteredPath.Add(t.a.location);
alteredPath.Add(t.c.location);
} else {
alteredPath.Add(t.a.location);
alteredPath.Add(t.b.location);
alteredPath.Add(t.c.location);
}
if ( path.Count > 2 ) {
path.RemoveAt(0);
path.RemoveAt(0);
path.RemoveAt(0);
}
}
if ( path.Count > 0 ) {
alteredPath.Add(path[0]);
path.RemoveAt(0);
}
if ( path.Count > 0 ) {
alteredPath.Add(path[0]);
path.RemoveAt(0);
}
return alteredPath;
}
public static List CreateTrinodeList(List path, List nodeList) {
//Create a list of Trinodes from a path. Every group of 3 path locations becomes a single Trinode,
//And the "D" node location is the 4th potential location.
List trinodeList = new List();
int iteration = 0;
bool addingNodes = true;
while ( addingNodes ) {
if ( iteration + 2 <= path.Count - 1 ) {
TriNode t = new TriNode();
Node a = GetNodeByLocation(path[iteration], nodeList);
Node b = GetNodeByLocation(path[iteration + 1], nodeList);
Node c = GetNodeByLocation(path[iteration + 2], nodeList);
Node d = DNodeLocation(a, b, c);
t.aLocation = iteration;
t.bLocation = iteration + 1;
t.cLocation = iteration + 2;
t.a = a;
t.b = b;
t.c = c;
t.d = d;
trinodeList.Add(t);
} else {
addingNodes = false;
}
iteration += 2;
}
return trinodeList;
}
public static Node GetNodeByLocation(Vector2 loc, List nodeList) {
//Return node based on v2 grid location
Node returnNode = new Node();
foreach ( Node n in nodeList ) {
if ( n.location == loc ) {
returnNode = n;
break;
}
}
return returnNode;
}
public static Node DNodeLocation(Node a, Node b, Node c) {
//Find location of "D node" based on other nodes. Critical for path smoothing
Node dNode = new Node();
if ( a.location.x != c.location.x && a.location.y != c.location.y ) {
if ( a.location.x != b.location.x ) {
dNode.location.x = a.location.x;
dNode.location.y = c.location.y;
dNode.valid = true;
}
if ( a.location.y != b.location.y ) {
dNode.location.x = c.location.x;
dNode.location.y = a.location.y;
dNode.valid = true;
}
if ( dNode.location == a.location || dNode.location == b.location || dNode.location == c.location ) {
Debug.Log("Identical location- d: " + dNode.location + "," + a.location + "," + b.location + "," + c.location);
}
} else {
dNode.valid = false;
}
return dNode;
}
public static void FindBestPath(Node startingNode, List nodeList, Node endingNode) {
//This script runs the pathfinding heavy lifting, and sets the parents required for
//RetracePath() to figure out and format
List openList = new List();
List closedList = new List();
bool pathFinished = false;
int max = 5000;
int iteration = 0;
openList.Add(startingNode);
while ( !pathFinished && iteration < max ) {
iteration++;
if ( iteration == max ) {
throw new System.Exception("Maximum iterations reached, path not found.");
}
Node currentNode;
List sortedOpen = new List();
sortedOpen = openList;
if ( sortedOpen.Count > 1 ) {
sortedOpen.Sort((n1, n2) => n1.cost.CompareTo(n2.cost));
}
if ( iteration == 1 ) {
currentNode = startingNode;
} else {
currentNode = sortedOpen[0];
}
openList.Remove(currentNode);
closedList.Add(currentNode);
if ( currentNode == endingNode ) {
pathFinished = true;
break;
}
//Figure out neighbors, if valid
List adjacentNodes = new List();
foreach ( Node n in nodeList ) {
if ( n.location.x == currentNode.location.x && n.location.y == currentNode.location.y - 1 ) {
if ( n.valid && !closedList.Contains(n) ) {
adjacentNodes.Add(n);
}
}
if ( n.location.x == currentNode.location.x && n.location.y == currentNode.location.y + 1 ) {
if ( n.valid && !closedList.Contains(n) ) {
adjacentNodes.Add(n);
}
}
if ( n.location.x == currentNode.location.x + 1 && n.location.y == currentNode.location.y ) {
if ( n.valid && !closedList.Contains(n) ) {
adjacentNodes.Add(n);
}
}
if ( n.location.x == currentNode.location.x - 1 && n.location.y == currentNode.location.y ) {
if ( n.valid && !closedList.Contains(n) ) {
adjacentNodes.Add(n);
}
}
}
//Establish Costs
foreach ( Node n in adjacentNodes ) {
n.hcost = Vector2.Distance(n.location, endingNode.location);
n.gcost = Vector2.Distance(n.location, startingNode.location);
n.cost = n.hcost + n.gcost;
}
foreach ( Node n in adjacentNodes ) {
float newCost = currentNode.gcost + n.gcost;
if ( newCost > n.gcost || !openList.Contains(n) ) {
n.gcost = newCost;
n.hcost = Vector2.Distance(n.location, endingNode.location);
n.parent = currentNode;
if ( !openList.Contains(n) ) {
openList.Add(n);
}
}
}
}
}
public static List RetracePath(Node startingNode, Node endingNode) {
//Move backward through found path, and create a list via the parent nodes. Then reverse.
List vpath = new List();
Node currentNode = endingNode;
while ( currentNode != startingNode ) {
vpath.Add(currentNode.location);
currentNode = currentNode.parent;
}
vpath.Reverse();
return vpath;
}
public static Vector2 FindGridLoc(GameObject input, LevelGenerator lg) {
//Search the entire grid until gameobject matches a cooresponding grid location (not ideal)
Vector2 gridLoc = new Vector2(input.transform.position.x, input.transform.position.y);
GameObject matchingTile = null;
for ( int x = 0; x < lg.roomWidth; x++ ) {
for ( int y = 0; y < lg.roomHeight; y++ ) {
GameObject tile = lg.world_blocks[x, y].tile_object;
Vector2 tileV2 = new Vector2(tile.transform.position.x, tile.transform.position.y);
if ( tileV2 == gridLoc ) {
matchingTile = tile;
break;
}
}
}
Vector2 returnVector = new Vector2(matchingTile.GetComponent().gridLocX, matchingTile.GetComponent().gridLocY);
return returnVector;
}
public static float MeasurePathDistance(List path) {
float returnValue = 0;
if (path.Count > 0) {
for (int x=0; x < path.Count -1; x++ ) {
returnValue += Vector2.Distance(path[x], path[x + 1]);
}
}
return returnValue;
}
}
public class Node {
//Pathfinding class for determining travel costs
public Vector2 location; //grid location, not world position
public float cost; //g+h
public float gcost; //distance from start
public float hcost; //distance from end
public Node parent; //parent node for pathfinding
public bool valid; //validity bool
}
public class TriNode {
//Path smoothing class for determining whether or not we can move diagonally
//while still avoiding moving through the corner of a tile.
public Node a;
public Node b;
public Node c;
public Node d;
public int aLocation;
public int bLocation;
public int cLocation;
public bool isDeletable = false;
}
}