diff --git a/hoomd-geometry/src/shape/convex_polytope.rs b/hoomd-geometry/src/shape/convex_polytope.rs index 7db4a0e38..cc7f9de06 100644 --- a/hoomd-geometry/src/shape/convex_polytope.rs +++ b/hoomd-geometry/src/shape/convex_polytope.rs @@ -3,6 +3,8 @@ //! N-Dimensional generalization of a convex polyhedron. +use std::f64::consts::SQRT_2; + use serde::{Deserialize, Serialize}; use crate::{BoundingSphereRadius, Error, SupportMapping}; @@ -205,6 +207,174 @@ impl ConvexPolytope .try_into() .expect("convex polytope should have a positive bounding radius") } + + /// Build the N-dimensional generalization of an octahedron with unit edge length. + /// + /// This shape, also referred to as the hyperoctahedron or cross polytope, is + /// defined by the set of coordinates `{0...±√2/2...0}` for all combinations of + /// `±√2/2` and `N-1` zeros. + /// + /// # Example + /// ``` + /// use approxim::assert_relative_eq; + /// use hoomd_geometry::{ + /// BoundingSphereRadius, Volume, + /// shape::{ConvexPolyhedron, ConvexPolytope}, + /// }; + /// + /// # fn main() -> Result<(), hoomd_geometry::Error> { + /// let diamond = ConvexPolytope::<2>::orthoplex(); + /// assert_relative_eq!(diamond.volume(), 1.0); + /// + /// let octahedron = ConvexPolyhedron::orthoplex(); + /// assert_relative_eq!( + /// octahedron.bounding_sphere_radius().get(), + /// f64::sqrt(2.0) + /// ); + /// + /// // Cross polytopes always have 2*N vertices + /// let octachoron = ConvexPolytope::<4, 8>::orthoplex(); + /// assert_eq!(octachoron.vertices().len(), 8); + /// # Ok(()) + /// # } + /// ``` + /// + /// # Panics + /// If `N=0` or `N > MAX_VERTICES/2`. + #[inline] + #[must_use] + pub fn orthoplex() -> Self { + assert!( + N != 0, + "An orthoplex is not well-defined in zero dimensions!" + ); + let sqrt_2_halves = SQRT_2 / 2.0; + let bounding_radius = SQRT_2.try_into().expect("Hard-coded value"); + let mut vertices = ArrayVec::<_, MAX_VERTICES>::new(); + + for nonzero_index in 0..N { + let coord = Cartesian::::from(std::array::from_fn(|i| { + f64::from(i == nonzero_index) * sqrt_2_halves + })); + vertices.push(coord); + vertices.push(-coord); + } + Self { + vertices, + bounding_radius, + } + } + + /// Build the N-dimensional generalization of a tetrahedron with unit edge length. + /// + /// # Example + /// ``` + /// use approxim::assert_relative_eq; + /// use hoomd_geometry::{ + /// BoundingSphereRadius, Volume, + /// shape::{ConvexPolygon, ConvexPolyhedron, ConvexPolytope}, + /// }; + /// + /// # fn main() -> Result<(), hoomd_geometry::Error> { + /// let triangle = ConvexPolygon::simplex(); + /// assert_relative_eq!(triangle.volume(), f64::sqrt(3.0) / 4.0); + /// + /// let tetrahedron = ConvexPolyhedron::simplex(); + /// assert_relative_eq!( + /// tetrahedron.bounding_sphere_radius().get(), + /// f64::sqrt(3.0 / 8.0) + /// ); + /// + /// // Simplices always have N+1 vertices + /// let pentachoron = ConvexPolytope::<4, 5>::simplex(); + /// assert_eq!(pentachoron.vertices().len(), 5); + /// # Ok(()) + /// # } + /// ``` + /// + /// # Panics + /// If `N+1 > MAX_VERTICES`. + #[inline] + #[must_use] + pub fn simplex() -> Self { + // https://en.wikipedia.org/wiki/Simplex#Cartesian_coordinates_for_a_regular_n-dimensional_simplex_in_Rn + let inv_sqrt2 = SQRT_2.recip(); + + let mut vertices = ArrayVec::<_, MAX_VERTICES>::new(); + + // Un-centered: N vertices at scaled standard basis positions + 1 shared vertex + for k in 0..N { + vertices.push(std::array::from_fn(|i| f64::from(i == k) * inv_sqrt2).into()); + } + + let c = (1.0 - f64::sqrt(N as f64 + 1.0)) / (f64::sqrt(2.0) * N as f64); + vertices.push([c; N].into()); + + // Center by subtracting centroid + let center = Cartesian::from([(inv_sqrt2 + c) / (N as f64 + 1.0); N]); + vertices.iter_mut().for_each(|vertex| *vertex -= center); + + Self { + vertices, + bounding_radius: f64::sqrt(N as f64 / (2.0 * (N as f64 + 1.0))) + .try_into() + .expect("sqrt of positive is positive"), + } + } + + /// Build the N-dimensional generalization of a cube with unit edge length. + /// + /// This shape, also referred to as the hypercube or orthotope, is defined by + /// the signed, length-N combinations of `{±0.5}`. + /// + /// # Example + /// ``` + /// use approxim::assert_relative_eq; + /// use hoomd_geometry::{ + /// BoundingSphereRadius, Volume, + /// shape::{ConvexPolygon, ConvexPolyhedron, ConvexPolytope}, + /// }; + /// + /// # fn main() -> Result<(), hoomd_geometry::Error> { + /// let square = ConvexPolygon::hypercube(); + /// assert_relative_eq!(square.volume(), 1.0); + /// + /// let cube = ConvexPolyhedron::hypercube(); + /// assert_relative_eq!(cube.bounding_sphere_radius().get(), f64::sqrt(3.0)); + /// + /// // Hypercubes have 2^N vertices + /// let hypercube = ConvexPolytope::<4, 16>::hypercube(); + /// assert_eq!(hypercube.vertices().len(), 16); + /// # Ok(()) + /// # } + /// ``` + /// + /// # Panics + /// If `N=0` or `2^N > MAX_VERTICES`. + #[inline] + #[must_use] + pub fn hypercube() -> Self { + #[inline] + fn signed_combinations_of_one_half() -> Vec> { + (0..(1usize << N)) + .map(|bits| { + std::array::from_fn(|i| if bits & (1 << i) == 0 { 0.5 } else { -0.5 }).into() + }) + .collect() + } + assert!( + N != 0, + "Cross polytope is not well-defined in zero dimensions!" + ); + let bounding_radius = f64::sqrt(N as f64) + .try_into() + .expect("sqrt(positive) is positive."); + + Self { + vertices: ArrayVec::<_, MAX_VERTICES>::from_iter(signed_combinations_of_one_half::()), + bounding_radius, + } + } } impl SupportMapping> diff --git a/hoomd-geometry/src/xenocollide.rs b/hoomd-geometry/src/xenocollide.rs index e41460b0b..dd0d9b85d 100644 --- a/hoomd-geometry/src/xenocollide.rs +++ b/hoomd-geometry/src/xenocollide.rs @@ -34,13 +34,484 @@ use crate::SupportMapping; use hoomd_vector::{Cartesian, Cross, InnerProduct, Rotate, RotationMatrix}; -/// Maximum allowed iterations for Xenocollide in 2D -const XENOCOLLIDE_2D_MAX_ITER: usize = 1024; -/// Maximum allowed iterations for Xenocollide in 3D -const XENOCOLLIDE_3D_MAX_ITER: usize = 1024; +/// Maximum allowed iterations for Xenocollide portal refinement. +const XENOCOLLIDE_MAX_ITER: usize = 1024; + +/// Result of portal discovery. +pub(crate) enum Discovery { + /// Collision result already determined during discovery. + Known(bool), + /// Portal discovered — proceed to refinement. + Found([Cartesian; N]), +} + +/// Dimension-specific operations for Minkowski Portal Refinement. +/// +/// This trait encapsulates the operations that differ between MPR in various dimensions +/// - Tolerance for convergence check +/// - Portal discovery (can be done directly in 2D, requires search in 3D and up) +/// - Portal vertex replacement logic +pub(crate) trait MinkowskiPortalRefinement { + /// Dimension-specific convergence tolerance. + const TOLERANCE: f64; + + /// Compute the outward-facing normal to the portal (facing toward the surface of the minkowski difference) + fn outward_normal(portal: &[Cartesian; N], interior: &Cartesian) -> Cartesian; + + /// Discover the initial portal for MPR refinement. + /// + /// The initial portal will be an (N-1)-simplex, through which the ray `-v0` must + /// pass on its way to the origin. + fn discover_portal(s: &MinkowskiDifference, v0: &Cartesian) -> Discovery + where + A: SupportMapping>, + B: SupportMapping>; + + /// Check whether the portal has reached the surface of the Minkowski + /// difference within numerical precision (in which case we can determine overlap). + /// + /// Returns `Some(result)` if convergence is detected, or `None` if + /// refinement can continue. + fn tolerance_check( + portal: &[Cartesian; N], + v_new: &Cartesian, + normal: &Cartesian, + ) -> Option; + + /// Narrow the portal by replacing one vertex with a new support point. + /// + /// The portal vertices and `v_new` form an N-simplex whose interior face + /// is the current portal. The origin ray enters through this face and must + /// exit through one of the outer faces. This method identifies which outer + /// face the ray exits through and replaces the portal vertex opposite that + /// face with `v_new`, ensuring that the origin ray always passes through the portal + fn narrow_portal(interior: &Cartesian, portal: &mut [Cartesian; N], v_new: Cartesian); +} + +impl MinkowskiPortalRefinement<2> for Cartesian<2> { + const TOLERANCE: f64 = 1e-16; + + #[inline] + fn outward_normal(portal: &[Cartesian<2>; 2], interior: &Cartesian<2>) -> Cartesian<2> { + let mut n = (portal[1] - portal[0]).perpendicular(); + if (portal[0] - *interior).dot(&n) < 0.0 { + n = -n; + } + n + } + + #[inline] + fn discover_portal>, B: SupportMapping>>( + s: &MinkowskiDifference<2, A, B>, + v0: &Cartesian<2>, + ) -> Discovery<2> { + // Find the support point in the direction of the origin ray + let v1 = s.composite_support_mapping(-*v0); + + // v_perp is on the same side as the origin if v1.dot(v_perp) < 0 + let mut v_perp_v1v0 = (v1 - *v0).perpendicular(); + if v1.dot(&v_perp_v1v0) > 0.0 { + v_perp_v1v0 = -v_perp_v1v0; + } + + // Support point perpendicular to plane containing the origin, v0, and v1 + let v2 = s.composite_support_mapping(v_perp_v1v0); + + // NOTE: this assumes the origin is within the shape. This assumption matches + // HOOMD-Blue, but is important to note regardless. + + Discovery::Found([v1, v2]) + } + + #[inline] + fn tolerance_check( + portal: &[Cartesian<2>; 2], + v_new: &Cartesian<2>, + _normal: &Cartesian<2>, + ) -> Option { + // In 2D, we either find a valid vertex or require further search to be sure + let d = (*v_new - portal[0]) - (*v_new - portal[0]).project(&(portal[1] - portal[0])); + if d.norm_squared() < Self::TOLERANCE * v_new.norm_squared() { + return Some(true); + } + None + } + + #[inline] + fn narrow_portal(interior: &Cartesian<2>, portal: &mut [Cartesian<2>; 2], v_new: Cartesian<2>) { + let mut v_perp = (v_new - *interior).perpendicular(); + // Orient toward portal[0] + if (portal[0] - v_new).dot(&v_perp) < 0.0 { + v_perp = -v_perp; + } + if v_new.dot(&v_perp) < 0.0 { + // Origin is on the portal[0] side — replace portal[1] + portal[1] = v_new; + } else { + // Origin is on the portal[1] side — replace portal[0] + portal[0] = v_new; + } + } +} + +impl MinkowskiPortalRefinement<3> for Cartesian<3> { + const TOLERANCE: f64 = 2e-12; + + #[inline] + fn outward_normal(portal: &[Cartesian<3>; 3], interior: &Cartesian<3>) -> Cartesian<3> { + let e1 = portal[1] - portal[0]; + let e2 = portal[2] - portal[0]; + let mut n = e1.cross(&e2); + if (portal[0] - *interior).dot(&n) < 0.0 { + n = -n; + } + n + } + + #[inline] + fn discover_portal>, B: SupportMapping>>( + s: &MinkowskiDifference<3, A, B>, + v0: &Cartesian<3>, + ) -> Discovery<3> { + // Interior point at origin implies overlap + if v0.into_iter().all(|x| x.abs() < Self::TOLERANCE) { + return Discovery::Known(true); + } + // Support point in the direction of the origin ray + let mut v1 = s.composite_support_mapping(-*v0); + + // Equivalent to v1 . (v1-v0) <= 0 by convexity + if v1.dot(v0) > 0.0 { + return Discovery::Known(false); + } + + // Direction perpendicular to v0, v1 plane + let n = v1.cross(v0); + + // Cross product is zero if v0,v1 collinear with origin, but we have already + // determined the origin is within the v1 support plane. + // If the origin is on a line between v1 and v0, particles overlap. + if n.into_iter().all(|x| x.abs() < Self::TOLERANCE) { + return Discovery::Known(true); + } + + // Support point perpendicular to plane containing the origin, v0, and v1 + let mut v2 = s.composite_support_mapping(n); + + if v2.dot(&n) < 0.0 { + return Discovery::Known(false); + } + + // Support point perpendicular to plane containing interior point and first 2 supports + let mut n = (v1 - *v0).cross(&(v2 - *v0)); + // Maintain known handedness of the portal + if n.dot(v0) > 0.0 { + (v1, v2) = (v2, v1); + n = -n; + } + + // while origin_ray_does_not_intersect_candidate() + let mut count = 0_usize; + let v3 = loop { + count += 1; + + if count >= XENOCOLLIDE_MAX_ITER { + return Discovery::Known(true); + } + + let v3 = s.composite_support_mapping(n); + if v3.dot(&n) <= 0.0 { + return Discovery::Known(false); + } + + // If origin lies on the opposite side of the plane from our third support + // point, use the outer facing plane normal. + // Check the v3, v0, v1 plane for validity + if v1.cross(&v3).dot(v0) < 0.0 { + v2 = v3; // Preserve handedness + n = (v1 - *v0).cross(&(v2 - *v0)); + continue; + } + if v3.cross(&v2).dot(v0) < 0.0 { + v1 = v3; // Preserve handedness + n = (v1 - *v0).cross(&(v2 - *v0)); + continue; + } + break v3; + }; + + Discovery::Found([v1, v2, v3]) + } + + #[inline] + fn tolerance_check( + portal: &[Cartesian<3>; 3], + v_new: &Cartesian<3>, + normal: &Cartesian<3>, + ) -> Option { + let tolerance = Self::TOLERANCE * normal.norm(); // Handle non-unit shapes + + // Check if v_new is on the portal plane: if so, no more refinement is possible + let d = (*v_new - portal[0]).dot(normal); + if d.abs() < tolerance { + return Some(false); + } + // Check if origin is on the portal plane: if so, intersection detected + let d = portal[0].dot(normal); + if d.abs() < tolerance { + return Some(true); + } + None + } + + #[inline] + fn narrow_portal(interior: &Cartesian<3>, portal: &mut [Cartesian<3>; 3], v_new: Cartesian<3>) { + let [v1, v2, v3] = *portal; + // Test origin against the three planes that separate the new portal candidates + // using the triple product identities as an optimization: + // (v1 % v4) * v0 == v1 * (v4 % v0) > 0 if origin inside (v1, v4, v0) + // (v2 % v4) * v0 == v2 * (v4 % v0) > 0 if origin inside (v2, v4, v0) + // (v3 % v4) * v0 == v3 * (v4 % v0) > 0 if origin inside (v3, v4, v0) + let v_perp = v_new.cross(interior); + + #[expect( + clippy::match_same_arms, + reason = "Clearly illustrate translation from c." + )] + match ( + v_perp.dot(&v1) > 0.0, + v_perp.dot(&v2) > 0.0, + v_perp.dot(&v3) > 0.0, + ) { + (true, true, _) => portal[0] = v_new, // Inside v1 && inside v2 => eliminate v1 + (true, false, _) => portal[2] = v_new, // Inside v1 && OUTside v2 => eliminate v3 + (false, _, true) => portal[1] = v_new, // OUTside v1 && inside v3 => eliminate v2 + (false, _, false) => portal[0] = v_new, // OUTside v1 && OUTside v3 => eliminate v1 + } + } +} + +/// Find a vector perpendicular to the plane spanned by `a` and `b` in R^4. +/// +/// `counary_cross` requires 3 inputs but during early portal discovery we only have +/// 2 vectors. This function uses a standard basis vector as the 3rd input — at least +/// one of the 4 basis vectors is guaranteed to be non-coplanar with any 2 given vectors. +#[inline] +fn perp_to_2d_subspace(a: Cartesian<4>, b: Cartesian<4>, tol: f64) -> Option> { + for i in 0..4 { + let mut e = [0.0f64; 4]; + e[i] = 1.0; + let n = Cartesian::<4>::counary_cross(&[a, b, e.into()]); + if n.into_iter().any(|x| x.abs() > tol) { + return Some(n); + } + } + None +} + +impl MinkowskiPortalRefinement<4> for Cartesian<4> { + const TOLERANCE: f64 = 2e-12; + + #[inline] + fn outward_normal(portal: &[Cartesian<4>; 4], interior: &Cartesian<4>) -> Cartesian<4> { + let e1 = portal[1] - portal[0]; + let e2 = portal[2] - portal[0]; + let e3 = portal[3] - portal[0]; + let mut n = Self::counary_cross(&[e1, e2, e3]); + if (portal[0] - *interior).dot(&n) < 0.0 { + n = -n; + } + n + } + + fn discover_portal>, B: SupportMapping>>( + s: &MinkowskiDifference<4, A, B>, + v0: &Cartesian<4>, + ) -> Discovery<4> { + // Interior point at origin implies overlap + if v0.into_iter().all(|x| x.abs() < Self::TOLERANCE) { + return Discovery::Known(true); + } + + // v1: first support point + let mut v1 = s.composite_support_mapping(-*v0); + if v1.dot(v0) > 0.0 { + return Discovery::Known(false); + } + + // v2: support in direction perpendicular to the v0-v1 plane + let Some(n) = perp_to_2d_subspace(*v0, v1, Self::TOLERANCE) else { + return Discovery::Known(true); // v0, v1, origin collinear + }; + + let mut v2 = s.composite_support_mapping(n); + if v2.dot(&n) < 0.0 { + return Discovery::Known(false); + } + + // v3: support in direction perpendicular to the v0-v1-v2 subspace + let Some(mut n) = perp_to_2d_subspace(v1 - *v0, v2 - *v0, Self::TOLERANCE) else { + return Discovery::Known(true); + }; + // Orient normal away from v0 (maintain handedness) + if n.dot(v0) > 0.0 { + (v1, v2) = (v2, v1); + n = -n; + } + let v3 = s.composite_support_mapping(n); + if v3.dot(&n) <= 0.0 { + return Discovery::Known(false); + } + + // Recompute portal normal from 3 edge vectors + let mut n = Self::counary_cross(&[v1 - *v0, v2 - *v0, v3 - *v0]); + // Coplanar v1, v2, v3 with v0 produce a zero normal — origin lies on a flat + // boundary region which we treat as overlap + if n.into_iter().all(|x| x.abs() < Self::TOLERANCE) { + return Discovery::Known(true); + } + if n.dot(v0) > 0.0 { + n = -n; + } + + // v4-validation loop + // The 3 face checks use the 4D quadruple product: + // counary_cross(&[v_a, v_b, v4]).dot(&v0) = det([v_a, v_b, v4, v0]) + // Negative means origin is on the wrong side of that face. + let mut v3 = v3; + let mut count = 0_usize; + let v4 = loop { + count += 1; + if count >= XENOCOLLIDE_MAX_ITER { + return Discovery::Known(true); + } + + let v4 = s.composite_support_mapping(n); + if v4.dot(&n) <= 0.0 { + return Discovery::Known(false); + } + + // Each face test is a 4×4 determinant. For axis-aligned shapes, the + // support vertices are frequently *coplanar* with `v0`, so the determinant + // lands within a few ULP of zero. `< 0.0` then fires on floating-point + // noise, replacing a vertex and bouncing the portal between states until + // `MAX_ITER` bails out with a false-positive overlap. + // + // A determinant within `det_band` of zero means the origin lies ON the + // face (within precision), which we treat as enclosed. Only a clearly- + // negative determinant triggers a vertex replacement. + let det_tol = Self::TOLERANCE * v1.norm() * v2.norm() * v3.norm() * v0.norm(); + + // Face (v1, v2, v4) — opposite v3 + if Self::counary_cross(&[v1, v2, v4]).dot(v0) < -det_tol { + v3 = v4; + n = Self::counary_cross(&[v1 - *v0, v2 - *v0, v3 - *v0]); + if n.dot(v0) > 0.0 { + n = -n; + } + continue; + } + // Face (v2, v3, v4) — opposite v1 + if Self::counary_cross(&[v2, v3, v4]).dot(v0) < -det_tol { + v1 = v4; + n = Self::counary_cross(&[v1 - *v0, v2 - *v0, v3 - *v0]); + if n.dot(v0) > 0.0 { + n = -n; + } + continue; + } + // Face (v1, v4, v3) — opposite v2 + if Self::counary_cross(&[v1, v4, v3]).dot(v0) < -det_tol { + v2 = v4; + n = Self::counary_cross(&[v1 - *v0, v2 - *v0, v3 - *v0]); + if n.dot(v0) > 0.0 { + n = -n; + } + continue; + } + break v4; + }; + + Discovery::Found([v1, v2, v3, v4]) + } + + #[inline] + fn tolerance_check( + portal: &[Cartesian<4>; 4], + v_new: &Cartesian<4>, + normal: &Cartesian<4>, + ) -> Option { + let tolerance = Self::TOLERANCE * normal.norm(); + + let d = (*v_new - portal[0]).dot(normal); + if d.abs() < tolerance { + return Some(false); + } + let d = portal[0].dot(normal); + if d.abs() < tolerance { + return Some(true); + } + None + } + #[inline] + fn narrow_portal(interior: &Cartesian<4>, portal: &mut [Cartesian<4>; 4], v_new: Cartesian<4>) { + // The 4-simplex [portal[0..4] , v_new] has 5 tetrahedral faces. + // The entry face is the current portal (portal[0..4]). + // The 4 exit faces each contain v_new and all portal vertices except portal[i]. + // + // The origin ray enters through the portal and must exit through one of these + // 4 faces. To determine which, we compute the outward-facing normal to each exit + // face and check whether the origin lies on the outward side: + // + // n_i = inward normal of the face opposite portal[i] + // + // The signed distance from a point P to the hyperplane through v_new with normal n + // is (P − v_new) . n. The origin (P = 0) has distance −v_new . n, and interior + // has distance (interior − v_new) . n. We orient n toward portal[i] so that the + // origin is on the opposite (outward) side when v_new . n > 0. + // Among faces the ray crosses (d > 0 and num > 0), we pick the one with smallest + // t = num / (num + d) and fall back to the face with largest d if none qualify. + // + // NOTE: This is fundementally different from 3d, where the cross product + // uniquely identifies the correct facet (plane, in that case) to exit through. + // In 4D, we need to find the *best* facet + let mut exit_face: Option<(usize, f64)> = None; + let mut fallback_i = 0; + let mut fallback_d = f64::NEG_INFINITY; + + for i in 0..4 { + let edges = std::array::from_fn(|k| portal[(i + k + 1) % 4] - v_new); + let n = Self::counary_cross(&edges); + // Skip near-degenerate faces. + if n.norm_squared() < Self::TOLERANCE * Self::TOLERANCE * interior.norm_squared() { + continue; + } + let n = if (portal[i] - v_new).dot(&n) < 0.0 { + -n + } else { + n + }; + let d = v_new.dot(&n); + let num = (*interior - v_new).dot(&n); + + if d > fallback_d { + fallback_d = d; + fallback_i = i; + } + if d > 0.0 && num > 0.0 { + let t = num / (num + d); + match exit_face { + Some((_, best_t)) if t >= best_t => {} + _ => exit_face = Some((i, t)), + } + } + } + portal[exit_face.map_or(fallback_i, |(i, _)| i)] = v_new; + } +} /// Stateful type that efficiently computes repeated Minkowski differences. -struct MinkowskiDifference< +pub(crate) struct MinkowskiDifference< 'a, const N: usize, A: SupportMapping>, @@ -99,263 +570,117 @@ impl<'a, const N: usize, A: SupportMapping>, B: SupportMapping>, B: SupportMapping>>( +pub(crate) fn collide( sa: &A, sb: &B, - v_ij: &Cartesian<2>, + v_ij: &Cartesian, q_ij: &R, ) -> bool where - RotationMatrix<2>: From, + A: SupportMapping>, + B: SupportMapping>, + R: Copy, + RotationMatrix: From, + Cartesian: MinkowskiPortalRefinement, { - const TOLERANCE: f64 = 1e-16; let s = MinkowskiDifference::new(sa, sb, v_ij, *q_ij); + let v0 = *v_ij; - // Phase 1: Portal discovery - // Obtain a point lying deep within B⊖A - let v0 = *v_ij; // self.centroid()-other.centroid() in extrinsic coords - - // TODO: This is unsafe for types like `ConvexPolytope`. Users can construct - // such types where the origin is outside the shape. Option 1: Validate - // the vertices when constructing `ConvexPolytope` Option 2: Implement - // a `SomePointInside` trait that must always return a point inside the - // shape. For `ConvexPolytope` this could be the mean of the vertices. - // `ConvexPolytope` makes vertices private, so this point could be - // precomputed and result in no performance impact to xenocollide. - - // Find the support point in the direction of the origin ray - let mut v1 = s.composite_support_mapping(-v0); // negative, to ensure ||v1|| > 0 - - // v_perp is on the same side as the origin if v1.dot(v_perp) < 0 - let mut v_perp_v1v0 = (v1 - v0).perpendicular(); - if v1.dot(&v_perp_v1v0) > 0.0 { - v_perp_v1v0 = -v_perp_v1v0; - } - - // Support point perpendicular to plane containing the origin, v0, and v1 - let mut v2 = s.composite_support_mapping(v_perp_v1v0); - - // 2. Portal Refinement - // Now we have three points which form our portal + // Portal discovery + let mut portal = match Cartesian::::discover_portal(&s, &v0) { + Discovery::Found(p) => p, + Discovery::Known(r) => return r, + }; + // Portal refinement + // The loop is the same in general dimension, but the outward facing normal function + // depends on the (N-1)-ary cross product (perp in 2d, cross in 3d) + // See https://ncatlab.org/nlab/show/cross+product#counary for further details on + // this operation let mut count = 0_usize; loop { count += 1; - // Vector normal to the portal segment, facing away from the interior point - let mut v_perp_v2v1 = (v2 - v1).perpendicular(); - if (v1 - v0).dot(&v_perp_v2v1) < 0.0 { - v_perp_v2v1 = -v_perp_v2v1; + let normal = Cartesian::::outward_normal(&portal, &v0); + + // If the portal's (N-1)-simplex is degenerate, the normal is near zero and the + // rest of the algorithm does not make sense (so we fail safe -> overlap) + if normal.norm_squared() + < Cartesian::::TOLERANCE * Cartesian::::TOLERANCE * v0.norm_squared() + { + return true; } - // Check if origin is inside or overlapping the initial portal - if v1.dot(&v_perp_v2v1) >= 0.0 { - // FUTURE: `v1.dot(&v_perp_v2v1) / v_perp_v2v1.norm()` is the approximate overlap distance + // Hit test: is the origin enclosed by the portal? + if portal[0].dot(&normal) >= 0.0 { return true; } - // Support point in the direction of the portal - let v3 = s.composite_support_mapping(v_perp_v2v1); + // Support query in the direction of the portal normal + let v_new = s.composite_support_mapping(normal); - // If the origin is outside the support plane, return false (no overlap) - if v3.dot(&v_perp_v2v1) < 0.0 { + // Miss test: is the origin outside the support plane? + if v_new.dot(&normal) < 0.0 { return false; } - // are we within an epsilon of the surface of the shape? If yes, done (overlap) - let d = (v3 - v1) - (v3 - v1).project(&(v2 - v1)); - if d.norm_squared() < TOLERANCE * v3.norm_squared() { - // FUTURE: `d.norm()` is the approximate overlap distance - return true; + // Can we numerically distinguish the portal face and the support plane? + if let Some(result) = Cartesian::::tolerance_check(&portal, &v_new, &normal) { + return result; } - // Choose new portal, which may either be v3v2 or v1v3 - let mut v_perp_v3v0 = (v3 - v0).perpendicular(); - // make v_perp_v3v0 point toward v1 - if (v1 - v3).dot(&v_perp_v3v0) < 0.0 { - v_perp_v3v0 = -v_perp_v3v0; - } - if v3.dot(&v_perp_v3v0) < 0.0 { - // Origin is on the v1 side, so new portal is v3v1 - v2 = v3; - } else { - // Origin is on the v2 side, so new portal is v3v2 - v1 = v3; - } + // Face test and vertex replacement (dimension-specific) TODO + Cartesian::::narrow_portal(&v0, &mut portal, v_new); - if count >= XENOCOLLIDE_2D_MAX_ITER { - // FUTURE: `TOLERANCE` is the approximate overlap distance + if count >= XENOCOLLIDE_MAX_ITER { return true; } } } -/// Detect collision between two convex 3D objects via Minkowski Portal Refinement. -#[inline(never)] -pub fn collide3d( +/// Detect collision between two convex 2D objects via Minkowski Portal Refinement. +#[inline] +pub fn collide2d>, B: SupportMapping>>( sa: &A, sb: &B, - v_ij: &Cartesian<3>, // Probably ok to take ownership? + v_ij: &Cartesian<2>, q_ij: &R, ) -> bool +where + RotationMatrix<2>: From, +{ + collide::<2, R, A, B>(sa, sb, v_ij, q_ij) +} + +/// Detect collision between two convex 3D objects via Minkowski Portal Refinement. +#[inline(never)] +pub fn collide3d(sa: &A, sb: &B, v_ij: &Cartesian<3>, q_ij: &R) -> bool where A: SupportMapping>, B: SupportMapping>, R: Copy, RotationMatrix<3>: From, { - const TOLERANCE: f64 = 2e-12; - - if v_ij.into_iter().all(|x| x.abs() < TOLERANCE) { - // Interior point is at the origin => shapes overlap - return true; - } - - let s = MinkowskiDifference::new(sa, sb, v_ij, *q_ij); - - // Phase 1: Portal discovery - // Obtain a point lying deep within B⊖A - let v0 = *v_ij; // self.centroid()-other.centroid() in extrinsic coords - - // find_candidate_portal() - - // Support point in the direction of the origin ray - let mut v1 = s.composite_support_mapping(-v0); // negative, to ensure ||v1|| > 0 - - // Equivalent to v1 . (v1-v0) <= 0 by convexity - if v1.dot(&v0) > 0.0 { - return false; // Origin is outside the v1 support plane - } - - // Direction perpendicular to v0, v1 plane - let n = v1.cross(&v0); - - // Cross product is zero if v0,v1 collinear with origin, but we have already - // determined origin is within v1 support plane. If origin is on a line between - // v1 and v0, particles overlap. - if n.into_iter().all(|x| x.abs() < TOLERANCE) { - return true; - } - - // Support point perpendicular to plane containing the origin, v0, and v1 - let mut v2 = s.composite_support_mapping(n); - - if v2.dot(&n) < 0.0 { - return false; // Origin lies outside the v2 support plane - } - - // Support point perpendicular to plane containing interior point and first 2 supports - let mut n = (v1 - v0).cross(&(v2 - v0)); - // Maintain known handedness of the portal - if n.dot(&v0) > 0.0 { - (v1, v2) = (v2, v1); - n = -n; - } - - // while origin_ray_does_not_intersect_candidate() - let mut count = 0_usize; - let mut v3 = loop { - count += 1; - - if count >= XENOCOLLIDE_3D_MAX_ITER { - return true; - } - - let v3 = s.composite_support_mapping(n); - if v3.dot(&n) <= 0.0 { - return false; // Origin is outside the v3 support plane - } - - // If origin lies on the opposite side of the plane from our third support - // point, use the outer facing plane normal. - // Check the v3, v0, v1 plane for validity - if v1.cross(&v3).dot(&v0) < 0.0 { - v2 = v3; // Preserve handedness - n = (v1 - v0).cross(&(v2 - v0)); - continue; // Continue iterating to find a valid portal - } - if v3.cross(&v2).dot(&v0) < 0.0 { - v1 = v3; // Preserve handedness - n = (v1 - v0).cross(&(v2 - v0)); - continue; - } - break v3; // If we've made it this far, we've found a valid portal - }; - - count = 0; - loop { - count += 1; - - // Outer-facing normal of the current portal - n = (v2 - v1).cross(&(v3 - v1)); - - // Check if origin is inside (or overlapping) the portal - if n.dot(&v1) >= 0.0 { - // We already know that the origin lies within 3 of the faces of our portal - // simplex. If it lies within the final face, it lies within B⊖A - return true; - } - - // Support point in direction of outer-facing normal of portal - // This point helps us determine how far outside the portal the origin lies - let v4 = s.composite_support_mapping(n); - - // If the origin is outside the support plane, it cannot lie inside B⊖A - if n.dot(&v4) < 0.0 { - return false; - } - - // Are we within an epsilon of the surface of the shape? If yes, done, one way or another. - n = (v2 - v1).cross(&(v3 - v1)); - let mut d = (v4 - v1).dot(&n); - - // Scale the tolerance with the size of the shapes. - let tolerance = TOLERANCE * n.norm(); - - // First, check if v4 is on plane (v2, v1, v3) - if d.abs() < tolerance { - // No more refinement possible, but not intersection detected - return false; - } - // Second, check if origin is on plane (v2, v1, v3) and has been missed by other checks - d = v1.dot(&n); - if d.abs() < tolerance { - return true; - } - - // Choose a new portal. Two of its edges will be from the planes (v4,v0,v1), - // (v4,v0,v2), (v4,v0,v3). Find which two have the origin on the same side. - - /* Test origin against the three planes that separate the new portal candidates - Note: We're taking advantage of the triple product identities here - as an optimization - (v1 % v4) * v0 == v1 * (v4 % v0) > 0 if origin inside (v1, v4, v0) - (v2 % v4) * v0 == v2 * (v4 % v0) > 0 if origin inside (v2, v4, v0) - (v3 % v4) * v0 == v3 * (v4 % v0) > 0 if origin inside (v3, v4, v0) - */ - let v_perp_v4v0 = v4.cross(&v0); + collide::<3, R, A, B>(sa, sb, v_ij, q_ij) +} - // Compiles to the same code as the original if-else, despite the extra dot - #[expect( - clippy::match_same_arms, - reason = "Clearly illustrate translation from c." - )] - match ( - v_perp_v4v0.dot(&v1) > 0.0, - v_perp_v4v0.dot(&v2) > 0.0, - v_perp_v4v0.dot(&v3) > 0.0, - ) { - (true, true, _) => v1 = v4, // Inside v1 && inside v2 => eliminate v1 - (true, false, _) => v3 = v4, // Inside v1 && OUTside v2 => eliminate v3 - (false, _, true) => v2 = v4, // OUTside v1 && inside v3 => eliminate v2 - (false, _, false) => v1 = v4, // OUTside v1 && OUTside v3 => eliminate v1 - } - if count >= XENOCOLLIDE_3D_MAX_ITER { - return true; - } - } +/// Detect collision between two convex 4D objects via Minkowski Portal Refinement. +#[inline(never)] +pub fn collide4d(sa: &A, sb: &B, v_ij: &Cartesian<4>, q_ij: &R) -> bool +where + A: SupportMapping>, + B: SupportMapping>, + R: Copy, + RotationMatrix<4>: From, +{ + collide::<4, R, A, B>(sa, sb, v_ij, q_ij) } #[cfg(test)] @@ -364,9 +689,10 @@ mod tests { use crate::IntersectsAt; use rstest::*; - use crate::shape::{Circle, Hypercuboid, Hypersphere}; + use crate::shape::{Circle, ConvexPolytope, Hypercuboid, Hypersphere}; use hoomd_utility::valid::PositiveReal; use hoomd_vector::{Angle, Rotation, Versor}; + use rand::{RngExt, SeedableRng, rngs::StdRng}; #[rstest( v => [[0.1, 0.1], [999.9, 0.0], [0.0, 5.123_f64.next_down()], [0.0, 5.123_000_001]], @@ -449,4 +775,316 @@ mod tests { let overlaps = collide3d(&c0, &c1, &v.into(), &theta); assert_eq!(overlaps, c0.intersects_aligned(&c1, &v.into())); } + + #[rstest( + v => [ + [0.1, 0.1, 0.1, 0.1], + [999.9, 0.0, 0.0, -10.9], + [0.0, 5.123, 0.0, 0.0], + [0.0, 0.0, 0.0, 5.123_000_001], + ], + radius => [0.001, 1.0, 4.123], + )] + fn test_4d_spheres_collide(v: [f64; 4], radius: f64) { + let (s0, s1) = ( + Hypersphere { + radius: 1.0.try_into().expect("test value is a positive real"), + }, + Hypersphere::<4> { + radius: radius.try_into().expect("test value is a positive real"), + }, + ); + let q_ij = RotationMatrix::<4>::default(); + + let overlaps = collide4d(&s0, &s1, &v.into(), &q_ij); + + assert_eq!( + overlaps, + s0.intersects_at(&s1, &Cartesian::from(v), &q_ij), + "4D Xenocollide result did not match standard implementation!" + ); + } + + #[rstest( + v => [ + [0.1, 0.1, 0.1, 0.1], + [999.9, 0.0, 0.0, 0.05], + [0.0, 5.123, 0.0, 0.0], + [0.0, 0.0, 0.0, 5.123_000_000_001], + ], + tesseract => [ + [1.0.try_into().expect("test value is a positive real"); 4], + [999.0.try_into().expect("test value is a positive real"), 0.1.try_into().expect("test value is a positive real"), 0.5.try_into().expect("test value is a positive real"), 1.0.try_into().expect("test value is a positive real")], + ], + )] + fn test_tesseracts_collide(v: [f64; 4], tesseract: [PositiveReal; 4]) { + let c0 = Hypercuboid { + edge_lengths: tesseract, + }; + let c1 = Hypercuboid { + edge_lengths: [1.0.try_into().expect("test value is a positive real"); 4], + }; + let q_ij = RotationMatrix::<4>::default(); + + let overlaps = collide4d(&c0, &c1, &v.into(), &q_ij); + assert_eq!(overlaps, c0.intersects_aligned(&c1, &v.into())); + } + + /// Sweep two unit hypercubes from overlapping to separated along a diagonal direction, + /// verifying collide4d matches the analytical result at every step. + #[rstest( + direction => [ + [1.0_f64, 1.0, 1.0, 1.0], + [2.0_f64, 1.0, 1.0, 1.0], + [1.0_f64, 1.0, 1.0, 3.0], + [1.0_f64, 2.0, 3.0, 1.0], + ], + )] + fn test_4d_hypercuboid_diagonal_separation_sweep(direction: [f64; 4]) { + let one: PositiveReal = 1.0.try_into().unwrap(); + let c0 = Hypercuboid { + edge_lengths: [one; 4], + }; + let c1 = Hypercuboid { + edge_lengths: [one; 4], + }; + + // Half-edge-lengths: 0.5 each, sum = 1.0 each. + // Critical t = min_i(1.0 / |dir_i|) + let critical_t = (0..4) + .map(|i| 1.0 / direction[i].abs()) + .reduce(f64::min) + .unwrap(); + + let t_start = critical_t - 0.001; + let t_end = critical_t + 0.001; + let steps = 10_000; + let dt = (t_end - t_start) / f64::from(steps); + + let q_ij = RotationMatrix::<4>::default(); + + for step in 0..=steps { + let t = t_start + dt * f64::from(step); + let d: Cartesian<4> = direction.map(|d_i| d_i * t).into(); + let expected = c0.intersects_aligned(&c1, &d); + let result = collide4d(&c0, &c1, &d, &q_ij); + assert_eq!( + result, expected, + "Mismatch at step {step}, t = {t:.12}, critical_t = {critical_t:.12}" + ); + } + } + + #[rstest(seed => [0_usize, 1, 2, 7, 42, 2024])] + fn test_4d_hypercuboid_random_near_boundary(seed: usize) { + let one: PositiveReal = 1.0.try_into().unwrap(); + let c0 = Hypercuboid { + edge_lengths: [one; 4], + }; + let c1 = Hypercuboid { + edge_lengths: [one; 4], + }; + let q_ij = RotationMatrix::<4>::default(); + + let mut rng = StdRng::seed_from_u64(seed as u64); + let shell = 1e-9_f64; + let samples = 100_000_usize; + for _ in 0..samples { + // Uniform direction in [-1, 1]^4 (scaling is absorbed by `t0`). + let dir: Cartesian<4> = rng.random(); + let max_abs = dir.into_iter().map(f64::abs).fold(0.0_f64, f64::max); + if max_abs < f64::EPSILON { + continue; + } + let t0 = 1.0 / max_abs; + let t = t0 + (rng.random::() * 2.0 - 1.0) * shell; + let d = dir * t; + let expected = c0.intersects_aligned(&c1, &d); + let result = collide4d(&c0, &c1, &d, &q_ij); + assert_eq!( + result, expected, + "near-boundary mismatch: dir = {dir:?}, t = {t}, d = {d:?}" + ); + } + } + + /// Two unit hypercubes are separated by `d = [2t, t, t, t]`; the analytical boundary + /// is `t = 0.5` (`|d_0| = 1`). Just past it (e.g. `t = 0.5000158`) the + /// shapes do not overlap, but a coplanar portal previously drove MPR into a cycle + /// that falesely reported overlap. + #[test] + fn test_4d_hypercuboid_diagonal_2111_near_boundary() { + let one: PositiveReal = 1.0.try_into().unwrap(); + let c0 = Hypercuboid { + edge_lengths: [one; 4], + }; + let c1 = Hypercuboid { + edge_lengths: [one; 4], + }; + let q_ij = RotationMatrix::<4>::default(); + + for t in [ + 0.5_f64, + 0.5 + 5e-6, + 0.5 + 1.58e-5, // Exact failing case from previous code + 0.5 + 3e-5, + 0.5 + 1e-4, + 0.5 - 1e-4, + ] { + let d: Cartesian<4> = [2.0 * t, t, t, t].into(); + let expected = c0.intersects_aligned(&c1, &d); + let result = collide4d(&c0, &c1, &d, &q_ij); + assert_eq!(result, expected, "t = {t}, d = {d:?}"); + } + } + + /// Sweep two tesseracts (vertex-based `ConvexPolytope::hypercube()`) along + /// axis-aligned directions, crossing the Minkowski-difference boundary at a cubical + /// facet *center*. + #[rstest( + direction => [ + [1.0_f64, 0.0, 0.0, 0.0], + [0.0_f64, 1.0, 0.0, 0.0], + [0.0_f64, 0.0, 1.0, 0.0], + [0.0_f64, 0.0, 0.0, 1.0], + [1.0_f64, 1.0, 0.0, 0.0], + [0.0_f64, 1.0, 1.0, 0.0], + [0.0_f64, 0.0, 1.0, 1.0], + [2.0_f64, 1.0, 0.0, 0.0], + ], + )] + fn test_4d_hypercube_polytope_axis_separation_sweep(direction: [f64; 4]) { + let c0 = ConvexPolytope::<4, 16>::hypercube(); + let c1 = ConvexPolytope::<4, 16>::hypercube(); + + // Half-edge 0.5 each; collide4d reports overlap (incl. touching) iff |d[i]| <= 1.0 + // for all i. Critical t = min_i(1.0 / |dir_i|); zero components contribute infinity. + let critical_t = (0..4) + .map(|i| 1.0 / direction[i].abs()) + .reduce(f64::min) + .unwrap(); + + let t_start = critical_t - 0.001; + let t_end = critical_t + 0.001; + let steps = 10_000; + let dt = (t_end - t_start) / f64::from(steps); + + let q_ij = RotationMatrix::<4>::default(); + + for step in 0..=steps { + let t = t_start + dt * f64::from(step); + let d: Cartesian<4> = direction.map(|d_i| d_i * t).into(); + let expected = (0..4).all(|i| d[i].abs() <= 1.0); + let result = collide4d(&c0, &c1, &d, &q_ij); + assert_eq!( + result, expected, + "Mismatch at step {step}, t = {t:.12}, critical_t = {critical_t:.12}" + ); + } + } + + /// Stress-test collide4d with two hyperspheres near their overlap boundary. + /// + /// Generates random displacement directions uniformly on S^3 using random + /// unit quaternions ([`Versor`], Muller/Marsaglia Method 19) and random + /// radii in a thin shell of width 1e-3 centered on the overlap radius. + #[rstest( + r0 => [1.0, 0.5, 3.7], + r1 => [1.0, 2.0, 0.8], + seed => [0, 1, 42], + )] + fn test_4d_hypersphere_shell_overlap(r0: f64, r1: f64, seed: u64) { + let boundary = r0 + r1; + let shell_half_width = 5e-10; + let n_samples = 10_000; + + let s0 = Hypersphere::<4> { + radius: r0.try_into().unwrap(), + }; + let s1 = Hypersphere::<4> { + radius: r1.try_into().unwrap(), + }; + let q_ij = RotationMatrix::<4>::default(); + + let mut rng = StdRng::seed_from_u64(seed); + + for i in 0..n_samples { + // Random unit quaternion → uniform direction on S^3 + let q: Versor = rng.random(); + let q = q.get(); + let dir: Cartesian<4> = [q.scalar, q.vector[0], q.vector[1], q.vector[2]].into(); + + let r = boundary - shell_half_width + rng.random::() * 2.0 * shell_half_width; + let d = dir * r; + + let analytical = r < boundary; + let result = collide4d(&s0, &s1, &d, &q_ij); + assert_eq!( + result, analytical, + "Mismatch at sample {i}, |d| = {r:.12}, boundary = {boundary:.12}" + ); + } + } + + /// Construct a 4-simplex from 5 vertices drawn from c·{±1}⁴. + /// + /// 9 of the 10 edges have length 2c√2; the v₃–v₄ edge has length 4c. + fn pentachoron(c: f64) -> ConvexPolytope<4, 8> { + ConvexPolytope::<4, 8>::with_vertices([ + Cartesian::from([c, c, c, c]), + Cartesian::from([c, -c, -c, c]), + Cartesian::from([-c, c, -c, c]), + Cartesian::from([-c, -c, c, c]), + Cartesian::from([c, c, -c, -c]), + ]) + .unwrap() + } + + /// Sweep two simplices along vertex-to-vertex (edge) directions. + /// + /// For two identical simplices with vertices {`v_i`}, a sweep along the + /// edge direction `d0 = v_a − v_b` has an exact analytical boundary: + /// the t* at intersection equals the edge length `|v_a − v_b|` (9 edges have + /// length 2c*sqrt(2), 1 edge has length 4c). + #[rstest( + edge => [ + [0.0_f64, 2.0, 2.0, 0.0], // v0 - v1 + [2.0_f64, 0.0, 2.0, 0.0], // v0 - v2 + [2.0_f64, 2.0, 0.0, 0.0], // v0 - v3 + [0.0_f64, 0.0, 2.0, 2.0], // v0 - v4 + [2.0_f64, -2.0, 0.0, 0.0], // v1 - v2 + [2.0_f64, 0.0, -2.0, 0.0], // v1 - v3 + [0.0_f64, -2.0, 0.0, 2.0], // v1 - v4 + [0.0_f64, 2.0, -2.0, 0.0], // v2 - v3 + [-2.0_f64, 0.0, 0.0, 2.0], // v2 - v4 + [-2.0_f64, -2.0, 2.0, 2.0], // v3 - v4 + ], + c => [1.0, 0.5, 2.0], + )] + fn test_4d_pentachoron_edge_sweep(edge: [f64; 4], c: f64) { + let s0 = pentachoron(c); + let s1 = pentachoron(c); + + let edge: Cartesian<4> = edge.map(|e| e * c).into(); + let edge_length = edge.norm(); + let direction = edge / edge_length; + + let t_start = edge_length - 1.0 / 3.0; + let t_end = edge_length + 0.01; + let steps = 10_000; + let dt = (t_end - t_start) / f64::from(steps); + + let q_ij = RotationMatrix::<4>::default(); + + for step in 0..=steps { + let t = t_start + dt * f64::from(step); + let d = direction * t; + let analytical = t < edge_length; + let result = collide4d(&s0, &s1, &d, &q_ij); + assert_eq!( + result, analytical, + "Mismatch at step {step}, t = {t:.12}, edge_length = {edge_length:.12}" + ); + } + } } diff --git a/hoomd-vector/src/cartesian.rs b/hoomd-vector/src/cartesian.rs index e3349d9f9..09d8464b9 100644 --- a/hoomd-vector/src/cartesian.rs +++ b/hoomd-vector/src/cartesian.rs @@ -18,7 +18,10 @@ use rand::{ }; use crate::{Cross, Error, InnerProduct, Metric, Rotate, Unit, Vector}; -use hoomd_linear_algebra::{MatMul, matrix::Matrix}; +use hoomd_linear_algebra::{ + MatMul, + matrix::{Matrix, Matrix33}, +}; /// A [`Vector`] represented by `N` `f64` coordinates. ///