A3- AStar Pathfinding

Intro

Abstract

This project implements an incremental variant of A* search, one of the most widely used pathfinding algorithms in games. By distributing the computation accross multiple frames, the algorithm avoids blocking the main game loop and helps maintain a stable frame rate. Another modification of A* allows it to return partial solutions when the goal cannot be found.

flowchart TD
    Start([Start: FindPathIncremental])
    Expand[Expand up to max nodes]
    Goal{Goal found?}
    Complete[Complete<br/>Path ready]
    Frontier{Frontier empty?}
    Partial[Partial<br/>Closest node returned]
    InProgress[InProgress<br/>More nodes can be expanded]

    Start --> Expand --> Goal
    Goal -- Yes --> Complete
    Goal -- No --> Frontier
    Frontier -- Yes --> Partial
    Frontier -- No --> InProgress

Incremental Search

• Each call to FindPathIncremental() processes a limited number of nodes (maxNumNodesToExplore) rather than the entire graph.
• The current state of the search is indicated by the enum PathSearchResultType
  • InProgress
  • Partial
  • Complete

Finding the closest node to the goal

• To find the closest node to the goal:
  • In the SearchNodeRecords of closedNodes, use the metadata to estimate the cost to the goal node
  • If nodes are tied, use the one with smallest CostSoFar
  • For null heuristic/Dijkstra’s, since h(n) = 0, compute the euclidean distance manually

A-star Implementation

• Increment FindPathIncremental(), which calculates an incremental update of A* and may be called several times before the goal is found (if possible)

AStarPathSearchImpl.cs

// compile_check
// Remove the line above if you are subitting to GradeScope for a grade. But leave it if you only want to check
// that your code compiles and the autograder can access your public methods.
 
using System.Collections;
using System.Collections.Generic;
using Priority_Queue;
using UnityEngine;
 
 
namespace GameAICourse
{
 
 
    public class AStarPathSearchImpl
    {
 
        // Please change this string to your name
        public const string StudentAuthorName = "George P. Burdell ← Not your name, change it!";
 
 
        // Null Heuristic for Dijkstra
        public static float HeuristicNull(Vector2 nodeA, Vector2 nodeB)
        {
            return 0f;
        }
 
        // Null Cost for Greedy Best First
        public static float CostNull(Vector2 nodeA, Vector2 nodeB)
        {
            return 0f;
        }
 
 
 
        // Heuristic distance fuction implemented with manhattan distance
        public static float HeuristicManhattan(Vector2 nodeA, Vector2 nodeB)
        {
            // The following code is just a placeholder so that the method has a valid return
            // You will replace it with the correct implementation
            return 0f;
 
            //END CODE 
        }
 
        // Heuristic distance function implemented with Euclidean distance
        public static float HeuristicEuclidean(Vector2 nodeA, Vector2 nodeB)
        {
            //STUDENT CODE HERE
 
            // The following code is just a placeholder so that the method has a valid return
            // You will replace it with the correct implementation
            return 0f;
 
            //END CODE 
        }
 
 
        // Cost is only ever called on adjacent nodes. So we will always use Euclidean distance.
        // We could use Manhattan dist for 4-way connected grids and avoid sqrroot and mults.
        // But we will avoid that for simplicity.
        public static float Cost(Vector2 nodeA, Vector2 nodeB)
        {
            //STUDENT CODE HERE
 
            // The following code is just a placeholder so that the method has a valid return
            // You will replace it with the correct implementation
            return 0f;
 
            //END STUDENT CODE
        }
 
 
 
