Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add a partial path return option for AStar #88047

Merged
merged 1 commit into from
Apr 4, 2024
Merged

Add a partial path return option for AStar #88047

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
59 changes: 59 additions & 0 deletions core/math/a_star.compat.inc
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
65 changes: 53 additions & 12 deletions core/math/a_star.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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) {
Expand All @@ -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;
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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));
Expand All @@ -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;
Expand Down Expand Up @@ -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));
Expand All @@ -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;
Expand Down Expand Up @@ -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")
Expand Down Expand Up @@ -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));
Expand All @@ -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;
Expand Down Expand Up @@ -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));
Expand All @@ -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;
Expand Down Expand Up @@ -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) {
Expand All @@ -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;
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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")
Expand Down
25 changes: 21 additions & 4 deletions core/math/a_star.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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);

Expand All @@ -121,6 +126,12 @@ class AStar3D : public RefCounted {
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;

Expand Down Expand Up @@ -149,8 +160,8 @@ class AStar3D : public RefCounted {
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();
Expand All @@ -171,6 +182,12 @@ class AStar2D : public RefCounted {
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;

Expand Down Expand Up @@ -199,8 +216,8 @@ class AStar2D : public RefCounted {
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() {}
Expand Down
48 changes: 48 additions & 0 deletions core/math/a_star_grid_2d.compat.inc
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Loading
Loading