feat: implement new "small-steps" solver + joint improvements

10 files changed, 549 insertions, 21 deletions

diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index ad4d226..851bc8a 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -34,7 +34,10 @@ mod heightfield3;
 mod joints3;
 // mod joints3;
 mod character_controller3;
+mod debug_chain_high_mass_ratio3;
+mod debug_cube_high_mass_ratio3;
 mod debug_internal_edges3;
+mod debug_long_chain3;
 mod joint_motor_position3;
 mod keva3;
 mod locked_rotations3;
@@ -45,8 +48,10 @@ mod primitives3;
 mod restitution3;
 mod rope_joints3;
 mod sensor3;
+mod spring_joints3;
 mod trimesh3;
 mod vehicle_controller3;
+mod vehicle_joints3;
 
 fn demo_name_from_command_line() -> Option<String> {
     let mut args = std::env::args();
@@ -105,8 +110,10 @@ pub fn main() {
         ("Restitution", restitution3::init_world),
         ("Rope Joints", rope_joints3::init_world),
         ("Sensor", sensor3::init_world),
+        ("Spring Joints", spring_joints3::init_world),
         ("TriMesh", trimesh3::init_world),
         ("Vehicle controller", vehicle_controller3::init_world),
+        ("Vehicle joints", vehicle_joints3::init_world),
         ("Keva tower", keva3::init_world),
         ("Newton cradle", newton_cradle3::init_world),
         ("(Debug) multibody_joints", debug_articulations3::init_world),
@@ -122,6 +129,15 @@ pub fn main() {
         ),
         ("(Debug) friction", debug_friction3::init_world),
         ("(Debug) internal edges", debug_internal_edges3::init_world),
+        ("(Debug) long chain", debug_long_chain3::init_world),
+        (
+            "(Debug) high mass ratio: chain",
+            debug_chain_high_mass_ratio3::init_world,
+        ),
+        (
+            "(Debug) high mass ratio: cube",
+            debug_cube_high_mass_ratio3::init_world,
+        ),
         ("(Debug) triangle", debug_triangle3::init_world),
         ("(Debug) trimesh", debug_trimesh3::init_world),
         ("(Debug) cylinder", debug_cylinder3::init_world),
diff --git a/examples3d/debug_chain_high_mass_ratio3.rs b/examples3d/debug_chain_high_mass_ratio3.rs
new file mode 100644
index 0000000..809311e
--- /dev/null
+++ b/examples3d/debug_chain_high_mass_ratio3.rs
@@ -0,0 +1,73 @@
+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 mut multibody_joints = MultibodyJointSet::new();
+    let use_articulations = false;
+
+    /*
+     * Create a chain with a very heavy ball at the end.
+     */
+    let num = 17;
+    let rad = 0.2;
+
+    let mut body_handles = Vec::new();
+
+    for i in 0..num {
+        let fi = i as f32;
+
+        let status = if i == 0 {
+            RigidBodyType::Fixed
+        } else {
+            RigidBodyType::Dynamic
+        };
+
+        let ball_rad = if i == num - 1 { rad * 10.0 } else { rad };
+        let shift1 = rad * 1.1;
+        let shift2 = ball_rad + rad * 0.1;
+        let z = if i == 0 {
+            0.0
+        } else {
+            (fi - 1.0) * 2.0 * shift1 + shift1 + shift2
+        };
+
+        let rigid_body = RigidBodyBuilder::new(status)
+            .translation(vector![0.0, 0.0, z])
+            .additional_solver_iterations(16);
+        let child_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::ball(ball_rad);
+        colliders.insert_with_parent(collider, child_handle, &mut bodies);
+
+        // Vertical joint.
+        if i > 0 {
+            let parent_handle = *body_handles.last().unwrap();
+            let joint = if i == 1 {
+                SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift1 * 2.0])
+            } else {
+                SphericalJointBuilder::new()
+                    .local_anchor1(point![0.0, 0.0, shift1])
+                    .local_anchor2(point![0.0, 0.0, -shift2])
+            };
+
+            if use_articulations {
+                multibody_joints.insert(parent_handle, child_handle, joint, true);
+            } else {
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
+            }
+        }
+
+        body_handles.push(child_handle);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
+}
diff --git a/examples3d/debug_cube_high_mass_ratio3.rs b/examples3d/debug_cube_high_mass_ratio3.rs
new file mode 100644
index 0000000..ffaf793
--- /dev/null
+++ b/examples3d/debug_cube_high_mass_ratio3.rs
@@ -0,0 +1,96 @@
+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 impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    let num_levels = 4;
+    let stick_len = 2.0;
+    let stick_rad = 0.2;
+
+    /*
+     * Floor.
+     */
+    let floor_body =
+        RigidBodyBuilder::fixed().translation(vector![0.0, -stick_len - stick_rad, 0.0]);
+    let floor_handle = bodies.insert(floor_body);
+    let floor_cube = ColliderBuilder::cuboid(stick_len, stick_len, stick_len);
+    colliders.insert_with_parent(floor_cube, floor_handle, &mut bodies);
+
+    /*
+     * Create a stack of capsule with a very heavy cube on top.
+     */
+    for i in 0..num_levels {
+        let fi = i as f32;
+
+        let body = RigidBodyBuilder::dynamic().translation(vector![
+            0.0,
+            fi * stick_rad * 4.0,
+            -(stick_len / 2.0 - stick_rad)
+        ]);
+        let handle = bodies.insert(body);
+        let capsule = ColliderBuilder::cuboid(stick_len / 2.0, stick_rad, stick_rad);
+        colliders.insert_with_parent(capsule, handle, &mut bodies);
+
+        let body = RigidBodyBuilder::dynamic().translation(vector![
+            0.0,
+            fi * stick_rad * 4.0,
+            (stick_len / 2.0 - stick_rad)
+        ]);
+        let handle = bodies.insert(body);
+        let capsule = ColliderBuilder::cuboid(stick_len / 2.0, stick_rad, stick_rad);
+        colliders.insert_with_parent(capsule, handle, &mut bodies);
+
+        let body = RigidBodyBuilder::dynamic().translation(vector![
+            -(stick_len / 2.0 - stick_rad),
+            (fi + 0.5) * stick_rad * 4.0,
+            0.0
+        ]);
+        let handle = bodies.insert(body);
+        let capsule = ColliderBuilder::cuboid(stick_rad, stick_rad, stick_len / 2.0);
+        colliders.insert_with_parent(capsule, handle, &mut bodies);
+
+        let body = RigidBodyBuilder::dynamic().translation(vector![
+            (stick_len / 2.0 - stick_rad),
+            (fi + 0.5) * stick_rad * 4.0,
+            0.0
+        ]);
+        let handle = bodies.insert(body);
+        let capsule = ColliderBuilder::cuboid(stick_rad, stick_rad, stick_len / 2.0);
+        colliders.insert_with_parent(capsule, handle, &mut bodies);
+    }
+
+    /*
+     * Big cube on top.
+     */
+    let cube_len = stick_len * 2.0;
+    let floor_body = RigidBodyBuilder::dynamic()
+        .translation(vector![
+            0.0,
+            cube_len / 2.0 + (num_levels as f32 - 0.25) * stick_rad * 4.0,
+            0.0
+        ])
+        .additional_solver_iterations(36);
+    let floor_handle = bodies.insert(floor_body);
+    let floor_cube = ColliderBuilder::cuboid(cube_len / 2.0, cube_len / 2.0, cube_len / 2.0);
+    colliders.insert_with_parent(floor_cube, floor_handle, &mut bodies);
+
+    let small_mass =
+        MassProperties::from_cuboid(1.0, vector![stick_rad, stick_rad, stick_len / 2.0]).mass();
+    let big_mass =
+        MassProperties::from_cuboid(1.0, vector![cube_len / 2.0, cube_len / 2.0, cube_len / 2.0])
+            .mass();
+    println!("debug_cube_high_mass_ratio3: small stick mass: {small_mass}, big cube mass: {big_mass}, mass_ratio: {}", big_mass / small_mass);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
+}
diff --git a/examples3d/debug_long_chain3.rs b/examples3d/debug_long_chain3.rs
new file mode 100644
index 0000000..e2bb990
--- /dev/null
+++ b/examples3d/debug_long_chain3.rs
@@ -0,0 +1,63 @@
+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 mut multibody_joints = MultibodyJointSet::new();
+    let use_articulations = false;
+
+    /*
+     * Create the long chain.
+     */
+    let num = 100;
+    let rad = 0.2;
+    let shift = rad * 2.2;
+
+    let mut body_handles = Vec::new();
+
+    for i in 0..num {
+        let fi = i as f32;
+
+        let status = if i == 0 {
+            RigidBodyType::Fixed
+        } else {
+            RigidBodyType::Dynamic
+        };
+
+        let rigid_body = RigidBodyBuilder::new(status).translation(vector![0.0, 0.0, fi * shift]);
+        let child_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::ball(rad);
+        colliders.insert_with_parent(collider, child_handle, &mut bodies);
+
+        // Vertical joint.
+        if i > 0 {
+            let parent_handle = *body_handles.last().unwrap();
+            let joint = if i == 1 {
+                SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift])
+            } else {
+                SphericalJointBuilder::new()
+                    .local_anchor1(point![0.0, 0.0, shift / 2.0])
+                    .local_anchor2(point![0.0, 0.0, -shift / 2.0])
+            };
+
+            if use_articulations {
+                multibody_joints.insert(parent_handle, child_handle, joint, true);
+            } else {
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
+            }
+        }
+
+        body_handles.push(child_handle);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
+}
diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs
index dc4deed..18a11e7 100644
--- a/examples3d/joints3.rs
+++ b/examples3d/joints3.rs
@@ -20,7 +20,6 @@ fn create_coupled_joints(
     let joint1 = GenericJointBuilder::new(JointAxesMask::empty())
         .limits(JointAxis::X, [-3.0, 3.0])
         .limits(JointAxis::Y, [0.0, 3.0])
-        .limits(JointAxis::Z, [0.0, 3.0])
         .coupled_axes(JointAxesMask::Y | JointAxesMask::Z);
 
     if use_articulations {
diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs
index 3b2715a..1a5704c 100644
--- a/examples3d/one_way_platforms3.rs
+++ b/examples3d/one_way_platforms3.rs
@@ -90,7 +90,7 @@ pub fn init_world(testbed: &mut Testbed) {
      * depending on their position.
      */
     testbed.add_callback(move |graphics, physics, _, run_state| {
-        if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 {
+        if run_state.timestep_id % 200 == 0 && physics.bodies.len() <= 7 {
             // Spawn a new cube.
             let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.5);
             let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 6.0, 20.0]);
diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs
index d42cb76..9025afa 100644
--- a/examples3d/platform3.rs
+++ b/examples3d/platform3.rs
@@ -29,12 +29,12 @@ pub fn init_world(testbed: &mut Testbed) {
 
     let shift = rad * 2.0;
     let centerx = shift * num as f32 / 2.0;
-    let centery = shift / 2.0 + 3.04;
     let centerz = shift * num as f32 / 2.0;
 
     for i in 0usize..num {
         for j in 0usize..num {
             for k in 0usize..num {
+                let centery = if j >= num / 2 { 5.0 } else { 3.0 };
                 let x = i as f32 * shift - centerx;
                 let y = j as f32 * shift + centery;
                 let z = k as f32 * shift - centerz;
@@ -51,11 +51,8 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Setup a velocity-based kinematic rigid body.
      */
-    let platform_body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![
-        0.0,
-        1.5 + 0.8,
-        -10.0 * rad
-    ]);
+    let platform_body =
+        RigidBodyBuilder::kinematic_velocity_based().translation(vector![0.0, 1.5 + 0.8, 0.0]);
     let velocity_based_platform_handle = bodies.insert(platform_body);
     let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0);
     colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
@@ -65,8 +62,8 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let platform_body = RigidBodyBuilder::kinematic_position_based().translation(vector![
         0.0,
-        2.0 + 1.5 + 0.8,
-        -10.0 * rad
+        3.0 + 1.5 + 0.8,
+        0.0
     ]);
     let position_based_platform_handle = bodies.insert(platform_body);
     let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0);
@@ -75,22 +72,17 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Setup a callback to control the platform.
      */
-    let mut count = 0;
     testbed.add_callback(move |_, physics, _, run_state| {
-        count += 1;
-        if count % 100 > 50 {
-            return;
-        }
-
         let velocity = vector![
             0.0,
-            (run_state.time * 5.0).sin(),
-            run_state.time.sin() * 5.0
+            (run_state.time * 2.0).sin(),
+            run_state.time.sin() * 2.0
         ];
 
         // Update the velocity-based kinematic body by setting its velocity.
         if let Some(platform) = physics.bodies.get_mut(velocity_based_platform_handle) {
             platform.set_linvel(velocity, true);
+            platform.set_angvel(vector![0.0, 0.2, 0.0], true);
         }
 
         // Update the position-based kinematic body by setting its next position.
diff --git a/examples3d/rope_joints3.rs b/examples3d/rope_joints3.rs
index 329364b..9696682 100644
--- a/examples3d/rope_joints3.rs
+++ b/examples3d/rope_joints3.rs
@@ -80,9 +80,7 @@ pub fn init_world(testbed: &mut Testbed) {
     let collider = ColliderBuilder::ball(rad);
     colliders.insert_with_parent(collider, child_handle, &mut bodies);
 
-    let joint = RopeJointBuilder::new()
-        .local_anchor2(point![0.0, 0.0, 0.0])
-        .limits([2.0, 2.0]);
+    let joint = RopeJointBuilder::new(2.0);
     impulse_joints.insert(character_handle, child_handle, joint, true);
 
     /*
diff --git a/examples3d/spring_joints3.rs b/examples3d/spring_joints3.rs
new file mode 100644
index 0000000..3336402
--- /dev/null
+++ b/examples3d/spring_joints3.rs
@@ -0,0 +1,64 @@
+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();
+
+    /*
+     * Fixed ground to attach one end of the joints.
+     */
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+
+    /*
+     * Spring joints with a variety of spring parameters.
+     * The middle one has uses critical damping.
+     */
+    let num = 30;
+    let radius = 0.5;
+    let mass = Ball::new(radius).mass_properties(1.0).mass();
+    let stiffness = 1.0e3;
+    let critical_damping = 2.0 * (stiffness * mass).sqrt();
+    for i in 0..=num {
+        let x_pos = -6.0 + 1.5 * i as f32;
+        let ball_pos = point![x_pos, 4.5, 0.0];
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .translation(ball_pos.coords)
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::ball(radius);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let damping_ratio = i as f32 / (num as f32 / 2.0);
+        let damping = damping_ratio * critical_damping;
+        let joint = SpringJointBuilder::new(0.0, stiffness, damping)
+            .local_anchor1(ball_pos - Vector::y() * 3.0);
+        impulse_joints.insert(ground_handle, handle, joint, true);
+
+        // Box that will fall on to of the springed balls, makes the simulation funier to watch.
+        let rigid_body =
+            RigidBodyBuilder::dynamic().translation(ball_pos.coords + Vector::y() * 5.0);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(radius, radius, radius).density(100.0);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world_with_params(
+        bodies,
+        colliders,
+        impulse_joints,
+        multibody_joints,
+        vector![0.0, -9.81, 0.0],
+        (),
+    );
+    testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]);
+}
diff --git a/examples3d/vehicle_joints3.rs b/examples3d/vehicle_joints3.rs
new file mode 100644
index 0000000..ddd672c
--- /dev/null
+++ b/examples3d/vehicle_joints3.rs
@@ -0,0 +1,227 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::{KeyCode, 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 = Vector::new(60.0, 0.4, 60.0);
+    let nsubdivs = 100;
+
+    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![-7.0, 0.0, 0.0])
+        .friction(1.0);
+    colliders.insert(collider);
+
+    /*
+     * Vehicle we will control manually, simulated using joints.
+     * Strongly inspired from https://github.com/h3r2tic/cornell-mcray/blob/main/src/car.rs (MIT/Apache license).
+     */
+    const CAR_GROUP: Group = Group::GROUP_1;
+
+    let wheel_params = [
+        vector![0.6874, 0.2783, -0.7802],
+        vector![-0.6874, 0.2783, -0.7802],
+        vector![0.64, 0.2783, 1.0254],
+        vector![-0.64, 0.2783, 1.0254],
+    ];
+    // TODO: lower center of mass?
+    // let mut center_of_mass = wheel_params.iter().sum().unwrap() / 4.0;
+    // center_of_mass.y = 0.0;
+
+    let suspension_height = 0.12;
+    let max_steering_angle = 35.0f32.to_radians();
+    let drive_strength = 1.0;
+    let wheel_radius = 0.28;
+    let car_position = point![0.0, wheel_radius + suspension_height, 0.0];
+    let body_position_in_car_space = vector![0.0, 0.4739, 0.0];
+
+    let body_position = car_position + body_position_in_car_space;
+
+    let body_co = ColliderBuilder::cuboid(0.65, 0.3, 0.9)
+        .density(100.0)
+        .collision_groups(InteractionGroups::new(CAR_GROUP, !CAR_GROUP));
+    let body_rb = RigidBodyBuilder::dynamic()
+        .position(body_position.into())
+        .build();
+    let body_handle = bodies.insert(body_rb);
+    colliders.insert_with_parent(body_co, body_handle, &mut bodies);
+
+    let mut steering_joints = vec![];
+    let mut motor_joints = vec![];
+
+    for (wheel_id, wheel_pos_in_car_space) in wheel_params.into_iter().enumerate() {
+        let is_front = wheel_id >= 2;
+        let wheel_center = car_position + wheel_pos_in_car_space;
+
+        let axle_mass_props = MassProperties::from_ball(100.0, wheel_radius);
+        let axle_rb = RigidBodyBuilder::dynamic()
+            .position(wheel_center.into())
+            .additional_mass_properties(axle_mass_props);
+        let axle_handle = bodies.insert(axle_rb);
+
+        // This is a fake cylinder collider that we add only because our testbed can
+        // only render colliders. Setting it as sensor makes it show up as wireframe.
+        let wheel_fake_co = ColliderBuilder::cylinder(wheel_radius / 2.0, wheel_radius)
+            .rotation(Vector::z() * std::f32::consts::FRAC_PI_2)
+            .sensor(true)
+            .density(0.0)
+            .collision_groups(InteractionGroups::none());
+
+        // The actual wheel collider. Simulating the wheel as a ball is interesting as it
+        // is mathematically simpler than a cylinder and cheaper to compute for collision-detection.
+        let wheel_co = ColliderBuilder::ball(wheel_radius)
+            .density(100.0)
+            .collision_groups(InteractionGroups::new(CAR_GROUP, !CAR_GROUP))
+            .friction(1.0);
+        let wheel_rb = RigidBodyBuilder::dynamic().position(wheel_center.into());
+        let wheel_handle = bodies.insert(wheel_rb);
+        colliders.insert_with_parent(wheel_co, wheel_handle, &mut bodies);
+        colliders.insert_with_parent(wheel_fake_co, wheel_handle, &mut bodies);
+
+        let suspension_attachment_in_body_space =
+            wheel_pos_in_car_space - body_position_in_car_space;
+
+        // Suspension between the body and the axle.
+        let mut locked_axes =
+            JointAxesMask::X | JointAxesMask::Z | JointAxesMask::ANG_X | JointAxesMask::ANG_Z;
+        if !is_front {
+            locked_axes |= JointAxesMask::ANG_Y;
+        }
+
+        let mut suspension_joint = GenericJointBuilder::new(locked_axes)
+            .limits(JointAxis::Y, [0.0, suspension_height])
+            .motor_position(JointAxis::Y, 0.0, 1.0e4, 1.0e3)
+            .local_anchor1(suspension_attachment_in_body_space.into());
+
+        if is_front {
+            suspension_joint =
+                suspension_joint.limits(JointAxis::AngY, [-max_steering_angle, max_steering_angle]);
+        }
+
+        let body_axle_joint_handle =
+            impulse_joints.insert(body_handle, axle_handle, suspension_joint, true);
+
+        // Joint between the axle and the wheel.
+        let wheel_joint = RevoluteJointBuilder::new(Vector::x_axis());
+        let wheel_joint_handle =
+            impulse_joints.insert(axle_handle, wheel_handle, wheel_joint, true);
+
+        if is_front {
+            steering_joints.push(body_axle_joint_handle);
+            motor_joints.push(wheel_joint_handle);
+        }
+    }
+
+    /*
+     * Callback to control the wheels motors.
+     */
+    testbed.add_callback(move |gfx, physics, _, _| {
+        let Some(gfx) = gfx else { return };
+
+        let mut thrust = 0.0;
+        let mut steering = 0.0;
+        let mut boost = 1.0;
+
+        for key in gfx.keys().get_pressed() {
+            match *key {
+                KeyCode::Right => {
+                    steering = -1.0;
+                }
+                KeyCode::Left => {
+                    steering = 1.0;
+                }
+                KeyCode::Up => {
+                    thrust = -drive_strength;
+                }
+                KeyCode::Down => {
+                    thrust = drive_strength;
+                }
+                KeyCode::ShiftRight => {
+                    boost = 1.5;
+                }
+                _ => {}
+            }
+        }
+
+        if thrust != 0.0 || steering != 0.0 {
+            physics.bodies.get_mut(body_handle).unwrap().wake_up(true);
+        }
+
+        // Apply steering to the axles.
+        for steering_handle in &steering_joints {
+            let steering_joint = physics.impulse_joints.get_mut(*steering_handle).unwrap();
+            steering_joint.data.set_motor_position(
+                JointAxis::AngY,
+                max_steering_angle * steering,
+                1.0e4,
+                1.0e3,
+            );
+        }
+
+        // Apply thrust.
+        // Pseudo-differential adjusting speed of engines depending on steering arc
+        // Higher values result in more drifty behavior.
+        let differential_strength = 0.5;
+        let sideways_shift = (max_steering_angle * steering).sin() * differential_strength;
+        let speed_diff = if sideways_shift > 0.0 {
+            f32::hypot(1.0, sideways_shift)
+        } else {
+            1.0 / f32::hypot(1.0, sideways_shift)
+        };
+
+        let ms = [1.0 / speed_diff, speed_diff];
+        for (motor_handle, &ms) in motor_joints.iter().copied().zip(ms.iter()) {
+            let motor_joint = physics.impulse_joints.get_mut(motor_handle).unwrap();
+            motor_joint.data.set_motor_velocity(
+                JointAxis::AngX,
+                -30.0 * thrust * ms * boost,
+                1.0e2,
+            );
+        }
+    });
+
+    /*
+     * Create some cubes on the ground.
+     */
+    // 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..1 {
+    //     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);
+    //         }
+    //     }
+    // }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point!(10.0, 10.0, 10.0), Point::origin());
+}