        public static PathSearchResultType FindPathIncremental(
            GetNodeCount getNodeCount,
            GetNode getNode,
            GetNodeAdjacencies getAdjacencies,
            CostCallback G,
            CostCallback H,
            int startNodeIndex, int goalNodeIndex,
            int maxNumNodesToExplore, bool doInitialization,
            ref int currentNodeIndex,
            ref Dictionary<int, PathSearchNodeRecord> searchNodeRecords,
            ref SimplePriorityQueue<int, float> openNodes, ref HashSet<int> closedNodes, ref List<int> returnPath)
        {
            PathSearchResultType pathResult = PathSearchResultType.InProgress;
 
            var nodeCount = getNodeCount();
 
            if (startNodeIndex >= nodeCount || goalNodeIndex >= nodeCount ||
                startNodeIndex < 0 || goalNodeIndex < 0 ||
                maxNumNodesToExplore <= 0 ||
                (!doInitialization &&
                 (openNodes == null || closedNodes == null || currentNodeIndex < 0 ||
                  currentNodeIndex >= nodeCount )))
 
                return PathSearchResultType.InitializationError;
 
 
            // STUDENT CODE HERE
 
            // The following code is just a placeholder so that the method has a valid return
            // You will replace it with the correct implementation
 
            pathResult = PathSearchResultType.Complete;
 
            searchNodeRecords = new Dictionary<int, PathSearchNodeRecord>();
            openNodes = new SimplePriorityQueue<int, float>();
            closedNodes = new HashSet<int>();
 
            returnPath = new List<int>();
 
            returnPath.Add(startNodeIndex);
 
            return pathResult;
 
            //END STUDENT CODE HERE
        }
 
    }
 
}

Incremental graph search (DFS/BFS)

• A sample incremental search program, BasicPathSearchImpl.cs, can be used as a guide
  • It does NOT use heuristics (so it’s not A*). It’s basically a priority-queue–based BFS/DFS hybrid that runs in chunks

BasicPathSearchImpl.cs

