Move all the contact manifold computations out of Rapier.

41 files changed, 87 insertions, 2494 deletions

diff --git a/src/data/arena.rs b/src/data/arena.rs
index a3af45c..3a24cd1 100644
--- a/src/data/arena.rs
+++ b/src/data/arena.rs
@@ -3,7 +3,7 @@
 //! See  https://github.com/fitzgen/generational-arena/blob/master/src/lib.rs.
 //! This has been modified to have a fully deterministic deserialization (including for the order of
 //! Index attribution after a deserialization of the arena.
-use buckler::partitioning::IndexedData;
+use eagl::partitioning::IndexedData;
 use std::cmp;
 use std::iter::{self, Extend, FromIterator, FusedIterator};
 use std::mem;
diff --git a/src/data/hashmap.rs b/src/data/hashmap.rs
deleted file mode 100644
index d2ea980..0000000
--- a/src/data/hashmap.rs
+++ /dev/null
@@ -1,137 +0,0 @@
-//! A hash-map that behaves deterministically when the
-//! `enhanced-determinism` feature is enabled.
-
-#[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))]
-use indexmap::IndexMap as StdHashMap;
-#[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))]
-use std::collections::HashMap as StdHashMap;
-
-/// Serializes only the capacity of a hash-map instead of its actual content.
-#[cfg(feature = "serde-serialize")]
-pub fn serialize_hashmap_capacity<S: serde::Serializer, K, V, H: std::hash::BuildHasher>(
-    map: &StdHashMap<K, V, H>,
-    s: S,
-) -> Result<S::Ok, S::Error> {
-    s.serialize_u64(map.capacity() as u64)
-}
-
-/// Creates a new hash-map with its capacity deserialized from `d`.
-#[cfg(feature = "serde-serialize")]
-pub fn deserialize_hashmap_capacity<
-    'de,
-    D: serde::Deserializer<'de>,
-    K,
-    V,
-    H: std::hash::BuildHasher + Default,
->(
-    d: D,
-) -> Result<StdHashMap<K, V, H>, D::Error> {
-    struct CapacityVisitor;
-    impl<'de> serde::de::Visitor<'de> for CapacityVisitor {
-        type Value = u64;
-
-        fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
-            write!(formatter, "an integer between 0 and 2^64")
-        }
-
-        fn visit_u64<E: serde::de::Error>(self, val: u64) -> Result<Self::Value, E> {
-            Ok(val)
-        }
-    }
-
-    let capacity = d.deserialize_u64(CapacityVisitor)? as usize;
-    Ok(StdHashMap::with_capacity_and_hasher(
-        capacity,
-        Default::default(),
-    ))
-}
-
-/*
- * FxHasher taken from rustc_hash, except that it does not depend on the pointer size.
- */
-#[cfg(feature = "enhanced-determinism")]
-pub type FxHashMap32<K, V> = indexmap::IndexMap<K, V, std::hash::BuildHasherDefault<FxHasher32>>;
-#[cfg(feature = "enhanced-determinism")]
-pub use {self::FxHashMap32 as HashMap, indexmap::map::Entry};
-#[cfg(not(feature = "enhanced-determinism"))]
-pub use {rustc_hash::FxHashMap as HashMap, std::collections::hash_map::Entry};
-
-const K: u32 = 0x9e3779b9;
-
-// Same as FxHasher, but with the guarantee that the internal hash is
-// an u32 instead of something that depends on the platform.
-pub struct FxHasher32 {
-    hash: u32,
-}
-
-impl Default for FxHasher32 {
-    #[inline]
-    fn default() -> FxHasher32 {
-        FxHasher32 { hash: 0 }
-    }
-}
-
-impl FxHasher32 {
-    #[inline]
-    fn add_to_hash(&mut self, i: u32) {
-        use std::ops::BitXor;
-        self.hash = self.hash.rotate_left(5).bitxor(i).wrapping_mul(K);
-    }
-}
-
-impl std::hash::Hasher for FxHasher32 {
-    #[inline]
-    fn write(&mut self, mut bytes: &[u8]) {
-        use std::convert::TryInto;
-        let read_u32 = |bytes: &[u8]| u32::from_ne_bytes(bytes[..4].try_into().unwrap());
-        let mut hash = FxHasher32 { hash: self.hash };
-        assert!(std::mem::size_of::<u32>() <= 8);
-        while bytes.len() >= std::mem::size_of::<u32>() {
-            hash.add_to_hash(read_u32(bytes) as u32);
-            bytes = &bytes[std::mem::size_of::<u32>()..];
-        }
-        if (std::mem::size_of::<u32>() > 4) && (bytes.len() >= 4) {
-            hash.add_to_hash(u32::from_ne_bytes(bytes[..4].try_into().unwrap()) as u32);
-            bytes = &bytes[4..];
-        }
-        if (std::mem::size_of::<u32>() > 2) && bytes.len() >= 2 {
-            hash.add_to_hash(u16::from_ne_bytes(bytes[..2].try_into().unwrap()) as u32);
-            bytes = &bytes[2..];
-        }
-        if (std::mem::size_of::<u32>() > 1) && bytes.len() >= 1 {
-            hash.add_to_hash(bytes[0] as u32);
-        }
-        self.hash = hash.hash;
-    }
-
-    #[inline]
-    fn write_u8(&mut self, i: u8) {
-        self.add_to_hash(i as u32);
-    }
-
-    #[inline]
-    fn write_u16(&mut self, i: u16) {
-        self.add_to_hash(i as u32);
-    }
-
-    #[inline]
-    fn write_u32(&mut self, i: u32) {
-        self.add_to_hash(i as u32);
-    }
-
-    #[inline]
-    fn write_u64(&mut self, i: u64) {
-        self.add_to_hash(i as u32);
-        self.add_to_hash((i >> 32) as u32);
-    }
-
-    #[inline]
-    fn write_usize(&mut self, i: usize) {
-        self.add_to_hash(i as u32);
-    }
-
-    #[inline]
-    fn finish(&self) -> u64 {
-        self.hash as u64
-    }
-}
diff --git a/src/data/maybe_serializable_data.rs b/src/data/maybe_serializable_data.rs
deleted file mode 100644
index 8b14e1a..0000000
--- a/src/data/maybe_serializable_data.rs
+++ /dev/null
@@ -1,17 +0,0 @@
-use downcast_rs::{impl_downcast, DowncastSync};
-#[cfg(feature = "serde-serialize")]
-use erased_serde::Serialize;
-
-/// Piece of data that may be serializable.
-pub trait MaybeSerializableData: DowncastSync {
-    /// Convert this shape as a serializable entity.
-    #[cfg(feature = "serde-serialize")]
-    fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
-        None
-    }
-
-    /// Clones `self`.
-    fn clone_dyn(&self) -> Box<dyn MaybeSerializableData>;
-}
-
-impl_downcast!(sync MaybeSerializableData);
diff --git a/src/data/mod.rs b/src/data/mod.rs
index 672bf94..eb0a229 100644
--- a/src/data/mod.rs
+++ b/src/data/mod.rs
@@ -1,11 +1,9 @@
 //! Data structures modified with guaranteed deterministic behavior after deserialization.
 
 pub use self::coarena::Coarena;
