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

Apply accelerations during velocity solver

4 files changed, 121 insertions, 40 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 73b8969..859beb2 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -75,8 +75,10 @@ pub struct RigidBody {
     pub linear_damping: Real,
     /// Damping factor for gradually slowing down the angular motion of the rigid-body.
     pub angular_damping: Real,
-    pub(crate) linacc: Vector<Real>,
-    pub(crate) angacc: AngVector<Real>,
+    /// Accumulation of external forces (only for dynamic bodies).
+    pub(crate) force: Vector<Real>,
+    /// Accumulation of external torques (only for dynamic bodies).
+    pub(crate) torque: AngVector<Real>,
     pub(crate) colliders: Vec<ColliderHandle>,
     pub(crate) gravity_scale: Real,
     /// Whether or not this rigid-body is sleeping.
@@ -105,8 +107,8 @@ impl RigidBody {
             effective_world_inv_inertia_sqrt: AngularInertia::zero(),
             linvel: Vector::zeros(),
             angvel: na::zero(),
-            linacc: Vector::zeros(),
-            angacc: na::zero(),
+            force: Vector::zeros(),
+            torque: na::zero(),
             gravity_scale: 1.0,
             linear_damping: 0.0,
             angular_damping: 0.0,
@@ -133,14 +135,22 @@ 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 {
-            self.linvel += (gravity * self.gravity_scale + self.linacc) * dt;
-            self.linacc = na::zero();
+            self.force += gravity * self.gravity_scale * self.mass();
         }
+    }
+
+    #[cfg(not(feature = "parallel"))] // in parallel solver this is not needed
+    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 += self.angacc * dt;
-        self.angacc = na::zero();
+        self.linvel += linear_acc * dt;
+        self.angvel += angular_acc * dt;
+        self.force = na::zero();
+        self.torque = na::zero();
     }
 
     /// The mass properties of this rigid-body.
@@ -467,14 +477,16 @@ impl RigidBody {
             }
         }
     }
+}
 
-    /*
-     * Application of forces/impulses.
-     */
+/// ## Applying forces and torques
+impl RigidBody {
     /// Applies a force at the center-of-mass of this rigid-body.
+    /// The force will be applied in the next simulation step.
+    /// This does nothing on non-dynamic bodies.
     pub fn apply_force(&mut self, force: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.linacc += force * self.effective_inv_mass;
+            self.force += force;
 
             if wake_up {
                 self.wake_up(true);
@@ -482,10 +494,13 @@ impl RigidBody {
         }
     }
 
-    /// Applies an impulse at the center-of-mass of this rigid-body.
-    pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) {
+    /// Applies a torque at the center-of-mass of this rigid-body.
+    /// The torque will be applied in the next simulation step.
+    /// This does nothing on non-dynamic bodies.
+    #[cfg(feature = "dim2")]
+    pub fn apply_torque(&mut self, torque: Real, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.linvel += impulse * self.effective_inv_mass;
+            self.torque += torque;
 
             if wake_up {
                 self.wake_up(true);
@@ -494,11 +509,12 @@ impl RigidBody {
     }
 
     /// Applies a torque at the center-of-mass of this rigid-body.
-    #[cfg(feature = "dim2")]
-    pub fn apply_torque(&mut self, torque: Real, wake_up: bool) {
+    /// The torque will be applied in the next simulation step.
+    /// This does nothing on non-dynamic bodies.
+    #[cfg(feature = "dim3")]
+    pub fn apply_torque(&mut self, torque: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.angacc += self.effective_world_inv_inertia_sqrt
-                * (self.effective_world_inv_inertia_sqrt * torque);
+            self.torque += torque;
 
             if wake_up {
                 self.wake_up(true);
@@ -506,20 +522,39 @@ impl RigidBody {
         }
     }
 
-    /// Applies a torque at the center-of-mass of this rigid-body.
-    #[cfg(feature = "dim3")]
-    pub fn apply_torque(&mut self, torque: Vector<Real>, wake_up: bool) {
+    /// Applies a force at the given world-space point of this rigid-body.
+    /// The force will be applied in the next simulation step.
+    /// This does nothing on non-dynamic bodies.
+    pub fn apply_force_at_point(&mut self, force: Vector<Real>, point: Point<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.angacc += self.effective_world_inv_inertia_sqrt
-                * (self.effective_world_inv_inertia_sqrt * torque);
+            self.force += force;
+            self.torque += (point - self.world_com).gcross(force);
 
             if wake_up {
                 self.wake_up(true);
             }
         }
     }
+}
+
+/// ## Applying impulses and angular impulses
+impl RigidBody {
+    /// Applies an impulse at the center-of-mass of this rigid-body.
+    /// The impulse is applied right away, changing the linear velocity.
+    /// This does nothing on non-dynamic bodies.
+    pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.linvel += impulse * self.effective_inv_mass;
 
-    /// Applies an impulsive torque at the center-of-mass of this rigid-body.
+            if wake_up {
+                self.wake_up(true);
+            }
+        }
+    }
+
+    /// Applies an angular impulse at the center-of-mass of this rigid-body.
+    /// The impulse is applied right away, changing the angular velocity.
+    /// This does nothing on non-dynamic bodies.
     #[cfg(feature = "dim2")]
     pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
@@ -532,7 +567,9 @@ impl RigidBody {
         }
     }
 
-    /// Applies an impulsive torque at the center-of-mass of this rigid-body.
+    /// Applies an angular impulse at the center-of-mass of this rigid-body.
+    /// The impulse is applied right away, changing the angular velocity.
+    /// This does nothing on non-dynamic bodies.
     #[cfg(feature = "dim3")]
     pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
@@ -545,14 +582,9 @@ impl RigidBody {
         }
     }
 
-    /// Applies a force at the given world-space point of this rigid-body.
-    pub fn apply_force_at_point(&mut self, force: Vector<Real>, point: Point<Real>, wake_up: bool) {
-        let torque = (point - self.world_com).gcross(force);
-        self.apply_force(force, wake_up);
-        self.apply_torque(torque, wake_up);
-    }
-
     /// Applies an impulse at the given world-space point of this rigid-body.
+    /// The impulse is applied right away, changing the linear and/or angular velocities.
+    /// This does nothing on non-dynamic bodies.
     pub fn apply_impulse_at_point(
         &mut self,
         impulse: Vector<Real>,
@@ -563,7 +595,9 @@ impl RigidBody {
         self.apply_impulse(impulse, wake_up);
         self.apply_torque_impulse(torque_impulse, wake_up);
     }
+}
 
+impl RigidBody {
     /// The velocity of the given world-space point on this rigid-body.
     pub fn velocity_at_point(&self, point: &Point<Real>) -> Vector<Real> {
         let dpt = point - self.world_com;
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.
          */