using System.Collections;
using System.Collections.Generic;
using Priority_Queue;
using UnityEngine;
public class BasicPathSearchImpl
{
    static float G(Vector2 a, Vector2 b)
    {
        return Vector2.Distance(a, b);
    }
    public static PathSearchResultType FindPathIncremental(
       //List<Vector2> nodes,
       //List<List<int>> edges,
       GetNodeCount getNodeCount,
       GetNode getNode,
       GetNodeAdjacencies adjacencies,
       int startNodeIndex, int goalNodeIndex,
       bool IsBFS, //true for BFS, false for DFS,
       bool randomExpansion, // true if expanded edges added in random order
       int maxNumNodesToExplore, bool doInitialization,
       ref int currentNodeIndex,
       ref Dictionary<int, PathSearchNodeRecord> searchNodeRecords,
       ref SimplePriorityQueue<int, float> openNodes, ref HashSet<int> closedNodes, ref List<int> returnPath)
    {
        var nodeCount = getNodeCount();
        PathSearchResultType pathResult = PathSearchResultType.InProgress;
        if (
            //nodes == null ||
            startNodeIndex >= nodeCount || goalNodeIndex >= nodeCount ||
            //edges == null || startNodeIndex >= edges.Count || goalNodeIndex >= edges.Count ||
            //edges.Count != nodes.Count ||
            startNodeIndex < 0 || goalNodeIndex < 0 ||
            maxNumNodesToExplore <= 0 ||
            (!doInitialization &&
             (openNodes == null || closedNodes == null || currentNodeIndex < 0 ||
              currentNodeIndex >= nodeCount
              //|| currentNodeIndex >= edges.Count
              )))
        {
            searchNodeRecords = new Dictionary<int, PathSearchNodeRecord>();
            openNodes = new SimplePriorityQueue<int, float>();
            closedNodes = new HashSet<int>();
 
            return PathSearchResultType.InitializationError;
        }
 
        //float max_dfs_priority = Mathf.Pow(2f, 20f);
 
        if (doInitialization)
        {
            currentNodeIndex = startNodeIndex;
            searchNodeRecords = new Dictionary<int, PathSearchNodeRecord>();
            openNodes = new SimplePriorityQueue<int, float>();
            closedNodes = new HashSet<int>();
            var firstNodeRecord = new PathSearchNodeRecord(currentNodeIndex);
            searchNodeRecords.Add(firstNodeRecord.NodeIndex, firstNodeRecord);
            //float startingPriority = IsBFS ? 0f : max_dfs_priority;
            float startingPriority = 0f;
            openNodes.Enqueue(firstNodeRecord.NodeIndex, startingPriority);
            returnPath = new List<int>();
        }
 
        float currentPriority = 0f;
 
        if (openNodes.Count > 0)
            currentPriority = openNodes.GetPriority(openNodes.First);
 
        // DFS priorities go negative so our priority queue always gives the most recently explored nodes
        // we take ABS here to minimize changes to logic elsewhere in the code
        currentPriority = Mathf.Abs(currentPriority);
 
        int nodesProcessed = 0;
        while (nodesProcessed < maxNumNodesToExplore && openNodes.Count > 0)
        {
            //Find the smallest element in the open list using the estimated total cost
            var currentNodeRecord = searchNodeRecords[openNodes.First]; 
            currentNodeIndex = currentNodeRecord.NodeIndex;
 
   
            ++nodesProcessed;
 
            // goal check should be in edge expansion for better time performance!
            // However, doing so means goal found sooner. But if we are using DFS
            // we are probably intentionally looking for long paths...
            if (currentNodeIndex == goalNodeIndex)
                break;
 
 
            bool updateOpen = false;
 
            // In range [0.0, 1.0]
            // if DFS with randomExpansion, smaller value results in tighter
            // path. Larger value more meandering path.
            float updateOpenThreshold = 0.5f;
 
 
            PathSearchNodeRecord edgeNodeRecord = null;
 
            //var currEdges = edges[currentNodeIndex];
            var currEdges = adjacencies(currentNodeIndex);
 
            // Just for fun. If random expansion is enabled, DFS can generate funny paths
            if (randomExpansion)
            {
                List<int> shuffleEdges = new List<int>(currEdges);
 
                currEdges = new List<int>(shuffleEdges.Count);
 
                while(shuffleEdges.Count > 0)
                {
                    int i = Random.Range(0, shuffleEdges.Count);
                    currEdges.Add(shuffleEdges[i]);
                    shuffleEdges.RemoveAt(i);
                }
            }
            
   
            foreach (var edgeNodeIndex in currEdges)
            {
                var costToEdgeNode = currentNodeRecord.CostSoFar +
                    G(getNode(currentNodeIndex), getNode(edgeNodeIndex));
 
 
                if (closedNodes.Contains(edgeNodeIndex))
                {
                    continue;
                }
                else if(openNodes.Contains(edgeNodeIndex))
                {
 
                    if (!IsBFS)
                    {
                        if (randomExpansion)
                            updateOpen = Random.value > updateOpenThreshold;
                        else
                            updateOpen = true;
                    }
 
 
                    if (updateOpen)
                        edgeNodeRecord = searchNodeRecords[edgeNodeIndex];
                    else
                        continue;
                }
                else
                {
                    edgeNodeRecord = new PathSearchNodeRecord(edgeNodeIndex);      
                }
 
                currentPriority += 1;
 
                edgeNodeRecord.FromNodeIndex = currentNodeIndex;
         
                searchNodeRecords[edgeNodeIndex] = edgeNodeRecord;
 
                // This simple trick allows this code to support both BFS and DFS.
                var newPriority = IsBFS ? currentPriority : -currentPriority;
 
                if (!openNodes.Contains(edgeNodeIndex))
                {
                    openNodes.Enqueue(edgeNodeIndex, newPriority);
                }
                else
                {
                    if (IsBFS)
                        Debug.LogError("Shouldn't rewrite open set in BFS!");
 
                    if(updateOpen)
                        openNodes.UpdatePriority(edgeNodeIndex, newPriority);
 
                }
 
            } //foreach() edge processing of current node
 
            openNodes.Remove(currentNodeIndex);
            closedNodes.Add(currentNodeIndex);
        } //while
        if (openNodes.Count <= 0 && currentNodeIndex != goalNodeIndex)
        {
            pathResult = PathSearchResultType.Partial;
            //find the closest node we looked at and use for partial path
            int closest = -1;
            float closestDist = float.MaxValue;
            foreach (var n in closedNodes)
            {
                var nrec = searchNodeRecords[n];
 
                // Hmmm. I bet if we were using this partial path code with an A* implementation 
                // we could avoid calculating Euclidean distance again using cached metadata.
                // (But we would need to deduce whether said code was running in Dijkstra
                // mode with a zero constant Heuristic() func)
                // Otherwise, we must calculate the distance
                var d = Vector2.Distance(getNode(nrec.NodeIndex), getNode(goalNodeIndex));
                if (d < closestDist)
                {
                    closest = n;
                    closestDist = d;
                }
            }
            if (closest >= 0)
            {
                currentNodeIndex = closest;
            }
        }
        else if (currentNodeIndex == goalNodeIndex)
        {
            pathResult = PathSearchResultType.Complete;
        }
 
 
        if (pathResult != PathSearchResultType.InProgress)
        {
            // processing complete, a path (possibly partial) can be generated returned
            returnPath = new List<int>();
            while (currentNodeIndex != startNodeIndex)
            {
                returnPath.Add(currentNodeIndex);
                currentNodeIndex = searchNodeRecords[currentNodeIndex].FromNodeIndex;
            }
            returnPath.Add(startNodeIndex);
            returnPath.Reverse();
        }
 
        return pathResult;
 
    }
}

