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.

4 files changed, 63 insertions, 12 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 210713b..7d9f964 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -135,16 +135,20 @@ impl RigidBody {
         self.active_set_timestamp = 0;
     }
 
-    pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector<Real>) {
+    pub(crate) fn add_gravity(&mut self, gravity: Vector<Real>) {
         if self.effective_inv_mass != 0.0 {
-            let acceleration = self.force * self.effective_inv_mass;
-            self.linvel += (gravity * self.gravity_scale + acceleration) * dt;
-            self.force = na::zero();
+            self.force += gravity * self.gravity_scale * self.mass();
         }
+    }
 
-        let angular_acceleration = self.effective_world_inv_inertia_sqrt
+    pub(crate) fn integrate_accelerations(&mut self, dt: Real) {
+        let linear_acc = self.force * self.effective_inv_mass;
+        let angular_acc = self.effective_world_inv_inertia_sqrt
             * (self.effective_world_inv_inertia_sqrt * self.torque);
-        self.angvel += angular_acceleration * dt;
+
+        self.linvel += linear_acc * dt;
+        self.angvel += angular_acc * dt;
+        self.force = na::zero();
         self.torque = na::zero();
     }
 
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();
         }
     }
 }
diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs
index af8e9c0..99c1ec5 100644
--- a/src/dynamics/solver/parallel_island_solver.rs
+++ b/src/dynamics/solver/parallel_island_solver.rs
@@ -184,6 +184,31 @@ impl ParallelIslandSolver {
         self.positions
             .resize(bodies.active_island(island_id).len(), Isometry::identity());
 
+        {
+            // Initialize `mj_lambdas` (per-body velocity deltas) with external accelerations (gravity etc):
+
+            let island_range = bodies.active_island_range(island_id);
+            let active_bodies = &bodies.active_dynamic_set[island_range];
+            let bodies = &mut bodies.bodies;
+
+            let thread = &self.thread;
+
+            concurrent_loop! {
+                let batch_size = thread.batch_size;
+                for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
+                    let rb = &mut bodies[handle.0];
+                    let dvel = &mut self.mj_lambdas[rb.active_set_offset];
+
+                    dvel.linear += rb.force * (rb.effective_inv_mass * params.dt);
+                    rb.force = na::zero();
+
+                    // dvel.angular is actually storing angular velocity delta multiplied by the square root of the inertia tensor:
+                    dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt;
+                    rb.torque = na::zero();
+                }
+            }
+        }
+
         for _ in 0..num_task_per_island {
             // We use AtomicPtr because it is Send+Sync while *mut is not.
             // See https://internals.rust-lang.org/t/shouldnt-pointers-be-send-sync-or/8818
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index 89cf34e..4f35f3b 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -32,6 +32,18 @@ impl VelocitySolver {
         self.mj_lambdas
             .resize(bodies.active_island(island_id).len(), DeltaVel::zero());
 
+        // Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc):
+        bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
+            let dvel = &mut self.mj_lambdas[rb.active_set_offset];
+
+            dvel.linear += rb.force * (rb.effective_inv_mass * params.dt);
+            rb.force = na::zero();
+
+            // dvel.angular is actually storing angular velocity delta multiplied by the square root of the inertia tensor:
+            dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt;
+            rb.torque = na::zero();
+        });
+
         /*
          * Warmstart constraints.
          */