87 lines
2 KiB
Rust
87 lines
2 KiB
Rust
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))
|
|
}
|