Priority Queue

• The SimplePriorityQueue<int, float> simulates queue and stack behavior
  • BFS: Elements added with increasing priority
  • DFS: Elements added with negative increasing priority

Millington Pseudocode

• Use BasicPathSearchImpl.cs and the Millington pseduocode below for reference:

Millington pseduocode

# https://learning.oreilly.com/library/view/ai-for-games/9781351053280/
 
def pathfindAStar(graph: Graph, start: Node, goal: Node, heuristic: Heuristic) -> List[Connection]:
 
    # This structure is used to keep track of the
    # information we need for each node
    class NodeRecord:
        node: Node
        connection: Connection
        costSoFar: float
        estimatedTotalCost: float
 
    # Initialize the record for the start node
    startRecord = NodeRecord()
    startRecord.node = start
    startRecord.connection = None
    startRecord.costSoFar = 0
    startRecord.estimatedTotalCost = heuristic.estimate(start)
 
    # Initialize the open and closed lists
    open = PathfindingList()
    open += startRecord
    closed = PathfindingList()
 
    # Iterate through processing each node
    while len(open) > 0:
        
        # Find the smallest element in the open list (using the estimatedTotalCost)
        current = open.smallestElement()
 
        # If it is the goal node, then terminate
        if current.node == goal: 
            break
 
        # Otherwise get its outgoing connections
        connections = graph.getConnections(current.node)
 
        # Loop through each connection in turn
        for connection in connections:
 
            # Get the cost estimate for the end node
            endNode = connection.getToNode()
            endNodeCost = current.costSoFar + connection.getCost()
 
            # If the node is closed we may have to skip, or remove it from the closed list.
            if closed.contains(endNode):
 
                # Here we find the record in the closed list corresponding to the endNode.
                endNodeRecord = closed.find(endNode)
 
                # If we didn't find a shorter route, skip
                if endNodeRecord.costSoFar <= endNodeCost:
                    continue
 
                # Otherwise remove it from the closed list
                closed -= endNodeRecord
 
                # We can use the node's old cost values to calculate its heuristic without calling
                # the possibly expensive heuristic function
                # endNodeHeuristic = endNodeRecord.cost - endNodeRecord.costSoFar
                endNodeHeuristic = endNodeRecord.estimatedTotalCost - endNodeRecord.costSoFar
 
            # Skip if the node is open and we've not found a better route
            elif open.contains(endNode):
 
                # Here we find the record in the open list corresponding to the endNode.
                endNodeRecord = open.find(endNode)
 
                # If our route is no better, then skip
                if endNodeRecord.costSoFar <= endNodeCost:
                    continue
 
                # We can use the node's old cost values to calculate its heuristic without calling
                # the possibly expensive heuristic function
                # endNodeHeuristic = endNodeRecord.cost - endNodeRecord.costSoFar
                endNodeHeuristic = endNodeRecord.estimatedTotalCost - endNodeRecord.costSoFar
 
            # Otherwise we know we've got an unvisited node, so make a record for it
            else:
                endNodeRecord = NodeRecord()
                endNodeRecord.node = endNode
 
                # We'll need to calculate the heuristic value using the function, 
                # since we don't have an existing record to use
                endNodeHeuristic = heuristic.estimate(endNode)
 
            # We're here if we need to update the node
            # Update the cost, estimate and connection
            # endNodeRecord.cost = endNodeCost
            endNodeRecord.costSoFar = endNodeCost
            endNodeRecord.connection = connection
            endNodeRecord.estimatedTotalCost = endNodeCost + endNodeHeuristic
 
            ## TODO be careful!
           # IF the Open set already contains the node then you need to update the node's priority to the newest estimatedTotalCost if it changed!
            # And add it to the open list
            if not open.contains(endNode):
                open += endNodeRecord
 
        # We've finished looking at the connections for
        # the current node, so add it to the closed list
        # and remove it from the open list
        open -= current
        closed += current
 
    # We're here if we've either found the goal, or if we've no more nodes to search, find which.
    if current.node != goal:
        # We've run out of nodes without finding the goal, so there's no solution
        return None
 
    else:
        # Compile the list of connections in the path
        path = []
 
        # Work back along the path, accumulating connections
        while current.node != start:
            path += current.connection
            current = current.connection.getFromNode()
 
        # Reverse the path, and return it
        return reverse(path)

