From 31a3b418f72f8665db1420f0c220cce28ac0069a Mon Sep 17 00:00:00 2001 From: Rie Date: Wed, 13 Nov 2024 16:16:48 +0100 Subject: [PATCH] Fix obstacle avoidance and 3D editor ignoring transform --- .../navigation_obstacle_3d_editor_plugin.cpp | 55 +++++++++++-------- scene/2d/navigation_obstacle_2d.cpp | 24 ++++++-- scene/2d/navigation_obstacle_2d.h | 1 + scene/3d/navigation_obstacle_3d.cpp | 32 +++++++++-- scene/3d/navigation_obstacle_3d.h | 1 + 5 files changed, 79 insertions(+), 34 deletions(-) diff --git a/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp b/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp index 94ad20f05a2..efa98d8e4d9 100644 --- a/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp +++ b/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp @@ -69,8 +69,10 @@ void NavigationObstacle3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { } float height = obstacle->get_height(); - Basis gbi = obstacle->get_global_basis().inverse(); + const Basis safe_basis = Basis(Vector3(0.0, 1.0, 0.0), obstacle->get_global_rotation().y, obstacle->get_global_basis().get_scale().abs().maxf(0.001)); + const Basis gbi = obstacle->get_global_basis().inverse(); + const Basis safe_global_basis = gbi * safe_basis; const int vertex_count = vertices.size(); Vector lines_mesh_vertices; @@ -83,21 +85,22 @@ void NavigationObstacle3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { Vector3 point = vertices[i]; Vector3 next_point = vertices[(i + 1) % vertex_count]; - Vector3 direction = next_point.direction_to(point); + Vector3 direction = safe_basis.xform(next_point.direction_to(point)); Vector3 arrow_dir = direction.cross(Vector3(0.0, 1.0, 0.0)); Vector3 edge_middle = point + ((next_point - point) * 0.5); - lines_mesh_vertices_ptrw[vertex_index++] = gbi.xform(edge_middle); - lines_mesh_vertices_ptrw[vertex_index++] = gbi.xform(edge_middle + (arrow_dir * 0.5)); + // Ensure vector stays perpendicular even when scaled non-uniformly. + lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(edge_middle); + lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(edge_middle) + gbi.xform(arrow_dir) * 0.5; - lines_mesh_vertices_ptrw[vertex_index++] = gbi.xform(point); - lines_mesh_vertices_ptrw[vertex_index++] = gbi.xform(next_point); + lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(point); + lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(next_point); - lines_mesh_vertices_ptrw[vertex_index++] = gbi.xform(Vector3(point.x, height, point.z)); - lines_mesh_vertices_ptrw[vertex_index++] = gbi.xform(Vector3(next_point.x, height, next_point.z)); + lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(Vector3(point.x, height, point.z)); + lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(Vector3(next_point.x, height, next_point.z)); - lines_mesh_vertices_ptrw[vertex_index++] = gbi.xform(point); - lines_mesh_vertices_ptrw[vertex_index++] = gbi.xform(Vector3(point.x, height, point.z)); + lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(point); + lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(Vector3(point.x, height, point.z)); } Vector polygon_2d_vertices; @@ -138,7 +141,8 @@ int NavigationObstacle3DGizmoPlugin::subgizmos_intersect_ray(const EditorNode3DG NavigationObstacle3D *obstacle_node = Object::cast_to(p_gizmo->get_node_3d()); ERR_FAIL_NULL_V(obstacle_node, -1); - Transform3D gt = Transform3D(Basis(), obstacle_node->get_global_position()); + const Vector3 safe_scale = obstacle_node->get_global_basis().get_scale().abs().maxf(0.001); + const Transform3D gt = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y), obstacle_node->get_global_position()); const Vector &vertices = obstacle_node->get_vertices(); for (int idx = 0; idx < vertices.size(); ++idx) { @@ -160,7 +164,8 @@ Vector NavigationObstacle3DGizmoPlugin::subgizmos_intersect_frustum(const E NavigationObstacle3D *obstacle_node = Object::cast_to(p_gizmo->get_node_3d()); ERR_FAIL_NULL_V(obstacle_node, contained_points); - Transform3D gt = Transform3D(Basis(), obstacle_node->get_global_position()); + const Vector3 safe_scale = obstacle_node->get_global_basis().get_scale().abs().maxf(0.001); + const Transform3D gt = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y), obstacle_node->get_global_position()); const Vector &vertices = obstacle_node->get_vertices(); for (int idx = 0; idx < vertices.size(); ++idx) { @@ -188,9 +193,8 @@ Transform3D NavigationObstacle3DGizmoPlugin::get_subgizmo_transform(const Editor const Vector &vertices = obstacle_node->get_vertices(); ERR_FAIL_INDEX_V(p_id, vertices.size(), Transform3D()); - Basis gbi = obstacle_node->get_global_basis().inverse(); - - Transform3D subgizmo_transform = Transform3D(Basis(), gbi.xform(vertices[p_id])); + const Basis safe_basis_inverse = Basis(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y, obstacle_node->get_global_basis().get_scale().abs().maxf(0.001)).inverse(); + Transform3D subgizmo_transform = Transform3D(Basis(), safe_basis_inverse.xform(vertices[p_id])); return subgizmo_transform; } @@ -198,14 +202,13 @@ void NavigationObstacle3DGizmoPlugin::set_subgizmo_transform(const EditorNode3DG NavigationObstacle3D *obstacle_node = Object::cast_to(p_gizmo->get_node_3d()); ERR_FAIL_NULL(obstacle_node); - Basis gb = obstacle_node->get_global_basis(); - + const Basis safe_basis = Basis(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y, obstacle_node->get_global_basis().get_scale().abs().maxf(0.001)); Vector3 new_vertex_pos = p_transform.origin; Vector vertices = obstacle_node->get_vertices(); ERR_FAIL_INDEX(p_id, vertices.size()); - Vector3 vertex = gb.xform(new_vertex_pos); + Vector3 vertex = safe_basis.xform(new_vertex_pos); vertex.y = 0.0; vertices.write[p_id] = vertex; @@ -216,14 +219,14 @@ void NavigationObstacle3DGizmoPlugin::commit_subgizmos(const EditorNode3DGizmo * NavigationObstacle3D *obstacle_node = Object::cast_to(p_gizmo->get_node_3d()); ERR_FAIL_NULL(obstacle_node); - Basis gb = obstacle_node->get_global_basis(); + const Basis safe_basis = Basis(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y, obstacle_node->get_global_basis().get_scale().abs().maxf(0.001)); Vector vertices = obstacle_node->get_vertices(); Vector restore_vertices = vertices; for (int i = 0; i < p_ids.size(); ++i) { const int idx = p_ids[i]; - Vector3 vertex = gb.xform(p_restore[i].origin); + Vector3 vertex = safe_basis.xform(p_restore[i].origin); vertex.y = 0.0; restore_vertices.write[idx] = vertex; } @@ -446,7 +449,8 @@ EditorPlugin::AfterGUIInput NavigationObstacle3DEditorPlugin::forward_3d_gui_inp Vector3 ray_from = p_camera->project_ray_origin(mouse_position); Vector3 ray_dir = p_camera->project_ray_normal(mouse_position); - Transform3D gt = Transform3D(Basis(), obstacle_node->get_global_position()); + const Vector3 safe_scale = obstacle_node->get_global_basis().get_scale().abs().maxf(0.001); + const Transform3D gt = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y), obstacle_node->get_global_position()); Transform3D gi = gt.affine_inverse(); Plane projection_plane(Vector3(0.0, 1.0, 0.0), gt.origin); @@ -666,7 +670,8 @@ EditorPlugin::AfterGUIInput NavigationObstacle3DEditorPlugin::forward_3d_gui_inp Vector3 ray_from = p_camera->project_ray_origin(mouse_position); Vector3 ray_dir = p_camera->project_ray_normal(mouse_position); - Transform3D gt = Transform3D(Basis(), obstacle_node->get_global_position()); + const Vector3 safe_scale = obstacle_node->get_global_basis().get_scale().abs().maxf(0.001); + const Transform3D gt = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y), obstacle_node->get_global_position()); Transform3D gi = gt.affine_inverse(); Plane projection_plane(Vector3(0.0, 1.0, 0.0), gt.origin); @@ -762,7 +767,9 @@ void NavigationObstacle3DEditorPlugin::redraw() { rs->mesh_add_surface_from_arrays(point_lines_mesh_rid, RS::PRIMITIVE_LINES, point_lines_mesh_array); rs->instance_set_surface_override_material(point_lines_instance_rid, 0, line_material->get_rid()); - rs->instance_set_transform(point_lines_instance_rid, Transform3D(Basis(), obstacle_node->get_global_position())); + const Vector3 safe_scale = obstacle_node->get_global_basis().get_scale().abs().maxf(0.001); + const Transform3D gt = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y), obstacle_node->get_global_position()); + rs->instance_set_transform(point_lines_instance_rid, gt); Array point_handle_mesh_array; point_handle_mesh_array.resize(Mesh::ARRAY_MAX); @@ -787,7 +794,7 @@ void NavigationObstacle3DEditorPlugin::redraw() { rs->mesh_add_surface_from_arrays(point_handle_mesh_rid, RS::PRIMITIVE_POINTS, point_handle_mesh_array); rs->instance_set_surface_override_material(point_handles_instance_rid, 0, handle_material->get_rid()); - rs->instance_set_transform(point_handles_instance_rid, Transform3D(Basis(), obstacle_node->get_global_position())); + rs->instance_set_transform(point_handles_instance_rid, gt); } NavigationObstacle3DEditorPlugin *NavigationObstacle3DEditorPlugin::singleton = nullptr; diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp index c6d1447ad89..46bcda9c6dd 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 &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 34e585142d4..6e3596acf43 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 68a6505897f..7be01589f46 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 &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 99a5770f025..25f376e68b6 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); };