add partial path return option for astar

* AStar2D, AStar3D and AStarGrid2D now can return a partial path if the destination point isn't reachable but still in the map. This option is available for both get_point_path and get_id_path

6 files changed, 222 insertions, 24 deletions

diff --git a/core/math/a_star.compat.inc b/core/math/a_star.compat.inc
new file mode 100644
index 0000000000..664d7ffd5e
--- /dev/null
+++ b/core/math/a_star.compat.inc
@@ -0,0 +1,59 @@
+/**************************************************************************/
+/*  a_star.compat.inc                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef DISABLE_DEPRECATED
+
+Vector<int64_t> AStar3D::_get_id_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id) {
+	return get_id_path(p_from_id, p_to_id, false);
+}
+
+Vector<Vector3> AStar3D::_get_point_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id) {
+	return get_point_path(p_from_id, p_to_id, false);
+}
+
+void AStar3D::_bind_compatibility_methods() {
+	ClassDB::bind_compatibility_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar3D::_get_id_path_bind_compat_88047);
+	ClassDB::bind_compatibility_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar3D::_get_point_path_bind_compat_88047);
+}
+
+Vector<int64_t> AStar2D::_get_id_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id) {
+	return get_id_path(p_from_id, p_to_id, false);
+}
+
+Vector<Vector2> AStar2D::_get_point_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id) {
+	return get_point_path(p_from_id, p_to_id, false);
+}
+
+void AStar2D::_bind_compatibility_methods() {
+	ClassDB::bind_compatibility_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar2D::_get_id_path_bind_compat_88047);
+	ClassDB::bind_compatibility_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar2D::_get_point_path_bind_compat_88047);
+}
+
+#endif // DISABLE_DEPRECATED
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")
diff --git a/core/math/a_star.h b/core/math/a_star.h
index 0758500c8a..8e054c4789 100644
--- a/core/math/a_star.h
+++ b/core/math/a_star.h
@@ -60,6 +60,10 @@ class AStar3D : public RefCounted {
 		real_t f_score = 0;
 		uint64_t open_pass = 0;
 		uint64_t closed_pass = 0;
+
+		// Used for getting closest_point_of_last_pathing_call.
+		real_t abs_g_score = 0;
+		real_t abs_f_score = 0;
 	};
 
 	struct SortPoints {
@@ -109,6 +113,7 @@ class AStar3D : public RefCounted {
 
 	OAHashMap<int64_t, Point *> points;
 	HashSet<Segment, Segment> segments;
+	Point *last_closest_point = nullptr;
 
 	bool _solve(Point *begin_point, Point *end_point);
 
@@ -121,6 +126,12 @@ protected:
 	GDVIRTUAL2RC(real_t, _estimate_cost, int64_t, int64_t)
 	GDVIRTUAL2RC(real_t, _compute_cost, int64_t, int64_t)
 
+#ifndef DISABLE_DEPRECATED
+	Vector<int64_t> _get_id_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id);
+	Vector<Vector3> _get_point_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id);
+	static void _bind_compatibility_methods();
+#endif
+
 public:
 	int64_t get_available_point_id() const;
 
@@ -149,8 +160,8 @@ public:
 	int64_t get_closest_point(const Vector3 &p_point, bool p_include_disabled = false) const;
 	Vector3 get_closest_position_in_segment(const Vector3 &p_point) const;
 
-	Vector<Vector3> get_point_path(int64_t p_from_id, int64_t p_to_id);
-	Vector<int64_t> get_id_path(int64_t p_from_id, int64_t p_to_id);
+	Vector<Vector3> get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false);
+	Vector<int64_t> get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false);
 
 	AStar3D() {}
 	~AStar3D();
@@ -171,6 +182,12 @@ protected:
 	GDVIRTUAL2RC(real_t, _estimate_cost, int64_t, int64_t)
 	GDVIRTUAL2RC(real_t, _compute_cost, int64_t, int64_t)
 
+#ifndef DISABLE_DEPRECATED
+	Vector<int64_t> _get_id_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id);
+	Vector<Vector2> _get_point_path_bind_compat_88047(int64_t p_from_id, int64_t p_to_id);
+	static void _bind_compatibility_methods();
+#endif
+
 public:
 	int64_t get_available_point_id() const;
 
@@ -199,8 +216,8 @@ public:
 	int64_t get_closest_point(const Vector2 &p_point, bool p_include_disabled = false) const;
 	Vector2 get_closest_position_in_segment(const Vector2 &p_point) const;
 
-	Vector<Vector2> get_point_path(int64_t p_from_id, int64_t p_to_id);
-	Vector<int64_t> get_id_path(int64_t p_from_id, int64_t p_to_id);
+	Vector<Vector2> get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false);
+	Vector<int64_t> get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false);
 
 	AStar2D() {}
 	~AStar2D() {}
diff --git a/core/math/a_star_grid_2d.compat.inc b/core/math/a_star_grid_2d.compat.inc
new file mode 100644
index 0000000000..e7124c2477
--- /dev/null
+++ b/core/math/a_star_grid_2d.compat.inc
@@ -0,0 +1,48 @@
+/**************************************************************************/
+/*  a_star_grid_2d.compat.inc                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef DISABLE_DEPRECATED
+
+#include "core/variant/typed_array.h"
+
+TypedArray<Vector2i> AStarGrid2D::_get_id_path_bind_compat_88047(const Vector2i &p_from_id, const Vector2i &p_to_id) {
+	return get_id_path(p_from_id, p_to_id, false);
+}
+
+Vector<Vector2> AStarGrid2D::_get_point_path_bind_compat_88047(const Vector2i &p_from_id, const Vector2i &p_to_id) {
+	return get_point_path(p_from_id, p_to_id, false);
+}
+
+void AStarGrid2D::_bind_compatibility_methods() {
+	ClassDB::bind_compatibility_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStarGrid2D::_get_id_path_bind_compat_88047);
+	ClassDB::bind_compatibility_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStarGrid2D::_get_point_path_bind_compat_88047);
+}
+
+#endif // DISABLE_DEPRECATED
diff --git a/core/math/a_star_grid_2d.cpp b/core/math/a_star_grid_2d.cpp
index d17f465ab8..f272407869 100644
--- a/core/math/a_star_grid_2d.cpp
+++ b/core/math/a_star_grid_2d.cpp
@@ -29,6 +29,7 @@
 /**************************************************************************/
 
 #include "a_star_grid_2d.h"
