summaryrefslogtreecommitdiffstats
path: root/core/math/a_star.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'core/math/a_star.cpp')
-rw-r--r--core/math/a_star.cpp121
1 files changed, 81 insertions, 40 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index f0f160940d..4497604947 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -29,6 +29,7 @@
/**************************************************************************/
#include "a_star.h"
+#include "a_star.compat.inc"
#include "core/math/geometry_3d.h"
#include "core/object/script_language.h"
@@ -69,7 +70,7 @@ void AStar3D::add_point(int64_t p_id, const Vector3 &p_pos, real_t p_weight_scal
}
Vector3 AStar3D::get_point_position(int64_t p_id) const {
- Point *p;
+ Point *p = nullptr;
bool p_exists = points.lookup(p_id, p);
ERR_FAIL_COND_V_MSG(!p_exists, Vector3(), vformat("Can't get point's position. Point with id: %d doesn't exist.", p_id));
@@ -77,7 +78,7 @@ Vector3 AStar3D::get_point_position(int64_t p_id) const {
}
void AStar3D::set_point_position(int64_t p_id, const Vector3 &p_pos) {
- Point *p;
+ Point *p = nullptr;
bool p_exists = points.lookup(p_id, p);
ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set point's position. Point with id: %d doesn't exist.", p_id));
@@ -85,7 +86,7 @@ void AStar3D::set_point_position(int64_t p_id, const Vector3 &p_pos) {
}
real_t AStar3D::get_point_weight_scale(int64_t p_id) const {
- Point *p;
+ Point *p = nullptr;
bool p_exists = points.lookup(p_id, p);
ERR_FAIL_COND_V_MSG(!p_exists, 0, vformat("Can't get point's weight scale. Point with id: %d doesn't exist.", p_id));
@@ -93,7 +94,7 @@ real_t AStar3D::get_point_weight_scale(int64_t p_id) const {
}
void AStar3D::set_point_weight_scale(int64_t p_id, real_t p_weight_scale) {
- Point *p;
+ Point *p = nullptr;
bool p_exists = points.lookup(p_id, p);
ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set point's weight scale. Point with id: %d doesn't exist.", p_id));
ERR_FAIL_COND_MSG(p_weight_scale < 0.0, vformat("Can't set point's weight scale less than 0.0: %f.", p_weight_scale));
@@ -102,7 +103,7 @@ void AStar3D::set_point_weight_scale(int64_t p_id, real_t p_weight_scale) {
}
void AStar3D::remove_point(int64_t p_id) {
- Point *p;
+ Point *p = nullptr;
bool p_exists = points.lookup(p_id, p);
ERR_FAIL_COND_MSG(!p_exists, vformat("Can't remove point. Point with id: %d doesn't exist.", p_id));
@@ -130,11 +131,11 @@ void AStar3D::remove_point(int64_t p_id) {
void AStar3D::connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) {
ERR_FAIL_COND_MSG(p_id == p_with_id, vformat("Can't connect point with id: %d to itself.", p_id));
- Point *a;
+ Point *a = nullptr;
bool from_exists = points.lookup(p_id, a);
ERR_FAIL_COND_MSG(!from_exists, vformat("Can't connect points. Point with id: %d doesn't exist.", p_id));
- Point *b;
+ Point *b = nullptr;
bool to_exists = points.lookup(p_with_id, b);
ERR_FAIL_COND_MSG(!to_exists, vformat("Can't connect points. Point with id: %d doesn't exist.", p_with_id));
@@ -166,11 +167,11 @@ void AStar3D::connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional
}
void AStar3D::disconnect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) {
- Point *a;
+ Point *a = nullptr;
bool a_exists = points.lookup(p_id, a);
ERR_FAIL_COND_MSG(!a_exists, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_id));
- Point *b;
+ Point *b = nullptr;
bool b_exists = points.lookup(p_with_id, b);
ERR_FAIL_COND_MSG(!b_exists, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_with_id));
@@ -220,7 +221,7 @@ PackedInt64Array AStar3D::get_point_ids() {
}
Vector<int64_t> AStar3D::get_point_connections(int64_t p_id) {
- Point *p;
+ Point *p = nullptr;
bool p_exists = points.lookup(p_id, p);
ERR_FAIL_COND_V_MSG(!p_exists, Vector<int64_t>(), vformat("Can't get point's connections. Point with id: %d doesn't exist.", p_id));
@@ -319,6 +320,7 @@ Vector3 AStar3D::get_closest_position_in_segment(const Vector3 &p_point) const {
}
bool AStar3D::_solve(Point *begin_point, Point *end_point) {
+ last_closest_point = nullptr;
pass++;
if (!end_point->enabled) {
@@ -332,11 +334,18 @@ bool AStar3D::_solve(Point *begin_point, Point *end_point) {
begin_point->g_score = 0;
begin_point->f_score = _estimate_cost(begin_point->id, end_point->id);
+ begin_point->abs_g_score = 0;
+ begin_point->abs_f_score = _estimate_cost(begin_point->id, end_point->id);
open_list.push_back(begin_point);
while (!open_list.is_empty()) {
Point *p = open_list[0]; // The currently processed point.
+ // Find point closer to end_point, or same distance to end_point but closer to begin_point.
+ if (last_closest_point == nullptr || last_closest_point->abs_f_score > p->abs_f_score || (last_closest_point->abs_f_score >= p->abs_f_score && last_closest_point->abs_g_score > p->abs_g_score)) {
+ last_closest_point = p;
+ }
+
if (p == end_point) {
found_route = true;
break;
@@ -368,6 +377,8 @@ bool AStar3D::_solve(Point *begin_point, Point *end_point) {
e->prev_point = p;
e->g_score = tentative_g_score;
e->f_score = e->g_score + _estimate_cost(e->id, end_point->id);
+ e->abs_g_score = tentative_g_score;
+ e->abs_f_score = e->f_score - e->g_score;
if (new_point) { // The position of the new points is already known.
sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptr());
@@ -386,11 +397,11 @@ real_t AStar3D::_estimate_cost(int64_t p_from_id, int64_t p_to_id) {
return scost;
}
- Point *from_point;
+ Point *from_point = nullptr;
bool from_exists = points.lookup(p_from_id, from_point);
ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
- Point *to_point;
+ Point *to_point = nullptr;
bool to_exists = points.lookup(p_to_id, to_point);
ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_to_id));
@@ -403,23 +414,23 @@ real_t AStar3D::_compute_cost(int64_t p_from_id, int64_t p_to_id) {
return scost;
}
- Point *from_point;
+ Point *from_point = nullptr;
bool from_exists = points.lookup(p_from_id, from_point);
ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id));
- Point *to_point;
+ Point *to_point = nullptr;
bool to_exists = points.lookup(p_to_id, to_point);
ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id));
return from_point->pos.distance_to(to_point->pos);
}
-Vector<Vector3> AStar3D::get_point_path(int64_t p_from_id, int64_t p_to_id) {
- Point *a;
+Vector<Vector3> AStar3D::get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
+ Point *a = nullptr;
bool from_exists = points.lookup(p_from_id, a);
ERR_FAIL_COND_V_MSG(!from_exists, Vector<Vector3>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_from_id));
- Point *b;
+ Point *b = nullptr;
bool to_exists = points.lookup(p_to_id, b);
ERR_FAIL_COND_V_MSG(!to_exists, Vector<Vector3>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_to_id));
@@ -434,7 +445,12 @@ Vector<Vector3> AStar3D::get_point_path(int64_t p_from_id, int64_t p_to_id) {
bool found_route = _solve(begin_point, end_point);
if (!found_route) {
- return Vector<Vector3>();
+ if (!p_allow_partial_path || last_closest_point == nullptr) {
+ return Vector<Vector3>();
+ }
+
+ // Use closest point instead.
+ end_point = last_closest_point;
}
Point *p = end_point;
@@ -463,12 +479,12 @@ Vector<Vector3> AStar3D::get_point_path(int64_t p_from_id, int64_t p_to_id) {
return path;
}
-Vector<int64_t> AStar3D::get_id_path(int64_t p_from_id, int64_t p_to_id) {
- Point *a;
+Vector<int64_t> AStar3D::get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
+ Point *a = nullptr;
bool from_exists = points.lookup(p_from_id, a);
ERR_FAIL_COND_V_MSG(!from_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id));
- Point *b;
+ Point *b = nullptr;
bool to_exists = points.lookup(p_to_id, b);
ERR_FAIL_COND_V_MSG(!to_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id));
@@ -483,7 +499,12 @@ Vector<int64_t> AStar3D::get_id_path(int64_t p_from_id, int64_t p_to_id) {
bool found_route = _solve(begin_point, end_point);
if (!found_route) {
- return Vector<int64_t>();
+ if (!p_allow_partial_path || last_closest_point == nullptr) {
+ return Vector<int64_t>();
+ }
+
+ // Use closest point instead.
+ end_point = last_closest_point;
}
Point *p = end_point;
@@ -513,7 +534,7 @@ Vector<int64_t> AStar3D::get_id_path(int64_t p_from_id, int64_t p_to_id) {
}
void AStar3D::set_point_disabled(int64_t p_id, bool p_disabled) {
- Point *p;
+ Point *p = nullptr;
bool p_exists = points.lookup(p_id, p);
ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set if point is disabled. Point with id: %d doesn't exist.", p_id));
@@ -521,7 +542,7 @@ void AStar3D::set_point_disabled(int64_t p_id, bool p_disabled) {
}
bool AStar3D::is_point_disabled(int64_t p_id) const {
- Point *p;
+ Point *p = nullptr;
bool p_exists = points.lookup(p_id, p);
ERR_FAIL_COND_V_MSG(!p_exists, false, vformat("Can't get if point is disabled. Point with id: %d doesn't exist.", p_id));
@@ -555,8 +576,8 @@ void AStar3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar3D::get_closest_point, DEFVAL(false));
ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar3D::get_closest_position_in_segment);
- ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar3D::get_point_path);
- ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar3D::get_id_path);
+ ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id", "allow_partial_path"), &AStar3D::get_point_path, DEFVAL(false));
+ ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id", "allow_partial_path"), &AStar3D::get_id_path, DEFVAL(false));
GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id")
GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
@@ -660,11 +681,11 @@ real_t AStar2D::_estimate_cost(int64_t p_from_id, int64_t p_to_id) {
return scost;
}
- AStar3D::Point *from_point;
+ AStar3D::Point *from_point = nullptr;
bool from_exists = astar.points.lookup(p_from_id, from_point);
ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
- AStar3D::Point *to_point;
+ AStar3D::Point *to_point = nullptr;
bool to_exists = astar.points.lookup(p_to_id, to_point);
ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_to_id));
@@ -677,23 +698,23 @@ real_t AStar2D::_compute_cost(int64_t p_from_id, int64_t p_to_id) {
return scost;
}
- AStar3D::Point *from_point;
+ AStar3D::Point *from_point = nullptr;
bool from_exists = astar.points.lookup(p_from_id, from_point);
ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id));
- AStar3D::Point *to_point;
+ AStar3D::Point *to_point = nullptr;
bool to_exists = astar.points.lookup(p_to_id, to_point);
ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id));
return from_point->pos.distance_to(to_point->pos);
}
-Vector<Vector2> AStar2D::get_point_path(int64_t p_from_id, int64_t p_to_id) {
- AStar3D::Point *a;
+Vector<Vector2> AStar2D::get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
+ AStar3D::Point *a = nullptr;
bool from_exists = astar.points.lookup(p_from_id, a);
ERR_FAIL_COND_V_MSG(!from_exists, Vector<Vector2>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_from_id));
- AStar3D::Point *b;
+ AStar3D::Point *b = nullptr;
bool to_exists = astar.points.lookup(p_to_id, b);
ERR_FAIL_COND_V_MSG(!to_exists, Vector<Vector2>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_to_id));
@@ -707,7 +728,12 @@ Vector<Vector2> AStar2D::get_point_path(int64_t p_from_id, int64_t p_to_id) {
bool found_route = _solve(begin_point, end_point);
if (!found_route) {
- return Vector<Vector2>();
+ if (!p_allow_partial_path || astar.last_closest_point == nullptr) {
+ return Vector<Vector2>();
+ }
+
+ // Use closest point instead.
+ end_point = astar.last_closest_point;
}
AStar3D::Point *p = end_point;
@@ -736,12 +762,12 @@ Vector<Vector2> AStar2D::get_point_path(int64_t p_from_id, int64_t p_to_id) {
return path;
}
-Vector<int64_t> AStar2D::get_id_path(int64_t p_from_id, int64_t p_to_id) {
- AStar3D::Point *a;
+Vector<int64_t> AStar2D::get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
+ AStar3D::Point *a = nullptr;
bool from_exists = astar.points.lookup(p_from_id, a);
ERR_FAIL_COND_V_MSG(!from_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id));
- AStar3D::Point *b;
+ AStar3D::Point *b = nullptr;
bool to_exists = astar.points.lookup(p_to_id, b);
ERR_FAIL_COND_V_MSG(!to_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id));
@@ -756,7 +782,12 @@ Vector<int64_t> AStar2D::get_id_path(int64_t p_from_id, int64_t p_to_id) {
bool found_route = _solve(begin_point, end_point);
if (!found_route) {
- return Vector<int64_t>();
+ if (!p_allow_partial_path || astar.last_closest_point == nullptr) {
+ return Vector<int64_t>();
+ }
+
+ // Use closest point instead.
+ end_point = astar.last_closest_point;
}
AStar3D::Point *p = end_point;
@@ -786,6 +817,7 @@ Vector<int64_t> AStar2D::get_id_path(int64_t p_from_id, int64_t p_to_id) {
}
bool AStar2D::_solve(AStar3D::Point *begin_point, AStar3D::Point *end_point) {
+ astar.last_closest_point = nullptr;
astar.pass++;
if (!end_point->enabled) {
@@ -799,11 +831,18 @@ bool AStar2D::_solve(AStar3D::Point *begin_point, AStar3D::Point *end_point) {
begin_point->g_score = 0;
begin_point->f_score = _estimate_cost(begin_point->id, end_point->id);
+ begin_point->abs_g_score = 0;
+ begin_point->abs_f_score = _estimate_cost(begin_point->id, end_point->id);
open_list.push_back(begin_point);
while (!open_list.is_empty()) {
AStar3D::Point *p = open_list[0]; // The currently processed point.
+ // Find point closer to end_point, or same distance to end_point but closer to begin_point.
+ if (astar.last_closest_point == nullptr || astar.last_closest_point->abs_f_score > p->abs_f_score || (astar.last_closest_point->abs_f_score >= p->abs_f_score && astar.last_closest_point->abs_g_score > p->abs_g_score)) {
+ astar.last_closest_point = p;
+ }
+
if (p == end_point) {
found_route = true;
break;
@@ -835,6 +874,8 @@ bool AStar2D::_solve(AStar3D::Point *begin_point, AStar3D::Point *end_point) {
e->prev_point = p;
e->g_score = tentative_g_score;
e->f_score = e->g_score + _estimate_cost(e->id, end_point->id);
+ e->abs_g_score = tentative_g_score;
+ e->abs_f_score = e->f_score - e->g_score;
if (new_point) { // The position of the new points is already known.
sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptr());
@@ -874,8 +915,8 @@ void AStar2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar2D::get_closest_point, DEFVAL(false));
ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar2D::get_closest_position_in_segment);
- ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar2D::get_point_path);
- ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar2D::get_id_path);
+ ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id", "allow_partial_path"), &AStar2D::get_point_path, DEFVAL(false));
+ ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id", "allow_partial_path"), &AStar2D::get_id_path, DEFVAL(false));
GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id")
GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")