nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add PD and PID controller implementations (#804)

Browse Source 
* 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
This commit is contained in:
Sébastien Crozet
2025-03-05 14:06:49 +01:00
committed by GitHub
parent 955795dfbb
commit 108a2a18d6
19 changed files with 1275 additions and 242 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src_testbed/camera3d.rs
View File

@@ -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)]

2
src_testbed/lib.rs
View File

@@ -26,7 +26,7 @@ mod plugin;
mod save;
mod settings;
mod testbed;
mod ui;
pub mod ui;
#[cfg(feature = "dim2")]
pub mod math {

192
src_testbed/testbed.rs
View File

@@ -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<RigidBodyHandle>,
pub character_body: Option<RigidBodyHandle>,
pub character_controller: Option<KinematicCharacterController>,
#[cfg(feature = "dim3")]
pub vehicle_controller: Option<DynamicRayCastVehicleController>,
// pub grabbed_object: Option<DefaultBodyPartHandle>,
@@ -177,7 +178,7 @@ struct OtherBackends {
}
struct Plugins(Vec<Box<dyn TestbedPlugin>>);
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<Mesh>,
@@ -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<KeyCode>,
mouse: &'a SceneMouse,
}
pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
graphics: Option<TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f>>,
pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f, 'g, 'h> {
graphics: Option<TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f, 'g, 'h>>,
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<Real> {
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<f32> {
(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<KinematicCharacterController>) {
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<KeyCode>) {
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::<Real>(),
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<KeyCode>) {
// 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,
};

53
src_testbed/ui.rs
View File

@@ -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();