TurnBasedStrategyCourse/Assets/Scripts/Pathfinding.cs

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

public class Pathfinding : MonoBehaviour {
    private const int moveStraightCost = 10;
    private const int moveDiagonalCost = 14;

    private const float raycastOffsetDistance = 5f;

    [SerializeField] private Transform gridDebugObjectPrefab;
    [SerializeField] private LayerMask obstaclesLayerMask;
    public static Pathfinding Instance { get; private set; }
    private GridSystem<PathNode> GridSystem { get; set; }

    private int Width { get; set; }
    private int Height { get; set; }
    public float CellSize { get; private set; }

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

        Instance = this;
    }

    public void Setup(int width, int height, float cellSize) {
        Width = width;
        Height = height;
        CellSize = cellSize;
        GridSystem = new(width, height, cellSize, 0, LevelGrid.FLOOR_HEIGHT, (_, gridPosition) => new(gridPosition));
        // GridSystem.CreateDebugObjects(gridDebugObjectPrefab);

        for (int x = 0; x < Width; x++) {
            for (int z = 0; z < Height; z++) {
                GridPosition gridPosition = new(x, z, 0);
                Vector3 worldPosition = LevelGrid.Instance.GetWorldPosition(gridPosition);
                if (Physics.Raycast(worldPosition + Vector3.down * raycastOffsetDistance, Vector3.up, raycastOffsetDistance * 2, obstaclesLayerMask)) {
                    GetNode(x, z).IsWalkable = false;
                }
            }
        }
    }

    public bool IsWalkableGridPosition(GridPosition gridPosition) => GridSystem.GetGridObject(gridPosition).IsWalkable;

    public void SetWalkableGridPosition(GridPosition gridPosition, bool isWalkable) => GridSystem.GetGridObject(gridPosition).IsWalkable = isWalkable;

    public List<GridPosition> FindPath(GridPosition startGridPosition, GridPosition endGridPosition, out int pathLength) {
        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, 0));
                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) {
                // Reached final node
                pathLength = endNode.FCost;
                return CalculatePath(endNode);
            }

            openList.Remove(currentNode);
            closedList.Add(currentNode); //currentNode is tested

            foreach (PathNode neighbourNode in GetNeighbourList(currentNode).Where(neighbourNode => !closedList.Contains(neighbourNode))) {
                if (!neighbourNode.IsWalkable) {
                    closedList.Add(neighbourNode); // Not walkable
                    continue;
                }

                int tentativeGCost = currentNode.GCost + CalculateDistance(currentNode.GridPosition, neighbourNode.GridPosition);
                if (tentativeGCost >= neighbourNode.GCost) continue;
                neighbourNode.CameFromPathNode = currentNode;
                neighbourNode.GCost = tentativeGCost;
                neighbourNode.HCost = CalculateDistance(neighbourNode.GridPosition, endGridPosition);
                if (!openList.Contains(neighbourNode)) openList.Add(neighbourNode);
            }
        }

        pathLength = 0; //no path found
        return null;
    }

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

    private static 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 IEnumerable<PathNode> GetNeighbourList(PathNode currentNode) {
        List<PathNode> neighbourList = new();
        GridPosition gridPosition = currentNode.GridPosition;

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

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

        if (gridPosition.ZPosition - 1 >= 0) neighbourList.Add(GetNode(gridPosition.XPosition - 0, gridPosition.ZPosition - 1)); //Down
        if (gridPosition.ZPosition + 1 < GridSystem.Height) neighbourList.Add(GetNode(gridPosition.XPosition - 0, gridPosition.ZPosition + 1)); //Up

        return neighbourList;
    }

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

    private static 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();
    }

    public bool HasPath(GridPosition startGridPosition, GridPosition endGridPosition) => FindPath(startGridPosition, endGridPosition, out _) != null;

    public int GetPathLength(GridPosition startGridPosition, GridPosition endGridPosition) {
        FindPath(startGridPosition, endGridPosition, out int pathLength);
        return pathLength;
    }
}