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.cpp65
1 files changed, 53 insertions, 12 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index fb54058bd9..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"
@@ -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());
@@ -414,7 +425,7 @@ real_t AStar3D::_compute_cost(int64_t p_from_id, int64_t 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) {
+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));
@@ -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,7 +479,7 @@ 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) {
+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));
@@ -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;
@@ -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")
@@ -688,7 +709,7 @@ real_t AStar2D::_compute_cost(int64_t p_from_id, int64_t 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) {
+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));
@@ -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,7 +762,7 @@ 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) {
+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));
@@ -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")