2 files changed, 24 insertions, 27 deletions
diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index 9b06a80..79c4495 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -52,32 +52,27 @@ impl CCDSolver {
     ///
     /// The `impacts` should be the result of a previous call to `self.predict_next_impacts`.
     pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) {
-        match impacts {
-            PredictedImpacts::Impacts(tois) => {
-                for (handle, toi) in tois {
-                    let rb = bodies.index_mut_internal(*handle);
-                    let local_com = &rb.mprops.local_mprops.local_com;
-
-                    let min_toi = (rb.ccd.ccd_thickness
-                        * 0.15
-                        * crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels)))
-                    .min(dt);
-                    // println!(
-                    //     "Min toi: {}, Toi: {}, thick: {}, max_vel: {}",
-                    //     min_toi,
-                    //     toi,
-                    //     rb.ccd.ccd_thickness,
-                    //     rb.ccd.max_point_velocity(&rb.integrated_vels)
-                    // );
-                    let new_pos = rb.integrated_vels.integrate(
-                        toi.max(min_toi),
-                        &rb.pos.position,
-                        &local_com,
-                    );
-                    rb.pos.next_position = new_pos;
-                }
+        if let PredictedImpacts::Impacts(tois) = impacts {
+            for (handle, toi) in tois {
+                let rb = bodies.index_mut_internal(*handle);
+                let local_com = &rb.mprops.local_mprops.local_com;
+
+                let min_toi = (rb.ccd.ccd_thickness
+                    * 0.15
+                    * crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels)))
+                .min(dt);
+                // println!(
+                //     "Min toi: {}, Toi: {}, thick: {}, max_vel: {}",
+                //     min_toi,
+                //     toi,
+                //     rb.ccd.ccd_thickness,
+                //     rb.ccd.max_point_velocity(&rb.integrated_vels)
+                // );
+                let new_pos =
+                    rb.integrated_vels
+                        .integrate(toi.max(min_toi), &rb.pos.position, local_com);
+                rb.pos.next_position = new_pos;
             }
-            _ => {}
         }
     }
 
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index 6f5a47d..719f3c5 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -169,13 +169,15 @@ impl TOIEntry {
 
 impl PartialOrd for TOIEntry {
     fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
-        (-self.toi).partial_cmp(&(-other.toi))
+        Some(self.cmp(other))
     }
 }
 
 impl Ord for TOIEntry {
     fn cmp(&self, other: &Self) -> std::cmp::Ordering {
-        self.partial_cmp(other).unwrap()
+        (-self.toi)
+            .partial_cmp(&(-other.toi))
+            .unwrap_or(std::cmp::Ordering::Equal)
     }
 }