From 96b8c4d3239e70568a50f98e4494708c02126d69 Mon Sep 17 00:00:00 2001
From: smix8 <52464204+smix8@users.noreply.github.com>
Date: Sun, 15 May 2022 20:29:23 +0200
Subject: [PATCH] Fix NavigationObstacle2D/3D get_global_transform() error

Fixes NavigationObstacle2D/3D reporting a 'get_global_transform: Condition "!is_inside_tree()" error when estimating the agent radius.

The collisionshapes that are lower in the SceneTree order than the obstacle node are not loaded in the SceneTree yet so the global_transform function fails.

Also adds warning message when this happens.

(cherry picked from commit cc707412e909f117ad53ebe53e19bad43acf086a)
---
 scene/2d/navigation_obstacle_2d.cpp | 9 ++++++---
 scene/3d/navigation_obstacle.cpp    | 9 ++++++---
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp
index 0ed000954..ad53cc018 100644
--- a/scene/2d/navigation_obstacle_2d.cpp
+++ b/scene/2d/navigation_obstacle_2d.cpp
@@ -93,7 +93,7 @@ void NavigationObstacle2D::_notification(int p_what) {
 			parent_node2d = nullptr;
 		} break;
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
-			if (parent_node2d) {
+			if (parent_node2d && parent_node2d->is_inside_tree()) {
 				Navigation2DServer::get_singleton()->agent_set_position(agent, parent_node2d->get_global_transform().get_origin());
 			}
 
@@ -156,13 +156,13 @@ void NavigationObstacle2D::reevaluate_agent_radius() {
 }
 
 real_t NavigationObstacle2D::estimate_agent_radius() const {
-	if (parent_node2d) {
+	if (parent_node2d && parent_node2d->is_inside_tree()) {
 		// Estimate the radius of this physics body
 		real_t radius = 0.0;
 		for (int i(0); i < parent_node2d->get_child_count(); i++) {
 			// For each collision shape
 			CollisionShape2D *cs = Object::cast_to<CollisionShape2D>(parent_node2d->get_child(i));
-			if (cs) {
+			if (cs && cs->is_inside_tree()) {
 				// Take the distance between the Body center to the shape center
 				real_t r = cs->get_transform().get_origin().length();
 				if (cs->get_shape().is_valid()) {
@@ -173,6 +173,9 @@ real_t NavigationObstacle2D::estimate_agent_radius() const {
 				r *= MAX(s.x, s.y);
 				// Takes the biggest radius
 				radius = MAX(radius, r);
+			} else if (cs && !cs->is_inside_tree()) {
+				WARN_PRINT("A CollisionShape2D of the NavigationObstacle2D parent node was not inside the SceneTree when estimating the obstacle radius."
+						   "\nMove the NavigationObstacle2D to a child position below any CollisionShape2D node of the parent node so the CollisionShape2D is already inside the SceneTree.");
 			}
 		}
 		Vector2 s = parent_node2d->get_global_transform().get_scale();
diff --git a/scene/3d/navigation_obstacle.cpp b/scene/3d/navigation_obstacle.cpp
index ee08e8f62..27ce81750 100644
--- a/scene/3d/navigation_obstacle.cpp
+++ b/scene/3d/navigation_obstacle.cpp
@@ -94,7 +94,7 @@ void NavigationObstacle::_notification(int p_what) {
 			parent_spatial = nullptr;
 		} break;
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
-			if (parent_spatial) {
+			if (parent_spatial && parent_spatial->is_inside_tree()) {
 				NavigationServer::get_singleton()->agent_set_position(agent, parent_spatial->get_global_transform().origin);
 			}
 
@@ -164,13 +164,13 @@ void NavigationObstacle::reevaluate_agent_radius() {
 }
 
 real_t NavigationObstacle::estimate_agent_radius() const {
-	if (parent_spatial) {
+	if (parent_spatial && parent_spatial->is_inside_tree()) {
 		// Estimate the radius of this physics body
 		real_t radius = 0.0;
 		for (int i(0); i < parent_spatial->get_child_count(); i++) {
 			// For each collision shape
 			CollisionShape *cs = Object::cast_to<CollisionShape>(parent_spatial->get_child(i));
-			if (cs) {
+			if (cs && cs->is_inside_tree()) {
 				// Take the distance between the Body center to the shape center
 				real_t r = cs->get_transform().origin.length();
 				if (cs->get_shape().is_valid()) {
@@ -181,6 +181,9 @@ real_t NavigationObstacle::estimate_agent_radius() const {
 				r *= MAX(s.x, MAX(s.y, s.z));
 				// Takes the biggest radius
 				radius = MAX(radius, r);
+			} else if (cs && !cs->is_inside_tree()) {
+				WARN_PRINT("A CollisionShape of the NavigationObstacle parent node was not inside the SceneTree when estimating the obstacle radius."
+						   "\nMove the NavigationObstacle to a child position below any CollisionShape node of the parent node so the CollisionShape is already inside the SceneTree.");
 			}
 		}
 		Vector3 s = parent_spatial->get_global_transform().basis.get_scale();