Merge pull request #350 from dimforge/broad-phase-fix

Fix some corner cases in the broad-phase

2 files changed, 15 insertions, 0 deletions

diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs
index 2c8c5e3..8f47396 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs
@@ -352,8 +352,16 @@ impl BroadPhase {
             .compute_aabb(co_pos)
             .loosened(prediction_distance / 2.0);
 
+        if aabb.mins.coords.iter().any(|e| !e.is_finite())
+            || aabb.maxs.coords.iter().any(|e| !e.is_finite())
+        {
+            // Reject AABBs with non-finite values.
+            return false;
+        }
+
         aabb.mins = super::clamp_point(aabb.mins);
         aabb.maxs = super::clamp_point(aabb.maxs);
+
         let prev_aabb;
 
         let layer_id = if let Some(proxy) = self.proxies.get_mut(*proxy_index) {
diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs
index 216ea05..4316ecd 100644
--- a/src/geometry/broad_phase_multi_sap/sap_layer.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs
@@ -240,6 +240,13 @@ impl SAPLayer {
                     let region_proxy = &mut proxies[region_id];
                     let region = region_proxy.data.as_region_mut();
 
+                    // NOTE: sometimes, rounding errors will generate start/end indices
+                    //       that lie outside of the actual region’s AABB.
+                    // TODO: is there a smarter, more efficient way of dealing with this?
+                    if !region_proxy.aabb.intersects(aabb_to_discretize) {
+                        continue;
+                    }
+
                     if let Some(actual_aabb) = actual_aabb {
                         // NOTE: if the actual AABB doesn't intersect the
                         //       region’s AABB, then we need to delete the