Some small performance improvements.

4 files changed, 13 insertions, 12 deletions

diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index 41b195c..134b8a6 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -52,7 +52,7 @@ impl CCDSolver {
                             * crate::utils::inv(body.max_point_velocity()))
                         .min(dt);
                         // println!("Min toi: {}, Toi: {}", min_toi, toi);
-                        body.integrate_next_position(toi.max(min_toi), false);
+                        body.integrate_next_position(toi.max(min_toi));
                     }
                 }
             }
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index e4373c0..e039bfc 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -38,7 +38,7 @@ pub struct IntegrationParameters {
     /// Each cached impulse are multiplied by this coefficient in `[0, 1]`
     /// when they are re-used to initialize the solver (default `1.0`).
     pub warmstart_coeff: Real,
-    /// Correction factor to avoid large warmstart impulse after a strong impact.
+    /// Correction factor to avoid large warmstart impulse after a strong impact (default `10.0`).
     pub warmstart_correction_slope: Real,
 
     /// 0-1: how much of the velocity to dampen out in the constraint solver?
@@ -165,7 +165,7 @@ impl Default for IntegrationParameters {
             velocity_solve_fraction: 1.0,
             velocity_based_erp: 0.0,
             warmstart_coeff: 1.0,
-            warmstart_correction_slope: 1.0,
+            warmstart_correction_slope: 10.0,
             allowed_linear_error: 0.005,
             prediction_distance: 0.002,
             allowed_angular_error: 0.001,
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 0b3c01d..8176227 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -457,15 +457,14 @@ impl RigidBody {
         shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
     }
 
-    pub(crate) fn integrate_next_position(&mut self, dt: Real, apply_damping: bool) {
-        // TODO: do we want to apply damping before or after the velocity integration?
-        if apply_damping {
-            self.linvel *= 1.0 / (1.0 + dt * self.linear_damping);
-            self.angvel *= 1.0 / (1.0 + dt * self.angular_damping);
-        }
+    pub(crate) fn apply_damping(&mut self, dt: Real) {
+        self.linvel *= 1.0 / (1.0 + dt * self.linear_damping);
+        self.angvel *= 1.0 / (1.0 + dt * self.angular_damping);
+    }
 
+    pub(crate) fn integrate_next_position(&mut self, dt: Real) {
         self.next_position = self.integrate_velocity(dt) * self.position;
-        let _ = self.next_position.rotation.renormalize();
+        let _ = self.next_position.rotation.renormalize_fast();
     }
 
     /// The linear velocity of this rigid-body.
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index c117457..c684cc5 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -77,7 +77,8 @@ impl IslandSolver {
 
             counters.solver.velocity_update_time.resume();
             bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
-                rb.integrate_next_position(params.dt, true)
+                rb.apply_damping(params.dt);
+                rb.integrate_next_position(params.dt);
             });
             counters.solver.velocity_update_time.pause();
         } else {
@@ -87,7 +88,8 @@ impl IslandSolver {
             bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
                 // Since we didn't run the velocity solver we need to integrate the accelerations here
                 rb.integrate_accelerations(params.dt);
-                rb.integrate_next_position(params.dt, true);
+                rb.apply_damping(params.dt);
+                rb.integrate_next_position(params.dt);
             });
             counters.solver.velocity_update_time.pause();
         }