-pub use self::maybe_serializable_data::MaybeSerializableData;
+pub use eagl::utils::MaybeSerializableData;
 
 pub mod arena;
 mod coarena;
 pub(crate) mod graph;
-pub(crate) mod hashmap;
-mod maybe_serializable_data;
 pub mod pubsub;
diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs
index 28f149a..76e9de2 100644
--- a/src/dynamics/mod.rs
+++ b/src/dynamics/mod.rs
@@ -9,7 +9,7 @@ pub use self::joint::{
 };
 pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
 pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
-pub use buckler::shape::MassProperties;
+pub use eagl::shape::MassProperties;
 // #[cfg(not(feature = "parallel"))]
 pub(crate) use self::joint::JointGraphEdge;
 pub(crate) use self::rigid_body::RigidBodyChanges;
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
index 79c69c6..ce42da8 100644
--- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
@@ -13,7 +13,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::{Cholesky, Matrix6, Vector6, U3};
 #[cfg(feature = "dim2")]
 use {
-    crate::utils::SdpMatrix3,
+    eagl::utils::SdpMatrix3,
     na::{Matrix3, Vector3},
 };
 
diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
index 49cfc7a..a2c7c2c 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
@@ -8,7 +8,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 #[cfg(feature = "dim2")]
 use {
-    crate::utils::SdpMatrix2,
+    eagl::utils::SdpMatrix2,
     na::{Matrix2, Vector2},
 };
 
diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
index c05c08e..86e0c78 100644
--- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
+++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
@@ -12,7 +12,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 #[cfg(feature = "dim2")]
 use {
-    crate::utils::SdpMatrix2,
+    eagl::utils::SdpMatrix2,
     na::{Matrix2, Vector2},
 };
 
diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs
index 56c05df..4242d77 100644
--- a/src/geometry/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap.rs
@@ -1,10 +1,10 @@
-use crate::data::hashmap::HashMap;
 use crate::data::pubsub::Subscription;
 use crate::dynamics::RigidBodySet;
 use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider};
 use crate::math::{Point, Vector, DIM};
 use bit_vec::BitVec;
