Add a character controller implementation

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 }

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 }

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 }

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 }

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

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

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",

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

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

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(
+                                        &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<Real>,
+    ) -> (Option<Vector<Real>>, 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<Real>,
+        dims: &Vector2<Real>,
+        mut filter: QueryFilter,
+        stair_handle: ColliderHandle,
+        translation_remaining: &mut Vector<Real>,
+        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<ContactManifold> = 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,
+                    );
+                }
+            }
+        }
+    }
+}

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;

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::*;

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<Real>,
         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)
     }

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;
+use rapier::pipeline::{PhysicsHooks, QueryFilter};
-#[cfg(feature = "dim3")]
-use rapier::{geometry::Ray, pipeline::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<RigidBodyHandle>,
+    pub character_body: Option<RigidBodyHandle>,
     //    pub grabbed_object: Option<DefaultBodyPartHandle>,
     //    pub grabbed_object_constraint: Option<DefaultJointConstraintHandle>,
     pub grabbed_object_plane: (Point3<f32>, Vector3<f32>),
@@ -133,6 +135,7 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
     meshes: &'a mut Assets<Mesh>,
     materials: &'a mut Assets<BevyMaterial>,
     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<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::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<KeyCode>) {
         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));