Merge pull request #120 from dimforge/contact_modification

Add the ability to modify the contact points seen by the constraints solver

24 files changed, 739 insertions, 195 deletions

diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs
index 5040c8a..f4430aa 100644
--- a/examples2d/all_examples2.rs
+++ b/examples2d/all_examples2.rs
@@ -18,6 +18,7 @@ mod debug_box_ball2;
 mod heightfield2;
 mod joints2;
 mod locked_rotations2;
+mod one_way_platforms2;
 mod platform2;
 mod polyline2;
 mod pyramid2;
@@ -65,6 +66,7 @@ pub fn main() {
         ("Heightfield", heightfield2::init_world),
         ("Joints", joints2::init_world),
         ("Locked rotations", locked_rotations2::init_world),
+        ("One-way platforms", one_way_platforms2::init_world),
         ("Platform", platform2::init_world),
         ("Polyline", polyline2::init_world),
         ("Pyramid", pyramid2::init_world),
diff --git a/examples2d/damping2.rs b/examples2d/damping2.rs
index 43ca2b6..68fd77d 100644
--- a/examples2d/damping2.rs
+++ b/examples2d/damping2.rs
@@ -40,6 +40,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world_with_gravity(bodies, colliders, joints, Vector2::zeros());
+    testbed.set_world_with_params(bodies, colliders, joints, Vector2::zeros(), ());
     testbed.look_at(Point2::new(3.0, 2.0), 50.0);
 }
diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs
new file mode 100644
index 0000000..551eaf1
--- /dev/null
+++ b/examples2d/one_way_platforms2.rs
@@ -0,0 +1,143 @@
+use na::{Point2, Vector2};
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderHandle, ColliderSet};
+use rapier2d::pipeline::{ContactModificationContext, PhysicsHooks, PhysicsHooksFlags};
+use rapier_testbed2d::Testbed;
+
+struct OneWayPlatformHook {
+    platform1: ColliderHandle,
+    platform2: ColliderHandle,
+}
+
+impl PhysicsHooks for OneWayPlatformHook {
+    fn active_hooks(&self) -> PhysicsHooksFlags {
+        PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS
+    }
+
+    fn modify_solver_contacts(&self, context: &mut ContactModificationContext) {
+        // The allowed normal for the first platform is its local +y axis, and the
+        // allowed normal for the second platform is its local -y axis.
+        //
+        // Now we have to be careful because the `manifold.local_n1` normal points
+        // toward the outside of the shape of `context.co1`. So we need to flip the
+        // allowed normal direction if the platform is in `context.collider_handle2`.
+        //
+        // Therefore:
+        // - If context.collider_handle1 == self.platform1 then the allowed normal is +y.
+        // - If context.collider_handle2 == self.platform1 then the allowed normal is -y.
+        // - If context.collider_handle1 == self.platform2 then its allowed normal +y needs to be flipped to -y.
+        // - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y.
+        let mut allowed_local_n1 = Vector2::zeros();
+
+        if context.collider_handle1 == self.platform1 {
+            allowed_local_n1 = Vector2::y();
+        } else if context.collider_handle2 == self.platform1 {
+            // Flip the allowed direction.
+            allowed_local_n1 = -Vector2::y();
+        }
+
+        if context.collider_handle1 == self.platform2 {
+            allowed_local_n1 = -Vector2::y();
+        } else if context.collider_handle2 == self.platform2 {
+            // Flip the allowed direction.
+            allowed_local_n1 = -Vector2::y();
+        }
+
+        // Call the helper function that simulates one-way platforms.
+        context.update_as_oneway_platform(&allowed_local_n1, 0.1);
+
+        // Set the surface velocity of the accepted contacts.
+        let tangent_velocity = if context.collider_handle1 == self.platform1
+            || context.collider_handle2 == self.platform2
+        {
+            -12.0
+        } else {
+            12.0
+        };
+
+        for contact in context.solver_contacts.iter_mut() {
+            contact.tangent_velocity.x = tangent_velocity;
+        }
+    }
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+
+    let collider = ColliderBuilder::cuboid(25.0, 0.5)
+        .translation(30.0, 2.0)
+        .modify_solver_contacts(true)
+        .build();
+    let platform1 = colliders.insert(collider, handle, &mut bodies);
+    let collider = ColliderBuilder::cuboid(25.0, 0.5)
+        .translation(-30.0, -2.0)
+        .modify_solver_contacts(true)
+        .build();
+    let platform2 = colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Setup the one-way platform hook.
+     */
+    let physics_hooks = OneWayPlatformHook {
+        platform1,
+        platform2,
+    };
+
+    /*
+     * Spawn cubes at regular intervals and apply a custom gravity
+     * depending on their position.
+     */
+    testbed.add_callback(move |mut window, mut graphics, physics, _, run_state| {
+        if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 {
+            // Spawn a new cube.
+            let collider = ColliderBuilder::cuboid(1.5, 2.0).build();
+            let body = RigidBodyBuilder::new_dynamic()
+                .translation(20.0, 10.0)
+                .build();
+            let handle = physics.bodies.insert(body);
+            physics
+                .colliders
+                .insert(collider, handle, &mut physics.bodies);
+
+            if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) {
+                graphics.add(window, handle, &physics.bodies, &physics.colliders);
+            }
+        }
+
+        physics.bodies.foreach_active_dynamic_body_mut(|_, body| {
+            if body.position().translation.y > 1.0 {
+                body.set_gravity_scale(1.0, false);
+            } else if body.position().translation.y < -1.0 {
+                body.set_gravity_scale(-1.0, false);
+            }
+        });
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world_with_params(
+        bodies,
+        colliders,
+        joints,
+        Vector2::new(0.0, -9.81),
+        physics_hooks,
+    );
+    testbed.look_at(Point2::new(0.0, 0.0), 20.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index 9cbf4c3..8a71665 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -29,6 +29,7 @@ mod heightfield3;
 mod joints3;
 mod keva3;
 mod locked_rotations3;
+mod one_way_platforms3;
 mod platform3;
 mod primitives3;
 mod restitution3;
@@ -83,6 +84,7 @@ pub fn main() {
         ("Heightfield", heightfield3::init_world),
         ("Joints", joints3::init_world),
         ("Locked rotations", locked_rotations3::init_world),
+        ("One-way platforms", one_way_platforms3::init_world),
         ("Platform", platform3::init_world),
         ("Restitution", restitution3::init_world),
         ("Sensor", sensor3::init_world),
diff --git a/examples3d/damping3.rs b/examples3d/damping3.rs
index 8c68d3b..3847ceb 100644
--- a/examples3d/damping3.rs
+++ b/examples3d/damping3.rs
@@ -40,6 +40,6 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Set up the testbed.
      */
-    testbed.set_world_with_gravity(bodies, colliders, joints, Vector3::zeros());
+    testbed.set_world_with_params(bodies, colliders, joints, Vector3::zeros(), ());
     testbed.look_at(Point3::new(2.0, 2.5, 20.0), Point3::new(2.0, 2.5, 0.0));
 }
diff --git a/examples3d/fountain3.rs b/examples3d/fountain3.rs
index 5acc2e8..caaa21b 100644
--- a/examples3d/fountain3.rs
+++ b/examples3d/fountain3.rs
@@ -23,16 +23,14 @@ pub fn init_world(testbed: &mut Testbed) {
     let handle = bodies.insert(rigid_body);
     let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
     colliders.insert(collider, handle, &mut bodies);
-    let mut k = 0;
 
     // Callback that will be executed on the main loop to handle proximities.
-    testbed.add_callback(move |mut window, mut graphics, physics, _, _| {
-        k += 1;
+    testbed.add_callback(move |mut window, mut graphics, physics, _, run_state| {
         let rigid_body = RigidBodyBuilder::new_dynamic()
             .translation(0.0, 10.0, 0.0)
             .build();
         let handle = physics.bodies.insert(rigid_body);
-        let collider = match k % 3 {
+        let collider = match run_state.timestep_id % 3 {
             0 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
             1 => ColliderBuilder::cone(rad, rad).build(),
             _ => ColliderBuilder::cuboid(rad, rad, rad).build(),
diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs
new file mode 100644
index 0000000..173d03d
--- /dev/null
+++ b/examples3d/one_way_platforms3.rs
@@ -0,0 +1,143 @@
+use na::{Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderHandle, ColliderSet};
+use rapier3d::pipeline::{ContactModificationContext, PhysicsHooks, PhysicsHooksFlags};
+use rapier_testbed3d::Testbed;
+
+struct OneWayPlatformHook {
+    platform1: ColliderHandle,
+    platform2: ColliderHandle,
+}
+
+impl PhysicsHooks for OneWayPlatformHook {
+    fn active_hooks(&self) -> PhysicsHooksFlags {
+        PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS
+    }
+
+    fn modify_solver_contacts(&self, context: &mut ContactModificationContext) {
+        // The allowed normal for the first platform is its local +y axis, and the
+        // allowed normal for the second platform is its local -y axis.
+        //
+        // Now we have to be careful because the `manifold.local_n1` normal points
+        // toward the outside of the shape of `context.co1`. So we need to flip the
+        // allowed normal direction if the platform is in `context.collider_handle2`.
+        //
+        // Therefore:
+        // - If context.collider_handle1 == self.platform1 then the allowed normal is +y.
+        // - If context.collider_handle2 == self.platform1 then the allowed normal is -y.
+        // - If context.collider_handle1 == self.platform2 then its allowed normal +y needs to be flipped to -y.
+        // - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y.
+        let mut allowed_local_n1 = Vector3::zeros();
+
+        if context.collider_handle1 == self.platform1 {
+            allowed_local_n1 = Vector3::y();
+        } else if context.collider_handle2 == self.platform1 {
+            // Flip the allowed direction.
+            allowed_local_n1 = -Vector3::y();
+        }
+
+        if context.collider_handle1 == self.platform2 {
+            allowed_local_n1 = -Vector3::y();
+        } else if context.collider_handle2 == self.platform2 {
+            // Flip the allowed direction.
+            allowed_local_n1 = -Vector3::y();
+        }
+
+        // Call the helper function that simulates one-way platforms.
+        context.update_as_oneway_platform(&allowed_local_n1, 0.1);
+
+        // Set the surface velocity of the accepted contacts.
+        let tangent_velocity = if context.collider_handle1 == self.platform1
+            || context.collider_handle2 == self.platform2
+        {
+            -12.0
+        } else {
+            12.0
+        };
+
+        for contact in context.solver_contacts.iter_mut() {
+            contact.tangent_velocity.z = tangent_velocity;
+        }
+    }
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+
+    let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0)
+        .translation(0.0, 2.0, 30.0)
+        .modify_solver_contacts(true)
+        .build();
+    let platform1 = colliders.insert(collider, handle, &mut bodies);
+    let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0)
+        .translation(0.0, -2.0, -30.0)
+        .modify_solver_contacts(true)
+        .build();
+    let platform2 = colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Setup the one-way platform hook.
+     */
+    let physics_hooks = OneWayPlatformHook {
+        platform1,
+        platform2,
+    };
+
+    /*
+     * Spawn cubes at regular intervals and apply a custom gravity
+     * depending on their position.
+     */
+    testbed.add_callback(move |mut window, mut graphics, physics, _, run_state| {
+        if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 {
+            // Spawn a new cube.
+            let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.5).build();
+            let body = RigidBodyBuilder::new_dynamic()
+                .translation(0.0, 6.0, 20.0)
+                .build();
+            let handle = physics.bodies.insert(body);
+            physics
+                .colliders
+                .insert(collider, handle, &mut physics.bodies);
+
+            if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) {
+                graphics.add(window, handle, &physics.bodies, &physics.colliders);
+            }
+        }
+
+        physics.bodies.foreach_active_dynamic_body_mut(|_, body| {
+            if body.position().translation.y > 1.0 {
+                body.set_gravity_scale(1.0, false);
+            } else if body.position().translation.y < -1.0 {
+                body.set_gravity_scale(-1.0, false);
+            }
+        });
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world_with_params(
+        bodies,
+        colliders,
+        joints,
+        Vector3::new(0.0, -9.81, 0.0),
+        physics_hooks,
+    );
+    testbed.look_at(Point3::new(-100.0, 0.0, 0.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index 79d5827..f459d4f 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -234,13 +234,27 @@ impl RigidBodySet {
         self.bodies.get(handle.0)
     }
 
+    fn mark_as_modified(
+        handle: RigidBodyHandle,
+        rb: &mut RigidBody,
+        modified_bodies: &mut Vec<RigidBodyHandle>,
+        modified_all_bodies: bool,
+    ) {
+        if !modified_all_bodies && !rb.changes.contains(RigidBodyChanges::MODIFIED) {
+            rb.changes = RigidBodyChanges::MODIFIED;
+            modified_bodies.push(handle);
+        }
+    }
+
     /// Gets a mutable reference to the rigid-body with the given handle.
     pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
         let result = self.bodies.get_mut(handle.0)?;
-        if !self.modified_all_bodies && !result.changes.contains(RigidBodyChanges::MODIFIED) {
-            result.changes = RigidBodyChanges::MODIFIED;
-            self.modified_bodies.push(handle);
-        }
+        Self::mark_as_modified(
+            handle,
+            result,
+            &mut self.modified_bodies,
+            self.modified_all_bodies,
+        );
         Some(result)
     }
 
@@ -300,6 +314,26 @@ impl RigidBodySet {
             .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
     }
 
+    /// Applies the given function on all the active dynamic rigid-bodies
+    /// contained by this set.
+    #[inline(always)]
+    pub fn foreach_active_dynamic_body_mut(
+        &mut self,
+        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
+    ) {
+        for handle in &self.active_dynamic_set {
+            if let Some(rb) = self.bodies.get_mut(handle.0) {
+                Self::mark_as_modified(
+                    *handle,
+                    rb,
+                    &mut self.modified_bodies,
+                    self.modified_all_bodies,
+                );
+                f(*handle, rb)
+            }
+        }
+    }
+
     #[inline(always)]
     pub(crate) fn foreach_active_body_mut_internal(
         &mut self,
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 2cac93d..2de9807 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -124,7 +124,7 @@ pub(crate) struct VelocityConstraint {
     pub mj_lambda1: usize,
     pub mj_lambda2: usize,
     pub manifold_id: ContactManifoldIndex,
-    pub manifold_contact_id: usize,
+    pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
     pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS],
 }
@@ -152,7 +152,7 @@ impl VelocityConstraint {
         let force_dir1 = -manifold.data.normal;
         let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
 
-        for (l, manifold_points) in manifold
+        for (_l, manifold_points) in manifold
             .data
             .solver_contacts
             .chunks(MAX_MANIFOLD_POINTS)
@@ -168,7 +168,7 @@ impl VelocityConstraint {
                 mj_lambda1,
                 mj_lambda2,
                 manifold_id,
-                manifold_contact_id: l * MAX_MANIFOLD_POINTS,
+                manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
                 num_contacts: manifold_points.len() as u8,
             };
 
@@ -211,7 +211,7 @@ impl VelocityConstraint {
                 constraint.mj_lambda1 = mj_lambda1;
                 constraint.mj_lambda2 = mj_lambda2;
                 constraint.manifold_id = manifold_id;
-                constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
+                constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS];
                 constraint.num_contacts = manifold_points.len() as u8;
             }
 
@@ -224,6 +224,8 @@ impl VelocityConstraint {
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
 
                 constraint.limit = manifold_point.friction;
+                constraint.manifold_contact_id[k] = manifold_point.contact_id;
+
                 // Normal part.
                 {
                     let gcross1 = rb1
@@ -271,7 +273,8 @@ impl VelocityConstraint {
                                 + rb2.effective_inv_mass
                                 + gcross1.gdot(gcross1)
                                 + gcross2.gdot(gcross2));
-                        let rhs = (vel1 - vel2).dot(&tangents1[j]);
+                        let rhs =
+                            (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
                         #[cfg(feature = "dim2")]
                         let impulse = manifold_point.data.tangent_impulse * warmstart_coeff;
                         #[cfg(feature = "dim3")]
@@ -292,7 +295,7 @@ impl VelocityConstraint {
             if push {
                 out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
             } else {
-                out_constraints[manifold.data.constraint_index + l] =
+                out_constraints[manifold.data.constraint_index + _l] =
                     AnyVelocityConstraint::Nongrouped(constraint);
             }
         }
@@ -382,19 +385,18 @@ impl VelocityConstraint {
 
     pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
         let manifold = &mut manifolds_all[self.manifold_id];
-        let k_base = self.manifold_contact_id;
 
         for k in 0..self.num_contacts as usize {
-            let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
-            active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
+            let contact_id = self.manifold_contact_id[k];
+            let active_contact = &mut manifold.points[contact_id as usize];
+            active_contact.data.impulse = self.elements[k].normal_part.impulse;
             #[cfg(feature = "dim2")]
             {
-                active_contacts[k_base + k].data.tangent_impulse =
-                    self.elements[k].tangent_part[0].impulse;
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse;
             }
             #[cfg(feature = "dim3")]
             {
-                active_contacts[k_base + k].data.tangent_impulse = [
+                active_contact.data.tangent_impulse = [
                     self.elements[k].tangent_part[0].impulse,
                     self.elements[k].tangent_part[1].impulse,
                 ];
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 3e068e7..97fa261 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -55,7 +55,7 @@ pub(crate) struct WVelocityConstraint {
     pub mj_lambda1: [usize; SIMD_WIDTH],
     pub mj_lambda2: [usize; SIMD_WIDTH],
     pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
-    pub manifold_contact_id: usize,
+    pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
 }
 
 impl WVelocityConstraint {
@@ -116,7 +116,7 @@ impl WVelocityConstraint {
                 mj_lambda1,
                 mj_lambda2,
                 manifold_id,
-                manifold_contact_id: l,
+                manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
                 num_contacts: num_points as u8,
             };
 
@@ -130,6 +130,8 @@ impl WVelocityConstraint {
                 );
                 let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
                 let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
+                let tangent_velocity =
+                    Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
 
                 let impulse =
                     SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
@@ -141,6 +143,8 @@ impl WVelocityConstraint {
                 let vel2 = linvel2 + angvel2.gcross(dp2);
 
                 constraint.limit = friction;
+                constraint.manifold_contact_id[k] =
+                    array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
 
                 // Normal part.
                 {
@@ -179,7 +183,7 @@ impl WVelocityConstraint {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
                     let r = SimdReal::splat(1.0)
                         / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
-                    let rhs = (vel1 - vel2).dot(&tangents1[j]);
+                    let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]);
 
                     constraint.elements[k].tangent_parts[j] = WVelocityConstraintElementPart {
                         gcross1,
@@ -332,17 +336,17 @@ impl WVelocityConstraint {
 
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
-                let k_base = self.manifold_contact_id;
-                let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
-                active_contacts[k_base + k].data.impulse = impulses[ii];
+                let contact_id = self.manifold_contact_id[k][ii];
+                let active_contact = &mut manifold.points[contact_id as usize];
+                active_contact.data.impulse = impulses[ii];
 
                 #[cfg(feature = "dim2")]
                 {
-                    active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii];
+                    active_contact.data.tangent_impulse = tangent_impulses[ii];
                 }
                 #[cfg(feature = "dim3")]
                 {
-                    active_contacts[k_base + k].data.tangent_impulse =
+                    active_contact.data.tangent_impulse =
                         [tangent_impulses[ii], bitangent_impulses[ii]];
                 }
             }
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 88f864a..beb07ed 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -49,7 +49,7 @@ pub(crate) struct VelocityGroundConstraint {
     pub limit: Real,
     pub mj_lambda2: usize,
     pub manifold_id: ContactManifoldIndex,
-    pub manifold_contact_id: usize,
+    pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
     pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
 }
@@ -68,17 +68,17 @@ impl VelocityGroundConstraint {
         let mut rb2 = &bodies[manifold.data.body_pair.body2];
         let flipped = !rb2.is_dynamic();
 
-        let force_dir1 = if flipped {
+        let (force_dir1, flipped_multiplier) = if flipped {
             std::mem::swap(&mut rb1, &mut rb2);
-            manifold.data.normal
+            (manifold.data.normal, -1.0)
         } else {
-            -manifold.data.normal
+            (-manifold.data.normal, 1.0)
         };
 
         let mj_lambda2 = rb2.active_set_offset;
         let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
 
-        for (l, manifold_points) in manifold
+        for (_l, manifold_points) in manifold
             .data
             .solver_contacts
             .chunks(MAX_MANIFOLD_POINTS)
@@ -92,7 +92,7 @@ impl VelocityGroundConstraint {
                 limit: 0.0,
                 mj_lambda2,
                 manifold_id,
-                manifold_contact_id: l * MAX_MANIFOLD_POINTS,
+                manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
                 num_contacts: manifold_points.len() as u8,
             };
 
@@ -123,7 +123,7 @@ impl VelocityGroundConstraint {
                     .as_nongrouped_ground_mut()
                     .unwrap()
             } else {
-                unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
+                unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
             };
 
             #[cfg(target_arch = "wasm32")]
@@ -133,7 +133,7 @@ impl VelocityGroundConstraint {
                 constraint.limit = 0.0;
                 constraint.mj_lambda2 = mj_lambda2;
                 constraint.manifold_id = manifold_id;
-                constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
+                constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS];
                 constraint.num_contacts = manifold_points.len() as u8;
             }
 
@@ -145,6 +145,7 @@ impl VelocityGroundConstraint {
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
 
                 constraint.limit = manifold_point.friction;
+                constraint.manifold_contact_id[k] = manifold_point.contact_id;
 
                 // Normal part.
                 {
@@ -178,7 +179,9 @@ impl VelocityGroundConstraint {
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dp2.gcross(-tangents1[j]));
                         let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
-                        let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
+                        let rhs = (vel1 - vel2
+                            + flipped_multiplier * manifold_point.tangent_velocity)
+                            .dot(&tangents1[j]);
                         #[cfg(feature = "dim2")]
                         let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff;
                         #[cfg(feature = "dim3")]
@@ -199,7 +202,7 @@ impl VelocityGroundConstraint {
             if push {
                 out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
             } else {
-                out_constraints[manifold.data.constraint_index + l] =
+                out_constraints[manifold.data.constraint_index + _l] =
                     AnyVelocityConstraint::NongroupedGround(constraint);
             }
         }
@@ -267,19 +270,18 @@ impl VelocityGroundConstraint {
     // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
     pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
         let manifold = &mut manifolds_all[self.manifold_id];
-        let k_base = self.manifold_contact_id;
 
         for k in 0..self.num_contacts as usize {
-            let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
-            active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
+            let contact_id = self.manifold_contact_id[k];
+            let active_contact = &mut manifold.points[contact_id as usize];
+            active_contact.data.impulse = self.elements[k].normal_part.impulse;
             #[cfg(feature = "dim2")]
             {
-                active_contacts[k_base + k].data.tangent_impulse =
-                    self.elements[k].tangent_part[0].impulse;
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse;
             }
             #[cfg(feature = "dim3")]
             {
-                active_contacts[k_base + k].data.tangent_impulse = [
+                active_contact.data.tangent_impulse = [
                     self.elements[k].tangent_part[0].impulse,
                     self.elements[k].tangent_part[1].impulse,
                 ];
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 29ba5e9..6e7216a 100644
--- a/