Unit Tests

AStarTest.cs

using System.Collections;
using System.Collections.Generic;
using NUnit.Framework;
using UnityEngine;
using UnityEngine.TestTools;
 
using GameAICourse;
 
namespace Tests
{
    public class AStarTest
    {
        // You can run the tests in this class in the Unity Editor if you open
        // the Test Runner Window and choose the EditMode tab
 
 
        // Annotate methods with [Test] or [TestCase(...)] to create tests like this one!
        [Test]
        public void TestName()
        {
            // Tests are performed through assertions. You can Google NUnit Assertion
            // documentation to learn about them. Several examples follow.
            Assert.That(AStarPathSearchImpl.StudentAuthorName, Is.Not.Contains("George P. Burdell"),
                "You forgot to change to your name!");
        }
 
 
        [TestCase(true)]
        [TestCase(false)]
        public void BasicTest(bool incrementalSearch)
        {
 
            Vector2[] _nodes = new Vector2[] {
                new Vector2(0.0f, 0.0f), //0
                new Vector2(0.0f, 1.0f), //1
                new Vector2(0.0f, 2.0f), //2
                new Vector2(0.0f, 3.0f), //3
                new Vector2(0.0f, 4.0f), //4
                new Vector2(0.0f, 5.0f), //5
            };
            int[][] _edges = new int[][] {
                new int[] {1 },         //0
                new int[] {2, 0 },      //1
                new int[] {3, 1 },      //2
                new int[] {4, 2 },      //3
                new int[] {5, 3 },      //4
                new int[] {4 }          //5
            };
 
            List<Vector2> nodes = new List<Vector2>(_nodes);
            List<List<int>> edges = new List<List<int>>();
 
            foreach (var eArray in _edges)
            {
                var elist = new List<int>(eArray);
                edges.Add(elist);
            }
 
            CostCallback G = AStarPathSearchImpl.Cost;
            CostCallback H = AStarPathSearchImpl.HeuristicEuclidean;
 
            var startNode = 0;
            var goalNode = nodes.Count - 1;
 
            var maxNumNodesToExplore = incrementalSearch ? 1 : int.MaxValue;
 
            int currentNodeIndex = 0;
 
            Dictionary<int, PathSearchNodeRecord> searchNodeRecord = null;
 
            Priority_Queue.SimplePriorityQueue<int, float> openNodes = null;
 
            HashSet<int> closedNodes = null;
 
            List<int> returnPath = null;
 
            var ret = PathSearchResultType.InProgress;
 
            int attempts = 0;
 
            int maxAllowedAttempts = 20;
 
            do
            {
                var init = attempts <= 0;
 
                ++attempts;
 
                ret = AStarPathSearchImpl.FindPathIncremental(
                    () => { return nodes.Count; },
                    (nindex) => { return nodes[nindex]; },
                    (nindex) => { return edges[nindex]; },
                    G, H,
                    startNode, goalNode, maxNumNodesToExplore, init,
                    ref currentNodeIndex, ref searchNodeRecord, ref openNodes, ref closedNodes,
                    ref returnPath);
            }
            while (ret == PathSearchResultType.InProgress && attempts < maxAllowedAttempts);
 
            Debug.Log($"Number of updates: {attempts}");
 
            Assert.That(ret, Is.EqualTo(PathSearchResultType.Complete));
            Assert.That(returnPath, Does.Contain(goalNode));
 
            if (incrementalSearch)
                Assert.That(attempts, Is.GreaterThan(1));
            else
                Assert.That(attempts, Is.EqualTo(1));
 
            // TODO write some good assertions as this test is incomplete
 
        }
 
