From ed8323cf96a5a9cbed6543619f98e6fd97ffcaee Mon Sep 17 00:00:00 2001 From: Robert Colton Date: Wed, 15 Jan 2025 16:29:15 -0500 Subject: [PATCH 1/7] Update a_star.h --- core/math/a_star.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/core/math/a_star.h b/core/math/a_star.h index e510923bd0f8..b5cdd19d3c42 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -159,6 +159,7 @@ 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 get_closest_segment(const Vector3 &p_point) const; Vector get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false); Vector get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false); @@ -215,6 +216,7 @@ 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 get_closest_segment(const Vector2 &p_point) const; Vector get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false); Vector get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false); From 6f59a828a258a08025d9a98e447f2c9523275027 Mon Sep 17 00:00:00 2001 From: Robert Colton Date: Wed, 15 Jan 2025 16:32:55 -0500 Subject: [PATCH 2/7] Update a_star.cpp --- core/math/a_star.cpp | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index 060b1bb1e53e..ceba352e3878 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -318,6 +318,35 @@ Vector3 AStar3D::get_closest_position_in_segment(const Vector3 &p_point) const { return closest_point; } +Vector AStar3D::get_closest_segment(const Vector3 &p_point) const { + real_t closest_dist = 1e20; + Vector3 closest_point; + + for (const Segment &E : segments) { + Point *from_point = nullptr, *to_point = nullptr; + points.lookup(E.key.first, from_point); + points.lookup(E.key.second, to_point); + + if (!(from_point->enabled && to_point->enabled)) { + continue; + } + + Vector3 segment[2] = { + from_point->pos, + to_point->pos, + }; + + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, segment); + real_t d = p_point.distance_squared_to(p); + if (d < closest_dist) { + closest_point = p; + closest_dist = d; + } + } + + return closest_point; +} + bool AStar3D::_solve(Point *begin_point, Point *end_point, bool p_allow_partial_path) { last_closest_point = nullptr; pass++; @@ -574,6 +603,7 @@ 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_closest_segment", "to_position"), &AStar3D::get_closest_segment); 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)); @@ -674,6 +704,11 @@ Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const { return Vector2(p.x, p.y); } +Vector AStar2D::get_closest_segment(const Vector2 &p_point) const { + Vector p = astar.get_closest_segment(Vector3(p_point.x, p_point.y, 0)); + return [Vector2(p.x, p.y)]; +} + real_t AStar2D::_estimate_cost(int64_t p_from_id, int64_t p_end_id) { real_t scost; if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_end_id, scost)) { @@ -913,6 +948,7 @@ 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_closest_segment", "to_position"), &AStar2D::get_closest_segment); 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)); From fc0ab5d75797d45b74e7a8c34130a017a6c1af85 Mon Sep 17 00:00:00 2001 From: Robert Colton Date: Wed, 15 Jan 2025 16:35:21 -0500 Subject: [PATCH 3/7] Update a_star.cpp --- core/math/a_star.cpp | 22 +--------------------- 1 file changed, 1 insertion(+), 21 deletions(-) diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index ceba352e3878..255d14762a74 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -293,27 +293,7 @@ Vector3 AStar3D::get_closest_position_in_segment(const Vector3 &p_point) const { real_t closest_dist = 1e20; Vector3 closest_point; - for (const Segment &E : segments) { - Point *from_point = nullptr, *to_point = nullptr; - points.lookup(E.key.first, from_point); - points.lookup(E.key.second, to_point); - - if (!(from_point->enabled && to_point->enabled)) { - continue; - } - - Vector3 segment[2] = { - from_point->pos, - to_point->pos, - }; - - Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, segment); - real_t d = p_point.distance_squared_to(p); - if (d < closest_dist) { - closest_point = p; - closest_dist = d; - } - } + this->get_closest_segment(p_point); return closest_point; } From 8e0ca9d654bcb164da7934c7c0d65bab2f109430 Mon Sep 17 00:00:00 2001 From: Robert Colton Date: Wed, 15 Jan 2025 21:34:33 -0500 Subject: [PATCH 4/7] Update a_star.cpp --- core/math/a_star.cpp | 31 +++++++++++++++++++++---------- 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index 255d14762a74..99795c6e0bb3 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -290,17 +290,22 @@ int64_t AStar3D::get_closest_point(const Vector3 &p_point, bool p_include_disabl } Vector3 AStar3D::get_closest_position_in_segment(const Vector3 &p_point) const { - real_t closest_dist = 1e20; - Vector3 closest_point; - - this->get_closest_segment(p_point); - + Vector closest_segment_points = this->get_closest_segment(p_point); + Point *from_point = nullptr, *to_point = nullptr; + points.lookup(closest_segment_points[0], from_point); + points.lookup(closest_segment_points[1], to_point); + Vector3 segment[2] = { + from_point->pos, + to_point->pos, + }; + Vector3 closest_point = Geometry3D::get_closest_point_to_segment(p_point, segment); return closest_point; } -Vector AStar3D::get_closest_segment(const Vector3 &p_point) const { +Vector AStar3D::get_closest_segment(const Vector3 &p_point) const { real_t closest_dist = 1e20; Vector3 closest_point; + int64_t closest_segment_point1 = -1, closest_segment_point2 = -1; for (const Segment &E : segments) { Point *from_point = nullptr, *to_point = nullptr; @@ -321,10 +326,16 @@ Vector AStar3D::get_closest_segment(const Vector3 &p_point) const { if (d < closest_dist) { closest_point = p; closest_dist = d; + closest_segment_point1 = E.key.first; + closest_segment_point2 = E.key.second; } } - return closest_point; + Vector closest_segment_points; + closest_segment_points.push_back(closest_segment_point1); + closest_segment_points.push_back(closest_segment_point2); + + return closest_segment_points; } bool AStar3D::_solve(Point *begin_point, Point *end_point, bool p_allow_partial_path) { @@ -684,9 +695,9 @@ Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const { return Vector2(p.x, p.y); } -Vector AStar2D::get_closest_segment(const Vector2 &p_point) const { - Vector p = astar.get_closest_segment(Vector3(p_point.x, p_point.y, 0)); - return [Vector2(p.x, p.y)]; +Vector AStar2D::get_closest_segment(const Vector2 &p_point) const { + Vector p = astar.get_closest_segment(Vector3(p_point.x, p_point.y, 0)); + return p; } real_t AStar2D::_estimate_cost(int64_t p_from_id, int64_t p_end_id) { From a783ed58b6dac65110de29a32ffd72ae5b85036f Mon Sep 17 00:00:00 2001 From: Robert Colton Date: Wed, 15 Jan 2025 21:49:59 -0500 Subject: [PATCH 5/7] Update AStar2D.xml --- doc/classes/AStar2D.xml | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/doc/classes/AStar2D.xml b/doc/classes/AStar2D.xml index 044e1206e9ab..8a991e4e462c 100644 --- a/doc/classes/AStar2D.xml +++ b/doc/classes/AStar2D.xml @@ -135,6 +135,15 @@ The result is in the segment that goes from [code]y = 0[/code] to [code]y = 5[/code]. It's the closest position in the segment to the given point. + + + + + Returns the point ids of the closest segment to [param to_position] which also holds the closest position in any segment. + This can be useful if you need more than just the closest point, so that you can start path finding from the nearest segment + or to snap objects to the tangent of the path. + + From 4384e9e86c4c32b944b3be8593d45ad3f6cee7ee Mon Sep 17 00:00:00 2001 From: Robert Colton Date: Wed, 15 Jan 2025 21:50:48 -0500 Subject: [PATCH 6/7] Update AStar3D.xml --- doc/classes/AStar3D.xml | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/doc/classes/AStar3D.xml b/doc/classes/AStar3D.xml index 31ee6d8e3c4e..f774315b3e15 100644 --- a/doc/classes/AStar3D.xml +++ b/doc/classes/AStar3D.xml @@ -177,6 +177,15 @@ The result is in the segment that goes from [code]y = 0[/code] to [code]y = 5[/code]. It's the closest position in the segment to the given point. + + + + + Returns the point ids of the closest segment to [param to_position] which also holds the closest position in any segment. + This can be useful if you need more than just the closest point, so that you can start path finding from the nearest segment + or to snap objects to the tangent of the path. + + From 34341dda4f209753b83cfdf6fac9439ff9c0912f Mon Sep 17 00:00:00 2001 From: Robert Colton Date: Wed, 15 Jan 2025 21:56:37 -0500 Subject: [PATCH 7/7] fix declared types --- core/math/a_star.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/math/a_star.h b/core/math/a_star.h index b5cdd19d3c42..f08e939424d0 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -159,7 +159,7 @@ 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 get_closest_segment(const Vector3 &p_point) const; + Vector get_closest_segment(const Vector3 &p_point) const; Vector get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false); Vector get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false); @@ -216,7 +216,7 @@ 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 get_closest_segment(const Vector2 &p_point) const; + Vector get_closest_segment(const Vector2 &p_point) const; Vector get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false); Vector get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path = false);