From f58b4f7c195ab7acf0778ea65c46ebf37ac8188c Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 21 Apr 2024 18:55:11 +0200 Subject: feat: add warmstarting to contact constraints resolution --- benchmarks2d/vertical_stacks2.rs | 6 +- examples2d/all_examples2.rs | 27 +++ examples2d/debug_vertical_column2.rs | 47 +++++ examples2d/s2d_arch.rs | 107 +++++++++++ examples2d/s2d_ball_and_chain.rs | 74 ++++++++ examples2d/s2d_bridge.rs | 56 ++++++ examples2d/s2d_card_house.rs | 79 +++++++++ examples2d/s2d_confined.rs | 65 +++++++ examples2d/s2d_far_pyramid.rs | 51 ++++++ examples2d/s2d_high_mass_ratio_1.rs | 66 +++++++ examples2d/s2d_high_mass_ratio_2.rs | 53 ++++++ examples2d/s2d_high_mass_ratio_3.rs | 49 ++++++ examples2d/s2d_joint_grid.rs | 68 +++++++ examples2d/s2d_pyramid.rs | 49 ++++++ src/dynamics/integration_parameters.rs | 130 +++++++------- .../contact_constraint/contact_constraints_set.rs | 11 ++ .../generic_one_body_constraint.rs | 18 ++ .../generic_one_body_constraint_element.rs | 86 +++++++++ .../generic_two_body_constraint.rs | 49 +++++- .../generic_two_body_constraint_element.rs | 196 +++++++++++++++++++++ .../contact_constraint/one_body_constraint.rs | 38 ++-- .../one_body_constraint_element.rs | 100 ++++------- .../contact_constraint/one_body_constraint_simd.rs | 59 +++++-- .../contact_constraint/two_body_constraint.rs | 60 +++++-- .../two_body_constraint_element.rs | 131 +++++++------- .../contact_constraint/two_body_constraint_simd.rs | 69 ++++++-- src/dynamics/solver/velocity_solver.rs | 56 +----- src/geometry/contact_pair.rs | 18 +- src/geometry/narrow_phase.rs | 2 + src/lib.rs | 6 + src_testbed/testbed.rs | 22 +-- src_testbed/ui.rs | 36 +++- 32 files changed, 1541 insertions(+), 343 deletions(-) create mode 100644 examples2d/debug_vertical_column2.rs create mode 100644 examples2d/s2d_arch.rs create mode 100644 examples2d/s2d_ball_and_chain.rs create mode 100644 examples2d/s2d_bridge.rs create mode 100644 examples2d/s2d_card_house.rs create mode 100644 examples2d/s2d_confined.rs create mode 100644 examples2d/s2d_far_pyramid.rs create mode 100644 examples2d/s2d_high_mass_ratio_1.rs create mode 100644 examples2d/s2d_high_mass_ratio_2.rs create mode 100644 examples2d/s2d_high_mass_ratio_3.rs create mode 100644 examples2d/s2d_joint_grid.rs create mode 100644 examples2d/s2d_pyramid.rs diff --git a/benchmarks2d/vertical_stacks2.rs b/benchmarks2d/vertical_stacks2.rs index a415693..aaf7933 100644 --- a/benchmarks2d/vertical_stacks2.rs +++ b/benchmarks2d/vertical_stacks2.rs @@ -11,14 +11,14 @@ pub fn init_world(testbed: &mut Testbed) { let multibody_joints = MultibodyJointSet::new(); - let num = 40; + let rad = 0.5; // 50.0 / 2.0; // 0.5; let rad = 50.0 / 2.0; // 0.5; /* * Ground */ let ground_size = num as f32 * rad * 10.0; - let ground_thickness = 1.0; + let ground_thickness = 25.0; let rigid_body = RigidBodyBuilder::fixed(); let ground_handle = bodies.insert(rigid_body); @@ -30,7 +30,7 @@ pub fn init_world(testbed: &mut Testbed) { */ let shiftx_centerx = [ - (rad * 2.0, -(num as f32) * rad * 2.0 * 1.5), + (rad * 2.0 + 0.0002, -(num as f32) * rad * 2.0 * 1.5), (rad * 2.0 + rad, num as f32 * rad * 2.0 * 1.5), ]; diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 97aa5d0..08fd996 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -17,6 +17,7 @@ mod damping2; mod debug_box_ball2; mod debug_compression2; mod debug_total_overlap2; +mod debug_vertical_column2; mod drum2; mod heightfield2; mod joint_motor_position2; @@ -28,6 +29,17 @@ mod polyline2; mod pyramid2; mod restitution2; mod rope_joints2; +mod s2d_arch; +mod s2d_ball_and_chain; +mod s2d_bridge; +mod s2d_card_house; +mod s2d_confined; +mod s2d_far_pyramid; +mod s2d_high_mass_ratio_1; +mod s2d_high_mass_ratio_2; +mod s2d_high_mass_ratio_3; +mod s2d_joint_grid; +mod s2d_pyramid; mod sensor2; mod trimesh2; @@ -86,6 +98,21 @@ pub fn main() { ("(Debug) box ball", debug_box_ball2::init_world), ("(Debug) compression", debug_compression2::init_world), ("(Debug) total overlap", debug_total_overlap2::init_world), + ( + "(Debug) vertical column", + debug_vertical_column2::init_world, + ), + ("(s2d) high mass ratio 1", s2d_high_mass_ratio_1::init_world), + ("(s2d) high mass ratio 2", s2d_high_mass_ratio_2::init_world), + ("(s2d) high mass ratio 3", s2d_high_mass_ratio_3::init_world), + ("(s2d) confined", s2d_confined::init_world), + ("(s2d) pyramid", s2d_pyramid::init_world), + ("(s2d) card house", s2d_card_house::init_world), + ("(s2d) arch", s2d_arch::init_world), + ("(s2d) bridge", s2d_bridge::init_world), + ("(s2d) ball and chain", s2d_ball_and_chain::init_world), + ("(s2d) joint grid", s2d_joint_grid::init_world), + ("(s2d) far pyramid", s2d_far_pyramid::init_world), ]; // Lexicographic sort, with stress tests moved at the end of the list. diff --git a/examples2d/debug_vertical_column2.rs b/examples2d/debug_vertical_column2.rs new file mode 100644 index 0000000..4ca50db --- /dev/null +++ b/examples2d/debug_vertical_column2.rs @@ -0,0 +1,47 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let num = 80; + let rad = 0.5; + + /* + * Ground + */ + let ground_size = 1.0; + let ground_thickness = 1.0; + + let rigid_body = RigidBodyBuilder::fixed(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).friction(0.3); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + /* + * Create the cubes + */ + + for i in 0..num { + let y = i as f32 * rad * 2.0 + ground_thickness + rad; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, y]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad).friction(0.3); + colliders.insert_with_parent(collider, handle, &mut bodies); + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + // testbed.harness_mut().physics.gravity.y = -981.0; + testbed.look_at(point![0.0, 2.5], 5.0); +} diff --git a/examples2d/s2d_arch.rs b/examples2d/s2d_arch.rs new file mode 100644 index 0000000..ea92e66 --- /dev/null +++ b/examples2d/s2d_arch.rs @@ -0,0 +1,107 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let mut ps1 = [ + Point::new(16.0, 0.0), + Point::new(14.93803712795643, 5.133601056842984), + Point::new(13.79871746027416, 10.24928069555078), + Point::new(12.56252963284711, 15.34107019122473), + Point::new(11.20040987372525, 20.39856541571217), + Point::new(9.66521217819836, 25.40369899225096), + Point::new(7.87179930638133, 30.3179337000085), + Point::new(5.635199558196225, 35.03820717801641), + Point::new(2.405937953536585, 39.09554102558315), + ]; + + let mut ps2 = [ + Point::new(24.0, 0.0), + Point::new(22.33619528222415, 6.02299846205841), + Point::new(20.54936888969905, 12.00964361211476), + Point::new(18.60854610798073, 17.9470321677465), + Point::new(16.46769273811807, 23.81367936585418), + Point::new(14.05325025774858, 29.57079353071012), + Point::new(11.23551045834022, 35.13775818285372), + Point::new(7.752568160730571, 40.30450679009583), + Point::new(3.016931552701656, 44.28891593799322), + ]; + + let scale = 0.25; + let friction = 0.6; + + for i in 0..9 { + ps1[i] *= scale; + ps2[i] *= scale; + } + + /* + * Ground + */ + let collider = ColliderBuilder::segment(point![-100.0, 0.0], point![100.0, 0.0]).friction(0.6); + colliders.insert(collider); + + /* + * Create the arch + */ + for i in 0..8 { + let ps = [ps1[i], ps2[i], ps2[i + 1], ps1[i + 1]]; + let rigid_body = RigidBodyBuilder::dynamic(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::convex_hull(&ps) + .unwrap() + .friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + } + + for i in 0..8 { + let ps = [ + point![-ps2[i].x, ps2[i].y], + point![-ps1[i].x, ps1[i].y], + point![-ps1[i + 1].x, ps1[i + 1].y], + point![-ps2[i + 1].x, ps2[i + 1].y], + ]; + let rigid_body = RigidBodyBuilder::dynamic(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::convex_hull(&ps) + .unwrap() + .friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + } + + { + let ps = [ + ps1[8], + ps2[8], + point![-ps1[8].x, ps1[8].y], + point![-ps2[8].x, ps2[8].y], + ]; + let rigid_body = RigidBodyBuilder::dynamic(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::convex_hull(&ps) + .unwrap() + .friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + } + + for i in 0..4 { + let rigid_body = + RigidBodyBuilder::dynamic().translation(vector![0.0, 0.5 + ps2[8].y + 1.0 * i as f32]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(2.0, 0.5).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5], 20.0); +} diff --git a/examples2d/s2d_ball_and_chain.rs b/examples2d/s2d_ball_and_chain.rs new file mode 100644 index 0000000..cbc1a1f --- /dev/null +++ b/examples2d/s2d_ball_and_chain.rs @@ -0,0 +1,74 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground = bodies.insert(RigidBodyBuilder::fixed()); + + /* + * Create the bridge. + */ + let count = 40; + let hx = 0.5; + let density = 20.0; + let friction = 0.6; + let capsule = ColliderBuilder::capsule_x(hx, 0.125) + .friction(friction) + .density(20.0); + + let mut prev = ground; + for i in 0..count { + let rigid_body = RigidBodyBuilder::dynamic() + .linear_damping(0.1) + .angular_damping(0.1) + .translation(vector![(1.0 + 2.0 * i as f32) * hx, count as f32 * hx]); + let handle = bodies.insert(rigid_body); + colliders.insert_with_parent(capsule.clone(), handle, &mut bodies); + + let pivot = point![(2.0 * i as f32) * hx, count as f32 * hx]; + let joint = RevoluteJointBuilder::new() + .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot)) + .local_anchor2(bodies[handle].position().inverse_transform_point(&pivot)) + .contacts_enabled(false); + impulse_joints.insert(prev, handle, joint, true); + prev = handle; + } + + let radius = 8.0; + let rigid_body = RigidBodyBuilder::dynamic() + .linear_damping(0.1) + .angular_damping(0.1) + .translation(vector![ + (1.0 + 2.0 * count as f32) * hx + radius - hx, + count as f32 * hx + ]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(radius) + .friction(friction) + .density(20.0); + colliders.insert_with_parent(collider, handle, &mut bodies); + + let pivot = point![(2.0 * count as f32) * hx, count as f32 * hx]; + let joint = RevoluteJointBuilder::new() + .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot)) + .local_anchor2(bodies[handle].position().inverse_transform_point(&pivot)) + .contacts_enabled(false); + impulse_joints.insert(prev, handle, joint, true); + prev = handle; + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5], 20.0); +} diff --git a/examples2d/s2d_bridge.rs b/examples2d/s2d_bridge.rs new file mode 100644 index 0000000..712997e --- /dev/null +++ b/examples2d/s2d_bridge.rs @@ -0,0 +1,56 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground = bodies.insert(RigidBodyBuilder::fixed()); + + /* + * Create the bridge. + */ + let density = 20.0; + let x_base = -80.0; + let count = 160; + let mut prev = ground; + + for i in 0..count { + let rigid_body = RigidBodyBuilder::dynamic() + .linear_damping(0.1) + .angular_damping(0.1) + .translation(vector![x_base + 0.5 + 1.0 * i as f32, 20.0]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(0.5, 0.125).density(20.0); + colliders.insert_with_parent(collider, handle, &mut bodies); + + let pivot = point![x_base + 1.0 * i as f32, 20.0]; + let joint = RevoluteJointBuilder::new() + .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot)) + .local_anchor2(bodies[handle].position().inverse_transform_point(&pivot)) + .contacts_enabled(false); + impulse_joints.insert(prev, handle, joint, true); + prev = handle; + } + + let pivot = point![x_base + 1.0 * count as f32, 20.0]; + let joint = RevoluteJointBuilder::new() + .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot)) + .local_anchor2(bodies[ground].position().inverse_transform_point(&pivot)) + .contacts_enabled(false); + impulse_joints.insert(prev, ground, joint, true); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5], 20.0); +} diff --git a/examples2d/s2d_card_house.rs b/examples2d/s2d_card_house.rs new file mode 100644 index 0000000..0862062 --- /dev/null +++ b/examples2d/s2d_card_house.rs @@ -0,0 +1,79 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let extent = 1.0; + let friction = 0.7; + + /* + * Ground + */ + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -2.0]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(40.0, 2.0).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + /* + * Create the cubes + */ + let scale = 10.0; + let card_height = 0.2 * scale; + let card_thickness = 0.001 * scale; + let angle0 = 25.0 * std::f32::consts::PI / 180.0; + let angle1 = -25.0 * std::f32::consts::PI / 180.0; + let angle2 = 0.5 * std::f32::consts::PI; + + let card_box = ColliderBuilder::cuboid(card_thickness, card_height).friction(friction); + + let mut nb = 5; + let mut z0 = 0.0; + let mut y = card_height - 0.02 * scale; + + while nb != 0 { + let mut z = z0; + + for i in 0..nb { + if i != nb - 1 { + let rigid_body = RigidBodyBuilder::dynamic() + .translation(vector![z + 0.25 * scale, y + card_height - 0.015 * scale]) + .rotation(angle2); + let ground_handle = bodies.insert(rigid_body); + colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies); + } + + let rigid_body = RigidBodyBuilder::dynamic() + .translation(vector![z, y]) + .rotation(angle1); + let ground_handle = bodies.insert(rigid_body); + colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies); + + z += 0.175 * scale; + + let rigid_body = RigidBodyBuilder::dynamic() + .translation(vector![z, y]) + .rotation(angle0); + let ground_handle = bodies.insert(rigid_body); + colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies); + + z += 0.175 * scale; + } + + y += card_height * 2.0 - 0.03 * scale; + z0 += 0.175 * scale; + nb -= 1; + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5], 20.0); +} diff --git a/examples2d/s2d_confined.rs b/examples2d/s2d_confined.rs new file mode 100644 index 0000000..46cae86 --- /dev/null +++ b/examples2d/s2d_confined.rs @@ -0,0 +1,65 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let radius = 0.5; + let grid_count = 25; + let friction = 0.6; + let max_count = grid_count * grid_count; + + /* + * Ground + */ + let collider = + ColliderBuilder::capsule(point![-10.5, 0.0], point![10.5, 0.0], radius).friction(friction); + colliders.insert(collider); + let collider = ColliderBuilder::capsule(point![-10.5, 0.0], point![-10.5, 20.5], radius) + .friction(friction); + colliders.insert(collider); + let collider = + ColliderBuilder::capsule(point![10.5, 0.0], point![10.5, 20.5], radius).friction(friction); + colliders.insert(collider); + let collider = ColliderBuilder::capsule(point![-10.5, 20.5], point![10.5, 20.5], radius) + .friction(friction); + colliders.insert(collider); + + /* + * Create the spheres + */ + let mut row = 0; + let mut count = 0; + let mut column = 0; + + while count < max_count { + row = 0; + for i in 0..grid_count { + let x = -8.75 + column as f32 * 18.0 / (grid_count as f32); + let y = 1.5 + row as f32 * 18.0 / (grid_count as f32); + let body = RigidBodyBuilder::dynamic() + .translation(vector![x, y]) + .gravity_scale(0.0); + let body_handle = bodies.insert(body); + let ball = ColliderBuilder::ball(radius).friction(friction); + colliders.insert_with_parent(ball, body_handle, &mut bodies); + + count += 1; + row += 1; + } + + column += 1; + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5], 20.0); +} diff --git a/examples2d/s2d_far_pyramid.rs b/examples2d/s2d_far_pyramid.rs new file mode 100644 index 0000000..f5e34c0 --- /dev/null +++ b/examples2d/s2d_far_pyramid.rs @@ -0,0 +1,51 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let origin = vector![100_000.0, -80_000.0]; + let extent = 1.0; + let friction = 0.6; + + /* + * Ground + */ + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -1.0] + origin); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(100.0, 1.0).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + /* + * Create the cubes + */ + let base_count = 10; + + let h = 0.5; + let shift = 1.25 * h; + + for i in 0..base_count { + let y = (2.0 * i as f32 + 1.0) * shift + 0.5; + + for j in i..base_count { + let x = (i as f32 + 1.0) * shift + 2.0 * (j as f32 - i as f32) * shift + - h * base_count as f32; + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y] + origin); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(h, h).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5] + origin, 20.0); +} diff --git a/examples2d/s2d_high_mass_ratio_1.rs b/examples2d/s2d_high_mass_ratio_1.rs new file mode 100644 index 0000000..b211a5e --- /dev/null +++ b/examples2d/s2d_high_mass_ratio_1.rs @@ -0,0 +1,66 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let extent = 1.0; + let friction = 0.5; + + /* + * Ground + */ + let ground_width = 66.0 * extent; + + let rigid_body = RigidBodyBuilder::fixed(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::segment( + point![-0.5 * 2.0 * ground_width, 0.0], + point![0.5 * 2.0 * ground_width, 0.0], + ) + .friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + /* + * Create the cubes + */ + + for j in 0..3 { + let mut count = 10; + let offset = -20.0 * extent + 2.0 * (count as f32 + 1.0) * extent * j as f32; + let mut y = extent; + + while count > 0 { + for i in 0..count { + let coeff = i as f32 - 0.5 * count as f32; + let yy = if count == 1 { y + 2.0 } else { y }; + let position = vector![2.0 * coeff * extent + offset, yy]; + let rigid_body = RigidBodyBuilder::dynamic().translation(position); + let parent = bodies.insert(rigid_body); + + let collider = ColliderBuilder::cuboid(extent, extent) + .density(if count == 1 { + (j as f32 + 1.0) * 100.0 + } else { + 1.0 + }) + .friction(friction); + colliders.insert_with_parent(collider, parent, &mut bodies); + } + + count -= 1; + y += 2.0 * extent; + } + } + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5], 20.0); +} diff --git a/examples2d/s2d_high_mass_ratio_2.rs b/examples2d/s2d_high_mass_ratio_2.rs new file mode 100644 index 0000000..518df95 --- /dev/null +++ b/examples2d/s2d_high_mass_ratio_2.rs @@ -0,0 +1,53 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let extent = 1.0; + let friction = 0.6; + + /* + * Ground + */ + let ground_width = 66.0 * extent; + + let rigid_body = RigidBodyBuilder::fixed(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::segment( + point![-0.5 * 2.0 * ground_width, 0.0], + point![0.5 * 2.0 * ground_width, 0.0], + ) + .friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + /* + * Create the cubes + */ + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![-9.0 * extent, 0.5 * extent]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![9.0 * extent, 0.5 * extent]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, (10.0 + 16.0) * extent]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(10.0 * extent, 10.0 * extent).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5], 20.0); +} diff --git a/examples2d/s2d_high_mass_ratio_3.rs b/examples2d/s2d_high_mass_ratio_3.rs new file mode 100644 index 0000000..f8dd3ee --- /dev/null +++ b/examples2d/s2d_high_mass_ratio_3.rs @@ -0,0 +1,49 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let extent = 1.0; + let friction = 0.6; + + /* + * Ground + */ + let ground_width = 66.0 * extent; + + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -2.0]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(40.0, 2.0).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + /* + * Create the cubes + */ + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![-9.0 * extent, 0.5 * extent]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![9.0 * extent, 0.5 * extent]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, (10.0 + 16.0) * extent]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(10.0 * extent, 10.0 * extent).friction(friction); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5], 20.0); +} diff --git a/examples2d/s2d_joint_grid.rs b/examples2d/s2d_joint_grid.rs new file mode 100644 index 0000000..97cea07 --- /dev/null +++ b/examples2d/s2d_joint_grid.rs @@ -0,0 +1,68 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground = bodies.insert(RigidBodyBuilder::fixed()); + + /* + * Create the bridge. + */ + let rad = 0.4; + let numi = 100; + let numk = 100; + let shift = 1.0; + let mut index = 0; + let mut handles = vec![RigidBodyHandle::invalid(); numi * numk]; + + for k in 0..numk { + for i in 0..numi { + let body_type = if k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0 { + RigidBodyType::Fixed + } else { + RigidBodyType::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(body_type) + .translation(vector![k as f32 * shift, -(i as f32) * shift]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad); + colliders.insert_with_parent(collider, handle, &mut bodies); + + if i > 0 { + let joint = RevoluteJointBuilder::new() + .local_anchor1(point![0.0, -0.5 * shift]) + .local_anchor2(point![0.0, 0.5 * shift]) + .contacts_enabled(false); + impulse_joints.insert(handles[index - 1], handle, joint, true); + } + + if k > 0 { + let joint = RevoluteJointBuilder::new() + .local_anchor1(point![0.5 * shift, 0.0]) + .local_anchor2(point![-0.5 * shift, 0.0]) + .contacts_enabled(false); + impulse_joints.insert(handles[index - numi], handle, joint, true); + } + + handles[index] = handle; + index += 1; + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5], 20.0); +} diff --git a/examples2d/s2d_pyramid.rs b/examples2d/s2d_pyramid.rs new file mode 100644 index 0000000..45e6310 --- /dev/null +++ b/examples2d/s2d_pyramid.rs @@ -0,0 +1,49 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let extent = 1.0; + + /* + * Ground + */ + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -1.0]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(100.0, 1.0).friction(0.6); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + + /* + * Create the cubes + */ + let base_count = 100; + + let h = 0.5; + let shift = 1.0 * h; + + for i in 0..base_count { + let y = (2.0 * i as f32 + 1.0) * shift; + + for j in i..base_count { + let x = (i as f32 + 1.0) * shift + 2.0 * (j as f32 - i as f32) * shift + - h * base_count as f32; + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(h, h).friction(0.6); + colliders.insert_with_parent(collider, ground_handle, &mut bodies); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![0.0, 2.5], 20.0); +} diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 093ebe9..80ca92c 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -4,7 +4,6 @@ use std::num::NonZeroUsize; // TODO: enabling the block solver in 3d introduces a lot of jitters in // the 3D domino demo. So for now we dont enable it in 3D. pub(crate) static BLOCK_SOLVER_ENABLED: bool = cfg!(feature = "dim2"); -pub(crate) static DISABLE_FRICTION_LIMIT_REAPPLY: bool = false; /// Parameters for a time-step of the physics engine. #[derive(Copy, Clone, Debug)] @@ -26,12 +25,12 @@ pub struct IntegrationParameters { /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration) /// will be compensated for during the velocity solve. - /// (default `0.8`). + /// (default `0.1`). pub erp: Real, /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization. /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations /// before stabilization). - /// (default `0.25`). + /// (default `20.0`). pub damping_ratio: Real, /// 0-1: multiplier for how much of the joint violation @@ -40,14 +39,21 @@ pub struct IntegrationParameters { pub joint_erp: Real, /// The fraction of critical damping applied to the joint for constraints regularization. - /// (default `0.25`). + /// (default `1.0`). pub joint_damping_ratio: Real, - /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). + /// The coefficient in `[0, 1]` applied to warmstart impulses, i.e., impulses that are used as the + /// initial solution (instead of 0) at the next simulation step. + /// + /// This should generally be set to 1. Can be set to 0 if using a large [`Self::erp`] value. + /// (default `1.0`). + pub warmstart_coefficient: Real, + + /// Amount of penetration the engine won’t attempt to correct (default: `0.001m`). pub allowed_linear_error: Real, /// Maximum amount of penetration the solver will attempt to resolve in one timestep. pub max_penetration_correction: Real, - /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). + /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002m`). pub prediction_distance: Real, /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`). pub num_solver_iterations: NonZeroUsize, @@ -55,8 +61,8 @@ pub struct IntegrationParameters { pub num_additional_friction_iterations: usize, /// Number of internal Project Gauss Seidel (PGS) iterations run at each solver iteration (default: `1`). pub num_internal_pgs_iterations: usize, - /// The maximum number of stabilization iterations run at each solver iterations (default: `10`). - pub max_internal_stabilization_iterations: usize, + /// The number of stabilization iterations run at each solver iterations (default: `2`). + pub num_internal_stabilization_iterations: usize, /// 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`). @@ -64,51 +70,6 @@ pub struct IntegrationParameters { } impl IntegrationParameters { - /// Configures the integration parameters to match the old PGS solver - /// from Rapier version <= 0.17. - /// - /// This solver was slightly faster than the new one but resulted - /// in less stable joints and worse convergence rates. - /// - /// This should only be used for comparison purpose or if you are - /// experiencing problems with the new solver. - /// - /// NOTE: this does not affect any [`RigidBody::additional_solver_iterations`] that will - /// still create solver iterations based on the new "small-steps" PGS solver. - /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`], - /// [`Self::joint_damping_ratio`] to their former default values. - pub fn switch_to_standard_pgs_solver(&mut self) { - self.num_internal_pgs_iterations *= self.num_solver_iterations.get(); - self.num_solver_iterations = NonZeroUsize::new(1).unwrap(); - self.erp = 0.8; - self.damping_ratio = 0.25; - self.joint_erp = 1.0; - self.joint_damping_ratio = 1.0; - } - - /// Configures the integration parameters to match the new "small-steps" PGS solver - /// from Rapier version >= 0.18. - /// - /// The "small-steps" PGS solver is the default one given by [`Self::default()`] so - /// calling this function is generally not needed unless - /// [`Self::switch_to_standard_pgs_solver()`] was called. - /// - /// This solver results in more stable joints and significantly better convergence - /// rates but is slightly slower in its default settings. - /// - /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`], - /// [`Self::joint_damping_ratio`] to their default values. - pub fn switch_to_small_steps_pgs_solver(&mut self) { - self.num_solver_iterations = NonZeroUsize::new(self.num_internal_pgs_iterations).unwrap(); - self.num_internal_pgs_iterations = 1; - - let default = Self::default(); - self.erp = default.erp; - self.damping_ratio = default.damping_ratio; - self.joint_erp = default.joint_erp; - self.joint_damping_ratio = default.joint_damping_ratio; - } - /// The inverse of the time-stepping length, i.e. the steps per seconds (Hz). /// /// This is zero if `self.dt` is zero. @@ -161,7 +122,7 @@ impl IntegrationParameters { // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass // / (dt * inv_erp_minus_one); // let cfm = 1.0 / (dt * dt * stiffness + dt * damping); - // NOTE: This simplies to cfm = cfm_coefff / projected_mass: + // NOTE: This simplifies to cfm = cfm_coeff / projected_mass: let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one / ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.damping_ratio); @@ -180,13 +141,14 @@ impl IntegrationParameters { // new_impulse = cfm_factor * (old_impulse - m * delta_vel) // // The value returned by this function is this cfm_factor that can be used directly - // in the constraints solver. + // in the constraint solver. 1.0 / (1.0 + cfm_coeff) } /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization pub fn joint_cfm_coeff(&self) -> Real { // Compute CFM assuming a critically damped spring multiplied by the damping ratio. + // The logic is similar to `Self::cfm_factor`. let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0; inv_erp_minus_one * inv_erp_minus_one / ((1.0 + inv_erp_minus_one) @@ -194,23 +156,23 @@ impl IntegrationParameters { * self.joint_damping_ratio * self.joint_damping_ratio) } -} -impl Default for IntegrationParameters { - fn default() -> Self { + /// Initialize the simulation paramaters with settings matching the TGS-soft solver + /// with warmstarting. + /// + /// This is the default configuration, equivalent to [`IntegrationParameters::default()`]. + pub fn tgs_soft() -> Self { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, - erp: 0.8, - damping_ratio: 1.0, + erp: 0.1, + damping_ratio: 20.0, joint_erp: 1.0, joint_damping_ratio: 1.0, - allowed_linear_error: 0.001, - max_penetration_correction: Real::MAX, - prediction_distance: 0.002, + warmstart_coefficient: 1.0, num_internal_pgs_iterations: 1, - max_internal_stabilization_iterations: 10, - num_additional_friction_iterations: 4, + num_internal_stabilization_iterations: 2, + num_additional_friction_iterations: 0, 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 @@ -218,7 +180,45 @@ impl Default for IntegrationParameters { // However we don't want it to be too small and end up with // tons of islands, reducing SIMD parallelism opportunities. min_island_size: 128, + allowed_linear_error: 0.001, + max_penetration_correction: Real::MAX, + prediction_distance: 0.002, max_ccd_substeps: 1, } } + + /// Initialize the simulation paramaters with settings matching the TGS-soft solver + /// **without** warmstarting. + /// + /// The [`IntegrationParameters::tgs_soft()`] configuration should be preferred unless + /// warmstarting proves to be undesirable for your use-case. + pub fn tgs_soft_without_warmstart() -> Self { + Self { + erp: 0.8, + damping_ratio: 1.0, + warmstart_coefficient: 0.0, + num_additional_friction_iterations: 4, + ..Self::tgs_soft() + } + } + + /// Initializes the integration parameters to match the legacy PGS solver from Rapier version <= 0.17. + /// + /// This exists mainly for testing and comparison purpose. + pub fn pgs_legacy() -> Self { + Self { + erp: 0.8, + damping_ratio: 0.25, + warmstart_coefficient: 0.0, + num_additional_friction_iterations: 4, + num_solver_iterations: NonZeroUsize::new(1).unwrap(), + ..Self::tgs_soft() + } + } +} + +impl Default for IntegrationParameters { + fn default() -> Self { + Self::tgs_soft() + } } diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs index 4d88949..6809c37 100644 --- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -442,6 +442,17 @@ impl ContactConstraintsSet { assert_eq!(curr_start, total_num_constraints); } + pub fn warmstart( + &mut self, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + let (jac, constraints) = self.iter_constraints_mut(); + for mut c in constraints { + c.warmstart(jac, solver_vels, generic_solver_vels); + } + } + pub fn solve_restitution( &mut self, solver_vels: &mut [SolverVel], diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs index d99ad60..301ce71 100644 --- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs @@ -259,6 +259,24 @@ impl GenericOneBodyConstraint { } } + pub fn warmstart( + &mut self, + jacobians: &DVector, + generic_solver_vels: &mut DVector, + ) { + let solver_vel2 = self.inner.solver_vel2; + + let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; + OneBodyConstraintElement::generic_warmstart_group( + elements, + jacobians, + self.ndofs2, + self.j_id, + solver_vel2, + generic_solver_vels, + ); + } + pub fn solve( &mut self, jacobians: &DVector, diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs index 1003a9a..4928f36 100644 --- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs @@ -7,6 +7,40 @@ use na::DVector; use na::SimdPartialOrd; impl OneBodyConstraintTangentPart { + #[inline] + pub fn generic_warmstart( + &mut self, + j_id2: usize, + jacobians: &DVector, + ndofs2: usize, + solver_vel2: usize, + solver_vels: &mut DVector, + ) { + #[cfg(feature = "dim2")] + { + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + self.impulse[0], + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + } + + #[cfg(feature = "dim3")] + { + let j_step = ndofs2 * 2; + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + self.impulse[0], + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + self.impulse[1], + &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2), + 1.0, + ); + } + } + #[inline] pub fn generic_solve( &mut self, @@ -71,6 +105,22 @@ impl OneBodyConstraintTangentPart { } impl OneBodyConstraintNormalPart { + #[inline] + pub fn generic_warmstart( + &mut self, + j_id2: usize, + jacobians: &DVector, + ndofs2: usize, + solver_vel2: usize, + solver_vels: &mut DVector, + ) { + solver_vels.rows_mut(solver_vel2, ndofs2).axpy( + self.impulse, + &jacobians.rows(j_id2 + ndofs2, ndofs2), + 1.0, + ); + } + #[inline] pub fn generic_solve( &mut self, @@ -99,6 +149,42 @@ impl OneBodyConstraintNormalPart { } impl OneBodyConstraintElement { + #[inline] + pub fn generic_warmstart_group( + elements: &mut [Self], + jacobians: &DVector, + ndofs2: usize, + // Jacobian index of the first constraint. + j_id: usize, + solver_vel2: usize, + solver_vels: &mut DVector, + ) { + let j_step = ndofs2 * 2 * DIM; + + // Solve penetration. + let mut nrm_j_id = j_id; + + for element in elements.iter_mut() { + element.normal_part.generic_warmstart( + nrm_j_id, + jacobians, + ndofs2, + solver_vel2, + solver_vels, + ); + nrm_j_id += j_step; + } + + // Solve friction. + let mut tng_j_id = j_id + ndofs2 * 2; + + for element in elements.iter_mut() { + let part = &mut element.tangent_part; + part.generic_warmstart(tng_j_id, jacobians, ndofs2, solver_vel2, solver_vels); + tng_j_id += j_step; + } + } + #[inline] pub fn generic_solve_group( cfm_factor: Real, diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs index 4fa8694..be839f0 100644 --- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs @@ -202,7 +202,7 @@ impl GenericTwoBodyConstraintBuilder { rhs: na::zero(), rhs_wo_bias: na::zero(), impulse_accumulator: na::zero(), - impulse: na::zero(), + impulse: manifold_point.warmstart_impulse, r, r_mat_elts: [0.0; 2], }; @@ -210,7 +210,8 @@ impl GenericTwoBodyConstraintBuilder { // Tangent parts. { - constraint.inner.elements[k].tangent_part.impulse = na::zero(); + constraint.inner.elements[k].tangent_part.impulse = + manifold_point.warmstart_tangent_impulse; for j in 0..DIM - 1 { let torque_dir1 = dp1.gcross(tangents1[j]); @@ -374,6 +375,50 @@ impl GenericTwoBodyConstraint { } } + pub fn warmstart( + &mut self, + jacobians: &DVector, + solver_vels: &mut [SolverVel], + generic_solver_vels: &mut DVector, + ) { + let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 { + GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1]) + } else { + GenericRhs::GenericId(self.inner.solver_vel1) + }; + + let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 { + GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2]) + } else { + GenericRhs::GenericId(self.inner.solver_vel2) + }; + + let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; + TwoBodyConstraintElement::generic_warmstart_group( + elements, + jacobians, + &self.inner.dir1, + #[cfg(feature = "dim3")] + &self.inner.tangent1, + &self.inner.im1, + &self.inner.im2, + self.ndofs1, + self.ndofs2, + self.j_id, + &mut solver_vel1, + &mut solver_vel2, + generic_solver_vels, + ); + + if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 { + solver_vels[self.inner.solver_vel1] = solver_vel1; + } + + if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 { + solver_vels[self.inner.solver_vel2] = solver_vel2; + } + } + pub fn solve( &mut self, jacobians: &DVector, diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs index 40740a0..389b603 100644 --- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs @@ -88,6 +88,95 @@ impl GenericRhs { } impl TwoBodyConstraintTangentPart { + #[inline] + pub fn generic_warmstart( + &mut self, + j_id: usize, + jacobians: &DVector, + tangents1: [&Vector; DIM - 1], + im1: &Vector, + im2: &Vector, + ndofs1: usize, + ndofs2: usize, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector, + ) { + let j_id1 = j_id1(j_id, ndofs1, ndofs2); + let j_id2 = j_id2(j_id, ndofs1, ndofs2); + #[cfg(feature = "dim3")] + let j_step = j_step(ndofs1, ndofs2); + + #[cfg(feature = "dim2")] + { + solver_vel1.apply_impulse( + j_id1, + ndofs1, + self.impulse[0], + jacobians, + tangents1[0], + &self.gcross1[0], + solver_vels, + im1, + ); + solver_vel2.apply_impulse( + j_id2, + ndofs2, + self.impulse[0], + jacobians, + &-tangents1[0], + &self.gcross2[0], + solver_vels, + im2, + ); + } + + #[cfg(feature = "dim3")] + { + solver_vel1.apply_impulse( + j_id1, + ndofs1, + self.impulse[0], + jacobians, + tangents1[0], + &self.gcross1[0], + solver_vels, + im1, + ); + solver_vel1.apply_impulse( + j_id1 + j_step, + ndofs1, + self.impulse[1], + jacobians, + tangents1[1], + &self.gcross1[1], + solver_vels, + im1, + ); + + solver_vel2.apply_impulse( + j_id2, + ndofs2, + self.impulse[0], + jacobians, + &-tangents1[0], + &self.gcross2[0], + solver_vels, + im2, + ); + solver_vel2.apply_impulse( + j_id2 + j_step, + ndofs2, + self.impulse[1], + jacobians, + &-tangents1[1], + &self.gcross2[1], + solver_vels, + im2, + ); + } + } + #[inline] pub fn generic_solve( &mut self, @@ -240,6 +329,45 @@ impl TwoBodyConstraintTangentPart { } impl TwoBodyConstraintNormalPart { + #[inline] + pub fn generic_warmstart( + &mut self, + j_id: usize, + jacobians: &DVector, + dir1: &Vector, + im1: &Vector, + im2: &Vector, + ndofs1: usize, + ndofs2: usize, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector, + ) { + let j_id1 = j_id1(j_id, ndofs1, ndofs2); + let j_id2 = j_id2(j_id, ndofs1, ndofs2); + + solver_vel1.apply_impulse( + j_id1, + ndofs1, + self.impulse, + jacobians, + dir1, + &self.gcross1, + solver_vels, + im1, + ); + solver_vel2.apply_impulse( + j_id2, + ndofs2, + self.impulse, + jacobians, + &-dir1, + &self.gcross2, + solver_vels, + im2, + ); + } + #[inline] pub fn generic_solve( &mut self, @@ -290,6 +418,74 @@ impl TwoBodyConstraintNormalPart { } impl TwoBodyConstraintElement { + #[inline] + pub fn generic_warmstart_group( + elements: &mut [Self], + jacobians: &DVector, + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im1: &Vector, + im2: &Vector, + // ndofs is 0 for a non-multibody body, or a multibody with zero + // degrees of freedom. + ndofs1: usize, + ndofs2: usize, + // Jacobian index of the first constraint. + j_id: usize, + solver_vel1: &mut GenericRhs, + solver_vel2: &mut GenericRhs, + solver_vels: &mut DVector, + ) { + let j_step = j_step(ndofs1, ndofs2) * DIM; + + // Solve penetration. + { + let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2); + + for element in elements.iter_mut() { + element.normal_part.generic_warmstart( + nrm_j_id, + jacobians, + dir1, + im1, + im2, + ndofs1, + ndofs2, + solver_vel1, + solver_vel2, + solver_vels, + ); + nrm_j_id += j_step; + } + } + + // Solve friction. + { + #[cfg(feature = "dim3")] + let tangents1 = [tangent1, &dir1.cross(tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&dir1.orthonormal_vector()]; + let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2); + + for element in elements.iter_mut() { + let part = &mut element.tangent_part; + part.generic_warmstart( + tng_j_id, + jacobians, + tangents1, + im1, + im2, + ndofs1, + ndofs2, + solver_vel1, + solver_vel2, + solver_vels, + ); + tng_j_id += j_step; + } + } + } + #[inline] pub fn generic_solve_group( cfm_factor: Real, diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs index 357f1c4..c31d2aa 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs @@ -151,7 +151,7 @@ impl OneBodyConstraintBuilder { gcross2, rhs: na::zero(), rhs_wo_bias: na::zero(), - impulse: na::zero(), + impulse: manifold_point.warmstart_impulse, impulse_accumulator: na::zero(), r: projected_mass, r_mat_elts: [0.0; 2], @@ -160,7 +160,8 @@ impl OneBodyConstraintBuilder { // Tangent parts. { - constraint.elements[k].tangent_part.impulse = na::zero(); + constraint.elements[k].tangent_part.impulse = + manifold_point.warmstart_tangent_impulse; for j in 0..DIM - 1 { let gcross2 = mprops2 @@ -317,13 +318,13 @@ impl OneBodyConstraintBuilder { element.normal_part.rhs_wo_bias = rhs_wo_bias; element.normal_part.rhs = new_rhs; element.normal_part.impulse_accumulator += element.normal_part.impulse; - element.normal_part.impulse = na::zero(); + element.normal_part.impulse *= params.warmstart_coefficient; } // Tangent part. { element.tangent_part.impulse_accumulator += element.tangent_part.impulse; - element.tangent_part.impulse = na::zero(); + element.tangent_part.impulse *= params.warmstart_coefficient; for j in 0..DIM - 1 { let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; @@ -369,6 +370,21 @@ impl OneBodyConstraint { } } + pub fn warmstart(&mut self, solver_vels: &mut [SolverVel]) { + let mut solver_vel2 = solver_vels[self.solver_vel2]; + + OneBodyConstraintElement::warmstart_group( + &mut self.elements[..self.num_contacts as usize], + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + &self.im2, + &mut solver_vel2, + ); + + solver_vels[self.solver_vel2] = solver_vel2; + } + pub fn solve( &mut self, solver_vels: &mut [SolverVel], @@ -400,17 +416,11 @@ impl OneBodyConstraint { for k in 0..self.num_contacts as usize { let contact_id = self.manifold_contact_id[k]; let active_contact = &mut manifold.points[contact_id as usize]; - active_contact.data.impulse = self.elements[k].normal_part.total_impulse(); - #[cfg(feature = "dim2")] - { - active_contact.data.tangent_impulse = - self.elements[k].tangent_part.total_impulse()[0]; - } - #[cfg(feature = "dim3")] - { - active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse(); - } + active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse; + active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse; + active_contact.data.impulse = self.elements[k].normal_part.total_impulse(); + active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse(); } } diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs index e5ae140..7ec8c5d 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs @@ -1,9 +1,7 @@ -use crate::dynamics::integration_parameters::{ - BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY, -}; +use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart; use crate::dynamics::solver::SolverVel; -use crate::math::{AngVector, Vector, DIM}; +use crate::math::{AngVector, TangentImpulse, Vector, DIM}; use crate::utils::{SimdBasis, SimdDot, SimdRealCopy}; use na::Vector2; @@ -12,14 +10,8 @@ pub(crate) struct OneBodyConstraintTangentPart { pub gcross2: [AngVector; DIM - 1], pub rhs: [N; DIM - 1], pub rhs_wo_bias: [N; DIM - 1], - #[cfg(feature = "dim2")] - pub impulse: na::Vector1, - #[cfg(feature = "dim3")] - pub impulse: na::Vector2, - #[cfg(feature = "dim2")] - pub impulse_accumulator: na::Vector1, - #[cfg(feature = "dim3")] - pub impulse_accumulator: na::Vector2, + pub impulse: TangentImpulse, + pub impulse_accumulator: TangentImpulse, #[cfg(feature = "dim2")] pub r: [N; 1], #[cfg(feature = "dim3")] @@ -43,57 +35,29 @@ impl OneBodyConstraintTangentPart { /// Total impulse applied across all the solver substeps. #[inline] - #[cfg(feature = "dim2")] - pub fn total_impulse(&self) -> na::Vector1 { + pub fn total_impulse(&self) -> TangentImpulse { self.impulse_accumulator + self.impulse } - /// Total impulse applied across all the solver substeps. #[inline] - #[cfg(feature = "dim3")] - pub fn total_impulse(&self) -> na::Vector2 { - self.impulse_accumulator + self.impulse - } - - #[inline] - pub fn apply_limit( + pub fn warmstart( &mut self, tangents1: [&Vector; DIM - 1], im2: &Vector, - limit: N, solver_vel2: &mut SolverVel, - ) where - AngVector: SimdDot, Result = N>, - { - if DISABLE_FRICTION_LIMIT_REAPPLY { - return; - } - + ) { #[cfg(feature = "dim2")] { - let new_impulse = self.impulse[0].simd_clamp(-limit, limit); - let dlambda = new_impulse - self.impulse[0]; - self.impulse[0] = new_impulse; - - solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda; - solver_vel2.angular += self.gcross2[0] * dlambda; + solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]; + solver_vel2.angular += self.gcross2[0] * self.impulse[0]; } #[cfg(feature = "dim3")] { - let new_impulse = self.impulse; - let new_impulse = { - let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags:: - disable_floating_point_exceptions(); - new_impulse.simd_cap_magnitude(limit) - }; - let dlambda = new_impulse - self.impulse; - self.impuls