feat: add IntegrationParameters::length_unit to adjust internal threshold based on user-defined length units

9 files changed, 77 insertions, 27 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 80ca92c..b883b0e 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -49,12 +49,33 @@ pub struct IntegrationParameters {
     /// (default `1.0`).
     pub warmstart_coefficient: Real,
 
+    /// The approximate size of most dynamic objects in the scale.
+    ///
+    /// This value is used internally to estimate some length-based tolerance. In particular, the
+    /// values [`IntegrationParametres::allowed_linear_error`],
+    /// [`IntegrationParametres::max_penetration_correction`],
+    /// [`IntegrationParametres::prediction_distance`], [`RigidBodyActivation::linear_threshold`]
+    /// are scaled by this value implicitly.
+    ///
+    /// This value can be understood as the number of units-per-meter in your physical world compared
+    /// to a human-sized world in meter. For example, in a 2d game, if your typical object size is 100
+    /// pixels, set the `[`Self::length_unit`]` parameter to 100.0. The physics engine will interpret
+    /// it as if 100 pixels is equivalent to 1 meter in its various internal threshold.
+    /// (default `1.0`).
+    pub length_unit: Real,
+
     /// Amount of penetration the engine won’t attempt to correct (default: `0.001m`).
-    pub allowed_linear_error: Real,
+    ///
+    /// This value is implicitly scaled by [`IntegrationParameters::length_unit`].
+    pub normalized_allowed_linear_error: Real,
     /// Maximum amount of penetration the solver will attempt to resolve in one timestep.
-    pub max_penetration_correction: Real,
+    ///
+    /// This value is implicitly scaled by [`IntegrationParameters::length_unit`].
+    pub normalized_max_penetration_correction: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002m`).
-    pub prediction_distance: Real,
+    ///
+    /// This value is implicitly scaled by [`IntegrationParameters::length_unit`].
+    pub normalized_prediction_distance: Real,
     /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
     pub num_solver_iterations: NonZeroUsize,
     /// Number of addition friction resolution iteration run during the last solver sub-step (default: `4`).
@@ -157,6 +178,22 @@ impl IntegrationParameters {
                 * self.joint_damping_ratio)
     }
 
+    pub fn allowed_linear_error(&self) -> Real {
+        self.normalized_allowed_linear_error * self.length_unit
+    }
+
+    pub fn max_penetration_correction(&self) -> Real {
+        if self.normalized_max_penetration_correction != Real::MAX {
+            self.normalized_max_penetration_correction * self.length_unit
+        } else {
+            Real::MAX
+        }
+    }
+
+    pub fn prediction_distance(&self) -> Real {
+        self.normalized_prediction_distance * self.length_unit
+    }
+
     /// Initialize the simulation paramaters with settings matching the TGS-soft solver
     /// with warmstarting.
     ///
@@ -180,21 +217,22 @@ impl 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,
+            normalized_allowed_linear_error: 0.001,
+            normalized_max_penetration_correction: Real::MAX,
+            normalized_prediction_distance: 0.002,
             max_ccd_substeps: 1,
+            length_unit: 1.0,
         }
     }
 
-    /// Initialize the simulation paramaters with settings matching the TGS-soft solver
+    /// Initialize the simulation parameters 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,
+            erp: 0.6,
             damping_ratio: 1.0,
             warmstart_coefficient: 0.0,
             num_additional_friction_iterations: 4,
diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs
index 8f18941..a576946 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -150,6 +150,7 @@ impl IslandManager {
     pub(crate) fn update_active_set_with_contacts(
         &mut self,
         dt: Real,
+        length_unit: Real,
         bodies: &mut RigidBodySet,
         colliders: &ColliderSet,
         narrow_phase: &NarrowPhase,
@@ -181,7 +182,7 @@ impl IslandManager {
             let sq_linvel = rb.vels.linvel.norm_squared();
             let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel);
 
-            update_energy(&mut rb.activation, sq_linvel, sq_angvel, dt);
+            update_energy(length_unit, &mut rb.activation, sq_linvel, sq_angvel, dt);
 
             if rb.activation.time_since_can_sleep >= rb.activation.time_until_sleep {
                 // Mark them as sleeping for now. This will
@@ -324,8 +325,15 @@ impl IslandManager {
     }
 }
 
-fn update_energy(activation: &mut RigidBodyActivation, sq_linvel: Real, sq_angvel: Real, dt: Real) {
-    if sq_linvel < activation.linear_threshold * activation.linear_threshold.abs()
+fn update_energy(
+    length_unit: Real,
+    activation: &mut RigidBodyActivation,
+    sq_linvel: Real,
+    sq_angvel: Real,
+    dt: Real,
+) {
+    let linear_threshold = activation.normalized_linear_threshold * length_unit;
+    if sq_linvel < linear_threshold * linear_threshold.abs()
         && sq_angvel < activation.angular_threshold * activation.angular_threshold.abs()
     {
         activation.time_since_can_sleep += dt;
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index d4043c2..7f95a31 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -1498,7 +1498,7 @@ impl RigidBodyBuilder {
         }
 
         if !self.can_sleep {
-            rb.activation.linear_threshold = -1.0;
+            rb.activation.normalized_linear_threshold = -1.0;
             rb.activation.angular_threshold = -1.0;
         }
 
diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs
index 998253c..c03233d 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -995,7 +995,10 @@ impl RigidBodyDominance {
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct RigidBodyActivation {
     /// The threshold linear velocity bellow which the body can fall asleep.
-    pub linear_threshold: Real,
+    ///
+    /// The value is "normalized", i.e., the actual threshold applied by the physics engine
+    /// is equal to this value multiplied by [`IntegrationParameters::length_unit`].
+    pub normalized_linear_threshold: Real,
     /// The angular linear velocity bellow which the body can fall asleep.
     pub angular_threshold: Real,
     /// The amount of time the rigid-body must remain below the thresholds to be put to sleep.
@@ -1014,7 +1017,7 @@ impl Default for RigidBodyActivation {
 
 impl RigidBodyActivation {
     /// The default linear velocity bellow which a body can be put to sleep.
-    pub fn default_linear_threshold() -> Real {
+    pub fn default_normalized_linear_threshold() -> Real {
         0.4
     }
 
@@ -1032,7 +1035,7 @@ impl RigidBodyActivation {
     /// Create a new rb_activation status initialised with the default rb_activation threshold and is active.
     pub fn active() -> Self {
         RigidBodyActivation {
-            linear_threshold: Self::default_linear_threshold(),
+            normalized_linear_threshold: Self::default_normalized_linear_threshold(),
             angular_threshold: Self::default_angular_threshold(),
             time_until_sleep: Self::default_time_until_sleep(),
             time_since_can_sleep: 0.0,
@@ -1043,7 +1046,7 @@ impl RigidBodyActivation {
     /// Create a new rb_activation status initialised with the default rb_activation threshold and is inactive.
     pub fn inactive() -> Self {
         RigidBodyActivation {
-            linear_threshold: Self::default_linear_threshold(),
+            normalized_linear_threshold: Self::default_normalized_linear_threshold(),
             angular_threshold: Self::default_angular_threshold(),
             time_until_sleep: Self::default_time_until_sleep(),
             time_since_can_sleep: Self::default_time_until_sleep(),
@@ -1054,7 +1057,7 @@ impl RigidBodyActivation {
     /// Create a new activation status that prevents the rigid-body from sleeping.
     pub fn cannot_sleep() -> Self {
         RigidBodyActivation {
-            linear_threshold: -1.0,
+            normalized_linear_threshold: -1.0,
             angular_threshold: -1.0,
             ..Self::active()
         }
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
index c31d2aa..2392493 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
@@ -310,8 +310,8 @@ impl OneBodyConstraintBuilder {
             {
                 let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
                 let rhs_bias = erp_inv_dt
-                    * (dist + params.allowed_linear_error)
-                        .clamp(-params.max_penetration_correction, 0.0);
+                    * (dist + params.allowed_linear_error())
+                        .clamp(-params.max_penetration_correction(), 0.0);
                 let new_rhs = rhs_wo_bias + rhs_bias;
                 is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
 
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
index 77dfc42..5710bc4 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
@@ -265,9 +265,9 @@ impl SimdOneBodyConstraintBuilder {
         let cfm_factor = SimdReal::splat(params.cfm_factor());
         let dt = SimdReal::splat(params.dt);
         let inv_dt = SimdReal::splat(params.inv_dt());
-        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
+        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
-        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
+        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction());
         let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
 
         let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs
index f0d95d9..39f4931 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs
@@ -410,8 +410,8 @@ impl TwoBodyConstraintBuilder {
             {
                 let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
                 let rhs_bias = erp_inv_dt
-                    * (dist + params.allowed_linear_error)
-                        .clamp(-params.max_penetration_correction, 0.0);
+                    * (dist + params.allowed_linear_error())
+                        .clamp(-params.max_penetration_correction(), 0.0);
                 let new_rhs = rhs_wo_bias + rhs_bias;
                 is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
 
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
index a6c387d..0b1b419 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
@@ -255,9 +255,9 @@ impl TwoBodyConstraintBuilderSimd {
         let cfm_factor = SimdReal::splat(params.cfm_factor());
         let dt = SimdReal::splat(params.dt);
         let inv_dt = SimdReal::splat(params.inv_dt());
-        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
+        let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
-        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
+        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction());
         let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
 
         let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]];
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 0c1685b..3884e19 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -113,7 +113,7 @@ impl PhysicsPipeline {
         self.broadphase_collider_pairs.clear();
         broad_phase.update(
             integration_parameters.dt,
-            integration_parameters.prediction_distance,
+            integration_parameters.prediction_distance(),
             colliders,
             bodies,
             modified_colliders,
@@ -143,7 +143,7 @@ impl PhysicsPipeline {
             events,
         );
         narrow_phase.compute_contacts(
-            integration_parameters.prediction_distance,
+            integration_parameters.prediction_distance(),
             integration_parameters.dt,
             bodies,
             colliders,
@@ -174,6 +174,7 @@ impl PhysicsPipeline {
         self.counters.stages.island_construction_time.resume();
         islands.update_active_set_with_contacts(
             integration_parameters.dt,
+            integration_parameters.length_unit,
             bodies,
             colliders,
             narrow_phase,