I’m not sure how complex your pathfinding needs are but I happened to be working on some pathfinding recently and I found A* to be pretty good for relatively simple needs. Basically you just need to split your terrain up into a grid of some kind, a square grid is easiest, and then calculate the ‘nodes’ of the grid around the starting point to the end point based on the weights of distance from the beginning and a heuristic (like linear distance to the end point). Here’s my implementation, sorry for the big block of code.
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
public class Pathfinding : MonoBehaviour
{
public Map map; //This is your grid filled with booleans, true being passable terrain
public List<Vector2> GetPath(int xStart, int yStart, int xEnd, int yEnd)
{
List<Node> openList = new List<Node>();
List<Node> closedList = new List<Node>();
Coords endCoords = new Coords { x = xEnd, y = yEnd };
openList.Add(new Node { parent = null, position = new Coords { x = xStart, y = yStart }, f = 0 }); //Add the starting node to the open list
while (openList.Count != 0)
{
Node q = openList.OrderBy(node => node.f).First(); //Get the Node with the least f from the open list and remove it from the list
openList.Remove(q);
Coords right = new Coords { x = q.position.x + 1, y = q.position.y }; //Find the four surrounding successors
Coords left = new Coords { x = q.position.x - 1, y = q.position.y };
Coords up = new Coords { x = q.position.x, y = q.position.y + 1 };
Coords down = new Coords { x = q.position.x, y = q.position.y - 1 };
List<Node> successors = new List<Node>();
if (map[right.x, right.y]) successors.Add(new Node { parent = q, position = right }); //Check if successors have valid Coords
if (map[left.x, left.y]) successors.Add(new Node { parent = q, position = left });
if (map[up.x, up.y]) successors.Add(new Node { parent = q, position = up });
if (map[down.x, down.y]) successors.Add(new Node { parent = q, position = down });
foreach (Node s in successors)
{
if (s.position == endCoords) //Stop search and return path if end Coord is found
{
List<Node> path = new List<Node>();
path.Add(s);
while (path.ElementAt(0).parent != null) //Go from the end through each parent to the beginning to find the path
{
path.Insert(0, path.ElementAt(0).parent); //Orders list with the beginning of the path at index 0
}
return GetVectors(path);
}
s.g = q.g + 1f; //g value is the distance in tiles from the starting node: 1 + the distance of the previous node
s.h = GetDistance(s.position, endCoords); //h is the linear distance from the node to the end
s.f = s.g + s.h; //f is the sum of g + h
//Skip if node has same position as another with lesser f in open or closed list
if (closedList.Where(n => (n.position == s.position) && (n.f <= s.f)).Any() ||
openList.Where(n => (n.position == s.position) && (n.f <= s.f)).Any()) continue;
openList.Add(s); //Add the successor to the open list
}
closedList.Add(q); //Add removed Node to the closed list
}
return new List<Vector2>();
}
public bool CheckLOS(int startX, int startY, int endX, int endY)
{
int vX = endX - startX; //The 2d vector components x,y from startX, startY
int vY = endY - startY;
float magnitude = Mathf.Sqrt(Mathf.Pow(vX, 2) + Mathf.Pow(vY, 2));
float unitX = vX / magnitude; //Unit vector components
float unitY = vY / magnitude;
//Determines which diagonal direction the vector points to get the points at the correct corners of the square
bool firstThirdQuadrants = vX * vY > 0;
if (vX == 0) //Handles the straight line cases
{
for (int i = startY; vY > 0 ? i < endY : i > endY; i += vY / Mathf.Abs(vY))
if (!map[startX, i]) return false;
return true;
}
else if (vY == 0)
{
for (int i = startX; vX > 0 ? i < endX : i > endX; i += vX / Mathf.Abs(vX))
if (!map[i, startY]) return false;
return true;
}
for (float i = 0; i < magnitude; i += 0.2f) //Steps along the vector in increments and checks the map
{
if (firstThirdQuadrants)
{
//Tests vectors from the farthest two corners in the square, depending on the diagonal direction
if (!map[Mathf.RoundToInt(unitX * i + startX - 0.5f), Mathf.RoundToInt(unitY * i + startY + 0.5f)] ||
!map[Mathf.RoundToInt(unitX * i + startX + 0.5f), Mathf.RoundToInt(unitY * i + startY - 0.5f)])
{
return false;
}
}
else
{
if (!map[Mathf.RoundToInt(unitX * i + startX - 0.5f), Mathf.RoundToInt(unitY * i + startY - 0.5f)] ||
!map[Mathf.RoundToInt(unitX * i + startX + 0.5f), Mathf.RoundToInt(unitY * i + startY + 0.5f)])
{
return false;
}
}
}
return true;
}
public float GetDistance(Coords start, Coords end)
{
return Mathf.Sqrt(Mathf.Pow(end.x - start.x, 2f) + Mathf.Pow(end.y - start.y, 2f));
}
public List<Vector2> GetVectors(List<Node> nodes)
{
List<Vector2> vectors = new List<Vector2>();
List<Node> path = nodes;
int targetIndex = 2;
while (targetIndex < path.Count) //Checks to see if any middle points can be removed from the path
{
if (!CheckLOS(path[targetIndex - 2].position.x, path[targetIndex - 2].position.y,
path[targetIndex].position.x, path[targetIndex].position.y))
targetIndex++;
else
path.RemoveAt(targetIndex - 1);
}
for (int i = 0; i < path.Count; i++) //Converts path Nodes to Vectors for ease of use
{
vectors.Add(new Vector2((float)path_.position.x, (float)path*.position.y));*_
}
return vectors;
}
}
//The Node class used in the A star algorithm
public class Node
{
public Node parent;
public Coords position;
public float f, g, h;
}
//Custom class with just two variables to store coordinates and overrides to compare with equality operators
public class Coords
{
public int x, y;
public override bool Equals(object obj)
{
if (obj == null) return false;
Coords p = obj as Coords;
if ((System.Object)p == null) return false;
return (x == p.x) && (y == p.y);
}
public bool Equals(Coords p)
{
if ((object)p == null) return false;
return (x == p.x) && (y == p.y);
}
public override int GetHashCode()
{
return x ^ y;
}
public static bool operator ==(Coords a, Coords b)
{
if (System.Object.ReferenceEquals(a, b)) return true;
if (((object)a == null) || ((object)b == null)) return false;
return a.x == b.x && a.y == b.y;
}
public static bool operator !=(Coords a, Coords b)
{
return !(a == b);
}
}
//Custom class to make a ‘fake’ jagged array with negative indices
public class Map
{
private bool[][] mapData;
public Map(int size = 200)
{
mapData = new bool[];
for (int i = 0; i < size; i++)
{
mapData = new bool;
}
}
public bool this[int xIndex, int yIndex]
{
get { return mapData[xIndex + 100][yIndex + 100]; }
set { mapData[xIndex + 100][yIndex + 100] = value; }
}
}