- A
std::vector
to store your nodes. You can stably refer to them by index now.A
std::vector
to store your nodes. You can stably refer to them by index now. - A
std::unordered_map
(or at least astd::map
if the external index cannot be hashed) to map from the external identifiers to internal indices into the vector.A
std::unordered_map
(or at least astd::map
if the external index cannot be hashed) to map from the external identifiers to internal indices into the vector.You should use a custom allocator for this, considering your specific use-case a simple linear allocator would work wonders.
- A
std::priority_queue
to store the workitems.A
std::priority_queue
to store the workitems.
template <class T, class F1, class F2>
std::vector<T> path(T start, T target, F1 heuristic, F2 next) {
using Cost = decltype(heuristic(start, target)
+ std::get<0>(next(start).begin()));
struct Node {
const T data;
std::size_t parent;
Cost cost;
};
struct Open {
Cost cost;
std::size_t id;
bool operator<(const Open& rhs) { return cost > rhs.cost; };
};
std::vector<Node> nodes {{ start, 0, heuristic(start, target)}};
std::unordered_map<T, std::size_t> ids {{ nodes[0].data, 0}};
// ^^ Find or write a simple linear allocator for all those nodes
std::priority_queue<Open> open;
open.emplace(nodes[0].cost, 0);
while (!open.empty()) {
auto [cost, id] = open.top();
open.pop();
if (nodes[id].cost != cost)
continue;
if (nodes[id].data == target) {
std::vector<T> r;
for (; id; id = nodes[id].parent)
r.push_back(nodes[id].data);
r.push_back(nodes[id].data);
std::reverse(r.begin(), r.end());
return r;
}
cost -= heuristic(nodes[id].data, target);
for (auto&& [weight, data] : next(nodes[id].data)) {
auto [p, added] = ids.try_emplace(data, nodes.size());
auto estimate = cost + weight + heuristic(data, target);
if (added)
nodes.emplace_back(data, id, estimate);
else if (estimate < nodes[p->second()].cost)
nodes[p->second()].cost = estimate;
else
continue;
open.emplace(estimate, p->second());
}
}
return {};
}