From f4bed7726e83ffaea478b50d7c76dad43176d02d Mon Sep 17 00:00:00 2001 From: Grey Date: Thu, 29 Aug 2024 13:57:19 +0800 Subject: [PATCH] Use workflows for mouse interactions (#236) Signed-off-by: Michael X. Grey --- Cargo.lock | 47 +- rmf_site_editor/Cargo.toml | 1 + rmf_site_editor/src/interaction/anchor.rs | 65 +- rmf_site_editor/src/interaction/assets.rs | 13 +- .../src/interaction/camera_controls/cursor.rs | 15 +- .../interaction/camera_controls/keyboard.rs | 8 +- .../src/interaction/camera_controls/utils.rs | 13 +- rmf_site_editor/src/interaction/cursor.rs | 210 +- rmf_site_editor/src/interaction/gizmo.rs | 51 +- rmf_site_editor/src/interaction/lift.rs | 2 +- rmf_site_editor/src/interaction/light.rs | 8 +- rmf_site_editor/src/interaction/mod.rs | 177 +- rmf_site_editor/src/interaction/mode.rs | 135 - .../src/interaction/model_preview.rs | 4 +- rmf_site_editor/src/interaction/picking.rs | 77 +- rmf_site_editor/src/interaction/select.rs | 822 +++++- .../src/interaction/select/create_edges.rs | 405 +++ .../src/interaction/select/create_path.rs | 308 +++ .../src/interaction/select/create_point.rs | 201 ++ .../src/interaction/select/place_object.rs | 112 + .../src/interaction/select/place_object_2d.rs | 207 ++ .../src/interaction/select/place_object_3d.rs | 505 ++++ .../interaction/select/replace_parent_3d.rs | 309 +++ .../src/interaction/select/replace_point.rs | 177 ++ .../src/interaction/select/replace_side.rs | 205 ++ .../src/interaction/select/select_anchor.rs | 765 ++++++ .../src/interaction/select_anchor.rs | 2331 ----------------- rmf_site_editor/src/interaction/visual_cue.rs | 41 +- rmf_site_editor/src/keyboard.rs | 56 +- rmf_site_editor/src/lib.rs | 6 +- rmf_site_editor/src/osm_slippy_map.rs | 1 - rmf_site_editor/src/site/anchor.rs | 55 +- rmf_site_editor/src/site/fiducial.rs | 72 +- rmf_site_editor/src/site/level.rs | 2 +- rmf_site_editor/src/site/mod.rs | 21 +- rmf_site_editor/src/site/model.rs | 106 +- rmf_site_editor/src/site/recall_plugin.rs | 4 +- rmf_site_editor/src/site/sdf.rs | 11 +- .../src/widgets/canvas_tooltips.rs | 79 + rmf_site_editor/src/widgets/creation.rs | 88 +- .../src/widgets/fuel_asset_browser.rs | 38 +- .../src/widgets/inspector/inspect_edge.rs | 15 +- .../src/widgets/inspector/inspect_fiducial.rs | 12 + .../src/widgets/inspector/inspect_point.rs | 119 + .../inspector/inspect_workcell_parent.rs | 14 +- rmf_site_editor/src/widgets/inspector/mod.rs | 6 +- rmf_site_editor/src/widgets/mod.rs | 64 +- .../src/widgets/selector_widget.rs | 2 +- rmf_site_editor/src/widgets/view_lights.rs | 2 +- rmf_site_editor/src/workcell/mod.rs | 17 +- rmf_site_editor/src/workcell/model.rs | 30 +- rmf_site_format/src/misc.rs | 39 + 52 files changed, 4934 insertions(+), 3139 deletions(-) delete mode 100644 rmf_site_editor/src/interaction/mode.rs create mode 100644 rmf_site_editor/src/interaction/select/create_edges.rs create mode 100644 rmf_site_editor/src/interaction/select/create_path.rs create mode 100644 rmf_site_editor/src/interaction/select/create_point.rs create mode 100644 rmf_site_editor/src/interaction/select/place_object.rs create mode 100644 rmf_site_editor/src/interaction/select/place_object_2d.rs create mode 100644 rmf_site_editor/src/interaction/select/place_object_3d.rs create mode 100644 rmf_site_editor/src/interaction/select/replace_parent_3d.rs create mode 100644 rmf_site_editor/src/interaction/select/replace_point.rs create mode 100644 rmf_site_editor/src/interaction/select/replace_side.rs create mode 100644 rmf_site_editor/src/interaction/select/select_anchor.rs delete mode 100644 rmf_site_editor/src/interaction/select_anchor.rs create mode 100644 rmf_site_editor/src/widgets/canvas_tooltips.rs create mode 100644 rmf_site_editor/src/widgets/inspector/inspect_point.rs diff --git a/Cargo.lock b/Cargo.lock index 9e07201a..bc6e5a4d 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -767,7 +767,7 @@ dependencies = [ [[package]] name = "bevy_impulse" version = "0.0.1" -source = "git+https://github.com/open-rmf/bevy_impulse?branch=main#3e681a91033427c5ae24995f4e53d19f656f278f" +source = "git+https://github.com/open-rmf/bevy_impulse?branch=main#2d86d5c58aaf18e5c9be933d3c5155ce1dc80dcc" dependencies = [ "anyhow", "arrayvec", @@ -778,14 +778,24 @@ dependencies = [ "bevy_derive", "bevy_ecs", "bevy_hierarchy", + "bevy_impulse_derive", "bevy_tasks", "bevy_time", "bevy_utils", - "crossbeam", "futures", "itertools", "smallvec", "thiserror", + "tokio", +] + +[[package]] +name = "bevy_impulse_derive" +version = "0.0.1" +source = "git+https://github.com/open-rmf/bevy_impulse?branch=main#2d86d5c58aaf18e5c9be933d3c5155ce1dc80dcc" +dependencies = [ + "quote", + "syn 1.0.109", ] [[package]] @@ -1843,19 +1853,6 @@ version = "1.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" -[[package]] -name = "crossbeam" -version = "0.8.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1137cd7e7fc0fb5d3c5a8678be38ec56e819125d8d7907411fe24ccb943faca8" -dependencies = [ - "crossbeam-channel", - "crossbeam-deque", - "crossbeam-epoch", - "crossbeam-queue", - "crossbeam-utils", -] - [[package]] name = "crossbeam-channel" version = "0.5.12" @@ -1884,15 +1881,6 @@ dependencies = [ "crossbeam-utils", ] -[[package]] -name = "crossbeam-queue" -version = "0.3.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "df0346b5d5e76ac2fe4e327c5fd1118d6be7c51dfb18f9b7922923f287471e35" -dependencies = [ - "crossbeam-utils", -] - [[package]] name = "crossbeam-utils" version = "0.8.19" @@ -4321,6 +4309,7 @@ dependencies = [ name = "rmf_site_editor" version = "0.0.1" dependencies = [ + "anyhow", "bevy", "bevy_egui", "bevy_gltf_export", @@ -4948,6 +4937,16 @@ dependencies = [ "log", ] +[[package]] +name = "tokio" +version = "1.39.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "daa4fb1bc778bd6f04cbfc4bb2d06a7396a8f299dc33ea1900cedaa316f467b1" +dependencies = [ + "backtrace", + "pin-project-lite", +] + [[package]] name = "toml" version = "0.8.10" diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml index 4405b978..69839f84 100644 --- a/rmf_site_editor/Cargo.toml +++ b/rmf_site_editor/Cargo.toml @@ -52,6 +52,7 @@ pathdiff = "*" tera = "1.19.1" ehttp = { version = "0.4", features = ["native-async"] } nalgebra = "0.32.5" +anyhow = "*" [target.'cfg(not(target_arch = "wasm32"))'.dependencies] clap = { version = "4.0.10", features = ["color", "derive", "help", "usage", "suggestions"] } diff --git a/rmf_site_editor/src/interaction/anchor.rs b/rmf_site_editor/src/interaction/anchor.rs index 5a9f2ca0..beef344b 100644 --- a/rmf_site_editor/src/interaction/anchor.rs +++ b/rmf_site_editor/src/interaction/anchor.rs @@ -23,6 +23,12 @@ use crate::{ }; use bevy::prelude::*; +/// Use this resource to indicate whether anchors should be constantly highlighted. +/// This is used during anchor selection modes to make it easier for users to know +/// where selectable anchors are. +#[derive(Clone, Copy, Debug, Resource)] +pub struct HighlightAnchors(pub bool); + #[derive(Component, Debug, Clone, Copy)] pub struct AnchorVisualization { pub body: Entity, @@ -37,6 +43,7 @@ pub fn add_anchor_visual_cues( >, categories: Query<&Category>, site_assets: Res, + highlight: Res, ) { for (e, parent, subordinate, anchor) in &new_anchors { let body_mesh = match categories.get(parent.get()).unwrap() { @@ -65,12 +72,29 @@ pub fn add_anchor_visual_cues( .insert(OutlineVisualization::Anchor { body }) .add_child(body); - // 3D anchors should always be visible with arrow cue meshes - if anchor.is_3D() { - entity_commands.insert(VisualCue::outline()); + let cue = if anchor.is_3D() { + // 3D anchors should always be visible with arrow cue meshes + VisualCue::outline() } else { - entity_commands.insert(VisualCue::outline().irregular()); - } + let mut cue = VisualCue::outline().irregular(); + cue.xray.set_always(highlight.0); + cue + }; + + entity_commands.insert(cue); + } +} + +pub fn on_highlight_anchors_change( + highlight: Res, + mut anchor_visual_cues: Query<&mut VisualCue, With>, +) { + if !highlight.is_changed() { + return; + } + + for mut cue in &mut anchor_visual_cues { + cue.xray.set_always(highlight.0); } } @@ -110,12 +134,12 @@ pub fn update_anchor_proximity_xray( } let p_c = match intersect_ground_params.ground_plane_intersection() { - Some(p) => p, + Some(p) => p.translation, None => return, }; for (anchor_tf, mut cue) in &mut anchors { - // TODO(MXG): Make the proximity range configurable + // TODO(@mxgrey): Make the proximity range configurable let proximity = { // We make the xray effect a little "sticky" so that there isn't an // ugly flicker for anchors that are right at the edge of the @@ -156,22 +180,6 @@ pub fn update_unassigned_anchor_cues( } } -pub fn update_anchor_cues_for_mode( - mode: Res, - mut anchors: Query<&mut VisualCue, With>, -) { - if !mode.is_changed() { - return; - } - - let anchor_always_visible = mode.is_selecting_anchor(); - for mut cue in &mut anchors { - if cue.xray.always() != anchor_always_visible { - cue.xray.set_always(anchor_always_visible); - } - } -} - pub fn update_anchor_visual_cues( mut commands: Commands, mut anchors: Query< @@ -191,10 +199,11 @@ pub fn update_anchor_visual_cues( mut visibility: Query<&mut Visibility>, mut materials: Query<&mut Handle>, deps: Query<&Dependents>, - cursor: Res, + mut cursor: ResMut, site_assets: Res, interaction_assets: Res, debug_mode: Option>, + gizmo_blockers: Res, ) { for ( a, @@ -235,8 +244,10 @@ pub fn update_anchor_visual_cues( .set_support_hovered(!hovered.support_hovering.is_empty()); } - if hovered.is_hovered { - set_visibility(cursor.frame, &mut visibility, false); + if hovered.is_hovered && !gizmo_blockers.blocking() { + cursor.add_blocker(a, &mut visibility); + } else { + cursor.remove_blocker(a, &mut visibility); } if hovered.cue() && selected.cue() { @@ -303,7 +314,7 @@ pub fn update_anchor_visual_cues( } // NOTE(MXG): Currently only anchors ever have support cues, so we filter down -// to entities with AnchorVisualCues. We will need to broaden that if any other +// to entities with AnchorVisualization. We will need to broaden that if any other // visual cue types ever have a supporting role. pub fn remove_deleted_supports_from_visual_cues( mut hovered: Query<&mut Hovered, With>, diff --git a/rmf_site_editor/src/interaction/assets.rs b/rmf_site_editor/src/interaction/assets.rs index 32250766..c33b34f5 100644 --- a/rmf_site_editor/src/interaction/assets.rs +++ b/rmf_site_editor/src/interaction/assets.rs @@ -203,11 +203,14 @@ impl InteractionAssets { commands.entity(drag_parent).with_children(|parent| { for (polyline, material) in &self.centimeter_finite_grid { - parent.spawn(PolylineBundle { - polyline: polyline.clone(), - material: material.clone(), - ..default() - }); + parent.spawn(( + PolylineBundle { + polyline: polyline.clone(), + material: material.clone(), + ..default() + }, + DisableXray, + )); } }); diff --git a/rmf_site_editor/src/interaction/camera_controls/cursor.rs b/rmf_site_editor/src/interaction/camera_controls/cursor.rs index 792da545..0d398d1f 100644 --- a/rmf_site_editor/src/interaction/camera_controls/cursor.rs +++ b/rmf_site_editor/src/interaction/camera_controls/cursor.rs @@ -20,7 +20,7 @@ use super::{ CameraCommandType, CameraControls, ProjectionMode, MAX_FOV, MAX_SCALE, MIN_FOV, MIN_SCALE, }; use crate::interaction::SiteRaycastSet; -use bevy::input::mouse::MouseWheel; +use bevy::input::mouse::{MouseScrollUnit, MouseWheel}; use bevy::prelude::*; use bevy::window::PrimaryWindow; use bevy_mod_raycast::deferred::RaycastSource; @@ -92,15 +92,10 @@ pub fn update_cursor_command( // Scroll input let mut scroll_motion = 0.0; for ev in mouse_wheel.read() { - #[cfg(not(target_arch = "wasm32"))] - { - scroll_motion += ev.y; - } - #[cfg(target_arch = "wasm32")] - { - // scrolling in wasm is a different beast - scroll_motion += 0.4 * ev.y / ev.y.abs(); - } + scroll_motion += match ev.unit { + MouseScrollUnit::Line => ev.y, + MouseScrollUnit::Pixel => ev.y / 100.0, + }; } // Command type, return if inactive diff --git a/rmf_site_editor/src/interaction/camera_controls/keyboard.rs b/rmf_site_editor/src/interaction/camera_controls/keyboard.rs index 85c7bffe..0faa7417 100644 --- a/rmf_site_editor/src/interaction/camera_controls/keyboard.rs +++ b/rmf_site_editor/src/interaction/camera_controls/keyboard.rs @@ -187,7 +187,7 @@ pub fn update_keyboard_command( // Set camera selection as orbit center, discard once orbit operation complete let camera_selection = match keyboard_command.camera_selection { - Some(camera_selection) => camera_selection, + Some(camera_selection) => Some(camera_selection), None => get_camera_selected_point( &camera, &camera_global_transform, @@ -195,6 +195,12 @@ pub fn update_keyboard_command( immediate_raycast, ), }; + + let Some(camera_selection) = camera_selection else { + warn!("Point could not be calculated for camera"); + return; + }; + if command_type == CameraCommandType::Orbit { camera_controls.orbit_center = Some(camera_selection); } diff --git a/rmf_site_editor/src/interaction/camera_controls/utils.rs b/rmf_site_editor/src/interaction/camera_controls/utils.rs index 2d1db1ec..ea586c59 100644 --- a/rmf_site_editor/src/interaction/camera_controls/utils.rs +++ b/rmf_site_editor/src/interaction/camera_controls/utils.rs @@ -74,12 +74,11 @@ pub fn get_camera_selected_point( camera_global_transform: &GlobalTransform, user_camera_display: Res, mut immediate_raycast: Raycast, -) -> Vec3 { +) -> Option { // Assume that the camera spans the full window, covered by egui panels let available_viewport_center = user_camera_display.region.center(); - let camera_ray = camera - .viewport_to_world(camera_global_transform, available_viewport_center) - .expect("Active camera does not have a valid ray from center of its viewport"); + let camera_ray = + camera.viewport_to_world(camera_global_transform, available_viewport_center)?; let camera_ray = Ray3d::new(camera_ray.origin, camera_ray.direction); let raycast_setting = RaycastSettings::default() .always_early_exit() @@ -89,13 +88,13 @@ pub fn get_camera_selected_point( let intersections = immediate_raycast.cast_ray(camera_ray, &raycast_setting); if intersections.len() > 0 { let (_, intersection_data) = &intersections[0]; - return intersection_data.position(); + return Some(intersection_data.position()); } else { - return get_groundplane_else_default_selection( + return Some(get_groundplane_else_default_selection( camera_ray.origin(), camera_ray.direction(), camera_ray.direction(), - ); + )); } } diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs index 375845fe..0eab8c75 100644 --- a/rmf_site_editor/src/interaction/cursor.rs +++ b/rmf_site_editor/src/interaction/cursor.rs @@ -21,8 +21,9 @@ use crate::{ site::{AnchorBundle, Pending, SiteAssets, Trashcan}, }; use bevy::{ecs::system::SystemParam, prelude::*, window::PrimaryWindow}; -use bevy_mod_raycast::{deferred::RaycastMesh, deferred::RaycastSource, primitives::rays::Ray3d}; -use rmf_site_format::{FloorMarker, Model, ModelMarker, PrimitiveShape, WallMarker, WorkcellModel}; +use bevy_mod_raycast::primitives::{rays::Ray3d, Primitive3d}; + +use rmf_site_format::{FloorMarker, Model, WallMarker, WorkcellModel}; use std::collections::HashSet; /// A resource that keeps track of the unique entities that play a role in @@ -93,6 +94,14 @@ impl Cursor { } } + pub fn clear_blockers(&mut self, visibility: &mut Query<&mut Visibility>) { + let had_blockers = !self.blockers.is_empty(); + self.blockers.clear(); + if had_blockers { + self.toggle_visibility(visibility); + } + } + fn toggle_visibility(&mut self, visibility: &mut Query<&mut Visibility>) { if let Ok(mut v) = visibility.get_mut(self.frame) { let new_visible = if self.should_be_visible() { @@ -106,9 +115,12 @@ impl Cursor { } } - fn remove_preview(&mut self, commands: &mut Commands) { + pub fn remove_preview(&mut self, commands: &mut Commands) { if let Some(current_preview) = self.preview_model { - commands.entity(current_preview).set_parent(self.trashcan); + commands.get_entity(current_preview).map(|mut e_mut| { + e_mut.set_parent(self.trashcan); + }); + self.preview_model = None; } } @@ -283,7 +295,23 @@ pub struct IntersectGroundPlaneParams<'w, 's> { } impl<'w, 's> IntersectGroundPlaneParams<'w, 's> { - pub fn ground_plane_intersection(&self) -> Option { + pub fn ground_plane_intersection(&self) -> Option { + let ground_plane = Primitive3d::Plane { + point: Vec3::ZERO, + normal: Vec3::Z, + }; + self.primitive_intersection(ground_plane) + } + + pub fn frame_plane_intersection(&self, frame: Entity) -> Option { + let tf = self.global_transforms.get(frame).ok()?; + let affine = tf.affine(); + let point = affine.translation.into(); + let normal = affine.matrix3.col(2).into(); + self.primitive_intersection(Primitive3d::Plane { point, normal }) + } + + pub fn primitive_intersection(&self, primitive: Primitive3d) -> Option { let window = self.primary_windows.get_single().ok()?; let cursor_position = window.cursor_position()?; let e_active_camera = self.camera_controls.active_camera(); @@ -292,144 +320,19 @@ impl<'w, 's> IntersectGroundPlaneParams<'w, 's> { let primary_window = self.primary_window.get_single().ok()?; let ray = Ray3d::from_screenspace(cursor_position, active_camera, camera_tf, primary_window)?; - let n_p = Vec3::Z; - let n_r = ray.direction(); - let denom = n_p.dot(n_r); - if denom.abs() < 1e-3 { - // Too close to parallel - return None; - } - - Some(ray.origin() - n_r * ray.origin().dot(n_p) / denom) - } -} - -pub fn update_cursor_transform( - mode: Res, - cursor: Res, - raycast_sources: Query<&RaycastSource>, - models: Query<(), Or<(With, With)>>, - mut transforms: Query<&mut Transform>, - hovering: Res, - intersect_ground_params: IntersectGroundPlaneParams, - mut visibility: Query<&mut Visibility>, -) { - match &*mode { - InteractionMode::Inspect => { - // TODO(luca) this will not work if more than one raycast source exist - let Ok(source) = raycast_sources.get_single() else { - return; - }; - let intersection = match source.get_nearest_intersection() { - Some((_, intersection)) => intersection, - None => { - return; - } - }; - - let mut transform = match transforms.get_mut(cursor.frame) { - Ok(transform) => transform, - Err(_) => { - return; - } - }; - - let ray = Ray3d::new(intersection.position(), intersection.normal()); - *transform = Transform::from_matrix(ray.to_aligned_transform([0., 0., 1.].into())); - } - InteractionMode::SelectAnchor(_) => { - let intersection = match intersect_ground_params.ground_plane_intersection() { - Some(intersection) => intersection, - None => { - return; - } - }; - - let mut transform = match transforms.get_mut(cursor.frame) { - Ok(transform) => transform, - Err(_) => { - return; - } - }; - - *transform = Transform::from_translation(intersection); - } - // TODO(luca) snap to features of meshes - InteractionMode::SelectAnchor3D(_mode) => { - let mut transform = match transforms.get_mut(cursor.frame) { - Ok(transform) => transform, - Err(_) => { - error!("No cursor transform found"); - return; - } - }; - - let Ok(source) = raycast_sources.get_single() else { - return; - }; - - // Check if there is an intersection to a mesh, if there isn't fallback to ground plane - if let Some((_, intersection)) = source.get_nearest_intersection() { - let Some(triangle) = intersection.triangle() else { - return; - }; - // Make sure we are hovering over a model and not anything else (i.e. anchor) - match cursor.preview_model { - None => { - if hovering.0.and_then(|e| models.get(e).ok()).is_some() { - // Find the closest triangle vertex - // TODO(luca) Also snap to edges of triangles or just disable altogether and snap - // to area, then populate a MeshConstraint component to be used by downstream - // spawning methods - // TODO(luca) there must be a better way to find a minimum given predicate in Rust - let triangle_vecs = vec![triangle.v1, triangle.v2]; - let position = intersection.position(); - let mut closest_vertex = triangle.v0; - let mut closest_dist = position.distance(triangle.v0.into()); - for v in triangle_vecs { - let dist = position.distance(v.into()); - if dist < closest_dist { - closest_dist = dist; - closest_vertex = v; - } - } - //closest_vertex = *triangle_vecs.iter().min_by(|position, ver| position.distance(**ver).cmp(closest_dist)).unwrap(); - let ray = Ray3d::new(closest_vertex.into(), intersection.normal()); - *transform = Transform::from_matrix( - ray.to_aligned_transform([0., 0., 1.].into()), - ); - set_visibility(cursor.frame, &mut visibility, true); - } else { - // Hide the cursor - set_visibility(cursor.frame, &mut visibility, false); - } - } - Some(_) => { - // If we are placing a model avoid snapping to faced and just project to - // ground plane - let intersection = match intersect_ground_params.ground_plane_intersection() - { - Some(intersection) => intersection, - None => { - return; - } - }; - set_visibility(cursor.frame, &mut visibility, true); - *transform = Transform::from_translation(intersection); - } - } - } else { - let intersection = match intersect_ground_params.ground_plane_intersection() { - Some(intersection) => intersection, - None => { - return; - } - }; - set_visibility(cursor.frame, &mut visibility, true); - *transform = Transform::from_translation(intersection); + let n = *match &primitive { + Primitive3d::Plane { normal, .. } => normal, + _ => { + warn!("Unsupported primitive type found"); + return None; } - } + }; + let p = ray + .intersects_primitive(primitive) + .map(|intersection| intersection.position())?; + + Some(Transform::from_translation(p).with_rotation(aligned_z_axis(n))) } } @@ -471,16 +374,19 @@ pub fn update_cursor_hover_visualization( } } -// This system makes sure model previews are not picked up by raycasting -pub fn make_model_previews_not_selectable( - mut commands: Commands, - new_models: Query, Added)>, - cursor: Res, -) { - if let Some(e) = cursor.preview_model.and_then(|m| new_models.get(m).ok()) { - commands - .entity(e) - .remove::() - .remove::>(); +pub fn aligned_z_axis(z: Vec3) -> Quat { + let z_length = z.length(); + if z_length < 1e-8 { + // The given direction is too close to singular + return Quat::IDENTITY; + } + + let axis = Vec3::Z.cross(z); + let axis_length = axis.length(); + if axis_length < 1e-8 { + // The change in angle is too close to zero + return Quat::IDENTITY; } + let angle = f32::asin(axis_length / z_length); + Quat::from_axis_angle(axis / axis_length, angle) } diff --git a/rmf_site_editor/src/interaction/gizmo.rs b/rmf_site_editor/src/interaction/gizmo.rs index 196dbaa3..dc537d7f 100644 --- a/rmf_site_editor/src/interaction/gizmo.rs +++ b/rmf_site_editor/src/interaction/gizmo.rs @@ -34,6 +34,23 @@ pub struct GizmoMaterialSet { pub drag: Handle, } +#[derive(Resource)] +pub struct GizmoBlockers { + pub selecting: bool, +} + +impl GizmoBlockers { + pub fn blocking(&self) -> bool { + self.selecting + } +} + +impl Default for GizmoBlockers { + fn default() -> Self { + Self { selecting: false } + } +} + impl GizmoMaterialSet { pub fn make_x_axis(materials: &mut Mut>) -> Self { Self { @@ -134,6 +151,7 @@ pub struct DragAxisBundle { pub gizmo: Gizmo, pub draggable: Draggable, pub axis: DragAxis, + pub selectable: Selectable, } impl DragAxisBundle { @@ -145,6 +163,10 @@ impl DragAxisBundle { along, frame: FrameOfReference::Local, }, + selectable: Selectable { + is_selectable: true, + element: for_entity, + }, } } @@ -172,6 +194,7 @@ pub struct DragPlaneBundle { pub gizmo: Gizmo, pub draggable: Draggable, pub plane: DragPlane, + pub selectable: Selectable, } impl DragPlaneBundle { @@ -183,6 +206,10 @@ impl DragPlaneBundle { in_plane, frame: FrameOfReference::Local, }, + selectable: Selectable { + is_selectable: true, + element: for_entity, + }, } } @@ -240,6 +267,7 @@ pub fn update_gizmo_click_start( &mut Handle, )>, mut selection_blocker: ResMut, + gizmo_blocker: Res, mut visibility: Query<&mut Visibility>, mouse_button_input: Res>, transforms: Query<(&Transform, &GlobalTransform)>, @@ -250,6 +278,16 @@ pub fn update_gizmo_click_start( mut click: EventWriter, mut removed_gizmos: RemovedComponents, ) { + if gizmo_blocker.blocking() { + if gizmo_blocker.is_changed() { + // This has started being blocked since the last cycle + cursor.clear_blockers(&mut visibility); + } + + // Don't start any gizmos + return; + } + for e in removed_gizmos.read() { cursor.remove_blocker(e, &mut visibility); } @@ -325,12 +363,14 @@ pub fn update_gizmo_click_start( pub fn update_gizmo_release( mut draggables: Query<(&Gizmo, &mut Draggable, &mut Handle)>, mut selection_blockers: ResMut, + gizmo_blockers: Res, mut gizmo_state: ResMut, mouse_button_input: Res>, - picked: Res, - mut change_pick: EventWriter, + mut picked: ResMut, ) { - if mouse_button_input.just_released(MouseButton::Left) { + let mouse_released = mouse_button_input.just_released(MouseButton::Left); + let gizmos_blocked = gizmo_blockers.blocking(); + if mouse_released || gizmos_blocked { if let GizmoState::Dragging(e) = *gizmo_state { if let Ok((gizmo, mut draggable, mut material)) = draggables.get_mut(e) { draggable.drag = None; @@ -346,10 +386,7 @@ pub fn update_gizmo_release( // to move the cursor off of whatever object it happens to be // hovering over after the drag is finished before interactions like // selecting or dragging can resume. - change_pick.send(ChangePick { - from: None, - to: picked.0, - }); + picked.refresh = true; } } } diff --git a/rmf_site_editor/src/interaction/lift.rs b/rmf_site_editor/src/interaction/lift.rs index efbf9457..0262ccc9 100644 --- a/rmf_site_editor/src/interaction/lift.rs +++ b/rmf_site_editor/src/interaction/lift.rs @@ -57,7 +57,7 @@ pub fn handle_lift_doormat_clicks( for click in clicks.read() { if let Ok(doormat) = doormats.get(click.0) { toggle.send(doormat.toggle_availability()); - select.send(Select(Some(doormat.for_lift))); + select.send(Select::new(Some(doormat.for_lift))); } } } diff --git a/rmf_site_editor/src/interaction/light.rs b/rmf_site_editor/src/interaction/light.rs index 9e7dc9d0..c978189f 100644 --- a/rmf_site_editor/src/interaction/light.rs +++ b/rmf_site_editor/src/interaction/light.rs @@ -16,7 +16,7 @@ */ use crate::{ - interaction::{DragPlaneBundle, HeadlightToggle, InteractionAssets, Selectable}, + interaction::{DragPlaneBundle, HeadlightToggle, InteractionAssets}, site::LightKind, }; use bevy::prelude::*; @@ -94,7 +94,6 @@ pub fn add_physical_light_visual_cues( material: assets.physical_light_cover_material.clone(), ..default() }) - .insert(Selectable::new(e)) .insert(DragPlaneBundle::new(e, Vec3::Z).globally()); point @@ -103,7 +102,6 @@ pub fn add_physical_light_visual_cues( material: light_material.clone(), ..default() }) - .insert(Selectable::new(e)) .insert(DragPlaneBundle::new(e, Vec3::Z).globally()); }) .id(); @@ -123,7 +121,6 @@ pub fn add_physical_light_visual_cues( material: assets.physical_light_cover_material.clone(), ..default() }) - .insert(Selectable::new(e)) .insert(DragPlaneBundle::new(e, Vec3::Z).globally()); spot.spawn(PbrBundle { @@ -131,7 +128,6 @@ pub fn add_physical_light_visual_cues( material: light_material.clone(), ..default() }) - .insert(Selectable::new(e)) .insert(DragPlaneBundle::new(e, Vec3::Z).globally()); }) .id(); @@ -151,7 +147,6 @@ pub fn add_physical_light_visual_cues( material: assets.direction_light_cover_material.clone(), ..default() }) - .insert(Selectable::new(e)) .insert(DragPlaneBundle::new(e, Vec3::Z).globally()); dir.spawn(PbrBundle { @@ -159,7 +154,6 @@ pub fn add_physical_light_visual_cues( material: light_material.clone(), ..default() }) - .insert(Selectable::new(e)) .insert(DragPlaneBundle::new(e, Vec3::Z).globally()); }) .id(); diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs index 16290689..2a57893b 100644 --- a/rmf_site_editor/src/interaction/mod.rs +++ b/rmf_site_editor/src/interaction/mod.rs @@ -55,9 +55,6 @@ pub use lift::*; pub mod light; pub use light::*; -pub mod mode; -pub use mode::*; - pub mod model_preview; pub use model_preview::*; @@ -82,9 +79,6 @@ pub use preview::*; pub mod select; pub use select::*; -pub mod select_anchor; -pub use select_anchor::*; - pub mod visual_cue; pub use visual_cue::*; @@ -134,8 +128,9 @@ pub enum InteractionUpdateSet { impl Plugin for InteractionPlugin { fn build(&self, app: &mut App) { app.add_state::() + .init_resource::() .configure_sets( - Update, + PostUpdate, ( SiteUpdateSet::AssignOrphansFlush, InteractionUpdateSet::AddVisuals, @@ -155,17 +150,10 @@ impl Plugin for InteractionPlugin { .init_resource::() .init_resource::() .init_resource::() - .init_resource::() - .init_resource::() - .init_resource::() .init_resource::() - .init_resource::() - .init_resource::() + .insert_resource(HighlightAnchors(false)) .add_event::() - .add_event::() + .add_event::() + .add_event::() + .add_systems( + Update, + ( + (apply_deferred, flush_impulses()) + .chain() + .in_set(SelectionServiceStages::PickFlush), + (apply_deferred, flush_impulses()) + .chain() + .in_set(SelectionServiceStages::HoverFlush), + (apply_deferred, flush_impulses()) + .chain() + .in_set(SelectionServiceStages::SelectFlush), + ), + ) + .add_plugins(( + InspectorServicePlugin::default(), + AnchorSelectionPlugin::default(), + ObjectPlacementPlugin::default(), + )); + + let inspector_service = app.world.resource::().inspector_service; + let new_selector_service = app.spawn_event_streaming_service::(Update); + let selection_workflow = app.world.spawn_io_workflow(build_selection_workflow( + inspector_service, + new_selector_service, + )); + + // Get the selection workflow running + app.world.command(|commands| { + commands.request((), selection_workflow).detach(); + }); + } +} + +/// This builder function creates the high-level workflow that manages "selection" +/// behavior, which is largely driven by mouse interactions. "Selection" behaviors +/// determine how the application responds to the mouse cursor hovering over +/// objects in the scene and what happens when the mouse is clicked. +/// +/// The default selection behavior is the "inspector" service which allows the +/// user to select objects in the scene so that their properties get displayed +/// in the inspector panel. The inspector service will automatically be run by +/// this workflow at startup. +/// +/// When the user asks for some other type of mouse interaction to begin, such as +/// drawing walls and floors, or placing models in the scene, this workflow will +/// "trim" (stop) the inspector workflow and inject the new requested interaction +/// mode into the workflow. The requested interaction mode is represented by a +/// service specified by the `selector` field of [`RunSelector`]. When that +/// service terminates, this workflow will resume running the inspector service +/// until the user requests some other mouse interaction service to run. +/// +/// In most cases downstream users will not need to call this function since the +/// [`SelectionPlugin`] will use this to build and run the default selection +/// workflow. If you are not using the [`SelectionPlugin`] that we provide and +/// want to customize the inspector service, then you could use this to build a +/// customized selection workflow by passingin a custom inspector service. +pub fn build_selection_workflow( + inspector_service: Service<(), ()>, + new_selector_service: Service<(), (), StreamOf>, +) -> impl FnOnce(Scope<(), ()>, &mut Builder) -> DeliverySettings { + move |scope, builder| { + // This creates a service that will listen to run_service_buffer. + // The job of this service is to atomically pull the most recent item + // out of the buffer and also close the buffer gate to ensure that we + // never have multiple selector services racing to be injected. If we + // don't bother to close the gate after pulling exactly one selection + // service, then it's theoretically possible for multiple selection + // services to get simultaneously injected after the trim operation + // finishes. + let process_new_selector_service = builder + .commands() + .spawn_service(process_new_selector.into_blocking_service()); + + // The run_service_buffer queues up the most recent RunSelector request + // sent in by the user. That request will be held in this buffer while + // we wait for any ongoing mouse interaction services to cleanly exit + // after we trigger the trim operation. + let run_service_buffer = builder.create_buffer::(BufferSettings::keep_last(1)); + let input = scope.input.fork_clone(builder); + // Run the default inspector service + let inspector = input.clone_chain(builder).then_node(inspector_service); + + // Create a node that reads RunSelector events from the world and streams + // them into the workflow. + let new_selector_node = input.clone_chain(builder).then_node(new_selector_service); + builder.connect(new_selector_node.output, scope.terminate); + new_selector_node + .streams + .chain(builder) + .inner() + .connect(run_service_buffer.input_slot()); + + let open_gate = builder.create_gate_open(run_service_buffer); + // Create an operation that trims the gate opening operation, the injected + // selector service, and the default inspector service. + let trim = builder.create_trim([TrimBranch::between(open_gate.input, inspector.input)]); + builder.connect(trim.output, open_gate.input); + + // Create a sequence where we listen for updates in the run service buffer, + // then pull an item out of the buffer (if available), then begin the + // trim of all ongoing selection services, then opening the gate of the + // buffer to allow new selection services to be started. + builder + .listen(run_service_buffer) + .then(process_new_selector_service) + .dispose_on_none() + .connect(trim.input); + + // After we open the gate it is safe to inject the user-requested selecion + // service. Once that service finishes, we will trigger the inspector to + // resume. + open_gate + .output + .chain(builder) + .map_block(|r: RunSelector| (r.input, r.selector)) + .then_injection() + .trigger() + .connect(inspector.input); + + // This workflow only makes sense to run in serial. + DeliverySettings::Serial + } +} + +fn process_new_selector( + In(key): In>, + mut access: BufferAccessMut, +) -> Option { + let Ok(mut buffer) = access.get_mut(&key) else { + return None; + }; + + let output = buffer.pull(); + if output.is_some() { + // We should lock the gate while the trim is going on so we can't have + // multiple new selectors trying to start at the same time + buffer.close_gate(); + } + + output +} + +#[derive(Debug, Clone, Copy, Event)] +pub struct RunSelector { + /// The select workflow will run this service until it terminates and then + /// revert back to the inspector selector. + selector: Service, ()>, + /// If there is input for the selector, it will be stored in a [`SelectorInput`] + /// component in this entity. The entity will be despawned as soon as the + /// input is extracted. + input: Option, +} + +#[derive(Component)] +pub struct SelectorInput(T); /// This component is put on entities with meshes to mark them as items that can /// be interacted with to @@ -97,11 +326,46 @@ pub struct Selection(pub Option); pub struct Hovering(pub Option); /// Used as an event to command a change in the selected entity. -#[derive(Default, Debug, Clone, Copy, Deref, DerefMut, Event)] -pub struct Select(pub Option); +#[derive(Default, Debug, Clone, Copy, Deref, DerefMut, Event, Stream)] +pub struct Select(pub Option); + +impl Select { + pub fn new(candidate: Option) -> Select { + Select(candidate.map(|c| SelectionCandidate::new(c))) + } + + pub fn provisional(candidate: Entity) -> Select { + Select(Some(SelectionCandidate::provisional(candidate))) + } +} + +#[derive(Debug, Clone, Copy)] +pub struct SelectionCandidate { + /// The entity that's being requested as a selection + pub candidate: Entity, + /// The entity was created specifically to be selected, so if it ends up + /// going unused by the workflow then it should be despawned. + pub provisional: bool, +} + +impl SelectionCandidate { + pub fn new(candidate: Entity) -> SelectionCandidate { + SelectionCandidate { + candidate, + provisional: false, + } + } + + pub fn provisional(candidate: Entity) -> SelectionCandidate { + SelectionCandidate { + candidate, + provisional: true, + } + } +} /// Used as an event to command a change in the hovered entity. -#[derive(Default, Debug, Clone, Copy, Deref, DerefMut, Event)] +#[derive(Default, Debug, Clone, Copy, Deref, DerefMut, Event, Stream)] pub struct Hover(pub Option); /// A resource to track what kind of blockers are preventing the selection @@ -110,22 +374,17 @@ pub struct Hover(pub Option); pub struct SelectionBlockers { /// An entity is being dragged pub dragging: bool, - /// An entity is being placed - pub placing: bool, } impl SelectionBlockers { pub fn blocking(&self) -> bool { - self.dragging || self.placing + self.dragging } } impl Default for SelectionBlockers { fn default() -> Self { - SelectionBlockers { - dragging: false, - placing: false, - } + SelectionBlockers { dragging: false } } } @@ -155,75 +414,372 @@ pub fn make_selectable_entities_pickable( } } -pub fn handle_selection_picking( - blockers: Option>, - mode: Res, - selectables: Query<&Selectable>, - anchors: Query<(), With>, +/// This allows an [`App`] to spawn a service that can stream Hover and +/// Select events that are managed by a filter. This can only be used with +/// [`App`] because some of the internal services are continuous, so they need +/// to be added to the schedule. +pub trait SpawnSelectionServiceExt { + fn spawn_selection_service( + &mut self, + ) -> Service<(), (), (Hover, Select)> + where + for<'w, 's> F::Item<'w, 's>: SelectionFilter; +} + +impl SpawnSelectionServiceExt for App { + fn spawn_selection_service( + &mut self, + ) -> Service<(), (), (Hover, Select)> + where + for<'w, 's> F::Item<'w, 's>: SelectionFilter, + { + let picking_service = self.spawn_continuous_service( + Update, + picking_service:: + .configure(|config: SystemConfigs| config.in_set(SelectionServiceStages::Pick)), + ); + + let hover_service = self.spawn_continuous_service( + Update, + hover_service:: + .configure(|config: SystemConfigs| config.in_set(SelectionServiceStages::Hover)), + ); + + let select_service = self.spawn_continuous_service( + Update, + select_service:: + .configure(|config: SystemConfigs| config.in_set(SelectionServiceStages::Select)), + ); + + self.world + .spawn_workflow::<_, _, (Hover, Select), _>(|scope, builder| { + let hover = builder.create_node(hover_service); + builder.connect(hover.streams, scope.streams.0); + builder.connect(hover.output, scope.terminate); + + let select = builder.create_node(select_service); + builder.connect(select.streams, scope.streams.1); + builder.connect(select.output, scope.terminate); + + // Activate all the services at the start + scope.input.chain(builder).fork_clone(( + |chain: Chain<_>| { + chain + .then(refresh_picked.into_blocking_callback()) + .then(picking_service) + .connect(scope.terminate) + }, + |chain: Chain<_>| chain.connect(hover.input), + |chain: Chain<_>| chain.connect(select.input), + )); + + // This is just a dummy buffer to let us have a cleanup workflow + let buffer = builder.create_buffer::<()>(BufferSettings::keep_all()); + builder.on_cleanup(buffer, |scope, builder| { + scope + .input + .chain(builder) + .trigger() + .then(clear_hover_select.into_blocking_callback()) + .connect(scope.terminate); + }); + }) + } +} + +// TODO(@mxgrey): Remove flush stages when we move to bevy 0.13 which can infer +// when to flush +#[derive(Debug, Hash, PartialEq, Eq, Clone, SystemSet)] +pub enum SelectionServiceStages { + Pick, + PickFlush, + Hover, + HoverFlush, + Select, + SelectFlush, +} + +#[derive(Resource)] +pub struct InspectorService { + /// Workflow that updates the [`Selection`] as well as [`Hovered`] and + /// [`Selected`] states in the application. + pub inspector_service: Service<(), ()>, + /// Workflow that outputs hover and select streams that are compatible with + /// a general inspector. This service never terminates. + pub inspector_select_service: Service<(), (), (Hover, Select)>, + pub inspector_cursor_transform: Service<(), ()>, + pub selection_update: Service, +} + +#[derive(Default)] +pub struct InspectorServicePlugin {} + +impl Plugin for InspectorServicePlugin { + fn build(&self, app: &mut App) { + let inspector_select_service = app.spawn_selection_service::(); + let inspector_cursor_transform = app.spawn_continuous_service( + Update, + inspector_cursor_transform + .configure(|config: SystemConfigs| config.in_set(SelectionServiceStages::Pick)), + ); + let selection_update = app.spawn_service(selection_update); + let keyboard_just_pressed = app + .world + .resource::() + .keyboard_just_pressed; + + let inspector_service = app.world.spawn_workflow(|scope, builder| { + let fork_input = scope.input.fork_clone(builder); + fork_input + .clone_chain(builder) + .then(inspector_cursor_transform) + .unused(); + fork_input + .clone_chain(builder) + .then_node(keyboard_just_pressed) + .streams + .chain(builder) + .inner() + .then(deselect_on_esc.into_blocking_callback()) + .unused(); + let selection = fork_input + .clone_chain(builder) + .then_node(inspector_select_service); + selection + .streams + .1 + .chain(builder) + .then(selection_update) + .unused(); + builder.connect(selection.output, scope.terminate); + }); + + app.world.insert_resource(InspectorService { + inspector_service, + inspector_select_service, + inspector_cursor_transform, + selection_update, + }); + } +} + +pub fn deselect_on_esc(In(code): In, mut select: EventWriter; +} + +#[derive(SystemParam)] +pub struct InspectorFilter<'w, 's> { + selectables: Query<'w, 's, &'static Selectable, (Without, Without)>, +} + +impl<'w, 's> SelectionFilter for InspectorFilter<'w, 's> { + fn filter_pick(&mut self, select: Entity) -> Option { + self.selectables + .get(select) + .ok() + .map(|selectable| selectable.element) + } + fn filter_select(&mut self, target: Entity) -> Option { + Some(target) + } + fn on_click(&mut self, hovered: Hover) -> Option, - mode: Res, blockers: Option>, -) { + filter: StaticSystemParam, + selection_blockers: Res, +) where + for<'w, 's> Filter::Item<'w, 's>: SelectionFilter, +{ + let Some(mut orders) = orders.get_mut(&key) else { + return; + }; + + if orders.is_empty() { + // Nothing is asking for this service to run + return; + } + + if selection_blockers.blocking() { + return; + } + + let mut filter = filter.into_inner(); + if let Some(new_hovered) = hover.read().last() { - if hovering.0 != new_hovered.0 { + let new_hovered = new_hovered.0.and_then(|e| filter.filter_select(e)); + if hovering.0 != new_hovered { if let Some(previous_hovered) = hovering.0 { if let Ok(mut hovering) = hovered.get_mut(previous_hovered) { hovering.is_hovered = false; } } - if let Some(new_hovered) = new_hovered.0 { + if let Some(new_hovered) = new_hovered { if let Ok(mut hovering) = hovered.get_mut(new_hovered) { hovering.is_hovered = true; } } - hovering.0 = new_hovered.0; + hovering.0 = new_hovered; + orders.for_each(|order| order.streams().send(Hover(new_hovered))); } } @@ -232,46 +788,148 @@ pub fn maintain_hovered_entities( let blocked = blockers.filter(|x| x.blocking()).is_some(); if clicked && !blocked { - if let Some(current_hovered) = hovering.0 { - // TODO(luca) refactor to remove this hack - // Skip if we are in SelectAnchor3D mode - if let InteractionMode::SelectAnchor3D(_) = &*mode { - return; - } - select.send(Select(Some(current_hovered))); + if let Some(new_select) = filter.on_click(Hover(hovering.0)) { + select.send(new_select); } } } -pub fn maintain_selected_entities( - mode: Res, - mut selected: Query<&mut Selected>, - mut selection: ResMut, +/// A continuous service that filters [`Select`] events and issues out a +/// [`Hover`] stream. +/// +/// This complements [`hover_service`] and [`hover_picking`] +/// and is the final piece of the [`SelectionService`] workflow. +pub fn select_service( + In(ContinuousService { key }): ContinuousServiceInput<(), (), Select>, + mut orders: ContinuousQuery<(), (), Select>, mut select: EventReader, + mut selected: Query<&mut Selected>, + mut selection: ResMut, +) { + if selection.0 != new_selection.map(|s| s.candidate) { + if let Some(previous_selection) = selection.0 { + if let Ok(mut selected) = selected.get_mut(previous_selection) { + selected.is_selected = false; } + } - selection.0 = new_selection.0; + if let Some(new_selection) = new_selection { + if let Ok(mut selected) = selected.get_mut(new_selection.candidate) { + selected.is_selected = true; + } } + + selection.0 = new_selection.map(|s| s.candidate); } } + +/// This is used to clear out the currently picked item at the start of a new +/// selection workflow to make sure the Hover events don't get lost during the +/// workflow switch. +pub fn refresh_picked(In(_): In<()>, mut picked: ResMut) { + picked.refresh = true; +} + +/// This is used to clear out hoverings and selections from a workflow that is +/// cleaning up so that these properties don't spill over into other workflows. +pub fn clear_hover_select( + In(_): In<()>, + mut hovered: Query<&mut Hovered>, + mut hovering: ResMut, + mut selected: Query<&mut Selected>, + mut selection: ResMut, +) { + if let Some(previous_hovering) = hovering.0.take() { + if let Ok(mut hovered) = hovered.get_mut(previous_hovering) { + hovered.is_hovered = false; + } + } + + if let Some(previous_selection) = selection.0.take() { + if let Ok(mut selected) = selected.get_mut(previous_selection) { + selected.is_selected = false; + } + } +} + +/// Update the virtual cursor (dagger and circle) transform while in inspector mode +pub fn inspector_cursor_transform( + In(ContinuousService { key }): ContinuousServiceInput<(), ()>, + orders: ContinuousQuery<(), ()>, + cursor: Res, + raycast_sources: Query<&RaycastSource>, + mut transforms: Query<&mut Transform>, +) { + let Some(orders) = orders.view(&key) else { + return; + }; + + if orders.is_empty() { + return; + } + + let Ok(source) = raycast_sources.get_single() else { + return; + }; + let intersection = match source.get_nearest_intersection() { + Some((_, intersection)) => intersection, + None => { + return; + } + }; + + let mut transform = match transforms.get_mut(cursor.frame) { + Ok(transform) => transform, + Err(_) => { + return; + } + }; + + let ray = Ray3d::new(intersection.position(), intersection.normal()); + *transform = Transform::from_matrix(ray.to_aligned_transform([0., 0., 1.].into())); +} diff --git a/rmf_site_editor/src/interaction/select/create_edges.rs b/rmf_site_editor/src/interaction/select/create_edges.rs new file mode 100644 index 00000000..aa75a250 --- /dev/null +++ b/rmf_site_editor/src/interaction/select/create_edges.rs @@ -0,0 +1,405 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{ + interaction::*, + site::{ChangeDependent, Pending, TextureNeedsAssignment}, +}; +use bevy::prelude::*; +use bevy_impulse::*; +use rmf_site_format::{Edge, Side}; +use std::borrow::Borrow; + +pub fn spawn_create_edges_service( + helpers: &AnchorSelectionHelpers, + app: &mut App, +) -> Service, ()> { + let anchor_setup = + app.spawn_service(anchor_selection_setup::.into_blocking_service()); + let state_setup = app.spawn_service(create_edges_setup.into_blocking_service()); + let update_preview = app.spawn_service(on_hover_for_create_edges.into_blocking_service()); + let update_current = app.spawn_service(on_select_for_create_edges.into_blocking_service()); + let handle_key_code = app.spawn_service(on_keyboard_for_create_edges.into_blocking_service()); + let cleanup_state = app.spawn_service(cleanup_create_edges.into_blocking_service()); + + helpers.spawn_anchor_selection_workflow( + anchor_setup, + state_setup, + update_preview, + update_current, + handle_key_code, + cleanup_state, + &mut app.world, + ) +} + +pub struct CreateEdges { + pub spawn_edge: fn(Edge, &mut Commands) -> Entity, + pub preview_edge: Option, + pub continuity: EdgeContinuity, + pub scope: AnchorScope, +} + +impl CreateEdges { + pub fn new>>( + continuity: EdgeContinuity, + scope: AnchorScope, + ) -> Self { + Self { + spawn_edge: create_edge::, + preview_edge: None, + continuity, + scope, + } + } + + pub fn new_with_texture>>( + continuity: EdgeContinuity, + scope: AnchorScope, + ) -> Self { + Self { + spawn_edge: create_edge_with_texture::, + preview_edge: None, + continuity, + scope, + } + } + + pub fn initialize_preview(&mut self, anchor: Entity, commands: &mut Commands) { + let edge = Edge::new(anchor, anchor); + let edge = (self.spawn_edge)(edge, commands); + self.preview_edge = Some(PreviewEdge { + edge, + side: Side::start(), + provisional_start: false, + }); + + commands.add(ChangeDependent::add(anchor, edge)); + } +} + +impl Borrow for CreateEdges { + fn borrow(&self) -> &AnchorScope { + &self.scope + } +} + +fn create_edge>>( + edge: Edge, + commands: &mut Commands, +) -> Entity { + let new_bundle: T = edge.into(); + commands.spawn((new_bundle, Pending)).id() +} + +fn create_edge_with_texture>>( + edge: Edge, + commands: &mut Commands, +) -> Entity { + let new_bundle: T = edge.into(); + commands + .spawn((new_bundle, TextureNeedsAssignment, Pending)) + .id() +} + +#[derive(Clone, Copy)] +pub struct PreviewEdge { + pub edge: Entity, + pub side: Side, + /// True if the start anchor of the edge was created specifically to build + /// this edge. If this true, we will despawn the anchor during cleanup if + /// the edge does not get completed. + pub provisional_start: bool, +} + +impl PreviewEdge { + pub fn cleanup( + &self, + edges: &Query<&'static Edge>, + commands: &mut Commands, + ) -> SelectionNodeResult { + let edge = edges.get(self.edge).or_broken_query()?; + for anchor in edge.array() { + commands.add(ChangeDependent::remove(anchor, self.edge)); + } + + if self.provisional_start { + // The start anchor was created specifically for this preview edge + // which we are about to despawn. Let's despawn both so we aren't + // littering the scene with unintended anchors. + commands + .get_entity(edge.start()) + .or_broken_query()? + .despawn_recursive(); + } + + commands + .get_entity(self.edge) + .or_broken_query()? + .despawn_recursive(); + Ok(()) + } +} + +pub enum EdgeContinuity { + /// Create just a single edge + Single, + /// Create a sequence of separate edges + Separate, + /// Create edges continuously, i.e. the beginning of the next edge will + /// automatically be the end of the previous edge. + Continuous, +} + +pub fn create_edges_setup( + In(key): In>, + mut access: BufferAccessMut, + cursor: Res, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + if state.preview_edge.is_none() { + state.initialize_preview(cursor.level_anchor_placement, &mut commands); + } + Ok(()) +} + +pub fn on_hover_for_create_edges( + In((hover, key)): In<(Hover, BufferKey)>, + mut access: BufferAccessMut, + mut cursor: ResMut, + mut visibility: Query<&mut Visibility>, + mut edges: Query<&mut Edge>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + // TODO(@mxgrey): Consider moving this logic into AnchorFilter since it gets + // used by all the different anchor selection modes. + let anchor = match hover.0 { + Some(anchor) => { + cursor.remove_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + anchor + } + None => { + cursor.add_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + cursor.level_anchor_placement + } + }; + + if let Some(preview) = &mut state.preview_edge { + // If we already have an active preview, then use the new anchor for the + // side that we currently need to select for. + let index = preview.side.index(); + let mut edge = edges.get_mut(preview.edge).or_broken_query()?; + + let old_anchor = edge.array()[index]; + if old_anchor != anchor { + let opposite_anchor = edge.array()[preview.side.opposite().index()]; + if opposite_anchor != old_anchor { + commands.add(ChangeDependent::remove(old_anchor, preview.edge)); + } + + edge.array_mut()[index] = anchor; + commands.add(ChangeDependent::add(anchor, preview.edge)); + } + } else { + // There is currently no active preview, so we need to create one. + let edge = Edge::new(anchor, anchor); + let edge = (state.spawn_edge)(edge, &mut commands); + state.preview_edge = Some(PreviewEdge { + edge, + side: Side::start(), + provisional_start: false, + }); + commands.add(ChangeDependent::add(anchor, edge)); + } + + Ok(()) +} + +pub fn on_select_for_create_edges( + In((selection, key)): In<(SelectionCandidate, BufferKey)>, + mut access: BufferAccessMut, + mut edges: Query<&mut Edge>, + mut commands: Commands, + cursor: Res, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + let anchor = selection.candidate; + if let Some(preview) = &mut state.preview_edge { + match preview.side { + Side::Left => { + // We are pinning down the first anchor of the edge + let mut edge = edges.get_mut(preview.edge).or_broken_query()?; + commands.add(ChangeDependent::remove(edge.left(), preview.edge)); + *edge.left_mut() = anchor; + commands.add(ChangeDependent::add(anchor, preview.edge)); + + if edge.right() != anchor { + commands.add(ChangeDependent::remove(edge.right(), preview.edge)); + } + + *edge.right_mut() = cursor.level_anchor_placement; + commands.add(ChangeDependent::add( + cursor.level_anchor_placement, + preview.edge, + )); + + preview.side = Side::Right; + preview.provisional_start = selection.provisional; + } + Side::Right => { + // We are finishing the edge + let mut edge = edges.get_mut(preview.edge).or_broken_query()?; + if edge.left() == anchor { + // The user is trying to use the same point for the start + // and end of an edge. Issue a warning and exit early. + warn!( + "You are trying to select an anchor {:?} for both the \ + start and end points of an edge, which is not allowed.", + anchor, + ); + return Ok(()); + } + *edge.right_mut() = anchor; + commands.add(ChangeDependent::add(anchor, preview.edge)); + commands + .get_entity(preview.edge) + .or_broken_query()? + .remove::(); + + match state.continuity { + EdgeContinuity::Single => { + state.preview_edge = None; + // This simply means we are terminating the workflow now + // because we have finished drawing the single edge + return Err(None); + } + EdgeContinuity::Separate => { + // Start drawing a new edge from a blank slate with the + // next selection + state.initialize_preview(cursor.level_anchor_placement, &mut commands); + } + EdgeContinuity::Continuous => { + // Start drawing a new edge, picking up from the end + // point of the previous edge + let edge = Edge::new(anchor, cursor.level_anchor_placement); + let edge = (state.spawn_edge)(edge, &mut commands); + state.preview_edge = Some(PreviewEdge { + edge, + side: Side::end(), + provisional_start: false, + }); + commands.add(ChangeDependent::add(anchor, edge)); + commands.add(ChangeDependent::add(cursor.level_anchor_placement, edge)); + } + } + } + } + } else { + // We have no preview at all yet somehow, so we'll need to create a + // fresh new edge to insert the selected anchor into + let edge = Edge::new(anchor, anchor); + let edge = (state.spawn_edge)(edge, &mut commands); + state.preview_edge = Some(PreviewEdge { + edge, + side: Side::start(), + provisional_start: selection.provisional, + }); + } + + Ok(()) +} + +pub fn on_keyboard_for_create_edges( + In((button, key)): In<(KeyCode, BufferKey)>, + mut access: BufferAccessMut, + mut edges: Query<&'static mut Edge>, + cursor: Res, + mut commands: Commands, +) -> SelectionNodeResult { + if !matches!(button, KeyCode::Escape) { + // The button was not the escape key, so there's nothing for us to do + // here. + return Ok(()); + } + + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + if let Some(preview) = &mut state.preview_edge { + if preview.side == Side::end() { + // We currently have an active preview edge and are selecting for + // the second point in the edge. Esc means we should back out of the + // current edge without exiting the edge creation workflow so the + // user can choose a different start point. + let mut edge = edges.get_mut(preview.edge).or_broken_query()?; + for anchor in edge.array() { + commands.add(ChangeDependent::remove(anchor, preview.edge)); + } + if preview.provisional_start { + commands + .get_entity(edge.start()) + .or_broken_query()? + .despawn_recursive(); + } + + *edge.left_mut() = cursor.level_anchor_placement; + *edge.right_mut() = cursor.level_anchor_placement; + preview.side = Side::start(); + preview.provisional_start = false; + commands.add(ChangeDependent::add( + cursor.level_anchor_placement, + preview.edge, + )); + } else { + // We are selecting for the first point in the edge. If the user has + // pressed Esc then that means they want to stop creating edges + // altogether. Return Err(None) to indicate that the workflow should + // exit cleaning. + return Err(None); + } + } else { + // We currently have no preview active at all. If the user hits Esc then + // they want to exit the workflow altogether. + return Err(None); + } + + Ok(()) +} + +pub fn cleanup_create_edges( + In(key): In>, + mut access: BufferAccessMut, + edges: Query<&'static Edge>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.pull().or_broken_state()?; + + if let Some(preview) = state.preview_edge { + // We created a preview, so we should despawn it while cleaning up + preview.cleanup(&edges, &mut commands)?; + } + Ok(()) +} diff --git a/rmf_site_editor/src/interaction/select/create_path.rs b/rmf_site_editor/src/interaction/select/create_path.rs new file mode 100644 index 00000000..3df425f2 --- /dev/null +++ b/rmf_site_editor/src/interaction/select/create_path.rs @@ -0,0 +1,308 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{ + interaction::*, + site::{ChangeDependent, Pending, TextureNeedsAssignment}, +}; +use bevy::prelude::*; +use bevy_impulse::*; +use rmf_site_format::Path; +use std::borrow::Borrow; + +use std::collections::HashSet; + +pub fn spawn_create_path_service( + helpers: &AnchorSelectionHelpers, + app: &mut App, +) -> Service, ()> { + let anchor_setup = + app.spawn_service(anchor_selection_setup::.into_blocking_service()); + let state_setup = app.spawn_service(create_path_setup.into_blocking_service()); + let update_preview = app.spawn_service(on_hover_for_create_path.into_blocking_service()); + let update_current = app.spawn_service(on_select_for_create_path.into_blocking_service()); + let handle_key_code = app.spawn_service(exit_on_esc::.into_blocking_service()); + let cleanup_state = app.spawn_service(cleanup_create_path.into_blocking_service()); + + helpers.spawn_anchor_selection_workflow( + anchor_setup, + state_setup, + update_preview, + update_current, + handle_key_code, + cleanup_state, + &mut app.world, + ) +} + +pub struct CreatePath { + /// Function pointer for spawning an initial path. + pub spawn_path: fn(Path, &mut Commands) -> Entity, + /// The path which is being built. This will initially be [`None`] until setup + /// happens, then `spawn_path` will be used to create this. For all the + /// services in the `create_path` workflow besides setup, this should + /// contain [`Some`]. + /// + /// If points are being added to an existing path, this could be initialized + /// as [`Some`] before the state is passed into the workflow. + pub path: Option, + /// A minimum for how many points need to be selected for the path to be + /// considered valid. Use 0 if there is no minimum. + pub minimum_points: usize, + /// Whether the path is allowed to have an inner loop. E.g. + /// `A -> B -> C -> D -> B` would be an inner loop. + pub allow_inner_loops: bool, + /// The path is implied to always be a complete loop. This has two consequences: + /// 1. If the first point gets re-selected later in the path then we automatically + /// consider the path to be finished. + /// 2. When (1) occurs, the first point does not get re-added to the path. + pub implied_complete_loop: bool, + /// A list of all anchors being used in the path which are provisional, + /// meaning they should be despawned if the path creation ends before + /// reaching the minimum number of points. + pub provisional_anchors: HashSet, + pub scope: AnchorScope, +} + +impl CreatePath { + pub fn new( + spawn_path: fn(Path, &mut Commands) -> Entity, + minimum_points: usize, + allow_inner_loops: bool, + implied_complete_loop: bool, + scope: AnchorScope, + ) -> Self { + Self { + spawn_path, + path: None, + allow_inner_loops, + minimum_points, + implied_complete_loop, + scope, + provisional_anchors: Default::default(), + } + } + + pub fn set_last( + &self, + chosen: Entity, + path_mut: &mut Path, + commands: &mut Commands, + ) -> SelectionNodeResult { + let path = self.path.or_broken_state()?; + let last = path_mut.0.last_mut().or_broken_state()?; + if chosen == *last { + // Nothing to change + return Ok(()); + } + + let previous = *last; + *last = chosen; + if !path_mut.0.contains(&previous) { + commands.add(ChangeDependent::remove(previous, path)); + } + + commands.add(ChangeDependent::add(chosen, path)); + Ok(()) + } +} + +impl Borrow for CreatePath { + fn borrow(&self) -> &AnchorScope { + &self.scope + } +} + +pub fn create_path_with_texture>>( + path: Path, + commands: &mut Commands, +) -> Entity { + let new_bundle: T = path.into(); + commands + .spawn((new_bundle, TextureNeedsAssignment, Pending)) + .id() +} + +pub fn create_path_setup( + In(key): In>, + mut access: BufferAccessMut, + cursor: Res, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + if state.path.is_none() { + let path = Path(vec![cursor.level_anchor_placement]); + let path = (state.spawn_path)(path, &mut commands); + commands.add(ChangeDependent::add(cursor.level_anchor_placement, path)); + state.path = Some(path); + } + + Ok(()) +} + +pub fn on_hover_for_create_path( + In((hover, key)): In<(Hover, BufferKey)>, + mut access: BufferAccessMut, + mut cursor: ResMut, + mut visibility: Query<&mut Visibility>, + mut paths: Query<&mut Path>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + let chosen = match hover.0 { + Some(anchor) => { + cursor.remove_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + anchor + } + None => { + cursor.add_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + cursor.level_anchor_placement + } + }; + + let path = state.path.or_broken_state()?; + let mut path_mut = paths.get_mut(path).or_broken_query()?; + state.set_last(chosen, path_mut.as_mut(), &mut commands) +} + +pub fn on_select_for_create_path( + In((selection, key)): In<(SelectionCandidate, BufferKey)>, + mut access: BufferAccessMut, + mut paths: Query<&mut Path>, + mut commands: Commands, + cursor: Res, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + let chosen = selection.candidate; + let provisional = selection.provisional; + let path = state.path.or_broken_state()?; + let mut path_mut = paths.get_mut(path).or_broken_query()?; + + if state.implied_complete_loop { + let first = path_mut.0.first().or_broken_state()?; + if chosen == *first && path_mut.0.len() >= state.minimum_points { + // The user has re-selected the first point and there are enough + // points in the path to meet the minimum requirement, so we can + // just end the workflow. + return Err(None); + } + } + + if !state.allow_inner_loops { + for a in &path_mut.0[..path_mut.0.len() - 1] { + if *a == chosen { + warn!( + "Attempting to create an inner loop in a type of path \ + which does not allow inner loops." + ); + return Ok(()); + } + } + } + + if path_mut.0.len() >= 2 { + if let Some(second_to_last) = path_mut.0.get(path_mut.0.len() - 2) { + if *second_to_last == chosen { + // Even if inner loops are allowed, we should never allow the same + // anchor to be chosen twice in a row. + warn!("Trying to select the same anchor for a path twice in a row"); + return Ok(()); + } + } + } + + state.set_last(chosen, path_mut.as_mut(), &mut commands)?; + if provisional { + state.provisional_anchors.insert(chosen); + } + + path_mut.0.push(cursor.level_anchor_placement); + commands.add(ChangeDependent::add(cursor.level_anchor_placement, path)); + + Ok(()) +} + +pub fn cleanup_create_path( + In(key): In>, + mut access: BufferAccessMut, + mut paths: Query<&'static mut Path>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.pull().or_broken_state()?; + + let Some(path) = state.path else { + // If there is no path then there is nothing to cleanup. This might + // happen if the setup needed to bail out for some reason. + return Ok(()); + }; + commands + .get_entity(path) + .or_broken_query()? + .remove::(); + let mut path_mut = paths.get_mut(path).or_broken_query()?; + + // First check if the len-1 meets the minimum point requirement. If not we + // should despawn the path as well as any provisional anchors that it used. + if path_mut.0.len() - 1 < state.minimum_points { + // We did not collect enough points for the path so we should despawn it + // as well as any provisional points it contains. + for a in &path_mut.0 { + commands.add(ChangeDependent::remove(*a, path)); + } + + for a in state.provisional_anchors { + if let Some(a_mut) = commands.get_entity(a) { + a_mut.despawn_recursive(); + } + } + + commands + .get_entity(path) + .or_broken_query()? + .despawn_recursive(); + } else { + if let Some(a) = path_mut.0.last() { + // The last point in the path is always a preview point so we need + // to pop it. + let a = *a; + path_mut.0.pop(); + if !path_mut.contains(&a) { + // Remove the dependency on the last point since it no longer + // exists in the path + commands.add(ChangeDependent::remove(a, path)); + } + } + + if path_mut.0.is_empty() { + // The path is empty... we shouldn't keep an empty path so let's + // just despawn it. + commands + .get_entity(path) + .or_broken_query()? + .despawn_recursive(); + } + } + + Ok(()) +} diff --git a/rmf_site_editor/src/interaction/select/create_point.rs b/rmf_site_editor/src/interaction/select/create_point.rs new file mode 100644 index 00000000..645d5dc1 --- /dev/null +++ b/rmf_site_editor/src/interaction/select/create_point.rs @@ -0,0 +1,201 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{ + interaction::*, + site::{ChangeDependent, Pending}, +}; +use bevy::prelude::*; +use bevy_impulse::*; +use rmf_site_format::Point; +use std::borrow::Borrow; + +pub fn spawn_create_point_service( + helpers: &AnchorSelectionHelpers, + app: &mut App, +) -> Service, ()> { + let anchor_setup = + app.spawn_service(anchor_selection_setup::.into_blocking_service()); + let state_setup = app.spawn_service(create_point_setup.into_blocking_service()); + let update_preview = app.spawn_service(on_hover_for_create_point.into_blocking_service()); + let update_current = app.spawn_service(on_select_for_create_point.into_blocking_service()); + let handle_key_code = app.spawn_service(exit_on_esc::.into_blocking_service()); + let cleanup_state = app.spawn_service(cleanup_create_point.into_blocking_service()); + + helpers.spawn_anchor_selection_workflow( + anchor_setup, + state_setup, + update_preview, + update_current, + handle_key_code, + cleanup_state, + &mut app.world, + ) +} + +pub struct CreatePoint { + /// Function pointer for spawning a point. + pub spawn_point: fn(Point, &mut Commands) -> Entity, + /// The point which is being created. This will initially be [`None`] until + /// setup happens, then `spawn_point` will be used to create this. For all + /// the services in the `create_point` workflow besides setup, this should + /// contain [`Some`]. + pub point: Option, + /// True if we should keep creating new points until the user presses Esc, + /// False if we should only create one point. + pub repeating: bool, + pub scope: AnchorScope, +} + +impl CreatePoint { + pub fn new>>(repeating: bool, scope: AnchorScope) -> Self { + Self { + spawn_point: create_point::, + point: None, + repeating, + scope, + } + } + + pub fn create_new_point(&mut self, anchor: Entity, commands: &mut Commands) { + let point = Point(anchor); + let point = (self.spawn_point)(point, commands); + commands.add(ChangeDependent::add(anchor, point)); + self.point = Some(point); + } +} + +impl Borrow for CreatePoint { + fn borrow(&self) -> &AnchorScope { + &self.scope + } +} + +fn create_point>>( + point: Point, + commands: &mut Commands, +) -> Entity { + let new_bundle: T = point.into(); + commands.spawn((new_bundle, Pending)).id() +} + +pub fn create_point_setup( + In(key): In>, + mut access: BufferAccessMut, + cursor: Res, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + if state.point.is_none() { + state.create_new_point(cursor.level_anchor_placement, &mut commands); + } + + Ok(()) +} + +fn change_point( + chosen: Entity, + point: Entity, + points: &mut Query<&mut Point>, + commands: &mut Commands, +) -> SelectionNodeResult { + let mut point_mut = points.get_mut(point).or_broken_query()?; + if point_mut.0 == chosen { + return Ok(()); + } + + commands.add(ChangeDependent::remove(point_mut.0, point)); + commands.add(ChangeDependent::add(chosen, point)); + point_mut.0 = chosen; + Ok(()) +} + +pub fn on_hover_for_create_point( + In((hover, key)): In<(Hover, BufferKey)>, + mut access: BufferAccessMut, + mut cursor: ResMut, + mut visibility: Query<&mut Visibility>, + mut points: Query<&mut Point>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + let chosen = match hover.0 { + Some(anchor) => { + cursor.remove_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + anchor + } + None => { + cursor.add_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + cursor.level_anchor_placement + } + }; + + let point = state.point.or_broken_state()?; + change_point(chosen, point, &mut points, &mut commands) +} + +pub fn on_select_for_create_point( + In((selection, key)): In<(SelectionCandidate, BufferKey)>, + mut access: BufferAccessMut, + cursor: Res, + mut points: Query<&mut Point>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + let point = state.point.or_broken_state()?; + change_point(selection.candidate, point, &mut points, &mut commands)?; + commands + .get_entity(point) + .or_broken_query()? + .remove::(); + if state.repeating { + state.create_new_point(cursor.level_anchor_placement, &mut commands); + return Ok(()); + } else { + state.point = None; + return Err(None); + } +} + +pub fn cleanup_create_point( + In(key): In>, + mut access: BufferAccessMut, + points: Query<&'static Point>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.pull().or_broken_state()?; + + let Some(point) = state.point else { + // If there is no point then there is nothing to cleanup. + return Ok(()); + }; + + let point_ref = points.get(point).or_broken_query()?; + commands.add(ChangeDependent::remove(point_ref.0, point)); + commands + .get_entity(point) + .or_broken_query()? + .despawn_recursive(); + + Ok(()) +} diff --git a/rmf_site_editor/src/interaction/select/place_object.rs b/rmf_site_editor/src/interaction/select/place_object.rs new file mode 100644 index 00000000..96a9f346 --- /dev/null +++ b/rmf_site_editor/src/interaction/select/place_object.rs @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{interaction::select::*, site::Model}; +use bevy::ecs::system::SystemParam; + +#[derive(Default)] +pub struct ObjectPlacementPlugin {} + +impl Plugin for ObjectPlacementPlugin { + fn build(&self, app: &mut App) { + let services = ObjectPlacementServices::from_app(app); + app.insert_resource(services); + } +} + +#[derive(Resource, Clone, Copy)] +pub struct ObjectPlacementServices { + pub place_object_2d: Service, ()>, + pub place_object_3d: Service, ()>, + pub replace_parent_3d: Service, ()>, + pub hover_service_object_3d: Service<(), (), Hover>, +} + +impl ObjectPlacementServices { + pub fn from_app(app: &mut App) -> Self { + let hover_service_object_3d = app.spawn_continuous_service( + Update, + hover_service:: + .configure(|config: SystemConfigs| config.in_set(SelectionServiceStages::Hover)), + ); + let place_object_2d = spawn_place_object_2d_workflow(app); + let place_object_3d = spawn_place_object_3d_workflow(hover_service_object_3d, app); + let replace_parent_3d = spawn_replace_parent_3d_workflow(hover_service_object_3d, app); + Self { + place_object_2d, + place_object_3d, + replace_parent_3d, + hover_service_object_3d, + } + } +} + +#[derive(SystemParam)] +pub struct ObjectPlacement<'w, 's> { + pub services: Res<'w, ObjectPlacementServices>, + pub commands: Commands<'w, 's>, +} + +impl<'w, 's> ObjectPlacement<'w, 's> { + pub fn place_object_2d(&mut self, object: Model, level: Entity) { + let state = self + .commands + .spawn(SelectorInput(PlaceObject2d { object, level })) + .id(); + self.send(RunSelector { + selector: self.services.place_object_2d, + input: Some(state), + }); + } + + pub fn place_object_3d( + &mut self, + object: PlaceableObject, + parent: Option, + workspace: Entity, + ) { + let state = self + .commands + .spawn(SelectorInput(PlaceObject3d { + object, + parent, + workspace, + })) + .id(); + self.send(RunSelector { + selector: self.services.place_object_3d, + input: Some(state), + }); + } + + pub fn replace_parent_3d(&mut self, object: Entity, workspace: Entity) { + let state = self + .commands + .spawn(SelectorInput(ReplaceParent3d { object, workspace })) + .id(); + self.send(RunSelector { + selector: self.services.replace_parent_3d, + input: Some(state), + }); + } + + fn send(&mut self, run: RunSelector) { + self.commands.add(move |world: &mut World| { + world.send_event(run); + }); + } +} diff --git a/rmf_site_editor/src/interaction/select/place_object_2d.rs b/rmf_site_editor/src/interaction/select/place_object_2d.rs new file mode 100644 index 00000000..295b3b65 --- /dev/null +++ b/rmf_site_editor/src/interaction/select/place_object_2d.rs @@ -0,0 +1,207 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{interaction::select::*, site::Model}; +use bevy::prelude::Input as UserInput; + +pub const PLACE_OBJECT_2D_MODE_LABEL: &'static str = "place_object_2d"; + +pub fn spawn_place_object_2d_workflow(app: &mut App) -> Service, ()> { + let setup = app.spawn_service(place_object_2d_setup.into_blocking_service()); + let find_position = app.spawn_continuous_service(Update, place_object_2d_find_placement); + let placement_chosen = app.spawn_service(on_placement_chosen_2d.into_blocking_service()); + let handle_key_code = + app.spawn_service(on_keyboard_for_place_object_2d.into_blocking_service()); + let cleanup = app.spawn_service(place_object_2d_cleanup.into_blocking_service()); + + let keyboard_just_pressed = app + .world + .resource::() + .keyboard_just_pressed; + + app.world.spawn_io_workflow(build_place_object_2d_workflow( + setup, + find_position, + placement_chosen, + handle_key_code, + cleanup, + keyboard_just_pressed, + )) +} + +pub fn build_place_object_2d_workflow( + setup: Service, SelectionNodeResult>, + find_placement: Service<(), Transform>, + placement_chosen: Service<(Transform, BufferKey), SelectionNodeResult>, + handle_key_code: Service, + cleanup: Service, ()>, + keyboard_just_pressed: Service<(), (), StreamOf>, +) -> impl FnOnce(Scope, ()>, &mut Builder) { + move |scope, builder| { + let buffer = builder.create_buffer::(BufferSettings::keep_last(1)); + + let setup_finished = scope + .input + .chain(builder) + .then(extract_selector_input::.into_blocking_callback()) + .branch_for_err(|err| err.connect(scope.terminate)) + .cancel_on_none() + .then_push(buffer) + .then_access(buffer) + .then(setup) + .branch_for_err(|err| err.map_block(print_if_err).connect(scope.terminate)) + .output() + .fork_clone(builder); + + setup_finished + .clone_chain(builder) + .then(find_placement) + .with_access(buffer) + .then(placement_chosen) + .fork_result( + |ok| ok.connect(scope.terminate), + |err| err.map_block(print_if_err).connect(scope.terminate), + ); + + let keyboard_node = setup_finished + .clone_chain(builder) + .then_node(keyboard_just_pressed); + keyboard_node + .streams + .chain(builder) + .inner() + .then(handle_key_code) + .fork_result( + |ok| ok.connect(scope.terminate), + |err| err.map_block(print_if_err).connect(scope.terminate), + ); + + builder.on_cleanup(buffer, move |scope, builder| { + scope + .input + .chain(builder) + .then(cleanup) + .connect(scope.terminate); + }); + } +} + +pub struct PlaceObject2d { + pub object: Model, + pub level: Entity, +} + +pub fn place_object_2d_setup( + In(key): In>, + mut access: BufferAccessMut, + mut cursor: ResMut, + mut visibility: Query<&mut Visibility>, + mut commands: Commands, + mut gizmo_blockers: ResMut, + mut highlight: ResMut, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_buffer()?; + + cursor.set_model_preview(&mut commands, Some(state.object.clone())); + set_visibility(cursor.dagger, &mut visibility, false); + set_visibility(cursor.halo, &mut visibility, false); + + highlight.0 = false; + gizmo_blockers.selecting = true; + + cursor.add_mode(PLACE_OBJECT_2D_MODE_LABEL, &mut visibility); + + Ok(()) +} + +pub fn place_object_2d_cleanup( + In(_): In>, + mut cursor: ResMut, + mut visibility: Query<&mut Visibility>, + mut commands: Commands, + mut gizmo_blockers: ResMut, +) { + cursor.remove_preview(&mut commands); + cursor.remove_mode(PLACE_OBJECT_2D_MODE_LABEL, &mut visibility); + gizmo_blockers.selecting = false; +} + +pub fn place_object_2d_find_placement( + In(ContinuousService { key }): ContinuousServiceInput<(), Transform>, + mut orders: ContinuousQuery<(), Transform>, + cursor: Res, + mut transforms: Query<&mut Transform>, + intersect_ground_params: IntersectGroundPlaneParams, + mouse_button_input: Res>, + blockers: Option>, +) { + let Some(mut orders) = orders.get_mut(&key) else { + return; + }; + + let Some(order) = orders.get_mut(0) else { + return; + }; + + // TODO(@mxgrey): Consider allowing models to be snapped to existing objects + // similar to how they can for the 3D object placement workflow. Either we + // need to introduce parent frames to the 2D sites or just don't bother with + // parenting. + if let Some(intersection) = intersect_ground_params.ground_plane_intersection() { + match transforms.get_mut(cursor.frame) { + Ok(mut transform) => { + *transform = intersection; + } + Err(err) => { + error!("No cursor transform found: {err}"); + } + } + + let clicked = mouse_button_input.just_pressed(MouseButton::Left); + let blocked = blockers.filter(|x| x.blocking()).is_some(); + if clicked && !blocked { + order.respond(intersection); + } + } else { + warn!("Unable to find a placement position. Try adjusting your camera angle."); + } +} + +pub fn on_keyboard_for_place_object_2d(In(key): In) -> SelectionNodeResult { + if matches!(key, KeyCode::Escape) { + // Simply end the workflow if the escape key was pressed + info!("Exiting 2D object placement"); + return Err(None); + } + + Ok(()) +} + +pub fn on_placement_chosen_2d( + In((placement, key)): In<(Transform, BufferKey)>, + mut access: BufferAccessMut, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let mut state = access.pull().or_broken_state()?; + + state.object.pose = placement.into(); + commands.spawn(state.object).set_parent(state.level); + + Ok(()) +} diff --git a/rmf_site_editor/src/interaction/select/place_object_3d.rs b/rmf_site_editor/src/interaction/select/place_object_3d.rs new file mode 100644 index 00000000..fe88a9c0 --- /dev/null +++ b/rmf_site_editor/src/interaction/select/place_object_3d.rs @@ -0,0 +1,505 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{ + interaction::select::*, + site::{ + Anchor, AnchorBundle, Dependents, FrameMarker, Model, NameInSite, NameInWorkcell, Pending, + SiteID, WorkcellModel, + }, + widgets::canvas_tooltips::CanvasTooltips, +}; +use bevy::{ecs::system::SystemParam, prelude::Input as UserInput}; +use bevy_mod_raycast::deferred::RaycastSource; +use std::borrow::Cow; + +pub const PLACE_OBJECT_3D_MODE_LABEL: &'static str = "place_object_3d"; + +pub fn spawn_place_object_3d_workflow( + hover_service: Service<(), (), Hover>, + app: &mut App, +) -> Service, ()> { + let setup = app.spawn_service(place_object_3d_setup); + let find_position = app.spawn_continuous_service(Update, place_object_3d_find_placement); + let placement_chosen = app.spawn_service(on_placement_chosen_3d.into_blocking_service()); + let handle_key_code = app.spawn_service(on_keyboard_for_place_object_3d); + let cleanup = app.spawn_service(place_object_3d_cleanup.into_blocking_service()); + let selection_update = app.world.resource::().selection_update; + let keyboard_just_pressed = app + .world + .resource::() + .keyboard_just_pressed; + + app.world.spawn_io_workflow(build_place_object_3d_workflow( + setup, + find_position, + placement_chosen, + handle_key_code, + cleanup, + hover_service.optional_stream_cast(), + selection_update, + keyboard_just_pressed, + )) +} + +pub fn build_place_object_3d_workflow( + setup: Service, SelectionNodeResult, Select>, + find_placement: Service, Transform, Select>, + placement_chosen: Service<(Transform, BufferKey), SelectionNodeResult>, + handle_key_code: Service<(KeyCode, BufferKey), SelectionNodeResult, Select>, + cleanup: Service, SelectionNodeResult>, + // Used to manage highlighting prospective parent frames + hover_service: Service<(), ()>, + // Used to manage highlighting the current parent frame + selection_update: Service, + keyboard_just_pressed: Service<(), (), StreamOf>, +) -> impl FnOnce(Scope, ()>, &mut Builder) { + move |scope, builder| { + let buffer = builder.create_buffer::(BufferSettings::keep_last(1)); + let selection_update_node = builder.create_node(selection_update); + let setup_node = scope + .input + .chain(builder) + .then(extract_selector_input::.into_blocking_callback()) + .branch_for_err(|err| err.connect(scope.terminate)) + .cancel_on_none() + .then_push(buffer) + .then_access(buffer) + .then_node(setup); + + builder.connect(setup_node.streams, selection_update_node.input); + + let begin_input_services = setup_node + .output + .chain(builder) + .branch_for_err(|err| err.map_block(print_if_err).connect(scope.terminate)) + .output() + .fork_clone(builder); + + let find_placement_node = begin_input_services + .clone_chain(builder) + .then_access(buffer) + .then_node(find_placement); + + find_placement_node + .output + .chain(builder) + .with_access(buffer) + .then(placement_chosen) + .fork_result( + |ok| ok.connect(scope.terminate), + |err| err.map_block(print_if_err).connect(scope.terminate), + ); + + builder.connect(find_placement_node.streams, selection_update_node.input); + + begin_input_services + .clone_chain(builder) + .then(hover_service) + .connect(scope.terminate); + + let keyboard = begin_input_services + .clone_chain(builder) + .then_node(keyboard_just_pressed); + let handle_key_node = keyboard + .streams + .chain(builder) + .inner() + .with_access(buffer) + .then_node(handle_key_code); + + handle_key_node + .output + .chain(builder) + .dispose_on_ok() + .map_block(print_if_err) + .connect(scope.terminate); + + builder.connect(handle_key_node.streams, selection_update_node.input); + + builder.on_cleanup(buffer, move |scope, builder| { + scope.input.chain(builder).then(cleanup).fork_result( + |ok| ok.connect(scope.terminate), + |err| err.map_block(print_if_err).connect(scope.terminate), + ); + }); + } +} + +pub struct PlaceObject3d { + pub object: PlaceableObject, + pub parent: Option, + pub workspace: Entity, +} + +#[derive(Clone, Debug)] +pub enum PlaceableObject { + Model(Model), + Anchor, + VisualMesh(WorkcellModel), + CollisionMesh(WorkcellModel), +} + +pub fn place_object_3d_setup( + In(srv): BlockingServiceInput, Select>, + mut access: BufferAccessMut, + mut cursor: ResMut, + mut visibility: Query<&mut Visibility>, + mut commands: Commands, + mut highlight: ResMut, + mut filter: PlaceObject3dFilter, + mut gizmo_blockers: ResMut, +) -> SelectionNodeResult { + let mut access = access.get_mut(&srv.request).or_broken_buffer()?; + let state = access.newest_mut().or_broken_buffer()?; + + match &state.object { + PlaceableObject::Anchor => { + // Make the anchor placement component of the cursor visible + set_visibility(cursor.frame_placement, &mut visibility, true); + set_visibility(cursor.dagger, &mut visibility, true); + set_visibility(cursor.halo, &mut visibility, true); + } + PlaceableObject::Model(m) => { + // Spawn the model as a child of the cursor + cursor.set_model_preview(&mut commands, Some(m.clone())); + set_visibility(cursor.dagger, &mut visibility, false); + set_visibility(cursor.halo, &mut visibility, false); + } + PlaceableObject::VisualMesh(m) | PlaceableObject::CollisionMesh(m) => { + // Spawn the model as a child of the cursor + cursor.set_workcell_model_preview(&mut commands, Some(m.clone())); + set_visibility(cursor.dagger, &mut visibility, false); + set_visibility(cursor.halo, &mut visibility, false); + } + } + + if let Some(parent) = state.parent { + let parent = filter.filter_select(parent); + state.parent = parent; + } + srv.streams.send(Select::new(state.parent)); + + highlight.0 = true; + gizmo_blockers.selecting = true; + + cursor.add_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); + + Ok(()) +} + +pub fn place_object_3d_cleanup( + In(_): In>, + mut cursor: ResMut, + mut visibility: Query<&mut Visibility>, + mut commands: Commands, + mut highlight: ResMut, + mut gizmo_blockers: ResMut, +) -> SelectionNodeResult { + cursor.remove_preview(&mut commands); + cursor.remove_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); + set_visibility(cursor.frame_placement, &mut visibility, false); + highlight.0 = false; + gizmo_blockers.selecting = false; + + Ok(()) +} + +pub fn place_object_3d_find_placement( + In(ContinuousService { key: srv_key }): ContinuousServiceInput< + BufferKey, + Transform, + Select, + >, + mut orders: ContinuousQuery, Transform, Select>, + mut buffer: BufferAccessMut, + mut cursor: ResMut, + raycast_sources: Query<&RaycastSource>, + mut transforms: Query<&mut Transform>, + intersect_ground_params: IntersectGroundPlaneParams, + mut visibility: Query<&mut Visibility>, + mut tooltips: ResMut, + keyboard_input: Res>, + mut hover: EventWriter, + hovering: Res, + mouse_button_input: Res>, + blockers: Option>, + meta: Query<(Option<&'static NameInSite>, Option<&'static SiteID>)>, + mut filter: PlaceObject3dFilter, +) { + let Some(mut orders) = orders.get_mut(&srv_key) else { + return; + }; + + let Some(order) = orders.get_mut(0) else { + return; + }; + + let key = order.request(); + let Ok(mut buffer) = buffer.get_mut(key) else { + error!("Unable to retrieve buffer in place_object_3d_cursor_transform"); + return; + }; + let Some(state) = buffer.newest_mut() else { + error!("Missing state in place_object_3d_cursor_transform"); + return; + }; + + if state.parent.is_some() { + tooltips.add(Cow::Borrowed("Esc: deselect current parent")); + } + + let project_to_plane = keyboard_input.any_pressed([KeyCode::ShiftLeft, KeyCode::ShiftRight]); + + let mut transform = match transforms.get_mut(cursor.frame) { + Ok(transform) => transform, + Err(err) => { + error!("No cursor transform found: {err}"); + return; + } + }; + + let Ok(source) = raycast_sources.get_single() else { + return; + }; + + // Check if there is an intersection with a mesh + let mut intersection: Option = None; + let mut new_hover = None; + let mut select_new_parent = false; + if !project_to_plane { + for (e, i) in source.intersections() { + let Some(e) = filter.filter_pick(*e) else { + continue; + }; + + if let Some(parent) = state.parent { + if e == parent { + new_hover = Some(parent); + cursor.add_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); + tooltips.add(Cow::Borrowed("Click to place")); + tooltips.add(Cow::Borrowed("+Shift: Project to parent frame")); + + // Don't use the intersection with the parent if the parent + // is an anchor because that results in silly orientations + // which the user probably does not want. + if !filter.anchors.contains(e) { + intersection = Some( + Transform::from_translation(i.position()) + .with_rotation(aligned_z_axis(i.normal())), + ); + } + break; + } + } else { + new_hover = Some(e); + select_new_parent = true; + cursor.remove_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); + tooltips.add(Cow::Borrowed("Click to set as parent")); + tooltips.add(Cow::Borrowed("+Shift: Project to ground plane")); + break; + } + } + } else { + cursor.add_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); + } + + if new_hover != hovering.0 { + hover.send(Hover(new_hover)); + } + + if !select_new_parent { + intersection = intersection.or_else(|| { + if let Some(parent) = state.parent { + tooltips.add(Cow::Borrowed("Click to place")); + cursor.add_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); + intersect_ground_params.frame_plane_intersection(parent) + } else { + tooltips.add(Cow::Borrowed("Click to place")); + cursor.add_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); + intersect_ground_params.ground_plane_intersection() + } + }); + + if let Some(intersection) = intersection { + *transform = intersection; + } + } + + let clicked = mouse_button_input.just_pressed(MouseButton::Left); + let blocked = blockers.filter(|x| x.blocking()).is_some(); + if clicked && !blocked { + if select_new_parent { + if let Some(new_parent) = new_hover { + state.parent = Some(new_parent); + order.streams().send(Select::new(Some(new_parent))); + if let Ok((name, id)) = meta.get(new_parent) { + let id = id.map(|id| id.0.to_string()); + info!( + "Placing object in the frame of [{}], id: {}", + name.map(|name| name.0.as_str()).unwrap_or(""), + id.as_ref().map(|id| id.as_str()).unwrap_or("*"), + ); + } + } + } else { + if let Some(intersection) = intersection { + // The user is choosing a location to place the object. + order.respond(intersection); + } else { + warn!("Unable to find a placement position. Try adjusting your camera angle."); + } + } + } +} + +#[derive(SystemParam)] +pub struct PlaceObject3dFilter<'w, 's> { + inspect: InspectorFilter<'w, 's>, + ignore: Query<'w, 's, (), Or<(With, With)>>, + // We aren't using this in the filter functions, we're sneaking this query + // into this system param to skirt around the 16-parameter limit for + // place_object_3d_find_placement + anchors: Query<'w, 's, (), With>, +} + +impl<'w, 's> SelectionFilter for PlaceObject3dFilter<'w, 's> { + fn filter_pick(&mut self, target: Entity) -> Option { + let e = self.inspect.filter_pick(target); + + if let Some(e) = e { + if self.ignore.contains(e) { + return None; + } + } + e + } + + fn filter_select(&mut self, target: Entity) -> Option { + self.inspect.filter_select(target) + } + + fn on_click(&mut self, _: Hover) -> Option, +) { + let Some(mut orders) = orders.get_mut(&key) else { + selected.clear(); + return; + }; + + let Some(order) = orders.get_mut(0) else { + // Clear the selected reader so we don't mistake an earlier signal as + // being intended for this workflow. + selected.clear(); + return; + }; + + let object = *order.request(); + for s in selected.read() { + // Allow users to signal the choice of parent by means other than clicking + match s.0 { + Some(s) => { + if let Some(e) = filter.filter_pick(s.candidate) { + order.respond(Some(e)); + return; + } + + info!( + "Received parent replacement selection signal for an invalid parent candidate" + ); + } + None => { + // The user has sent a signal to remove the object from its parent + order.respond(None); + return; + } + } + } + + let Ok(source) = raycast_sources.get_single() else { + return; + }; + + let mut hovered: Option = None; + let mut ignore_click = false; + for (e, _) in source.intersections() { + let Some(e) = filter.filter_pick(*e) else { + continue; + }; + + if AncestorIter::new(&parents, e) + .filter(|e| *e == object) + .next() + .is_some() + { + ignore_click = true; + tooltips.add(Cow::Borrowed( + "Cannot select a child of the object to be its parent", + )); + break; + } + + if e == object { + ignore_click = true; + tooltips.add(Cow::Borrowed( + "Cannot select an object to be its own parent", + )); + break; + } + + hovered = Some(e); + } + + if hovered.is_some() { + tooltips.add(Cow::Borrowed("Click to select this as the parent")); + } else if !ignore_click { + tooltips.add(Cow::Borrowed("Click to remove parent")); + } + + if hovered != hovering.0 { + hover.send(Hover(hovered)); + } + + let clicked = mouse_button_input.just_pressed(MouseButton::Left); + let blocked = blockers.filter(|x| x.blocking()).is_some(); + if clicked && !blocked && !ignore_click { + order.respond(hovered); + } +} + +pub fn replace_parent_3d_parent_chosen( + In((parent, key)): In<(Option, BufferKey)>, + access: BufferAccess, + mut dependents: Query<&mut Dependents>, + mut poses: Query<&mut Pose>, + global_tfs: Query<&GlobalTransform>, + parents: Query<&Parent>, + frames: Query<(), With>, + mut commands: Commands, + mut anchors: Query<&mut Anchor>, +) -> SelectionNodeResult { + let access = access.get(&key).or_broken_buffer()?; + let state = access.newest().or_broken_state()?; + + let parent = parent + .and_then(|p| { + if frames.contains(p) { + Some(p) + } else { + // The selected parent is not a frame, so find its first ancestor + // that contains a FrameMarker + AncestorIter::new(&parents, p).find(|e| frames.contains(*e)) + } + }) + .unwrap_or(state.workspace); + + let previous_parent = parents.get(state.object).or_broken_query()?.get(); + if parent == previous_parent { + info!("Object's parent remains the same"); + return Ok(()); + } + + let object_tf = global_tfs.get(state.object).or_broken_query()?.affine(); + let inv_parent_tf = global_tfs.get(parent).or_broken_query()?.affine().inverse(); + let relative_pose: Pose = Transform::from_matrix((inv_parent_tf * object_tf).into()).into(); + + let [mut previous_deps, mut new_deps] = dependents + .get_many_mut([previous_parent, parent]) + .or_broken_query()?; + + if let Ok(mut pose_mut) = poses.get_mut(state.object) { + *pose_mut = relative_pose; + } else { + let mut anchor = anchors.get_mut(state.object).or_broken_query()?; + *anchor = Anchor::Pose3D(relative_pose); + } + + // Do all mutations after everything is successfully queried so we don't + // risk an inconsistent/broken world due to a query failing. + commands + .get_entity(state.object) + .or_broken_query()? + .set_parent(parent); + previous_deps.remove(&state.object); + new_deps.insert(state.object); + + Ok(()) +} + +pub fn on_keyboard_for_replace_parent_3d(In(code): In) -> SelectionNodeResult { + if matches!(code, KeyCode::Escape) { + // Simply exit the workflow if the user presses esc + return Err(None); + } + + Ok(()) +} diff --git a/rmf_site_editor/src/interaction/select/replace_point.rs b/rmf_site_editor/src/interaction/select/replace_point.rs new file mode 100644 index 00000000..54339714 --- /dev/null +++ b/rmf_site_editor/src/interaction/select/replace_point.rs @@ -0,0 +1,177 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{ + interaction::*, + site::{ChangeDependent, Original}, +}; +use bevy::prelude::*; +use bevy_impulse::*; +use rmf_site_format::Point; +use std::borrow::Borrow; + +pub fn spawn_replace_point_service( + helpers: &AnchorSelectionHelpers, + app: &mut App, +) -> Service, ()> { + let anchor_setup = + app.spawn_service(anchor_selection_setup::.into_blocking_service()); + let state_setup = app.spawn_service(replace_point_setup.into_blocking_service()); + let update_preview = app.spawn_service(on_hover_for_replace_point.into_blocking_service()); + let update_current = app.spawn_service(on_select_for_replace_point.into_blocking_service()); + let handle_key_code = app.spawn_service(exit_on_esc::.into_blocking_service()); + let cleanup_state = app.spawn_service(cleanup_replace_point.into_blocking_service()); + + helpers.spawn_anchor_selection_workflow( + anchor_setup, + state_setup, + update_preview, + update_current, + handle_key_code, + cleanup_state, + &mut app.world, + ) +} + +pub struct ReplacePoint { + /// The point whose anchor is being replaced + pub point: Entity, + /// The original value of the point. This is None until setup occurs, then + /// its value will be available. + pub original: Option>, + /// The scope that the point exists in + pub scope: AnchorScope, + /// Keeps track of whether the replacement really happened. If false, the + /// cleanup will revert the point to its original state. If true, the cleanup + /// will not need to do anything. + pub replaced: bool, +} + +impl ReplacePoint { + pub fn new(point: Entity, scope: AnchorScope) -> Self { + Self { + point, + original: None, + scope, + replaced: false, + } + } + + pub fn set_chosen( + &mut self, + chosen: Entity, + points: &mut Query<&mut Point>, + commands: &mut Commands, + ) -> SelectionNodeResult { + let mut point_mut = points.get_mut(self.point).or_broken_query()?; + commands.add(ChangeDependent::remove(point_mut.0, self.point)); + point_mut.0 = chosen; + commands.add(ChangeDependent::add(chosen, self.point)); + Ok(()) + } +} + +impl Borrow for ReplacePoint { + fn borrow(&self) -> &AnchorScope { + &self.scope + } +} + +pub fn replace_point_setup( + In(key): In>, + mut access: BufferAccessMut, + mut points: Query<&'static mut Point>, + cursor: Res, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + let original = *points.get(state.point).or_broken_query()?; + state.original = Some(original); + commands.entity(state.point).insert(Original(original)); + state.set_chosen(cursor.level_anchor_placement, &mut points, &mut commands)?; + + Ok(()) +} + +pub fn on_hover_for_replace_point( + In((hover, key)): In<(Hover, BufferKey)>, + mut access: BufferAccessMut, + mut cursor: ResMut, + mut visibility: Query<&mut Visibility>, + mut points: Query<&mut Point>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + let chosen = match hover.0 { + Some(anchor) => { + cursor.remove_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + anchor + } + None => { + cursor.add_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + cursor.level_anchor_placement + } + }; + + state.set_chosen(chosen, &mut points, &mut commands) +} + +pub fn on_select_for_replace_point( + In((selection, key)): In<(SelectionCandidate, BufferKey)>, + mut access: BufferAccessMut, + mut points: Query<&mut Point>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + state.set_chosen(selection.candidate, &mut points, &mut commands)?; + state.replaced = true; + // Since the selection has been made, we should exit the workflow now + Err(None) +} + +pub fn cleanup_replace_point( + In(key): In>, + mut access: BufferAccessMut, + mut points: Query<&'static mut Point>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let mut state = access.pull().or_broken_state()?; + + commands + .get_entity(state.point) + .or_broken_query()? + .remove::>>(); + + if state.replaced { + // The anchor was fully replaced, so nothing furtehr to do + return Ok(()); + } + + let Some(original) = state.original else { + return Ok(()); + }; + + state.set_chosen(original.0, &mut points, &mut commands)?; + + Ok(()) +} diff --git a/rmf_site_editor/src/interaction/select/replace_side.rs b/rmf_site_editor/src/interaction/select/replace_side.rs new file mode 100644 index 00000000..fa0608ab --- /dev/null +++ b/rmf_site_editor/src/interaction/select/replace_side.rs @@ -0,0 +1,205 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{ + interaction::*, + site::{ChangeDependent, Original}, +}; +use bevy::prelude::*; +use bevy_impulse::*; +use rmf_site_format::{Edge, Side}; +use std::borrow::Borrow; + +pub fn spawn_replace_side_service( + helpers: &AnchorSelectionHelpers, + app: &mut App, +) -> Service, ()> { + let anchor_setup = + app.spawn_service(anchor_selection_setup::.into_blocking_service()); + let state_setup = app.spawn_service(replace_side_setup.into_blocking_service()); + let update_preview = app.spawn_service(on_hover_for_replace_side.into_blocking_service()); + let update_current = app.spawn_service(on_select_for_replace_side.into_blocking_service()); + let handle_key_code = app.spawn_service(exit_on_esc::.into_blocking_service()); + let cleanup_state = app.spawn_service(cleanup_replace_side.into_blocking_service()); + + helpers.spawn_anchor_selection_workflow( + anchor_setup, + state_setup, + update_preview, + update_current, + handle_key_code, + cleanup_state, + &mut app.world, + ) +} + +pub struct ReplaceSide { + /// The edge whose anchor is being replaced + pub edge: Entity, + /// The side of the edge which is being replaced + pub side: Side, + /// The original values for the edge. This is None until setup occurs, then + /// its value will be available. + pub original: Option>, + /// The scope that the edge exists in + pub scope: AnchorScope, + /// Keeps track of whether the replacement really happened. If false, the + /// cleanup will revert the edge to its original state. If true, the cleanup + /// will not need to do anything. + pub replaced: bool, +} + +impl ReplaceSide { + pub fn new(edge: Entity, side: Side, scope: AnchorScope) -> Self { + Self { + edge, + side, + scope, + original: None, + replaced: false, + } + } + + pub fn set_chosen( + &mut self, + chosen: Entity, + edges: &mut Query<&mut Edge>, + commands: &mut Commands, + ) -> SelectionNodeResult { + let original = self.original.or_broken_buffer()?; + let mut edge_mut = edges.get_mut(self.edge).or_broken_query()?; + + for a in edge_mut.array() { + // Remove both current dependencies in case both of them change. + // If either dependency doesn't change then they'll be added back + // later anyway. + commands.add(ChangeDependent::remove(a, self.edge)); + } + + if chosen == original.array()[self.side.opposite().index()] { + // The user is choosing the anchor on the opposite side of the edge as + // the replacement anchor. We take this to mean that the user wants to + // flip the edge. + *edge_mut.left_mut() = original.right(); + *edge_mut.right_mut() = original.left(); + } else { + edge_mut.array_mut()[self.side.index()] = chosen; + let opp = self.side.opposite().index(); + edge_mut.array_mut()[opp] = original.array()[opp]; + } + + for a in edge_mut.array() { + commands.add(ChangeDependent::add(a, self.edge)); + } + + Ok(()) + } +} + +impl Borrow for ReplaceSide { + fn borrow(&self) -> &AnchorScope { + &self.scope + } +} + +pub fn replace_side_setup( + In(key): In>, + mut access: BufferAccessMut, + mut edges: Query<&'static mut Edge>, + cursor: Res, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + let edge_ref = edges.get(state.edge).or_broken_query()?; + let original_edge: Edge = *edge_ref; + state.original = Some(original_edge); + commands.entity(state.edge).insert(Original(original_edge)); + state.set_chosen(cursor.level_anchor_placement, &mut edges, &mut commands)?; + + Ok(()) +} + +pub fn on_hover_for_replace_side( + In((hover, key)): In<(Hover, BufferKey)>, + mut access: BufferAccessMut, + mut cursor: ResMut, + mut visibility: Query<&mut Visibility>, + mut edges: Query<&mut Edge>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + + let chosen = match hover.0 { + Some(anchor) => { + cursor.remove_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + anchor + } + None => { + cursor.add_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + cursor.level_anchor_placement + } + }; + + state.set_chosen(chosen, &mut edges, &mut commands) +} + +pub fn on_select_for_replace_side( + In((selection, key)): In<(SelectionCandidate, BufferKey)>, + mut access: BufferAccessMut, + mut edges: Query<&mut Edge>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let state = access.newest_mut().or_broken_state()?; + state.set_chosen(selection.candidate, &mut edges, &mut commands)?; + state.replaced = true; + // Since the selection has been made, we should exit the workflow now + Err(None) +} + +pub fn cleanup_replace_side( + In(key): In>, + mut access: BufferAccessMut, + mut edges: Query<&'static mut Edge>, + mut commands: Commands, +) -> SelectionNodeResult { + let mut access = access.get_mut(&key).or_broken_buffer()?; + let mut state = access.pull().or_broken_state()?; + + commands + .get_entity(state.edge) + .or_broken_query()? + .remove::>>(); + + if state.replaced { + // The anchor was fully replaced, so nothing further to do + return Ok(()); + } + + // The anchor was not replaced so we need to revert to the original setup + let Some(original) = state.original else { + return Ok(()); + }; + + let revert = original.array()[state.side.index()]; + state.set_chosen(revert, &mut edges, &mut commands)?; + + Ok(()) +} diff --git a/rmf_site_editor/src/interaction/select/select_anchor.rs b/rmf_site_editor/src/interaction/select/select_anchor.rs new file mode 100644 index 00000000..399a63d4 --- /dev/null +++ b/rmf_site_editor/src/interaction/select/select_anchor.rs @@ -0,0 +1,765 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use bevy::prelude::*; +use bevy_impulse::*; + +use crate::{interaction::select::*, site::CurrentLevel}; +use rmf_site_format::{Fiducial, Floor, LevelElevation, Location, Path, Point}; + +#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Resource)] +pub enum AnchorScope { + Drawing, + General, + Site, +} + +impl AnchorScope { + pub fn is_site(&self) -> bool { + match self { + AnchorScope::Site => true, + _ => false, + } + } +} + +#[derive(Default)] +pub struct AnchorSelectionPlugin {} + +impl Plugin for AnchorSelectionPlugin { + fn build(&self, app: &mut App) { + let helpers = AnchorSelectionHelpers::from_app(app); + let services = AnchorSelectionServices::from_app(&helpers, app); + app.init_resource::() + .insert_resource(AnchorScope::General) + .insert_resource(helpers) + .insert_resource(services); + } +} + +#[derive(Resource, Clone, Copy)] +pub struct AnchorSelectionHelpers { + pub anchor_select_stream: Service<(), (), (Hover, Select)>, + pub anchor_cursor_transform: Service<(), ()>, + pub keyboard_just_pressed: Service<(), (), StreamOf>, + pub cleanup_anchor_selection: Service<(), ()>, +} + +impl AnchorSelectionHelpers { + pub fn from_app(app: &mut App) -> Self { + let anchor_select_stream = app.spawn_selection_service::(); + let anchor_cursor_transform = app.spawn_continuous_service( + Update, + select_anchor_cursor_transform + .configure(|config: SystemConfigs| config.in_set(SelectionServiceStages::Pick)), + ); + let cleanup_anchor_selection = app + .world + .spawn_service(cleanup_anchor_selection.into_blocking_service()); + + let keyboard_just_pressed = app + .world + .resource::() + .keyboard_just_pressed; + + Self { + anchor_select_stream, + anchor_cursor_transform, + keyboard_just_pressed, + cleanup_anchor_selection, + } + } + + pub fn spawn_anchor_selection_workflow( + &self, + anchor_setup: Service, SelectionNodeResult>, + state_setup: Service, SelectionNodeResult>, + update_preview: Service<(Hover, BufferKey), SelectionNodeResult>, + update_current: Service<(SelectionCandidate, BufferKey), SelectionNodeResult>, + handle_key_code: Service<(KeyCode, BufferKey), SelectionNodeResult>, + cleanup_state: Service, SelectionNodeResult>, + world: &mut World, + ) -> Service, ()> { + world.spawn_io_workflow(build_anchor_selection_workflow( + anchor_setup, + state_setup, + update_preview, + update_current, + handle_key_code, + cleanup_state, + self.anchor_cursor_transform, + self.anchor_select_stream, + self.keyboard_just_pressed, + self.cleanup_anchor_selection, + )) + } +} + +#[derive(Resource, Clone, Copy)] +pub struct AnchorSelectionServices { + pub create_edges: Service, ()>, + pub replace_side: Service, ()>, + pub create_path: Service, ()>, + pub create_point: Service, ()>, + pub replace_point: Service, ()>, +} + +impl AnchorSelectionServices { + pub fn from_app(helpers: &AnchorSelectionHelpers, app: &mut App) -> Self { + let create_edges = spawn_create_edges_service(helpers, app); + let replace_side = spawn_replace_side_service(helpers, app); + let create_path = spawn_create_path_service(helpers, app); + let create_point = spawn_create_point_service(helpers, app); + let replace_point = spawn_replace_point_service(helpers, app); + Self { + create_edges, + replace_side, + create_path, + create_point, + replace_point, + } + } +} + +#[derive(SystemParam)] +pub struct AnchorSelection<'w, 's> { + pub services: Res<'w, AnchorSelectionServices>, + pub commands: Commands<'w, 's>, +} + +impl<'w, 's> AnchorSelection<'w, 's> { + pub fn create_lanes(&mut self) { + self.create_edges::>(EdgeContinuity::Continuous, AnchorScope::General); + } + + pub fn create_measurements(&mut self) { + self.create_edges::>(EdgeContinuity::Separate, AnchorScope::Drawing) + } + + pub fn create_walls(&mut self) { + self.create_edges_with_texture::>( + EdgeContinuity::Continuous, + AnchorScope::General, + ); + } + + pub fn create_door(&mut self) { + self.create_edges::>(EdgeContinuity::Single, AnchorScope::General) + } + + pub fn create_lift(&mut self) { + self.create_edges::>(EdgeContinuity::Single, AnchorScope::Site) + } + + pub fn create_floor(&mut self) { + self.create_path::>( + create_path_with_texture::>, + 3, + false, + true, + AnchorScope::General, + ); + } + + pub fn create_location(&mut self) { + self.create_point::>(false, AnchorScope::General); + } + + pub fn create_site_fiducial(&mut self) { + self.create_point::>(false, AnchorScope::Site); + } + + pub fn create_drawing_fiducial(&mut self) { + self.create_point::>(false, AnchorScope::Drawing); + } + + pub fn create_edges>>( + &mut self, + continuity: EdgeContinuity, + scope: AnchorScope, + ) { + let state = self + .commands + .spawn(SelectorInput(CreateEdges::new::(continuity, scope))) + .id(); + + self.send(RunSelector { + selector: self.services.create_edges, + input: Some(state), + }); + } + + pub fn create_edges_with_texture>>( + &mut self, + continuity: EdgeContinuity, + scope: AnchorScope, + ) { + let state = self + .commands + .spawn(SelectorInput(CreateEdges::new_with_texture::( + continuity, scope, + ))) + .id(); + + self.send(RunSelector { + selector: self.services.create_edges, + input: Some(state), + }); + } + + pub fn replace_side(&mut self, edge: Entity, side: Side, category: Category) -> bool { + let scope = match category { + Category::Lane | Category::Wall | Category::Door => AnchorScope::General, + Category::Measurement => AnchorScope::Drawing, + Category::Lift => AnchorScope::Site, + _ => return false, + }; + let state = self + .commands + .spawn(SelectorInput(ReplaceSide::new(edge, side, scope))) + .id(); + + self.send(RunSelector { + selector: self.services.replace_side, + input: Some(state), + }); + + true + } + + pub fn create_path>>( + &mut self, + spawn_path: fn(Path, &mut Commands) -> Entity, + minimum_points: usize, + allow_inner_loops: bool, + implied_complete_loop: bool, + scope: AnchorScope, + ) { + let state = self + .commands + .spawn(SelectorInput(CreatePath::new( + spawn_path, + minimum_points, + allow_inner_loops, + implied_complete_loop, + scope, + ))) + .id(); + + self.send(RunSelector { + selector: self.services.create_path, + input: Some(state), + }); + } + + pub fn create_point>>( + &mut self, + repeating: bool, + scope: AnchorScope, + ) { + let state = self + .commands + .spawn(SelectorInput(CreatePoint::new::(repeating, scope))) + .id(); + + self.send(RunSelector { + selector: self.services.create_point, + input: Some(state), + }); + } + + pub fn replace_point(&mut self, point: Entity, scope: AnchorScope) { + let state = self + .commands + .spawn(SelectorInput(ReplacePoint::new(point, scope))) + .id(); + + self.send(RunSelector { + selector: self.services.replace_point, + input: Some(state), + }); + } + + fn send(&mut self, run: RunSelector) { + self.commands.add(move |world: &mut World| { + world.send_event(run); + }); + } +} + +#[derive(Resource, Default)] +pub struct HiddenSelectAnchorEntities { + /// All drawing anchors, hidden when users draw level entities such as walls, lanes, floors to + /// make sure they don't connect to drawing anchors + pub drawing_anchors: HashSet, +} + +/// The first five services should be customized for the State data. The services +/// that return [`SelectionNodeResult`] should return `Ok(())` if it is okay for the +/// workflow to continue as normal, and they should return `Err(None)` if it's +/// time for the workflow to terminate as normal. If the workflow needs to +/// terminate because of an error, return `Err(Some(_))`. +/// +/// In most cases you should use [`AnchorSelectionHelpers::spawn_anchor_selection_workflow`] +/// instead of running this function yourself directly, unless you know that you +/// need to customize the last four services. +/// +/// * `anchor_setup`: This is run once at the start of the workflow to prepare the +/// world to select anchors from the right kind of scope for the request. This +/// is usually just [`anchor_selection_setup`] instantiated for the right type +/// of state. +/// * `state_setup`: This is for any additional custom setup that is relevant to +/// the state information for your selection workflow. This gets run exactly once +/// immediately after `anchor_setup` +/// * `update_preview`: This is run each time a [`Hover`] signal arrives. This +/// is where you should put the logic to update the preview that's being displayed +/// for users. +/// * `update_current`: This is run each time a [`Select`] signal containing `Some` +/// value is sent. This is where you should put the logic to make a persistent +/// (rather than just a preview) modification to the world. +/// * `handle_key_code`: This is where you should put the logic for how your +/// workflow responds to various key codes. For example, should the workflow +/// exit? +/// * `cleanup_state`: This is where you should run anything that's needed to +/// clean up the state of the world after your workflow is finished running. +/// This will be run no matter whether your workflow terminates with a success, +/// terminates with a failure, or cancels prematurely. +/// +/// ### The remaining parameters can all be provided by [`AnchorSelectionHelpers`] in most cases: +/// +/// * `anchor_cursor_transform`: This service should update the 3D cursor transform. +/// A suitable service for this is available from [`AnchorSelectionHelpers`]. +/// * `anchor_select_stream`: This service should produce the [`Hover`] and [`Select`] +/// streams that hook into `update_preview` and `update_current` respectively. +/// A suitable service for this is provided by [`AnchorSelectionHelpers`]. +/// * `keyobard_just_pressed`: This service should produce [`KeyCode`] streams +/// when the keyboard gets pressed. A suitable service for this is provided by +/// [`AnchorSelectionHelpers`]. +/// * `cleanup_anchor_selection`: This service will run during the cleanup phase +/// and should cleanup any anchor-related modifications to the world. A suitable +/// service for this is provided by [`AnchorSelectionHelpers`]. +pub fn build_anchor_selection_workflow( + anchor_setup: Service, SelectionNodeResult>, + state_setup: Service, SelectionNodeResult>, + update_preview: Service<(Hover, BufferKey), SelectionNodeResult>, + update_current: Service<(SelectionCandidate, BufferKey), SelectionNodeResult>, + handle_key_code: Service<(KeyCode, BufferKey), SelectionNodeResult>, + cleanup_state: Service, SelectionNodeResult>, + anchor_cursor_transform: Service<(), ()>, + anchor_select_stream: Service<(), (), (Hover, Select)>, + keyboard_just_pressed: Service<(), (), StreamOf>, + cleanup_anchor_selection: Service<(), ()>, +) -> impl FnOnce(Scope, ()>, &mut Builder) { + move |scope, builder| { + let buffer = builder.create_buffer::(BufferSettings::keep_last(1)); + + let setup_node = builder.create_buffer_access(buffer); + scope + .input + .chain(builder) + .then(extract_selector_input.into_blocking_callback()) + // If the setup failed, then terminate right away. + .branch_for_err(|chain: Chain<_>| chain.connect(scope.terminate)) + .fork_option( + |some: Chain<_>| some.then_push(buffer).connect(setup_node.input), + |none: Chain<_>| none.connect(setup_node.input), + ); + + let begin_input_services = setup_node + .output + .chain(builder) + .map_block(|(_, key)| key) + .then(anchor_setup) + .branch_for_err(|err| err.map_block(print_if_err).connect(scope.terminate)) + .with_access(buffer) + .map_block(|(_, key)| key) + .then(state_setup) + .branch_for_err(|err| err.map_block(print_if_err).connect(scope.terminate)) + .output() + .fork_clone(builder); + + begin_input_services + .clone_chain(builder) + .then(anchor_cursor_transform) + .unused(); + + let select = begin_input_services + .clone_chain(builder) + .then_node(anchor_select_stream); + select + .streams + .0 + .chain(builder) + .with_access(buffer) + .then(update_preview) + .dispose_on_ok() + .map_block(print_if_err) + .connect(scope.terminate); + + select + .streams + .1 + .chain(builder) + .map_block(|s| s.0) + .dispose_on_none() + .with_access(buffer) + .then(update_current) + .dispose_on_ok() + .map_block(print_if_err) + .connect(scope.terminate); + + let keyboard = begin_input_services + .clone_chain(builder) + .then_node(keyboard_just_pressed); + keyboard + .streams + .chain(builder) + .inner() + .with_access(buffer) + .then(handle_key_code) + .dispose_on_ok() + .map_block(print_if_err) + .connect(scope.terminate); + + builder.on_cleanup(buffer, move |scope, builder| { + let state_node = builder.create_node(cleanup_state); + let anchor_node = builder.create_node(cleanup_anchor_selection); + + builder.connect(scope.input, state_node.input); + state_node.output.chain(builder).fork_result( + |ok| ok.connect(anchor_node.input), + |err| err.map_block(print_if_err).connect(anchor_node.input), + ); + + builder.connect(anchor_node.output, scope.terminate); + }); + } +} + +pub fn print_if_err(err: Option) { + if let Some(err) = err { + error!("{err}"); + } +} + +pub fn anchor_selection_setup>( + In(key): In>, + access: BufferAccess, + anchors: Query>, + drawings: Query<(), With>, + parents: Query<&'static Parent>, + mut visibility: Query<&'static mut Visibility>, + mut hidden_anchors: ResMut, + mut current_anchor_scope: ResMut, + mut cursor: ResMut, + mut highlight: ResMut, + mut gizmo_blockers: ResMut, +) -> SelectionNodeResult +where + State: 'static + Send + Sync, +{ + let access = access.get(&key).or_broken_buffer()?; + let state = access.newest().or_broken_state()?; + let scope: &AnchorScope = (&*state).borrow(); + match scope { + AnchorScope::General | AnchorScope::Site => { + // If we are working with normal level or site requests, hide all drawing anchors + for e in anchors + .iter() + .filter(|e| parents.get(*e).is_ok_and(|p| drawings.get(p.get()).is_ok())) + { + set_visibility(e, &mut visibility, false); + hidden_anchors.drawing_anchors.insert(e); + } + } + // Nothing to hide, it's done by the drawing editor plugin + AnchorScope::Drawing => {} + } + + if scope.is_site() { + set_visibility(cursor.site_anchor_placement, &mut visibility, true); + } else { + set_visibility(cursor.level_anchor_placement, &mut visibility, true); + } + + highlight.0 = true; + gizmo_blockers.selecting = true; + + *current_anchor_scope = *scope; + + cursor.add_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + set_visibility(cursor.dagger, &mut visibility, true); + set_visibility(cursor.halo, &mut visibility, true); + + Ok(()) +} + +pub fn cleanup_anchor_selection( + In(_): In<()>, + mut cursor: ResMut, + mut visibility: Query<&mut Visibility>, + mut hidden_anchors: ResMut, + mut anchor_scope: ResMut, + mut highlight: ResMut, + mut gizmo_blockers: ResMut, +) { + cursor.remove_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility); + set_visibility(cursor.site_anchor_placement, &mut visibility, false); + set_visibility(cursor.level_anchor_placement, &mut visibility, false); + for e in hidden_anchors.drawing_anchors.drain() { + set_visibility(e, &mut visibility, true); + } + + highlight.0 = false; + gizmo_blockers.selecting = false; + + *anchor_scope = AnchorScope::General; +} + +pub fn extract_selector_input( + In(e): In>, + world: &mut World, +) -> Result, ()> { + let Some(e) = e else { + // There is no input to provide, so move ahead with the workflow + return Ok(None); + }; + + let Some(mut e_mut) = world.get_entity_mut(e) else { + error!( + "Could not begin selector service because the input entity {e:?} \ + does not exist.", + ); + return Err(()); + }; + + let Some(input) = e_mut.take::>() else { + error!( + "Could not begin selector service because the input entity {e:?} \ + did not contain a value {:?}. This is a bug, please report it.", + std::any::type_name::>(), + ); + return Err(()); + }; + + e_mut.despawn_recursive(); + + Ok(Some(input.0)) +} + +#[derive(SystemParam)] +pub struct AnchorFilter<'w, 's> { + inspect: InspectorFilter<'w, 's>, + anchors: Query<'w, 's, (), With>, + cursor: Res<'w, Cursor>, + anchor_scope: Res<'w, AnchorScope>, + workspace: Res<'w, CurrentWorkspace>, + open_sites: Query<'w, 's, Entity, With>, + transforms: Query<'w, 's, &'static GlobalTransform>, + commands: Commands<'w, 's>, + current_drawing: Res<'w, CurrentEditDrawing>, + drawings: Query<'w, 's, &'static PixelsPerMeter, With>, + parents: Query<'w, 's, &'static Parent>, + levels: Query<'w, 's, (), With>, + current_level: Res<'w, CurrentLevel>, +} + +impl<'w, 's> SelectionFilter for AnchorFilter<'w, 's> { + fn filter_pick(&mut self, select: Entity) -> Option { + self.inspect + .filter_pick(select) + .and_then(|e| self.filter_target(e)) + } + + fn filter_select(&mut self, target: Entity) -> Option { + self.filter_target(target) + } + + fn on_click(&mut self, hovered: Hover) -> Option, - mut hover: EventWriter, - blockers: Option>, - workspace: Res, - open_sites: Query>, - current_drawing: Res, -) { - let mut request = match &*mode { - InteractionMode::SelectAnchor(request) => request.clone(), - _ => { - return; - } - }; - - if mode.is_changed() { - // The mode was changed to this one on this update cycle. We should - // check if something besides an anchor is being hovered, and clear it - // out if it is. - if let Some(hovering) = hovering.0 { - if anchors.contains(hovering) { - params - .cursor - .remove_mode(SELECT_ANCHOR_MODE_LABEL, &mut params.visibility); - } else { - hover.send(Hover(None)); - params - .cursor - .add_mode(SELECT_ANCHOR_MODE_LABEL, &mut params.visibility); - } - } else { - params - .cursor - .add_mode(SELECT_ANCHOR_MODE_LABEL, &mut params.visibility); - } - - // Make the anchor placement component of the cursor visible - if request.site_scope() { - set_visibility( - params.cursor.site_anchor_placement, - &mut params.visibility, - true, - ); - } else { - set_visibility( - params.cursor.level_anchor_placement, - &mut params.visibility, - true, - ); - } - - match request.scope { - Scope::General | Scope::Site => { - // If we are working with normal level or site requests, hide all drawing anchors - for anchor in params.anchors.iter().filter(|(e, _)| { - params - .parents - .get(*e) - .is_ok_and(|p| params.drawings.get(**p).is_ok()) - }) { - set_visibility(anchor.0, &mut params.visibility, false); - params.hidden_entities.drawing_anchors.insert(anchor.0); - } - } - // Nothing to hide, it's done by the drawing editor plugin - Scope::Drawing => {} - } - - // If we are creating a new object, then we should deselect anything - // that might be currently selected. - if request.begin_creating() { - if let Some(previous_selection) = selection.0 { - if let Ok(mut selected) = selected.get_mut(previous_selection) { - selected.is_selected = false; - } - selection.0 = None; - } - } - - if request.continuity.needs_original() { - // Keep track of the original anchor that we intend to replace so - // that we can revert any previews. - let for_element = match request.target { - Some(for_element) => for_element, - None => { - error!( - "for_element must be Some for ReplaceAnchor. \ - Reverting to Inspect Mode." - ); - params.cleanup(); - *mode = InteractionMode::Inspect; - return; - } - }; - - let original = match request.placement.save_original(for_element, &mut params) { - Some(original) => original, - None => { - error!( - "cannot locate an original anchor for \ - entity {:?}. Reverting to Inspect Mode.", - for_element, - ); - params.cleanup(); - *mode = InteractionMode::Inspect; - return; - } - }; - - request.continuity = SelectAnchorContinuity::ReplaceAnchor { - original_anchor: Some(original), - }; - // Save the new mode here in case it doesn't get saved by any - // branches in the rest of this system function. - *mode = InteractionMode::SelectAnchor(request.clone()); - } - } - - if hovering.is_changed() { - if hovering.0.is_none() { - params - .cursor - .add_mode(SELECT_ANCHOR_MODE_LABEL, &mut params.visibility); - } else { - params - .cursor - .remove_mode(SELECT_ANCHOR_MODE_LABEL, &mut params.visibility); - } - } - - if select.is_empty() { - let clicked = mouse_button_input.just_pressed(MouseButton::Left) - || touch_input.iter_just_pressed().next().is_some(); - let blocked = blockers.filter(|x| x.blocking()).is_some(); - - if clicked && !blocked { - // Since the user clicked but there are no actual selections, the - // user is effectively asking to create a new anchor at the current - // cursor location. We will create that anchor and treat it as if it - // were selected. - let tf = match transforms.get(params.cursor.frame) { - Ok(tf) => tf, - Err(_) => { - error!( - "Could not get transform for cursor frame \ - {:?} in SelectAnchor mode.", - params.cursor.frame, - ); - // TODO(MXG): Put in backout behavior here. - return; - } - }; - - let new_anchor = match request.scope { - Scope::Site => { - let site = workspace.to_site(&open_sites).expect("No current site??"); - let new_anchor = params.commands.spawn(AnchorBundle::at_transform(tf)).id(); - params.commands.entity(site).add_child(new_anchor); - new_anchor - } - Scope::Drawing => { - let drawing_entity = current_drawing - .target() - .expect("No drawing while spawning drawing anchor") - .drawing; - let (parent, ppm) = params - .drawings - .get(drawing_entity) - .expect("Entity being edited is not a drawing"); - // We also need to have a transform such that the anchor will spawn in the - // right spot - let pose = compute_parent_inverse_pose(&tf, &transforms, parent); - let ppm = ppm.0; - let new_anchor = params - .commands - .spawn(AnchorBundle::new([pose.trans[0], pose.trans[1]].into())) - .insert(Transform::from_scale(Vec3::new(ppm, ppm, 1.0))) - .set_parent(parent) - .id(); - new_anchor - } - Scope::General => params.commands.spawn(AnchorBundle::at_transform(tf)).id(), - }; - - request = match request.next(AnchorSelection::new(new_anchor), &mut params) { - Some(next_mode) => next_mode, - None => { - params.cleanup(); - *mode = InteractionMode::Inspect; - return; - } - }; - - *mode = InteractionMode::SelectAnchor(request); - } else { - // Offer a preview based on the current hovering status - let hovered = hovering.0.unwrap_or(params.cursor.level_anchor_placement); - let current = request - .target - .map(|target| request.placement.current(target, ¶ms)) - .flatten(); - - if Some(hovered) != current { - // We should only call this function if the current hovered - // anchor is not the one currently assigned. Otherwise we - // are wasting query+command effort. - match request.preview(hovered, &mut params) { - PreviewResult::Updated(next) => { - *mode = InteractionMode::SelectAnchor(next); - } - PreviewResult::Updated3D(next) => { - *mode = InteractionMode::SelectAnchor3D(next); - } - PreviewResult::Unchanged => { - // Do nothing, the mode has not changed - } - PreviewResult::Invalid => { - // Something was invalid about the request, so we - // will exit back to Inspect mode. - params.cleanup(); - *mode = InteractionMode::Inspect; - } - }; - } - } - } else { - for new_selection in select - .read() - .filter_map(|s| s.0) - .filter(|s| anchors.contains(*s)) - { - request = match request.next(AnchorSelection::existing(new_selection), &mut params) { - Some(next_mode) => next_mode, - None => { - params.cleanup(); - *mode = InteractionMode::Inspect; - return; - } - }; - } - - *mode = InteractionMode::SelectAnchor(request); - } -} - -fn compute_parent_inverse_pose( - tf: &GlobalTransform, - transforms: &Query<&GlobalTransform>, - parent: Entity, -) -> Pose { - let parent_tf = transforms - .get(parent) - .expect("Failed in fetching parent transform"); - - let inv_tf = parent_tf.affine().inverse(); - let goal_tf = tf.affine(); - let mut pose = Pose::default(); - pose.rot = pose.rot.as_euler_extrinsic_xyz(); - pose.align_with(&Transform::from_matrix((inv_tf * goal_tf).into())) -} - -pub fn handle_select_anchor_3d_mode( - mut mode: ResMut, - anchors: Query<(), With>, - transforms: Query<&GlobalTransform>, - hovering: Res, - mouse_button_input: Res>, - touch_input: Res, - mut params: SelectAnchorPlacementParams, - selection: Res, - mut select: EventReader