From 108a2a18d689e29990a6610341a91921f30253da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Wed, 5 Mar 2025 14:06:49 +0100 Subject: [PATCH] =?UTF-8?q?feat:=E2=80=AFadd=20PD=20and=20PID=20controller?= =?UTF-8?q?=20implementations=20(#804)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat: add a PID controller implementation * feat: add small rigid-body utilities + test interpolation test * fix: make scrolling weaker on macos * feat: add the option to use the PID controller in the character controller demo. * feat: add a stateless PD controller * feat(rapier_testbed): cleanup & support PidController in 2D too * chore: add comments for the PD and PID controllers * chore: update changelog * feat: rename PidErrors to PdErrors which is more accurate * fix cargo doc * chore: remove dead code * chore: make test module non-pub --- CHANGELOG.md | 29 +- examples2d/all_examples2.rs | 2 + examples2d/character_controller2.rs | 38 ++- examples2d/rope_joints2.rs | 24 +- examples2d/utils/character.rs | 250 ++++++++++++++++ examples2d/utils/mod.rs | 1 + examples3d/all_examples3.rs | 2 + examples3d/character_controller3.rs | 44 ++- examples3d/rope_joints3.rs | 23 +- examples3d/utils/character.rs | 261 ++++++++++++++++ examples3d/utils/mod.rs | 1 + src/control/mod.rs | 2 + src/control/pid_controller.rs | 411 ++++++++++++++++++++++++++ src/dynamics/rigid_body.rs | 27 +- src/dynamics/rigid_body_components.rs | 152 ++++++++-- src_testbed/camera3d.rs | 3 + src_testbed/lib.rs | 2 +- src_testbed/testbed.rs | 192 +++--------- src_testbed/ui.rs | 53 +--- 19 files changed, 1275 insertions(+), 242 deletions(-) create mode 100644 examples2d/utils/character.rs create mode 100644 examples2d/utils/mod.rs create mode 100644 examples3d/utils/character.rs create mode 100644 examples3d/utils/mod.rs create mode 100644 src/control/pid_controller.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index f00bb2f..738d84e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,27 +1,41 @@ +## Unreleased + +### Added + +- Add `PdController` and `PidController` for making it easier to control dynamic rigid-bodies at the velocity level. + This can for example be used as a building block for a dynamic character controller. +- Add `RigidBodyPosition::pose_errors` which computes the translational and rotational delta between + `RigidBodyPosition::position` and `::next_position`. +- Implement `Sub` for `RigidBodyVelocity`. +- Add `RigidBody::local_center_of_mass()` to get its center-of-mass in the rigid-body’s local-space. + ## v0.23.0 (08 Jan 2025) ### Fix -- The broad-phase region key has been replaced by an i64 in the f64 version of rapier, increasing the range before panics occur. +- The broad-phase region key has been replaced by an i64 in the f64 version of rapier, increasing the range before + panics occur. - Fix `BroadphaseMultiSap` not being able to serialize correctly with serde_json. -- Fix `KinematicCharacterController::move_shape` not respecting parameters `max_slope_climb_angle` and `min_slope_slide_angle`. +- Fix `KinematicCharacterController::move_shape` not respecting parameters `max_slope_climb_angle` and + `min_slope_slide_angle`. - Improve ground detection reliability for `KinematicCharacterController`. (#715) - Fix wasm32 default values for physics hooks filter to be consistent with native: `COMPUTE_IMPULSES`. - Fix changing a collider parent when ongoing collisions should be affected (#742): - - Fix collisions not being removed when a collider is parented to a rigidbody while in collision with it. - - Fix collisions not being added when the parent was removed while intersecting a (previously) sibling collider. + - Fix collisions not being removed when a collider is parented to a rigidbody while in collision with it. + - Fix collisions not being added when the parent was removed while intersecting a (previously) sibling collider. ### Added - `RigidBodySet` and `ColliderSet` have a new constructor `with_capacity`. - Use `profiling` crate to provide helpful profiling information in different tools. - - The testbeds have been updated to use `puffin_egui` + - The testbeds have been updated to use `puffin_egui` ### Modified - `InteractionGroups` default value for `memberships` is now `GROUP_1` (#706) - `ImpulseJointSet::get_mut` has a new parameter `wake_up: bool`, to wake up connected bodies. -- Removed unmaintained `instant` in favor of `web-time`. This effectively removes the `wasm-bindgen` transitive dependency as it's no longer needed. +- Removed unmaintained `instant` in favor of `web-time`. This effectively removes the `wasm-bindgen` transitive + dependency as it's no longer needed. - Significantly improve performances of `QueryPipeline::intersection_with_shape`. ## v0.22.0 (20 July 2024) @@ -748,7 +762,8 @@ Several new shape types are now supported: It is possible to build `ColliderDesc` using these new shapes: -- `ColliderBuilder::round_cuboid`, `ColliderBuilder::segment`, `ColliderBuilder::triangle`, `ColliderBuilder::round_triangle`, +- `ColliderBuilder::round_cuboid`, `ColliderBuilder::segment`, `ColliderBuilder::triangle`, + `ColliderBuilder::round_triangle`, `ColliderBuilder::convex_hull`, `ColliderBuilder::round_convex_hull`, `ColliderBuilder::polyline`, `ColliderBuilder::convex_decomposition`, `ColliderBuilder::round_convex_decomposition`, `ColliderBuilder::convex_polyline` (2D only), `ColliderBuilder::round_convex_polyline` (2D only), diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 4926b4e..0bb84ed 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -43,6 +43,8 @@ mod s2d_pyramid; mod sensor2; mod trimesh2; +mod utils; + #[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))] pub fn main() { let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ diff --git a/examples2d/character_controller2.rs b/examples2d/character_controller2.rs index 2acf50f..e95e110 100644 --- a/examples2d/character_controller2.rs +++ b/examples2d/character_controller2.rs @@ -1,3 +1,6 @@ +use crate::utils::character; +use crate::utils::character::CharacterControlMode; +use rapier2d::control::{KinematicCharacterController, PidController}; use rapier2d::prelude::*; use rapier_testbed2d::Testbed; use std::f32::consts::PI; @@ -25,7 +28,12 @@ pub fn init_world(testbed: &mut Testbed) { /* * Character we will control manually. */ - let rigid_body = RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0]); + let rigid_body = RigidBodyBuilder::kinematic_position_based() + .translation(vector![-3.0, 5.0]) + // The two config below makes the character + // nicer to control with the PID control enabled. + .gravity_scale(10.0) + .soft_ccd_prediction(10.0); let character_handle = bodies.insert(rigid_body); let collider = ColliderBuilder::capsule_y(0.3, 0.15); colliders.insert_with_parent(collider, character_handle, &mut bodies); @@ -110,7 +118,7 @@ pub fn init_world(testbed: &mut Testbed) { /* * Create a moving platform. */ - let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5]); + let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 0.0]); // .rotation(-0.3); let platform_handle = bodies.insert(body); let collider = ColliderBuilder::cuboid(2.0, ground_height); @@ -160,10 +168,34 @@ pub fn init_world(testbed: &mut Testbed) { } }); + /* + * Callback to update the character based on user inputs. + */ + let mut control_mode = CharacterControlMode::Kinematic; + let mut controller = KinematicCharacterController { + max_slope_climb_angle: impossible_slope_angle - 0.02, + min_slope_slide_angle: impossible_slope_angle - 0.02, + slide: true, + ..Default::default() + }; + let mut pid = PidController::default(); + + testbed.add_callback(move |graphics, physics, _, _| { + if let Some(graphics) = graphics { + character::update_character( + graphics, + physics, + &mut control_mode, + &mut controller, + &mut pid, + character_handle, + ); + } + }); + /* * Set up the testbed. */ testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); - testbed.set_character_body(character_handle); testbed.look_at(point![0.0, 1.0], 100.0); } diff --git a/examples2d/rope_joints2.rs b/examples2d/rope_joints2.rs index ec9b201..2b9d6b8 100644 --- a/examples2d/rope_joints2.rs +++ b/examples2d/rope_joints2.rs @@ -1,3 +1,6 @@ +use crate::utils::character; +use crate::utils::character::CharacterControlMode; +use rapier2d::control::{KinematicCharacterController, PidController}; use rapier2d::prelude::*; use rapier_testbed2d::Testbed; @@ -54,10 +57,29 @@ pub fn init_world(testbed: &mut Testbed) { let joint = RopeJointBuilder::new(2.0).local_anchor2(point![0.0, 0.0]); impulse_joints.insert(character_handle, child_handle, joint, true); + /* + * Callback to update the character based on user inputs. + */ + let mut control_mode = CharacterControlMode::Kinematic; + let mut controller = KinematicCharacterController::default(); + let mut pid = PidController::default(); + + testbed.add_callback(move |graphics, physics, _, _| { + if let Some(graphics) = graphics { + character::update_character( + graphics, + physics, + &mut control_mode, + &mut controller, + &mut pid, + character_handle, + ); + } + }); + /* * Set up the testbed. */ testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); - testbed.set_character_body(character_handle); testbed.look_at(point![0.0, 1.0], 100.0); } diff --git a/examples2d/utils/character.rs b/examples2d/utils/character.rs new file mode 100644 index 0000000..eb04fda --- /dev/null +++ b/examples2d/utils/character.rs @@ -0,0 +1,250 @@ +use rapier2d::{ + control::{CharacterLength, KinematicCharacterController, PidController}, + prelude::*, +}; +use rapier_testbed2d::ui::egui::Align2; +use rapier_testbed2d::{ + ui::egui::{ComboBox, Slider, Ui, Window}, + KeyCode, PhysicsState, TestbedGraphics, +}; + +#[derive(PartialEq, Eq, Clone, Copy, Debug)] +pub enum CharacterControlMode { + Kinematic, + Pid, +} + +pub fn update_character( + graphics: &mut TestbedGraphics, + physics: &mut PhysicsState, + control_mode: &mut CharacterControlMode, + controller: &mut KinematicCharacterController, + pid: &mut PidController, + character_handle: RigidBodyHandle, +) { + let prev_control_mode = *control_mode; + character_control_ui(graphics, controller, pid, control_mode); + + if *control_mode != prev_control_mode { + match control_mode { + CharacterControlMode::Kinematic => physics.bodies[character_handle] + .set_body_type(RigidBodyType::KinematicPositionBased, false), + CharacterControlMode::Pid => { + physics.bodies[character_handle].set_body_type(RigidBodyType::Dynamic, true) + } + } + } + + match *control_mode { + CharacterControlMode::Kinematic => { + update_kinematic_controller(graphics, physics, character_handle, controller) + } + CharacterControlMode::Pid => { + update_pid_controller(graphics, physics, character_handle, pid) + } + } +} + +fn character_movement_from_inputs( + gfx: &TestbedGraphics, + mut speed: Real, + artificial_gravity: bool, +) -> Vector { + let mut desired_movement = Vector::zeros(); + + for key in gfx.keys().get_pressed() { + match *key { + KeyCode::ArrowRight => { + desired_movement += Vector::x(); + } + KeyCode::ArrowLeft => { + desired_movement -= Vector::x(); + } + KeyCode::Space => { + desired_movement += Vector::y() * 2.0; + } + KeyCode::ControlRight => { + desired_movement -= Vector::y(); + } + KeyCode::ShiftRight => { + speed /= 10.0; + } + _ => {} + } + } + + desired_movement *= speed; + + if artificial_gravity { + desired_movement -= Vector::y() * speed; + } + + desired_movement +} + +fn update_pid_controller( + gfx: &mut TestbedGraphics, + phx: &mut PhysicsState, + character_handle: RigidBodyHandle, + pid: &mut PidController, +) { + let desired_movement = character_movement_from_inputs(gfx, 0.1, false); + let character_body = &mut phx.bodies[character_handle]; + + // Adjust the controlled axis depending on the keys pressed by the user. + // - If the user is jumping, enable control over Y. + // - If the user isn’t pressing any key, disable all linear controls to let + // gravity/collision do their thing freely. + let mut axes = AxisMask::ANG_Z; + + if desired_movement.norm() != 0.0 { + axes |= if desired_movement.y == 0.0 { + AxisMask::LIN_X + } else { + AxisMask::LIN_X | AxisMask::LIN_Y + } + }; + + pid.set_axes(axes); + + let corrective_vel = pid.rigid_body_correction( + phx.integration_parameters.dt, + character_body, + (character_body.translation() + desired_movement).into(), + RigidBodyVelocity::zero(), + ); + let new_vel = *character_body.vels() + corrective_vel; + + character_body.set_vels(new_vel, true); +} + +fn update_kinematic_controller( + gfx: &mut TestbedGraphics, + phx: &mut PhysicsState, + character_handle: RigidBodyHandle, + controller: &KinematicCharacterController, +) { + let speed = 0.1; + let desired_movement = character_movement_from_inputs(gfx, speed, true); + + let character_body = &phx.bodies[character_handle]; + let character_collider = &phx.colliders[character_body.colliders()[0]]; + let character_mass = character_body.mass(); + + let mut collisions = vec![]; + let mvt = controller.move_shape( + phx.integration_parameters.dt, + &phx.bodies, + &phx.colliders, + &phx.query_pipeline, + character_collider.shape(), + character_collider.position(), + desired_movement.cast::(), + QueryFilter::new().exclude_rigid_body(character_handle), + |c| collisions.push(c), + ); + + if mvt.grounded { + gfx.set_body_color(character_handle, [0.1, 0.8, 0.1]); + } else { + gfx.set_body_color(character_handle, [0.8, 0.1, 0.1]); + } + + controller.solve_character_collision_impulses( + phx.integration_parameters.dt, + &mut phx.bodies, + &phx.colliders, + &phx.query_pipeline, + character_collider.shape(), + character_mass, + &*collisions, + QueryFilter::new().exclude_rigid_body(character_handle), + ); + + let character_body = &mut phx.bodies[character_handle]; + let pose = character_body.position(); + character_body.set_next_kinematic_translation(pose.translation.vector + mvt.translation); +} + +fn character_control_ui( + gfx: &mut TestbedGraphics, + character_controller: &mut KinematicCharacterController, + pid_controller: &mut PidController, + control_mode: &mut CharacterControlMode, +) { + Window::new("Character Control") + .anchor(Align2::RIGHT_TOP, [-15.0, 15.0]) + .show(gfx.ui_context_mut().ctx_mut(), |ui| { + ComboBox::from_label("control mode") + .selected_text(format!("{:?}", *control_mode)) + .show_ui(ui, |ui| { + ui.selectable_value(control_mode, CharacterControlMode::Kinematic, "Kinematic"); + ui.selectable_value(control_mode, CharacterControlMode::Pid, "Pid"); + }); + + match control_mode { + CharacterControlMode::Kinematic => { + kinematic_control_ui(ui, character_controller); + } + CharacterControlMode::Pid => { + pid_control_ui(ui, pid_controller); + } + } + }); +} + +fn pid_control_ui(ui: &mut Ui, pid_controller: &mut PidController) { + let mut lin_kp = pid_controller.pd.lin_kp.x; + let mut lin_ki = pid_controller.lin_ki.x; + let mut lin_kd = pid_controller.pd.lin_kd.x; + let mut ang_kp = pid_controller.pd.ang_kp; + let mut ang_ki = pid_controller.ang_ki; + let mut ang_kd = pid_controller.pd.ang_kd; + + ui.add(Slider::new(&mut lin_kp, 0.0..=100.0).text("linear Kp")); + ui.add(Slider::new(&mut lin_ki, 0.0..=10.0).text("linear Ki")); + ui.add(Slider::new(&mut lin_kd, 0.0..=1.0).text("linear Kd")); + ui.add(Slider::new(&mut ang_kp, 0.0..=100.0).text("angular Kp")); + ui.add(Slider::new(&mut ang_ki, 0.0..=10.0).text("angular Ki")); + ui.add(Slider::new(&mut ang_kd, 0.0..=1.0).text("angular Kd")); + + pid_controller.pd.lin_kp.fill(lin_kp); + pid_controller.lin_ki.fill(lin_ki); + pid_controller.pd.lin_kd.fill(lin_kd); + pid_controller.pd.ang_kp = ang_kp; + pid_controller.ang_ki = ang_ki; + pid_controller.pd.ang_kd = ang_kd; +} + +fn kinematic_control_ui(ui: &mut Ui, character_controller: &mut KinematicCharacterController) { + ui.checkbox(&mut character_controller.slide, "slide") + .on_hover_text("Should the character try to slide against the floor if it hits it?"); + #[allow(clippy::useless_conversion)] + { + ui.add(Slider::new(&mut character_controller.max_slope_climb_angle, 0.0..=std::f32::consts::TAU.into()).text("max_slope_climb_angle")) + .on_hover_text("The maximum angle (radians) between the floor’s normal and the `up` vector that the character is able to climb."); + ui.add(Slider::new(&mut character_controller.min_slope_slide_angle, 0.0..=std::f32::consts::FRAC_PI_2.into()).text("min_slope_slide_angle")) + .on_hover_text("The minimum angle (radians) between the floor’s normal and the `up` vector before the character starts to slide down automatically."); + } + let mut is_snapped = character_controller.snap_to_ground.is_some(); + if ui.checkbox(&mut is_snapped, "snap_to_ground").changed { + match is_snapped { + true => { + character_controller.snap_to_ground = Some(CharacterLength::Relative(0.1)); + } + false => { + character_controller.snap_to_ground = None; + } + } + } + if let Some(snapped) = &mut character_controller.snap_to_ground { + match snapped { + CharacterLength::Relative(val) => { + ui.add(Slider::new(val, 0.0..=10.0).text("Snapped Relative Character Length")); + } + CharacterLength::Absolute(val) => { + ui.add(Slider::new(val, 0.0..=10.0).text("Snapped Absolute Character Length")); + } + } + } +} diff --git a/examples2d/utils/mod.rs b/examples2d/utils/mod.rs new file mode 100644 index 0000000..7889122 --- /dev/null +++ b/examples2d/utils/mod.rs @@ -0,0 +1 @@ +pub mod character; diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index a49414e..56451a6 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -6,6 +6,8 @@ use wasm_bindgen::prelude::*; use rapier_testbed3d::{Testbed, TestbedApp}; use std::cmp::Ordering; +mod utils; + mod ccd3; mod collision_groups3; mod compound3; diff --git a/examples3d/character_controller3.rs b/examples3d/character_controller3.rs index ce96bf8..189ac3f 100644 --- a/examples3d/character_controller3.rs +++ b/examples3d/character_controller3.rs @@ -1,4 +1,8 @@ -use rapier3d::{control::KinematicCharacterController, prelude::*}; +use crate::utils::character::{self, CharacterControlMode}; +use rapier3d::{ + control::{KinematicCharacterController, PidController}, + prelude::*, +}; use rapier_testbed3d::Testbed; pub fn init_world(testbed: &mut Testbed) { @@ -40,8 +44,12 @@ pub fn init_world(testbed: &mut Testbed) { /* * Character we will control manually. */ - let rigid_body = - RigidBodyBuilder::kinematic_position_based().translation(vector![0.0, 0.5, 0.0] * scale); + let rigid_body = RigidBodyBuilder::kinematic_position_based() + .translation(vector![0.0, 0.5, 0.0] * scale) + // The two config below makes the character + // nicer to control with the PID control enabled. + .gravity_scale(10.0) + .soft_ccd_prediction(10.0); let character_handle = bodies.insert(rigid_body); let collider = ColliderBuilder::capsule_y(0.3 * scale, 0.15 * scale); // 0.15, 0.3, 0.15); colliders.insert_with_parent(collider, character_handle, &mut bodies); @@ -124,7 +132,7 @@ pub fn init_world(testbed: &mut Testbed) { * Create a moving platform. */ let body = - RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0] * scale); + RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 0.0, 0.0] * scale); // .rotation(-0.3); let platform_handle = bodies.insert(body); let collider = ColliderBuilder::cuboid(2.0 * scale, ground_height * scale, 2.0 * scale); @@ -177,15 +185,33 @@ pub fn init_world(testbed: &mut Testbed) { }); /* - * Set up the testbed. + * Callback to update the character based on user inputs. */ - testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); - testbed.set_character_body(character_handle); - testbed.set_character_controller(Some(KinematicCharacterController { + let mut control_mode = CharacterControlMode::Kinematic; + let mut controller = KinematicCharacterController { max_slope_climb_angle: impossible_slope_angle - 0.02, min_slope_slide_angle: impossible_slope_angle - 0.02, slide: true, ..Default::default() - })); + }; + let mut pid = PidController::default(); + + testbed.add_callback(move |graphics, physics, _, _| { + if let Some(graphics) = graphics { + character::update_character( + graphics, + physics, + &mut control_mode, + &mut controller, + &mut pid, + character_handle, + ); + } + }); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); testbed.look_at(point!(10.0, 10.0, 10.0), Point::origin()); } diff --git a/examples3d/rope_joints3.rs b/examples3d/rope_joints3.rs index 11f0716..2b114da 100644 --- a/examples3d/rope_joints3.rs +++ b/examples3d/rope_joints3.rs @@ -1,3 +1,5 @@ +use crate::utils::character::{self, CharacterControlMode}; +use rapier3d::control::{KinematicCharacterController, PidController}; use rapier3d::prelude::*; use rapier_testbed3d::Testbed; @@ -83,10 +85,29 @@ pub fn init_world(testbed: &mut Testbed) { let joint = RopeJointBuilder::new(2.0); impulse_joints.insert(character_handle, child_handle, joint, true); + /* + * Callback to update the character based on user inputs. + */ + let mut control_mode = CharacterControlMode::Kinematic; + let mut controller = KinematicCharacterController::default(); + let mut pid = PidController::default(); + + testbed.add_callback(move |graphics, physics, _, _| { + if let Some(graphics) = graphics { + character::update_character( + graphics, + physics, + &mut control_mode, + &mut controller, + &mut pid, + character_handle, + ); + } + }); + /* * Set up the testbed. */ testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); - testbed.set_character_body(character_handle); testbed.look_at(point![10.0, 10.0, 10.0], point![0.0, 0.0, 0.0]); } diff --git a/examples3d/utils/character.rs b/examples3d/utils/character.rs new file mode 100644 index 0000000..5f65b70 --- /dev/null +++ b/examples3d/utils/character.rs @@ -0,0 +1,261 @@ +use rapier3d::{ + control::{CharacterLength, KinematicCharacterController, PidController}, + prelude::*, +}; +use rapier_testbed3d::{ + ui::egui::{Align2, ComboBox, Slider, Ui, Window}, + KeyCode, PhysicsState, TestbedGraphics, +}; + +#[derive(PartialEq, Eq, Clone, Copy, Debug)] +pub enum CharacterControlMode { + Kinematic, + Pid, +} + +pub fn update_character( + graphics: &mut TestbedGraphics, + physics: &mut PhysicsState, + control_mode: &mut CharacterControlMode, + controller: &mut KinematicCharacterController, + pid: &mut PidController, + character_handle: RigidBodyHandle, +) { + let prev_control_mode = *control_mode; + character_control_ui(graphics, controller, pid, control_mode); + + if *control_mode != prev_control_mode { + match control_mode { + CharacterControlMode::Kinematic => physics.bodies[character_handle] + .set_body_type(RigidBodyType::KinematicPositionBased, false), + CharacterControlMode::Pid => { + physics.bodies[character_handle].set_body_type(RigidBodyType::Dynamic, true) + } + } + } + + match *control_mode { + CharacterControlMode::Kinematic => { + update_kinematic_controller(graphics, physics, character_handle, controller) + } + CharacterControlMode::Pid => { + update_pid_controller(graphics, physics, character_handle, pid) + } + } +} + +fn character_movement_from_inputs( + gfx: &TestbedGraphics, + mut speed: Real, + artificial_gravity: bool, +) -> Vector { + let mut desired_movement = Vector::zeros(); + + let rot = gfx.camera_rotation(); + let mut rot_x = rot * Vector::x(); + let mut rot_z = rot * Vector::z(); + rot_x.y = 0.0; + rot_z.y = 0.0; + + for key in gfx.keys().get_pressed() { + match *key { + KeyCode::ArrowRight => { + desired_movement += rot_x; + } + KeyCode::ArrowLeft => { + desired_movement -= rot_x; + } + KeyCode::ArrowUp => { + desired_movement -= rot_z; + } + KeyCode::ArrowDown => { + desired_movement += rot_z; + } + KeyCode::Space => { + desired_movement += Vector::y() * 2.0; + } + KeyCode::ControlRight => { + desired_movement -= Vector::y(); + } + KeyCode::ShiftLeft => { + speed /= 10.0; + } + _ => {} + } + } + + desired_movement *= speed; + + if artificial_gravity { + desired_movement -= Vector::y() * speed; + } + + desired_movement +} + +fn update_pid_controller( + gfx: &mut TestbedGraphics, + phx: &mut PhysicsState, + character_handle: RigidBodyHandle, + pid: &mut PidController, +) { + let desired_movement = character_movement_from_inputs(gfx, 0.1, false); + let character_body = &mut phx.bodies[character_handle]; + + // Adjust the controlled axis depending on the keys pressed by the user. + // - If the user is jumping, enable control over Y. + // - If the user isn’t pressing any key, disable all linear controls to let + // gravity/collision do their thing freely. + let mut axes = AxisMask::ANG_X | AxisMask::ANG_Y | AxisMask::ANG_Z; + + if desired_movement.norm() != 0.0 { + axes |= if desired_movement.y == 0.0 { + AxisMask::LIN_X | AxisMask::LIN_Z + } else { + AxisMask::LIN_X | AxisMask::LIN_Z | AxisMask::LIN_Y + } + }; + + pid.set_axes(axes); + + let corrective_vel = pid.rigid_body_correction( + phx.integration_parameters.dt, + character_body, + (character_body.translation() + desired_movement).into(), + RigidBodyVelocity::zero(), + ); + let new_vel = *character_body.vels() + corrective_vel; + + character_body.set_vels(new_vel, true); +} + +fn update_kinematic_controller( + gfx: &mut TestbedGraphics, + phx: &mut PhysicsState, + character_handle: RigidBodyHandle, + controller: &KinematicCharacterController, +) { + let speed = 0.1; + let desired_movement = character_movement_from_inputs(gfx, speed, true); + + let character_body = &phx.bodies[character_handle]; + let character_collider = &phx.colliders[character_body.colliders()[0]]; + let character_mass = character_body.mass(); + + let mut collisions = vec![]; + let mvt = controller.move_shape( + phx.integration_parameters.dt, + &phx.bodies, + &phx.colliders, + &phx.query_pipeline, + character_collider.shape(), + character_collider.position(), + desired_movement.cast::(), + QueryFilter::new().exclude_rigid_body(character_handle), + |c| collisions.push(c), + ); + + if mvt.grounded { + gfx.set_body_color(character_handle, [0.1, 0.8, 0.1]); + } else { + gfx.set_body_color(character_handle, [0.8, 0.1, 0.1]); + } + + controller.solve_character_collision_impulses( + phx.integration_parameters.dt, + &mut phx.bodies, + &phx.colliders, + &phx.query_pipeline, + character_collider.shape(), + character_mass, + &*collisions, + QueryFilter::new().exclude_rigid_body(character_handle), + ); + + let character_body = &mut phx.bodies[character_handle]; + let pose = character_body.position(); + character_body.set_next_kinematic_translation(pose.translation.vector + mvt.translation); +} + +fn character_control_ui( + gfx: &mut TestbedGraphics, + character_controller: &mut KinematicCharacterController, + pid_controller: &mut PidController, + control_mode: &mut CharacterControlMode, +) { + Window::new("Character Control") + .anchor(Align2::RIGHT_TOP, [-15.0, 15.0]) + .show(gfx.ui_context_mut().ctx_mut(), |ui| { + ComboBox::from_label("control mode") + .selected_text(format!("{:?}", *control_mode)) + .show_ui(ui, |ui| { + ui.selectable_value(control_mode, CharacterControlMode::Kinematic, "Kinematic"); + ui.selectable_value(control_mode, CharacterControlMode::Pid, "Pid"); + }); + + match control_mode { + CharacterControlMode::Kinematic => { + kinematic_control_ui(ui, character_controller); + } + CharacterControlMode::Pid => { + pid_control_ui(ui, pid_controller); + } + } + }); +} + +fn pid_control_ui(ui: &mut Ui, pid_controller: &mut PidController) { + let mut lin_kp = pid_controller.pd.lin_kp.x; + let mut lin_ki = pid_controller.lin_ki.x; + let mut lin_kd = pid_controller.pd.lin_kd.x; + let mut ang_kp = pid_controller.pd.ang_kp.x; + let mut ang_ki = pid_controller.ang_ki.x; + let mut ang_kd = pid_controller.pd.ang_kd.x; + + ui.add(Slider::new(&mut lin_kp, 0.0..=100.0).text("linear Kp")); + ui.add(Slider::new(&mut lin_ki, 0.0..=10.0).text("linear Ki")); + ui.add(Slider::new(&mut lin_kd, 0.0..=1.0).text("linear Kd")); + ui.add(Slider::new(&mut ang_kp, 0.0..=100.0).text("angular Kp")); + ui.add(Slider::new(&mut ang_ki, 0.0..=10.0).text("angular Ki")); + ui.add(Slider::new(&mut ang_kd, 0.0..=1.0).text("angular Kd")); + + pid_controller.pd.lin_kp.fill(lin_kp); + pid_controller.lin_ki.fill(lin_ki); + pid_controller.pd.lin_kd.fill(lin_kd); + pid_controller.pd.ang_kp.fill(ang_kp); + pid_controller.ang_ki.fill(ang_ki); + pid_controller.pd.ang_kd.fill(ang_kd); +} + +fn kinematic_control_ui(ui: &mut Ui, character_controller: &mut KinematicCharacterController) { + ui.checkbox(&mut character_controller.slide, "slide") + .on_hover_text("Should the character try to slide against the floor if it hits it?"); + #[allow(clippy::useless_conversion)] + { + ui.add(Slider::new(&mut character_controller.max_slope_climb_angle, 0.0..=std::f32::consts::TAU.into()).text("max_slope_climb_angle")) + .on_hover_text("The maximum angle (radians) between the floor’s normal and the `up` vector that the character is able to climb."); + ui.add(Slider::new(&mut character_controller.min_slope_slide_angle, 0.0..=std::f32::consts::FRAC_PI_2.into()).text("min_slope_slide_angle")) + .on_hover_text("The minimum angle (radians) between the floor’s normal and the `up` vector before the character starts to slide down automatically."); + } + let mut is_snapped = character_controller.snap_to_ground.is_some(); + if ui.checkbox(&mut is_snapped, "snap_to_ground").changed { + match is_snapped { + true => { + character_controller.snap_to_ground = Some(CharacterLength::Relative(0.1)); + } + false => { + character_controller.snap_to_ground = None; + } + } + } + if let Some(snapped) = &mut character_controller.snap_to_ground { + match snapped { + CharacterLength::Relative(val) => { + ui.add(Slider::new(val, 0.0..=10.0).text("Snapped Relative Character Length")); + } + CharacterLength::Absolute(val) => { + ui.add(Slider::new(val, 0.0..=10.0).text("Snapped Absolute Character Length")); + } + } + } +} diff --git a/examples3d/utils/mod.rs b/examples3d/utils/mod.rs new file mode 100644 index 0000000..7889122 --- /dev/null +++ b/examples3d/utils/mod.rs @@ -0,0 +1 @@ +pub mod character; diff --git a/src/control/mod.rs b/src/control/mod.rs index 3f7dea1..15983e0 100644 --- a/src/control/mod.rs +++ b/src/control/mod.rs @@ -4,11 +4,13 @@ pub use self::character_controller::{ CharacterAutostep, CharacterCollision, CharacterLength, EffectiveCharacterMovement, KinematicCharacterController, }; +pub use self::pid_controller::{PdController, PdErrors, PidController}; #[cfg(feature = "dim3")] pub use self::ray_cast_vehicle_controller::{DynamicRayCastVehicleController, Wheel, WheelTuning}; mod character_controller; +mod pid_controller; #[cfg(feature = "dim3")] mod ray_cast_vehicle_controller; diff --git a/src/control/pid_controller.rs b/src/control/pid_controller.rs new file mode 100644 index 0000000..3b62423 --- /dev/null +++ b/src/control/pid_controller.rs @@ -0,0 +1,411 @@ +use crate::dynamics::{AxisMask, RigidBody, RigidBodyPosition, RigidBodyVelocity}; +use crate::math::{Isometry, Point, Real, Rotation, Vector}; +use parry::math::AngVector; + +/// A Proportional-Derivative (PD) controller. +/// +/// This is useful for controlling a rigid-body at the velocity level so it matches a target +/// pose. +/// +/// This is a [PID controller](https://en.wikipedia.org/wiki/Proportional%E2%80%93integral%E2%80%93derivative_controller) +/// without the Integral part to keep the API immutable, while having a behaviour generally +/// sufficient for games. +#[derive(Debug, Copy, Clone, PartialEq)] +pub struct PdController { + /// The Proportional gain applied to the instantaneous linear position errors. + /// + /// This is usually set to a multiple of the inverse of simulation step time + /// (e.g. `60` if the delta-time is `1.0 / 60.0`). + pub lin_kp: Vector, + /// The Derivative gain applied to the instantaneous linear velocity errors. + /// + /// This is usually set to a value in `[0.0, 1.0]` where `0.0` implies no damping + /// (no correction of velocity errors) and `1.0` implies complete damping (velocity errors + /// are corrected in a single simulation step). + pub lin_kd: Vector, + /// The Proportional gain applied to the instantaneous angular position errors. + /// + /// This is usually set to a multiple of the inverse of simulation step time + /// (e.g. `60` if the delta-time is `1.0 / 60.0`). + pub ang_kp: AngVector, + /// The Derivative gain applied to the instantaneous angular velocity errors. + /// + /// This is usually set to a value in `[0.0, 1.0]` where `0.0` implies no damping + /// (no correction of velocity errors) and `1.0` implies complete damping (velocity errors + /// are corrected in a single simulation step). + pub ang_kd: AngVector, + /// The axes affected by this controller. + /// + /// Only coordinate axes with a bit flags set to `true` will be taken into + /// account when calculating the errors and corrections. + pub axes: AxisMask, +} + +impl Default for PdController { + fn default() -> Self { + Self::new(60.0, 0.8, AxisMask::all()) + } +} + +/// A Proportional-Integral-Derivative (PID) controller. +/// +/// For video games, the Proportional-Derivative [`PdController`] is generally sufficient and +/// offers an immutable API. +#[derive(Debug, Copy, Clone, PartialEq)] +pub struct PidController { + /// The Proportional-Derivative (PD) part of this PID controller. + pub pd: PdController, + /// The translational error accumulated through time for the Integral part of the PID controller. + pub lin_integral: Vector, + /// The angular error accumulated through time for the Integral part of the PID controller. + pub ang_integral: AngVector, + /// The linear gain applied to the Integral part of the PID controller. + pub lin_ki: Vector, + /// The angular gain applied to the Integral part of the PID controller. + pub ang_ki: AngVector, +} + +impl Default for PidController { + fn default() -> Self { + Self::new(60.0, 1.0, 0.8, AxisMask::all()) + } +} + +/// Position or velocity errors measured for PID control. +pub struct PdErrors { + /// The linear (translational) part of the error. + pub linear: Vector, + /// The angular (rotational) part of the error. + pub angular: AngVector, +} + +impl From for PdErrors { + fn from(vels: RigidBodyVelocity) -> Self { + Self { + linear: vels.linvel, + angular: vels.angvel, + } + } +} + +impl PdController { + /// Initialized the PD controller with uniform gain. + /// + /// The same gain are applied on all axes. To configure per-axes gains, construct + /// the [`PdController`] by setting its fields explicitly instead. + /// + /// Only the axes specified in `axes` will be enabled (but the gain values are set + /// on all axes regardless). + pub fn new(kp: Real, kd: Real, axes: AxisMask) -> PdController { + #[cfg(feature = "dim2")] + return Self { + lin_kp: Vector::repeat(kp), + lin_kd: Vector::repeat(kd), + ang_kp: kp, + ang_kd: kd, + axes, + }; + + #[cfg(feature = "dim3")] + return Self { + lin_kp: Vector::repeat(kp), + lin_kd: Vector::repeat(kd), + ang_kp: AngVector::repeat(kp), + ang_kd: AngVector::repeat(kd), + axes, + }; + } + + /// Calculates the linear correction from positional and velocity errors calculated automatically + /// from a rigid-body and the desired positions/velocities. + /// + /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to + /// the inverse of the simulation step so the returned value is a linear rigid-body velocity + /// change. + pub fn linear_rigid_body_correction( + &self, + rb: &RigidBody, + target_pos: Point, + target_linvel: Vector, + ) -> Vector { + self.rigid_body_correction( + rb, + Isometry::from(target_pos), + RigidBodyVelocity { + linvel: target_linvel, + #[allow(clippy::clone_on_copy)] + angvel: rb.angvel().clone(), + }, + ) + .linvel + } + + /// Calculates the angular correction from positional and velocity errors calculated automatically + /// from a rigid-body and the desired positions/velocities. + /// + /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to + /// the inverse of the simulation step so the returned value is an angular rigid-body velocity + /// change. + pub fn angular_rigid_body_correction( + &self, + rb: &RigidBody, + target_rot: Rotation, + target_angvel: AngVector, + ) -> AngVector { + self.rigid_body_correction( + rb, + Isometry::from_parts(na::one(), target_rot), + RigidBodyVelocity { + linvel: *rb.linvel(), + angvel: target_angvel, + }, + ) + .angvel + } + + /// Calculates the linear and angular correction from positional and velocity errors calculated + /// automatically from a rigid-body and the desired poses/velocities. + /// + /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to + /// the inverse of the simulation step so the returned value is a rigid-body velocity + /// change. + pub fn rigid_body_correction( + &self, + rb: &RigidBody, + target_pose: Isometry, + target_vels: RigidBodyVelocity, + ) -> RigidBodyVelocity { + let pose_errors = RigidBodyPosition { + position: rb.pos.position, + next_position: target_pose, + } + .pose_errors(rb.local_center_of_mass()); + let vels_errors = target_vels - rb.vels; + self.correction(&pose_errors, &vels_errors.into()) + } + + /// Mask where each component is 1.0 or 0.0 depending on whether + /// the corresponding linear axis is enabled. + fn lin_mask(&self) -> Vector { + #[cfg(feature = "dim2")] + return Vector::new( + self.axes.contains(AxisMask::LIN_X) as u32 as Real, + self.axes.contains(AxisMask::LIN_Y) as u32 as Real, + ); + #[cfg(feature = "dim3")] + return Vector::new( + self.axes.contains(AxisMask::LIN_X) as u32 as Real, + self.axes.contains(AxisMask::LIN_Y) as u32 as Real, + self.axes.contains(AxisMask::LIN_Z) as u32 as Real, + ); + } + + /// Mask where each component is 1.0 or 0.0 depending on whether + /// the corresponding angular axis is enabled. + fn ang_mask(&self) -> AngVector { + #[cfg(feature = "dim2")] + return self.axes.contains(AxisMask::ANG_Z) as u32 as Real; + #[cfg(feature = "dim3")] + return Vector::new( + self.axes.contains(AxisMask::ANG_X) as u32 as Real, + self.axes.contains(AxisMask::ANG_Y) as u32 as Real, + self.axes.contains(AxisMask::ANG_Z) as u32 as Real, + ); + } + + /// Calculates the linear and angular correction from the given positional and velocity errors. + /// + /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to + /// the inverse of the simulation step so the returned value is a rigid-body velocity + /// change. + pub fn correction(&self, pose_errors: &PdErrors, vel_errors: &PdErrors) -> RigidBodyVelocity { + let lin_mask = self.lin_mask(); + let ang_mask = self.ang_mask(); + + RigidBodyVelocity { + linvel: (pose_errors.linear.component_mul(&self.lin_kp) + + vel_errors.linear.component_mul(&self.lin_kd)) + .component_mul(&lin_mask), + #[cfg(feature = "dim2")] + angvel: (pose_errors.angular * self.ang_kp + vel_errors.angular * self.ang_kd) + * ang_mask, + #[cfg(feature = "dim3")] + angvel: (pose_errors.angular.component_mul(&self.ang_kp) + + vel_errors.angular.component_mul(&self.ang_kd)) + .component_mul(&ang_mask), + } + } +} + +impl PidController { + /// Initialized the PDI controller with uniform gain. + /// + /// The same gain are applied on all axes. To configure per-axes gains, construct + /// the [`PidController`] by setting its fields explicitly instead. + /// + /// Only the axes specified in `axes` will be enabled (but the gain values are set + /// on all axes regardless). + pub fn new(kp: Real, ki: Real, kd: Real, axes: AxisMask) -> PidController { + #[cfg(feature = "dim2")] + return Self { + pd: PdController::new(kp, kd, axes), + lin_integral: na::zero(), + ang_integral: na::zero(), + lin_ki: Vector::repeat(ki), + ang_ki: ki, + }; + + #[cfg(feature = "dim3")] + return Self { + pd: PdController::new(kp, kd, axes), + lin_integral: na::zero(), + ang_integral: na::zero(), + lin_ki: Vector::repeat(ki), + ang_ki: AngVector::repeat(ki), + }; + } + + /// Set the axes errors and corrections are computed for. + /// + /// This doesn’t modify any of the gains. + pub fn set_axes(&mut self, axes: AxisMask) { + self.pd.axes = axes; + } + + /// Get the axes errors and corrections are computed for. + pub fn axes(&self) -> AxisMask { + self.pd.axes + } + + /// Resets to zero the accumulated linear and angular errors used by + /// the Integral part of the controller. + pub fn reset_integrals(&mut self) { + self.lin_integral = na::zero(); + self.ang_integral = na::zero(); + } + + /// Calculates the linear correction from positional and velocity errors calculated automatically + /// from a rigid-body and the desired positions/velocities. + /// + /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to + /// the inverse of the simulation step so the returned value is a linear rigid-body velocity + /// change. + /// + /// This method is mutable because of the need to update the accumulated positional + /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if + /// an immutable API is needed. + pub fn linear_rigid_body_correction( + &mut self, + dt: Real, + rb: &RigidBody, + target_pos: Point, + target_linvel: Vector, + ) -> Vector { + self.rigid_body_correction( + dt, + rb, + Isometry::from(target_pos), + RigidBodyVelocity { + linvel: target_linvel, + #[allow(clippy::clone_on_copy)] + angvel: rb.angvel().clone(), + }, + ) + .linvel + } + + /// Calculates the angular correction from positional and velocity errors calculated automatically + /// from a rigid-body and the desired positions/velocities. + /// + /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to + /// the inverse of the simulation step so the returned value is an angular rigid-body velocity + /// change. + /// + /// This method is mutable because of the need to update the accumulated positional + /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if + /// an immutable API is needed. + pub fn angular_rigid_body_correction( + &mut self, + dt: Real, + rb: &RigidBody, + target_rot: Rotation, + target_angvel: AngVector, + ) -> AngVector { + self.rigid_body_correction( + dt, + rb, + Isometry::from_parts(na::one(), target_rot), + RigidBodyVelocity { + linvel: *rb.linvel(), + #[allow(clippy::clone_on_copy)] + angvel: target_angvel.clone(), + }, + ) + .angvel + } + + /// Calculates the linear and angular correction from positional and velocity errors calculated + /// automatically from a rigid-body and the desired poses/velocities. + /// + /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to + /// the inverse of the simulation step so the returned value is a rigid-body velocity + /// change. + /// + /// This method is mutable because of the need to update the accumulated positional + /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if + /// an immutable API is needed. + pub fn rigid_body_correction( + &mut self, + dt: Real, + rb: &RigidBody, + target_pose: Isometry, + target_vels: RigidBodyVelocity, + ) -> RigidBodyVelocity { + let pose_errors = RigidBodyPosition { + position: rb.pos.position, + next_position: target_pose, + } + .pose_errors(rb.local_center_of_mass()); + let vels_errors = target_vels - rb.vels; + self.correction(dt, &pose_errors, &vels_errors.into()) + } + + /// Calculates the linear and angular correction from the given positional and velocity errors. + /// + /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to + /// the inverse of the simulation step so the returned value is a rigid-body velocity + /// change. + /// + /// This method is mutable because of the need to update the accumulated positional + /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if + /// an immutable API is needed. + pub fn correction( + &mut self, + dt: Real, + pose_errors: &PdErrors, + vel_errors: &PdErrors, + ) -> RigidBodyVelocity { + self.lin_integral += pose_errors.linear * dt; + self.ang_integral += pose_errors.angular * dt; + + let lin_mask = self.pd.lin_mask(); + let ang_mask = self.pd.ang_mask(); + + RigidBodyVelocity { + linvel: (pose_errors.linear.component_mul(&self.pd.lin_kp) + + vel_errors.linear.component_mul(&self.pd.lin_kd) + + self.lin_integral.component_mul(&self.lin_ki)) + .component_mul(&lin_mask), + #[cfg(feature = "dim2")] + angvel: (pose_errors.angular * self.pd.ang_kp + + vel_errors.angular * self.pd.ang_kd + + self.ang_integral * self.ang_ki) + * ang_mask, + #[cfg(feature = "dim3")] + angvel: (pose_errors.angular.component_mul(&self.pd.ang_kp) + + vel_errors.angular.component_mul(&self.pd.ang_kd) + + self.ang_integral.component_mul(&self.ang_ki)) + .component_mul(&ang_mask), + } + } +} diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 60f403e..3323f01 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -1,3 +1,5 @@ +#[cfg(doc)] +use super::IntegrationParameters; use crate::dynamics::{ LockedAxes, MassProperties, RigidBodyActivation, RigidBodyAdditionalMassProps, RigidBodyCcd, RigidBodyChanges, RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces, @@ -10,9 +12,6 @@ use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector}; use crate::utils::SimdCross; use num::Zero; -#[cfg(doc)] -use super::IntegrationParameters; - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A rigid body. /// @@ -237,6 +236,12 @@ impl RigidBody { &self.mprops.world_com } + /// The local-space center-of-mass of this rigid-body. + #[inline] + pub fn local_center_of_mass(&self) -> &Point { + &self.mprops.local_mprops.local_com + } + /// The mass-properties of this rigid-body. #[inline] pub fn mass_properties(&self) -> &RigidBodyMassProps { @@ -704,6 +709,11 @@ impl RigidBody { !self.vels.linvel.is_zero() || !self.vels.angvel.is_zero() } + /// The linear and angular velocity of this rigid-body. + pub fn vels(&self) -> &RigidBodyVelocity { + &self.vels + } + /// The linear velocity of this rigid-body. pub fn linvel(&self) -> &Vector { &self.vels.linvel @@ -721,6 +731,15 @@ impl RigidBody { &self.vels.angvel } + /// Set both the angular and linear velocity of this rigid-body. + /// + /// If `wake_up` is `true` then the rigid-body will be woken up if it was + /// put to sleep because it did not move for a while. + pub fn set_vels(&mut self, vels: RigidBodyVelocity, wake_up: bool) { + self.set_linvel(vels.linvel, wake_up); + self.set_angvel(vels.angvel, wake_up); + } + /// The linear velocity of this rigid-body. /// /// If `wake_up` is `true` then the rigid-body will be woken up if it was @@ -1481,7 +1500,7 @@ impl RigidBodyBuilder { /// Build a new rigid-body with the parameters configured with this builder. pub fn build(&self) -> RigidBody { let mut rb = RigidBody::new(); - rb.pos.next_position = self.position; // FIXME: compute the correct value? + rb.pos.next_position = self.position; rb.pos.position = self.position; rb.vels.linvel = self.linvel; rb.vels.angvel = self.angvel; diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index e451510..a34e7b2 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -1,3 +1,6 @@ +#[cfg(doc)] +use super::IntegrationParameters; +use crate::control::PdErrors; use crate::dynamics::MassProperties; use crate::geometry::{ ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, @@ -11,7 +14,7 @@ use crate::utils::{SimdAngularInertia, SimdCross, SimdDot}; use num::Zero; #[cfg(doc)] -use super::IntegrationParameters; +use crate::control::PidController; /// The unique handle of a rigid body added to a `RigidBodySet`. #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash, Default)] @@ -159,22 +162,11 @@ impl RigidBodyPosition { /// a time equal to `1.0 / inv_dt`. #[must_use] pub fn interpolate_velocity(&self, inv_dt: Real, local_com: &Point) -> RigidBodyVelocity { - let com = self.position * local_com; - let shift = Translation::from(com.coords); - let dpos = shift.inverse() * self.next_position * self.position.inverse() * shift; - - let angvel; - #[cfg(feature = "dim2")] - { - angvel = dpos.rotation.angle() * inv_dt; + let pose_err = self.pose_errors(local_com); + RigidBodyVelocity { + linvel: pose_err.linear * inv_dt, + angvel: pose_err.angular * inv_dt, } - #[cfg(feature = "dim3")] - { - angvel = dpos.rotation.scaled_axis() * inv_dt; - } - let linvel = dpos.translation.vector * inv_dt; - - RigidBodyVelocity { linvel, angvel } } /// Compute new positions after integrating the given forces and velocities. @@ -191,6 +183,32 @@ impl RigidBodyPosition { let new_vels = forces.integrate(dt, vels, mprops); new_vels.integrate(dt, &self.position, &mprops.local_mprops.local_com) } + + /// Computes the difference between [`Self::next_position`] and [`Self::position`]. + /// + /// This error measure can for example be used for interpolating the velocity between two poses, + /// or be given to the [`PidController`]. + /// + /// Note that interpolating the velocity can be done more conveniently with + /// [`Self::interpolate_velocity`]. + pub fn pose_errors(&self, local_com: &Point) -> PdErrors { + let com = self.position * local_com; + let shift = Translation::from(com.coords); + let dpos = shift.inverse() * self.next_position * self.position.inverse() * shift; + + let angular; + #[cfg(feature = "dim2")] + { + angular = dpos.rotation.angle(); + } + #[cfg(feature = "dim3")] + { + angular = dpos.rotation.scaled_axis(); + } + let linear = dpos.translation.vector; + + PdErrors { linear, angular } + } } impl From for RigidBodyPosition @@ -210,7 +228,34 @@ bitflags::bitflags! { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, PartialEq, Eq, Debug)] /// Flags affecting the behavior of the constraints solver for a given contact manifold. - // FIXME: rename this to LockedAxes + pub struct AxisMask: u8 { + /// The translational X axis. + const LIN_X = 1 << 0; + /// The translational Y axis. + const LIN_Y = 1 << 1; + /// The translational Z axis. + const LIN_Z = 1 << 2; + /// The rotational X axis. + #[cfg(feature = "dim3")] + const ANG_X = 1 << 3; + /// The rotational Y axis. + #[cfg(feature = "dim3")] + const ANG_Y = 1 << 4; + /// The rotational Z axis. + const ANG_Z = 1 << 5; + } +} + +impl Default for AxisMask { + fn default() -> Self { + AxisMask::empty() + } +} + +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + #[derive(Copy, Clone, PartialEq, Eq, Debug)] + /// Flags affecting the behavior of the constraints solver for a given contact manifold. pub struct LockedAxes: u8 { /// Flag indicating that the rigid-body cannot translate along the `X` axis. const TRANSLATION_LOCKED_X = 1 << 0; @@ -720,6 +765,25 @@ impl std::ops::AddAssign for RigidBodyVelocity { } } +impl std::ops::Sub for RigidBodyVelocity { + type Output = Self; + + #[must_use] + fn sub(self, rhs: Self) -> Self { + RigidBodyVelocity { + linvel: self.linvel - rhs.linvel, + angvel: self.angvel - rhs.angvel, + } + } +} + +impl std::ops::SubAssign for RigidBodyVelocity { + fn sub_assign(&mut self, rhs: Self) { + self.linvel -= rhs.linvel; + self.angvel -= rhs.angvel; + } +} + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone, Debug, Copy, PartialEq)] /// Damping factors to progressively slow down a rigid-body. @@ -1092,3 +1156,57 @@ impl RigidBodyActivation { self.time_since_can_sleep = self.time_until_sleep; } } + +#[cfg(test)] +mod tests { + use super::*; + use crate::math::Real; + + #[test] + fn test_interpolate_velocity() { + // Interpolate and then integrate the velocity to see if + // the end positions match. + #[cfg(feature = "f32")] + let mut rng = oorandom::Rand32::new(0); + #[cfg(feature = "f64")] + let mut rng = oorandom::Rand64::new(0); + + for i in -10..=10 { + let mult = i as Real; + let (local_com, curr_pos, next_pos); + #[cfg(feature = "dim2")] + { + local_com = Point::new(rng.rand_float(), rng.rand_float()); + curr_pos = Isometry::new( + Vector::new(rng.rand_float(), rng.rand_float()) * mult, + rng.rand_float(), + ); + next_pos = Isometry::new( + Vector::new(rng.rand_float(), rng.rand_float()) * mult, + rng.rand_float(), + ); + } + #[cfg(feature = "dim3")] + { + local_com = Point::new(rng.rand_float(), rng.rand_float(), rng.rand_float()); + curr_pos = Isometry::new( + Vector::new(rng.rand_float(), rng.rand_float(), rng.rand_float()) * mult, + Vector::new(rng.rand_float(), rng.rand_float(), rng.rand_float()), + ); + next_pos = Isometry::new( + Vector::new(rng.rand_float(), rng.rand_float(), rng.rand_float()) * mult, + Vector::new(rng.rand_float(), rng.rand_float(), rng.rand_float()), + ); + } + + let dt = 0.016; + let rb_pos = RigidBodyPosition { + position: curr_pos, + next_position: next_pos, + }; + let vel = rb_pos.interpolate_velocity(1.0 / dt, &local_com); + let interp_pos = vel.integrate(dt, &curr_pos, &local_com); + approx::assert_relative_eq!(interp_pos, next_pos, epsilon = 1.0e-5); + } + } +} diff --git a/src_testbed/camera3d.rs b/src_testbed/camera3d.rs index ce35d8d..d1ecda3 100644 --- a/src_testbed/camera3d.rs +++ b/src_testbed/camera3d.rs @@ -9,6 +9,9 @@ use bevy::prelude::*; use bevy::render::camera::Camera; use std::ops::RangeInclusive; +#[cfg(target_os = "macos")] +const LINE_TO_PIXEL_RATIO: f32 = 0.0005; +#[cfg(not(target_os = "macos"))] const LINE_TO_PIXEL_RATIO: f32 = 0.1; #[derive(Component, PartialEq, Debug, Clone, serde::Serialize, serde::Deserialize)] diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs index 1eec06f..1bbc465 100644 --- a/src_testbed/lib.rs +++ b/src_testbed/lib.rs @@ -26,7 +26,7 @@ mod plugin; mod save; mod settings; mod testbed; -mod ui; +pub mod ui; #[cfg(feature = "dim2")] pub mod math { diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index ce60235..45f494b 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -8,15 +8,16 @@ use std::num::NonZeroUsize; use crate::debug_render::{DebugRenderPipelineResource, RapierDebugRenderPlugin}; use crate::graphics::BevyMaterialComponent; +use crate::mouse::{self, track_mouse_state, MainCamera, SceneMouse}; use crate::physics::{DeserializedPhysicsSnapshot, PhysicsEvents, PhysicsSnapshot, PhysicsState}; use crate::plugin::TestbedPlugin; +use crate::save::SerializableTestbedState; +use crate::settings::ExampleSettings; +use crate::ui; use crate::{graphics::GraphicsManager, harness::RunState}; -use crate::{mouse, ui}; +use bevy::window::PrimaryWindow; use na::{self, Point2, Point3, Vector3}; -#[cfg(feature = "dim3")] -use rapier::control::DynamicRayCastVehicleController; -use rapier::control::KinematicCharacterController; use rapier::dynamics::{ ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyActivation, RigidBodyHandle, RigidBodySet, @@ -25,7 +26,9 @@ use rapier::dynamics::{ use rapier::geometry::Ray; use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase}; use rapier::math::{Real, Vector}; -use rapier::pipeline::{PhysicsHooks, QueryFilter, QueryPipeline}; +use rapier::pipeline::{PhysicsHooks, QueryPipeline}; +#[cfg(feature = "dim3")] +use rapier::{control::DynamicRayCastVehicleController, prelude::QueryFilter}; #[cfg(all(feature = "dim2", feature = "other-backends"))] use crate::box2d_backend::Box2dWorld; @@ -113,8 +116,6 @@ pub struct TestbedState { pub running: RunMode, pub draw_colls: bool, pub highlighted_body: Option, - pub character_body: Option, - pub character_controller: Option, #[cfg(feature = "dim3")] pub vehicle_controller: Option, // pub grabbed_object: Option, @@ -177,7 +178,7 @@ struct OtherBackends { } struct Plugins(Vec>); -pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> { +pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f, 'g, 'h> { graphics: &'a mut GraphicsManager, commands: &'a mut Commands<'d, 'e>, meshes: &'a mut Assets, @@ -186,12 +187,13 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> { #[allow(dead_code)] // Dead in 2D but not in 3D. camera_transform: GlobalTransform, camera: &'a mut OrbitCamera, + ui_context: &'a mut EguiContexts<'g, 'h>, keys: &'a ButtonInput, mouse: &'a SceneMouse, } -pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f> { - graphics: Option>, +pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f, 'g, 'h> { + graphics: Option>, harness: &'a mut Harness, state: &'a mut TestbedState, #[cfg(feature = "other-backends")] @@ -227,8 +229,6 @@ impl TestbedApp { running: RunMode::Running, draw_colls: false, highlighted_body: None, - character_body: None, - character_controller: None, #[cfg(feature = "dim3")] vehicle_controller: None, // grabbed_object: None, @@ -508,11 +508,15 @@ impl TestbedApp { } } -impl TestbedGraphics<'_, '_, '_, '_, '_, '_> { +impl<'g, 'h> TestbedGraphics<'_, '_, '_, '_, '_, '_, 'g, 'h> { pub fn set_body_color(&mut self, body: RigidBodyHandle, color: [f32; 3]) { self.graphics.set_body_color(self.materials, body, color); } + pub fn ui_context_mut(&mut self) -> &mut EguiContexts<'g, 'h> { + &mut *self.ui_context + } + pub fn add_body( &mut self, handle: RigidBodyHandle, @@ -564,25 +568,28 @@ impl TestbedGraphics<'_, '_, '_, '_, '_, '_> { self.mouse } + #[cfg(feature = "dim3")] + pub fn camera_rotation(&self) -> na::UnitQuaternion { + let (_, rot, _) = self.camera_transform.to_scale_rotation_translation(); + na::Unit::new_unchecked(na::Quaternion::new( + rot.w as Real, + rot.x as Real, + rot.y as Real, + rot.z as Real, + )) + } + #[cfg(feature = "dim3")] pub fn camera_fwd_dir(&self) -> Vector { (self.camera_transform * -Vec3::Z).normalize().into() } } -impl Testbed<'_, '_, '_, '_, '_, '_> { +impl Testbed<'_, '_, '_, '_, '_, '_, '_, '_> { pub fn set_number_of_steps_per_frame(&mut self, nsteps: usize) { self.state.nsteps = nsteps } - pub fn set_character_body(&mut self, handle: RigidBodyHandle) { - self.state.character_body = Some(handle); - } - - pub fn set_character_controller(&mut self, controller: Option) { - self.state.character_controller = controller; - } - #[cfg(feature = "dim3")] pub fn set_vehicle_controller(&mut self, controller: DynamicRayCastVehicleController) { self.state.vehicle_controller = Some(controller); @@ -648,7 +655,6 @@ impl Testbed<'_, '_, '_, '_, '_, '_> { .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true); self.state.highlighted_body = None; - self.state.character_body = None; #[cfg(feature = "dim3")] { self.state.vehicle_controller = None; @@ -808,133 +814,6 @@ impl Testbed<'_, '_, '_, '_, '_, '_> { } } - fn update_character_controller(&mut self, events: &ButtonInput) { - if self.state.running == RunMode::Stop { - return; - } - - if let Some(character_handle) = self.state.character_body { - let mut desired_movement = Vector::zeros(); - let mut speed = 0.1; - - #[cfg(feature = "dim2")] - for key in events.get_pressed() { - match *key { - KeyCode::ArrowRight => { - desired_movement += Vector::x(); - } - KeyCode::ArrowLeft => { - desired_movement -= Vector::x(); - } - KeyCode::Space => { - desired_movement += Vector::y() * 2.0; - } - KeyCode::ControlRight => { - desired_movement -= Vector::y(); - } - KeyCode::ShiftRight => { - speed /= 10.0; - } - _ => {} - } - } - - #[cfg(feature = "dim3")] - { - let (_, rot, _) = self - .graphics - .as_ref() - .unwrap() - .camera_transform - .to_scale_rotation_translation(); - let rot = na::Unit::new_unchecked(na::Quaternion::new(rot.w, rot.x, rot.y, rot.z)); - let mut rot_x = rot * Vector::x(); - let mut rot_z = rot * Vector::z(); - rot_x.y = 0.0; - rot_z.y = 0.0; - - for key in events.get_pressed() { - match *key { - KeyCode::ArrowRight => { - desired_movement += rot_x; - } - KeyCode::ArrowLeft => { - desired_movement -= rot_x; - } - KeyCode::ArrowUp => { - desired_movement -= rot_z; - } - KeyCode::ArrowDown => { - desired_movement += rot_z; - } - KeyCode::Space => { - desired_movement += Vector::y() * 2.0; - } - KeyCode::ControlRight => { - desired_movement -= Vector::y(); - } - KeyCode::ShiftLeft => { - speed /= 10.0; - } - _ => {} - } - } - } - - desired_movement *= speed; - desired_movement -= Vector::y() * speed; - - let controller = self.state.character_controller.unwrap_or_default(); - let phx = &mut self.harness.physics; - let character_body = &phx.bodies[character_handle]; - let character_collider = &phx.colliders[character_body.colliders()[0]]; - let character_mass = character_body.mass(); - - let mut collisions = vec![]; - let mvt = controller.move_shape( - phx.integration_parameters.dt, - &phx.bodies, - &phx.colliders, - &phx.query_pipeline, - character_collider.shape(), - character_collider.position(), - desired_movement.cast::(), - QueryFilter::new().exclude_rigid_body(character_handle), - |c| collisions.push(c), - ); - if let Some(graphics) = &mut self.graphics { - if mvt.grounded { - graphics.graphics.set_body_color( - graphics.materials, - character_handle, - [0.1, 0.8, 0.1], - ); - } else { - graphics.graphics.set_body_color( - graphics.materials, - character_handle, - [0.8, 0.1, 0.1], - ); - } - } - controller.solve_character_collision_impulses( - phx.integration_parameters.dt, - &mut phx.bodies, - &phx.colliders, - &phx.query_pipeline, - character_collider.shape(), - character_mass, - &*collisions, - QueryFilter::new().exclude_rigid_body(character_handle), - ); - - let character_body = &mut phx.bodies[character_handle]; - let pos = character_body.position(); - character_body.set_next_kinematic_translation(pos.translation.vector + mvt.translation); - // character_body.set_translation(pos.translation.vector + mvt.translation, false); - } - } - fn handle_common_events(&mut self, events: &ButtonInput) { // C can be used to write within profiling filter. if events.pressed(KeyCode::ControlLeft) || events.pressed(KeyCode::ControlRight) { @@ -1212,11 +1091,6 @@ fn egui_focus(mut ui_context: EguiContexts, mut cameras: Query<&mut OrbitCamera> } } -use crate::mouse::{track_mouse_state, MainCamera, SceneMouse}; -use crate::save::SerializableTestbedState; -use crate::settings::ExampleSettings; -use bevy::window::PrimaryWindow; - #[allow(clippy::type_complexity)] fn update_testbed( mut commands: Commands, @@ -1248,6 +1122,8 @@ fn update_testbed( // Handle inputs { + let wants_keyboard_inputs = ui_context.ctx_mut().wants_keyboard_input(); + let graphics_context = TestbedGraphics { graphics: &mut graphics, commands: &mut commands, @@ -1256,6 +1132,7 @@ fn update_testbed( components: &mut gfx_components, camera_transform: *cameras.single().1, camera: &mut cameras.single_mut().2, + ui_context: &mut ui_context, keys: &keys, mouse: &mouse, }; @@ -1269,10 +1146,9 @@ fn update_testbed( plugins: &mut plugins, }; - if !ui_context.ctx_mut().wants_keyboard_input() { + if !wants_keyboard_inputs { testbed.handle_common_events(&keys); } - testbed.update_character_controller(&keys); #[cfg(feature = "dim3")] { testbed.update_vehicle_controller(&keys); @@ -1371,6 +1247,7 @@ fn update_testbed( components: &mut gfx_components, camera_transform: *cameras.single().1, camera: &mut cameras.single_mut().2, + ui_context: &mut ui_context, keys: &keys, mouse: &mouse, }; @@ -1545,6 +1422,7 @@ fn update_testbed( components: &mut gfx_components, camera_transform: *cameras.single().1, camera: &mut cameras.single_mut().2, + ui_context: &mut ui_context, keys: &keys, mouse: &mouse, }; diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 05fe1e2..54d4f5e 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -1,4 +1,3 @@ -use rapier::control::CharacterLength; use rapier::counters::Counters; use rapier::math::Real; use std::num::NonZeroUsize; @@ -10,14 +9,16 @@ use crate::testbed::{ PHYSX_BACKEND_PATCH_FRICTION, PHYSX_BACKEND_TWO_FRICTION_DIR, }; +pub use bevy_egui::egui; + use crate::settings::SettingValue; use crate::PhysicsState; -use bevy_egui::egui::{Slider, Ui}; -use bevy_egui::{egui, EguiContexts}; +use bevy_egui::egui::{ComboBox, Slider, Ui, Window}; +use bevy_egui::EguiContexts; use rapier::dynamics::IntegrationParameters; use web_time::Instant; -pub fn update_ui( +pub(crate) fn update_ui( ui_context: &mut EguiContexts, state: &mut TestbedState, harness: &mut Harness, @@ -30,10 +31,10 @@ pub fn update_ui( example_settings_ui(ui_context, state); - egui::Window::new("Parameters").show(ui_context.ctx_mut(), |ui| { + Window::new("Parameters").show(ui_context.ctx_mut(), |ui| { if state.backend_names.len() > 1 && !state.example_names.is_empty() { let mut changed = false; - egui::ComboBox::from_label("backend") + ComboBox::from_label("backend") .width(150.0) .selected_text(state.backend_names[state.selected_backend]) .show_ui(ui, |ui| { @@ -247,46 +248,14 @@ pub fn update_ui( ui.checkbox(&mut debug_render.enabled, "debug render enabled"); state.flags.set(TestbedStateFlags::SLEEP, sleep); - state.flags.set(TestbedStateFlags::DRAW_SURFACES, draw_surfaces); + state + .flags + .set(TestbedStateFlags::DRAW_SURFACES, draw_surfaces); // state // .flags // .set(TestbedStateFlags::CONTACT_POINTS, contact_points); // state.flags.set(TestbedStateFlags::WIREFRAME, wireframe); ui.separator(); - if let Some(character_controller) = &mut state.character_controller { - ui.label("Character controller"); - ui.checkbox(&mut character_controller.slide, "slide").on_hover_text("Should the character try to slide against the floor if it hits it?"); - #[allow(clippy::useless_conversion)] - { - - ui.add(Slider::new(&mut character_controller.max_slope_climb_angle, 0.0..=std::f32::consts::TAU.into()).text("max_slope_climb_angle")) - .on_hover_text("The maximum angle (radians) between the floor’s normal and the `up` vector that the character is able to climb."); - ui.add(Slider::new(&mut character_controller.min_slope_slide_angle, 0.0..=std::f32::consts::FRAC_PI_2.into()).text("min_slope_slide_angle")) - .on_hover_text("The minimum angle (radians) between the floor’s normal and the `up` vector before the character starts to slide down automatically."); - } - let mut is_snapped = character_controller.snap_to_ground.is_some(); - if ui.checkbox(&mut is_snapped, "snap_to_ground").changed { - match is_snapped { - true => { - character_controller.snap_to_ground = Some(CharacterLength::Relative(0.1)); - }, - false => { - character_controller.snap_to_ground = None; - }, - } - } - if let Some(snapped) = &mut character_controller.snap_to_ground { - match snapped { - CharacterLength::Relative(val) => { - ui.add(Slider::new(val, 0.0..=10.0).text("Snapped Relative Character Length")); - }, - CharacterLength::Absolute(val) => { - ui.add(Slider::new(val, 0.0..=10.0).text("Snapped Absolute Character Length")); - }, - } - } - ui.separator(); - } let label = if state.running == RunMode::Stop { "Start (T)" } else { @@ -465,7 +434,7 @@ fn example_settings_ui(ui_context: &mut EguiContexts, state: &mut TestbedState) return; } - egui::Window::new("Example settings").show(ui_context.ctx_mut(), |ui| { + Window::new("Example settings").show(ui_context.ctx_mut(), |ui| { let mut any_changed = false; for (name, value) in state.example_settings.iter_mut() { let prev_value = value.clone();