diff --git a/src/pathfinder/npf/aystar.cpp b/src/pathfinder/npf/aystar.cpp index 3ca787722f..227c73d389 100644 --- a/src/pathfinder/npf/aystar.cpp +++ b/src/pathfinder/npf/aystar.cpp @@ -35,9 +35,9 @@ */ PathNode *AyStar::ClosedListIsInList(const AyStarNode *node) { - const auto result = this->closedlist_hash.find(std::make_pair(node->tile, node->direction)); + const auto result = this->closedlist_hash.find(this->HashKey(node->tile, node->direction)); - return (result == this->closedlist_hash.end()) ? nullptr : result->second; + return (result == this->closedlist_hash.end()) ? nullptr : this->closedlist_nodes[result->second]; } /** @@ -48,22 +48,22 @@ PathNode *AyStar::ClosedListIsInList(const AyStarNode *node) void AyStar::ClosedListAdd(const PathNode *node) { /* Add a node to the ClosedList */ - const auto new_node = MallocT(1); - *new_node = *node; + std::pair new_node = this->closedlist_nodes.Allocate(); + *(new_node.second) = *node; - this->closedlist_hash[std::make_pair(node->node.tile, node->node.direction)] = new_node; + this->closedlist_hash[this->HashKey(node->node.tile, node->node.direction)] = new_node.first; } /** * Check whether a node is in the open list. * @param node Node to search. - * @return If the node is available, it is returned, else \c nullptr is returned. + * @return If the node is available, it is returned, else \c UINT32_MAX is returned. */ -OpenListNode *AyStar::OpenListIsInList(const AyStarNode *node) +uint32 AyStar::OpenListIsInList(const AyStarNode *node) { - const auto result = this->openlist_hash.find(std::make_pair(node->tile, node->direction)); + const auto result = this->openlist_hash.find(this->HashKey(node->tile, node->direction)); - return (result == this->openlist_hash.end()) ? nullptr : result->second; + return (result == this->openlist_hash.end()) ? UINT32_MAX : result->second; } /** @@ -71,15 +71,16 @@ OpenListNode *AyStar::OpenListIsInList(const AyStarNode *node) * It deletes the returned node from the open list. * @returns the best node available, or \c nullptr of none is found. */ -OpenListNode *AyStar::OpenListPop() +std::pair AyStar::OpenListPop() { /* Return the item the Queue returns.. the best next OpenList item. */ - OpenListNode *res = (OpenListNode*)this->openlist_queue.Pop(); - if (res != nullptr) { - this->openlist_hash.erase(std::make_pair(res->path.node.tile, res->path.node.direction)); - } + uint32 idx = this->openlist_queue.Pop(); + if (idx == UINT32_MAX) return std::pair(idx, nullptr); - return res; + OpenListNode *res = this->openlist_nodes[idx]; + this->openlist_hash.erase(this->HashKey(res->path.node.tile, res->path.node.direction)); + + return std::make_pair(idx, res); } /** @@ -89,14 +90,16 @@ OpenListNode *AyStar::OpenListPop() void AyStar::OpenListAdd(PathNode *parent, const AyStarNode *node, int f, int g) { /* Add a new Node to the OpenList */ - OpenListNode *new_node = MallocT(1); + uint32 idx; + OpenListNode *new_node; + std::tie(idx, new_node) = this->openlist_nodes.Allocate(); new_node->g = g; new_node->path.parent = parent; new_node->path.node = *node; - this->openlist_hash[std::make_pair(node->tile, node->direction)] = new_node; + this->openlist_hash[this->HashKey(node->tile, node->direction)] = idx; /* Add it to the queue */ - this->openlist_queue.Push(new_node, f); + this->openlist_queue.Push(idx, f); } /** @@ -106,7 +109,6 @@ void AyStar::CheckTile(AyStarNode *current, OpenListNode *parent) { int new_f, new_g, new_h; PathNode *closedlist_parent; - OpenListNode *check; /* Check the new node against the ClosedList */ if (this->ClosedListIsInList(current) != nullptr) return; @@ -134,22 +136,23 @@ void AyStar::CheckTile(AyStarNode *current, OpenListNode *parent) closedlist_parent = this->ClosedListIsInList(&parent->path.node); /* Check if this item is already in the OpenList */ - check = this->OpenListIsInList(current); - if (check != nullptr) { - uint i; + uint32 check_idx = this->OpenListIsInList(current); + if (check_idx != UINT32_MAX) { + OpenListNode *check = this->openlist_nodes[check_idx]; + /* Yes, check if this g value is lower.. */ if (new_g >= check->g) return; - this->openlist_queue.Delete(check, 0); + this->openlist_queue.Delete(check_idx, 0); /* It is lower, so change it to this item */ check->g = new_g; check->path.parent = closedlist_parent; /* Copy user data, will probably have changed */ - for (i = 0; i < lengthof(current->user_data); i++) { + for (uint i = 0; i < lengthof(current->user_data); i++) { check->path.node.user_data[i] = current->user_data[i]; } /* Re-add it in the openlist_queue. */ - this->openlist_queue.Push(check, new_f); + this->openlist_queue.Push(check_idx, new_f); } else { /* A new node, add it to the OpenList */ this->OpenListAdd(closedlist_parent, current, new_f, new_g); @@ -170,7 +173,9 @@ int AyStar::Loop() int i; /* Get the best node from OpenList */ - OpenListNode *current = this->OpenListPop(); + OpenListNode *current; + uint32 current_idx; + std::tie(current_idx, current) = this->OpenListPop(); /* If empty, drop an error */ if (current == nullptr) return AYSTAR_EMPTY_OPENLIST; @@ -179,7 +184,7 @@ int AyStar::Loop() if (this->FoundEndNode != nullptr) { this->FoundEndNode(this, current); } - free(current); + this->openlist_nodes.Free(current_idx, current); return AYSTAR_FOUND_END_NODE; } @@ -196,7 +201,7 @@ int AyStar::Loop() } /* Free the node */ - free(current); + this->openlist_nodes.Free(current_idx, current); if (this->max_search_nodes != 0 && this->closedlist_hash.size() >= this->max_search_nodes) { /* We've expanded enough nodes */ @@ -212,16 +217,10 @@ int AyStar::Loop() */ void AyStar::Free() { - this->openlist_queue.Free(false); - /* 2nd argument above is false, below is true, to free the values only - * once */ - for (const auto& pair : this->openlist_hash) { - free(pair.second); - } + this->openlist_queue.Free(); + this->openlist_nodes.Clear(); this->openlist_hash.clear(); - for (const auto& pair : this->closedlist_hash) { - free(pair.second); - } + this->closedlist_nodes.Clear(); this->closedlist_hash.clear(); #ifdef AYSTAR_DEBUG printf("[AyStar] Memory free'd\n"); @@ -234,18 +233,14 @@ void AyStar::Free() */ void AyStar::Clear() { - /* Clean the Queue, but not the elements within. That will be done by - * the hash. */ - this->openlist_queue.Clear(false); + /* Clean the Queue. */ + this->openlist_queue.Clear(); + /* Clean the hashes */ - for (const auto& pair : this->openlist_hash) { - free(pair.second); - } + this->openlist_nodes.Clear(); this->openlist_hash.clear(); - for (const auto& pair : this->closedlist_hash) { - free(pair.second); - } + this->closedlist_nodes.Clear(); this->closedlist_hash.clear(); #ifdef AYSTAR_DEBUG @@ -315,10 +310,6 @@ void AyStar::Init(uint num_buckets) MemSetT(&neighbours, 0); MemSetT(&openlist_queue, 0); - /* Allocated the Hash for the OpenList and ClosedList */ - this->openlist_hash.reserve(num_buckets); - this->closedlist_hash.reserve(num_buckets); - /* Set up our sorting queue * BinaryHeap allocates a block of 1024 nodes * When that one gets full it reserves another one, till this number diff --git a/src/pathfinder/npf/aystar.h b/src/pathfinder/npf/aystar.h index 0026c1df79..72b1411afe 100644 --- a/src/pathfinder/npf/aystar.h +++ b/src/pathfinder/npf/aystar.h @@ -17,12 +17,14 @@ #define AYSTAR_H #include "queue.h" -#include #include #include "../../tile_type.h" #include "../../track_type.h" +#include "../../core/pod_pool.hpp" +#include "../../3rdparty/robin_hood/robin_hood.h" + /** Return status of #AyStar methods. */ enum AystarStatus { AYSTAR_FOUND_END_NODE, ///< An end node was found. @@ -166,13 +168,20 @@ struct AyStar { void CheckTile(AyStarNode *current, OpenListNode *parent); protected: - std::unordered_map, PathNode*, PairHash> closedlist_hash; + + inline uint32 HashKey(TileIndex tile, Trackdir td) const { return tile | (td << 28); } + + PodPool closedlist_nodes; + robin_hood::unordered_flat_map closedlist_hash; + BinaryHeap openlist_queue; ///< The open queue. - std::unordered_map, OpenListNode*, PairHash> openlist_hash; + + PodPool openlist_nodes; + robin_hood::unordered_flat_map openlist_hash; void OpenListAdd(PathNode *parent, const AyStarNode *node, int f, int g); - OpenListNode *OpenListIsInList(const AyStarNode *node); - OpenListNode *OpenListPop(); + uint32 OpenListIsInList(const AyStarNode *node); + std::pair OpenListPop(); void ClosedListAdd(const PathNode *node); PathNode *ClosedListIsInList(const AyStarNode *node); diff --git a/src/pathfinder/npf/queue.cpp b/src/pathfinder/npf/queue.cpp index a6a920a009..7893e6b4bf 100644 --- a/src/pathfinder/npf/queue.cpp +++ b/src/pathfinder/npf/queue.cpp @@ -25,31 +25,18 @@ const int BinaryHeap::BINARY_HEAP_BLOCKSIZE_MASK = BinaryHeap::BINARY_HEAP_BLOCK /** * Clears the queue, by removing all values from it. Its state is - * effectively reset. If free_items is true, each of the items cleared - * in this way are free()'d. + * effectively reset. */ -void BinaryHeap::Clear(bool free_values) +void BinaryHeap::Clear() { /* Free all items if needed and free all but the first blocks of memory */ uint i; - uint j; for (i = 0; i < this->blocks; i++) { if (this->elements[i] == nullptr) { /* No more allocated blocks */ break; } - /* For every allocated block */ - if (free_values) { - for (j = 0; j < (1 << BINARY_HEAP_BLOCKSIZE_BITS); j++) { - /* For every element in the block */ - if ((this->size >> BINARY_HEAP_BLOCKSIZE_BITS) == i && - (this->size & BINARY_HEAP_BLOCKSIZE_MASK) == j) { - break; // We're past the last element - } - free(this->elements[i][j].item); - } - } if (i != 0) { /* Leave the first block of memory alone */ free(this->elements[i]); @@ -62,14 +49,13 @@ void BinaryHeap::Clear(bool free_values) /** * Frees the queue, by reclaiming all memory allocated by it. After - * this it is no longer usable. If free_items is true, any remaining - * items are free()'d too. + * this it is no longer usable. */ -void BinaryHeap::Free(bool free_values) +void BinaryHeap::Free() { uint i; - this->Clear(free_values); + this->Clear(); for (i = 0; i < this->blocks; i++) { if (this->elements[i] == nullptr) break; free(this->elements[i]); @@ -81,7 +67,7 @@ void BinaryHeap::Free(bool free_values) * Pushes an element into the queue, at the appropriate place for the queue. * Requires the queue pointer to be of an appropriate type, of course. */ -bool BinaryHeap::Push(void *item, int priority) +bool BinaryHeap::Push(uint32 item, int priority) { if (this->size == this->max_size) return false; dbg_assert(this->size < this->max_size); @@ -130,7 +116,7 @@ bool BinaryHeap::Push(void *item, int priority) * known, which speeds up the deleting for some queue's. Should be -1 * if not known. */ -bool BinaryHeap::Delete(void *item, int priority) +bool BinaryHeap::Delete(uint32 item, int priority) { uint i = 0; @@ -189,14 +175,12 @@ bool BinaryHeap::Delete(void *item, int priority) * Pops the first element from the queue. What exactly is the first element, * is defined by the exact type of queue. */ -void *BinaryHeap::Pop() +uint32 BinaryHeap::Pop() { - void *result; - - if (this->size == 0) return nullptr; + if (this->size == 0) return UINT32_MAX; /* The best item is always on top, so give that as result */ - result = this->GetElement(1).item; + uint32 result = this->GetElement(1).item; /* And now we should get rid of this item... */ this->Delete(this->GetElement(1).item, this->GetElement(1).priority); diff --git a/src/pathfinder/npf/queue.h b/src/pathfinder/npf/queue.h index 738d9d9a11..f46e513ba4 100644 --- a/src/pathfinder/npf/queue.h +++ b/src/pathfinder/npf/queue.h @@ -14,7 +14,7 @@ struct BinaryHeapNode { - void *item; + uint32 item; int priority; }; @@ -30,11 +30,11 @@ struct BinaryHeap { void Init(uint max_size); - bool Push(void *item, int priority); - void *Pop(); - bool Delete(void *item, int priority); - void Clear(bool free_values); - void Free(bool free_values); + bool Push(uint32 item, int priority); + uint32 Pop(); + bool Delete(uint32 item, int priority); + void Clear(); + void Free(); /** * Get an element from the #elements.