Skip to content

Optimize the open list #40

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
justinhj merged 4 commits into from
Sep 26, 2026
Merged

Optimize the open list #40

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
7424a4c
Optimize the open list
justinhj Sep 26, 2026
08387c5
revert some churn
justinhj Sep 26, 2026
3ee685b
Second stage of optimization
justinhj Sep 26, 2026
eeb211c
Update README with 1.3 release info
justinhj Sep 26, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,12 @@ Looking for a C# version? Checkout the companion repository [astar-algorithm-csh

### Release notes

[v1.3](https://github.com/justinhj/astar-algorithm-cpp/releases/tag/v1.3)
Performance optimizations for the open list and addition of a reproducible benchmark suite:
- Open list state membership lookup is now O(1) using an `unordered_set`, eliminating the previous O(N) linear search per successor.
- The open list binary heap is now an indexed heap with each node tracking its `heap_index`, replacing O(N) `std::make_heap` operations on decrease-key with O(log N) sift-up operations.
- Added a 1,000,000-search reproducible 2D grid benchmark (`bench.cpp`) and modernized unit testing with doctest.

[v1.2](https://github.com/justinhj/astar-algorithm-cpp/releases/tag/v1.2)
Breaking changes! C++ 11 is now the minimum required C++ standard.
User is now required to provide a Hash function for their Node type. Thanks to a contribution from @btdubs the closed
Expand Down
132 changes: 103 additions & 29 deletions stlastar.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,15 +26,15 @@ given where due.
#ifndef STLASTAR_H
#define STLASTAR_H
// used for text debugging
#include <stdio.h>

#include <assert.h>
#include <stdio.h>

// stl includes
#include <algorithm>
#include <cfloat>
#include <cstdint>
#include <unordered_set>
#include <vector>
#include <algorithm>

// fast fixed size memory allocator, used for fast node memory management
#include "fsa.h"
Expand Down Expand Up @@ -78,7 +78,9 @@ class AStarSearch {
float h; // heuristic estimate of distance to goal
float f; // sum of cumulative cost of predecessors and self and heuristic

Node() : parent(0), child(0), g(0.0f), h(0.0f), f(0.0f) {}
size_t heap_index; // index in m_OpenList, or SIZE_MAX when not on the heap

Node() : parent(0), child(0), g(0.0f), h(0.0f), f(0.0f), heap_index(SIZE_MAX) {}

bool operator==(const Node& otherNode) const {
return this->m_UserState.IsSameState(otherNode.m_UserState);
Expand Down Expand Up @@ -148,10 +150,12 @@ class AStarSearch {

// Push the start node on the Open list

m_OpenList.push_back(m_Start); // heap now unsorted
m_Start->heap_index = m_OpenList.size();
m_OpenList.push_back(m_Start);
siftUp(m_Start->heap_index);

// Sort back element into heap
push_heap(m_OpenList.begin(), m_OpenList.end(), HeapCompare_f());
m_OpenSet.insert(m_Start);
AssertHeapInvariants();

// Initialise counter for search steps
m_Steps = 0;
Expand Down Expand Up @@ -181,8 +185,14 @@ class AStarSearch {

// Pop the best node (the one with the lowest f)
Node* n = m_OpenList.front(); // get pointer to the node
pop_heap(m_OpenList.begin(), m_OpenList.end(), HeapCompare_f());
swapNodes(0, m_OpenList.size() - 1);
m_OpenList.pop_back();
n->heap_index = SIZE_MAX;
if (!m_OpenList.empty()) {
siftDown(0);
}
m_OpenSet.erase(n);
AssertHeapInvariants();

// Check for the goal, once we pop that we're done
if (n->m_UserState.IsGoal(m_Goal->m_UserState)) {
Expand Down Expand Up @@ -257,18 +267,10 @@ class AStarSearch {
// If it is but the node that is already on them is better (lower g)
// then we can forget about this successor

// First linear search of open list to find node
typename std::unordered_set<Node*, NodeHash, NodeEqual>::iterator openlist_result;
openlist_result = m_OpenSet.find(*successor);

typename std::vector<Node*>::iterator openlist_result;

for (openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end();
openlist_result++) {
if ((*openlist_result)->m_UserState.IsSameState((*successor)->m_UserState)) {
break;
}
}

if (openlist_result != m_OpenList.end()) {
if (openlist_result != m_OpenSet.end()) {
// we found this state on open

if ((*openlist_result)->g <= newg) {
Expand Down Expand Up @@ -319,13 +321,17 @@ class AStarSearch {
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);

// Sort back element into heap
push_heap(m_OpenList.begin(), m_OpenList.end(), HeapCompare_f());
siftUp((*closedlist_result)->heap_index);

// Add to open set
m_OpenSet.insert(*closedlist_result);
AssertHeapInvariants();

// Fix thanks to ...
// Greg Douglas <gregdouglasmail@gmail.com>
Expand All @@ -338,7 +344,7 @@ class AStarSearch {
// 1 - Update old version of this node in open list
// 2 - sort heap again in open list

else if (openlist_result != m_OpenList.end()) {
else if (openlist_result != m_OpenSet.end()) {
// Update open node with successor node AStar data
//*(*openlist_result) = *(*successor);
(*openlist_result)->parent = (*successor)->parent;
Expand All @@ -349,11 +355,8 @@ class AStarSearch {
// Free successor node
FreeNode((*successor));

// re-make the heap
// make_heap rather than sort_heap is an essential bug fix
// thanks to Mike Ryynanen for pointing this out and then explaining
// it in detail. sort_heap called on an invalid heap does not work
make_heap(m_OpenList.begin(), m_OpenList.end(), HeapCompare_f());
siftUp((*openlist_result)->heap_index);
AssertHeapInvariants();
}

// New successor
Expand All @@ -362,10 +365,14 @@ class AStarSearch {

else {
// Push successor node into open list
(*successor)->heap_index = m_OpenList.size();
m_OpenList.push_back((*successor));

// Sort back element into heap
push_heap(m_OpenList.begin(), m_OpenList.end(), HeapCompare_f());
siftUp((*successor)->heap_index);

// Add to open set
m_OpenSet.insert(*successor);
AssertHeapInvariants();
}
}

Expand Down Expand Up @@ -568,6 +575,68 @@ class AStarSearch {
}

private: // methods
void swapNodes(size_t i, size_t j) {
if (i == j) return;
std::swap(m_OpenList[i], m_OpenList[j]);
m_OpenList[i]->heap_index = i;
m_OpenList[j]->heap_index = j;
}

void siftUp(size_t i) {
HeapCompare_f compare;
while (i > 0) {
size_t parent = (i - 1) / 2;
if (compare(m_OpenList[parent], m_OpenList[i])) {
swapNodes(parent, i);
i = parent;
} else {
break;
}
}
}

void siftDown(size_t i) {
HeapCompare_f compare;
size_t size = m_OpenList.size();
while (true) {
size_t smallest = i;
size_t left = 2 * i + 1;
size_t right = 2 * i + 2;

if (left < size && compare(m_OpenList[smallest], m_OpenList[left])) {
smallest = left;
}
if (right < size && compare(m_OpenList[smallest], m_OpenList[right])) {
smallest = right;
}

if (smallest != i) {
swapNodes(i, smallest);
i = smallest;
} else {
break;
}
}
}

void AssertHeapInvariants() const {
#if !defined(NDEBUG)
HeapCompare_f compare;
for (size_t i = 0; i < m_OpenList.size(); ++i) {
assert(m_OpenList[i] != nullptr);
assert(m_OpenList[i]->heap_index == i);
size_t left = 2 * i + 1;
size_t right = 2 * i + 2;
if (left < m_OpenList.size()) {
assert(!compare(m_OpenList[i], m_OpenList[left]));
}
if (right < m_OpenList.size()) {
assert(!compare(m_OpenList[i], m_OpenList[right]));
}
}
#endif
}

// This is called when a search fails or is cancelled to free all used
// memory
void FreeAllNodes() {
Expand All @@ -576,12 +645,14 @@ class AStarSearch {

while (iterOpen != m_OpenList.end()) {
Node* n = (*iterOpen);
n->heap_index = SIZE_MAX;
FreeNode(n);

iterOpen++;
}

m_OpenList.clear();
m_OpenSet.clear();

// iterate closed list and delete unused nodes
typename std::unordered_set<Node*, NodeHash, NodeEqual>::iterator iterClosed;
Expand All @@ -607,6 +678,7 @@ class AStarSearch {

while (iterOpen != m_OpenList.end()) {
Node* n = (*iterOpen);
n->heap_index = SIZE_MAX;

if (!n->child) {
FreeNode(n);
Expand All @@ -618,6 +690,7 @@ class AStarSearch {
}

m_OpenList.clear();
m_OpenSet.clear();

// iterate closed list and delete unused nodes
typename std::unordered_set<Node*, NodeHash, NodeEqual>::iterator iterClosed;
Expand Down Expand Up @@ -679,6 +752,7 @@ class AStarSearch {
}
};
std::unordered_set<Node*, NodeHash, NodeEqual> m_ClosedList;
std::unordered_set<Node*, NodeHash, NodeEqual> m_OpenSet;

// Successors is a vector filled out by the user each type successors to a node
// are generated
Expand Down
Loading