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 AStar2D/AStar3D get_closest_segment with ids #101624

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
+48 −1
Open

Add AStar2D/AStar3D get_closest_segment with ids #101624

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
ed8323c
Update a_star.h
RobertBColton Jan 15, 2025
6f59a82
Update a_star.cpp
RobertBColton Jan 15, 2025
fc0ab5d
Update a_star.cpp
RobertBColton Jan 15, 2025
8e0ca9d
Update a_star.cpp
RobertBColton Jan 16, 2025
a783ed5
Update AStar2D.xml
RobertBColton Jan 16, 2025
4384e9e
Update AStar3D.xml
RobertBColton Jan 16, 2025
34341dd
fix declared types
RobertBColton Jan 16, 2025
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
29 changes: 28 additions & 1 deletion core/math/a_star.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<int64_t> 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<int64_t> 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;
Expand All @@ -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<int64_t> 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) {
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -674,6 +695,11 @@ Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const {
return Vector2(p.x, p.y);
}

Vector<int64_t> AStar2D::get_closest_segment(const Vector2 &p_point) const {
Vector<int64_t> 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)) {
Expand Down Expand Up @@ -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));
Expand Down
2 changes: 2 additions & 0 deletions core/math/a_star.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<int64_t> get_closest_segment(const Vector3 &p_point) const;

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);
Expand Down Expand Up @@ -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<int64_t> get_closest_segment(const Vector2 &p_point) const;

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);
Expand Down
9 changes: 9 additions & 0 deletions doc/classes/AStar2D.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
</description>
</method>
<method name="get_closest_segment" qualifiers="const">
<return type="PackedInt64Array" />
<param index="0" name="to_position" type="Vector2" />
<description>
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.
</description>
</method>
<method name="get_id_path">
<return type="PackedInt64Array" />
<param index="0" name="from_id" type="int" />
Expand Down
9 changes: 9 additions & 0 deletions doc/classes/AStar3D.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
</description>
</method>
<method name="get_closest_segment" qualifiers="const">
<return type="PackedInt64Array" />
<param index="0" name="to_position" type="Vector3" />
<description>
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.
</description>
</method>
<method name="get_id_path">
<return type="PackedInt64Array" />
<param index="0" name="from_id" type="int" />
Expand Down