mirror of
https://github.com/godotengine/godot.git
synced 2024-11-21 11:32:13 +00:00
Merge pull request #99176 from tracefree/obstacle_transform
Fix obstacle avoidance and 3D editor ignoring transform
This commit is contained in:
commit
8d530bc95a
@ -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<Vector3> 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<Vector2> polygon_2d_vertices;
|
||||
@ -138,7 +141,8 @@ int NavigationObstacle3DGizmoPlugin::subgizmos_intersect_ray(const EditorNode3DG
|
||||
NavigationObstacle3D *obstacle_node = Object::cast_to<NavigationObstacle3D>(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<Vector3> &vertices = obstacle_node->get_vertices();
|
||||
|
||||
for (int idx = 0; idx < vertices.size(); ++idx) {
|
||||
@ -160,7 +164,8 @@ Vector<int> NavigationObstacle3DGizmoPlugin::subgizmos_intersect_frustum(const E
|
||||
NavigationObstacle3D *obstacle_node = Object::cast_to<NavigationObstacle3D>(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<Vector3> &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<Vector3> &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<NavigationObstacle3D>(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<Vector3> 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<NavigationObstacle3D>(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<Vector3> vertices = obstacle_node->get_vertices();
|
||||
Vector<Vector3> 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;
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
@ -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);
|
||||
};
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user