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

29
CHANGELOG.md
View File

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

2
examples2d/all_examples2.rs
View File

@@ -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![

38
examples2d/character_controller2.rs
View File

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

24
examples2d/rope_joints2.rs
View File

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

250
examples2d/utils/character.rs Normal file
View File

@@ -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<Real> {
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::<Real>(),
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"));
}
}
}
}

1
examples2d/utils/mod.rs Normal file
View File

@@ -0,0 +1 @@
pub mod character;

2
examples3d/all_examples3.rs
View File

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

44
examples3d/character_controller3.rs
View File

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

23
examples3d/rope_joints3.rs
View File

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

261
examples3d/utils/character.rs Normal file
View File

@@ -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<Real> {
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::<Real>(),
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"));
}
}
}
}

1
examples3d/utils/mod.rs Normal file
View File

@@ -0,0 +1 @@
pub mod character;

2
src/control/mod.rs
View File

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

411
src/control/pid_controller.rs Normal file
View File

@@ -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<Real>,
/// 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<Real>,
/// 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<Real>,
/// 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<Real>,
/// 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<Real>,
/// The angular error accumulated through time for the Integral part of the PID controller.
pub ang_integral: AngVector<Real>,
/// The linear gain applied to the Integral part of the PID controller.
pub lin_ki: Vector<Real>,
/// The angular gain applied to the Integral part of the PID controller.
pub ang_ki: AngVector<Real>,
}
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<Real>,
/// The angular (rotational) part of the error.
pub angular: AngVector<Real>,
}
impl From<RigidBodyVelocity> 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<Real>,
target_linvel: Vector<Real>,
) -> Vector<Real> {
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<Real>,
target_angvel: AngVector<Real>,
) -> AngVector<Real> {
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<Real>,
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<Real> {
#[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<Real> {
#[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<Real>,
target_linvel: Vector<Real>,
) -> Vector<Real> {
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<Real>,
target_angvel: AngVector<Real>,
) -> AngVector<Real> {
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<Real>,
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),
}
}
}

27
src/dynamics/rigid_body.rs
View File

@@ -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<Real> {
&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<Real> {
&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;

152
src/dynamics/rigid_body_components.rs
View File

@@ -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<Real>) -> 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<Real>) -> 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<T> From<T> 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<RigidBodyVelocity> for RigidBodyVelocity {
}
}
impl std::ops::Sub<RigidBodyVelocity> 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<RigidBodyVelocity> 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);
}
}
}

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