From 4bac39354981da7c2357bde749eecff653809081 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robin=20H=C3=BCbner?= Date: Fri, 16 Aug 2019 00:22:52 +0200 Subject: [PATCH] astar performance improvements, use oahashmap --- core/math/a_star.cpp | 239 +++++++++++++++++++++++++------------------ core/math/a_star.h | 26 ++--- core/oa_hash_map.h | 34 +++++- 3 files changed, 185 insertions(+), 114 deletions(-) diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index b61119d8df4..aea42a1edf8 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -40,7 +40,17 @@ int AStar::get_available_point_id() const { return 1; } - return points.back()->key() + 1; + // calculate our new next available point id if bigger than before or next id already contained in set of points. + if (points.has(last_free_id)) { + int cur_new_id = last_free_id; + while (points.has(cur_new_id)) { + cur_new_id++; + } + int &non_const = const_cast(last_free_id); + non_const = cur_new_id; + } + + return last_free_id; } void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { @@ -48,7 +58,10 @@ void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { ERR_FAIL_COND(p_id < 0); ERR_FAIL_COND(p_weight_scale < 1); - if (!points.has(p_id)) { + Point *found_pt; + bool p_exists = points.lookup(p_id, found_pt); + + if (!p_exists) { Point *pt = memnew(Point); pt->id = p_id; pt->pos = p_pos; @@ -57,84 +70,98 @@ void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) { pt->open_pass = 0; pt->closed_pass = 0; pt->enabled = true; - points[p_id] = pt; + points.set(p_id, pt); } else { - points[p_id]->pos = p_pos; - points[p_id]->weight_scale = p_weight_scale; + found_pt->pos = p_pos; + found_pt->weight_scale = p_weight_scale; } } Vector3 AStar::get_point_position(int p_id) const { - ERR_FAIL_COND_V(!points.has(p_id), Vector3()); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND_V(!p_exists, Vector3()); - return points[p_id]->pos; + return p->pos; } void AStar::set_point_position(int p_id, const Vector3 &p_pos) { - ERR_FAIL_COND(!points.has(p_id)); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND(!p_exists); - points[p_id]->pos = p_pos; + p->pos = p_pos; } real_t AStar::get_point_weight_scale(int p_id) const { - ERR_FAIL_COND_V(!points.has(p_id), 0); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND_V(!p_exists, 0); - return points[p_id]->weight_scale; + return p->weight_scale; } void AStar::set_point_weight_scale(int p_id, real_t p_weight_scale) { - ERR_FAIL_COND(!points.has(p_id)); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND(!p_exists); ERR_FAIL_COND(p_weight_scale < 1); - points[p_id]->weight_scale = p_weight_scale; + p->weight_scale = p_weight_scale; } void AStar::remove_point(int p_id) { - ERR_FAIL_COND(!points.has(p_id)); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND(!p_exists); - Point *p = points[p_id]; + for (OAHashMap::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { - for (Set::Element *E = p->neighbours.front(); E; E = E->next()) { - - Segment s(p_id, E->get()->id); + Segment s(p_id, (*it.key)); segments.erase(s); - E->get()->neighbours.erase(p); - E->get()->unlinked_neighbours.erase(p); + (*it.value)->neighbours.remove(p->id); + (*it.value)->unlinked_neighbours.remove(p->id); } - for (Set::Element *E = p->unlinked_neighbours.front(); E; E = E->next()) { + for (OAHashMap::Iterator it = p->unlinked_neighbours.iter(); it.valid; it = p->unlinked_neighbours.next_iter(it)) { - Segment s(p_id, E->get()->id); + Segment s(p_id, (*it.key)); segments.erase(s); - E->get()->neighbours.erase(p); - E->get()->unlinked_neighbours.erase(p); + (*it.value)->neighbours.remove(p->id); + (*it.value)->unlinked_neighbours.remove(p->id); } memdelete(p); - points.erase(p_id); + points.remove(p_id); + last_free_id = p_id; } void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) { - ERR_FAIL_COND(!points.has(p_id)); - ERR_FAIL_COND(!points.has(p_with_id)); ERR_FAIL_COND(p_id == p_with_id); - Point *a = points[p_id]; - Point *b = points[p_with_id]; - a->neighbours.insert(b); + Point *a; + bool from_exists = points.lookup(p_id, a); + ERR_FAIL_COND(!from_exists); - if (bidirectional) - b->neighbours.insert(a); - else - b->unlinked_neighbours.insert(a); + Point *b; + bool to_exists = points.lookup(p_with_id, b); + ERR_FAIL_COND(!to_exists); + + a->neighbours.set(b->id, b); + + if (bidirectional) { + b->neighbours.set(a->id, a); + } else { + b->unlinked_neighbours.set(a->id, a); + } Segment s(p_id, p_with_id); if (s.from == p_id) { @@ -147,6 +174,7 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) { segments.insert(s); } + void AStar::disconnect_points(int p_id, int p_with_id) { Segment s(p_id, p_with_id); @@ -154,12 +182,18 @@ void AStar::disconnect_points(int p_id, int p_with_id) { segments.erase(s); - Point *a = points[p_id]; - Point *b = points[p_with_id]; - a->neighbours.erase(b); - a->unlinked_neighbours.erase(b); - b->neighbours.erase(a); - b->unlinked_neighbours.erase(a); + Point *a; + bool a_exists = points.lookup(p_id, a); + CRASH_COND(!a_exists); + + Point *b; + bool b_exists = points.lookup(p_with_id, b); + CRASH_COND(!b_exists); + + a->neighbours.remove(b->id); + a->unlinked_neighbours.remove(b->id); + b->neighbours.remove(a->id); + b->unlinked_neighbours.remove(a->id); } bool AStar::has_point(int p_id) const { @@ -171,8 +205,8 @@ Array AStar::get_points() { Array point_list; - for (const Map::Element *E = points.front(); E; E = E->next()) { - point_list.push_back(E->key()); + for (OAHashMap::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { + point_list.push_back(*(it.key)); } return point_list; @@ -180,14 +214,14 @@ Array AStar::get_points() { PoolVector AStar::get_point_connections(int p_id) { - ERR_FAIL_COND_V(!points.has(p_id), PoolVector()); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND_V(!p_exists, PoolVector()); PoolVector point_list; - Point *p = points[p_id]; - - for (Set::Element *E = p->neighbours.front(); E; E = E->next()) { - point_list.push_back(E->get()->id); + for (OAHashMap::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { + point_list.push_back((*it.key)); } return point_list; @@ -201,9 +235,9 @@ bool AStar::are_points_connected(int p_id, int p_with_id) const { void AStar::clear() { - for (const Map::Element *E = points.front(); E; E = E->next()) { - - memdelete(E->get()); + last_free_id = 0; + for (OAHashMap::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { + memdelete(*(it.value)); } segments.clear(); points.clear(); @@ -214,14 +248,14 @@ int AStar::get_closest_point(const Vector3 &p_point) const { int closest_id = -1; real_t closest_dist = 1e20; - for (const Map::Element *E = points.front(); E; E = E->next()) { + for (OAHashMap::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) { - if (!E->get()->enabled) - continue; //Disabled points should not be considered - real_t d = p_point.distance_squared_to(E->get()->pos); + if (!(*it.value)->enabled) continue; // Disabled points should not be considered. + + real_t d = p_point.distance_squared_to((*it.value)->pos); if (closest_id < 0 || d < closest_dist) { closest_dist = d; - closest_id = E->key(); + closest_id = *(it.key); } } @@ -230,8 +264,8 @@ int AStar::get_closest_point(const Vector3 &p_point) const { Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const { - real_t closest_dist = 1e20; bool found = false; + real_t closest_dist = 1e20; Vector3 closest_point; for (const Set::Element *E = segments.front(); E; E = E->next()) { @@ -262,8 +296,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { pass++; - if (!end_point->enabled) - return false; + if (!end_point->enabled) return false; bool found_route = false; @@ -272,13 +305,9 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { begin_point->g_score = 0; begin_point->f_score = _estimate_cost(begin_point->id, end_point->id); - open_list.push_back(begin_point); - while (true) { - - if (open_list.size() == 0) // No path found - break; + while (!open_list.empty()) { Point *p = open_list[0]; // The currently processed point @@ -291,24 +320,23 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { open_list.remove(open_list.size() - 1); p->closed_pass = pass; // Mark the point as closed - for (Set::Element *E = p->neighbours.front(); E; E = E->next()) { + for (OAHashMap::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) { - Point *e = E->get(); // The neighbour point + Point *e = *(it.value); // The neighbour point - if (!e->enabled || e->closed_pass == pass) + if (!e->enabled || e->closed_pass == pass) { continue; + } real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * e->weight_scale; bool new_point = false; - if (e->open_pass != pass) { // The point wasn't inside the open list - + if (e->open_pass != pass) { // The point wasn't inside the open list. e->open_pass = pass; open_list.push_back(e); new_point = true; - } else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous - + } else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous. continue; } @@ -316,10 +344,11 @@ bool AStar::_solve(Point *begin_point, Point *end_point) { e->g_score = tentative_g_score; e->f_score = e->g_score + _estimate_cost(e->id, end_point->id); - if (new_point) // The position of the new points is already known + if (new_point) { // The position of the new points is already known. sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptrw()); - else + } else { sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptrw()); + } } } @@ -331,7 +360,15 @@ float AStar::_estimate_cost(int p_from_id, int p_to_id) { if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_estimate_cost)) return get_script_instance()->call(SceneStringNames::get_singleton()->_estimate_cost, p_from_id, p_to_id); - return points[p_from_id]->pos.distance_to(points[p_to_id]->pos); + Point *from_point; + bool from_exists = points.lookup(p_from_id, from_point); + CRASH_COND(!from_exists); + + Point *to_point; + bool to_exists = points.lookup(p_to_id, to_point); + CRASH_COND(!to_exists); + + return from_point->pos.distance_to(to_point->pos); } float AStar::_compute_cost(int p_from_id, int p_to_id) { @@ -339,16 +376,26 @@ float AStar::_compute_cost(int p_from_id, int p_to_id) { if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_compute_cost)) return get_script_instance()->call(SceneStringNames::get_singleton()->_compute_cost, p_from_id, p_to_id); - return points[p_from_id]->pos.distance_to(points[p_to_id]->pos); + Point *from_point; + bool from_exists = points.lookup(p_from_id, from_point); + CRASH_COND(!from_exists); + + Point *to_point; + bool to_exists = points.lookup(p_to_id, to_point); + CRASH_COND(!to_exists); + + return from_point->pos.distance_to(to_point->pos); } PoolVector AStar::get_point_path(int p_from_id, int p_to_id) { - ERR_FAIL_COND_V(!points.has(p_from_id), PoolVector()); - ERR_FAIL_COND_V(!points.has(p_to_id), PoolVector()); + Point *a; + bool from_exists = points.lookup(p_from_id, a); + ERR_FAIL_COND_V(!from_exists, PoolVector()); - Point *a = points[p_from_id]; - Point *b = points[p_to_id]; + Point *b; + bool to_exists = points.lookup(p_to_id, b); + ERR_FAIL_COND_V(!to_exists, PoolVector()); if (a == b) { PoolVector ret; @@ -360,11 +407,8 @@ PoolVector AStar::get_point_path(int p_from_id, int p_to_id) { Point *end_point = b; bool found_route = _solve(begin_point, end_point); + if (!found_route) return PoolVector(); - if (!found_route) - return PoolVector(); - - // Midpoints Point *p = end_point; int pc = 1; // Begin point while (p != begin_point) { @@ -393,11 +437,13 @@ PoolVector AStar::get_point_path(int p_from_id, int p_to_id) { PoolVector AStar::get_id_path(int p_from_id, int p_to_id) { - ERR_FAIL_COND_V(!points.has(p_from_id), PoolVector()); - ERR_FAIL_COND_V(!points.has(p_to_id), PoolVector()); + Point *a; + bool from_exists = points.lookup(p_from_id, a); + ERR_FAIL_COND_V(!from_exists, PoolVector()); - Point *a = points[p_from_id]; - Point *b = points[p_to_id]; + Point *b; + bool to_exists = points.lookup(p_to_id, b); + ERR_FAIL_COND_V(!to_exists, PoolVector()); if (a == b) { PoolVector ret; @@ -409,11 +455,8 @@ PoolVector AStar::get_id_path(int p_from_id, int p_to_id) { Point *end_point = b; bool found_route = _solve(begin_point, end_point); + if (!found_route) return PoolVector(); - if (!found_route) - return PoolVector(); - - // Midpoints Point *p = end_point; int pc = 1; // Begin point while (p != begin_point) { @@ -442,16 +485,20 @@ PoolVector AStar::get_id_path(int p_from_id, int p_to_id) { void AStar::set_point_disabled(int p_id, bool p_disabled) { - ERR_FAIL_COND(!points.has(p_id)); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND(!p_exists); - points[p_id]->enabled = !p_disabled; + p->enabled = !p_disabled; } bool AStar::is_point_disabled(int p_id) const { - ERR_FAIL_COND_V(!points.has(p_id), false); + Point *p; + bool p_exists = points.lookup(p_id, p); + ERR_FAIL_COND_V(!p_exists, false); - return !points[p_id]->enabled; + return !p->enabled; } void AStar::_bind_methods() { @@ -487,13 +534,11 @@ void AStar::_bind_methods() { } AStar::AStar() { - + last_free_id = 0; pass = 1; } AStar::~AStar() { - - pass = 1; clear(); } diff --git a/core/math/a_star.h b/core/math/a_star.h index cbabcce9741..53aaaa1f6c4 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -31,6 +31,7 @@ #ifndef ASTAR_H #define ASTAR_H +#include "core/oa_hash_map.h" #include "core/reference.h" /** @@ -43,8 +44,6 @@ class AStar : public Reference { GDCLASS(AStar, Reference); - uint64_t pass; - struct Point { int id; @@ -52,10 +51,10 @@ class AStar : public Reference { real_t weight_scale; bool enabled; - Set neighbours; - Set unlinked_neighbours; + OAHashMap neighbours; + OAHashMap unlinked_neighbours; - // Used for pathfinding + // Used for pathfinding. Point *prev_point; real_t g_score; real_t f_score; @@ -63,16 +62,15 @@ class AStar : public Reference { uint64_t closed_pass; }; - Map points; - struct SortPoints { - _FORCE_INLINE_ bool operator()(const Point *A, const Point *B) const { // Returns true when the Point A is worse than Point B - if (A->f_score > B->f_score) + _FORCE_INLINE_ bool operator()(const Point *A, const Point *B) const { // Returns true when the Point A is worse than Point B. + if (A->f_score > B->f_score) { return true; - else if (A->f_score < B->f_score) + } else if (A->f_score < B->f_score) { return false; - else - return A->g_score < B->g_score; // If the f_costs are the same then prioritize the points that are further away from the start + } else { + return A->g_score < B->g_score; // If the f_costs are the same then prioritize the points that are further away from the start. + } } }; @@ -100,6 +98,10 @@ class AStar : public Reference { } }; + int last_free_id; + uint64_t pass; + + OAHashMap points; Set segments; bool _solve(Point *begin_point, Point *end_point); diff --git a/core/oa_hash_map.h b/core/oa_hash_map.h index e52d36a8592..83621bec141 100644 --- a/core/oa_hash_map.h +++ b/core/oa_hash_map.h @@ -62,7 +62,7 @@ private: static const uint32_t EMPTY_HASH = 0; static const uint32_t DELETED_HASH_BIT = 1 << 31; - _FORCE_INLINE_ uint32_t _hash(const TKey &p_key) { + _FORCE_INLINE_ uint32_t _hash(const TKey &p_key) const { uint32_t hash = Hasher::hash(p_key); if (hash == EMPTY_HASH) { @@ -74,7 +74,7 @@ private: return hash; } - _FORCE_INLINE_ uint32_t _get_probe_length(uint32_t p_pos, uint32_t p_hash) { + _FORCE_INLINE_ uint32_t _get_probe_length(uint32_t p_pos, uint32_t p_hash) const { p_hash = p_hash & ~DELETED_HASH_BIT; // we don't care if it was deleted or not uint32_t original_pos = p_hash % capacity; @@ -90,7 +90,7 @@ private: num_elements++; } - bool _lookup_pos(const TKey &p_key, uint32_t &r_pos) { + bool _lookup_pos(const TKey &p_key, uint32_t &r_pos) const { uint32_t hash = _hash(p_key); uint32_t pos = hash % capacity; uint32_t distance = 0; @@ -151,6 +151,7 @@ private: distance++; } } + void _resize_and_rehash() { TKey *old_keys = keys; @@ -190,6 +191,26 @@ public: _FORCE_INLINE_ uint32_t get_capacity() const { return capacity; } _FORCE_INLINE_ uint32_t get_num_elements() const { return num_elements; } + bool empty() const { + return num_elements == 0; + } + + void clear() { + + for (uint32_t i = 0; i < capacity; i++) { + + if (hashes[i] & DELETED_HASH_BIT) { + continue; + } + + hashes[i] |= DELETED_HASH_BIT; + values[i].~TValue(); + keys[i].~TKey(); + } + + num_elements = 0; + } + void insert(const TKey &p_key, const TValue &p_value) { if ((float)num_elements / (float)capacity > 0.9) { @@ -219,7 +240,7 @@ public: * if r_data is not NULL then the value will be written to the object * it points to. */ - bool lookup(const TKey &p_key, TValue &r_data) { + bool lookup(const TKey &p_key, TValue &r_data) const { uint32_t pos = 0; bool exists = _lookup_pos(p_key, pos); @@ -232,7 +253,7 @@ public: return false; } - _FORCE_INLINE_ bool has(const TKey &p_key) { + _FORCE_INLINE_ bool has(const TKey &p_key) const { uint32_t _pos = 0; return _lookup_pos(p_key, _pos); } @@ -302,6 +323,9 @@ public: return it; } + OAHashMap(const OAHashMap &) = delete; // Delete the copy constructor so we don't get unexpected copies and dangling pointers. + OAHashMap &operator=(const OAHashMap &) = delete; // Same for assignment operator. + OAHashMap(uint32_t p_initial_capacity = 64) { capacity = p_initial_capacity;