feat: implement new "small-steps" solver + joint improvements

2 files changed, 18 insertions, 17 deletions

diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
index f214b8c..706c810 100644
--- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
+++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
@@ -8,7 +8,7 @@ use crate::geometry::{Cone, Cylinder};
 use crate::math::{Isometry, Point, Real, Vector, DIM};
 use crate::pipeline::debug_render_pipeline::debug_render_backend::DebugRenderObject;
 use crate::pipeline::debug_render_pipeline::DebugRenderStyle;
-use crate::utils::WBasis;
+use crate::utils::SimdBasis;
 use std::any::TypeId;
 use std::collections::HashMap;
 
@@ -221,7 +221,7 @@ impl DebugRenderPipeline {
         }
 
         if self.mode.contains(DebugRenderMode::MULTIBODY_JOINTS) {
-            for (handle, multibody, link) in multibody_joints.iter() {
+            for (handle, _, multibody, link) in multibody_joints.iter() {
                 let anc_color = self.style.multibody_joint_anchor_color;
                 let sep_color = self.style.multibody_joint_separation_color;
                 let parent = multibody.link(link.parent_id().unwrap()).unwrap();
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 7423296..aa5ed23 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -1,14 +1,14 @@
 //! Physics pipeline structures.
 
 use crate::counters::Counters;
-#[cfg(not(feature = "parallel"))]
+// #[cfg(not(feature = "parallel"))]
 use crate::dynamics::IslandSolver;
+#[cfg(feature = "parallel")]
+use crate::dynamics::JointGraphEdge;
 use crate::dynamics::{
     CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
     RigidBodyChanges, RigidBodyHandle, RigidBodyPosition, RigidBodyType,
 };
-#[cfg(feature = "parallel")]
-use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
     BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair,
     ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex,
@@ -206,19 +206,14 @@ impl PhysicsPipeline {
 
         self.counters.stages.update_time.resume();
         for handle in islands.active_dynamic_bodies() {
+            // TODO: should that be moved to the solver (just like we moved
+            //       the multibody dynamics update) since it depends on dt?
             let rb = bodies.index_mut_internal(*handle);
             rb.mprops.update_world_mass_properties(&rb.pos.position);
             let effective_mass = rb.mprops.effective_mass();
             rb.forces
                 .compute_effective_force_and_torque(&gravity, &effective_mass);
         }
-
-        for multibody in &mut multibody_joints.multibodies {
-            multibody
-                .1
-                .update_dynamics(integration_parameters.dt, bodies);
-            multibody.1.update_acceleration(bodies);
-        }
         self.counters.stages.update_time.pause();
 
         self.counters.stages.solver_time.resume();
@@ -263,6 +258,10 @@ impl PhysicsPipeline {
             let manifold_indices = &self.manifold_indices[..];
             let joint_constraint_indices = &self.joint_constraint_indices[..];
 
+            // PERF: right now, we are only doing islands-based parallelism.
+            //       Intra-island parallelism (that hasn’t been ported to the new
+            //       solver yet) will be supported in the future.
+            self.counters.solver.velocity_resolution_time.resume();
             rayon::scope(|scope| {
                 enable_flush_to_zero!();
 
@@ -280,20 +279,22 @@ impl PhysicsPipeline {
                             std::mem::transmute(multibody_joints.load(Ordering::Relaxed))
                         };
 
+                        let mut counters = Counters::new(false);
                         solver.init_and_solve(
-                            scope,
                             island_id,
-                            islands,
+                            &mut counters,
                             integration_parameters,
+                            islands,
                             bodies,
-                            manifolds,
-                            &manifold_indices[island_id],
+                            &mut manifolds[..],
+                            &self.manifold_indices[island_id],
                             impulse_joints,
-                            &joint_constraint_indices[island_id],
+                            &self.joint_constraint_indices[island_id],
                             multibody_joints,
                         )
                     });
             });
+            self.counters.solver.velocity_resolution_time.pause();
         }
 
         // Generate contact force events if needed.