Pathfinding make unit go diagonal instead of straight on the z axis

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;
}

}

You have x and z swapped around here. It should be


    if (gridPosition.z - 1 >= 0)
    {
        //Down Node
        neighborList.Add(GetNode(gridPosition.x + 0, gridPosition.z - 1));
    }

    if (gridPosition.z + 1 < gridSystem.GetHeight())
    {
        //Up Node
        neighborList.Add(GetNode(gridPosition.x + 0, gridPosition.z + 1));
    }
1 Like

Wow lol
Thanks for the help!

This topic was automatically closed 20 days after the last reply. New replies are no longer allowed.

Privacy & Terms