maxmustermann/godot
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Use a binary heap for the open list of Astar

Browse source
This commit is contained in:
Daw11 2019-05-16 20:09:47 +02:00
parent e20fb10d35
commit cc7be6c643
2 changed files with 48 additions and 67 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

93
core/math/a_star.cpp
View file

@ -54,7 +54,8 @@ void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) {
pt->pos = p_pos;
pt->weight_scale = p_weight_scale;
pt->prev_point = NULL;
pt->last_pass = 0;
pt->open_pass = 0;
pt->closed_pass = 0;
pt->enabled = true;
points[p_id] = pt;
} else {
@ -246,86 +247,62 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
if (!end_point->enabled)
return false;
SelfList<Point>::List open_list;
bool found_route = false;
for (Set<Point *>::Element *E = begin_point->neighbours.front(); E; E = E->next()) {
Vector<Point *> open_list;
SortArray<Point *, SortPoints> sorter;
Point *n = E->get();
begin_point->g_score = 0;
begin_point->f_score = _estimate_cost(begin_point->id, end_point->id);
if (!n->enabled)
continue;
n->prev_point = begin_point;
n->distance = _compute_cost(begin_point->id, n->id) * n->weight_scale;
n->last_pass = pass;
open_list.add(&n->list);
}
open_list.push_back(begin_point);
while (true) {
if (open_list.first() == NULL) {
// No path found
if (open_list.size() == 0) // No path found
break;
}
// Check open list
SelfList<Point> *least_cost_point = open_list.first();
real_t least_cost = Math_INF;
Point *p = open_list[0]; // The currently processed point
// TODO: Cache previous results
for (SelfList<Point> *E = open_list.first(); E; E = E->next()) {
Point *p = E->self();
real_t cost = p->distance;
cost += _estimate_cost(p->id, end_point->id);
if (cost < least_cost) {
least_cost_point = E;
least_cost = cost;
}
}
Point *p = least_cost_point->self();
if (p == end_point) {
found_route = true;
break;
}
sorter.pop_heap(0, open_list.size(), open_list.ptrw()); // Remove the current point from the open list
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()) {
Point *e = E->get();
Point *e = E->get(); // The neighbour point
if (!e->enabled)
if (!e->enabled || e->closed_pass == pass)
continue;
real_t distance = _compute_cost(p->id, e->id) * e->weight_scale + p->distance;
real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * e->weight_scale;
if (e->last_pass == pass) {
// Already visited, is this cheaper?
bool new_point = false;
if (e->distance > distance) {
e->prev_point = p;
e->distance = distance;
}
} else {
// Add to open neighbours
if (e->open_pass != pass) { // The point wasn't inside the open list
e->prev_point = p;
e->distance = distance;
e->last_pass = pass; // Mark as used
open_list.add(&e->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
continue;
}
e->prev_point = p;
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
sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptrw());
else
sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptrw());
}
open_list.remove(least_cost_point);
}
// Clear the openf list
while (open_list.first()) {
open_list.remove(open_list.first());
}
return found_route;
@ -352,8 +329,6 @@ 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>());
pass++;
Point *a = points[p_from_id];
Point *b = points[p_to_id];
@ -403,8 +378,6 @@ 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>());
pass++;
Point *a = points[p_from_id];
Point *b = points[p_to_id];

22
core/math/a_star.h
View file

@ -48,26 +48,34 @@ class AStar : public Reference {
struct Point {
SelfList<Point> list;
int id;
Vector3 pos;
real_t weight_scale;
uint64_t last_pass;
bool enabled;
Set<Point *> neighbours;
// Used for pathfinding
Point *prev_point;
real_t distance;
Point() :
list(this) {}
real_t g_score;
real_t f_score;
uint64_t open_pass;
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)
return true;
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
}
};
struct Segment {
union {
struct {