Fix bugs and cleanup

README.md

@@ -23,6 +23,7 @@ Looking for a C# version? Checkout the companion repository [astar-algorithm-csh
 
 [v1.3.1](https://github.com/justinhj/astar-algorithm-cpp/releases/tag/v1.3.1) 
 Bug fixes, safety hardening, and codebase modernization:
+- Fixed use-after-erase iterator bug in `SearchStep()` when reopening nodes from the closed list.
 - Guarded `FreeSolutionNodes()` against failed or uninitialized searches to eliminate potential use-after-free.
 - Updated `~FixedSizeAllocator` to properly invoke destructors on live objects, avoiding resource leaks when states hold non-trivial members.
 - Added double-free, alignment, and bounds validation to `FixedSizeAllocator::free()`.

stlastar.h

@@ -319,27 +319,28 @@ class AStarSearch {
                 // 3 - Sort heap again in open list
 
                 if (closedlist_result != m_ClosedList.end()) {
+                    Node* closed_node = *closedlist_result;
+
                     // Update closed node with successor node AStar data
-                    //*(*closedlist_result) = *(*successor);
-                    (*closedlist_result)->parent = (*successor)->parent;
-                    (*closedlist_result)->g = (*successor)->g;
-                    (*closedlist_result)->h = (*successor)->h;
-                    (*closedlist_result)->f = (*successor)->f;
+                    closed_node->parent = (*successor)->parent;
+                    closed_node->g = (*successor)->g;
+                    closed_node->h = (*successor)->h;
+                    closed_node->f = (*successor)->f;
 
                     // Free successor node
                     FreeNode((*successor));
 
-                    // Push closed node into open list
-                    (*closedlist_result)->heap_index = m_OpenList.size();
-                    m_OpenList.push_back((*closedlist_result));
-
                     // Remove closed node from closed list
                     m_ClosedList.erase(closedlist_result);
 
-                    siftUp((*closedlist_result)->heap_index);
+                    // Push closed node into open list
+                    closed_node->heap_index = m_OpenList.size();
+                    m_OpenList.push_back(closed_node);
+
+                    siftUp(closed_node->heap_index);
 
                     // Add to open set
-                    m_OpenSet.insert(*closedlist_result);
+                    m_OpenSet.insert(closed_node);
                     AssertHeapInvariants();
 
                     // Fix thanks to ...
@@ -354,17 +355,18 @@ class AStarSearch {
                 // 2 - sort heap again in open list
 
                 else if (openlist_result != m_OpenSet.end()) {
+                    Node* open_node = *openlist_result;
+
                     // Update open node with successor node AStar data
-                    //*(*openlist_result) = *(*successor);
-                    (*openlist_result)->parent = (*successor)->parent;
-                    (*openlist_result)->g = (*successor)->g;
-                    (*openlist_result)->h = (*successor)->h;
-                    (*openlist_result)->f = (*successor)->f;
+                    open_node->parent = (*successor)->parent;
+                    open_node->g = (*successor)->g;
+                    open_node->h = (*successor)->h;
+                    open_node->f = (*successor)->f;
 
                     // Free successor node
                     FreeNode((*successor));
 
-                    siftUp((*openlist_result)->heap_index);
+                    siftUp(open_node->heap_index);
                     AssertHeapInvariants();
                 }
 

tests.cpp

@@ -317,3 +317,63 @@ TEST_CASE("FixedSizeAllocator Guard Against Double Free") {
     allocator.free(p2);
 }
 
+class ReopenTestNode {
+   public:
+    int id;
+    ReopenTestNode() : id(0) {}
+    ReopenTestNode(int _id) : id(_id) {}
+
+    float GoalDistanceEstimate(ReopenTestNode& goal) {
+        if (id == 1) return 1.0f;   // B: heuristic 1 -> f = 10 + 1 = 11
+        if (id == 2) return 12.0f;  // C: heuristic 12 -> f = 1 + 12 = 13
+        if (id == 3) return 0.0f;   // Goal G
+        return 10.0f;
+    }
+    bool IsGoal(ReopenTestNode& goal) { return id == goal.id; }
+    bool GetSuccessors(AStarSearch<ReopenTestNode>* astar, ReopenTestNode* parent) {
+        if (id == 0) {  // A -> B (cost 10), A -> C (cost 1)
+            ReopenTestNode b(1), c(2);
+            astar->AddSuccessor(b);
+            astar->AddSuccessor(c);
+        } else if (id == 1) {  // B -> G (cost 100)
+            ReopenTestNode g(3);
+            astar->AddSuccessor(g);
+        } else if (id == 2) {  // C -> B (cost 1) which reopens B with cheaper cost (g=2 < 10)
+            ReopenTestNode b(1);
+            astar->AddSuccessor(b);
+        }
+        return true;
+    }
+    float GetCost(ReopenTestNode& successor) {
+        if (id == 0 && successor.id == 1) return 10.0f;
+        if (id == 0 && successor.id == 2) return 1.0f;
+        if (id == 2 && successor.id == 1) return 1.0f;
+        if (id == 1 && successor.id == 3) return 100.0f;
+        return 1.0f;
+    }
+    bool IsSameState(ReopenTestNode& rhs) { return id == rhs.id; }
+    size_t Hash() { return std::hash<int>{}(id); }
+};
+
+TEST_CASE("Reopen Node From Closed List When Cheaper Path Found") {
+    AStarSearch<ReopenTestNode> astar;
+    ReopenTestNode start(0);
+    ReopenTestNode goal(3);
+
+    astar.SetStartAndGoalStates(start, goal);
+
+    unsigned int state;
+    do {
+        state = astar.SearchStep();
+    } while (state == AStarSearch<ReopenTestNode>::SEARCH_STATE_SEARCHING);
+
+    CHECK(state == AStarSearch<ReopenTestNode>::SEARCH_STATE_SUCCEEDED);
+    // Path should be A(0) -> C(2) -> B(1) -> G(3) with total cost 1 + 1 + 100 = 102
+    // rather than A(0) -> B(1) -> G(3) with cost 10 + 100 = 110
+    CHECK(astar.GetSolutionCost() == doctest::Approx(102.0f));
+
+    astar.FreeSolutionNodes();
+    astar.EnsureMemoryFreed();
+}
+
+