diff options
Diffstat (limited to 'src/math/line_segment.rs')
| -rw-r--r-- | src/math/line_segment.rs | 287 |
1 files changed, 287 insertions, 0 deletions
diff --git a/src/math/line_segment.rs b/src/math/line_segment.rs new file mode 100644 index 0000000..e6ca70f --- /dev/null +++ b/src/math/line_segment.rs @@ -0,0 +1,287 @@ +use super::{Rect, Vec2}; +use alga::general::{ClosedDiv, ClosedMul, ClosedSub}; +use nalgebra::{RealField, Scalar}; +use num_traits::Zero; +use std::cmp::Ordering; + +#[derive(Debug, Clone)] +pub struct LineSegment<T: Scalar + Copy> { + pub start: Vec2<T>, + pub end: Vec2<T>, +} + +impl<T: Scalar + Copy> LineSegment<T> { + pub fn new(start: Vec2<T>, end: Vec2<T>) -> Self { + Self { start, end } + } + + /* Helper function to check if this line contains a point. This function is very efficient, but + * must only be used for points that are collinear with the segment. This is checked only by + * assertion, so make sure that everything is ok in release mode. + */ + pub(crate) fn contains_collinear(&self, point: Vec2<T>) -> bool + where + T: PartialOrd, + { + point.x <= super::partial_max(self.start.x, self.end.x) + && point.x >= super::partial_min(self.start.x, self.end.x) + && point.y <= super::partial_max(self.start.y, self.end.y) + && point.y >= super::partial_min(self.start.y, self.end.y) + } + + /// Checks if two segments intersect. This is much more efficient than trying to find the exact + /// point of intersection, so it should be used if it is not strictly necessary to know it. + pub fn intersect(ls1: &LineSegment<T>, ls2: &LineSegment<T>) -> bool + where + T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero, + { + /* This algorithm works by checking the triplet orientation of the first lines points with the + * first point of the second line. After that it does the same for the second point of the + * second line. If the orientations are different, that must mean the second line starts + * "before" the first line and ends "after" it. It does the same, but with the roles of first + * and second line reversed. If both of these conditions are met, it follows that the lines + * must have crossed. + * + * Edge case: If an orientation comes out as collinear, the point of the other line that was + * checked may be on the other line or after/before it (if you extend the segment). If it is on + * the other line, this also means, the lines cross, since one line "stands" on the other. + * If however none of the collinear cases are like this, the lines cannot touch, because line + * segment a point was collinear with was not long enough. + */ + + // Cache the necessary orientations. + let ls1_ls2start_orientation = triplet_orientation(ls1.start, ls1.end, ls2.start); + let ls1_ls2end_orientation = triplet_orientation(ls1.start, ls1.end, ls2.end); + let ls2_ls1start_orientation = triplet_orientation(ls2.start, ls2.end, ls1.start); + let ls2_ls1end_orientation = triplet_orientation(ls2.start, ls2.end, ls1.end); + + // Check for the first case that wase described (general case). + if ls1_ls2start_orientation != ls1_ls2end_orientation + && ls2_ls1start_orientation != ls2_ls1end_orientation + { + return true; + } + + // Check if the start of ls2 lies on ls1 + if ls1_ls2start_orientation == TripletOrientation::Collinear + && ls1.contains_collinear(ls2.start) + { + return true; + } + // Check if the end of ls2 lies on ls1 + if ls1_ls2end_orientation == TripletOrientation::Collinear + && ls1.contains_collinear(ls2.end) + { + return true; + } + + // Check if the start of ls1 lies on ls2 + if ls2_ls1start_orientation == TripletOrientation::Collinear + && ls2.contains_collinear(ls1.start) + { + return true; + } + // Check if the end of ls1 lies on ls2 + if ls2_ls1end_orientation == TripletOrientation::Collinear + && ls2.contains_collinear(ls1.end) + { + return true; + } + + false + } + + pub fn intersection(line_a: &LineSegment<T>, line_b: &LineSegment<T>) -> Option<Vec2<T>> + where + T: ClosedSub + ClosedMul + ClosedDiv + Zero + RealField, + { + // Calculate the differences of each line value from starting point to end point + // coordinate. + let dax = line_a.start.x - line_a.end.x; + let dbx = line_b.start.x - line_b.end.x; + let day = line_a.start.y - line_a.end.y; + let dby = line_b.start.y - line_b.end.y; + + // Calculate determinant to see, if the lines are parallel or not + // TODO: Collinearity check? + let d = (dax * dby) - (day * dbx); + if d == T::zero() { + None + } else { + let ad = (line_a.start.x * line_a.end.y) - (line_a.start.y * line_a.end.x); + let bd = (line_b.start.x * line_b.end.y) - (line_b.start.y * line_b.end.x); + + let out = Vec2::new(((ad * dbx) - (dax * bd)) / d, ((ad * dby) - (day * bd)) / d); + + /* Since the line intersection, not the line segment intersection is calculated, a + * bounding check is necessary, to see if the intersection still lies inside both of + * the segments. We know it's on the lines, so checking with the lines bounding box is + * faster than checking where on the line exactly it would be. + */ + if Rect::bounding_rect(line_a.start, line_a.end).contains(out) + && Rect::bounding_rect(line_b.start, line_b.end).contains(out) + { + Some(out) + } else { + None + } + } + } + + /// Find all segments, into which this LineSegment would be splitted, when the points provided + /// would cut the segment. The points must be on the line, otherwise this does not make sense. + /// Also, no segments of length zero (start point = end point) will be created. + pub fn segments(&self, split_points: &[Vec2<T>]) -> Vec<Vec2<T>> + where + T: ClosedSub + ClosedMul + PartialOrd + Zero, + { + // Make sure all segments are collinear with the segment and actually on it + assert_eq!( + split_points + .iter() + .find(|&p| triplet_orientation(self.start, self.end, *p) + != TripletOrientation::Collinear), + None + ); + assert_eq!( + split_points.iter().find(|&p| !self.contains_collinear(*p)), + None + ); + + let mut segments = Vec::with_capacity(split_points.len() + 2); + segments.push(self.start); + segments.push(self.end); + for point in split_points { + segments.push(*point); + } + + segments.sort_unstable_by(|a, b| a.partial_cmp(b).unwrap_or(Ordering::Greater)); + segments.dedup(); + if self.start > self.end { + segments.reverse(); + } + + segments + } + + /// Checks if this line segment and the other line segment are the same, ignoring the direction + /// in which the lines are going, in other words, which of the vectors the line starts at and which + /// vector the line ends at is irrelevant. + pub fn eq_ignore_dir(&self, other: &LineSegment<T>) -> bool { + (self.start == other.start && self.end == other.end) + || (self.end == other.start && self.start == other.end) + } +} + +#[derive(PartialEq, Eq)] +pub(crate) enum TripletOrientation { + Clockwise, + Counterclockwise, + Collinear, +} + +/// Helper function to determine which direction one would turn to traverse from the first point +/// over the second to the third point. The third option is collinear, in which case the three points +/// are on the same line. +pub(crate) fn triplet_orientation<T>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> TripletOrientation +where + T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero, +{ + /* Check the slopes of the vector from a to b and b to c. If the slope of ab is greater than + * that of bc, the rotation is clockwise. If ab is smaller than bc it's counterclockwise. If + * they are the same it follows that the three points are collinear. + */ + match (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y) { + q if q > T::zero() => TripletOrientation::Counterclockwise, + q if q < T::zero() => TripletOrientation::Clockwise, + _ => TripletOrientation::Collinear, + } +} + +/// Calculate the angle between three points. Guaranteed to have 0 <= angle < 2Pi. The angle is +/// calculated as the angle between a line from a to b and then from b to c, increasing +/// counterclockwise. +/// +/// # Panics +/// If the length from a to b or the length from b to c is zero. +pub(crate) fn triplet_angle<T>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> T +where + T: Scalar + Copy + ClosedSub + RealField + Zero, +{ + assert!(a != b); + assert!(b != c); + + // Handle the extreme 0 and 180 degree cases + let orientation = triplet_orientation(a, b, c); + if orientation == TripletOrientation::Collinear { + if LineSegment::new(a, b).contains_collinear(c) + || LineSegment::new(b, c).contains_collinear(a) + { + return T::zero(); + } else { + return T::pi(); + } + } + + // Calculate the vectors out of the points + let ba = a - b; + let bc = c - b; + + // Calculate the angle between 0 and Pi. + let angle = ((ba * bc) / (ba.length() * bc.length())).acos(); + + // Make angle into a full circle angle by looking at the orientation of the triplet. + let angle = match orientation { + TripletOrientation::Counterclockwise => T::pi() + (T::pi() - angle), + _ => angle, + }; + + angle +} + +#[cfg(test)] +mod test { + use super::*; + use std::f64::consts::PI; + + #[test] + fn contains_collinear() { + let segment = LineSegment::new(Vec2::new(0., 0.), Vec2::new(2., 2.)); + + assert!(segment.contains_collinear(Vec2::new(0., 0.))); + assert!(segment.contains_collinear(Vec2::new(1., 1.))); + assert!(segment.contains_collinear(Vec2::new(2., 2.))); + + assert!(!segment.contains_collinear(Vec2::new(-1., -1.))); + assert!(!segment.contains_collinear(Vec2::new(3., 3.))); + assert!(!segment.contains_collinear(Vec2::new(-1., 0.))); + } + + #[test] + fn triplet_angle() { + assert_eq!( + super::triplet_angle(Vec2::new(0., 0.), Vec2::new(0., -1.), Vec2::new(-1., -1.)), + 1.5 * PI + ); + assert_eq!( + super::triplet_angle(Vec2::new(2., 2.), Vec2::new(0., 0.), Vec2::new(-1., -1.)), + PI + ); + assert_eq!( + super::triplet_angle(Vec2::new(3., 1.), Vec2::new(0., 0.), Vec2::new(6., 2.)), + 0. + ); + assert_eq!( + super::triplet_angle(Vec2::new(3., 1.), Vec2::new(0., 0.), Vec2::new(-6., -2.)), + PI + ); + assert_eq!( + super::triplet_angle(Vec2::new(0., 1.), Vec2::new(0., 2.), Vec2::new(-3., 2.)), + 0.5 * PI + ); + assert_eq!( + super::triplet_angle(Vec2::new(3., 1.), Vec2::new(2., 2.), Vec2::new(0., 2.)), + 0.75 * PI + ); + } +} |