diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index 060b1bb1e53e..99795c6e0bb3 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -290,8 +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 { + 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 { 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; @@ -312,10 +326,16 @@ Vector3 AStar3D::get_closest_position_in_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) { @@ -574,6 +594,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 +695,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 p; +} + 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 +939,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)); diff --git a/core/math/a_star.h b/core/math/a_star.h index e510923bd0f8..f08e939424d0 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); 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. + + 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. + +