feat: add warmstarting to contact constraints resolution

2 files changed, 34 insertions, 24 deletions

diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 7be4594..a575f98 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -102,10 +102,12 @@ bitflags! {
     }
 }
 
-#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)]
 pub enum RapierSolverType {
-    SmallStepsPgs,
-    StandardPgs,
+    #[default]
+    TgsSoft,
+    TgsSoftNoWarmstart,
+    PgsLegacy,
 }
 
 pub type SimulationBuilders = Vec<(&'static str, fn(&mut Testbed))>;
@@ -214,7 +216,7 @@ impl TestbedApp {
             example_names: Vec::new(),
             selected_example: 0,
             selected_backend: RAPIER_BACKEND,
-            solver_type: RapierSolverType::SmallStepsPgs,
+            solver_type: RapierSolverType::default(),
             physx_use_two_friction_directions: true,
             nsteps: 1,
             camera_locked: false,
@@ -1202,18 +1204,6 @@ fn update_testbed(
             }
             plugins.0.clear();
 
-            if state.selected_example != prev_example {
-                harness.physics.integration_parameters = IntegrationParameters::default();
-
-                match state.solver_type {
-                    RapierSolverType::SmallStepsPgs => {} // It’s already the default.
-                    RapierSolverType::StandardPgs => harness
-                        .physics
-                        .integration_parameters
-                        .switch_to_standard_pgs_solver(),
-                }
-            }
-
             let selected_example = state.selected_example;
             let graphics = &mut *graphics;
             let meshes = &mut *meshes;
diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs
index e8b82bd..4df9d87 100644
--- a/src_testbed/ui.rs
+++ b/src_testbed/ui.rs
@@ -12,6 +12,7 @@ use crate::testbed::{
 use crate::PhysicsState;
 use bevy_egui::egui::Slider;
 use bevy_egui::{egui, EguiContexts};
+use rapier::dynamics::IntegrationParameters;
 
 pub fn update_ui(
     ui_context: &mut EguiContexts,
@@ -109,8 +110,9 @@ pub fn update_ui(
                 .selected_text(format!("{:?}", state.solver_type))
                 .show_ui(ui, |ui| {
                     let solver_types = [
-                        RapierSolverType::SmallStepsPgs,
-                        RapierSolverType::StandardPgs,
+                        RapierSolverType::TgsSoft,
+                        RapierSolverType::TgsSoftNoWarmstart,
+                        RapierSolverType::PgsLegacy,
                     ];
                     for sty in solver_types {
                         changed = ui
@@ -122,11 +124,15 @@ pub fn update_ui(
 
             if changed {
                 match state.solver_type {
-                    RapierSolverType::SmallStepsPgs => {
-                        integration_parameters.switch_to_small_steps_pgs_solver()
+                    RapierSolverType::TgsSoft => {
+                        *integration_parameters = IntegrationParameters::tgs_soft();
                     }
-                    RapierSolverType::StandardPgs => {
-                        integration_parameters.switch_to_standard_pgs_solver()
+                    RapierSolverType::TgsSoftNoWarmstart => {
+                        *integration_parameters =
+                            IntegrationParameters::tgs_soft_without_warmstart();
+                    }
+                    RapierSolverType::PgsLegacy => {
+                        *integration_parameters = IntegrationParameters::pgs_legacy();
                     }
                 }
             }
@@ -146,17 +152,31 @@ pub fn update_ui(
             ui.add(
                 Slider::new(
                     &mut integration_parameters.num_additional_friction_iterations,
-                    1..=40,
+                    0..=40,
                 )
                 .text("num additional frict. iters."),
             );
             ui.add(
                 Slider::new(
-                    &mut integration_parameters.max_internal_stabilization_iterations,
+                    &mut integration_parameters.num_internal_stabilization_iterations,
                     1..=100,
                 )
                 .text("max internal stabilization iters."),
             );
+            ui.add(
+                Slider::new(&mut integration_parameters.warmstart_coefficient, 0.0..=1.0)
+                    .text("warmstart coefficient"),
+            );
+            ui.add(Slider::new(&mut integration_parameters.erp, 0.0..=1.0).text("erp"));
+            ui.add(
+                Slider::new(&mut integration_parameters.damping_ratio, 0.0..=20.0)
+                    .text("damping ratio"),
+            );
+            ui.add(Slider::new(&mut integration_parameters.joint_erp, 0.0..=1.0).text("joint erp"));
+            ui.add(
+                Slider::new(&mut integration_parameters.joint_damping_ratio, 0.0..=20.0)
+                    .text("joint damping ratio"),
+            );
         }
 
         #[cfg(feature = "parallel")]