Fix obstacle avoidance and 3D editor ignoring transform

4 files changed, 48 insertions, 10 deletions

diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp
index c6d1447ad8..46bcda9c6d 100644
--- a/scene/2d/navigation_obstacle_2d.cpp
+++ b/scene/2d/navigation_obstacle_2d.cpp
@@ -89,7 +89,7 @@ void NavigationObstacle2D::_notification(int p_what) {
 			previous_transform = get_global_transform();
 			// need to trigger map controlled agent assignment somehow for the fake_agent since obstacles use no callback like regular agents
 			NavigationServer2D::get_singleton()->obstacle_set_avoidance_enabled(obstacle, avoidance_enabled);
-			_update_position(get_global_position());
+			_update_transform();
 			set_physics_process_internal(true);
 #ifdef DEBUG_ENABLED
 			RS::get_singleton()->canvas_item_set_parent(debug_canvas_item, get_world_2d()->get_canvas());
@@ -142,7 +142,7 @@ void NavigationObstacle2D::_notification(int p_what) {
 
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
 			if (is_inside_tree()) {
-				_update_position(get_global_position());
+				_update_transform();
 
 				if (velocity_submitted) {
 					velocity_submitted = false;
@@ -206,7 +206,8 @@ NavigationObstacle2D::~NavigationObstacle2D() {
 
 void NavigationObstacle2D::set_vertices(const Vector<Vector2> &p_vertices) {
 	vertices = p_vertices;
-	NavigationServer2D::get_singleton()->obstacle_set_vertices(obstacle, vertices);
+	const Transform2D node_transform = is_inside_tree() ? get_global_transform() : Transform2D();
+	NavigationServer2D::get_singleton()->obstacle_set_vertices(obstacle, node_transform.xform(vertices));
 #ifdef DEBUG_ENABLED
 	queue_redraw();
 #endif // DEBUG_ENABLED
@@ -237,7 +238,8 @@ void NavigationObstacle2D::set_radius(real_t p_radius) {
 
 	radius = p_radius;
 
-	NavigationServer2D::get_singleton()->obstacle_set_radius(obstacle, radius);
+	const Vector2 safe_scale = (is_inside_tree() ? get_global_scale() : get_scale()).abs().maxf(0.001);
+	NavigationServer2D::get_singleton()->obstacle_set_radius(obstacle, safe_scale[safe_scale.max_axis_index()] * radius);
 #ifdef DEBUG_ENABLED
 	queue_redraw();
 #endif // DEBUG_ENABLED
@@ -341,6 +343,18 @@ void NavigationObstacle2D::_update_position(const Vector2 p_position) {
 #endif // DEBUG_ENABLED
 }
 
+void NavigationObstacle2D::_update_transform() {
+	_update_position(get_global_position());
+	// Prevent non-positive or non-uniform scaling of dynamic obstacle radius.
+	const Vector2 safe_scale = get_global_scale().abs().maxf(0.001);
+	const float scaling_max_value = safe_scale[safe_scale.max_axis_index()];
+	NavigationServer2D::get_singleton()->obstacle_set_radius(obstacle, scaling_max_value * radius);
+	NavigationServer2D::get_singleton()->obstacle_set_vertices(obstacle, get_global_transform().translated(-get_global_position()).xform(vertices));
+#ifdef DEBUG_ENABLED
+	queue_redraw();
+#endif // DEBUG_ENABLED
+}
+
 #ifdef DEBUG_ENABLED
 void NavigationObstacle2D::_update_fake_agent_radius_debug() {
 	if (radius > 0.0 && NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_radius()) {
@@ -373,7 +387,7 @@ void NavigationObstacle2D::_update_static_obstacle_debug() {
 		debug_obstacle_polygon_colors.resize(debug_obstacle_polygon_vertices.size());
 		debug_obstacle_polygon_colors.fill(debug_static_obstacle_face_color);
 
-		RS::get_singleton()->canvas_item_add_polygon(debug_canvas_item, debug_obstacle_polygon_vertices, debug_obstacle_polygon_colors);
+		RS::get_singleton()->canvas_item_add_polygon(debug_canvas_item, get_global_transform().xform(debug_obstacle_polygon_vertices), debug_obstacle_polygon_colors);
 
 		Color debug_static_obstacle_edge_color;
 
diff --git a/scene/2d/navigation_obstacle_2d.h b/scene/2d/navigation_obstacle_2d.h
index 34e585142d..6e3596acf4 100644
--- a/scene/2d/navigation_obstacle_2d.h
+++ b/scene/2d/navigation_obstacle_2d.h
@@ -111,6 +111,7 @@ public:
 private:
 	void _update_map(RID p_map);
 	void _update_position(const Vector2 p_position);
+	void _update_transform();
 };
 
 #endif // NAVIGATION_OBSTACLE_2D_H
diff --git a/scene/3d/navigation_obstacle_3d.cpp b/scene/3d/navigation_obstacle_3d.cpp
index 68a6505897..7be01589f4 100644
--- a/scene/3d/navigation_obstacle_3d.cpp
+++ b/scene/3d/navigation_obstacle_3d.cpp
@@ -94,7 +94,7 @@ void NavigationObstacle3D::_notification(int p_what) {
 			}
 			// need to trigger map controlled agent assignment somehow for the fake_agent since obstacles use no callback like regular agents
 			NavigationServer3D::get_singleton()->obstacle_set_avoidance_enabled(obstacle, avoidance_enabled);
-			_update_position(get_global_position());
+			_update_transform();
 			set_physics_process_internal(true);
 #ifdef DEBUG_ENABLED
 			_update_debug();
@@ -153,7 +153,7 @@ void NavigationObstacle3D::_notification(int p_what) {
 
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
 			if (is_inside_tree()) {
-				_update_position(get_global_position());
+				_update_transform();
 
 				if (velocity_submitted) {
 					velocity_submitted = false;
@@ -258,7 +258,11 @@ NavigationObstacle3D::~NavigationObstacle3D() {
 void NavigationObstacle3D::set_vertices(const Vector<Vector3> &p_vertices) {
 	vertices = p_vertices;
 
-	NavigationServer3D::get_singleton()->obstacle_set_vertices(obstacle, vertices);
+	const Basis basis = is_inside_tree() ? get_global_basis() : get_basis();
+	const float rotation_y = is_inside_tree() ? get_global_rotation().y : get_rotation().y;
+	const Vector3 safe_scale = basis.get_scale().abs().maxf(0.001);
+	const Transform3D safe_transform = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), rotation_y), Vector3());
+	NavigationServer3D::get_singleton()->obstacle_set_vertices(obstacle, safe_transform.xform(vertices));
 #ifdef DEBUG_ENABLED
 	_update_static_obstacle_debug();
 	update_gizmos();
@@ -289,7 +293,10 @@ void NavigationObstacle3D::set_radius(real_t p_radius) {
 	}
 
 	radius = p_radius;
-	NavigationServer3D::get_singleton()->obstacle_set_radius(obstacle, radius);
+
+	// Prevent non-positive or non-uniform scaling of dynamic obstacle radius.
+	const Vector3 safe_scale = (is_inside_tree() ? get_global_basis() : get_basis()).get_scale().abs().maxf(0.001);
+	NavigationServer3D::get_singleton()->obstacle_set_radius(obstacle, safe_scale[safe_scale.max_axis_index()] * radius);
 
 #ifdef DEBUG_ENABLED
 	_update_fake_agent_radius_debug();
@@ -304,7 +311,8 @@ void NavigationObstacle3D::set_height(real_t p_height) {
 	}
 
 	height = p_height;
-	NavigationServer3D::get_singleton()->obstacle_set_height(obstacle, height);
+	const float scale_factor = MAX(Math::abs((is_inside_tree() ? get_global_basis() : get_basis()).get_scale().y), 0.001);
+	NavigationServer3D::get_singleton()->obstacle_set_height(obstacle, scale_factor * height);
 
 #ifdef DEBUG_ENABLED
 	_update_static_obstacle_debug();
@@ -407,6 +415,20 @@ void NavigationObstacle3D::_update_position(const Vector3 p_position) {
 	NavigationServer3D::get_singleton()->obstacle_set_position(obstacle, p_position);
 }
 
+void NavigationObstacle3D::_update_transform() {
+	_update_position(get_global_position());
+
+	// Prevent non-positive or non-uniform scaling of dynamic obstacle radius.
+	const Vector3 safe_scale = get_global_basis().get_scale().abs().maxf(0.001);
+	const float scaling_max_value = safe_scale[safe_scale.max_axis_index()];
+	NavigationServer3D::get_singleton()->obstacle_set_radius(obstacle, scaling_max_value * radius);
+
+	// Apply modified node transform which only takes y-axis rotation into account to vertices.
+	const Transform3D safe_transform = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), get_global_rotation().y), Vector3());
+	NavigationServer3D::get_singleton()->obstacle_set_vertices(obstacle, safe_transform.xform(vertices));
+	NavigationServer3D::get_singleton()->obstacle_set_height(obstacle, safe_scale.y * height);
+}
+
 void NavigationObstacle3D::_update_use_3d_avoidance(bool p_use_3d_avoidance) {
 	NavigationServer3D::get_singleton()->obstacle_set_use_3d_avoidance(obstacle, use_3d_avoidance);
 	_update_map(map_current);
diff --git a/scene/3d/navigation_obstacle_3d.h b/scene/3d/navigation_obstacle_3d.h
index 99a5770f02..25f376e68b 100644
--- a/scene/3d/navigation_obstacle_3d.h
+++ b/scene/3d/navigation_obstacle_3d.h
@@ -121,6 +121,7 @@ public:
 private:
 	void _update_map(RID p_map);
 	void _update_position(const Vector3 p_position);
+	void _update_transform();
 	void _update_use_3d_avoidance(bool p_use_3d_avoidance);
 };