From d87f1247689ae82996aeac77b6e9870bbc88142d Mon Sep 17 00:00:00 2001
From: smix8 <52464204+smix8@users.noreply.github.com>
Date: Mon, 13 Feb 2023 13:18:13 +0100
Subject: 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.
---
 scene/3d/navigation_agent_3d.cpp | 16 ++++++++++++++++
 scene/3d/navigation_link_3d.cpp  | 38 ++++++++++++++++++++++++++++++++++++++
 scene/3d/navigation_link_3d.h    |  6 ++++++
 3 files changed, 60 insertions(+)

(limited to 'scene/3d')

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; }
 
-- 
cgit v1.2.3