        // TODO write more tests!
 
    }
}

SubmissionNavMesh Generation

AStarPathSearchImpl.cs

using System;
using System.Collections;
using System.Collections.Generic;
using Codice.Client.Common.TreeGrouper;
using JetBrains.Annotations;
using NUnit.Framework.Constraints;
 
using Priority_Queue;
using UnityEngine;
 
 
namespace GameAICourse
{
 
    public class AStarPathSearchImpl
    {
        // Null Heuristic for Dijkstra
        public static float HeuristicNull(Vector2 nodeA, Vector2 nodeB)
        {
            return 0f;
        }
 
        // Null Cost for Greedy Best First
        public static float CostNull(Vector2 nodeA, Vector2 nodeB)
        {
            return 0f;
        }
 
        // Heuristic distance fuction implemented with manhattan distance
        public static float HeuristicManhattan(Vector2 nodeA, Vector2 nodeB)
        {
            float x1 = nodeA.x, y1 = nodeA.y, x2 = nodeB.x, y2 = nodeB.y;
            float d = Math.Abs(x2 - x1) + Math.Abs(y2 - y1);
 
            return d;
        }
 
        // Heuristic distance function implemented with Euclidean distance
        public static float HeuristicEuclidean(Vector2 nodeA, Vector2 nodeB)
        {
            float x1 = nodeA.x, y1 = nodeA.y, x2 = nodeB.x, y2 = nodeB.y;
            float dx = (x2 - x1), dy = (y2 - y1);
            float d = Mathf.Sqrt(dx*dx + dy*dy);
 
            return d;
        }
 
 
        // Cost is only ever called on adjacent nodes. So we will always use Euclidean distance.
        // We could use Manhattan dist for 4-way connected grids and avoid sqrroot and mults.
        // But we will avoid that for simplicity.
        public static float Cost(Vector2 nodeA, Vector2 nodeB)
        {
            float cost = HeuristicEuclidean(nodeA, nodeB);
            return cost;
        }
 
 
 
