From d999e0e8c61aa2f115dc256e8011543cda8af7ef Mon Sep 17 00:00:00 2001
From: Emil Ernerfeldt <emil.ernerfeldt@gmail.com>
Date: Wed, 3 Feb 2021 18:18:03 +0100
Subject: Apply accelerations during velocity solver

Closes https://github.com/dimforge/rapier/issues/97

Instead of applying accelerations from gravity and external forces
as a separate step, this PR switches to applying them in the velocity solver.
---
 src/dynamics/solver/island_solver.rs | 22 ++++++++++++++++------
 1 file changed, 16 insertions(+), 6 deletions(-)

(limited to 'src/dynamics/solver/island_solver.rs')

diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index deed8c2..d0866bf 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -35,7 +35,9 @@ impl IslandSolver {
         joints: &mut [JointGraphEdge],
         joint_indices: &[JointIndex],
     ) {
-        if manifold_indices.len() != 0 || joint_indices.len() != 0 {
+        let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0;
+
+        if has_constraints {
             counters.solver.velocity_assembly_time.resume();
             self.contact_constraints
                 .init(island_id, params, bodies, manifolds, manifold_indices);
@@ -54,13 +56,13 @@ impl IslandSolver {
                 &mut self.joint_constraints.velocity_constraints,
             );
             counters.solver.velocity_resolution_time.pause();
-        }
 
-        counters.solver.velocity_update_time.resume();
-        bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt));
-        counters.solver.velocity_update_time.pause();
+            counters.solver.velocity_update_time.resume();
+            bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
+                rb.integrate(params.dt)
+            });
+            counters.solver.velocity_update_time.pause();
 
-        if manifold_indices.len() != 0 || joint_indices.len() != 0 {
             counters.solver.position_resolution_time.resume();
             self.position_solver.solve(
                 island_id,
@@ -70,6 +72,14 @@ impl IslandSolver {
                 &self.joint_constraints.position_constraints,
             );
             counters.solver.position_resolution_time.pause();
+        } else {
+            counters.solver.velocity_update_time.resume();
+            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(params.dt);
+            });
+            counters.solver.velocity_update_time.pause();
         }
     }
 }
-- 
cgit