Merge pull request #102 from EmbarkStudios/apply-forces-in-velocity-solver

Apply accelerations during velocity solver

1 files changed, 16 insertions, 6 deletions

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();
         }
     }
 }