Add NavigationLink helper functions for global positions

Adds helper functions to set the links start and end position with global positions or get them as global positions.

Adds global start and end position for the navigation link to the 'link_reached' signal of NavigationAgent. That signal gets emitted when a navigation link waypoint is reached. Requires that 'owner' meta data is enabled on the NavigationAgent.

10 files changed, 176 insertions, 0 deletions

diff --git a/doc/classes/NavigationAgent2D.xml b/doc/classes/NavigationAgent2D.xml
index 92fd8bcc6a..c6d03fb6d8 100644
--- a/doc/classes/NavigationAgent2D.xml
+++ b/doc/classes/NavigationAgent2D.xml
@@ -171,6 +171,8 @@
 				- [code]type[/code]: Always [constant NavigationPathQueryResult2D.PATH_SEGMENT_TYPE_LINK].
 				- [code]rid[/code]: The [RID] of the link.
 				- [code]owner[/code]: The object which manages the link (usually [NavigationLink2D]).
+				- [code]link_entry_position[/code]: If [code]owner[/code] is available and the owner is a [NavigationLink2D], it will contain the global position of the link's point the agent is entering.
+				- [code]link_exit_position[/code]: If [code]owner[/code] is available and the owner is a [NavigationLink2D], it will contain the global position of the link's point which the agent is exiting.
 			</description>
 		</signal>
 		<signal name="navigation_finished">
diff --git a/doc/classes/NavigationAgent3D.xml b/doc/classes/NavigationAgent3D.xml
index 0ed11bc477..294fe49408 100644
--- a/doc/classes/NavigationAgent3D.xml
+++ b/doc/classes/NavigationAgent3D.xml
@@ -174,6 +174,8 @@
 				- [code]type[/code]: Always [constant NavigationPathQueryResult3D.PATH_SEGMENT_TYPE_LINK].
 				- [code]rid[/code]: The [RID] of the link.
 				- [code]owner[/code]: The object which manages the link (usually [NavigationLink3D]).
+				- [code]link_entry_position[/code]: If [code]owner[/code] is available and the owner is a [NavigationLink2D], it will contain the global position of the link's point the agent is entering.
+				- [code]link_exit_position[/code]: If [code]owner[/code] is available and the owner is a [NavigationLink2D], it will contain the global position of the link's point which the agent is exiting.
 			</description>
 		</signal>
 		<signal name="navigation_finished">
diff --git a/doc/classes/NavigationLink2D.xml b/doc/classes/NavigationLink2D.xml
index b3f4367675..3f5b1fb184 100644
--- a/doc/classes/NavigationLink2D.xml
+++ b/doc/classes/NavigationLink2D.xml
@@ -10,6 +10,18 @@
 		<link title="Using NavigationLinks">$DOCS_URL/tutorials/navigation/navigation_using_navigationlinks.html</link>
 	</tutorials>
 	<methods>
+		<method name="get_global_end_position" qualifiers="const">
+			<return type="Vector2" />
+			<description>
+				Returns the [member end_position] that is relative to the link as a global position.
+			</description>
+		</method>
+		<method name="get_global_start_position" qualifiers="const">
+			<return type="Vector2" />
+			<description>
+				Returns the [member start_position] that is relative to the link as a global position.
+			</description>
+		</method>
 		<method name="get_navigation_layer_value" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="layer_number" type="int" />
@@ -17,6 +29,20 @@
 				Returns whether or not the specified layer of the [member navigation_layers] bitmask is enabled, given a [code]layer_number[/code] between 1 and 32.
 			</description>
 		</method>
+		<method name="set_global_end_position">
+			<return type="void" />
+			<param index="0" name="position" type="Vector2" />
+			<description>
+				Sets the [member end_position] that is relative to the link from a global [param position].
+			</description>
+		</method>
+		<method name="set_global_start_position">
+			<return type="void" />
+			<param index="0" name="position" type="Vector2" />
+			<description>
+				Sets the [member start_position] that is relative to the link from a global [param position].
+			</description>
+		</method>
 		<method name="set_navigation_layer_value">
 			<return type="void" />
 			<param index="0" name="layer_number" type="int" />
diff --git a/doc/classes/NavigationLink3D.xml b/doc/classes/NavigationLink3D.xml
index 4dff226042..4081426dcd 100644
--- a/doc/classes/NavigationLink3D.xml
+++ b/doc/classes/NavigationLink3D.xml
@@ -10,6 +10,18 @@
 		<link title="Using NavigationLinks">$DOCS_URL/tutorials/navigation/navigation_using_navigationlinks.html</link>
 	</tutorials>
 	<methods>
+		<method name="get_global_end_position" qualifiers="const">
+			<return type="Vector3" />
+			<description>
+				Returns the [member end_position] that is relative to the link as a global position.
+			</description>
+		</method>
+		<method name="get_global_start_position" qualifiers="const">
+			<return type="Vector3" />
+			<description>
+				Returns the [member start_position] that is relative to the link as a global position.
+			</description>
+		</method>
 		<method name="get_navigation_layer_value" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="layer_number" type="int" />
