From 36e85d0708e53a01731dfa95a9a2b4792ef03fe2 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 2 Oct 2022 17:36:30 +0200 Subject: Add a character controller implementation --- crates/rapier2d-f64/Cargo.toml | 2 +- crates/rapier2d/Cargo.toml | 2 +- crates/rapier3d-f64/Cargo.toml | 2 +- crates/rapier3d/Cargo.toml | 2 +- examples2d/all_examples2.rs | 2 + examples2d/character_controller2.rs | 150 ++++++++ examples3d/all_examples3.rs | 4 + examples3d/character_controller3.rs | 157 ++++++++ examples3d/newton_cradle3.rs | 45 +++ src/control/character_controller.rs | 729 ++++++++++++++++++++++++++++++++++++ src/control/mod.rs | 8 + src/lib.rs | 2 + src/pipeline/query_pipeline.rs | 4 +- src_testbed/testbed.rs | 142 ++++++- 14 files changed, 1239 insertions(+), 12 deletions(-) create mode 100644 examples2d/character_controller2.rs create mode 100644 examples3d/character_controller3.rs create mode 100644 examples3d/newton_cradle3.rs create mode 100644 src/control/character_controller.rs create mode 100644 src/control/mod.rs diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index 6e8b038..d61e3c5 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -49,7 +49,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry2d-f64 = "0.9" +parry2d-f64 = "0.10" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index 329f9eb..4bdb0c3 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -49,7 +49,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry2d = "0.9" +parry2d = "0.10" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml index dfca0ef..0cab401 100644 --- a/crates/rapier3d-f64/Cargo.toml +++ b/crates/rapier3d-f64/Cargo.toml @@ -49,7 +49,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry3d-f64 = "0.9" +parry3d-f64 = "0.10" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml index efe7e1a..a4d7895 100644 --- a/crates/rapier3d/Cargo.toml +++ b/crates/rapier3d/Cargo.toml @@ -49,7 +49,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry3d = "0.9" +parry3d = "0.10" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 3a97d39..aa3782b 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -10,6 +10,7 @@ use std::cmp::Ordering; mod add_remove2; mod ccd2; +mod character_controller2; mod collision_groups2; mod convex_polygons2; mod damping2; @@ -61,6 +62,7 @@ pub fn main() { let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ ("Add remove", add_remove2::init_world), ("CCD", ccd2::init_world), + ("Character controller", character_controller2::init_world), ("Collision groups", collision_groups2::init_world), ("Convex polygons", convex_polygons2::init_world), ("Damping", damping2::init_world), diff --git a/examples2d/character_controller2.rs b/examples2d/character_controller2.rs new file mode 100644 index 0000000..6f1ed92 --- /dev/null +++ b/examples2d/character_controller2.rs @@ -0,0 +1,150 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground_size = 5.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height]); + let floor_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height); + colliders.insert_with_parent(collider, floor_handle, &mut bodies); + + /* + * Character we will control manually. + */ + let rigid_body = RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0]); + let character_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(0.15, 0.3); + colliders.insert_with_parent(collider, character_handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 8; + let rad = 0.1; + + let shift = rad * 2.0; + let centerx = shift * (num / 2) as f32; + let centery = rad; + + for j in 0usize..4 { + for i in 0..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad); + colliders.insert_with_parent(collider, handle, &mut bodies); + } + } + + /* + * Create some stairs. + */ + let stair_width = 1.0; + let stair_height = 0.1; + for i in 0..10 { + let x = i as f32 * stair_width / 2.0; + let y = i as f32 * stair_height * 1.5 + 3.0; + + let collider = ColliderBuilder::cuboid(stair_width / 2.0, stair_height / 2.0) + .translation(vector![x, y]); + colliders.insert(collider); + } + + /* + * Create a slope we can climb. + */ + let slope_angle = 0.2; + let slope_size = 2.0; + let collider = ColliderBuilder::cuboid(slope_size, ground_height) + .translation(vector![ground_size + slope_size, -ground_height + 0.4]) + .rotation(slope_angle); + colliders.insert(collider); + + /* + * Create a slope we can’t climb. + */ + let impossible_slope_angle = 0.9; + let impossible_slope_size = 2.0; + let collider = ColliderBuilder::cuboid(slope_size, ground_height) + .translation(vector![ + ground_size + slope_size * 2.0 + impossible_slope_size - 0.9, + -ground_height + 2.3 + ]) + .rotation(impossible_slope_angle); + colliders.insert(collider); + + /* + * Create a moving platform. + */ + let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5]); + // .rotation(-0.3); + let platform_handle = bodies.insert(body); + let collider = ColliderBuilder::cuboid(2.0, ground_height); + colliders.insert_with_parent(collider, platform_handle, &mut bodies); + + /* + * More complex ground. + */ + let ground_size = Vector::new(10.0, 1.0); + let nsubdivs = 20; + + let heights = DVector::from_fn(nsubdivs + 1, |i, _| { + (i as f32 * ground_size.x / (nsubdivs as f32) / 2.0).cos() * 1.5 + }); + + let collider = + ColliderBuilder::heightfield(heights, ground_size).translation(vector![-8.0, 5.0]); + colliders.insert(collider); + + /* + * A tilting dynamic body with a limited joint. + */ + let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0]); + let ground_handle = bodies.insert(ground); + let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0]); + let handle = bodies.insert(body); + let collider = ColliderBuilder::cuboid(1.0, 0.1); + colliders.insert_with_parent(collider, handle, &mut bodies); + let joint = RevoluteJointBuilder::new().limits([-0.3, 0.3]); + impulse_joints.insert(ground_handle, handle, joint, true); + + /* + * Setup a callback to move the platform. + */ + testbed.add_callback(move |_, physics, _, run_state| { + let linvel = vector![ + (run_state.time * 2.0).sin() * 2.0, + (run_state.time * 5.0).sin() * 1.5 + ]; + let angvel = run_state.time.sin() * 0.5; + + // Update the velocity-based kinematic body by setting its velocity. + if let Some(platform) = physics.bodies.get_mut(platform_handle) { + platform.set_linvel(linvel, true); + // NOTE: interaction with rotating platforms isn’t handled very well yet. + // platform.set_angvel(angvel, true); + } + }); + + /* + * 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/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index d7fb4e3..f599fdb 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -33,8 +33,10 @@ mod fountain3; mod heightfield3; mod joints3; // mod joints3; +mod character_controller3; mod keva3; mod locked_rotations3; +mod newton_cradle3; mod one_way_platforms3; mod platform3; mod primitives3; @@ -79,6 +81,7 @@ pub fn main() { .to_camel_case(); let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ + ("Character controller", character_controller3::init_world), ("Fountain", fountain3::init_world), ("Primitives", primitives3::init_world), ("Multibody joints", joints3::init_world_with_articulations), @@ -98,6 +101,7 @@ pub fn main() { ("Sensor", sensor3::init_world), ("TriMesh", trimesh3::init_world), ("Keva tower", keva3::init_world), + ("Newton cradle", newton_cradle3::init_world), ("(Debug) multibody_joints", debug_articulations3::init_world), ( "(Debug) add/rm collider", diff --git a/examples3d/character_controller3.rs b/examples3d/character_controller3.rs new file mode 100644 index 0000000..939cfd1 --- /dev/null +++ b/examples3d/character_controller3.rs @@ -0,0 +1,157 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground_size = 5.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]); + let floor_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); + colliders.insert_with_parent(collider, floor_handle, &mut bodies); + + /* + * Character we will control manually. + */ + let rigid_body = + RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0, 0.0]); + let character_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::capsule_y(0.3, 0.15); // 0.15, 0.3, 0.15); + colliders.insert_with_parent(collider, character_handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 8; + let rad = 0.1; + + let shift = rad * 2.0; + let centerx = shift * (num / 2) as f32; + let centery = rad; + + for j in 0usize..4 { + for k in 0usize..4 { + for i in 0..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + let z = k as f32 * shift + centerx; + + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad); + colliders.insert_with_parent(collider, handle, &mut bodies); + } + } + } + + /* + * Create some stairs. + */ + let stair_width = 1.0; + let stair_height = 0.1; + for i in 0..10 { + let x = i as f32 * stair_width / 2.0; + let y = i as f32 * stair_height * 1.5 + 3.0; + + let collider = ColliderBuilder::cuboid(stair_width / 2.0, stair_height / 2.0, stair_width) + .translation(vector![x, y, 0.0]); + colliders.insert(collider); + } + + /* + * Create a slope we can climb. + */ + let slope_angle = 0.2; + let slope_size = 2.0; + let collider = ColliderBuilder::cuboid(slope_size, ground_height, slope_size) + .translation(vector![ground_size + slope_size, -ground_height + 0.4, 0.0]) + .rotation(Vector::z() * slope_angle); + colliders.insert(collider); + + /* + * Create a slope we can’t climb. + */ + let impossible_slope_angle = 0.9; + let impossible_slope_size = 2.0; + let collider = ColliderBuilder::cuboid(slope_size, ground_height, ground_size) + .translation(vector![ + ground_size + slope_size * 2.0 + impossible_slope_size - 0.9, + -ground_height + 2.3, + 0.0 + ]) + .rotation(Vector::z() * impossible_slope_angle); + colliders.insert(collider); + + /* + * Create a moving platform. + */ + let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0]); + // .rotation(-0.3); + let platform_handle = bodies.insert(body); + let collider = ColliderBuilder::cuboid(2.0, ground_height, 2.0); + colliders.insert_with_parent(collider, platform_handle, &mut bodies); + + /* + * More complex ground. + */ + let ground_size = Vector::new(10.0, 1.0, 10.0); + let nsubdivs = 20; + + let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + (i as f32 * ground_size.x / (nsubdivs as f32) / 2.0).cos() + + (j as f32 * ground_size.z / (nsubdivs as f32) / 2.0).cos() + }); + + let collider = + ColliderBuilder::heightfield(heights, ground_size).translation(vector![-8.0, 5.0, 0.0]); + colliders.insert(collider); + + /* + * A tilting dynamic body with a limited joint. + */ + let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 0.0]); + let ground_handle = bodies.insert(ground); + let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0]); + let handle = bodies.insert(body); + let collider = ColliderBuilder::cuboid(1.0, 0.1, 2.0); + colliders.insert_with_parent(collider, handle, &mut bodies); + let joint = RevoluteJointBuilder::new(Vector::z_axis()).limits([-0.3, 0.3]); + impulse_joints.insert(ground_handle, handle, joint, true); + + /* + * Setup a callback to move the platform. + */ + testbed.add_callback(move |_, physics, _, run_state| { + let linvel = vector![ + (run_state.time * 2.0).sin() * 2.0, + (run_state.time * 5.0).sin() * 1.5, + 0.0 + ]; + // let angvel = run_state.time.sin() * 0.5; + + // Update the velocity-based kinematic body by setting its velocity. + if let Some(platform) = physics.bodies.get_mut(platform_handle) { + platform.set_linvel(linvel, true); + // NOTE: interaction with rotating platforms isn’t handled very well yet. + // platform.set_angvel(angvel, true); + } + }); + + /* + * 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::origin()); +} diff --git a/examples3d/newton_cradle3.rs b/examples3d/newton_cradle3.rs new file mode 100644 index 0000000..b281149 --- /dev/null +++ b/examples3d/newton_cradle3.rs @@ -0,0 +1,45 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let radius = 0.5; + let length = 10.0 * radius; + let rb = RigidBodyBuilder::dynamic(); + let co = ColliderBuilder::ball(radius).restitution(1.0); + + let n = 5; + + for i in 0..n { + let (ball_pos, attach) = ( + vector![i as Real * 2.2 * radius, 0.0, 0.0], + Vector::y() * length, + ); + let vel = if i >= n - 1 { + vector![7.0, 0.0, 0.0] + } else { + Vector::zeros() + }; + + let ground = bodies.insert(RigidBodyBuilder::fixed().translation(ball_pos + attach)); + let rb = rb.clone().translation(ball_pos).linvel(vel); + let handle = bodies.insert(rb); + colliders.insert_with_parent(co.clone(), handle, &mut bodies); + + let joint = SphericalJointBuilder::new().local_anchor2(attach.into()); + impulse_joints.insert(ground, handle, joint, true); + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); +} diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs new file mode 100644 index 0000000..049aef8 --- /dev/null +++ b/src/control/character_controller.rs @@ -0,0 +1,729 @@ +use crate::dynamics::RigidBodySet; +use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, Shape, TOI}; +use crate::math::{Isometry, Point, Real, UnitVector, Vector}; +use crate::pipeline::{QueryFilter, QueryFilterFlags, QueryPipeline}; +use crate::utils; +use na::{RealField, Vector2}; +use parry::bounding_volume::BoundingVolume; +use parry::math::Translation; +use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; + +#[derive(Copy, Clone, Debug, PartialEq)] +/// A length measure used for various options of a character controller. +pub enum CharacterLength { + /// The length is specified relative to some of the character shape’s size. + /// + /// For example setting `CharacterAutostep::max_height` to `CharaceterLentgh::Relative(0.1)` + /// for a shape with an height equal to 20.0 will result in a maximum step heigth + /// of `0.1 * 20.0 = 2.0`. + Relative(Real), + /// The lengt his specified as an aboslute value, independent from the character shape’s size. + /// + /// For example setting `CharacterAutostep::max_height` to `CharaceterLentgh::Relative(0.1)` + /// for a shape with an height equal to 20.0 will result in a maximum step heigth + /// of `0.1` (the shape height is ignored in for this value). + Absolute(Real), +} + +impl CharacterLength { + pub fn map_absolute(self, f: impl FnOnce(Real) -> Real) -> Self { + if let Self::Absolute(value) = self { + Self::Absolute(f(value)) + } else { + self + } + } + + fn eval(self, value: Real) -> Real { + match self { + Self::Relative(x) => value * x, + Self::Absolute(x) => x, + } + } +} + +/// Configuration for the auto-stepping character controller feature. +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct CharacterAutostep { + /// The maximum step height a character can automatically step over. + pub max_height: CharacterLength, + /// The minimum width of free space that must be available after stepping on a stair. + pub min_width: CharacterLength, + /// Can the character automatically step over dynamic bodies too? + pub include_dynamic_bodies: bool, +} + +impl Default for CharacterAutostep { + fn default() -> Self { + Self { + max_height: CharacterLength::Relative(0.25), + min_width: CharacterLength::Relative(0.5), + include_dynamic_bodies: true, + } + } +} + +/// A collision between the character and its environment during its movement. +pub struct CharacterCollision { + /// The collider hit by the character. + pub handle: ColliderHandle, + /// The position of the character when the collider was hit. + pub character_pos: Isometry, + /// The translation that was already applied to the character when the hit happens. + pub translation_applied: Vector, + /// The translations that was still waiting to be applied to the character when the hit happens. + pub translation_remaining: Vector, + /// Geometric information about the hit. + pub toi: TOI, +} + +/// A character controller for kinematic bodies. +#[derive(Copy, Clone, Debug)] +pub struct KinematicCharacterController { + /// The direction that goes "up". Used to determine where the floor is, and the floor’s angle. + pub up: UnitVector, + /// A small gap to preserve between the character and its surroundings. + /// + /// This value should not be too large to avoid visual artifacts, but shouldn’t be too small + /// (must not be zero) to improve numerical stability of the character controller. + pub offset: CharacterLength, + /// Should the character try to slide against the floor if it hits it? + pub slide: bool, + /// Should the character automatically step over small obstacles? + pub autostep: Option, + /// The maximum angle (radians) between the floor’s normal and the `up` vector that the + /// character is able to climb. + pub max_slope_climb_angle: Real, + /// The minimum angle (radians) between the floor’s normal and the `up` vector before the + /// character starts to slide down automatically. + pub min_slope_slide_angle: Real, + /// Should the character be automatically snapped to the ground if the distance between + /// the ground and its feed are smaller than the specified threshold? + pub snap_to_ground: Option, +} + +impl Default for KinematicCharacterController { + fn default() -> Self { + Self { + up: Vector::y_axis(), + offset: CharacterLength::Relative(0.01), + slide: true, + autostep: Some(CharacterAutostep::default()), + max_slope_climb_angle: Real::frac_pi_4(), + min_slope_slide_angle: Real::frac_pi_4(), + snap_to_ground: Some(CharacterLength::Relative(0.2)), + } + } +} + +/// The effective movement computed by the character controller. +pub struct EffectiveCharacterMovement { + /// The movement to apply. + pub translation: Vector, + /// Is the character touching the ground after applying `EffictiveKineamticMovement::translation`? + pub grounded: bool, +} + +impl KinematicCharacterController { + fn check_and_fix_penetrations(&self) { + /* + // 1/ Check if the body is grounded and if there are penetrations. + let mut grounded = false; + let mut penetrating = false; + + let mut contacts = vec![]; + + let aabb = shape + .compute_aabb(shape_pos) + .loosened(self.offset); + queries.colliders_with_aabb_intersecting_aabb(&aabb, |handle| { + // TODO: apply the filter. + if let Some(collider) = colliders.get(*handle) { + if let Ok(Some(contact)) = parry::query::contact( + &shape_pos, + shape, + collider.position(), + collider.shape(), + self.offset, + ) { + contacts.push((contact, collider)); + } + } + + true + }); + */ + } + + /// Computes the possible movement for a shape. + pub fn move_shape( + &self, + dt: Real, + bodies: &RigidBodySet, + colliders: &ColliderSet, + queries: &QueryPipeline, + character_shape: &dyn Shape, + character_pos: &Isometry, + desired_translation: Vector, + filter: QueryFilter, + mut events: impl FnMut(CharacterCollision), + ) -> EffectiveCharacterMovement { + let mut result = EffectiveCharacterMovement { + translation: Vector::zeros(), + grounded: false, + }; + + let extents = character_shape.compute_local_aabb().extents(); + let up_extent = extents.dot(&self.up); + let side_extent = (extents - *self.up * up_extent).norm(); + let dims = Vector2::new(side_extent, up_extent); + + // 1. Check and fix penetrations. + self.check_and_fix_penetrations(); + + let mut translation_remaining = desired_translation; + + // Check if we are grounded at the initial position. + let grounded_at_starting_pos = self.detect_grounded_status_and_apply_friction( + dt, + bodies, + colliders, + queries, + character_shape, + &character_pos, + &dims, + filter, + None, + None, + ); + + // println!("Init grounded status: {grounded_at_starting_pos}"); + + let mut max_iters = 20; + let mut kinematic_friction_translation = Vector::zeros(); + let offset = self.offset.eval(dims.y); + + while let Some((translation_dir, translation_dist)) = + UnitVector::try_new_and_get(translation_remaining, 1.0e-5) + { + if max_iters == 0 { + break; + } else { + max_iters -= 1; + } + + // 2. Cast towards the movement direction. + if let Some((handle, toi)) = queries.cast_shape( + bodies, + colliders, + &(Translation::from(result.translation) * character_pos), + &translation_dir, + character_shape, + translation_dist + offset, + false, + filter, + ) { + // We hit something, compute the allowed self. + let allowed_dist = + (toi.toi - (-toi.normal1.dot(&translation_dir)) * offset).max(0.0); + let allowed_translation = *translation_dir * allowed_dist; + result.translation += allowed_translation; + translation_remaining -= allowed_translation; + + events(CharacterCollision { + handle, + character_pos: Translation::from(result.translation) * character_pos, + translation_applied: result.translation, + translation_remaining, + toi, + }); + + if let (Some(translation_on_slope), _) = + self.handle_slopes(&toi, &mut translation_remaining) + { + translation_remaining = translation_on_slope; + } else { + // If the slope is too big, try to step on the stair. + self.handle_stairs( + bodies, + colliders, + queries, + character_shape, + &(Translation::from(result.translation) * character_pos), + &dims, + filter, + handle, + &mut translation_remaining, + &mut result, + ); + } + } else { + // No interference along the path. + result.translation += translation_remaining; + translation_remaining.fill(0.0); + break; + } + + result.grounded = self.detect_grounded_status_and_apply_friction( + dt, + bodies, + colliders, + queries, + character_shape, + &(Translation::from(result.translation) * character_pos), + &dims, + filter, + Some(&mut kinematic_friction_translation), + Some(&mut translation_remaining), + ); + + if !self.slide { + break; + } + } + + // If needed, and if we are not already grounded, snap to the ground. + if grounded_at_starting_pos { + self.snap_to_ground( + bodies, + colliders, + queries, + character_shape, + &(Translation::from(result.translation) * character_pos), + &dims, + filter, + &mut result, + ); + } + + // Return the result. + result + } + + fn snap_to_ground( + &self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + queries: &QueryPipeline, + character_shape: &dyn Shape, + character_pos: &Isometry, + dims: &Vector2, + filter: QueryFilter, + result: &mut EffectiveCharacterMovement, + ) -> Option<(ColliderHandle, TOI)> { + if let Some(snap_distance) = self.snap_to_ground { + let snap_distance = snap_distance.eval(dims.y); + if result.translation.dot(&self.up) < 1.0e-5 { + let offset = self.offset.eval(dims.y); + if let Some((hit_handle, hit)) = queries.cast_shape( + bodies, + colliders, + character_pos, + &-self.up, + character_shape, + snap_distance + offset, + false, + filter, + ) { + // Apply the snap. + result.translation -= *self.up * (hit.toi - offset).max(0.0); + result.grounded = true; + return Some((hit_handle, hit)); + } + } + } + + None + } + + fn detect_grounded_status_and_apply_friction( + &self, + dt: Real, + bodies: &RigidBodySet, + colliders: &ColliderSet, + queries: &QueryPipeline, + character_shape: &dyn Shape, + character_pos: &Isometry, + dims: &Vector2, + filter: QueryFilter, + mut kinematic_friction_translation: Option<&mut Vector>, + mut translation_remaining: Option<&mut Vector>, + ) -> bool { + let prediction = self.offset.eval(dims.y) * 1.1; + + // TODO: allow custom dispatchers. + let dispatcher = DefaultQueryDispatcher; + + let mut manifolds: Vec = vec![]; + let character_aabb = character_shape + .compute_aabb(character_pos) + .loosened(prediction); + + let mut grounded = false; + + queries.colliders_with_aabb_intersecting_aabb(&character_aabb, |handle| { + if let Some(collider) = colliders.get(*handle) { + if filter.test(bodies, *handle, collider) { + manifolds.clear(); + let pos12 = character_pos.inv_mul(collider.position()); + let _ = dispatcher.contact_manifolds( + &pos12, + character_shape, + collider.shape(), + prediction, + &mut manifolds, + &mut None, + ); + + if let (Some(kinematic_friction_translation), Some(translation_remaining)) = ( + kinematic_friction_translation.as_deref_mut(), + translation_remaining.as_deref_mut(), + ) { + let init_kinematic_friction_translation = *kinematic_friction_translation; + let kinematic_parent = collider + .parent + .and_then(|p| bodies.get(p.handle)) + .filter(|rb| rb.is_kinematic()); + + for m in &manifolds { + let normal1 = character_pos * m.local_n1; + let normal2 = -normal1; + + if normal1.dot(&self.up) <= -1.0e-5 { + grounded = true; + } + + if let Some(kinematic_parent) = kinematic_parent { + let mut num_active_contacts = 0; + let mut manifold_center = Point::origin(); + + for contact in &m.points { + if contact.dist <= prediction { + num_active_contacts += 1; + let contact_point = collider.position() * contact.local_p2; + let target_vel = + kinematic_parent.velocity_at_point(&contact_point); + + let normal_target_mvt = target_vel.dot(&normal2) * dt; + let normal_current_mvt = + translation_remaining.dot(&normal2); + + manifold_center += contact_point.coords; + *translation_remaining += normal2 + * (normal_target_mvt - normal_current_mvt).max(0.0); + } + } + + if num_active_contacts > 0 { + let target_vel = kinematic_parent.velocity_at_point( + &(manifold_center / num_active_contacts as Real), + ); + let tangent_platform_mvt = + (target_vel - normal2 * target_vel.dot(&normal2)) * dt; + kinematic_friction_translation.zip_apply( + &tangent_platform_mvt, + |y, x| { + if x.abs() > (*y).abs() { + *y = x; + } + }, + ); + } + } + } + + *translation_remaining += + *kinematic_friction_translation - init_kinematic_friction_translation; + } else { + for m in &manifolds { + let normal = character_pos * m.local_n1; + + if normal.dot(&self.up) <= -1.0e-5 { + for contact in &m.points { + if contact.dist <= prediction { + grounded = true; + return false; // We can stop the search early. + } + } + } + } + } + } + } + true + }); + + grounded + } + + fn handle_slopes( + &self, + hit: &TOI, + translation_remaining: &Vector, + ) -> (Option>, Real) { + let vertical_translation_remaining = *self.up * (self.up.dot(translation_remaining)); + let horizontal_translation_remaining = + *translation_remaining - vertical_translation_remaining; + + // The idea behind this `if` statement is as follows: + // - If there is any amount of horizontal translations, then the intended + // climb/slide down movement is decided by that translation. + // - If there is no horizontal translation, then we only have gravity. In that case, + // we take the vertical movement into account to decide if we need to slide down. + let sliding_translation_remaining = if horizontal_translation_remaining != Vector::zeros() { + horizontal_translation_remaining + - *hit.normal1 * (horizontal_translation_remaining).dot(&hit.normal1) + } else { + vertical_translation_remaining + - *hit.normal1 * (vertical_translation_remaining).dot(&hit.normal1) + }; + + // Check if there is a slope we can climb. + let angle_with_floor = self.up.angle(&hit.normal1); + let climbing = self.up.dot(&sliding_translation_remaining) >= 0.0; + + if !climbing { + // Moving down the slope. + let remaining = if angle_with_floor >= self.min_slope_slide_angle { + // Can slide down. + sliding_translation_remaining + } else { + // To avoid sliding down, we remove the sliding component due to the vertical + // part of the movement but have to keep the component due to the horizontal + // part of the self. + *translation_remaining + - (*hit.normal1 * horizontal_translation_remaining.dot(&hit.normal1) + + vertical_translation_remaining) + // Remove the complete vertical part. + }; + + (Some(remaining), -angle_with_floor) + } else { + // Moving up the slope. + let remaining = if angle_with_floor <= self.max_slope_climb_angle { + // Let’s climb by cancelling from the desired movement the part that + // doesn’t line up with the slope, and continuing the loop. + Some(sliding_translation_remaining) + } else { + // The slope was too steep. + None + }; + + (remaining, angle_with_floor) + } + } + + fn handle_stairs( + &self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + queries: &QueryPipeline, + character_shape: &dyn Shape, + character_pos: &Isometry, + dims: &Vector2, + mut filter: QueryFilter, + stair_handle: ColliderHandle, + translation_remaining: &mut Vector, + result: &mut EffectiveCharacterMovement, + ) -> bool { + if let Some(autostep) = self.autostep { + let min_width = autostep.min_width.eval(dims.x); + let max_height = autostep.max_height.eval(dims.y); + + if !autostep.include_dynamic_bodies { + if colliders + .get(stair_handle) + .and_then(|co| co.parent) + .and_then(|p| bodies.get(p.handle)) + .map(|b| b.is_dynamic()) + == Some(true) + { + // The "stair" is a dynamic body, which the user wants to ignore. + return false; + } + + filter.flags |= QueryFilterFlags::EXCLUDE_DYNAMIC; + } + + let shifted_character_pos = Translation::from(*self.up * max_height) * character_pos; + + if let Some(horizontal_dir) = (*translation_remaining + - *self.up * translation_remaining.dot(&self.up)) + .try_normalize(1.0e-5) + { + if queries + .cast_shape( + bodies, + colliders, + character_pos, + &self.up, + character_shape, + max_height, + false, + filter, + ) + .is_some() + { + // We can’t go up. + return false; + } + + if queries + .cast_shape( + bodies, + colliders, + &shifted_character_pos, + &horizontal_dir, + character_shape, + min_width, + false, + filter, + ) + .is_some() + { + // We don’t have enough room on the stair to stay on it. + return false; + } + + // Check that we are not getting into a ramp that is too steep + // after stepping. + if let Some((_, hit)) = queries.cast_shape( + bodies, + colliders, + &(Translation::from(horizontal_dir * min_width) * shifted_character_pos), + &-self.up, + character_shape, + max_height, + false, + filter, + ) { + let vertical_translation_remaining = + *self.up * (self.up.dot(translation_remaining)); + let horizontal_translation_remaining = + *translation_remaining - vertical_translation_remaining; + let sliding_movement = horizontal_translation_remaining + - *hit.normal1 * horizontal_translation_remaining.dot(&hit.normal1); + + let angle_with_floor = self.up.angle(&hit.normal1); + let climbing = self.up.dot(&sliding_movement) >= 0.0; + + if climbing && angle_with_floor > self.max_slope_climb_angle { + return false; // The target ramp is too steep. + } + } + + // We can step, we need to find the actual step height. + let step_height = self.offset.eval(dims.y) + max_height + - queries + .cast_shape( + bodies, + colliders, + &(Translation::from(horizontal_dir * min_width) + * shifted_character_pos), + &-self.up, + character_shape, + max_height, + false, + filter, + ) + .map(|hit| hit.1.toi) + .unwrap_or(max_height); + + // Remove the step height from the vertical part of the self. + *translation_remaining -= + *self.up * translation_remaining.dot(&self.up).clamp(0.0, step_height); + + // Advance the collider on the step horizontally, to make sure further + // movement won’t just get stuck on its edge. + let horizontal_nudge = + horizontal_dir * min_width.min(horizontal_dir.dot(translation_remaining)); + *translation_remaining -= horizontal_nudge; + + result.translation += *self.up * step_height + horizontal_nudge; + return true; + } + } + + false + } + + pub fn solve_character_collision_impulses( + &self, + dt: Real, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + queries: &QueryPipeline, + character_shape: &dyn Shape, + character_mass: Real, + collision: &CharacterCollision, + filter: QueryFilter, + ) { + let extents = character_shape.compute_local_aabb().extents(); + let up_extent = extents.dot(&self.up); + let movement_to_transfer = + *collision.toi.normal1 * collision.translation_remaining.dot(&collision.toi.normal1); + let prediction = self.offset.eval(up_extent) * 1.1; + + // TODO: allow custom dispatchers. + let dispatcher = DefaultQueryDispatcher; + + let mut manifolds: Vec = vec![]; + let character_aabb = character_shape + .compute_aabb(&collision.character_pos) + .loosened(prediction); + + queries.colliders_with_aabb_intersecting_aabb(&character_aabb, |handle| { + if let Some(collider) = colliders.get(*handle) { + if let Some(parent) = collider.parent { + if filter.test(bodies, *handle, collider) { + if let Some(body) = bodies.get(parent.handle) { + if body.is_dynamic() { + manifolds.clear(); + let pos12 = collision.character_pos.inv_mul(collider.position()); + let prev_manifolds_len = manifolds.len(); + let _ = dispatcher.contact_manifolds( + &pos12, + character_shape, + collider.shape(), + prediction, + &mut manifolds, + &mut None, + ); + + for m in &mut manifolds[prev_manifolds_len..] { + m.data.rigid_body2 = Some(parent.handle); + m.data.normal = collision.character_pos * m.local_n1; + } + } + } + } + } + } + true + }); + + let velocity_to_transfer = movement_to_transfer * utils::inv(dt); + + for manifold in &manifolds { + let body_handle = manifold.data.rigid_body2.unwrap(); + let body = &mut bodies[body_handle]; + + for pt in &manifold.points { + if pt.dist <= prediction { + let body_mass = body.mass(); + let contact_point = body.position() * pt.local_p2; + let delta_vel_per_contact = (velocity_to_transfer + - body.velocity_at_point(&contact_point)) + .dot(&manifold.data.normal); + let mass_ratio = body_mass * character_mass / (body_mass + character_mass); + + body.apply_impulse_at_point( + manifold.data.normal * delta_vel_per_contact.max(0.0) * mass_ratio, + contact_point, + true, + ); + } + } + } + } +} diff --git a/src/control/mod.rs b/src/control/mod.rs new file mode 100644 index 0000000..0290f2c --- /dev/null +++ b/src/control/mod.rs @@ -0,0 +1,8 @@ +//! Utilities for controlling the trajectories of objects in a non-physical way. + +pub use self::character_controller::{ + CharacterAutostep, CharacterCollision, CharacterLength, EffectiveCharacterMovement, + KinematicCharacterController, +}; + +mod character_controller; diff --git a/src/lib.rs b/src/lib.rs index 586b3f5..baffedc 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -130,6 +130,7 @@ pub(crate) const INVALID_USIZE: usize = INVALID_U32 as usize; /// The string version of Rapier. pub const VERSION: &str = env!("CARGO_PKG_VERSION"); +pub mod control; pub mod counters; pub mod data; pub mod dynamics; @@ -202,6 +203,7 @@ pub mod math { /// Prelude containing the common types defined by Rapier. pub mod prelude { + // pub use crate::controller::*; pub use crate::dynamics::*; pub use crate::geometry::*; pub use crate::math::*; diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 86fa7b6..6cf646d 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -168,7 +168,7 @@ impl<'a> QueryFilter<'a> { } /// Exclude from the query any collider attached to a kinematic rigid-body. - pub fn exclude_dynamic(self) -> Self { + pub fn exclude_dynamic() -> Self { QueryFilterFlags::EXCLUDE_DYNAMIC.into() } @@ -705,6 +705,7 @@ impl QueryPipeline { shape_vel: &Vector, shape: &dyn Shape, max_toi: Real, + stop_at_penetration: bool, filter: QueryFilter, ) -> Option<(ColliderHandle, TOI)> { let pipeline_shape = self.as_composite_shape(bodies, colliders, filter); @@ -715,6 +716,7 @@ impl QueryPipeline { &pipeline_shape, shape, max_toi, + stop_at_penetration, ); self.qbvh.traverse_best_first(&mut visitor).map(|h| h.1) } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 3d4f814..7de339c 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -8,16 +8,17 @@ use crate::plugin::TestbedPlugin; use crate::{debug_render, ui}; use crate::{graphics::GraphicsManager, harness::RunState}; -use na::{self, Point2, Point3, Vector3}; +use na::{self, Point2, Point3, Quaternion, Unit, Vector3}; +use rapier::control::{CharacterAutostep, CharacterLength, KinematicCharacterController}; use rapier::dynamics::{ ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyActivation, RigidBodyHandle, RigidBodySet, }; +#[cfg(feature = "dim3")] +use rapier::geometry::Ray; use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase}; use rapier::math::{Real, Vector}; -use rapier::pipeline::PhysicsHooks; -#[cfg(feature = "dim3")] -use rapier::{geometry::Ray, pipeline::QueryFilter}; +use rapier::pipeline::{PhysicsHooks, QueryFilter}; #[cfg(all(feature = "dim2", feature = "other-backends"))] use crate::box2d_backend::Box2dWorld; @@ -98,6 +99,7 @@ pub struct TestbedState { pub running: RunMode, pub draw_colls: bool, pub highlighted_body: Option, + pub character_body: Option, // pub grabbed_object: Option, // pub grabbed_object_constraint: Option, pub grabbed_object_plane: (Point3, Vector3), @@ -133,6 +135,7 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> { meshes: &'a mut Assets, materials: &'a mut Assets, components: &'a mut Query<'b, 'f, (&'c mut Transform,)>, + camera_transform: GlobalTransform, camera: &'a mut OrbitCamera, } @@ -173,6 +176,7 @@ impl TestbedApp { running: RunMode::Running, draw_colls: false, highlighted_body: None, + character_body: None, // grabbed_object: None, // grabbed_object_constraint: None, grabbed_object_plane: (Point3::origin(), na::zero()), @@ -447,6 +451,10 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { self.state.nsteps = nsteps } + pub fn set_character_body(&mut self, handle: RigidBodyHandle) { + self.state.character_body = Some(handle); + } + pub fn allow_grabbing_behind_ground(&mut self, allow: bool) { self.state.can_grab_behind_ground = allow; } @@ -502,6 +510,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { .action_flags .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true); + self.state.character_body = None; self.state.highlighted_body = None; #[cfg(all(feature = "dim2", feature = "other-backends"))] @@ -614,6 +623,121 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { self.plugins.0.push(Box::new(plugin)); } + fn update_character_controller(&mut self, events: &Input) { + 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::Right => { + desired_movement += Vector::x(); + } + KeyCode::Left => { + desired_movement -= Vector::x(); + } + KeyCode::Space => { + desired_movement += Vector::y() * 2.0; + } + KeyCode::RControl => { + desired_movement -= Vector::y(); + } + KeyCode::RShift => { + speed /= 10.0; + } + _ => {} + } + } + + #[cfg(feature = "dim3")] + { + let (_, rot, _) = self + .graphics + .as_ref() + .unwrap() + .camera_transform + .to_scale_rotation_translation(); + let rot = Unit::new_unchecked(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::Right => { + desired_movement += rot_x; + } + KeyCode::Left => { + desired_movement -= rot_x; + } + KeyCode::Up => { + desired_movement -= rot_z; + } + KeyCode::Down => { + desired_movement += rot_z; + } + KeyCode::Space => { + desired_movement += Vector::y() * 2.0; + } + KeyCode::RControl => { + desired_movement -= Vector::y(); + } + KeyCode::RShift => { + speed /= 10.0; + } + _ => {} + } + } + } + + desired_movement *= speed; + desired_movement -= Vector::y() * speed; + + let controller = KinematicCharacterController::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, + QueryFilter::new().exclude_rigid_body(character_handle), + |c| collisions.push(c), + ); + + for collision in &collisions { + controller.solve_character_collision_impulses( + phx.integration_parameters.dt, + &mut phx.bodies, + &phx.colliders, + &phx.query_pipeline, + character_collider.shape(), + character_mass, + collision, + 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: &Input) { for key in events.get_just_released() { match *key { @@ -923,7 +1047,8 @@ fn update_testbed( meshes: &mut *meshes, materials: &mut *materials, components: &mut gfx_components, - camera: &mut cameras.iter_mut().next().unwrap().2, + camera_transform: *cameras.single().1, + camera: &mut cameras.single_mut().2, }; let mut testbed = Testbed { @@ -936,6 +1061,7 @@ fn update_testbed( }; testbed.handle_common_events(&*keys); + testbed.update_character_controller(&*keys); } // Update UI @@ -1010,7 +1136,8 @@ fn update_testbed( meshes: &mut *meshes, materials: &mut *materials, components: &mut gfx_components, - camera: &mut cameras.iter_mut().next().unwrap().2, + camera_transform: *cameras.single().1, + camera: &mut cameras.single_mut().2, }; let mut testbed = Testbed { @@ -1160,7 +1287,8 @@ fn update_testbed( meshes: &mut *meshes, materials: &mut *materials, components: &mut gfx_components, - camera: &mut cameras.iter_mut().next().unwrap().2, + camera_transform: *cameras.single().1, + camera: &mut cameras.single_mut().2, }; harness.step_with_graphics(Some(&mut testbed_graphics)); -- cgit From a8865296695db04969e9dbe806c489187ba20d0d Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 2 Oct 2022 17:55:23 +0200 Subject: Fix warnings --- examples2d/character_controller2.rs | 2 +- src/control/character_controller.rs | 16 ++++++++++++++++ src_testbed/testbed.rs | 7 ++++--- 3 files changed, 21 insertions(+), 4 deletions(-) diff --git a/examples2d/character_controller2.rs b/examples2d/character_controller2.rs index 6f1ed92..ff500e9 100644 --- a/examples2d/character_controller2.rs +++ b/examples2d/character_controller2.rs @@ -131,7 +131,7 @@ pub fn init_world(testbed: &mut Testbed) { (run_state.time * 2.0).sin() * 2.0, (run_state.time * 5.0).sin() * 1.5 ]; - let angvel = run_state.time.sin() * 0.5; + // let angvel = run_state.time.sin() * 0.5; // Update the velocity-based kinematic body by setting its velocity. if let Some(platform) = physics.bodies.get_mut(platform_handle) { diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs index 049aef8..b8f73bc 100644 --- a/src/control/character_controller.rs +++ b/src/control/character_controller.rs @@ -26,6 +26,8 @@ pub enum CharacterLength { } impl CharacterLength { + /// Returns `self` with its value changed by the closure `f` if `self` is the `Self::Absolute` + /// variant. pub fn map_absolute(self, f: impl FnOnce(Real) -> Real) -> Self { if let Self::Absolute(value) = self { Self::Absolute(f(value)) @@ -34,6 +36,16 @@ impl CharacterLength { } } + /// Returns `self` with its value changed by the closure `f` if `self` is the `Self::Relative` + /// variant. + pub fn map_relative(self, f: impl FnOnce(Real) -> Real) -> Self { + if let Self::Relative(value) = self { + Self::Relative(f(value)) + } else { + self + } + } + fn eval(self, value: Real) -> Real { match self { Self::Relative(x) => value * x, @@ -647,6 +659,10 @@ impl KinematicCharacterController { false } + /// For a given collision between a character and its environment, this method will apply + /// impulses to the rigid-bodies surrounding the character shape at the time of the collision. + /// Note that the impulse calculation is only approximate as it is not based on a global + /// constraints resolution scheme. pub fn solve_character_collision_impulses( &self, dt: Real, diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 7de339c..b79175c 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -8,8 +8,8 @@ use crate::plugin::TestbedPlugin; use crate::{debug_render, ui}; use crate::{graphics::GraphicsManager, harness::RunState}; -use na::{self, Point2, Point3, Quaternion, Unit, Vector3}; -use rapier::control::{CharacterAutostep, CharacterLength, KinematicCharacterController}; +use na::{self, Point2, Point3, Vector3}; +use rapier::control::KinematicCharacterController; use rapier::dynamics::{ ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyActivation, RigidBodyHandle, RigidBodySet, @@ -135,6 +135,7 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> { meshes: &'a mut Assets, materials: &'a mut Assets, components: &'a mut Query<'b, 'f, (&'c mut Transform,)>, + #[allow(dead_code)] // Dead in 2D but not in 3D. camera_transform: GlobalTransform, camera: &'a mut OrbitCamera, } @@ -662,7 +663,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { .unwrap() .camera_transform .to_scale_rotation_translation(); - let rot = Unit::new_unchecked(Quaternion::new(rot.w, rot.x, rot.y, rot.z)); + 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; -- cgit From 9a4afcc43e68dec024cf577cf0afcc76ffbaf2c5 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 2 Oct 2022 18:30:14 +0200 Subject: Fix f64 build --- src_testbed/testbed.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index b79175c..180bfd4 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -714,7 +714,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { &phx.query_pipeline, character_collider.shape(), character_collider.position(), - desired_movement, + desired_movement.cast::(), QueryFilter::new().exclude_rigid_body(character_handle), |c| collisions.push(c), ); -- cgit From f7bec3c49cf6ca24d2878a9dcd20656d26de5cc7 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 2 Oct 2022 18:30:26 +0200 Subject: Update CHANGELOG --- CHANGELOG.md | 22 +++++++++++++++++++++- src/lib.rs | 1 - src/pipeline/query_pipeline.rs | 3 +++ 3 files changed, 24 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5aeb23a..29e5b73 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,8 +1,28 @@ ## Unreleased +### Added +- Add a **kinematic character** controller implementation. See the `control` module. The character controller currently + supports the following features: + - Slide on uneven terrains + - Interaction with dynamic bodies. + - Climb stairs automatically. + - Automatically snap the body to the floor when going downstairs. + - Prevent sliding up slopes that are too steep + - Prevent sliding down slopes that are not steep enough + - Interactions with moving platforms. + - Report information on the obstacles it hit on its path. +- Implement `serde` serialization/deserialization for `CollisionEvents` when the `serde-serialize` feature is enabled + + ### Modified - The methods `Collider::set_rotation`, `RigidBody::set_rotation`, and `RigidBody::set_next_kinematic_rotation` now take a rotation (`UnitQuaternion` or `UnitComplex`) instead of a vector/angle. - +- The method `QueryFilter::exclude_dynamic` is now a static method (the `self` argument was removed). +- The `QueryPipeline::cast_shape` method has a new argument `stop_at_penertation`. If set to `false`, the linear + shape-cast won’t immediately stop if the shape is penetrating another shape at its starti