        public static PathSearchResultType FindPathIncremental(
            GetNodeCount getNodeCount,
            GetNode getNode,
            GetNodeAdjacencies getAdjacencies,
            CostCallback G,
            CostCallback H,
            int startNodeIndex, int goalNodeIndex,
            int maxNumNodesToExplore, bool doInitialization,
            ref int currentNodeIndex,
            ref Dictionary<int, PathSearchNodeRecord> searchNodeRecords,
            ref SimplePriorityQueue<int, float> openNodes, ref HashSet<int> closedNodes, ref List<int> returnPath)
        {
            PathSearchResultType pathResult = PathSearchResultType.InProgress;
 
            var nodeCount = getNodeCount();
 
            if (startNodeIndex >= nodeCount || goalNodeIndex >= nodeCount ||
                startNodeIndex < 0 || goalNodeIndex < 0 ||
                maxNumNodesToExplore <= 0 ||
                (!doInitialization &&
                 (openNodes == null || closedNodes == null || currentNodeIndex < 0 ||
                  currentNodeIndex >= nodeCount )))
            {
                searchNodeRecords = new Dictionary<int, PathSearchNodeRecord>();
                openNodes = new SimplePriorityQueue<int, float>();
                closedNodes = new HashSet<int>();
                return PathSearchResultType.InitializationError;
            }
 
            Vector2 start = getNode(startNodeIndex);
            Vector2 goal = getNode(goalNodeIndex);
 
            // Initialize start node record 
            if (doInitialization)
            {
                // Debug.Log($"maxNumNodesToExplore: {maxNumNodesToExplore} | Nodes: {nodeCount} | startNodeIndex: {startNodeIndex} | goalNodeIndex: {goalNodeIndex}");
                searchNodeRecords = new Dictionary<int, PathSearchNodeRecord>();
                var firstNodeRecord = new PathSearchNodeRecord(startNodeIndex);
                searchNodeRecords.Add(startNodeIndex, firstNodeRecord);
                firstNodeRecord.CostSoFar = 0;
                float h = H(start, goal);
                firstNodeRecord.EstimatedTotalCost = h;
                // Initalize frontier and visited 
                openNodes = new SimplePriorityQueue<int, float>();
                openNodes.Enqueue(firstNodeRecord.NodeIndex, h);
                closedNodes = new HashSet<int>();
                returnPath = new List<int>();
            }
 
            // References: 
            // - BasicPathSearchImpl.cs
            // - Millington Pseudocode
            // - CS6601 A1 (Spring 2024)
            // Iteratively process nodes
            int nodesProcessed = 0;
            while (nodesProcessed < maxNumNodesToExplore && openNodes.Count > 0)
            {
                currentNodeIndex = openNodes.Dequeue();
                nodesProcessed++;
                var currentNodeRecord = searchNodeRecords[currentNodeIndex];
                var f1 = currentNodeRecord.EstimatedTotalCost;
                var g1 = currentNodeRecord.CostSoFar;
                // Debug.Log($"\n[DEBUG] currentNodeINdex:{currentNodeIndex} | f1: {f1} | g1:{g1}");
                // Goal check
                if (currentNodeIndex == goalNodeIndex)
                {
                    // Debug.Log("Expanded goal node");
                    break;
                }
 
                // Add node index to visited set
                closedNodes.Add(currentNodeIndex);
 
                // Check neighbor nodes
                foreach (var neighborIndex in getAdjacencies(currentNodeIndex))
                {
                    // Debug.Log($"NeighborIndex: {neighborIndex}");
                    Vector2 neighbor = getNode(neighborIndex);
 
                    // Get the cost for expanding neighbor node
                    var g2 = g1 + G(getNode(currentNodeIndex), neighbor);
                    var f2 = g2 + H(neighbor,goal);
                    // Debug.Log($"[DEBUG] g1:{g1}, G:{G(getNode(currentNodeIndex), getNode(neighborIndex))}");
                    // Debug.Log($"[DEBUG] H:{H(neighbor,goal)}");
                    // f(n) = g(n) + h(n) = g2 + h(neighbor)
                    // Debug.Log($"[DEBUG] g2:{g2}, f2:{f2}");
 
                    if (!closedNodes.Contains(neighborIndex) && !openNodes.Contains(neighborIndex))
                    {
                        // Create new edge/record
                        var neighborNodeRecord = new PathSearchNodeRecord(neighborIndex); 
                        neighborNodeRecord.FromNodeIndex = currentNodeIndex; // Update [From current node] -> neighbor
                        neighborNodeRecord.CostSoFar = g2;
                        neighborNodeRecord.EstimatedTotalCost = f2;
                        // Add index/node record to record list
                        searchNodeRecords.Add(neighborIndex, neighborNodeRecord);
                        // Add to forntier
                        openNodes.Enqueue(neighborNodeRecord.NodeIndex, f2);
                        // Debug.Log($"[DEBUG] neighborIndex:{neighborIndex} added to frontier");
                        
                    }
                    else if (openNodes.Contains(neighborIndex))
                    {
                        // If node already in frontier, if shorter path found, update its priority with new estimatedTotalCost
                        var oldPriority = openNodes.GetPriority(neighborIndex);
                        if (f2 < oldPriority)
                        {
                            openNodes.UpdatePriority(neighborIndex, f2);
                            var neighborNodeRecord = searchNodeRecords[neighborIndex];
                            neighborNodeRecord.FromNodeIndex = currentNodeIndex;
                            neighborNodeRecord.CostSoFar = g2;
                            neighborNodeRecord.EstimatedTotalCost = f2;
                            // Debug.Log($"[DEBUG] Updated neighborNode {neighborIndex}'s priority from {oldPriority} to {f2}");
                        }
                    }
 
                    
                }
            }
 
            static void FormReturnPath(int currentNodeIndex, int startNodeIndex, ref List<int> returnPath, ref Dictionary<int, PathSearchNodeRecord> searchNodeRecords)
            {
                // Debug.Log($"[DEBUG] FormReturnPath currentNodeIndex:{currentNodeIndex}");
                while(currentNodeIndex != startNodeIndex)
                {
                    returnPath.Add(currentNodeIndex);
                    currentNodeIndex = searchNodeRecords[currentNodeIndex].FromNodeIndex;
                }
                returnPath.Add(startNodeIndex);
                returnPath.Reverse();
 
            }
 
            if (currentNodeIndex == goalNodeIndex)
            {
                pathResult = PathSearchResultType.Complete;
                // Debug.Log("Forming completed path");
            }
            else if (openNodes.Count <= 0)
            {
                pathResult = PathSearchResultType.Partial;
                // Debug.Log("Forming partial path");
 
                int closestToGoalIndex; 
                var startNodeRecord = searchNodeRecords[startNodeIndex];
                float h1 = startNodeRecord.EstimatedTotalCost;
                bool isNullHeuristic = h1 == 0 ? true : false;
                float closestToGoalCost = Single.PositiveInfinity;
                int smallestCostSoFarIndex;
                float smallestCostsoFar = Single.PositiveInfinity; 
 
                foreach (int nodeIndex in closedNodes)
                {
                    var nodeRecord = searchNodeRecords[nodeIndex];
                    float costSoFar = nodeRecord.CostSoFar;
 
                    // f1 = g1 + h1 ->  h1 = f1 - g1
                    if (isNullHeuristic)
                    {
                        // For null heuristic/Dijkstra’s, since h(n) = 0, compute the euclidean distance manually
                        h1 = HeuristicEuclidean(getNode(nodeIndex), goal);                  
                    }
                    else
                    {
                        h1 = nodeRecord.EstimatedTotalCost - nodeRecord.CostSoFar;
                    }
                    // Debug.Log($"[DEBUG] h1:{h1}, closetToGoalCost:{closestToGoalCost}");
                    if (h1 < closestToGoalCost)
                    {
                        closestToGoalIndex = nodeIndex;
                        closestToGoalCost = h1;
                        // set as "goal" index for path formation below
                        currentNodeIndex = nodeIndex;
                    } 
                    // If nodes are tied, use the one with smallest CostSoFar
                    else if (Mathf.Approximately(h1, closestToGoalCost))
                    {
                        // Debug.Log("Nodes tied, using smaller CostSoFar");
                        if (costSoFar < smallestCostsoFar){
                            closestToGoalIndex = nodeIndex;
                            closestToGoalCost = h1;  
                            currentNodeIndex = nodeIndex;
                        }
                        
                    }
                    // Update smallest cost so far
                    if (costSoFar < smallestCostsoFar){
                        smallestCostSoFarIndex = nodeIndex;
                        smallestCostsoFar = costSoFar;
                    }
                }
            }
 
            if (pathResult != PathSearchResultType.InProgress)
            {
                FormReturnPath(currentNodeIndex, startNodeIndex, ref returnPath, ref searchNodeRecords);
                // Debug.Log($"{pathResult} Return path: {string.Join(", ", returnPath)}\n");
            }
 
            return pathResult;
        }
 
    }
 
}