From 9b87f06a856c4d673642e210f8b0986cfdbac3af Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 21 Jan 2024 21:02:23 +0100 Subject: feat: implement new "small-steps" solver + joint improvements --- examples2d/one_way_platforms2.rs | 2 +- examples2d/rope_joints2.rs | 4 +- examples3d/all_examples3.rs | 16 + examples3d/debug_chain_high_mass_ratio3.rs | 73 + examples3d/debug_cube_high_mass_ratio3.rs | 96 ++ examples3d/debug_long_chain3.rs | 63 + examples3d/joints3.rs | 1 - examples3d/one_way_platforms3.rs | 2 +- examples3d/platform3.rs | 24 +- examples3d/rope_joints3.rs | 4 +- examples3d/spring_joints3.rs | 64 + examples3d/vehicle_joints3.rs | 227 ++++ src/control/ray_cast_vehicle_controller.rs | 2 +- src/dynamics/ccd/mod.rs | 3 + src/dynamics/integration_parameters.rs | 26 +- src/dynamics/island_manager.rs | 30 +- src/dynamics/joint/generic_joint.rs | 25 +- .../joint/impulse_joint/impulse_joint_set.rs | 2 +- src/dynamics/joint/mod.rs | 2 + src/dynamics/joint/multibody_joint/mod.rs | 4 +- src/dynamics/joint/multibody_joint/multibody.rs | 56 +- .../joint/multibody_joint/multibody_joint.rs | 30 +- .../joint/multibody_joint/multibody_joint_set.rs | 46 +- .../joint/multibody_joint/unit_multibody_joint.rs | 51 +- src/dynamics/joint/rope_joint.rs | 104 +- src/dynamics/joint/spring_joint.rs | 172 +++ src/dynamics/mod.rs | 6 +- src/dynamics/rigid_body.rs | 41 +- src/dynamics/rigid_body_components.rs | 45 +- src/dynamics/solver/categorization.rs | 32 +- .../contact_constraint/contact_constraints_set.rs | 528 ++++++++ .../generic_one_body_constraint.rs | 292 ++++ .../generic_one_body_constraint_element.rs | 147 ++ .../generic_two_body_constraint.rs | 433 ++++++ .../generic_two_body_constraint_element.rs | 367 +++++ src/dynamics/solver/contact_constraint/mod.rs | 29 + .../contact_constraint/one_body_constraint.rs | 382 ++++++ .../one_body_constraint_element.rs | 232 ++++ .../contact_constraint/one_body_constraint_simd.rs | 372 ++++++ .../contact_constraint/two_body_constraint.rs | 470 +++++++ .../two_body_constraint_element.rs | 271 ++++ .../contact_constraint/two_body_constraint_simd.rs | 370 ++++++ src/dynamics/solver/delta_vel.rs | 57 - src/dynamics/solver/generic_velocity_constraint.rs | 378 ------ .../solver/generic_velocity_constraint_element.rs | 349 ----- .../solver/generic_velocity_ground_constraint.rs | 240 ---- .../generic_velocity_ground_constraint_element.rs | 143 -- src/dynamics/solver/interaction_groups.rs | 18 +- src/dynamics/solver/island_solver.rs | 93 +- .../joint_constraint/any_joint_constraint.rs | 97 ++ .../solver/joint_constraint/joint_constraint.rs | 540 -------- .../joint_constraint/joint_constraint_builder.rs | 1401 ++++++++++++++++++++ .../joint_constraint/joint_constraints_set.rs | 446 +++++++ .../joint_constraint/joint_generic_constraint.rs | 552 ++++++++ .../joint_generic_constraint_builder.rs | 1286 ++++++++++++++++++ .../joint_generic_velocity_constraint.rs | 562 -------- .../joint_generic_velocity_constraint_builder.rs | 886 ------------- .../joint_constraint/joint_velocity_constraint.rs | 387 +++--- .../joint_velocity_constraint_builder.rs | 1127 ---------------- src/dynamics/solver/joint_constraint/mod.rs | 23 +- src/dynamics/solver/mod.rs | 81 +- src/dynamics/solver/parallel_island_solver.rs | 34 +- src/dynamics/solver/parallel_solver_constraints.rs | 169 ++- src/dynamics/solver/parallel_velocity_solver.rs | 88 +- src/dynamics/solver/solver_body.rs | 59 + src/dynamics/solver/solver_constraints.rs | 564 -------- src/dynamics/solver/solver_constraints_set.rs | 241 ++++ src/dynamics/solver/solver_vel.rs | 59 + src/dynamics/solver/velocity_constraint.rs | 441 ------ src/dynamics/solver/velocity_constraint_element.rs | 211 --- src/dynamics/solver/velocity_constraint_wide.rs | 294 ---- src/dynamics/solver/velocity_ground_constraint.rs | 281 ---- .../solver/velocity_ground_constraint_element.rs | 181 --- .../solver/velocity_ground_constraint_wide.rs | 277 ---- src/dynamics/solver/velocity_solver.rs | 406 +++--- src/geometry/contact_pair.rs | 2 +- src/geometry/narrow_phase.rs | 8 +- .../debug_render_pipeline/debug_render_pipeline.rs | 4 +- src/pipeline/physics_pipeline.rs | 31 +- src/utils.rs | 108 +- src_testbed/box2d_backend.rs | 7 +- src_testbed/lib.rs | 1 + src_testbed/physx_backend.rs | 7 +- src_testbed/testbed.rs | 17 +- src_testbed/ui.rs | 43 +- 85 files changed, 9856 insertions(+), 7489 deletions(-) create mode 100644 examples3d/debug_chain_high_mass_ratio3.rs create mode 100644 examples3d/debug_cube_high_mass_ratio3.rs create mode 100644 examples3d/debug_long_chain3.rs create mode 100644 examples3d/spring_joints3.rs create mode 100644 examples3d/vehicle_joints3.rs create mode 100644 src/dynamics/joint/spring_joint.rs create mode 100644 src/dynamics/solver/contact_constraint/contact_constraints_set.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/mod.rs create mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs create mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint.rs create mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint_element.rs create mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs delete mode 100644 src/dynamics/solver/delta_vel.rs delete mode 100644 src/dynamics/solver/generic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/generic_velocity_constraint_element.rs delete mode 100644 src/dynamics/solver/generic_velocity_ground_constraint.rs delete mode 100644 src/dynamics/solver/generic_velocity_ground_constraint_element.rs create mode 100644 src/dynamics/solver/joint_constraint/any_joint_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint_builder.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraints_set.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs create mode 100644 src/dynamics/solver/solver_body.rs delete mode 100644 src/dynamics/solver/solver_constraints.rs create mode 100644 src/dynamics/solver/solver_constraints_set.rs create mode 100644 src/dynamics/solver/solver_vel.rs delete mode 100644 src/dynamics/solver/velocity_constraint.rs delete mode 100644 src/dynamics/solver/velocity_constraint_element.rs delete mode 100644 src/dynamics/solver/velocity_constraint_wide.rs delete mode 100644 src/dynamics/solver/velocity_ground_constraint.rs delete mode 100644 src/dynamics/solver/velocity_ground_constraint_element.rs delete mode 100644 src/dynamics/solver/velocity_ground_constraint_wide.rs diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index dd50201..d16d0cc 100644 --- a/examples2d/one_way_platforms2.rs +++ b/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]); diff --git a/examples2d/rope_joints2.rs b/examples2d/rope_joints2.rs index 451dd1d..ec9b201 100644 --- a/examples2d/rope_joints2.rs +++ b/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); /* 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 { 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()); +} diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs index 1e2da51..30979e1 100644 --- a/src/control/ray_cast_vehicle_controller.rs +++ b/src/control/ray_cast_vehicle_controller.rs @@ -4,7 +4,7 @@ use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ColliderHandle, ColliderSet, Ray}; use crate::math::{Point, Real, Rotation, Vector}; use crate::pipeline::{QueryFilter, QueryPipeline}; -use crate::utils::{WCross, WDot}; +use crate::utils::{SimdCross, SimdDot}; /// A character controller to simulate vehicles using ray-casting for the wheels. pub struct DynamicRayCastVehicleController { diff --git a/src/dynamics/ccd/mod.rs b/src/dynamics/ccd/mod.rs index 84807fa..a73ca85 100644 --- a/src/dynamics/ccd/mod.rs +++ b/src/dynamics/ccd/mod.rs @@ -1,3 +1,6 @@ +// TODO: not sure why it complains about PredictedImpacts being unused, +// making it private or pub(crate) triggers a different error. +#[allow(unused_imports)] pub use self::ccd_solver::{CCDSolver, PredictedImpacts}; pub use self::toi_entry::TOIEntry; diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 6d3acc5..d07527f 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,4 +1,5 @@ use crate::math::Real; +use std::num::NonZeroUsize; /// Parameters for a time-step of the physics engine. #[derive(Copy, Clone, Debug)] @@ -43,15 +44,10 @@ pub struct IntegrationParameters { pub max_penetration_correction: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). pub prediction_distance: Real, - /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`). - pub max_velocity_iterations: usize, - /// Maximum number of iterations performed to solve friction constraints (default: `8`). - pub max_velocity_friction_iterations: usize, - /// Maximum number of iterations performed to remove the energy introduced by penetration corrections (default: `1`). - pub max_stabilization_iterations: usize, - /// If `false`, friction and non-penetration constraints will be solved in the same loop. Otherwise, - /// non-penetration constraints are solved first, and friction constraints are solved after (default: `true`). - pub interleave_restitution_and_friction_resolution: bool, + /// Number of iterations performed to solve friction constraints at solver iteration (default: `2`). + pub num_friction_iteration_per_solver_iteration: usize, + /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`). + pub num_solver_iterations: NonZeroUsize, /// Minimum number of dynamic bodies in each active island (default: `128`). pub min_island_size: usize, /// Maximum number of substeps performed by the solver (default: `1`). @@ -151,17 +147,15 @@ impl Default for IntegrationParameters { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, - erp: 0.8, - damping_ratio: 0.25, + erp: 0.6, + damping_ratio: 1.0, joint_erp: 1.0, joint_damping_ratio: 1.0, - allowed_linear_error: 0.001, // 0.005 + allowed_linear_error: 0.001, max_penetration_correction: Real::MAX, prediction_distance: 0.002, - max_velocity_iterations: 4, - max_velocity_friction_iterations: 8, - max_stabilization_iterations: 1, - interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability. + num_friction_iteration_per_solver_iteration: 2, + num_solver_iterations: NonZeroUsize::new(4).unwrap(), // TODO: what is the optimal value for min_island_size? // It should not be too big so that we don't end up with // huge islands that don't fit in cache. diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index ef5d50a..4d14147 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -4,7 +4,7 @@ use crate::dynamics::{ }; use crate::geometry::{ColliderSet, NarrowPhase}; use crate::math::Real; -use crate::utils::WDot; +use crate::utils::SimdDot; /// Structure responsible for maintaining the set of active rigid-bodies, and /// putting non-moving rigid-bodies to sleep to save computation times. @@ -14,6 +14,7 @@ pub struct IslandManager { pub(crate) active_dynamic_set: Vec, pub(crate) active_kinematic_set: Vec, pub(crate) active_islands: Vec, + pub(crate) active_islands_additional_solver_iterations: Vec, active_set_timestamp: u32, #[cfg_attr(feature = "serde-serialize", serde(skip))] can_sleep: Vec, // Workspace. @@ -28,6 +29,7 @@ impl IslandManager { active_dynamic_set: vec![], active_kinematic_set: vec![], active_islands: vec![], + active_islands_additional_solver_iterations: vec![], active_set_timestamp: 0, can_sleep: vec![], stack: vec![], @@ -127,6 +129,10 @@ impl IslandManager { &self.active_dynamic_set[island_range] } + pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize { + self.active_islands_additional_solver_iterations[island_id] + } + #[inline(always)] pub(crate) fn iter_active_bodies<'a>(&'a self) -> impl Iterator + 'a { self.active_dynamic_set @@ -232,12 +238,21 @@ impl IslandManager { // let t = instant::now(); // Propagation of awake state and awake island computation through the // traversal of the interaction graph. + self.active_islands_additional_solver_iterations.clear(); self.active_islands.clear(); self.active_islands.push(0); // The max avoid underflow when the stack is empty. let mut island_marker = self.stack.len().max(1) - 1; + // NOTE: islands containing a body with non-standard number of iterations won’t + // be merged with another island, unless another island with standard + // iterations number already started before and got continued due to the + // `min_island_size`. That could be avoided by pushing bodies with non-standard + // iterations on top of the stack (and other bodies on the back). Not sure it’s + // worth it though. + let mut additional_solver_iterations = 0; + while let Some(handle) = self.stack.pop() { let rb = bodies.index_mut_internal(handle); @@ -248,16 +263,23 @@ impl IslandManager { } if self.stack.len() < island_marker { - if self.active_dynamic_set.len() - *self.active_islands.last().unwrap() - >= min_island_size + if additional_solver_iterations != rb.additional_solver_iterations + || self.active_dynamic_set.len() - *self.active_islands.last().unwrap() + >= min_island_size { // We are starting a new island. + self.active_islands_additional_solver_iterations + .push(additional_solver_iterations); self.active_islands.push(self.active_dynamic_set.len()); + additional_solver_iterations = 0; } island_marker = self.stack.len(); } + additional_solver_iterations = + additional_solver_iterations.max(rb.additional_solver_iterations); + // Transmit the active state to all the rigid-bodies with colliders // in contact or joined with this collider. push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); @@ -281,6 +303,8 @@ impl IslandManager { self.active_dynamic_set.push(handle); } + self.active_islands_additional_solver_iterations + .push(additional_solver_iterations); self.active_islands.push(self.active_dynamic_set.len()); // println!( // "Extraction: {}, num islands: {}", diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 5096e15..83e27be 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,7 +1,7 @@ use crate::dynamics::solver::MotorParameters; use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint}; use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM}; -use crate::utils::{WBasis, WReal}; +use crate::utils::{SimdBasis, SimdRealCopy}; #[cfg(feature = "dim3")] use crate::dynamics::SphericalJoint; @@ -121,7 +121,7 @@ pub struct JointLimits { pub impulse: N, } -impl Default for JointLimits { +impl Default for JointLimits { fn default() -> Self { Self { min: -N::splat(Real::MAX), @@ -131,6 +131,16 @@ impl Default for JointLimits { } } +impl From<[N; 2]> for JointLimits { + fn from(value: [N; 2]) -> Self { + Self { + min: value[0], + max: value[1], + impulse: N::splat(0.0), + } + } +} + /// A joint’s motor along one of its degrees of freedom. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] @@ -210,14 +220,23 @@ pub struct GenericJoint { /// The degrees-of-freedoms motorised by this joint. pub motor_axes: JointAxesMask, /// The coupled degrees of freedom of this joint. + /// + /// Note that coupling degrees of freedoms (DoF) changes the interpretation of the coupled joint’s limits and motors. + /// If multiple linear DoF are limited/motorized, only the limits/motor configuration for the first + /// coupled linear DoF is applied to all coupled linear DoF. Similarly, if multiple angular DoF are limited/motorized + /// only the limits/motor configuration for the first coupled angular DoF is applied to all coupled angular DoF. pub coupled_axes: JointAxesMask, /// The limits, along each degrees of freedoms of this joint. /// /// Note that the limit must also be explicitly enabled by the `limit_axes` bitmask. + /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF limit and `limit_axis` + /// bitmask is applied to the coupled linear (resp. angular) axes. pub limits: [JointLimits; SPATIAL_DIM], /// The motors, along each degrees of freedoms of this joint. /// - /// Note that the mostor must also be explicitly enabled by the `motors` bitmask. + /// Note that the motor must also be explicitly enabled by the `motor_axes` bitmask. + /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes` + /// bitmask is applied to the coupled linear (resp. angular) axes. pub motors: [JointMotor; SPATIAL_DIM], /// Are contacts between the attached rigid-bodies enabled? pub contacts_enabled: bool, diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index f21f950..faae1f6 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -214,7 +214,7 @@ impl ImpulseJointSet { // // .map(|e| &mut e.weight) // } - #[cfg(not(feature = "parallel"))] + // #[cfg(not(feature = "parallel"))] pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] { &mut self.joint_graph.graph.edges[..] } diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 93cb0ab..423d4c2 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -6,6 +6,7 @@ pub use self::multibody_joint::*; pub use self::prismatic_joint::*; pub use self::revolute_joint::*; pub use self::rope_joint::*; +pub use self::spring_joint::*; #[cfg(feature = "dim3")] pub use self::spherical_joint::*; @@ -21,3 +22,4 @@ mod rope_joint; #[cfg(feature = "dim3")] mod spherical_joint; +mod spring_joint; diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index a701350..c789004 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -2,7 +2,9 @@ pub use self::multibody::Multibody; pub use self::multibody_joint::MultibodyJoint; -pub use self::multibody_joint_set::{MultibodyIndex, MultibodyJointHandle, MultibodyJointSet}; +pub use self::multibody_joint_set::{ + MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId, +}; pub use self::multibody_link::MultibodyLink; pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint}; diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 63e87e2..c3092ae 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1,16 +1,13 @@ use super::multibody_link::{MultibodyLink, MultibodyLinkVec}; use super::multibody_workspace::MultibodyWorkspace; -use crate::dynamics::{ - solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet, - RigidBodyType, RigidBodyVelocity, -}; +use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity}; #[cfg(feature = "dim3")] use crate::math::Matrix; use crate::math::{ AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM, }; use crate::prelude::MultibodyJoint; -use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix}; +use crate::utils::{IndexMut2, SimdAngularInertia, SimdCross, SimdCrossMatrix}; use na::{self, DMatrix, DVector, DVectorView, DVectorViewMut, Dyn, OMatrix, SMatrix, SVector, LU}; #[repr(C)] @@ -372,6 +369,7 @@ impl Multibody { self.accelerations.fill(0.0); + // Eqn 42 to 45 for i in 0..self.links.len() { let link = &self.links[i]; let rb = &bodies[link.rigid_body]; @@ -400,7 +398,7 @@ impl Multibody { } acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23)); - acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23); + acc.linvel += acc.angvel.gcross(link.shift23); self.workspace.accs[i] = acc; @@ -728,7 +726,7 @@ impl Multibody { /// The generalized velocity at the multibody_joint of the given link. #[inline] - pub(crate) fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView { + pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView { let ndofs = link.joint().ndofs(); DVectorView::from_slice( &self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs], @@ -829,8 +827,10 @@ impl Multibody { } } + // TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians + // (i.e. just something used by the velocity solver’s small steps). /// Apply forward-kinematics to this multibody and its related rigid-bodies. - pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) { + pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) { // Special case for the root, which has no parent. { let link = &mut self.links[0]; @@ -839,7 +839,7 @@ impl Multibody { if let Some(rb) = bodies.get_mut_internal(link.rigid_body) { rb.pos.next_position = link.local_to_world; - if update_mass_props { + if update_rb_mass_props { rb.mprops.update_world_mass_properties(&link.local_to_world); } } @@ -873,7 +873,7 @@ impl Multibody { "A rigid-body that is not at the root of a multibody must be dynamic." ); - if update_mass_props { + if update_rb_mass_props { link_rb .mprops .update_world_mass_properties(&link.local_to_world); @@ -951,40 +951,4 @@ impl Multibody { .sum(); (num_constraints, num_constraints) } - - #[inline] - pub(crate) fn generate_internal_constraints( - &self, - params: &IntegrationParameters, - j_id: &mut usize, - jacobians: &mut DVector, - out: &mut Vec, - mut insert_at: Option, - ) { - if !cfg!(feature = "parallel") { - let num_constraints: usize = self - .links - .iter() - .map(|l| l.joint().num_velocity_constraints()) - .sum(); - - let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2; - if jacobians.nrows() < required_jacobian_len { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); - } - } - - for link in self.links.iter() { - link.joint().velocity_constraints( - params, - self, - link, - 0, - j_id, - jacobians, - out, - &mut insert_at, - ); - } - } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index da650e6..62fc434 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -1,4 +1,4 @@ -use crate::dynamics::solver::AnyJointVelocityConstraint; +use crate::dynamics::solver::JointGenericOneBodyConstraint; use crate::dynamics::{ joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink, RigidBodyVelocity, @@ -254,15 +254,15 @@ impl MultibodyJoint { params: &IntegrationParameters, multibody: &Multibody, link: &MultibodyLink, - dof_id: usize, - j_id: &mut usize, + mut j_id: usize, jacobians: &mut DVector, - constraints: &mut Vec, - insert_at: &mut Option, - ) { + constraints: &mut [JointGenericOneBodyConstraint], + ) -> usize { + let j_id = &mut j_id; let locked_bits = self.data.locked_axes.bits(); let limit_bits = self.data.limit_axes.bits(); let motor_bits = self.data.motor_axes.bits(); + let mut num_constraints = 0; let mut curr_free_dof = 0; for i in 0..DIM { @@ -281,11 +281,11 @@ impl MultibodyJoint { &self.data.motors[i], self.coords[i], limits, - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); } @@ -296,11 +296,11 @@ impl MultibodyJoint { link, [self.data.limits[i].min, self.data.limits[i].max], self.coords[i], - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); } curr_free_dof += 1; @@ -331,11 +331,11 @@ impl MultibodyJoint { link, limits, self.coords[i], - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); Some(limits) } else { @@ -350,15 +350,17 @@ impl MultibodyJoint { &self.data.motors[i], self.coords[i], limits, - dof_id + curr_free_dof, + curr_free_dof, j_id, jacobians, constraints, - insert_at, + &mut num_constraints, ); } curr_free_dof += 1; } } + + num_constraints } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index a4b338a..a428c8b 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -53,13 +53,24 @@ impl IndexedData for MultibodyJointHandle { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq, Eq)] -pub struct MultibodyJointLink { - pub graph_id: RigidBodyGraphIndex, +/// Indexes usable to get a multibody link from a `MultibodyJointSet`. +/// +/// ```rust +/// // With: +/// // multibody_joint_set: MultibodyJointSet +/// // multibody_link_id: MultibodyLinkId +/// let multibody = &multibody_joint_set[multibody_link_id.multibody]; +/// let link = multibody.link(multibody_link_id.id).expect("Link not found."); +pub struct MultibodyLinkId { + pub(crate) graph_id: RigidBodyGraphIndex, + /// The multibody index to be used as `&multibody_joint_set[multibody]` to + /// retrieve the multibody reference. pub multibody: MultibodyIndex, + /// The multibody link index to be given to [`Multibody::link`]. pub id: usize, } -impl Default for MultibodyJointLink { +impl Default for MultibodyLinkId { fn default() -> Self { Self { graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32), @@ -78,7 +89,7 @@ impl Default for MultibodyJointLink { #[derive(Clone)] pub struct MultibodyJointSet { pub(crate) multibodies: Arena, // NOTE: a Slab would be sufficient. - pub(crate) rb2mb: Coarena, + pub(crate) rb2mb: Coarena, // NOTE: this is mostly for the island extraction. So perhaps we won’t need // that any more in the future when we improve our island builder. pub(crate) connectivity_graph: InteractionGraph, @@ -97,13 +108,22 @@ impl MultibodyJointSet { } /// Iterates through all the multibody joints from this set. - pub fn iter(&self) -> impl Iterator { + pub fn iter( + &self, + ) -> impl Iterator< + Item = ( + MultibodyJointHandle, + &MultibodyLinkId, + &Multibody, + &MultibodyLink, + ), + > { self.rb2mb .iter() .filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user. .map(|(h, link)| { let mb = &self.multibodies[link.multibody.0]; - (MultibodyJointHandle(h), mb, mb.link(link.id).unwrap()) + (MultibodyJointHandle(h), link, mb, mb.link(link.id).unwrap()) }) } @@ -118,7 +138,7 @@ impl MultibodyJointSet { let data = data.into(); let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| { let mb_handle = self.multibodies.insert(Multibody::with_root(body1)); - MultibodyJointLink { + MultibodyLinkId { graph_id: self.connectivity_graph.graph.add_node(body1), multibody: MultibodyIndex(mb_handle), id: 0, @@ -127,7 +147,7 @@ impl MultibodyJointSet { let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| { let mb_handle = self.multibodies.insert(Multibody::with_root(body2)); - MultibodyJointLink { + MultibodyLinkId { graph_id: self.connectivity_graph.graph.add_node(body2), multibody: MultibodyIndex(mb_handle), id: 0, @@ -257,7 +277,7 @@ impl MultibodyJointSet { /// Returns the link of this multibody attached to the given rigid-body. /// /// Returns `None` if `rb` isn’t part of any rigid-body. - pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyJointLink> { + pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyLinkId> { self.rb2mb.get(rb.0) } @@ -340,15 +360,15 @@ impl MultibodyJointSet { // NOTE: if there is a joint between these two bodies, then // one of the bodies must be the parent of the other. let link1 = mb.link(id1.id)?; - let parent1 = link1.parent_id()?; + let parent1 = link1.parent_id(); - if parent1 == id2.id { + if parent1 == Some(id2.id) { Some((MultibodyJointHandle(rb1.0), mb, &link1)) } else { let link2 = mb.link(id2.id)?; - let parent2 = link2.parent_id()?; + let parent2 = link2.parent_id(); - if parent2 == id1.id { + if parent2 == Some(id1.id) { Some((MultibodyJointHandle(rb2.0), mb, &link2)) } else { None diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index a1ec483..d6efd12 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -1,9 +1,7 @@ #![allow(missing_docs)] // For downcast. use crate::dynamics::joint::MultibodyLink; -use crate::dynamics::solver::{ - AnyJointVelocityConstraint, JointGenericVelocityGroundConstraint, WritebackId, -}; +use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId}; use crate::dynamics::{IntegrationParameters, JointMotor, Multibody}; use crate::math::Real; use na::DVector; @@ -19,18 +17,16 @@ pub fn unit_joint_limit_constraint( dof_id: usize, j_id: &mut usize, jacobians: &mut DVector, - constraints: &mut Vec, - insert_at: &mut Option, + constraints: &mut [JointGenericOneBodyConstraint], + insert_at: &mut usize, ) { let ndofs = multibody.ndofs(); - let joint_velocity = multibody.joint_velocity(link); - let min_enabled = curr_pos < limits[0]; let max_enabled = limits[1] < curr_pos; let erp_inv_dt = params.joint_erp_inv_dt(); let cfm_coeff = params.joint_cfm_coeff(); let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt; - let rhs_wo_bias = joint_velocity[dof_id]; + let rhs_wo_bias = 0.0; let dof_j_id = *j_id + dof_id + link.assembly_id; jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0); @@ -46,8 +42,8 @@ pub fn unit_joint_limit_constraint( max_enabled as u32 as Real * Real::MAX, ]; - let constraint = JointGenericVelocityGroundConstraint { - mj_lambda2: multibody.solver_id, + let constraint = JointGenericOneBodyConstraint { + solver_vel2: multibody.solver_id, ndofs2: ndofs, j_id2: *j_id, joint_id: usize::MAX, @@ -61,14 +57,9 @@ pub fn unit_joint_limit_constraint( writeback_id: WritebackId::Limit(dof_id), }; - if let Some(at) = insert_at { - constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint); - *at += 1; - } else { - constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint( - constraint, - )); - } + constraints[*insert_at] = constraint; + *insert_at += 1; + *j_id += 2 * ndofs; } @@ -84,13 +75,11 @@ pub