+#include "a_star_grid_2d.compat.inc"
 
 #include "core/variant/typed_array.h"
 
@@ -446,6 +447,7 @@ void AStarGrid2D::_get_nbors(Point *p_point, LocalVector<Point *> &r_nbors) {
 }
 
 bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
+	last_closest_point = nullptr;
 	pass++;
 
 	if (p_end_point->solid) {
@@ -459,12 +461,19 @@ bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
 
 	p_begin_point->g_score = 0;
 	p_begin_point->f_score = _estimate_cost(p_begin_point->id, p_end_point->id);
+	p_begin_point->abs_g_score = 0;
+	p_begin_point->abs_f_score = _estimate_cost(p_begin_point->id, p_end_point->id);
 	open_list.push_back(p_begin_point);
 	end = p_end_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 == p_end_point) {
 			found_route = true;
 			break;
@@ -508,6 +517,9 @@ bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
 			e->g_score = tentative_g_score;
 			e->f_score = e->g_score + _estimate_cost(e->id, p_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());
 			} else {
@@ -546,7 +558,7 @@ Vector2 AStarGrid2D::get_point_position(const Vector2i &p_id) const {
 	return _get_point_unchecked(p_id)->pos;
 }
 
-Vector<Vector2> AStarGrid2D::get_point_path(const Vector2i &p_from_id, const Vector2i &p_to_id) {
+Vector<Vector2> AStarGrid2D::get_point_path(const Vector2i &p_from_id, const Vector2i &p_to_id, bool p_allow_partial_path) {
 	ERR_FAIL_COND_V_MSG(dirty, Vector<Vector2>(), "Grid is not initialized. Call the update method.");
 	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_from_id), Vector<Vector2>(), vformat("Can't get id path. Point %s out of bounds %s.", p_from_id, region));
 	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_to_id), Vector<Vector2>(), vformat("Can't get id path. Point %s out of bounds %s.", p_to_id, region));
