From cc7be6c64316e72bfdbb523f52b479394b3bff76 Mon Sep 17 00:00:00 2001 From: Daw11 Date: Thu, 16 May 2019 20:09:47 +0200 Subject: [PATCH] Use a binary heap for the open list of Astar --- core/math/a_star.cpp | 91 ++++++++++++++++---------------------------- core/math/a_star.h | 22 +++++++---- 2 files changed, 47 insertions(+), 66 deletions(-) diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index e1388ad2ac2a..3d71e66f8060 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -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::List open_list; - bool found_route = false; - for (Set::Element *E = begin_point->neighbours.front(); E; E = E->next()) { - - Point *n = E->get(); + Vector open_list; + SortArray sorter; - if (!n->enabled) - continue; + begin_point->g_score = 0; + begin_point->f_score = _estimate_cost(begin_point->id, end_point->id); - 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 *least_cost_point = open_list.first(); - real_t least_cost = Math_INF; - // TODO: Cache previous results - for (SelfList *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 = open_list[0]; // The currently processed point - 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::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; + + bool new_point = false; - if (e->last_pass == pass) { - // Already visited, is this cheaper? + if (e->open_pass != pass) { // The point wasn't inside the open list - if (e->distance > distance) { - e->prev_point = p; - e->distance = distance; - } - } else { - // Add to open neighbours + 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 - e->prev_point = p; - e->distance = distance; - e->last_pass = pass; // Mark as used - open_list.add(&e->list); + continue; } - } - open_list.remove(least_cost_point); - } + e->prev_point = p; + e->g_score = tentative_g_score; + e->f_score = e->g_score + _estimate_cost(e->id, end_point->id); - // Clear the openf list - while (open_list.first()) { - open_list.remove(open_list.first()); + 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()); + } } return found_route; @@ -352,8 +329,6 @@ 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()); - pass++; - Point *a = points[p_from_id]; Point *b = points[p_to_id]; @@ -403,8 +378,6 @@ 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()); - pass++; - Point *a = points[p_from_id]; Point *b = points[p_to_id]; diff --git a/core/math/a_star.h b/core/math/a_star.h index c63e1aa4dc69..fac8a9d312ad 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -48,26 +48,34 @@ class AStar : public Reference { struct Point { - SelfList list; - int id; Vector3 pos; real_t weight_scale; - uint64_t last_pass; bool enabled; Set 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 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 {