Optimize the open list

[justinhj]

Optimize open list with indexed binary heap and O(1) state lookup

Previously, open list membership checks performed an O(N) linear scan
over std::vector<Node*> m_OpenList for every generated successor,
scaling quadratically with search frontier size. Furthermore, when a
cheaper path to an existing open node was found (decrease-key),
std::make_heap was called to re-heapify the entire vector in O(N).

This change combines two optimizations to make open list operations
strictly O(1) lookup and O(log N) updates:

1. O(1) State Membership Lookup (m_OpenSet):
   - Added std::unordered_set<Node*, NodeHash, NodeEqual> m_OpenSet
     alongside m_OpenList, mirroring m_ClosedList.
   - Replaced linear scan of m_OpenList with m_OpenSet.find(*successor),
     dropping membership checks from O(N) to O(1).
   - Maintained m_OpenSet across push, pop, reopen, and cleanup sites.

2. Indexed Binary Heap:
   - Added `size_t heap_index` to Node, initialized to SIZE_MAX
     ("not on heap").
   - Implemented swapNodes(i, j), siftUp(i), and siftDown(i) using the
     existing HeapCompare_f ordering.
   - Replaced push_heap call sites (start node, new successor, reopen)
     with push_back + siftUp.
   - Replaced pop_heap + pop_back with swapping root to back, pop_back,
     setting popped node's heap_index to SIZE_MAX, and siftDown(0).
   - Replaced O(N) make_heap on decrease-key with O(log N) siftUp on
     the updated node's heap_index.
   - Added AssertHeapInvariants() in debug mode to verify heap indices
     and ordering invariants across all operations.

Benchmark Results (20,000 searches on 1000x1000 grid):

+-------------------------------------+------------+-----------+--------------+
| Stage                               | Total Time | Avg/Search| Success/Fail |
+-------------------------------------+------------+-----------+--------------+
| Baseline (linear scan + make_heap)  |   5.539 s  | 276.96 us |  391 / 19609 |
| OpenSet (O(1) lookup + make_heap)   |   4.839 s  | 241.94 us |  391 / 19609 |
| Indexed Heap (OpenSet + siftUp/Down)|   4.495 s  | 224.77 us |  435 / 19565 |
+-------------------------------------+------------+-----------+--------------+
Total speedup: ~19% reduction in execution time over baseline.

Note: SiftUp localizes heap updates instead of globally rebuilding via
make_heap, which alters equal-f tie-breaking order. Under the fixed
1,000-node memory budget, this enabled 44 additional marginal searches
to reach the goal before exhausting nodes.