diff --git a/.cargo/config.toml b/.cargo/config.toml new file mode 100644 index 0000000..db11ffd --- /dev/null +++ b/.cargo/config.toml @@ -0,0 +1,3 @@ +[target.x86_64-pc-windows-msvc] +linker = "rust-lld" +rustflags = ["-Zshare-generics=y"] \ No newline at end of file diff --git a/Cargo.lock b/Cargo.lock index 05d31cb..49f53e8 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1,6 +1,6 @@ # This file is automatically @generated by Cargo. # It is not intended for manual editing. -version = 3 +version = 4 [[package]] name = "ab_glyph" @@ -1066,6 +1066,7 @@ dependencies = [ "bevy_eventlistener", "bevy_mod_picking", "bevy_rapier3d", + "roxmltree", "tracing", ] @@ -1344,18 +1345,6 @@ dependencies = [ "thiserror", ] -[[package]] -name = "calloop-wayland-source" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0f0ea9b9476c7fad82841a8dbb380e2eae480c21910feba80725b46931ed8f02" -dependencies = [ - "calloop", - "rustix", - "wayland-backend", - "wayland-client", -] - [[package]] name = "cc" version = "1.0.87" @@ -2512,15 +2501,6 @@ version = "2.7.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "523dc4f511e55ab87b694dc30d0f820d60906ef06413f93d4d7a1385599cc149" -[[package]] -name = "memmap2" -version = "0.9.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fe751422e4a8caa417e13c3ea66452215d7d63e19e604f4980461212f3ae1322" -dependencies = [ - "libc", -] - [[package]] name = "metal" version = "0.27.0" @@ -3134,15 +3114,6 @@ version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "43d84d1d7a6ac92673717f9f6d1518374ef257669c24ebc5ac25d5033828be58" -[[package]] -name = "quick-xml" -version = "0.31.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1004a344b30a54e2ee58d66a71b32d2db2feb0a31f9a2d302bf0536f15de2a33" -dependencies = [ - "memchr", -] - [[package]] name = "quote" version = "1.0.35" @@ -3304,6 +3275,15 @@ dependencies = [ "serde_derive", ] +[[package]] +name = "roxmltree" +version = "0.14.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "921904a62e410e37e215c40381b7117f830d9d89ba60ab5236170541dd25646b" +dependencies = [ + "xmlparser", +] + [[package]] name = "rustc-hash" version = "1.1.0" @@ -3358,31 +3338,12 @@ dependencies = [ "winapi-util", ] -[[package]] -name = "scoped-tls" -version = "1.0.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e1cf6437eb19a8f4a6cc0f7dca544973b0b78843adbfeb3683d1a94a0024a294" - [[package]] name = "scopeguard" version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" -[[package]] -name = "sctk-adwaita" -version = "0.8.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "82b2eaf3a5b264a521b988b2e73042e742df700c4f962cde845d1541adb46550" -dependencies = [ - "ab_glyph", - "log", - "memmap2", - "smithay-client-toolkit", - "tiny-skia", -] - [[package]] name = "serde" version = "1.0.197" @@ -3475,31 +3436,6 @@ dependencies = [ "serde", ] -[[package]] -name = "smithay-client-toolkit" -version = "0.18.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "922fd3eeab3bd820d76537ce8f582b1cf951eceb5475c28500c7457d9d17f53a" -dependencies = [ - "bitflags 2.4.2", - "calloop", - "calloop-wayland-source", - "cursor-icon", - "libc", - "log", - "memmap2", - "rustix", - "thiserror", - "wayland-backend", - "wayland-client", - "wayland-csd-frame", - "wayland-cursor", - "wayland-protocols", - "wayland-protocols-wlr", - "wayland-scanner", - "xkeysym", -] - [[package]] name = "smol_str" version = "0.2.1" @@ -3536,12 +3472,6 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" -[[package]] -name = "strict-num" -version = "0.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6637bab7722d379c8b41ba849228d680cc12d0a45ba1fa2b48f2a30577a06731" - [[package]] name = "svg_fmt" version = "0.4.1" @@ -3635,31 +3565,6 @@ dependencies = [ "once_cell", ] -[[package]] -name = "tiny-skia" -version = "0.11.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "83d13394d44dae3207b52a326c0c85a8bf87f1541f23b0d143811088497b09ab" -dependencies = [ - "arrayref", - "arrayvec", - "bytemuck", - "cfg-if", - "log", - "tiny-skia-path", -] - -[[package]] -name = "tiny-skia-path" -version = "0.11.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c9e7fc0c2e86a30b117d0462aa261b72b7a99b7ebd7deb3a14ceda95c5bdc93" -dependencies = [ - "arrayref", - "bytemuck", - "strict-num", -] - [[package]] name = "tinyvec" version = "1.6.0" @@ -3942,114 +3847,6 @@ version = "0.2.91" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4f186bd2dcf04330886ce82d6f33dd75a7bfcf69ecf5763b89fcde53b6ac9838" -[[package]] -name = "wayland-backend" -version = "0.3.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9d50fa61ce90d76474c87f5fc002828d81b32677340112b4ef08079a9d459a40" -dependencies = [ - "cc", - "downcast-rs", - "rustix", - "scoped-tls", - "smallvec", - "wayland-sys", -] - -[[package]] -name = "wayland-client" -version = "0.31.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "82fb96ee935c2cea6668ccb470fb7771f6215d1691746c2d896b447a00ad3f1f" -dependencies = [ - "bitflags 2.4.2", - "rustix", - "wayland-backend", - "wayland-scanner", -] - -[[package]] -name = "wayland-csd-frame" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "625c5029dbd43d25e6aa9615e88b829a5cad13b2819c4ae129fdbb7c31ab4c7e" -dependencies = [ - "bitflags 2.4.2", - "cursor-icon", - "wayland-backend", -] - -[[package]] -name = "wayland-cursor" -version = "0.31.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "71ce5fa868dd13d11a0d04c5e2e65726d0897be8de247c0c5a65886e283231ba" -dependencies = [ - "rustix", - "wayland-client", - "xcursor", -] - -[[package]] -name = "wayland-protocols" -version = "0.31.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8f81f365b8b4a97f422ac0e8737c438024b5951734506b0e1d775c73030561f4" -dependencies = [ - "bitflags 2.4.2", - "wayland-backend", - "wayland-client", - "wayland-scanner", -] - -[[package]] -name = "wayland-protocols-plasma" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "23803551115ff9ea9bce586860c5c5a971e360825a0309264102a9495a5ff479" -dependencies = [ - "bitflags 2.4.2", - "wayland-backend", - "wayland-client", - "wayland-protocols", - "wayland-scanner", -] - -[[package]] -name = "wayland-protocols-wlr" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ad1f61b76b6c2d8742e10f9ba5c3737f6530b4c243132c2a2ccc8aa96fe25cd6" -dependencies = [ - "bitflags 2.4.2", - "wayland-backend", - "wayland-client", - "wayland-protocols", - "wayland-scanner", -] - -[[package]] -name = "wayland-scanner" -version = "0.31.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "63b3a62929287001986fb58c789dce9b67604a397c15c611ad9f747300b6c283" -dependencies = [ - "proc-macro2", - "quick-xml", - "quote", -] - -[[package]] -name = "wayland-sys" -version = "0.31.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "15a0c8eaff5216d07f226cb7a549159267f3467b289d9a2e52fd3ef5aae2b7af" -dependencies = [ - "dlib", - "log", - "pkg-config", -] - [[package]] name = "web-sys" version = "0.3.67" @@ -4488,7 +4285,6 @@ version = "0.29.10" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4c824f11941eeae66ec71111cc2674373c772f482b58939bb4066b642aa2ffcf" dependencies = [ - "ahash", "android-activity", "atomic-waker", "bitflags 2.4.2", @@ -4502,7 +4298,6 @@ dependencies = [ "js-sys", "libc", "log", - "memmap2", "ndk 0.8.0", "ndk-sys 0.5.0+25.2.9519653", "objc2 0.4.1", @@ -4512,16 +4307,10 @@ dependencies = [ "raw-window-handle 0.6.0", "redox_syscall 0.3.5", "rustix", - "sctk-adwaita", - "smithay-client-toolkit", "smol_str", "unicode-segmentation", "wasm-bindgen", "wasm-bindgen-futures", - "wayland-backend", - "wayland-client", - "wayland-protocols", - "wayland-protocols-plasma", "web-sys", "web-time", "windows-sys 0.48.0", @@ -4571,12 +4360,6 @@ version = "0.13.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e63e71c4b8bd9ffec2c963173a4dc4cbde9ee96961d4fcb4429db9929b606c34" -[[package]] -name = "xcursor" -version = "0.3.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6a0ccd7b4a5345edfcd0c3535718a4e9ff7798ffc536bb5b5a0e26ff84732911" - [[package]] name = "xi-unicode" version = "0.3.0" @@ -4608,6 +4391,12 @@ version = "0.8.19" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0fcb9cbac069e033553e8bb871be2fbdffcab578eb25bd0f7c508cedc6dcd75a" +[[package]] +name = "xmlparser" +version = "0.13.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "66fee0b777b0f5ac1c69bb06d361268faafa61cd4682ae064a171c16c433e9e4" + [[package]] name = "zerocopy" version = "0.7.32" diff --git a/Cargo.toml b/Cargo.toml index 3f4a9a3..7abd6e1 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,8 +4,12 @@ version = "0.1.0" edition = "2021" [dependencies] -bevy = { version = "0.13.2", features = ["wayland", "dynamic_linking", "jpeg"] } +bevy = { version = "0.13.2", features = ["jpeg"] } bevy_eventlistener = { version = "0.7" } bevy_rapier3d = { version = "0.25.0", features = ["debug-render-3d"] } bevy_mod_picking = { version = "0.18.2", default-features = false, features = ["backend_rapier"] } tracing = "0.1" +roxmltree = "0.14.1" + +[target.'cfg(not(target_os = "windows"))'.dependencies] +bevy = { version = "0.13.2", features = ["dynamic_linking", "jpeg"] } \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..c0f6394 --- /dev/null +++ b/README.md @@ -0,0 +1,101 @@ +# Foundation Robotics - Project D: Virtual Camera Sensor + +## Project Description + +This project implements a virtual camera sensor attached to a simulated robot within the Bevy game engine. The primary goal is to render the scene from the robot's point-of-view (POV) into a separate 2D window, simulating the output of a real-world camera sensor used in robotics. + +The simulation involves: +* Setting up a 3D environment with walls and a floor using Bevy primitives. +* Simulating a TurtleBot4 robot model with physics using the `bevy_rapier3d` plugin. +* Creating a dedicated sensor camera entity parented to the robot chassis. +* Rendering the sensor camera's view to a secondary application window. +* Allowing configuration of the sensor camera's intrinsic parameters (like focal length and sensor size) via an external XML file. +* Providing an interactive main camera for debugging and observing the overall scene. + +## Features + +* **Virtual Camera Sensor:** A Bevy `Camera3dBundle` simulates the robot's camera. +* **Bevy Engine Integration:** Built entirely within the Bevy game engine framework using Rust. +* **Physics Simulation:** Uses `bevy_rapier3d` for rigid body dynamics, collision detection, and joints for the robot model. +* **TurtleBot4 Robot Model:** Spawns a TurtleBot4 model with physics-based chassis and wheels. Basic forward motion is implemented via motor joints. +* **Robot Point-of-View (POV):** The sensor camera is attached (parented) to the robot's chassis, ensuring its view updates correctly as the robot moves and rotates (Extrinsic Parameters). +* **Secondary Sensor View Window:** A separate OS window displays *only* the output from the robot's sensor camera. +* **Configurable Intrinsic Parameters:** Camera intrinsics (focal length, sensor dimensions, near/far planes) can be configured via an XML file (`assets/robots/camera_params.xml`). The system updates the camera's projection (FoV, aspect ratio) based on these parameters and the window size. +* **Interactive Main Camera:** A separate pan/orbit/zoom camera (`PanOrbitCamera`) allows users to navigate and view the main simulation scene freely. +* **Basic Arena Environment:** A simple environment with walls and a floor for the robot to navigate. +* **Draggable Robot:** The robot chassis can be dragged using the mouse via `bevy_mod_picking`. + +## How it Works + +The project leverages several key Bevy and Rust concepts: + +1. **Bevy ECS (Entity-Component-System):** The simulation is structured around Bevy's ECS. Entities (like the robot, walls, cameras) have Components (like `Transform`, `Collider`, `SensorCamera`, `PanOrbitCamera`) that store data, and Systems (like `update_camera_system`, `velocity_control`, `update_sensor_camera_projection`) contain the logic that operates on these components. + +2. **Robot Simulation (`turtlebot4.rs`):** + * The `turtlebot4::spawn` function creates the robot. + * A main `chassis_entity` with `RigidBody::Dynamic`, `Collider`, and `ColliderMassProperties` components forms the core physics body. + * Wheels are separate entities with their own physics properties, connected to the chassis via `ImpulseJoint` components (revolute joints). + * The `velocity_control` system applies forces to the wheel joints to simulate basic motor control. + * A GLTF model (`turtlebot4.glb`) is loaded and attached for visuals. + +3. **Sensor Camera Implementation (`sensor_camera.rs`, `turtlebot4.rs`, `main.rs`):** + * **Creation & Parenting:** Inside `turtlebot4::spawn`, a `Camera3dBundle` is spawned *as a child* of the `chassis_entity` using `with_children`. The camera's `Transform` is set relative to the chassis (e.g., slightly forward and up). + * **Transform Propagation:** Because the camera is a child of the chassis, Bevy automatically updates its world `GlobalTransform` whenever the chassis moves or rotates. This ensures the camera always moves *with* the robot, providing the correct POV. + * **Rendering Target:** The `Camera` component's `target` field is set to `RenderTarget::Window(WindowRef::Entity(secondary_window_entity))`. This tells Bevy to render this camera's output specifically to the secondary window entity, which is identified during setup in `main.rs`. + * **Intrinsic Parameters:** + * The `SensorCamera` component holds configurable parameters (focal length, sensor size, near/far, principal point offset). + * The `load_camera_parameters` system reads these values from `assets/robots/camera_params.xml` at startup and stores them in a `CameraParametersResource`. + * The `update_sensor_camera_projection` system reads the `SensorCamera` component attached to the camera entity and the target window's dimensions. + * It calculates the vertical Field of View (FoV) based on sensor height and focal length, and the aspect ratio from the window size. + * It updates the standard Bevy `Projection::Perspective` component on the camera entity with the calculated FoV, aspect ratio, and the near/far values from `SensorCamera`. + * **Limitation:** The `principal_point_offset` is defined but *not* currently used, as this would require implementing a custom projection matrix instead of using Bevy's standard `PerspectiveProjection`. + +4. **Secondary Window (`main.rs`):** + * The `setup_secondary_window` system creates a new Bevy `Window` entity at startup. + * The main `setup` system queries for window entities, identifies the one *without* the `PrimaryWindow` marker component (which Bevy adds automatically to the main window), and gets its `WindowRef` to pass to the robot spawner. + +5. **Main Interactive Camera (`camera.rs`):** + * The `PanOrbitCamera` component holds state for orbiting (focus point, radius) and accumulates mouse input (pan, rotation, scroll). + * The `accumulate_mouse_events_system` reads mouse button, motion, and wheel events and updates the accumulators in `PanOrbitCamera`. + * The `update_camera_system` uses the accumulated inputs, applies smoothing (LERP), and updates the `Transform` component of the main camera entity to achieve panning, orbiting (around the focus point), and zooming (adjusting the radius). + +## Setup and Running + +1. **Install Rust:** If you don't have it, install the Rust toolchain: [https://www.rust-lang.org/tools/install](https://www.rust-lang.org/tools/install) +2. **Clone Repository:** + ```bash + git clone + cd + ``` +3. **Build and Run:** + ```bash + # For development (faster compile, slower runtime) + cargo run + + # For release (slower compile, faster runtime) + cargo run --release + ``` + This will compile the code and launch two windows: the main simulation view and the "Sensor Preview" window showing the robot's POV. + +## Configuration + +The primary configuration point is the camera intrinsic parameters file: + +* **File:** `assets/robots/camera_params.xml` +* **Parameters:** + * ``: Focal length in millimeters. + * ``: Sensor width in millimeters. + * ``: Sensor height in millimeters. + * ``: Horizontal offset of the principal point from the sensor center (in mm). *Currently NOT USED by the projection.* + * ``: Vertical offset of the principal point from the sensor center (in mm). *Currently NOT USED by the projection.* + * ``: Near clipping plane distance. + * ``: Far clipping plane distance. + +Modify the values in this XML file and restart the application to see the effects on the sensor camera's view (primarily FoV and aspect ratio adaptation). + +## Dependencies + +* [Bevy Engine](https://bevyengine.org/): The core game engine framework. +* [Bevy Rapier3D](https://rapier.rs/docs/user_guides/bevy_plugin/): Physics engine integration for Bevy. +* [roxmltree](https://crates.io/crates/roxmltree): Used internally by `sensor_camera.rs` for parsing the XML parameters file. +* [bevy_mod_picking](https://crates.io/crates/bevy_mod_picking): Used for mouse picking/dragging functionality. \ No newline at end of file diff --git a/assets/robots/camera_params.xml b/assets/robots/camera_params.xml new file mode 100644 index 0000000..240ed10 --- /dev/null +++ b/assets/robots/camera_params.xml @@ -0,0 +1,9 @@ + + 35.0 + 36.0 + 24.0 + 0.0 + 0.0 + 0.1 + 1000.0 + \ No newline at end of file diff --git a/src/camera.rs b/src/camera.rs index 2f62c70..bbe3d97 100644 --- a/src/camera.rs +++ b/src/camera.rs @@ -1,18 +1,17 @@ + use bevy::prelude::*; use bevy::input::mouse::{MouseWheel,MouseMotion}; use bevy::render::camera::Projection; use bevy::window::{PrimaryWindow, Window}; -const LERP: f32 = 0.1; -// ANCHOR: example -/// Tags an entity as capable of panning and orbiting. +const LERP_FACTOR: f32 = 0.1; + #[derive(Component)] pub struct PanOrbitCamera { - /// The "focus point" to orbit around. It is automatically updated when panning the camera + pub focus: Vec3, pub radius: f32, pub upside_down: bool, - // These accumulate the events pub pan: Vec2, pub rotation_move: Vec2, pub scroll: f32, @@ -25,7 +24,7 @@ impl Default for PanOrbitCamera { focus: Vec3::ZERO, radius: 5.0, upside_down: false, - // These accumulate the events + pan: Vec2::ZERO, rotation_move: Vec2::ZERO, scroll: 0.0, @@ -38,119 +37,130 @@ pub fn accumulate_mouse_events_system( mut ev_motion: EventReader, mut ev_scroll: EventReader, input_mouse: Res>, - mut query: Query<&mut PanOrbitCamera>, + mut query: Query<&mut PanOrbitCamera>, ) { - // need to accumulate these and apply them to all cameras + let orbit_button = MouseButton::Right; + let pan_button = MouseButton::Middle; + let mut pan = Vec2::ZERO; let mut rotation_move = Vec2::ZERO; let mut scroll = 0.0; let mut orbit_button_changed = false; - - let orbit_button = MouseButton::Right; - let pan_button = MouseButton::Middle; if input_mouse.pressed(orbit_button) { + for ev in ev_motion.read() { rotation_move += ev.delta; } } else if input_mouse.pressed(pan_button) { - // Pan only if we're not rotating at the moment + for ev in ev_motion.read() { pan += ev.delta; } + } else { + + ev_motion.clear(); } + + for ev in ev_scroll.read() { - scroll += ev.y; + scroll += ev.y; } + if input_mouse.just_released(orbit_button) || input_mouse.just_pressed(orbit_button) { orbit_button_changed = true; } + let pan_sensitivity = 1.0; + let rotation_sensitivity = 0.5; + let scroll_sensitivity = 1.0; + for mut camera in query.iter_mut() { - camera.orbit_button_changed |= orbit_button_changed; - camera.pan += 2.0 * pan; - camera.rotation_move += 2.0 * rotation_move; - camera.scroll += 2.0 * scroll; + camera.orbit_button_changed |= orbit_button_changed; + camera.pan += pan * pan_sensitivity; + camera.rotation_move += rotation_move * rotation_sensitivity; + camera.scroll += scroll * scroll_sensitivity; } - ev_motion.clear(); } -/// Pan the camera with middle mouse click, zoom with scroll wheel, orbit with right mouse click. + pub fn update_camera_system( + windows: Query<&Window, With>, + mut query: Query<(&mut PanOrbitCamera, &mut Transform, &Projection)>, ) { + + let Ok(window) = windows.get_single() else { return }; + let window_size = Vec2::new(window.width(), window.height()); + for (mut camera, mut transform, projection) in query.iter_mut() { if camera.orbit_button_changed { - // only check for upside down when orbiting started or ended this frame - // if the camera is "upside" down, panning horizontally would be inverted, so invert the input to make it correct - let up = transform.rotation * Vec3::Y; - camera.upside_down = up.y <= 0.0; - - camera.orbit_button_changed = false; + let up_direction = transform.rotation * Vec3::Y; + camera.upside_down = up_direction.y <= 0.0; + camera.orbit_button_changed = false; } - let mut any = false; - if camera.rotation_move.length_squared() > 0.5 { - any = true; - let rotation_move = camera.rotation_move * LERP; - camera.rotation_move -= rotation_move; + let mut rotation_change = Vec2::ZERO; + let mut pan_change = Vec2::ZERO; + let mut scroll_change = 0.0; + let mut needs_transform_update = false; + + if camera.rotation_move.length_squared() > 0.01 { + needs_transform_update = true; + rotation_change = camera.rotation_move * LERP_FACTOR; + camera.rotation_move -= rotation_change; - let window = get_primary_window_size(&windows); - let delta_x = { - let delta = rotation_move.x / window.x * std::f32::consts::PI * 2.0; - if camera.upside_down { -delta } else { delta } + let delta_yaw = { + let delta = rotation_change.x / window_size.x * std::f32::consts::PI * 2.0; + if camera.upside_down { delta } else { -delta } }; - let delta_y = rotation_move.y / window.y * std::f32::consts::PI; - let yaw = Quat::from_rotation_y(-delta_x); - let pitch = Quat::from_rotation_x(-delta_y); - transform.rotation = yaw * transform.rotation; // rotate around global y axis - transform.rotation *= pitch; // rotate around local x axis - } - - if camera.pan.length_squared() > 0.5 { - any = true; - let mut pan = camera.pan * LERP; - camera.pan -= pan; - // make panning distance independent of resolution and FOV, - let window = get_primary_window_size(&windows); - if let Projection::Perspective(projection) = projection { - pan *= Vec2::new(projection.fov * projection.aspect_ratio, projection.fov) / window; + let delta_pitch = rotation_change.y / window_size.y * std::f32::consts::PI; + + let yaw_quat = Quat::from_rotation_y(delta_yaw); + let pitch_quat = Quat::from_rotation_x(delta_pitch); + transform.rotation = yaw_quat * transform.rotation * pitch_quat; + } + + if camera.pan.length_squared() > 0.01 { + needs_transform_update = true; + + pan_change = camera.pan * LERP_FACTOR; + camera.pan -= pan_change; + + + let mut pan_scaled = pan_change / window_size; + if let Projection::Perspective(persp) = projection { + pan_scaled *= Vec2::new(persp.fov * persp.aspect_ratio, persp.fov); } - // translate by local axes - let right = transform.rotation * Vec3::X * -pan.x; - let up = transform.rotation * Vec3::Y * pan.y; - // make panning proportional to distance away from focus point + + let right = transform.rotation * Vec3::X * -pan_scaled.x; + let up = transform.rotation * Vec3::Y * pan_scaled.y; + let translation = (right + up) * camera.radius; camera.focus += translation; - } - - if camera.scroll.abs() > 0.5 { - any = true; - - let scroll = camera.scroll * LERP; - camera.scroll -= scroll; - camera.radius -= scroll * camera.radius * 0.05; - // dont allow zoom to reach zero or you get stuck + } + + if camera.scroll.abs() > 0.01 { + needs_transform_update = true; + scroll_change = camera.scroll * LERP_FACTOR; + camera.scroll -= scroll_change; + + let scroll_adjusted = scroll_change * 0.1; + camera.radius *= 1.0 - scroll_adjusted; + camera.radius = f32::max(camera.radius, 0.05); } - if any { - // emulating parent/child to make the yaw/y-axis rotation behave like a turntable - // parent = x and y rotation - // child = z-offset - let rot_matrix = Mat3::from_quat(transform.rotation); - transform.translation = camera.focus + rot_matrix.mul_vec3(Vec3::new(0.0, 0.0, camera.radius)); + if needs_transform_update { + + let rotation_matrix = Mat3::from_quat(transform.rotation); + let offset = rotation_matrix.mul_vec3(Vec3::new(0.0, 0.0, camera.radius)); + transform.translation = camera.focus + offset; } } - - // consume any remaining events, so they don't pile up if we don't need them - // (and also to avoid Bevy warning us about not checking events every frame update) - } -fn get_primary_window_size(windows: &Query<&Window, With>) -> Vec2 { - let window = windows.get_single().unwrap(); - Vec2::new(window.width(), window.height()) -} +// Helper function (no longer needed as window query is done in the system) +// fn get_primary_window_size(windows: &Query<&Window, With>) -> Vec2 { ... } \ No newline at end of file diff --git a/src/drag.rs b/src/drag.rs index 286e47a..cdb0b52 100644 --- a/src/drag.rs +++ b/src/drag.rs @@ -3,18 +3,15 @@ use bevy_eventlistener::{callbacks::Listener, event_listener::{EntityEvent, On}} use bevy_mod_picking::{events::{Drag, DragEnd, DragStart, Pointer}, focus, picking_core::Pickable, pointer::PointerButton}; use bevy_rapier3d::dynamics::ExternalImpulse; + #[derive(Component)] pub struct Target { - /// the camera on which this drag is occuring pub camera: Entity, - /// allows calculating the drag target from the mouse pub origin: Vec3, - /// the offset from the center of mass where the drag started pub offset: Vec3, - /// distance of the drag (as last reported by events>) pub distance: Vec2, } @@ -69,15 +66,12 @@ pub fn drag_system( mut drag_events: EventReader>, mut target: Query<(&mut Target, &GlobalTransform, &mut ExternalImpulse)>, camera_transforms: Query<&GlobalTransform, With>, - //mut gizmos: Gizmos ) { if let Ok((mut target, target_transform, mut target_force)) = target.get_single_mut() { - /* update the cached target distance */ if let Some(last_drag_event) = drag_events.read().last() { target.distance = last_drag_event.distance; } - /* convert drag target distance */ let camera_transform = camera_transforms .get(target.camera) .unwrap(); @@ -86,14 +80,11 @@ pub fn drag_system( target.distance.y * camera_transform.up(); drag_target_offset.y = 0.0; - // TODO: improve zoom factor for lower camera altitudes let zoom_factor = (camera_transform.translation() - target.origin).length() * 0.0011; let drag_target = target.origin + (drag_target_offset * zoom_factor); let drag_point = target_transform.transform_point(target.offset); - // TODO: make gain a factor of object weight - const GAIN: f32 = 1.5; - // TODO: use PID control? + const GAIN: f32 = 0.8; let drag_impulse = (drag_target - drag_point) .clamp(Vec3::NEG_ONE, Vec3::ONE) * GAIN; target_force.impulse = drag_impulse; diff --git a/src/main.rs b/src/main.rs index 28b1853..3b9b374 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,21 +1,29 @@ use bevy::prelude::*; +use bevy::render::camera::{Camera, RenderTarget}; +use bevy::window::{PrimaryWindow, Window, WindowRef}; use bevy_mod_picking::DefaultPickingPlugins; +use bevy_rapier3d::prelude::{Damping, Restitution, RigidBody}; +use bevy_rapier3d::prelude::*; + use bevy_rapier3d::{ geometry::{Collider, CollisionGroups, Group}, plugin::{NoUserData, RapierConfiguration, RapierPhysicsPlugin, TimestepMode}, - render::RapierDebugRenderPlugin}; + render::RapierDebugRenderPlugin, +}; mod turtlebot4; mod camera; mod drag; +mod sensor_camera; const STATIC_GROUP: Group = Group::GROUP_1; const CHASSIS_INTERNAL_GROUP: Group = Group::GROUP_2; const CHASSIS_GROUP: Group = Group::GROUP_3; + fn enable_physics( keyboard_input: Res>, - mut rapier: ResMut + mut rapier: ResMut, ) { if keyboard_input.pressed(KeyCode::Space) { rapier.physics_pipeline_active = true; @@ -26,9 +34,54 @@ fn enable_physics( // rapier.query_pipeline_active = false; // } } +#[derive(Component)] +pub struct SphereRobot; + +fn spawn_rolling_sphere( + commands: &mut Commands, + meshes: &mut ResMut>, + materials: &mut ResMut>, + position: Vec3, +) -> Entity { + let sphere_radius = 0.2; + + commands.spawn(( + PbrBundle { + mesh: meshes.add(Sphere::new(sphere_radius)), + material: materials.add(Color::rgb(0.8, 0.2, 0.2)), + transform: Transform::from_translation(position), + ..default() + }, + RigidBody::Dynamic, + Collider::ball(sphere_radius), + ColliderMassProperties::Mass(1.0), + Damping { + linear_damping: 0.1, + angular_damping: 0.3, + }, + Restitution::coefficient(0.3), + Friction::coefficient(0.5), + turtlebot4::RobotVelocity::default(), + SphereRobot, + Name::new("Rolling Sphere"), + )).id() +} + +fn sphere_movement_control( + mut spheres: Query<(&mut ExternalForce, &turtlebot4::RobotVelocity, &Transform), With>, + time: Res