a* pathfinding algorithm curves when I added my heap heuristic?

119034-pathfinding.pngMy pathfinding algorithm works perfectly fine when I don’t have a heap but when I implemented my pathfinding to include a heap my pathing curves even when there are no obstacles. I am am using a modified implementation of 2d a* pathfinding for a 3d grid?

My node script:

using UnityEngine;
using System.Collections;
                  
public class Node : IHeapItem<Node>
{ 

    public bool walkable;
    
    public int gCost;
    public int hCost;
    public Vector3 WorldPosition;

    public int gridX;
    public int gridY;
    public int gridZ;
    public Node parent;
    public int heapIndex;

	public Node(Vector3 worldpos, bool _walkable, int _gridX, int _gridY, int _gridZ)
    {
        WorldPosition = worldpos;
        walkable = _walkable;
        gridX = _gridX;
        gridY = _gridY;
        gridZ = _gridZ;
      
    }
    public int fCost
    {
        get
        {
            return gCost + hCost;
        }
    }

    
     public int HeapIndex
     {
        get
        {
            return heapIndex;
        }
        set
        {
            heapIndex = value;
        }
       }

    public int CompareTo(Node nodeToCompare)
    {
        int compare = fCost.CompareTo(nodeToCompare.fCost);
        if (compare == 0)                                  //if there are elevation issues with node, temporarily comment out code here
        {
           compare = hCost.CompareTo(nodeToCompare.hCost);
        }
        return -compare;
    }
}

my pathfinding algorithm

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
public class pathfinding : MonoBehaviour {   

    public Transform seeker, target;
    List<Node> tempList = new List<Node>();
    Grid GetGrid;
	void Awake ()
    {
        GetGrid = GetComponent<Grid>(); //allows this script to use public variables from Grid script
	}
	
    void Update()
    {
        if (Input.GetButtonDown("Jump"))
        {
            FindPath(seeker.position, target.position, 1);
        }
      
    }

	void FindPath(Vector3 startPos, Vector3 targetPos, int EnemyHeight)
    {


        Stopwatch sw = new Stopwatch();
        sw.Start();

        Node startNode = GetGrid.NodeFromWorldPosition(startPos);
        Node targetNode = GetGrid.NodeFromWorldPosition(targetPos);
                                                          
        Heap<Node> openSet = new Heap<Node>(GetGrid.MaxSize); 
        HashSet<Node> closedSet = new HashSet<Node>();
        openSet.Add(startNode);
        while (openSet.Count > 0)
        {
            
                          closedSet.Add(currentNode);

                if (currentNode == targetNode)
                {
                sw.Stop();
                print("path found: " + sw.ElapsedMilliseconds + " ms");
                RetracePath(startNode, targetNode);
                    return;
                }
            
            

            foreach (Node neighbour in GetGrid.GetNeighbours(currentNode))
            {
                bool SetToContinue = false;
                if (!neighbour.walkable || closedSet.Contains(neighbour))
                {
                    continue;
                }

                if (EnemyHeight > 0)
                {
                                   
                        for (int i = 1; i < EnemyHeight + 1; i++)
                        {
                            if (GetGrid.grid[neighbour.gridX, neighbour.gridY, neighbour.gridZ + i] == null || GetGrid.grid[neighbour.gridX, neighbour.gridY, neighbour.gridZ + i].walkable == false/* || GetGrid.grid[neighbour.gridX, neighbour.gridY, neighbour.gridZ - 1] == null || GetGrid.grid[neighbour.gridX, neighbour.gridY, neighbour.gridZ - 1].walkable == true*/)
                            {

                            SetToContinue = true;
                            break;
                            }
                        }
                    

                }
                if(SetToContinue == true)
                {
                    continue;
                }
                int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
                if(newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
                {
                    neighbour.gCost = newMovementCostToNeighbour; //Check on this later but I think this is if the neighbour of the node being evaluated has a more efficent path to the evaluated node
                    neighbour.hCost = GetDistance(neighbour, targetNode);

                    neighbour.parent = currentNode;

                    if (!openSet.Contains(neighbour))
                    {
                        openSet.Add(neighbour);
                    }
                    else
                    {
                         openSet.UpdateItem(neighbour);
                     }

                }

            }
            

        }

    }
    void RetracePath(Node startNode, Node endNode)
    {
       
        List<Node> path = new List<Node>();
        Node currentNode = endNode;
        while(currentNode != startNode)
        {
            path.Add(currentNode);
            currentNode = currentNode.parent;
        }
        path.Reverse();
        GetGrid.path = path;
    }

    int GetDistance(Node nodeA, Node nodeB)
    {
        int distX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
        int distY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
        int distZ = Mathf.Abs(nodeA.gridZ - nodeB.gridZ);
        int cost = 0;
        // cube diagonal
        if (distX < distY && distX < distZ)
        {
            cost += distX * 17;
            distY -= distX;
            distX = distZ - distX;
        }
        else if (distY < distZ)
        {
            cost += distY * 17;
            distX -= distY;
            distY = distZ - distY;
        }
        else
        {
            cost += distZ * 17;
            distX -= distZ;
            distY -= distZ;
        }

        // remaining distance 2d in distX and distY
        if (distX < distY)
        {
            cost += distX * 14;
            distX = distY - distX;
        }
        else
        {
            cost += distY * 14;
            distX -= distY;
        }
        return cost + distX * 10;
    }

}

my heap algorithm

using UnityEngine;
using System.Collections;
using System;

