diff --git a/include/warthog/memory/node_pool.h b/include/warthog/memory/node_pool.h index cc5f5e8..4d780ff 100644 --- a/include/warthog/memory/node_pool.h +++ b/include/warthog/memory/node_pool.h @@ -24,26 +24,109 @@ // #include "cpool.h" +#include +#include +#include +#include #include -#include - namespace warthog::memory { namespace node_pool_ns { -static const uint64_t NBS = 8; // node block size; set this >= 8 -static const uint64_t LOG2_NBS = 3; -static const uint64_t NBS_MASK = 7; +constexpr uint64_t LOG2_NBS = 6; // node block size = 2^n, n >= 3 +constexpr uint64_t NBS = 1 << LOG2_NBS; +constexpr uint64_t NBS_MASK = NBS - 1; +static_assert(LOG2_NBS >= 3, "must be at least 3 for size of 8"); } class node_pool { public: + struct data_deleter_ptr + { + void (*del)(void*) = nullptr; + + constexpr data_deleter_ptr() noexcept = default; + constexpr data_deleter_ptr(void (*p)(void*)) noexcept : del(p) { } + + void + operator()(void* data) const noexcept + { + (*del)(data); + } + }; + node_pool(); node_pool(size_t num_nodes); ~node_pool(); + template< + std::derived_from T = search::search_node, + typename... NodeArgs> + void + set_type(NodeArgs... node_args) noexcept + { + clear(); + using arg_type = std::tuple; + if constexpr(sizeof...(NodeArgs) != 0) + { + create_block_data_ = static_cast( + new arg_type(std::forward(node_args)...)); + } + block_type_sizes_ = sizeof(T); + size_t block_sz = node_pool_ns::NBS * sizeof(T); + blockspool_ = std::make_unique(block_sz, 1); + create_block_ = [](void* block_p, sn_id_t block_id, + void* data) noexcept { + assert(block_p != nullptr); + T* block = reinterpret_cast(block_p); + pad_id current_id = pad_id{block_id << node_pool_ns::LOG2_NBS}; + for(uint32_t i = 0; i < node_pool_ns::NBS; ++i, ++current_id.id) + { + if constexpr(sizeof...(NodeArgs) == 0) + { + std::construct_at(block + i, current_id); + } + else + { + assert(data != nullptr); + std::apply( + [block_i = block + i, + current_id](NodeX&&... args) { + std::construct_at(block_i, current_id, args...); + }, + *static_cast(data)); + } + } + }; + clear_ = [](node_pool& np) noexcept { + if(np.blocks_) + { + for(size_t i = 0; i < np.num_blocks_; ++i) + { + T* nodes = reinterpret_cast(np.blocks_[i]); + if(nodes != nullptr) + { + for(uint32_t j = 0; j < node_pool_ns::NBS; ++j) + { + std::destroy_at(nodes + j); + } + } + } + } + if constexpr(sizeof...(NodeArgs) == 0) + { + if(!np.create_block_data_) + { + arg_type* args + = static_cast(np.create_block_data_); + delete args; + } + } + }; + } + // return a warthog::search_node object corresponding to the given id. // if the node has already been generated, return a pointer to the // previous instance; otherwise allocate memory for a new object. @@ -58,13 +141,24 @@ class node_pool size_t mem(); + // reset nodes + void + clear(); + private: void init(size_t nblocks); + void + release(); - size_t num_blocks_; - search::search_node** blocks_; - cpool* blockspool_; + size_t num_blocks_ = 0; + std::unique_ptr blocks_; + size_t block_type_sizes_ = 0; + std::unique_ptr blockspool_; + void (*create_block_)(void* block, sn_id_t bock_id, void* data) noexcept + = nullptr; + void (*clear_)(node_pool& np) = nullptr; + void* create_block_data_; // uint64_t* node_init_; // uint64_t node_init_sz_; }; diff --git a/include/warthog/search/search_node.h b/include/warthog/search/search_node.h index 90b9eba..bdfa9e4 100644 --- a/include/warthog/search/search_node.h +++ b/include/warthog/search/search_node.h @@ -16,23 +16,15 @@ namespace warthog::search { -class search_node +struct search_node { -public: - search_node(pad_id id = pad_id::max()) - : id_(id), parent_id_(warthog::SN_ID_MAX), g_(warthog::COST_MAX), - f_(warthog::COST_MAX), ub_(warthog::COST_MAX), status_(0), - priority_(warthog::INF32), search_number_(UINT32_MAX) - { - refcount_++; - } - - ~search_node() { refcount_--; } + search_node() noexcept = default; + search_node(pad_id id) noexcept : id_(id) { } inline void init( uint32_t search_number, pad_id parent_id, cost_t g, cost_t f, - cost_t ub = warthog::COST_MAX) + cost_t ub = warthog::COST_MAX) noexcept { parent_id_ = parent_id; f_ = f; @@ -43,103 +35,103 @@ class search_node } inline uint32_t - get_search_number() const + get_search_number() const noexcept { return search_number_; } inline void - set_search_number(uint32_t search_number) + set_search_number(uint32_t search_number) noexcept { search_number_ = search_number; } inline pad_id - get_id() const + get_id() const noexcept { return id_; } inline void - set_id(pad_id id) + set_id(pad_id id) noexcept { id_ = id; } inline bool - get_expanded() const + get_expanded() const noexcept { return status_; } inline void - set_expanded(bool expanded) + set_expanded(bool expanded) noexcept { status_ = expanded; } inline pad_id - get_parent() const + get_parent() const noexcept { return parent_id_; } inline void - set_parent(pad_id parent_id) + set_parent(pad_id parent_id) noexcept { parent_id_ = parent_id; } inline uint32_t - get_priority() const + get_priority() const noexcept { return priority_; } inline void - set_priority(uint32_t priority) + set_priority(uint32_t priority) noexcept { priority_ = priority; } inline cost_t - get_g() const + get_g() const noexcept { return g_; } inline void - set_g(cost_t g) + set_g(cost_t g) noexcept { g_ = g; } inline cost_t - get_f() const + get_f() const noexcept { return f_; } inline void - set_f(cost_t f) + set_f(cost_t f) noexcept { f_ = f; } inline cost_t - get_ub() const + get_ub() const noexcept { return ub_; } inline void - set_ub(cost_t ub) + set_ub(cost_t ub) noexcept { ub_ = ub; } inline void - relax(cost_t g, pad_id parent_id) + relax(cost_t g, pad_id parent_id) noexcept { assert(g < g_); f_ = (f_ - g_) + g; @@ -149,7 +141,7 @@ class search_node } inline bool - operator<(const search_node& other) const + operator<(const search_node& other) const noexcept { // static uint64_t SIGN_MASK = UINT64_MAX & (1ULL<<63); // cost_t result = this->f_ - other.f_; @@ -170,7 +162,7 @@ class search_node } inline bool - operator>(const search_node& other) const + operator>(const search_node& other) const noexcept { if(f_ > other.f_) { return true; } if(f_ < other.f_) { return false; } @@ -181,14 +173,14 @@ class search_node } inline bool - operator==(const search_node& other) const + operator==(const search_node& other) const noexcept { if(!(*this < other) && !(*this > other)) { return true; } return false; } inline bool - operator<=(const search_node& other) const + operator<=(const search_node& other) const noexcept { if(*this < other) { return true; } if(!(*this > other)) { return true; } @@ -196,50 +188,34 @@ class search_node } inline bool - operator>=(const search_node& other) const + operator>=(const search_node& other) const noexcept { if(*this > other) { return true; } if(!(*this < other)) { return true; } return false; } - inline void - print(std::ostream& out) const - { - out << "search_node id:" << get_id().id; - out << " p_id: "; - out << parent_id_.id; - out << " g: " << g_ << " f: " << this->get_f() << " ub: " << ub_ - << " expanded: " << get_expanded() << " " - << " search_number_: " << search_number_; - } + void + print(std::ostream& out) const; uint32_t - mem() + mem() noexcept { return sizeof(*this); } - static uint32_t - get_refcount() - { - return refcount_; - } - -private: - pad_id id_; - pad_id parent_id_; + pad_id id_ = pad_id(warthog::SN_ID_MAX); + pad_id parent_id_ = pad_id(warthog::SN_ID_MAX); - cost_t g_; - cost_t f_; - cost_t ub_; + cost_t g_ = warthog::COST_MAX; + cost_t f_ = warthog::COST_MAX; + cost_t ub_ = warthog::COST_MAX; // TODO steal the high-bit from priority instead of ::status_ ? - uint8_t status_; // open or closed - uint32_t priority_; // expansion priority + uint8_t status_ = 0; // open or closed + uint32_t priority_ = warthog::INF32; // expansion priority - uint32_t search_number_; - static uint32_t refcount_; + uint32_t search_number_ = UINT32_MAX; }; struct cmp_less_search_node diff --git a/src/memory/node_pool.cpp b/src/memory/node_pool.cpp index d2df7a1..147bdae 100644 --- a/src/memory/node_pool.cpp +++ b/src/memory/node_pool.cpp @@ -5,16 +5,23 @@ namespace warthog::memory { -node_pool::node_pool(size_t num_nodes) : blocks_(0) +node_pool::node_pool() = default; +node_pool::node_pool(size_t num_nodes) { init(num_nodes); + set_type(); +} + +node_pool::~node_pool() +{ + clear(); } void node_pool::init(size_t num_nodes) { num_blocks_ = ((num_nodes) >> node_pool_ns::LOG2_NBS) + 1; - blocks_ = new search::search_node*[num_blocks_]; + blocks_ = std::make_unique(num_blocks_); for(size_t i = 0; i < num_blocks_; i++) { blocks_[i] = 0; @@ -25,26 +32,6 @@ node_pool::init(size_t num_nodes) // DEFAULT_CHUNK_SIZE and assign addresses // from that pool in order to generate blocks of nodes. when the pool is // full, cpool pre-allocates more, one chunk at a time. - size_t block_sz = node_pool_ns::NBS * sizeof(search::search_node); - blockspool_ = new cpool(block_sz, 1); -} - -node_pool::~node_pool() -{ - // delete [] node_init_; - - blockspool_->reclaim(); - delete blockspool_; - - for(size_t i = 0; i < num_blocks_; i++) - { - if(blocks_[i] != 0) - { - // std::cerr << "deleting block: "<> node_pool_ns::LOG2_NBS; sn_id_t list_id = sn_id_t{node_id} & node_pool_ns::NBS_MASK; - - // id outside the pool address range - if(block_id > num_blocks_) { return 0; } + assert(block_id < num_blocks_); // add a new block of nodes if necessary if(!blocks_[block_id]) { // std::cerr << "generating block: "<allocate()) - search::search_node[node_pool_ns::NBS]; - - // initialise memory - sn_id_t current_id = sn_id_t{node_id} - list_id; - for(uint32_t i = 0; i < node_pool_ns::NBS; i += 8) - { - new(&blocks_[block_id][i]) - search::search_node(pad_id{current_id++}); - new(&blocks_[block_id][i + 1]) - search::search_node(pad_id{current_id++}); - new(&blocks_[block_id][i + 2]) - search::search_node(pad_id{current_id++}); - new(&blocks_[block_id][i + 3]) - search::search_node(pad_id{current_id++}); - new(&blocks_[block_id][i + 4]) - search::search_node(pad_id{current_id++}); - new(&blocks_[block_id][i + 5]) - search::search_node(pad_id{current_id++}); - new(&blocks_[block_id][i + 6]) - search::search_node(pad_id{current_id++}); - new(&blocks_[block_id][i + 7]) - search::search_node(pad_id{current_id++}); - } + blocks_[block_id] + = reinterpret_cast(blockspool_->allocate()); + create_block_(blocks_[block_id], block_id, create_block_data_); } // return the node from its position in the assocated block - return &(blocks_[block_id][list_id]); + return reinterpret_cast( + blocks_[block_id] + list_id * block_type_sizes_); +} + +void +node_pool::release() +{ + num_blocks_ = 0; + blocks_ = nullptr; + blockspool_ = nullptr; + create_block_ = nullptr; } search::search_node* node_pool::get_ptr(pad_id node_id) { - sn_id_t block_id = sn_id_t{node_id} >> node_pool_ns::LOG2_NBS; - sn_id_t list_id = sn_id_t{node_id} & node_pool_ns::NBS_MASK; - - // id outside the pool address range - if(block_id > num_blocks_) { return 0; } - - if(!blocks_[block_id]) { return 0; } - return &(blocks_[block_id][list_id]); + assert((sn_id_t{node_id} >> node_pool_ns::LOG2_NBS) < num_blocks_); + sn_id_t block_id = static_cast(node_id) >> node_pool_ns::LOG2_NBS; + sn_id_t list_id = static_cast(node_id) & node_pool_ns::NBS_MASK; + assert(block_id < num_blocks_); + assert(blocks_[block_id] != nullptr); + return reinterpret_cast( + blocks_[block_id] + list_id * block_type_sizes_); } size_t @@ -112,4 +85,14 @@ node_pool::mem() return bytes; } +void +node_pool::clear() +{ + if(clear_) { (*clear_)(*this); } + blockspool_ = nullptr; + create_block_ = nullptr; + clear_ = nullptr; + create_block_data_ = nullptr; +} + } // namespace warthog::memory diff --git a/src/search/search_node.cpp b/src/search/search_node.cpp index 4e90feb..cae2985 100644 --- a/src/search/search_node.cpp +++ b/src/search/search_node.cpp @@ -1,6 +1,18 @@ #include -unsigned int warthog::search::search_node::refcount_ = 0; +namespace warthog::search +{ + +void +search_node::print(std::ostream& out) const +{ + out << "search_node id:" << get_id().id; + out << " p_id: "; + out << parent_id_.id; + out << " g: " << g_ << " f: " << this->get_f() << " ub: " << ub_ + << " expanded: " << get_expanded() << " " + << " search_number_: " << search_number_; +} std::ostream& operator<<(std::ostream& str, const warthog::search::search_node& sn) @@ -9,3 +21,5 @@ operator<<(std::ostream& str, const warthog::search::search_node& sn) return str; } + +} // namespace warthog::search