TurnBasedStrategyCourse/Assets/Scripts/Pathfinding.cs

using System.Collections.Generic;
using System.Linq;
using Grid;
using UnityEngine;

public class Pathfinding : MonoBehaviour {
	public static Pathfinding Instance { get; private set; }
	
	private const int moveStraightCost = 10;
	private const int moveDiagonalCost = 14;
	[SerializeField] private Transform gridDebugObjectPrefab;
	private float cellSize;
	private GridSystem<PathNode> gridSystem;
	private int height;
	private int width;

	private void Awake() {
		if (Instance is not null) {
			Debug.LogError($"There is more than one Pathfinding! {transform} - {Instance}");
			Destroy(gameObject);
			return;
		}
		Instance = this;

		gridSystem = new(10, 10, 2f, (_, gridPosition) => new(gridPosition));
		gridSystem.CreateDebugObjects(gridDebugObjectPrefab);
	}

	public List<GridPosition> FindPath(GridPosition startGridPosition, GridPosition endGridPosition) {
		List<PathNode> openList = new();
		List<PathNode> closedList = new();

		PathNode startNode = gridSystem.GetGridObject(startGridPosition);
		PathNode endNode = gridSystem.GetGridObject(endGridPosition);
		openList.Add(startNode);

		for (int x = 0; x < gridSystem.Width; x++) {
			for (int z = 0; z < gridSystem.Height; z++) {
				PathNode pathNode = gridSystem.GetGridObject(new(x, z));
				pathNode.GCost = int.MaxValue;
				pathNode.HCost = 0;
				pathNode.CameFromPathNode = null;
			}
		}

		startNode.GCost = 0;
		startNode.HCost = CalculateDistance(startGridPosition, endGridPosition);

		while (openList.Count > 0) {
			PathNode currentNode = GetLowestFCostPathNode(openList);
			
			if (currentNode == endNode) return CalculatePath(endNode); // Reached final node
			
			openList.Remove(currentNode); 
			closedList.Add(currentNode); //currentNode is tested

			foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) {
				if (closedList.Contains(neighbourNode)) continue;
				int tentativeGCost = currentNode.GCost + CalculateDistance(currentNode.GridPosition, neighbourNode.GridPosition);
				if (tentativeGCost < neighbourNode.GCost) {
					neighbourNode.CameFromPathNode = currentNode;
					neighbourNode.GCost = tentativeGCost;
					neighbourNode.HCost = CalculateDistance(neighbourNode.GridPosition, endGridPosition);
					if (!openList.Contains(neighbourNode)) openList.Add(neighbourNode);
				}
			}
		}
		return null; //no path found
	}

	private int CalculateDistance(GridPosition gridPositionA, GridPosition gridPositionB) {
		GridPosition gridPositionDistance = gridPositionA - gridPositionB;
		int xDistance = Mathf.Abs(gridPositionDistance.X);
		int zDistance = Mathf.Abs(gridPositionDistance.Z);
		int remaining = Mathf.Abs(xDistance - zDistance);
		return moveDiagonalCost * Mathf.Min(xDistance, zDistance) + moveStraightCost * remaining;
	}

	private PathNode GetLowestFCostPathNode(IReadOnlyList<PathNode> pathNodeList) {
		PathNode lowestFCostPathNode = pathNodeList[0];
		foreach (PathNode currentPathNode in pathNodeList.Where(t => t.FCost < lowestFCostPathNode.FCost)) lowestFCostPathNode = currentPathNode;
		return lowestFCostPathNode;
	}

	private List<PathNode> GetNeighbourList(PathNode currentNode) {
		List<PathNode> neighbourList = new();
		GridPosition gridPosition = currentNode.GridPosition;

		if (gridPosition.X - 1 >= 0) {
			neighbourList.Add(GetNode(gridPosition.X - 1, gridPosition.Z + 0)); //Left
			if (gridPosition.Z - 1 >= 0) neighbourList.Add(GetNode(gridPosition.X - 1, gridPosition.Z - 1)); //LeftDown
			if (gridPosition.Z < gridSystem.Height) neighbourList.Add(GetNode(gridPosition.X - 1, gridPosition.Z + 1)); //LeftUp
		}

		if (gridPosition.X + 1 < gridSystem.Width) {
			neighbourList.Add(GetNode(gridPosition.X + 1, gridPosition.Z + 0)); //Right
			if (gridPosition.Z - 1 >= 0) neighbourList.Add(GetNode(gridPosition.X + 1, gridPosition.Z - 1)); //RightDown
			if (gridPosition.Z + 1 < gridSystem.Height) neighbourList.Add(GetNode(gridPosition.X + 1, gridPosition.Z + 1)); //RightUp
		}

		if (gridPosition.Z - 1 >= 0) neighbourList.Add(GetNode(gridPosition.X - 0, gridPosition.Z - 1)); //Down
		if (gridPosition.Z + 1 < gridSystem.Height) neighbourList.Add(GetNode(gridPosition.X - 0, gridPosition.Z + 1)); //Up

		return neighbourList;
	}

	private PathNode GetNode(int x, int z) => gridSystem.GetGridObject(new(x, z));

	private List<GridPosition> CalculatePath(PathNode endNode) {
		List<PathNode> pathNodeList = new() { endNode };
		PathNode currentNode = endNode;
		while (currentNode.CameFromPathNode != null) {
			pathNodeList.Add(currentNode.CameFromPathNode);
			currentNode = currentNode.CameFromPathNode;
		}
		pathNodeList.Reverse();
		return pathNodeList.Select(pathNode => pathNode.GridPosition).ToList();
	}
}