    public class Heap<T> where T : IHeapItem<T>{
    
        T[] items;
        int currentItemCount;
    
        public Heap(int maxHeapSize)
        {
            items = new T[maxHeapSize];
        }
    
        public void Add(T item)
        {
            item.HeapIndex = currentItemCount;
            items[currentItemCount] = item;
            SortUp(item);
            currentItemCount++;
        }
    
        public T RemoveFirst()
        {
            T firstItem = items[0];
            currentItemCount--;
            items[0] = items[currentItemCount];
            items[0].HeapIndex = 0;
            SortDown(items[0]);
            return firstItem;
        }
    
        public void UpdateItem(T item)
        {
            SortUp(item);
        }
    
        public int Count
        {
            get
            {
                return currentItemCount;
            }
        }
    
        public bool Contains(T item)
        {
            return Equals(items[item.HeapIndex], item);
        }
    
        void SortDown(T item)
        {
            while (true)
            {
                int childIndexLeft = item.HeapIndex * 2 + 1;
                int childIndexRight = item.HeapIndex * 2 + 2;
                int swapIndex = 0;
    
                if (childIndexLeft < currentItemCount)
                {
                    swapIndex = childIndexLeft;
                    if(childIndexRight < currentItemCount)
                    {
                        if (items[childIndexLeft].CompareTo(items[childIndexRight]) < 0)
                        {
                            swapIndex = childIndexRight;
                        }
                    }
                    if(item.CompareTo(items[swapIndex]) < 0)
                    {
                        Swap(item, items[swapIndex]);
                    }
                    else
                    {
                        return;
                    }
                }
                else
                {
                    return;
                }
            }
        }
    
        void SortUp(T item)
        {
            int parentIndex = (item.HeapIndex - 1 / 2);
            while (true)
            {
                T parentItem = items[parentIndex];
                if (item.CompareTo(parentItem) > 0) //checks to see if node has higher priority or cost then parent node(node above node that connects to node)
                {
                    Swap(item, parentItem);
                }
                else
                {
                    break;
                }
            }
            
           
    
           }
        void Swap(T itemA, T itemB)
        {
            items[itemA.HeapIndex] = itemB;
            items[itemB.HeapIndex] = itemA;
            int itemAIndex = itemA.HeapIndex;
            itemA.HeapIndex = itemB.HeapIndex;
            itemB.HeapIndex = itemAIndex;
        }
    }
    
    
    
    public interface IHeapItem<T>: IComparable<T>
    {
        int HeapIndex
        {
            get;
            set;
        }
    }

I did not go through all your code or verify that everything is correct, that’s your job. However a common error is that the heuristic may over estimate the minimal cost in which case the whole algorithm won’t work anymore. That’s why you usually use the plain euclidean distance as heuristic. We don’t know your gCost per lateral / 2d diagonal / 3d diagonal move. Make sure it’s always at least as high as your heuristic. So a lateral move has to have a cost of 10 or greater.

ps: I just found an error in your binary heap implementation. This line is wrong:

int parentIndex = (item.HeapIndex - 1 / 2);

it should be

int parentIndex = (item.HeapIndex - 1) / 2;

because your line just sets parentIndex to item.HeapIndex since “1/2” is equal to “0”