split and snap

src/geometry.rs

@@ -0,0 +1,87 @@
+use geo::{Coord, Point};
+
+use crate::crossings::BoundaryHit;
+
+pub fn cross(a: Coord<f64>, b: Coord<f64>) -> f64 {
+    a.x * b.y - a.y * b.x
+}
+
+pub fn interpolate(start: Point<f64>, end: Point<f64>, t: f64) -> Point<f64> {
+    Point::new(
+        start.x() + t * (end.x() - start.x()),
+        start.y() + t * (end.y() - start.y()),
+    )
+}
+
+pub fn segment_intersection(
+    p: Coord<f64>,
+    p2: Coord<f64>,
+    q: Coord<f64>,
+    q2: Coord<f64>,
+) -> Option<(f64, Point<f64>)> {
+    let r = Coord {
+        x: p2.x - p.x,
+        y: p2.y - p.y,
+    };
+
+    let s = Coord {
+        x: q2.x - q.x,
+        y: q2.y - q.y,
+    };
+
+    let denominator = cross(r, s);
+
+    if denominator.abs() < 1e-14 {
+        return None;
+    }
+
+    let qp = Coord {
+        x: q.x - p.x,
+        y: q.y - p.y,
+    };
+
+    let t = cross(qp, s) / denominator;
+    let u = cross(qp, r) / denominator;
+
+    const EPSILON: f64 = 1e-10;
+
+    if !(-EPSILON..=1.0 + EPSILON).contains(&t) || !(-EPSILON..=1.0 + EPSILON).contains(&u) {
+        return None;
+    }
+
+    let t = t.clamp(0.0, 1.0);
+
+    let point = Point::new(p.x + t * r.x, p.y + t * r.y);
+
+    Some((t, point))
+}
+
+pub fn deduplicate_hits(mut hits: Vec<BoundaryHit>) -> Vec<BoundaryHit> {
+    hits.sort_by(|a, b| {
+        a.position
+            .partial_cmp(&b.position)
+            .unwrap_or(std::cmp::Ordering::Equal)
+    });
+
+    let mut result = Vec::new();
+
+    for hit in hits {
+        let duplicate = result
+            .iter()
+            .any(|existing: &BoundaryHit| (existing.position - hit.position).abs() < 1e-9);
+
+        if !duplicate {
+            result.push(hit);
+        }
+    }
+
+    result
+}
+
+/// Recalculate the point directly from the original track segment.
+///
+/// This guarantees that the returned point lies on the segment rather than
+/// relying on the coordinates calculated during the intersection operation.
+pub fn snap_to_segment(start: Point<f64>, end: Point<f64>, position: f64) -> Point<f64> {
+    interpolate(start, end, position.clamp(0.0, 1.0))
+}