diff --git a/src/s2/edge_crossings/crosser.rs b/src/s2/edge_crossings/crosser.rs new file mode 100644 index 0000000..0f3aff7 --- /dev/null +++ b/src/s2/edge_crossings/crosser.rs @@ -0,0 +1,459 @@ +//! This module defines [`EdgeCrosser`], which allows edges to be efficiently tested +//! for intersection with a given fixed edge `ab`. It is especially efficient when testing +//! for intersection with an edge chain connecting vertices `v₀`, `v₁`, `v₂`, ... + +use crate::consts::*; +use super::{ + Crossing, SignedCrossing, robust_cross_prod, signed_vertex_crossing, vertex_crossing, +}; +use crate::s2::point::Point; +use crate::s2::predicates::{Direction, robust_sign, triage_sign}; + +fn invert_dir(d: Direction) -> Direction { + match d { + Direction::CounterClockwise => Direction::Clockwise, + Direction::Clockwise => Direction::CounterClockwise, + Direction::Indeterminate => Direction::Indeterminate, + } +} + +/// `EdgeCrosser` allows edges to be efficiently tested for intersection with a +/// given fixed edge `ab`. It is especially efficient when testing for +/// intersection with an edge chain connecting vertices `v₀`, `v₁`, `v₂`, ... +/// +/// # Examples +/// +/// Testing against independent, disjoint edges: +/// ```rust +/// # use s2::point::Point; +/// # use s2::edge_crossings::{EdgeCrosser, Crossing}; +/// fn count_intersections(a: &Point, b: &Point, edges: &[(Point, Point)]) -> usize { +/// let mut crosser = EdgeCrosser::new(a, b); +/// edges.iter() +/// .filter(|e| crosser.crossing_sign(&e.0, &e.1) >= Crossing::MaybeCross) +/// .count() +/// } +/// ``` +/// +/// Testing against a connected edge chain to avoid redundant vertex evaluations: +/// ```rust +/// # use s2::point::Point; +/// # use s2::edge_crossings::{EdgeCrosser, Crossing}; +/// fn count_chain_intersections(a: &Point, b: &Point, chain: &[Point]) -> usize { +/// if chain.len() < 2 { return 0; } +/// +/// let mut count = 0; +/// let mut crosser = EdgeCrosser::new_with_c(a, b, &chain[0]); +/// for vertex in &chain[1..] { +/// if crosser.chain_crossing_sign(vertex) >= Crossing::MaybeCross { +/// count += 1; +/// } +/// } +/// count +/// } +/// ``` +#[derive(Copy, Clone)] +pub struct EdgeCrosser { + a: Point, + b: Point, + have_tangents: bool, + a_tangent: Point, + b_tangent: Point, + c: Point, + acb: Direction, + bda: Direction, +} + +impl EdgeCrosser { + /// Creates an `EdgeCrosser` with the fixed edge `ab`. + pub fn new(a: &Point, b: &Point) -> Self { + debug_assert!(a.0.is_unit()); + debug_assert!(b.0.is_unit()); + EdgeCrosser { + a: *a, + b: *b, + have_tangents: false, + a_tangent: Point::default(), + b_tangent: Point::default(), + c: Point::default(), + acb: Direction::Indeterminate, + bda: Direction::Indeterminate, + } + } + + /// Creates an `EdgeCrosser` with the fixed edge `ab` and the first vertex `c`. + pub fn new_with_c(a: &Point, b: &Point, c: &Point) -> Self { + let mut crosser = Self::new(a, b); + crosser.restart_at(c); + crosser + } + + /// Sets the current vertex of the chain to `c`. + pub fn restart_at(&mut self, c: &Point) { + debug_assert!(c.0.is_unit()); + self.c = *c; + self.acb = invert_dir(triage_sign(&self.a, &self.b, &self.c)); + } + + /// Reports whether the edge `ab` intersects the edge `cd`. + /// Returns [`Crossing::Cross`] if `ab` crosses `cd` at a point that is interior to both edges. + /// Returns [`Crossing::MaybeCross`] if any two vertices from different edges are the same. + /// Returns [`Crossing::DoNotCross`] otherwise. + /// + /// Note that if an edge is degenerate (`a == b` or `c == d`), the return value + /// is [`Crossing::MaybeCross`] if two vertices from different edges are the same and [`Crossing::DoNotCross`] otherwise. + /// + /// Properties of this function: + /// + /// 1. `crossing_sign(b, a, c, d) == crossing_sign(a, b, c, d)` + /// 2. `crossing_sign(c, d, a, b) == crossing_sign(a, b, c, d)` + /// 3. `crossing_sign(a, b, c, d) == Crossing::MaybeCross` if `a == c`, `a == d`, `b == c`, `b == d` + /// 4. `crossing_sign(a, b, c, d) <= Crossing::MaybeCross` if `a == b` or `c == d` + /// + /// This function implements an exact, consistent perturbation model such + /// that no three points are ever considered to be collinear. This means + /// that even if you have 4 points `A, B, C, D` that lie exactly in a line + /// (say, around the equator), `C` and `D` will be treated as being slightly to + /// one side or the other of `AB`. This is done in a way such that the + /// results are always consistent with [`robust_sign`]. + /// + /// Note that if you want to check an edge against a chain of other edges, + /// it is slightly more efficient to use the [`EdgeCrosser::chain_crossing_sign`] method. + pub fn crossing_sign(&mut self, c: &Point, d: &Point) -> Crossing { + debug_assert!(c.0.is_unit()); + debug_assert!(d.0.is_unit()); + if c != &self.c { + self.restart_at(c); + } + self.chain_crossing_sign(d) + } + + // Note: In C++ S2Geometry, these single-argument chain methods are function overloads + // (e.g. CrossingSign(c, d) and CrossingSign(d)). Since Rust does not support method + // overloading, we prefix the chain-based equivalents with `chain_`. + + /// Behaves like [`EdgeCrosser::crossing_sign`] but uses the last vertex + /// passed to one of the crossing methods (or [`EdgeCrosser::restart_at`]) as the first vertex of the current edge. + pub fn chain_crossing_sign(&mut self, d: &Point) -> Crossing { + debug_assert!(d.0.is_unit()); + let bda = triage_sign(&self.a, &self.b, d); + if self.acb == invert_dir(bda) && bda != Direction::Indeterminate { + self.c = *d; + self.acb = invert_dir(bda); + return Crossing::DoNotCross; + } + self.bda = bda; + self.crossing_sign_internal(d) + } + + /// This method extends the concept of a "crossing" to the case where `ab` + /// and `cd` have a vertex in common. The two edges may or may not cross, + /// according to the rules defined in [`vertex_crossing`]. The rules + /// are designed so that point containment tests can be implemented simply + /// by counting edge crossings. Similarly, determining whether one edge + /// chain crosses another edge chain can be implemented by counting. + /// + /// Returns true if `crossing_sign(c, d)` is [`Crossing::Cross`], or `ab` and `cd` share a vertex + /// and `vertex_crossing(a, b, c, d)` returns true. + pub fn edge_or_vertex_crossing(&mut self, c: &Point, d: &Point) -> bool { + debug_assert!(c.0.is_unit()); + debug_assert!(d.0.is_unit()); + if c != &self.c { + self.restart_at(c); + } + self.chain_edge_or_vertex_crossing(d) + } + + pub fn chain_edge_or_vertex_crossing(&mut self, d: &Point) -> bool { + debug_assert!(d.0.is_unit()); + let c = self.c; // copy since chain_crossing_sign clobbers self.c + let crossing = self.chain_crossing_sign(d); + match crossing { + Crossing::DoNotCross => false, + Crossing::Cross => true, + Crossing::MaybeCross => vertex_crossing(&self.a, &self.b, &c, d), + } + } + + /// Like [`EdgeCrosser::edge_or_vertex_crossing`], but returns [`SignedCrossing::LeftToRight`] if `ab` crosses `cd` from left to + /// right, [`SignedCrossing::RightToLeft`] if `ab` crosses `cd` from right to left, and [`SignedCrossing::None`] otherwise. This + /// implies that if `cd` bounds some region according to the "interior is on + /// the left" rule, this function returns [`SignedCrossing::LeftToRight`] when `ab` exits the region and [`SignedCrossing::RightToLeft`] + /// when `ab` enters. + /// + /// This method allows computing the change in winding number from point `a` to + /// point `b` by summing the signed edge crossings of `ab` with the edges of the + /// loop(s) used to define the winding number. + pub fn signed_edge_or_vertex_crossing(&mut self, c: &Point, d: &Point) -> SignedCrossing { + debug_assert!(c.0.is_unit()); + debug_assert!(d.0.is_unit()); + if c != &self.c { + self.restart_at(c); + } + self.chain_signed_edge_or_vertex_crossing(d) + } + + pub fn chain_signed_edge_or_vertex_crossing(&mut self, d: &Point) -> SignedCrossing { + debug_assert!(d.0.is_unit()); + let c = self.c; + let crossing = self.chain_crossing_sign(d); + match crossing { + Crossing::DoNotCross => SignedCrossing::None, + Crossing::Cross => match self.last_interior_crossing_sign() { + Direction::CounterClockwise => SignedCrossing::RightToLeft, + Direction::Clockwise => SignedCrossing::LeftToRight, + Direction::Indeterminate => SignedCrossing::None, + }, + Crossing::MaybeCross => signed_vertex_crossing(&self.a, &self.b, &c, d), + } + } + + /// If the preceding call to [`EdgeCrosser::crossing_sign`] returned [`Crossing::Cross`], this method returns the direction + /// of the crossing. + pub fn last_interior_crossing_sign(&self) -> Direction { + self.acb + } + + fn crossing_sign_internal(&mut self, d: &Point) -> Crossing { + let result = self.crossing_sign_internal2(d); + self.c = *d; + self.acb = invert_dir(self.bda); + result + } + + fn crossing_sign_internal2(&mut self, d: &Point) -> Crossing { + if !self.have_tangents { + let norm = robust_cross_prod(&self.a, &self.b); + self.a_tangent = Point(self.a.0.cross(&norm.0)); + self.b_tangent = Point(norm.0.cross(&self.b.0)); + self.have_tangents = true; + } + + let error_bound = (1.5 + 1.0 / 3.0f64.sqrt()) * DBL_EPSILON; + if (self.c.0.dot(&self.a_tangent.0) > error_bound && d.0.dot(&self.a_tangent.0) > error_bound) + || (self.c.0.dot(&self.b_tangent.0) > error_bound && d.0.dot(&self.b_tangent.0) > error_bound) + { + return Crossing::DoNotCross; + } + + if self.a == self.c || self.a == *d || self.b == self.c || self.b == *d { + return Crossing::MaybeCross; + } + + if self.a == self.b || self.c == *d { + return Crossing::DoNotCross; + } + + if self.acb == Direction::Indeterminate { + self.acb = invert_dir(robust_sign(&self.a, &self.b, &self.c)); + } + if self.bda == Direction::Indeterminate { + self.bda = robust_sign(&self.a, &self.b, d); + } + if self.bda != self.acb { + return Crossing::DoNotCross; + } + + let cbd = invert_dir(robust_sign(&self.c, d, &self.b)); + if cbd != self.acb { + return Crossing::DoNotCross; + } + + let dac = robust_sign(&self.c, d, &self.a); + if dac != self.acb { + Crossing::DoNotCross + } else { + Crossing::Cross + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::r3::vector::Vector; + use crate::s2::point::Point; + + #[test] + #[should_panic] + fn test_invalid_default_points() { + let p = Point::default(); + let _crosser = EdgeCrosser::new(&p, &p); + } + + #[test] + #[should_panic] + fn test_invalid_nan_points() { + let nan = std::f64::NAN; + let p = Point(Vector::new(nan, nan, nan)); + let _crosser = EdgeCrosser::new(&p, &p); + } + + fn test_crossing( + a: &Point, + b: &Point, + c: &Point, + d: &Point, + crossing_sign: i32, + signed_crossing_sign: i32, + ) { + let mut crossing_sign = crossing_sign; + if a == c || a == d || b == c || b == d { + crossing_sign = 0; + } + + let expected_cross = match crossing_sign { + 1 => Crossing::Cross, + 0 => Crossing::MaybeCross, + -1 => Crossing::DoNotCross, + _ => unreachable!(), + }; + + let edge_or_vertex = signed_crossing_sign != 0; + + let expected_signed = match signed_crossing_sign { + 1 => SignedCrossing::RightToLeft, + 0 => SignedCrossing::None, + -1 => SignedCrossing::LeftToRight, + _ => unreachable!(), + }; + + assert_eq!( + crate::s2::edge_crossings::crossing_sign(a, b, c, d), + expected_cross + ); + if a == c || a == d || b == c || b == d { + assert_eq!( + crate::s2::edge_crossings::edge_or_vertex_crossing(a, b, c, d), + edge_or_vertex + ); + assert_eq!( + crate::s2::edge_crossings::signed_vertex_crossing(a, b, c, d) as i32, + expected_signed as i32 + ); + } + + let mut crosser = EdgeCrosser::new_with_c(a, b, c); + assert_eq!(crosser.chain_crossing_sign(d), expected_cross); + assert_eq!(crosser.chain_crossing_sign(c), expected_cross); + + if a == c || a == d || b == c || b == d { + crosser.restart_at(c); + assert_eq!(crosser.chain_edge_or_vertex_crossing(d), edge_or_vertex); + assert_eq!(crosser.chain_edge_or_vertex_crossing(c), edge_or_vertex); + + crosser.restart_at(c); + assert_eq!( + crosser.chain_signed_edge_or_vertex_crossing(d), + expected_signed + ); + assert_eq!( + crosser + .chain_signed_edge_or_vertex_crossing(c) + .winding_delta(), + -expected_signed.winding_delta() + ); + } + } + + fn test_crossings_permutations( + a: Point, + b: Point, + c: Point, + d: Point, + crossing_sign: i32, + signed_crossing_sign: i32, + ) { + let a = Point(a.0.normalize()); + let b = Point(b.0.normalize()); + let c = Point(c.0.normalize()); + let d = Point(d.0.normalize()); + test_crossing(&a, &b, &c, &d, crossing_sign, signed_crossing_sign); + test_crossing(&b, &a, &c, &d, crossing_sign, -signed_crossing_sign); + test_crossing(&a, &b, &d, &c, crossing_sign, -signed_crossing_sign); + test_crossing(&b, &a, &d, &c, crossing_sign, signed_crossing_sign); + test_crossing(&a, &a, &c, &d, -1, 0); + test_crossing(&a, &b, &c, &c, -1, 0); + test_crossing(&a, &a, &c, &c, -1, 0); + test_crossing(&a, &b, &a, &b, 0, 1); + if crossing_sign == 0 { + test_crossing(&c, &d, &a, &b, crossing_sign, 0); + } else { + test_crossing(&c, &d, &a, &b, crossing_sign, -signed_crossing_sign); + } + } + + #[test] + fn test_crossings() { + test_crossings_permutations( + Point(Vector::new(1.0, 2.0, 1.0)), + Point(Vector::new(1.0, -3.0, 0.5)), + Point(Vector::new(1.0, -0.5, -3.0)), + Point(Vector::new(0.1, 0.5, 3.0)), + 1, + 1, + ); + + test_crossings_permutations( + Point(Vector::new(1.0, 2.0, 1.0)), + Point(Vector::new(1.0, -3.0, 0.5)), + Point(Vector::new(-1.0, 0.5, 3.0)), + Point(Vector::new(-0.1, -0.5, -3.0)), + -1, + 0, + ); + + test_crossings_permutations( + Point(Vector::new(0.0, 0.0, -1.0)), + Point(Vector::new(0.0, 1.0, 0.0)), + Point(Vector::new(0.0, 0.0, 1.0)), + Point(Vector::new(0.0, 1.0, 1.0)), + -1, + 0, + ); + + test_crossings_permutations( + Point(Vector::new(1.0, 0.0, 0.0)), + Point(Vector::new(0.0, 1.0, 0.0)), + Point(Vector::new(1.0, -0.1, 1.0)), + Point(Vector::new(1.0, 1.0, -0.1)), + 1, + 1, + ); + + test_crossings_permutations( + Point(Vector::new(7.0, -2.0, 3.0)), + Point(Vector::new(2.0, 3.0, 4.0)), + Point(Vector::new(2.0, 3.0, 4.0)), + Point(Vector::new(-1.0, 2.0, 5.0)), + 0, + -1, + ); + + test_crossings_permutations( + Point(Vector::new(1.0, 1.0, 1.0)), + Point(Vector::new(1.0, 1.0 + std::f64::EPSILON, -1.0)), + Point(Vector::new(1.0, -1.0, 0.0)), + Point(Vector::new(1.0, 1.0, 0.0)), + -1, + 0, + ); + } + + #[test] + fn test_collinear_edges_that_dont_touch() { + let a = Point(Vector::new(1.0, 1.0, 1.0).normalize()); + let b = Point(Vector::new(1.0, -1.0, 1.0).normalize()); + let c = Point(Vector::new(-1.0, -1.0, 1.0).normalize()); + let d = Point(Vector::new(-1.0, 1.0, 1.0).normalize()); + let mut crosser = EdgeCrosser::new_with_c(&a, &b, &c); + assert_eq!(crosser.chain_crossing_sign(&d), Crossing::DoNotCross); + } + + #[test] + fn test_coincident_zero_length_edges_that_dont_touch() { + let a = Point(Vector::new(1.0, 1.0, 1.0).normalize()); + let mut crosser = EdgeCrosser::new_with_c(&a, &a, &a); + assert_eq!(crosser.chain_crossing_sign(&a), Crossing::MaybeCross); + } +} diff --git a/src/s2/edge_crossings/mod.rs b/src/s2/edge_crossings/mod.rs new file mode 100644 index 0000000..073690c --- /dev/null +++ b/src/s2/edge_crossings/mod.rs @@ -0,0 +1,578 @@ +//! Defines functions related to determining whether two geodesic edges cross +//! and for computing intersection points. +//! +//! The predicates [`crossing_sign`], [`vertex_crossing`], and [`edge_or_vertex_crossing`] +//! are robust, meaning that they produce correct, consistent results even in +//! pathological cases. +//! +//! See also [`EdgeCrosser`] (which efficiently tests an edge against a sequence +//! of other edges). + +use crate::consts::*; +use crate::r3::vector::Vector; +use crate::s2::point::{Point, ordered_ccw}; + +mod crosser; +pub use crosser::EdgeCrosser; + +const DBL_ERR: f64 = DBL_EPSILON / 2.0; + +/// INTERSECTION_ERROR is an upper bound on the distance from the intersection +/// point returned by get_intersection() to the true intersection point. +pub const INTERSECTION_ERROR: f64 = 8.0 * DBL_ERR; + +/// This value can be used as the Builder snap_radius() to ensure that edges +/// that have been displaced by up to INTERSECTION_ERROR are merged back +/// together again. For example this can happen when geometry is intersected +/// with a set of tiles and then unioned. It is equal to twice the +/// intersection error because input edges might have been displaced in +/// opposite directions. +pub const INTERSECTION_MERGE_RADIUS: f64 = 16.0 * DBL_ERR; + +/// A Crossing indicates how edges cross. +#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)] +pub enum Crossing { + DoNotCross = -1, + MaybeCross = 0, + Cross = 1, +} + +/// Indicates the direction an edge crosses another edge, used for computing winding numbers. +#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)] +pub enum SignedCrossing { + LeftToRight = -1, + None = 0, + RightToLeft = 1, +} + +impl SignedCrossing { + /// Returns the crossing parity as an integer delta for summing winding numbers. + pub fn winding_delta(&self) -> i32 { + *self as i32 + } +} + +/// Returns a vector whose direction is guaranteed to be very close to the exact +/// mathematical cross product of the given unit-length vectors "a" and "b", but +/// whose magnitude is arbitrary. Unlike a.cross(b), this statement is true +/// even when "a" and "b" are very nearly parallel (i.e., a ~= b or a ~= -b). +/// Specifically, the direction of the result vector differs from the exact cross +/// product by at most 6 * DBL_ERR radians. +/// +/// When a == -b exactly, the result is consistent with the symbolic perturbation +/// model used by [`robust_sign`](crate::s2::predicates::robust_sign). In other words, even antipodal +/// point pairs have a consistent and well-defined edge between them. (In fact +/// this is true for any pair of distinct points whose vectors are parallel.) +/// +/// When a == b exactly, an arbitrary vector orthogonal to "a" is returned. +/// [From a strict mathematical viewpoint it would be better to return (0, 0, 0), +/// but this behavior helps to avoid special cases in client code.] +/// +/// This function has the following properties: +/// +/// 1. `robust_cross_prod(a, b) != 0` for all `a`, `b` +/// 2. `robust_cross_prod(b, a) == -robust_cross_prod(a, b)` unless `a == b` +/// 3. `robust_cross_prod(-a, b) == -robust_cross_prod(a, b)` unless `a` and `b` are exactly proportional +/// 4. `robust_cross_prod(a, -b) == -robust_cross_prod(a, b)` unless `a` and `b` are exactly proportional +/// +/// Note that if you want the result to be unit-length, you must call `normalize()` +/// explicitly. (The result is always scaled such that `normalize()` can be called +/// without precision loss due to floating-point underflow.) +pub fn robust_cross_prod(a: &Point, b: &Point) -> Point { + debug_assert!(a.0.is_unit()); + debug_assert!(b.0.is_unit()); + let mut result = Point::default(); + if get_stable_cross_prod(a, b, &mut result) { + return result; + } + if a == b { + return a.ortho(); + } + symbolic_cross_prod(a, b) +} + +fn get_stable_cross_prod(a: &Point, b: &Point, result: &mut Point) -> bool { + let robust_cross_prod_error = 6.0 * DBL_ERR; + let sqrt3 = 3.0f64.sqrt(); + let min_norm = + (32.0 * sqrt3 * DBL_ERR) / (robust_cross_prod_error / DBL_ERR - (1.0 + 2.0 * sqrt3)); + + let x = a.0 - b.0; + let y = a.0 + b.0; + result.0 = x.cross(&y); + result.0.norm2() >= min_norm * min_norm +} + +fn symbolic_cross_prod(a: &Point, b: &Point) -> Point { + if a.0 < b.0 { + ensure_normalizable(&symbolic_cross_prod_sorted(a, b)) + } else { + Point(ensure_normalizable(&symbolic_cross_prod_sorted(b, a)).0 * -1.0) + } +} + +fn symbolic_cross_prod_sorted(a: &Point, b: &Point) -> Point { + debug_assert!(a.0 < b.0); + + if b.0.x != 0.0 || b.0.y != 0.0 { + return Point(Vector::new(-b.0.y, b.0.x, 0.0)); + } + if b.0.z != 0.0 { + return Point(Vector::new(b.0.z, 0.0, 0.0)); + } + + if a.0.x != 0.0 || a.0.y != 0.0 { + return Point(Vector::new(a.0.y, -a.0.x, 0.0)); + } + if a.0.z != 0.0 { + return Point(Vector::new(-a.0.z, 0.0, 0.0)); + } + + Point(Vector::new(1.0, 0.0, 0.0)) +} + +fn is_normalizable(p: &Point) -> bool { + let max_abs = p.0.x.abs().max(p.0.y.abs()).max(p.0.z.abs()); + max_abs >= 2.0f64.powi(-242) +} + +fn ensure_normalizable(p: &Point) -> Point { + if !is_normalizable(p) { + let max_abs = p.0.x.abs().max(p.0.y.abs()).max(p.0.z.abs()); + let ilogb = max_abs.log2().floor() as i32; + let scale = 2.0f64.powi(-1 - ilogb); + return Point(p.0 * scale); + } + *p +} + +/// This function determines whether the edge `ab` intersects the edge `cd`. +/// Returns [`Crossing::Cross`] if `ab` crosses `cd` at a point that is interior to both edges. +/// Returns [`Crossing::MaybeCross`] if any two vertices from different edges are the same. +/// Returns [`Crossing::DoNotCross`] otherwise. +/// +/// Note that if an edge is degenerate (`a == b` or `c == d`), the return value +/// is [`Crossing::MaybeCross`] if two vertices from different edges are the same and [`Crossing::DoNotCross`] otherwise. +/// +/// Properties of this function: +/// +/// 1. `crossing_sign(b, a, c, d) == crossing_sign(a, b, c, d)` +/// 2. `crossing_sign(c, d, a, b) == crossing_sign(a, b, c, d)` +/// 3. `crossing_sign(a, b, c, d) == Crossing::MaybeCross` if `a == c`, `a == d`, `b == c`, `b == d` +/// 4. `crossing_sign(a, b, c, d) <= Crossing::MaybeCross` if `a == b` or `c == d` +/// +/// This function implements an exact, consistent perturbation model such +/// that no three points are ever considered to be collinear. This means +/// that even if you have 4 points `a`, `b`, `c`, `d` that lie exactly in a line +/// (say, around the equator), `c` and `d` will be treated as being slightly to +/// one side or the other of `ab`. This is done in a way such that the +/// results are always consistent with [`robust_sign`](crate::s2::predicates::robust_sign). +/// +/// Note that if you want to check an edge against a collection of other edges, +/// it is much more efficient to use an [`EdgeCrosser`]. +pub fn crossing_sign(a: &Point, b: &Point, c: &Point, d: &Point) -> Crossing { + let mut crosser = EdgeCrosser::new_with_c(a, b, c); + crosser.chain_crossing_sign(d) +} + +/// Given two edges `ab` and `cd` where at least two vertices are identical +/// (i.e. `crossing_sign(a, b, c, d) == Crossing::MaybeCross`), this function defines whether the +/// two edges cross in such a way that point-in-polygon containment tests can +/// be implemented by counting the number of edge crossings. The basic rule is +/// that a crossing occurs if `ab` is encountered after `cd` during a CCW sweep +/// around the shared vertex starting from a fixed reference point. +/// +/// Note that according to this rule, if `ab` crosses `cd` then in general `cd` +/// does not cross `ab`. However, this leads to the correct result when +/// counting polygon edge crossings. For example, suppose that `a`, `b`, `c` are +/// three consecutive vertices of a CCW polygon. If we now consider the edge +/// crossings of a segment `bp` as `p` sweeps around `b`, the crossing number +/// changes parity exactly when `bp` crosses `ba` or `bc`. +/// +/// Useful properties of this function: +/// +/// 1. `vertex_crossing(a, a, c, d) == vertex_crossing(a, b, c, c) == false` +/// 2. `vertex_crossing(a, b, a, b) == vertex_crossing(a, b, b, a) == true` +/// 3. `vertex_crossing(a, b, c, d) == vertex_crossing(a, b, d, c) == vertex_crossing(b, a, c, d) == vertex_crossing(b, a, d, c)` +/// 4. If exactly one of `a, b` equals one of `c, d`, then exactly one of +/// `vertex_crossing(a, b, c, d)` and `vertex_crossing(c, d, a, b)` is true +/// +/// # Panics +/// +/// Panics in debug builds if called with 4 distinct vertices. +pub fn vertex_crossing(a: &Point, b: &Point, c: &Point, d: &Point) -> bool { + debug_assert!( + a == c || a == d || b == c || b == d, + "vertex_crossing called with 4 distinct vertices" + ); + if a == b || c == d { + return false; + } + if a == c { + return b == d || ordered_ccw(&a.ortho(), d, b, a); + } + if b == d { + return ordered_ccw(&b.ortho(), c, a, b); + } + if a == d { + return b == c || ordered_ccw(&a.ortho(), c, b, a); + } + if b == c { + return ordered_ccw(&b.ortho(), d, a, b); + } + false +} + +/// Like [`vertex_crossing`] but returns [`SignedCrossing::LeftToRight`] if `ab` crosses `cd` from left to right, +/// [`SignedCrossing::RightToLeft`] if `ab` crosses `cd` from right to left, and [`SignedCrossing::None`] otherwise. This implies that +/// if `cd` bounds some region according to the "interior is on the left" rule, +/// this function returns [`SignedCrossing::LeftToRight`] when `ab` exits the region and [`SignedCrossing::RightToLeft`] when `ab` enters. +/// +/// This is a helper method that allows computing the change in winding number +/// from point `a` to point `b` by summing the signed edge crossings of `ab` with the +/// edges of the loop(s) used to define the winding number. +/// +/// Useful properties of this function: +/// +/// 1. `signed_vertex_crossing(a, a, c, d) == SignedCrossing::None` +/// 2. `signed_vertex_crossing(a, b, a, b) == SignedCrossing::RightToLeft` +/// 3. `signed_vertex_crossing(a, b, b, a) == SignedCrossing::LeftToRight` +/// 4. `signed_vertex_crossing(a, b, c, d).winding_delta() == -signed_vertex_crossing(a, b, d, c).winding_delta() == -signed_vertex_crossing(b, a, c, d).winding_delta() == signed_vertex_crossing(b, a, d, c).winding_delta()` +/// 5. If exactly one of `a, b` equals one of `c, d`, then exactly one of +/// `signed_vertex_crossing(a, b, c, d)` and `signed_vertex_crossing(c, d, a, b)` is non-zero +/// +/// # Panics +/// +/// Panics in debug builds if called with 4 distinct vertices. +pub fn signed_vertex_crossing(a: &Point, b: &Point, c: &Point, d: &Point) -> SignedCrossing { + debug_assert!( + a == c || a == d || b == c || b == d, + "signed_vertex_crossing called with 4 distinct vertices" + ); + if a == b || c == d { + return SignedCrossing::None; + } + if a == c { + return if b == d || ordered_ccw(&a.ortho(), d, b, a) { + SignedCrossing::RightToLeft + } else { + SignedCrossing::None + }; + } + if b == d { + return if ordered_ccw(&b.ortho(), c, a, b) { + SignedCrossing::RightToLeft + } else { + SignedCrossing::None + }; + } + if a == d { + return if b == c || ordered_ccw(&a.ortho(), c, b, a) { + SignedCrossing::LeftToRight + } else { + SignedCrossing::None + }; + } + if b == c { + return if ordered_ccw(&b.ortho(), d, a, b) { + SignedCrossing::LeftToRight + } else { + SignedCrossing::None + }; + } + SignedCrossing::None +} + +/// A convenience function that calls [`crossing_sign`] to handle cases +/// where all four vertices are distinct, and [`vertex_crossing`] to handle +/// cases where two or more vertices are the same. This defines a crossing +/// function such that point-in-polygon containment tests can be implemented +/// by simply counting edge crossings. +pub fn edge_or_vertex_crossing(a: &Point, b: &Point, c: &Point, d: &Point) -> bool { + match crossing_sign(a, b, c, d) { + Crossing::DoNotCross => false, + Crossing::Cross => true, + Crossing::MaybeCross => vertex_crossing(a, b, c, d), + } +} + +fn compare_edges(mut a0: Point, mut a1: Point, mut b0: Point, mut b1: Point) -> bool { + if a0.0 > a1.0 { + std::mem::swap(&mut a0, &mut a1); + } + if b0.0 > b1.0 { + std::mem::swap(&mut b0, &mut b1); + } + a0.0 < b0.0 || (a0.0 == b0.0 && a1.0 < b1.0) +} + +fn projection(x: &Vector, a_norm: &Vector, a_norm_len: f64, a0: &Point, a1: &Point) -> (f64, f64) { + let x0 = *x - a0.0; + let x1 = *x - a1.0; + let x0_dist2 = x0.norm2(); + let x1_dist2 = x1.norm2(); + + let dist; + let proj; + if x0_dist2 < x1_dist2 || (x0_dist2 == x1_dist2 && x0 < x1) { + dist = x0_dist2.sqrt(); + proj = x0.dot(a_norm); + } else { + dist = x1_dist2.sqrt(); + proj = x1.dot(a_norm); + } + + let bound = (((3.5 + 2.0 * 3f64.sqrt()) * a_norm_len + 32.0 * 3f64.sqrt() * DBL_ERR) * dist + + 1.5 * proj.abs()) + * DBL_ERR; + (proj, bound) +} + +fn get_intersection_stable_sorted(a0: &Point, a1: &Point, b0: &Point, b1: &Point) -> Option { + let a_norm = (a0.0 - a1.0).cross(&(a0.0 + a1.0)); + let a_norm_len = a_norm.norm(); + let b_len = (b1.0 - b0.0).norm(); + + let (mut b0_dist, b0_error) = projection(&b0.0, &a_norm, a_norm_len, a0, a1); + let (mut b1_dist, b1_error) = projection(&b1.0, &a_norm, a_norm_len, a0, a1); + + if b0_dist < b1_dist { + b0_dist = -b0_dist; + b1_dist = -b1_dist; + } + let dist_sum = b0_dist - b1_dist; + let error_sum = b0_error + b1_error; + if dist_sum <= error_sum { + return None; + } + + let x = (b1.0 * b0_dist) - (b0.0 * b1_dist); + let err = b_len * (b0_dist * b1_error - b1_dist * b0_error).abs() / (dist_sum - error_sum) + + 2.0 * dist_sum * DBL_ERR; + + let x_len2 = x.norm2(); + if x_len2 < f64::MIN_POSITIVE { + return None; + } + let x_len = x_len2.sqrt(); + let max_error = INTERSECTION_ERROR; + if err > (max_error - DBL_ERR) * x_len { + return None; + } + + Some(Point(x * (1.0 / x_len))) +} + +fn get_intersection_stable(a0: &Point, a1: &Point, b0: &Point, b1: &Point) -> Option { + let a_len2 = (a1.0 - a0.0).norm2(); + let b_len2 = (b1.0 - b0.0).norm2(); + if a_len2 < b_len2 || (a_len2 == b_len2 && compare_edges(*a0, *a1, *b0, *b1)) { + get_intersection_stable_sorted(b0, b1, a0, a1) + } else { + get_intersection_stable_sorted(a0, a1, b0, b1) + } +} + +// Emulates the tie-breaking behavior of C++ GetIntersectionExact without using bigdecimal. +fn get_intersection_exact(a0: &Point, a1: &Point, b0: &Point, b1: &Point) -> Point { + debug_assert!(a0.0.is_unit()); + debug_assert!(a1.0.is_unit()); + debug_assert!(b0.0.is_unit()); + debug_assert!(b1.0.is_unit()); + let mut a_norm = Point(a0.0.cross(&a1.0)); + let mut b_norm = Point(b0.0.cross(&b1.0)); + let x = Point(a_norm.0.cross(&b_norm.0)); + + if x.0.x != 0.0 || x.0.y != 0.0 || x.0.z != 0.0 { + let is_ccw = crate::s2::predicates::sign(a0, a1, b1); + let mut pt = Point(x.0.normalize()); + if !is_ccw { + pt = Point(pt.0 * -1.0); + } + return pt; + } + + if a_norm.0 == Vector::new(0.0, 0.0, 0.0) { + a_norm = symbolic_cross_prod(a0, a1); + } + if b_norm.0 == Vector::new(0.0, 0.0, 0.0) { + b_norm = symbolic_cross_prod(b0, b1); + } + + let mut pt = Point(Vector::new(10.0, 10.0, 10.0)); + if ordered_ccw(b0, a0, b1, &b_norm) && a0.0 < pt.0 { + pt = *a0; + } + if ordered_ccw(b0, a1, b1, &b_norm) && a1.0 < pt.0 { + pt = *a1; + } + if ordered_ccw(a0, b0, a1, &a_norm) && b0.0 < pt.0 { + pt = *b0; + } + if ordered_ccw(a0, b1, a1, &a_norm) && b1.0 < pt.0 { + pt = *b1; + } + + pt +} + +/// Given two edges `ab` and `cd` such that `crossing_sign(a, b, c, d) == Crossing::Cross`, returns +/// their intersection point. Useful properties of this function: +/// +/// 1. `get_intersection(b, a, c, d) == get_intersection(a, b, d, c) == get_intersection(a, b, c, d)` +/// 2. `get_intersection(c, d, a, b) == get_intersection(a, b, c, d)` +/// +/// The returned intersection point is guaranteed to be very close to the +/// true intersection point of `ab` and `cd`, even if the edges intersect at a +/// very small angle. See [`INTERSECTION_ERROR`] for details. +pub fn get_intersection(a0: &Point, a1: &Point, b0: &Point, b1: &Point) -> Point { + debug_assert!(crossing_sign(a0, a1, b0, b1) == Crossing::Cross); + let pt = match get_intersection_stable(a0, a1, b0, b1) { + Some(p) => p, + None => get_intersection_exact(a0, a1, b0, b1), + }; + debug_assert!(pt.0.is_unit()); + pt +} + +/// Returns true if the angle `abc` contains its vertex `b`. Containment is +/// defined such that if several polygons tile the region around a vertex, then +/// exactly one of those polygons contains that vertex. Returns false for +/// degenerate angles of the form `aba`. +/// +/// **Note**: This method is not sufficient to determine vertex containment in +/// polygons with duplicate vertices (such as the polygon `ABCADE`). Use +/// [`ContainsVertexQuery`](crate::s2::contains_vertex_query::ContainsVertexQuery) for such polygons. +/// +/// Useful properties of this function: +/// +/// 1. `angle_contains_vertex(a, b, a) == false` +/// 2. `angle_contains_vertex(a, b, c) == !angle_contains_vertex(c, b, a)` unless `a == c` +/// 3. Given vertices `v₁` ... `vₖ` ordered cyclically CCW around vertex `b`, +/// `angle_contains_vertex(vᵢ₊₁, b, vᵢ)` is true for exactly one value of `i`. +pub fn angle_contains_vertex(a: &Point, b: &Point, c: &Point) -> bool { + !ordered_ccw(&b.ortho(), c, a, b) +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::r3::vector::Vector; + use crate::s2::point::Point; + + // Note: The following tests from the C++ reference implementation are skipped + // because they test the ExactFloat (arbitrary precision) fallback paths which + // are not present in this pure-f64 port: + // - RobustCrossProdError + // - IntersectionError + // - GrazingIntersections + // - ExactIntersectionUnderflow + // - ExactIntersectionSign + // - SymbolicCrossProdConsistentWithSign + + #[test] + fn test_angle_contains_vertex() { + let a = Point(Vector::new(1.0, 0.0, 0.0)); + let b = Point(Vector::new(0.0, 1.0, 0.0)); + let ref_b = b.ortho(); + + // Degenerate angle ABA. + assert!(!angle_contains_vertex(&a, &b, &a)); + + // An angle where A == referenceDir(B). + assert!(angle_contains_vertex(&ref_b, &b, &a)); + + // An angle where C == referenceDir(B). + assert!(!angle_contains_vertex(&a, &b, &ref_b)); + + let v = Point(Vector::new(1.0, 1.0, 1.0).normalize()); + for _ in 0..10 { + let pts = crate::s2::point::regular_points( + &v, + crate::s1::angle::Angle::from(crate::s1::angle::Deg(1.0)), + 10, + ); + let mut num_contains = 0; + for i in 0..pts.len() { + if angle_contains_vertex(&pts[(i + 1) % pts.len()], &v, &pts[i]) { + num_contains += 1; + } + } + assert_eq!(num_contains, 1); + } + } + + #[test] + fn test_crossing_sign_simple() { + let a = Point(Vector::new(1.0, 0.0, 0.0)); + let b = Point(Vector::new(0.0, 1.0, 0.0)); + let c = Point(Vector::new(1.0, 1.0, 1.0).normalize()); + let d = Point(Vector::new(1.0, 1.0, -1.0).normalize()); + + let cross = crossing_sign(&a, &b, &c, &d); + assert_eq!(cross, Crossing::Cross); + + // Same edge test + assert_eq!(crossing_sign(&a, &b, &a, &b), Crossing::MaybeCross); + } + + #[test] + fn test_get_intersection_invariants() { + use crate::s2::random; + use rand::Rng; + use rand::SeedableRng; + use rand::rngs::StdRng; + + let mut rng = StdRng::seed_from_u64(12345); + let iters = 5000; + + for _ in 0..iters { + let mut a; + let mut b; + let mut c; + let mut d; + + loop { + a = random::point(&mut rng); + b = random::point(&mut rng); + + c = Point(Vector::new(a.0.y, a.0.x, a.0.z)); + d = Point(Vector::new(b.0.y, b.0.x, b.0.z)); + + if crossing_sign(&a, &b, &c, &d) == Crossing::Cross { + break; + } + } + + let result = get_intersection(&a, &b, &c, &d); + + let mut a2 = a; + let mut b2 = b; + let mut c2 = c; + let mut d2 = d; + + if rng.next_u32() % 2 == 0 { + std::mem::swap(&mut a2, &mut b2); + } + if rng.next_u32() % 2 == 0 { + std::mem::swap(&mut c2, &mut d2); + } + if rng.next_u32() % 2 == 0 { + std::mem::swap(&mut a2, &mut c2); + std::mem::swap(&mut b2, &mut d2); + } + + assert_eq!(result, get_intersection(&a2, &b2, &c2, &d2)); + } + } + + #[test] + fn test_compare_edges_order_invariant() { + let v0 = Point(Vector::new(0.0, 1.0, 0.0)); + let v1 = Point(Vector::new(1.0, 0.0, 0.0)); + assert!(!compare_edges(v0, v1, v0, v1)); + assert!(!compare_edges(v1, v0, v0, v1)); + assert!(!compare_edges(v0, v1, v1, v0)); + assert!(!compare_edges(v1, v0, v1, v0)); + } +} diff --git a/src/s2/mod.rs b/src/s2/mod.rs index 8d1ed03..355e69d 100644 --- a/src/s2/mod.rs +++ b/src/s2/mod.rs @@ -17,6 +17,8 @@ pub mod edgeutil; pub mod metric; pub mod predicates; +pub mod edge_crossings; + pub mod shape; #[cfg(test)] diff --git a/src/s2/predicates.rs b/src/s2/predicates.rs index a820cc6..00c663a 100644 --- a/src/s2/predicates.rs +++ b/src/s2/predicates.rs @@ -55,7 +55,7 @@ const MAX_DETERMINANT_ERROR: f64 = 1.8274 * DBL_EPSILON; /// its sign with certainty. const DET_ERROR_MULTIPLIER: f64 = 3.2321 * DBL_EPSILON; -#[derive(PartialEq, Eq)] +#[derive(PartialEq, Eq, Clone, Copy)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum Direction { Clockwise,