I suppose PathfindingLinks
would be the content of the next lesson. I would be hoping that when the “real” linking stuff between the grids is implemented it will fix the issue at hand…
I can paste my latest version of Pathfinding.cs
for sure, but be aware it’s unlikely to directly fit into a “vanilla copy” of the course project from gitlab…
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class Pathfinding : MonoBehaviour
{
public static Pathfinding Instance => _instance;
private const int MOVE_STRAIGHT_COST = 10;
private const int MOVE_DIAGONAL_COST = 14;
[SerializeField] private Transform _gridDebugObjectPrefab;
[SerializeField] private bool allowDiagonalMovement = true;
[SerializeField] private LayerMask _obstaclesLayerMask;
[SerializeField] private LayerMask _groundPlaneLayerMask;
[SerializeField] private bool _enableDebugObjects = false;
private int _width;
private int _height;
private float _cellSize;
private int _floorAmount;
private bool _isHexGrid;
private List<IGridSystem<PathNode>> _gridSystemList;
private static Pathfinding _instance;
private void Awake()
{
if (null != Instance)
{
Debug.LogError($"Duplicate instance of Pathfinding encountered. {transform}; Instance is {Instance}");
Destroy(this);
return;
}
else
{
_instance = this;
}
}
public void Setup(int width, int height, float cellSize, int floorAmount)
{
this._width = width;
this._height = height;
this._cellSize = cellSize;
this._floorAmount = floorAmount;
_isHexGrid = LevelGrid.Instance.IsHexGrid;
_gridSystemList = new();
IGridSystem<PathNode> newGrid;
if (_isHexGrid)
{
for (int floor = 0; floor < _floorAmount; floor++)
{
newGrid = new GridSystemHex<PathNode>(width, height, cellSize, floor, LevelGrid.FLOOR_HEIGHT,
(GridSystemHex<PathNode> g, GridPosition gridPosition) => new PathNode(gridPosition));
_gridSystemList.Add(newGrid);
}
}
else
{
for (int floor = 0; floor < _floorAmount; floor++)
{
newGrid = new GridSystem<PathNode>(width, height, cellSize, floor, LevelGrid.FLOOR_HEIGHT,
(GridSystem<PathNode> g, GridPosition gridPosition) => new PathNode(gridPosition));
_gridSystemList.Add(newGrid);
}
}
if (_enableDebugObjects)
{
for (int floor = 0; floor < _floorAmount; floor++)
{
GetGridSystem(floor).CreateDebugObjects(_gridDebugObjectPrefab, transform);
}
}
// Scan the grid for obstacles and mark the grid cells accordingly
for (int x = 0; x < width; x++)
{
for (int z = 0; z < height; z++)
{
for (int floor = 0; floor < floorAmount; floor++)
{
GridPosition gridPosition = new GridPosition(x, z, floor);
Vector3 worldPosition = LevelGrid.Instance.GetWorldPosition(gridPosition);
PathNode currentNode = TryGetNode(x, z, floor);
// By default nodes are not walkable
currentNode.IsWalkable = false;
// Need to offset the origin of the ray so we're not starting from within a collider
float rayCastOffsetDistance = 1f; // Must be lower than the floor height
// To hit a plane we must fire the ray from above...
if (Physics.Raycast(
worldPosition + Vector3.up * rayCastOffsetDistance,
Vector3.down,
rayCastOffsetDistance * 2,
_groundPlaneLayerMask))
{
currentNode.IsWalkable = true;
}
// For obstacles we are firing from below, as we did all the time...
if (Physics.Raycast(
worldPosition + Vector3.down * rayCastOffsetDistance,
Vector3.up,
rayCastOffsetDistance * 2,
_obstaclesLayerMask))
{
currentNode.IsWalkable = false;
}
}
}
}
}
public List<GridPosition> FindPath(GridPosition startGridPosition, GridPosition endGridPosition, out int pathLength)
{
if (startGridPosition.floor > 0)
{
Debug.DrawLine(LevelGrid.Instance.GetWorldPosition(startGridPosition),
LevelGrid.Instance.GetWorldPosition(endGridPosition), Color.magenta, 10f);
}
// Nodes queued for searching
List<PathNode> openList = new();
// Nodes already searched
List<PathNode> closedList = new();
PathNode startNode = GetGridSystem(startGridPosition.floor).GetGridObject(startGridPosition);
openList.Add(startNode);
//Debug.Log($"Calculating path for {startGridPosition} to {endGridPosition}");
// Initialize all nodes for a fresh round
ClearPathNodeInformation();
// calculate cost values for starting node
startNode.GCost = 0;
startNode.HCost = CalculateHeuristicDistance(startGridPosition, endGridPosition);
startNode.CalculateFCost();
PathNode endNode = GetGridSystem(endGridPosition.floor).GetGridObject(endGridPosition);
// Start the actual search loop
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostPathNode(openList);
if (currentNode == endNode)
{
// Arrived at destination
pathLength = endNode.FCost;
return CalculatePath(endNode);
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
// We've been here before...
continue;
}
if (!neighbourNode.IsWalkable)
{
// Node is blocked so mark as done and skip it
closedList.Add(neighbourNode);
continue;
}
int tentativeGCost = currentNode.GCost;
if (_isHexGrid)
{
tentativeGCost += MOVE_STRAIGHT_COST;
}
else
{
tentativeGCost += CalculateCartesianGCost(currentNode.GridPosition, neighbourNode.GridPosition);
}
if (tentativeGCost < neighbourNode.GCost)
{
// Found a better path than we had before
neighbourNode.SetCameFromPathNode(currentNode);
neighbourNode.GCost = tentativeGCost;
neighbourNode.HCost = CalculateHeuristicDistance(startGridPosition, endGridPosition);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// No path found
pathLength = 0;
return null;
}
// Course implementation
private List<GridPosition> CalculatePathHugo(PathNode endNode)
{
List<PathNode> pathNodeList = new();
pathNodeList.Add(endNode);
PathNode currentNode = endNode;
while (null != currentNode.GetCameFromPathNode())
{
pathNodeList.Add(currentNode.GetCameFromPathNode());
currentNode = currentNode.GetCameFromPathNode();
}
pathNodeList.Reverse();
List<GridPosition> gridPositionList = new();
foreach (PathNode pathNode in pathNodeList)
{
gridPositionList.Add(pathNode.GridPosition);
}
return gridPositionList;
}
private List<GridPosition> CalculatePath(PathNode endNode)
{
List<PathNode> pathNodeList = new();
PathNode currentNode = endNode;
do
{
pathNodeList.Add(currentNode);
currentNode = currentNode.GetCameFromPathNode();
}
while (null != currentNode);
pathNodeList.Reverse();
List<GridPosition> gridPositionList = new();
foreach (PathNode pathNode in pathNodeList)
{
gridPositionList.Add(pathNode.GridPosition);
}
return gridPositionList;
}
#region Helper Functions
public int CalculateHeuristicDistance(GridPosition gridPositionA, GridPosition gridPositionB)
{
// Since all we need is actually just a generic heuristic value that
// will give a distance comparison between two points somewhere on the
// grid, we could as well use a bird's path and not bother at all about
// their grid cell type...
return Mathf.RoundToInt(MOVE_STRAIGHT_COST
* Vector3.Distance(
GetGridSystem(gridPositionA.floor).GetWorldPosition(gridPositionA),
GetGridSystem(gridPositionB.floor).GetWorldPosition(gridPositionB)));
}
public int CalculateCartesianGCost(GridPosition gridPositionA, GridPosition gridPositionB)
{
if (_isHexGrid)
{
// On a hex grid, all directions are the same distance
// (Also, we shouldn't be here...)
return MOVE_STRAIGHT_COST;
}
GridPosition gridPositionDistance = gridPositionA - gridPositionB;
if (!allowDiagonalMovement)
{
int distance = Mathf.Abs(gridPositionDistance.x) + Mathf.Abs(gridPositionDistance.z);
return distance * MOVE_STRAIGHT_COST;
}
else
{
int xDistance = Mathf.Abs(gridPositionDistance.x);
int zDistance = Mathf.Abs(gridPositionDistance.z);
int remaining = Mathf.Abs(xDistance - zDistance);
// On a cartesian grid, diagonals are sqrt(2)
return MOVE_DIAGONAL_COST * Mathf.Min(xDistance, zDistance) + MOVE_STRAIGHT_COST * remaining;
}
}
public void SetIsWalkablePosition(GridPosition gridPosition, bool state)
{
GetGridSystem(gridPosition.floor).GetGridObject(gridPosition).IsWalkable = state;
}
public bool IsWalkablePosition(GridPosition gridPosition)
{
return GetGridSystem(gridPosition.floor).GetGridObject(gridPosition).IsWalkable;
}
public bool HasPath(GridPosition startGridPosition, GridPosition endGridPosition)
{
return null != FindPath(startGridPosition, endGridPosition, out int pathLength);
}
public bool HasPath(GridPosition startGridPosition, GridPosition endGridPosition, out int returnPathLength)
{
bool hasPath = null != FindPath(startGridPosition, endGridPosition, out int pathLength);
returnPathLength = pathLength;
return hasPath;
}
public int GetPathLength(GridPosition startGridPosition, GridPosition endGridPosition)
{
FindPath(startGridPosition, endGridPosition, out int pathLength);
return pathLength;
}
private PathNode GetLowestFCostPathNode(List<PathNode> pathNodeList)
{
PathNode lowestFCostPathNode = pathNodeList[0];
for (int i = 0; i < pathNodeList.Count; i++)
{
if (pathNodeList[i].FCost < lowestFCostPathNode.FCost)
{
lowestFCostPathNode = pathNodeList[i];
}
}
return lowestFCostPathNode;
}
private IGridSystem<PathNode> GetGridSystem(int floor)
{
return _gridSystemList[floor];
}
private PathNode TryGetNode(int x, int z, int floor)
{
GridPosition gridPosition = new GridPosition(x, z, floor);
// Requested floor is outside range
if (floor < 0 || floor >= _floorAmount)
{
return null;
}
if (GetGridSystem(gridPosition.floor).IsValidGridPosition(gridPosition))
{
return GetGridSystem(gridPosition.floor).GetGridObject(gridPosition);
}
else
{
return null;
}
}
private List<PathNode> GetNeighbourList(PathNode currentNode)
{
List<PathNode> neighbourList = new();
GridPosition gridPosition = currentNode.GridPosition;
int currentFloor = gridPosition.floor;
// This is the same for hex and cartesian grids, even if "Above" and
// "Below" are a bit to the side of a hex grid cell
neighbourList.Add(TryGetNode(gridPosition.x - 1, gridPosition.z + 0, currentFloor)); //Left
neighbourList.Add(TryGetNode(gridPosition.x + 1, gridPosition.z + 0, currentFloor)); //Right
neighbourList.Add(TryGetNode(gridPosition.x + 0, gridPosition.z + 1, currentFloor)); //Above
neighbourList.Add(TryGetNode(gridPosition.x + 0, gridPosition.z - 1, currentFloor)); //Below
if (_isHexGrid)
{
bool isOddRow = (gridPosition.z % 2 == 1);
if (isOddRow)
{
neighbourList.Add(TryGetNode(gridPosition.x + 1, gridPosition.z + 1, currentFloor)); //UpRight
neighbourList.Add(TryGetNode(gridPosition.x + 1, gridPosition.z - 1, currentFloor)); //DownRight
}
else
{
neighbourList.Add(TryGetNode(gridPosition.x - 1, gridPosition.z + 1, currentFloor)); //UpLeft
neighbourList.Add(TryGetNode(gridPosition.x - 1, gridPosition.z - 1, currentFloor)); //DownLeft
}
}
else
{
if (allowDiagonalMovement)
{
neighbourList.Add(TryGetNode(gridPosition.x - 1, gridPosition.z + 1, currentFloor)); //UpLeft
neighbourList.Add(TryGetNode(gridPosition.x + 1, gridPosition.z + 1, currentFloor)); //UpRight
neighbourList.Add(TryGetNode(gridPosition.x - 1, gridPosition.z - 1, currentFloor)); //DownLeft
neighbourList.Add(TryGetNode(gridPosition.x + 1, gridPosition.z - 1, currentFloor)); //DownRight
}
}
//neighbourList.RemoveAll((PathNode pathNode) => (null == pathNode));
List<PathNode> totalNeighbourList = new();
totalNeighbourList.AddRange(neighbourList);
//Debug.Log($"Total neighbours before: {totalNeighbourList.Count}");
foreach (PathNode pathNode in neighbourList)
{
totalNeighbourList.Add(TryGetNode(gridPosition.x, gridPosition.z, currentFloor + 1)); //Above
totalNeighbourList.Add(TryGetNode(gridPosition.x, gridPosition.z, currentFloor - 1)); //Below
}
//Debug.Log($"Total neighbours after: {totalNeighbourList.Count}");
totalNeighbourList.RemoveAll((PathNode pathNode) => (null == pathNode));
//Debug.Log($"Total neighbours after culling: {totalNeighbourList.Count}");
return totalNeighbourList;
}
private void ClearPathNodeInformation()
{
foreach (IGridSystem<PathNode> gridSystem in _gridSystemList)
{
gridSystem.Map(pathNode =>
{
pathNode.GCost = int.MaxValue;
pathNode.HCost = 0;
pathNode.CalculateFCost();
pathNode.ResetCameFromPathNode();
});
}
}
#endregion
}