Merge pull request #31402 from profan/perf/astar-improvements

A* performance improvements, use OAHashMap.

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<int &>(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;
+		found_pt->pos = p_pos;
-		points[p_id]->weight_scale = p_weight_scale;
+		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<int, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) {
 
-	for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) {
+		Segment s(p_id, (*it.key));
-
-		Segment s(p_id, E->get()->id);
 		segments.erase(s);
 
-		E->get()->neighbours.erase(p);
+		(*it.value)->neighbours.remove(p->id);
-		E->get()->unlinked_neighbours.erase(p);
+		(*it.value)->unlinked_neighbours.remove(p->id);
 	}
 
-	for (Set<Point *>::Element *E = p->unlinked_neighbours.front(); E; E = E->next()) {
+	for (OAHashMap<int, Point *>::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);
+		(*it.value)->neighbours.remove(p->id);
-		E->get()->unlinked_neighbours.erase(p);
+		(*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 *a;
-	Point *b = points[p_with_id];
+	bool from_exists = points.lookup(p_id, a);
-	a->neighbours.insert(b);
+	ERR_FAIL_COND(!from_exists);
 
-	if (bidirectional)
+	Point *b;
-		b->neighbours.insert(a);
+	bool to_exists = points.lookup(p_with_id, b);
-	else
+	ERR_FAIL_COND(!to_exists);
-		b->unlinked_neighbours.insert(a);
+
+	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 *a;
-	Point *b = points[p_with_id];
+	bool a_exists = points.lookup(p_id, a);
-	a->neighbours.erase(b);
+	CRASH_COND(!a_exists);
-	a->unlinked_neighbours.erase(b);
+
-	b->neighbours.erase(a);
+	Point *b;
-	b->unlinked_neighbours.erase(a);
+	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<int, Point *>::Element *E = points.front(); E; E = E->next()) {
+	for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) {
-		point_list.push_back(E->key());
+		point_list.push_back(*(it.key));
 	}
 
 	return point_list;
@@ -180,14 +214,14 @@ Array AStar::get_points() {
 
 PoolVector<int> AStar::get_point_connections(int p_id) {
 
-	ERR_FAIL_COND_V(!points.has(p_id), PoolVector<int>());
+	Point *p;
+	bool p_exists = points.lookup(p_id, p);
+	ERR_FAIL_COND_V(!p_exists, PoolVector<int>());
 
 	PoolVector<int> point_list;
 
-	Point *p = points[p_id];
+	for (OAHashMap<int, Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) {
-
+		point_list.push_back((*it.key));
-	for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) {
-		point_list.push_back(E->get()->id);
 	}
 
 	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<int, Point *>::Element *E = points.front(); E; E = E->next()) {
+	last_free_id = 0;
-
+	for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) {
-		memdelete(E->get());
+		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<int, Point *>::Element *E = points.front(); E; E = E->next()) {
+	for (OAHashMap<int, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) {
 
-		if (!E->get()->enabled)
+		if (!(*it.value)->enabled) continue; // Disabled points should not be considered.
-			continue; //Disabled points should not be considered
+
-		real_t d = p_point.distance_squared_to(E->get()->pos);
+		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<Segment>::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)
+	if (!end_point->enabled) return false;
-		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) {
+	while (!open_list.empty()) {
-
-		if (open_list.size() == 0) // No path found
-			break;
 
 		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<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) {
+		for (OAHashMap<int, Point *>::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,12 +344,13 @@ 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());
 			}
 		}
+	}
 
 	return found_route;
 }
@@ -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<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) {
 
-	ERR_FAIL_COND_V(!points.has(p_from_id), PoolVector<Vector3>());
+	Point *a;
-	ERR_FAIL_COND_V(!points.has(p_to_id), PoolVector<Vector3>());
+	bool from_exists = points.lookup(p_from_id, a);
+	ERR_FAIL_COND_V(!from_exists, PoolVector<Vector3>());
 
-	Point *a = points[p_from_id];
+	Point *b;
-	Point *b = points[p_to_id];
+	bool to_exists = points.lookup(p_to_id, b);
+	ERR_FAIL_COND_V(!to_exists, PoolVector<Vector3>());
 
 	if (a == b) {
 		PoolVector<Vector3> ret;
@@ -360,11 +407,8 @@ PoolVector<Vector3> 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<Vector3>();
 
-	if (!found_route)
-		return PoolVector<Vector3>();
-
-	// Midpoints
 	Point *p = end_point;
 	int pc = 1; // Begin point
 	while (p != begin_point) {
@@ -393,11 +437,13 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) {
 
 PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
 
-	ERR_FAIL_COND_V(!points.has(p_from_id), PoolVector<int>());
+	Point *a;
-	ERR_FAIL_COND_V(!points.has(p_to_id), PoolVector<int>());
+	bool from_exists = points.lookup(p_from_id, a);
+	ERR_FAIL_COND_V(!from_exists, PoolVector<int>());
 
-	Point *a = points[p_from_id];
+	Point *b;
-	Point *b = points[p_to_id];
+	bool to_exists = points.lookup(p_to_id, b);
+	ERR_FAIL_COND_V(!to_exists, PoolVector<int>());
 
 	if (a == b) {
 		PoolVector<int> ret;
@@ -409,11 +455,8 @@ PoolVector<int> 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<int>();
 
-	if (!found_route)
-		return PoolVector<int>();
-
-	// Midpoints
 	Point *p = end_point;
 	int pc = 1; // Begin point
 	while (p != begin_point) {
@@ -442,16 +485,20 @@ PoolVector<int> 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();
 }
 

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<Point *> neighbours;
+		OAHashMap<int, Point *> neighbours;
-		Set<Point *> unlinked_neighbours;
+		OAHashMap<int, Point *> 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<int, Point *> 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
+		_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)
+			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
+			} 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
+				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<int, Point *> points;
 	Set<Segment> segments;
 
 	bool _solve(Point *begin_point, Point *end_point);

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;