When moving on the z axis, I can only move two tiles due to pathfinding making the unit move diagonally instead of straight but on the x, I can move straight through to the 4th tile.
Initially, I only wanted to use vertical and horizontal movements but it would move diagonally like this “>” so I added the diagonal direction but it does the same regardless.
Any help to pinpoint where the issue may lie would be extremely appreciated.
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class Pathfinding : MonoBehaviour
{
public static Pathfinding Instance { get; private set; }
private const int MOVE_STRAIGHT_COST = 10;
private const int MOVE_DIAGONAL_COST = 14;
[SerializeField] private Transform gridDebugObjectPrefab;
[SerializeField] private LayerMask obstaclesLayerMask;
private int width;
private int height;
private float cellSize;
private GridSystem<PathNode> gridSystem;
private void Awake()
{
Instance = this;
}
public void Setup(int width, int height, float cellSize)
{
this.width = width;
this.height = height;
this.cellSize = cellSize;
gridSystem = new GridSystem<PathNode>(width, height, cellSize,
(GridSystem<PathNode> g, GridPosition gridPosition) => new PathNode(gridPosition));
//gridSystem.CreateDebugObjects(gridDebugObjectPrefab);
for(int x = 0; x < width; x++)
{
for(int z = 0; z < height; z++)
{
GridPosition gridPosition = new GridPosition(x, z);
Vector3 worldPosition = LevelGrid.Instance.GetWorldPosition(gridPosition);
float raycastOffsetDistance = 5f;
if(Physics.Raycast(worldPosition + Vector3.down * raycastOffsetDistance,
Vector3.up, raycastOffsetDistance * 2,
obstaclesLayerMask))
{
GetNode(x, z).SetIsWalkable(false);
}
}
}
}
public List<GridPosition> FindPath(GridPosition startGridPosition, GridPosition endGridPosition, out int pathLength)
{
List<PathNode> openList = new List<PathNode>();
List<PathNode> closedList = new List<PathNode>();
PathNode startNode = gridSystem.GetGridObject(startGridPosition);
PathNode endNode = gridSystem.GetGridObject(endGridPosition);
openList.Add(startNode);
for(int x = 0; x < gridSystem.GetWidth(); x++)
{
for(int z = 0; z < gridSystem.GetHeight(); z++)
{
GridPosition gridPosition = new GridPosition(x, z);
PathNode pathNode = gridSystem.GetGridObject(gridPosition);
pathNode.SetGCost(int.MaxValue);
pathNode.SetHCost(0);
pathNode.CalculateFCost();
pathNode.ResetCameFromPathNode();
}
}
startNode.SetGCost(0);
startNode.SetHCost(CalculateDistance(startGridPosition, endGridPosition));
startNode.CalculateFCost();
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCostPathNode(openList);
if(currentNode == endNode)
{
//Reached final node
pathLength = endNode.GetFCost();
return CalculatePath(endNode);
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach(PathNode neighbourNode in GetNeighborList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
if (!neighbourNode.IsWalkable())
{
closedList.Add(neighbourNode);
continue;
}
int tentativeGCost = currentNode.GetGCost() +
CalculateDistance(currentNode.GetGridPosition(), neighbourNode.GetGridPosition());
if(tentativeGCost < neighbourNode.GetGCost())
{
neighbourNode.SetCameFromPathNode(currentNode);
neighbourNode.SetGCost(tentativeGCost);
neighbourNode.SetHCost(CalculateDistance(neighbourNode.GetGridPosition(), endGridPosition));
neighbourNode.CalculateFCost();
if(!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
// No path found
pathLength = 0;
return null;
}
public int CalculateDistance(GridPosition gridPostionA, GridPosition gridPositionB)
{
GridPosition gridPositionDistance = gridPostionA - gridPositionB;
int xDistance = Mathf.Abs(gridPositionDistance.x);
int zDistance = Mathf.Abs(gridPositionDistance.z);
int remaining = Mathf.Abs(xDistance - zDistance);
return MOVE_DIAGONAL_COST * Mathf.Min(xDistance, zDistance) + MOVE_STRAIGHT_COST * remaining;
}
private PathNode GetLowestFCostPathNode(List<PathNode> pathNodeList)
{
PathNode lowestFCostPathNode = pathNodeList[0];
for(int i = 0; i < pathNodeList.Count; i++)
{
if (pathNodeList[i].GetFCost() < lowestFCostPathNode.GetGCost())
{
lowestFCostPathNode = pathNodeList[i];
}
}
return lowestFCostPathNode;
}
private PathNode GetNode(int x, int z)
{
return gridSystem.GetGridObject(new GridPosition(x, z));
}
private List<PathNode> GetNeighborList(PathNode currentNode)
{
List<PathNode> neighborList = new List<PathNode>();
GridPosition gridPosition = currentNode.GetGridPosition();
if (gridPosition.x - 1 >= 0)
{
//Left Node
neighborList.Add(GetNode(gridPosition.x - 1, gridPosition.z + 0));
if(gridPosition.z - 1 >= 0)
{
//Left Down
neighborList.Add(GetNode(gridPosition.x - 1, gridPosition.z - 1));
}
if(gridPosition.z + 1 < gridSystem.GetHeight())
{
//Left Up
neighborList.Add(GetNode(gridPosition.x - 1, gridPosition.z + 1));
}
}
if (gridPosition.x + 1 < gridSystem.GetWidth())
{
//Right Node
neighborList.Add(GetNode(gridPosition.x + 1, gridPosition.z + 0));
if(gridPosition.z - 1 >= 0)
{
//Right Down
neighborList.Add(GetNode(gridPosition.x + 1, gridPosition.z - 1));
}
if(gridPosition.z + 1 < gridSystem.GetHeight())
{
//Right Up
neighborList.Add(GetNode(gridPosition.x + 1, gridPosition.z + 1));
}
}
if (gridPosition.z - 1 >= 0)
{
//Down Node
neighborList.Add(GetNode(gridPosition.z - 1, gridPosition.x + 0));
}
if (gridPosition.z + 1 < gridSystem.GetHeight())
{
//Up Node
neighborList.Add(GetNode(gridPosition.z + 1, gridPosition.x + 0));
}
return neighborList;
}
private List<GridPosition> CalculatePath(PathNode endNode)
{
List<PathNode> pathNodeList = new List<PathNode>();
pathNodeList.Add(endNode);
PathNode currentNode = endNode;
while(currentNode.GetCameFromPathNode() != null)
{
pathNodeList.Add(currentNode.GetCameFromPathNode());
currentNode = currentNode.GetCameFromPathNode();
}
pathNodeList.Reverse();
List<GridPosition> gridPositionList = new List<GridPosition>();
foreach(PathNode pathNode in pathNodeList)
{
gridPositionList.Add(pathNode.GetGridPosition());
}
return gridPositionList;
}
public bool IsWalkableGridPosition(GridPosition gridPosition)
{
return gridSystem.GetGridObject(gridPosition).IsWalkable();
}
public bool HasPath(GridPosition startGridPosition, GridPosition endGridPosition)
{
return FindPath(startGridPosition, endGridPosition, out int pathLength) != null;
}
public int GetPathLength(GridPosition startGridPosition, GridPosition endGridPosition)
{
FindPath(startGridPosition, endGridPosition, out int pathLength);
return pathLength;
}
}