@@ -17,6 +29,20 @@
 				Returns whether or not the specified layer of the [member navigation_layers] bitmask is enabled, given a [code]layer_number[/code] between 1 and 32.
 			</description>
 		</method>
+		<method name="set_global_end_position">
+			<return type="void" />
+			<param index="0" name="position" type="Vector3" />
+			<description>
+				Sets the [member end_position] that is relative to the link from a global [param position].
+			</description>
+		</method>
+		<method name="set_global_start_position">
+			<return type="void" />
+			<param index="0" name="position" type="Vector3" />
+			<description>
+				Sets the [member start_position] that is relative to the link from a global [param position].
+			</description>
+		</method>
 		<method name="set_navigation_layer_value">
 			<return type="void" />
 			<param index="0" name="layer_number" type="int" />
diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp
index 52a1213a49..1ee6a0b779 100644
--- a/scene/2d/navigation_agent_2d.cpp
+++ b/scene/2d/navigation_agent_2d.cpp
@@ -31,6 +31,7 @@
 #include "navigation_agent_2d.h"
 
 #include "core/math/geometry_2d.h"
+#include "scene/2d/navigation_link_2d.h"
 #include "scene/resources/world_2d.h"
 #include "servers/navigation_server_2d.h"
 
@@ -623,6 +624,21 @@ void NavigationAgent2D::update_navigation() {
 				}
 
 				details[SNAME("owner")] = owner;
+
+				if (waypoint_type == NavigationPathQueryResult2D::PATH_SEGMENT_TYPE_LINK) {
+					const NavigationLink2D *navlink = Object::cast_to<NavigationLink2D>(owner);
+					if (navlink) {
+						Vector2 link_global_start_position = navlink->get_global_start_position();
+						Vector2 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/2d/navigation_link_2d.cpp b/scene/2d/navigation_link_2d.cpp
index 26dca40176..8adb7c6305 100644
--- a/scene/2d/navigation_link_2d.cpp
+++ b/scene/2d/navigation_link_2d.cpp
@@ -54,6 +54,12 @@ void NavigationLink2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_end_position", "position"), &NavigationLink2D::set_end_position);
 	ClassDB::bind_method(D_METHOD("get_end_position"), &NavigationLink2D::get_end_position);
 
+	ClassDB::bind_method(D_METHOD("set_global_start_position", "position"), &NavigationLink2D::set_global_start_position);
+	ClassDB::bind_method(D_METHOD("get_global_start_position"), &NavigationLink2D::get_global_start_position);
+
+	ClassDB::bind_method(D_METHOD("set_global_end_position", "position"), &NavigationLink2D::set_global_end_position);
+	ClassDB::bind_method(D_METHOD("get_global_end_position"), &NavigationLink2D::get_global_end_position);
+
 	ClassDB::bind_method(D_METHOD("set_enter_cost", "enter_cost"), &NavigationLink2D::set_enter_cost);
 	ClassDB::bind_method(D_METHOD("get_enter_cost"), &NavigationLink2D::get_enter_cost);
 
@@ -271,6 +277,38 @@ void NavigationLink2D::set_end_position(Vector2 p_position) {
 #endif // DEBUG_ENABLED
 }
 
+void NavigationLink2D::set_global_start_position(Vector2 p_position) {
+	if (is_inside_tree()) {
+		set_start_position(to_local(p_position));
+	} else {
+		set_start_position(p_position);
+	}
+}
+
+Vector2 NavigationLink2D::get_global_start_position() const {
+	if (is_inside_tree()) {
+		return to_global(start_position);
+	} else {
+		return start_position;
+	}
+}
+
+void NavigationLink2D::set_global_end_position(Vector2 p_position) {
+	if (is_inside_tree()) {
+		set_end_position(to_local(p_position));
+	} else {
+		set_end_position(p_position);
+	}
+}
+
+Vector2 NavigationLink2D::get_global_end_position() const {
+	if (is_inside_tree()) {
+		return to_global(end_position);
+	} else {
+		return end_position;
+	}
+}
+
 void NavigationLink2D::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/2d/navigation_link_2d.h b/scene/2d/navigation_link_2d.h
index 5bf2a72358..8a24d611c9 100644
--- a/scene/2d/navigation_link_2d.h
+++ b/scene/2d/navigation_link_2d.h
@@ -78,6 +78,12 @@ public:
 	void set_end_position(Vector2 p_position);
 	Vector2 get_end_position() const { return end_position; }
 
+	void set_global_start_position(Vector2 p_position);
+	Vector2 get_global_start_position() const;
+
+	void set_global_end_position(Vector2 p_position);
+	Vector2 get_global_end_position() const;
+
 	void set_enter_cost(real_t p_enter_cost);
 	real_t get_enter_cost() const { return enter_cost; }
 
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; }