Feat: implement a "small-steps" velocity-based constraints solver + joint improvements

examples2d/one_way_platforms2.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.5, 2.0);
             let body = RigidBodyBuilder::dynamic().translation(vector![20.0, 10.0]);

examples2d/rope_joints2.rs

@@ -51,9 +51,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])
-        .limits([2.0, 2.0]);
+    let joint = RopeJointBuilder::new(2.0).local_anchor2(point![0.0, 0.0]);
     impulse_joints.insert(character_handle, child_handle, joint, true);
 
     /*

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

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

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

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

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 {

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

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.

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);
 
     /*