Merge pull request #73229 from smix8/navlink_global_positions_4.x

Add NavigationLink helper functions for global positions

3 files changed, 60 insertions, 0 deletions

diff --git a/scene/3d/navigation_agent_3d.cpp b/scene/3d/navigation_agent_3d.cpp
index 16f357194e..5b5ad62d64 100644
--- a/scene/3d/navigation_agent_3d.cpp
+++ b/scene/3d/navigation_agent_3d.cpp
@@ -30,6 +30,7 @@
 
 #include "navigation_agent_3d.h"
 
+#include "scene/3d/navigation_link_3d.h"
 #include "servers/navigation_server_3d.h"
 
 void NavigationAgent3D::_bind_methods() {
@@ -649,6 +650,21 @@ void NavigationAgent3D::update_navigation() {
 				}
 
 				details[SNAME("owner")] = owner;
+
+				if (waypoint_type == NavigationPathQueryResult3D::PATH_SEGMENT_TYPE_LINK) {
+					const NavigationLink3D *navlink = Object::cast_to<NavigationLink3D>(owner);
+					if (navlink) {
+						Vector3 link_global_start_position = navlink->get_global_start_position();
+						Vector3 link_global_end_position = navlink->get_global_end_position();
+						if (waypoint.distance_to(link_global_start_position) < waypoint.distance_to(link_global_end_position)) {
+							details[SNAME("link_entry_position")] = link_global_start_position;
+							details[SNAME("link_exit_position")] = link_global_end_position;
+						} else {
+							details[SNAME("link_entry_position")] = link_global_end_position;
+							details[SNAME("link_exit_position")] = link_global_start_position;
+						}
+					}
+				}
 			}
 
 			// Emit a signal for the waypoint
diff --git a/scene/3d/navigation_link_3d.cpp b/scene/3d/navigation_link_3d.cpp
index f47fcfaf51..9c4b8e7905 100644
--- a/scene/3d/navigation_link_3d.cpp
+++ b/scene/3d/navigation_link_3d.cpp
@@ -163,6 +163,12 @@ void NavigationLink3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_end_position", "position"), &NavigationLink3D::set_end_position);
 	ClassDB::bind_method(D_METHOD("get_end_position"), &NavigationLink3D::get_end_position);
 
+	ClassDB::bind_method(D_METHOD("set_global_start_position", "position"), &NavigationLink3D::set_global_start_position);
+	ClassDB::bind_method(D_METHOD("get_global_start_position"), &NavigationLink3D::get_global_start_position);
+
+	ClassDB::bind_method(D_METHOD("set_global_end_position", "position"), &NavigationLink3D::set_global_end_position);
+	ClassDB::bind_method(D_METHOD("get_global_end_position"), &NavigationLink3D::get_global_end_position);
+
 	ClassDB::bind_method(D_METHOD("set_enter_cost", "enter_cost"), &NavigationLink3D::set_enter_cost);
 	ClassDB::bind_method(D_METHOD("get_enter_cost"), &NavigationLink3D::get_enter_cost);
 
@@ -386,6 +392,38 @@ void NavigationLink3D::set_end_position(Vector3 p_position) {
 	update_configuration_warnings();
 }
 
+void NavigationLink3D::set_global_start_position(Vector3 p_position) {
+	if (is_inside_tree()) {
+		set_start_position(to_local(p_position));
+	} else {
+		set_start_position(p_position);
+	}
+}
+
+Vector3 NavigationLink3D::get_global_start_position() const {
+	if (is_inside_tree()) {
+		return to_global(start_position);
+	} else {
+		return start_position;
+	}
+}
+
+void NavigationLink3D::set_global_end_position(Vector3 p_position) {
+	if (is_inside_tree()) {
+		set_end_position(to_local(p_position));
+	} else {
+		set_end_position(p_position);
+	}
+}
+
+Vector3 NavigationLink3D::get_global_end_position() const {
+	if (is_inside_tree()) {
+		return to_global(end_position);
+	} else {
+		return end_position;
+	}
+}
+
 void NavigationLink3D::set_enter_cost(real_t p_enter_cost) {
 	ERR_FAIL_COND_MSG(p_enter_cost < 0.0, "The enter_cost must be positive.");
 	if (Math::is_equal_approx(enter_cost, p_enter_cost)) {
diff --git a/scene/3d/navigation_link_3d.h b/scene/3d/navigation_link_3d.h
index 5c9ec36189..991f45c85d 100644
--- a/scene/3d/navigation_link_3d.h
+++ b/scene/3d/navigation_link_3d.h
@@ -83,6 +83,12 @@ public:
 	void set_end_position(Vector3 p_position);
 	Vector3 get_end_position() const { return end_position; }
 
+	void set_global_start_position(Vector3 p_position);
+	Vector3 get_global_start_position() const;
+
+	void set_global_end_position(Vector3 p_position);
+	Vector3 get_global_end_position() const;
+
 	void set_enter_cost(real_t p_enter_cost);
 	real_t get_enter_cost() const { return enter_cost; }