aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--crates/rapier2d-f64/Cargo.toml2
-rw-r--r--crates/rapier2d/Cargo.toml2
-rw-r--r--crates/rapier3d-f64/Cargo.toml2
-rw-r--r--crates/rapier3d/Cargo.toml2
-rw-r--r--examples2d/all_examples2.rs2
-rw-r--r--examples2d/character_controller2.rs150
-rw-r--r--examples3d/all_examples3.rs4
-rw-r--r--examples3d/character_controller3.rs157
-rw-r--r--examples3d/newton_cradle3.rs45
-rw-r--r--src/control/character_controller.rs729
-rw-r--r--src/control/mod.rs8
-rw-r--r--src/lib.rs2
-rw-r--r--src/pipeline/query_pipeline.rs4
-rw-r--r--src_testbed/testbed.rs142
14 files changed, 1239 insertions, 12 deletions
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<Real>,
+ /// The translation that was already applied to the character when the hit happens.
+ pub translation_applied: Vector<Real>,
+ /// The translations that was still waiting to be applied to the character when the hit happens.
+ pub translation_remaining: Vector<Real>,
+ /// 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<Real>,
+ /// 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<CharacterAutostep>,
+ /// 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<CharacterLength>,
+}
+
+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<Real>,
+ /// 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<Real>,
+ desired_translation: Vector<Real>,
+ 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<Real>,
+ dims: &Vector2<Real>,
+ 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<Real>,
+ dims: &Vector2<Real>,
+ filter: QueryFilter,
+ mut kinematic_friction_translation: Option<&mut Vector<Real>>,
+ mut translation_remaining: Option<&mut Vector<Real>>,
+ ) -> bool {
+ let prediction = self.offset.eval(dims.y) * 1.1;
+
+ // TODO: allow custom dispatchers.
+ let dispatcher = DefaultQueryDispatcher;
+
+ let mut manifolds: Vec<ContactManifold> = 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(
+