Edit:
Ok so after @harold and @Deduplicator gave me a couple pieces of advise (Big thanks btw) I ended up with the following implementation. Performance win is not huge but there is one, about 100-300 ms for 8game solve and 50 ms for two dimensional vectors.
namespace {
// the reason why this structure is not in closure
// is because of operator< has to be friend
// and friend is only available outside closure
struct Open {
size_t id; // vector stack id instead of ptr
uint32_t g, h; // still separate variables
explicit Open(size_t id, uint32_t g, uint32_t h) : id(id), g(g), h(h) {}
// comparator for priority queue
friend bool operator<(Open lhs, Open rhs) { return lhs.g + lhs.h > rhs.g + rhs.h; }
};
} // namespace
/**
* @brief generic a* algorithm
*
* @tparam T any type with operator< and operator==
* @tparam Heuristic function, returns uint32_t
* @tparam Mutator function returns iterable of all next states of node
* @param initial initial state of node
* @param expected expected state of node
* @param heuristic function like manhattan or euclidean distance
* @param next next mutator
* @return std::vector<T> representing path from expected to initial
*/
template <class T, class Heuristic, class Mutator>
std::vector<T> path(const T& initial, const T& expected,
Heuristic heuristic, Mutator next, size_t nextWeight = 1) {
struct Node {
const T data; // actual data
size_t parent; // index to parent instead of ptr
uint32_t f;
explicit Node(const T& data, size_t parent, uint32_t f)
: data(data), parent(parent), f(f) {
}
};
std::vector<Node> nodes{Node(initial, 0, heuristic(initial, expected))}; // as was suggested
// now pq, there is no need to iterate through underlying vector anymore
std::priority_queue<Open> opened;
// map because i don't want to force user implement hash function for their data
std::map<T, size_t> closed{{initial, 0}};
// initial state
opened.emplace(0, 0, nodes[0].f);
while (not opened.empty()) {
// pop opened into curr
const auto curr = opened.top();
opened.pop();
// if goal found
if (nodes[curr.id].data == expected) {
// no lambda for better performance?
std::vector<T> result;
result.reserve(curr.g / nextWeight); // using g for our advantage
auto id = curr.id;
for (; id; id = nodes[id].parent)
result.push_back(std::move(nodes[id].data)); // move because we don't need this data anymore in closure
result.push_back(std::move(nodes[id].data));
return result; // not reversing this is users responsibility
}
// foreach node in successors
for (auto&& child : next(nodes[curr.id].data)) {
// trying to insert into closed map
const auto [it, inserted] = closed.try_emplace(child, nodes.size());
// precomputing g and h values
const auto g = curr.g + nextWeight;
const auto h = heuristic(child, expected);
// data wasn't in closed creating new node in nodes vector
if (inserted)
nodes.emplace_back(child, curr.id, g + h);
// if f cost of existed node is worse than current just updating cost
else if (g + h < nodes[it->second].f)
nodes[it->second].f = g + h;
else
continue;
// finally constructing new open node in open queue
opened.emplace(it->second, g, h);
}
}
return {}; // path not found
}
