asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

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

Browse source 
A* performance improvements, use OAHashMap.
This commit is contained in:
Rémi Verschelde 2019-08-21 11:27:12 +02:00 committed by GitHub
commit 05a4310899
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 185 additions and 114 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

239
core/math/a_star.cpp
View file

@ -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;
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<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, 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<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);
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<int, Point *>::Element *E = points.front(); E; E = E->next()) {
point_list.push_back(E->key());
for (OAHashMap<int, Point *>::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<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 (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) {
point_list.push_back(E->get()->id);
for (OAHashMap<int, Point *>::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<int, Point *>::Element *E = points.front(); E; E = E->next()) {
memdelete(E->get());
last_free_id = 0;
for (OAHashMap<int, Point *>::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<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)
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<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)
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<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>());
ERR_FAIL_COND_V(!points.has(p_to_id), PoolVector<Vector3>());
Point *a;
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 = points[p_to_id];
Point *b;
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>());
ERR_FAIL_COND_V(!points.has(p_to_id), PoolVector<int>());
Point *a;
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 = points[p_to_id];
Point *b;
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();
}

26
core/math/a_star.h
View file

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

34
core/oa_hash_map.h
View file

@ -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;