@@ -565,7 +577,12 @@ Vector<Vector2> AStarGrid2D::get_point_path(const Vector2i &p_from_id, const Vec
 
 	bool found_route = _solve(begin_point, end_point);
 	if (!found_route) {
-		return Vector<Vector2>();
+		if (!p_allow_partial_path || last_closest_point == nullptr) {
+			return Vector<Vector2>();
+		}
+
+		// Use closest point instead.
+		end_point = last_closest_point;
 	}
 
 	Point *p = end_point;
@@ -594,7 +611,7 @@ Vector<Vector2> AStarGrid2D::get_point_path(const Vector2i &p_from_id, const Vec
 	return path;
 }
 
-TypedArray<Vector2i> AStarGrid2D::get_id_path(const Vector2i &p_from_id, const Vector2i &p_to_id) {
+TypedArray<Vector2i> AStarGrid2D::get_id_path(const Vector2i &p_from_id, const Vector2i &p_to_id, bool p_allow_partial_path) {
 	ERR_FAIL_COND_V_MSG(dirty, TypedArray<Vector2i>(), "Grid is not initialized. Call the update method.");
 	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_from_id), TypedArray<Vector2i>(), vformat("Can't get id path. Point %s out of bounds %s.", p_from_id, region));
 	ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_to_id), TypedArray<Vector2i>(), vformat("Can't get id path. Point %s out of bounds %s.", p_to_id, region));
@@ -613,7 +630,12 @@ TypedArray<Vector2i> AStarGrid2D::get_id_path(const Vector2i &p_from_id, const V
 
 	bool found_route = _solve(begin_point, end_point);
 	if (!found_route) {
-		return TypedArray<Vector2i>();
+		if (!p_allow_partial_path || last_closest_point == nullptr) {
+			return TypedArray<Vector2i>();
+		}
+
+		// Use closest point instead.
+		end_point = last_closest_point;
 	}
 
 	Point *p = end_point;
@@ -672,8 +694,8 @@ void AStarGrid2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("clear"), &AStarGrid2D::clear);
 
 	ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStarGrid2D::get_point_position);
-	ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStarGrid2D::get_point_path);
-	ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStarGrid2D::get_id_path);
+	ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id", "allow_partial_path"), &AStarGrid2D::get_point_path, DEFVAL(false));
+	ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id", "allow_partial_path"), &AStarGrid2D::get_id_path, DEFVAL(false));
 
 	GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id")
 	GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
diff --git a/core/math/a_star_grid_2d.h b/core/math/a_star_grid_2d.h
index 69cb77dd3e..1a9f6dcc11 100644
--- a/core/math/a_star_grid_2d.h
+++ b/core/math/a_star_grid_2d.h
@@ -89,6 +89,10 @@ private:
 		uint64_t open_pass = 0;
 		uint64_t closed_pass = 0;
 
+		// Used for getting last_closest_point.
+		real_t abs_g_score = 0;
+		real_t abs_f_score = 0;
+
 		Point() {}
 
 		Point(const Vector2i &p_id, const Vector2 &p_pos) :
@@ -109,6 +113,7 @@ private:
 
 	LocalVector<LocalVector<Point>> points;
 	Point *end = nullptr;
+	Point *last_closest_point = nullptr;
 
 	uint64_t pass = 1;
 
@@ -152,6 +157,12 @@ protected:
 	GDVIRTUAL2RC(real_t, _estimate_cost, Vector2i, Vector2i)
 	GDVIRTUAL2RC(real_t, _compute_cost, Vector2i, Vector2i)
 
+#ifndef DISABLE_DEPRECATED
+	TypedArray<Vector2i> _get_id_path_bind_compat_88047(const Vector2i &p_from, const Vector2i &p_to);
+	Vector<Vector2> _get_point_path_bind_compat_88047(const Vector2i &p_from, const Vector2i &p_to);
+	static void _bind_compatibility_methods();
+#endif
+
 public:
 	void set_region(const Rect2i &p_region);
 	Rect2i get_region() const;
@@ -198,8 +209,8 @@ public:
 	void clear();
 
 	Vector2 get_point_position(const Vector2i &p_id) const;
-	Vector<Vector2> get_point_path(const Vector2i &p_from, const Vector2i &p_to);
-	TypedArray<Vector2i> get_id_path(const Vector2i &p_from, const Vector2i &p_to);
+	Vector<Vector2> get_point_path(const Vector2i &p_from, const Vector2i &p_to, bool p_allow_partial_path = false);
+	TypedArray<Vector2i> get_id_path(const Vector2i &p_from, const Vector2i &p_to, bool p_allow_partial_path = false);
 };
 
 VARIANT_ENUM_CAST(AStarGrid2D::DiagonalMode);