-use buckler::bounding_volume::{BoundingVolume, AABB};
+use eagl::bounding_volume::{BoundingVolume, AABB};
+use eagl::utils::hashmap::HashMap;
 use std::cmp::Ordering;
 use std::ops::{Index, IndexMut};
 
@@ -477,8 +477,8 @@ pub struct BroadPhase {
     #[cfg_attr(
         feature = "serde-serialize",
         serde(
-            serialize_with = "crate::data::hashmap::serialize_hashmap_capacity",
-            deserialize_with = "crate::data::hashmap::deserialize_hashmap_capacity"
+            serialize_with = "eagl::utils::hashmap::serialize_hashmap_capacity",
+            deserialize_with = "eagl::utils::hashmap::deserialize_hashmap_capacity"
         )
     )]
     reporting: HashMap<(u32, u32), bool>, // Workspace
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 1275358..db418a3 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,13 +1,13 @@
-use crate::buckler::shape::HalfSpace;
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
+use crate::eagl::shape::HalfSpace;
 use crate::geometry::InteractionGroups;
 use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
-use buckler::bounding_volume::AABB;
-use buckler::shape::{
+use eagl::bounding_volume::AABB;
+use eagl::shape::{
     Ball, Capsule, Cuboid, HeightField, Segment, Shape, ShapeType, TriMesh, Triangle,
 };
 #[cfg(feature = "dim3")]
-use buckler::shape::{Cone, Cylinder, RoundCylinder};
+use eagl::shape::{Cone, Cylinder, RoundCylinder};
 use na::Point3;
 use std::ops::Deref;
 use std::sync::Arc;
diff --git a/src/geometry/contact_generator/ball_ball_contact_generator.rs b/src/geometry/contact_generator/ball_ball_contact_generator.rs
deleted file mode 100644
index f2ca7af..0000000
--- a/src/geometry/contact_generator/ball_ball_contact_generator.rs
+++ /dev/null
@@ -1,103 +0,0 @@
-use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
-use crate::geometry::{Contact, ContactManifoldData, KinematicsCategory};
-use crate::math::{Point, Vector};
-#[cfg(feature = "simd-is-enabled")]
-use {
-    crate::geometry::contact_generator::PrimitiveContactGenerationContextSimd,
-    crate::geometry::{WBall, WContact},
-    crate::math::{Isometry, SimdReal, SIMD_WIDTH},
-    simba::simd::SimdValue,
-};
-
-#[cfg(feature = "simd-is-enabled")]
-fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry<SimdReal>) -> WContact {
-    let dcenter = ball2.center - ball1.center;
-    let center_dist = dcenter.magnitude();
-    let normal = dcenter / center_dist;
-
-    WContact {
-        local_p1: ball1.center + normal * ball1.radius,
-        local_p2: pos21 * (ball2.center - normal * ball2.radius),
-        local_n1: normal,
-        local_n2: pos21 * -normal,
-        fid1: [0; SIMD_WIDTH],
-        fid2: [0; SIMD_WIDTH],
-        dist: center_dist - ball1.radius - ball2.radius,
-    }
-}
-
-#[cfg(feature = "simd-is-enabled")]
-pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationContextSimd) {
-    let pos_ba = ctxt.positions2.inverse() * ctxt.positions1;
-    let radii_a =
-        SimdReal::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
-    let radii_b =
-        SimdReal::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
-
-    let wball_a = WBall::new(Point::origin(), radii_a);
-    let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b);
-    let contacts = generate_contacts_simd(&wball_a, &wball_b, &pos_ba);
-
-    for (i, manifold) in ctxt.manifolds.iter_mut().enumerate() {
-        // FIXME: compare the dist before extracting the contact.
-        let (contact, local_n1, local_n2) = contacts.extract(i);
-        if contact.dist <= ctxt.prediction_distance {
-            if manifold.points.len() != 0 {
-                manifold.points[0].copy_geometry_from(contact);
-            } else {
-                manifold.points.push(contact);
-            }
-
-            manifold.local_n1 = local_n1;
-            manifold.local_n2 = local_n2;
-            manifold.kinematics.category = KinematicsCategory::PointPoint;
-            manifold.kinematics.radius1 = radii_a.extract(i);
-            manifold.kinematics.radius2 = radii_b.extract(i);
-            ContactManifoldData::update_warmstart_multiplier(manifold);
-        } else {
-            manifold.points.clear();
-        }
-
-        manifold.sort_contacts(ctxt.prediction_distance);
-    }
-}
-
-pub fn generate_contacts_ball_ball(ctxt: &mut PrimitiveContactGenerationContext) {
-    let pos_ba = ctxt.position2.inverse() * ctxt.position1;
-    let radius_a = ctxt.shape1.as_ball().unwrap().radius;
-    let radius_b = ctxt.shape2.as_ball().unwrap().radius;
-
-    let dcenter = pos_ba.inverse_transform_point(&Point::origin()).coords;
-    let center_dist = dcenter.magnitude();
-    let dist = center_dist - radius_a - radius_b;
-
-    if dist < ctxt.prediction_distance {
-        let local_n1 = if center_dist != 0.0 {
-            dcenter / center_dist
-        } else {
-            Vector::y()
-        };
-
-        let local_n2 = pos_ba.inverse_transform_vector(&-local_n1);
-        let local_p1 = local_n1 * radius_a;
-        let local_p2 = local_n2 * radius_b;
-        let contact = Contact::new(local_p1.into(), local_p2.into(), 0, 0, dist);
-
-        if ctxt.manifold.points.len() != 0 {
-            ctxt.manifold.points[0].copy_geometry_from(contact);
-        } else {
-            ctxt.manifold.points.push(contact);
-        }
-
-        ctxt.manifold.local_n1 = local_n1;
-        ctxt.manifold.local_n2 = local_n2;
-        ctxt.manifold.kinematics.category = KinematicsCategory::PointPoint;
-        ctxt.manifold.kinematics.radius1 = radius_a;
-        ctxt.manifold.kinematics.radius2 = radius_b;
-        ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
-    } else {
-        ctxt.manifold.points.clear();
-    }
-
-    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
-}
diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs
deleted file mode 100644
index 88f1912..0000000
--- a/src/geometry/contact_generator/ball_convex_contact_generator.rs
+++ /dev/null
@@ -1,70 +0,0 @@
-use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
-use crate::geometry::{Ball, Contact, ContactManifoldData, KinematicsCategory};
-use crate::math::Isometry;
-use buckler::query::PointQuery;
-use na::Unit;
-
-pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) {
-    if let Some(ball1) = ctxt.shape1.as_ball() {
-        ctxt.manifold.swap_identifiers();
-        do_generate_contacts(ctxt.shape2, ball1, ctxt, true);
-    } else if let Some(ball2) = ctxt.shape2.as_ball() {
-        do_generate_contacts(ctxt.shape1, ball2, ctxt, false);
-    }
-
-    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
-}
-
-fn do_generate_contacts<P: ?Sized + PointQuery>(
-    point_query1: &P,
-    ball2: &Ball,
-    ctxt: &mut PrimitiveContactGenerationContext,
-    swapped: bool,
-) {
-    let position1;
-    let position2;
-
-    if swapped {
-        position1 = ctxt.position2;
-        position2 = ctxt.position1;
-    } else {
-        position1 = ctxt.position1;
-        position2 = ctxt.position2;
-    }
-
-    let local_p2_1 = position1.inverse_transform_point(&position2.translation.vector.into());
-    let proj = point_query1.project_local_point(&local_p2_1, cfg!(feature = "dim3"));
-    let dpos = local_p2_1 - proj.local_point;
-
-    #[allow(unused_mut)] // Because `mut local_n1, mut dist` is needed in 2D but not in 3D.
-    if let Some((mut local_n1, mut dist)) = Unit::try_new_and_get(dpos, 0.0) {
-        #[cfg(feature = "dim2")]
-        if proj.is_inside {
-            local_n1 = -local_n1;
-            dist = -dist;
-        }
-
-        if dist <= ball2.radius + ctxt.prediction_distance {
-            let local_n2 = position2.inverse_transform_vector(&(position1 * -*local_n1));
-            let local_p2 = (local_n2 * ball2.radius).into();
-            let contact_point = Contact::new(proj.local_point, local_p2, 0, 0, dist - ball2.radius);
-
-            if ctxt.manifold.points.len() != 1 {
-                ctxt.manifold.points.clear();
-                ctxt.manifold.points.push(contact_point);
-            } else {
-                // Copy only the geometry so we keep the warmstart impulses.
-                ctxt.manifold.points[0].copy_geometry_from(contact_point);
-            }
-
-            ctxt.manifold.local_n1 = *local_n1;
-            ctxt.manifold.local_n2 = local_n2;
-            ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint;
-            ctxt.manifold.kinematics.radius1 = 0.0;
-            ctxt.manifold.kinematics.radius2 = ball2.radius;
-            ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
-        } else {
-            ctxt.manifold.points.clear();
-        }
-    }
-}
diff --git a/src/geometry/contact_generator/ball_polygon_contact_generator.rs b/src/geometry/contact_generator/ball_polygon_contact_generator.rs
deleted file mode 100644
index 8b13789..0000000
--- a/src/geometry/contact_generator/ball_polygon_contact_generator.rs
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs
deleted file mode 100644
index 9090b36..0000000
--- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs
+++ /dev/null
@@ -1,202 +0,0 @@
-use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
-use crate::geometry::{Capsule, Contact, ContactManifold, ContactManifoldData, KinematicsCategory};
-use crate::math::Isometry;
-use crate::math::Vector;
-use approx::AbsDiffEq;
-#[cfg(feature = "dim2")]
-use buckler::shape::SegmentPointLocation;
-use na::Unit;
-
-pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) {
-    if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) {
-        generate_contacts(
-            ctxt.prediction_distance,
-            capsule1,
-            ctxt.position1,
-            capsule2,
-            ctxt.position2,
-            ctxt.manifold,
-        );
-    }
-
-    ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
-    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
-}
-
-#[cfg(feature = "dim2")]
-pub fn generate_contacts<'a>(
-    prediction_distance: f32,
-    capsule1: &'a Capsule,
-    pos1: &'a Isometry<f32>,
-    capsule2: &'a Capsule,
-    pos2: &'a Isometry<f32>,
-    manifold: &mut ContactManifold,
-) {
-    // FIXME: the contact kinematics is not correctly set here.
-    // We use the common "Point-Plane" kinematics with zero radius everytime.
-    // Instead we should select point/point ore point-plane (with non-zero
-    // radius for the point) depending on the features involved in the contact.
-    let pos12 = pos1.inverse() * pos2;
-    let pos21 = pos12.inverse();
-
-    let seg1 = capsule1.segment;
-    let seg2_1 = capsule2.segment.transformed(&pos12);
-    let (loc1, loc2) = buckler::query::details::closest_points_segment_segment_with_locations_nD(
-        (&seg1.a, &seg1.b),
-        (&seg2_1.a, &seg2_1.b),
-    );
-
-    // We do this clone to perform contact tracking and transfer impulses.
-    // FIXME: find a more efficient way of doing this.
-    let old_manifold_points = manifold.points.clone();
-    manifold.points.clear();
-
-    let swapped = false;
-
-    let fid1 = if let SegmentPointLocation::OnVertex(v1) = loc1 {
-        v1 as u8 * 2
-    } else {
-        1
-    };
-    let fid2 = if let SegmentPointLocation::OnVertex(v2) = loc2 {
-        v2 as u8 * 2
-    } else {
-        1
-    };
-
-    let bcoords1 = loc1.barycentric_coordinates();
-    let bcoords2 = loc2.barycentric_coordinates();
-    let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1];
-    let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1];
-
-    let local_n1 =
-        Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis());
-    let dist = (local_p2 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius;
-
-    if dist <= prediction_distance {
-        let local_n2 = pos21 * -local_n1;
-        let contact = Contact::new(local_p1, pos21 * local_p2, fid1, fid2, dist);
-        manifold.points.push(contact);
-
-        manifold.local_n1 = *local_n1;
-        manifold.local_n2 = *local_n2;
-        manifold.kinematics.category = KinematicsCategory::PlanePoint;
-        manifold.kinematics.radius1 = 0.0;
-        manifold.kinematics.radius2 = 0.0;
-    } else {
-        // No contact within tolerance.
-        return;
-    }
-
-    if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2_1.direction()) {
-        if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8
-            && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8
-        {
-            // Capsules axes are almost parallel and are almost perpendicular to the normal.
-            // Find a second contact point.
-            if let Some((clip_a, clip_b)) =
-                buckler::query::details::clip_segment_segment_with_normal(
-                    (seg1.a, seg1.b),
-                    (seg2_1.a, seg2_1.b),
-                    *local_n1,
-                )
-            {
-                let contact =
-                    if (clip_a.0 - local_p1).norm_squared() > f32::default_epsilon() * 100.0 {
-                        // Use clip_a as the second contact.
-                        Contact::new(
-                            clip_a.0,
-                            pos21 * clip_a.1,
-                            clip_a.2 as u8,
-                            clip_a.3 as u8,
-                            (clip_a.1 - clip_a.0).dot(&local_n1),
-                        )
-                    } else {
-                        // Use clip_b as the second contact.
-                        Contact::new(
-                            clip_b.0,
-                            pos21 * clip_b.1,
-                            clip_b.2 as u8,
-                            clip_b.3 as u8,
-                            (clip_b.1 - clip_b.0).dot(&local_n1),
-                        )
-                    };