use crate::{linestring_2d, LinestringError};
use ahash::AHashSet;
use cgmath::{ulps_eq, BaseFloat, Point2};
use core::fmt;
use std::cmp;
use std::collections::BTreeMap;
use std::convert::identity;
use std::marker::PhantomData;
#[derive(Clone, Copy)]
pub struct SiteEventKey<T: BaseFloat + Sync> {
pub pos: Point2<T>,
}
impl<T: BaseFloat + Sync> SiteEventKey<T> {
pub fn new(x: T, y: T) -> Self {
Self {
pos: Point2 { x, y },
}
}
}
impl<T: BaseFloat + Sync> std::fmt::Debug for SiteEventKey<T> {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
f.debug_tuple("")
.field(&self.pos.x)
.field(&self.pos.y)
.finish()
}
}
impl<T: BaseFloat + Sync> PartialOrd for SiteEventKey<T> {
#[inline(always)]
fn partial_cmp(&self, other: &Self) -> Option<cmp::Ordering> {
if ulps_eq!(&self.pos.y, &other.pos.y) {
if ulps_eq!(&self.pos.x, &other.pos.x) {
Some(cmp::Ordering::Equal)
} else {
self.pos.x.partial_cmp(&other.pos.x)
}
} else {
self.pos.y.partial_cmp(&other.pos.y)
}
}
}
impl<T: BaseFloat + Sync> Ord for SiteEventKey<T> {
#[inline(always)]
fn cmp(&self, other: &Self) -> cmp::Ordering {
self.partial_cmp(other).unwrap()
}
}
impl<T: BaseFloat + Sync> PartialEq for SiteEventKey<T> {
#[inline(always)]
fn eq(&self, other: &Self) -> bool {
linestring_2d::point_ulps_eq(self.pos, other.pos)
}
}
impl<T> Eq for SiteEventKey<T> where T: BaseFloat + Sync {}
struct MinMax<T: BaseFloat + Sync> {
best_left: Option<T>,
slope: MinMaxSlope<T>,
best_right: Option<T>,
}
impl<T: BaseFloat + Sync> MinMax<T> {
fn new() -> Self {
Self {
best_left: None,
best_right: None,
slope: MinMaxSlope::new(),
}
}
fn update(&mut self, pivot_x: T, candidate_x: T, candidate_slope: T, candidate_index: usize) {
if candidate_x < pivot_x {
if let Some(current_min) = self.best_left {
if ulps_eq!(¤t_min, &candidate_x) {
self.slope
.update_left(false, candidate_slope, candidate_index);
} else if current_min < candidate_x {
self.slope
.update_left(true, candidate_slope, candidate_index);
self.best_left = Some(candidate_x);
} else {
}
} else {
self.best_left = Some(candidate_x);
self.slope
.update_left(false, candidate_slope, candidate_index);
}
} else if candidate_x > pivot_x {
if let Some(current_max) = self.best_right {
if ulps_eq!(¤t_max, &candidate_x) {
self.slope
.update_right(false, candidate_slope, candidate_index);
} else if current_max > candidate_x {
self.slope
.update_right(true, candidate_slope, candidate_index);
self.best_right = Some(candidate_x);
} else {
}
} else {
self.best_right = Some(candidate_x);
self.slope
.update_right(false, candidate_slope, candidate_index);
}
}
}
fn clear(&mut self) {
self.best_left = None;
self.best_right = None;
self.slope.clear();
}
}
struct MinMaxSlope<T: BaseFloat + Sync> {
best_left: Option<T>, candidates_left: Vec<usize>,
best_right: Option<T>, candidates_right: Vec<usize>,
}
impl<T: BaseFloat + Sync> MinMaxSlope<T> {
fn new() -> Self {
Self {
best_left: None,
candidates_left: Vec::<usize>::new(),
best_right: None,
candidates_right: Vec::<usize>::new(),
}
}
fn update_both(&mut self, candidate_index: usize, lines: &[linestring_2d::Line2<T>]) {
let line = lines[candidate_index];
let candidate_slope = if ulps_eq!(&line.end.y, &line.start.y) {
T::infinity()
} else {
(line.end.x - line.start.x) / (line.end.y - line.start.y)
};
self.update_left(false, candidate_slope, candidate_index);
self.update_right(false, candidate_slope, candidate_index);
}
fn update_left(&mut self, clear: bool, candidate_slope: T, candidate_index: usize) {
if clear {
self.candidates_left.clear();
self.best_left = None;
}
if let Some(current_slope) = self.best_left {
if ulps_eq!(¤t_slope, &candidate_slope) {
self.candidates_left.push(candidate_index);
} else if candidate_slope < current_slope {
self.candidates_left.clear();
self.candidates_left.push(candidate_index);
self.best_left = Some(candidate_slope);
} else {
}
} else {
self.best_left = Some(candidate_slope);
self.candidates_left.push(candidate_index);
}
}
fn update_right(&mut self, clear: bool, candidate_slope: T, candidate_index: usize) {
if clear {
self.candidates_right.clear();
self.best_right = None;
}
if let Some(current_slope) = self.best_right {
if ulps_eq!(¤t_slope, &candidate_slope) {
self.candidates_right.push(candidate_index);
} else if candidate_slope > current_slope {
self.candidates_right.clear();
self.candidates_right.push(candidate_index);
self.best_right = Some(candidate_slope);
} else {
}
} else {
self.best_right = Some(candidate_slope);
self.candidates_right.push(candidate_index);
}
}
fn clear(&mut self) {
self.best_left = None;
self.candidates_left.clear();
self.best_right = None;
self.candidates_right.clear();
}
}
pub struct SiteEvent<T: BaseFloat + Sync> {
drop: Option<Vec<usize>>,
add: Option<Vec<usize>>,
intersection: Option<Vec<usize>>,
#[doc(hidden)]
pd: PhantomData<fn(T) -> T>,
}
impl<T: BaseFloat + Sync> SiteEvent<T> {
pub(crate) fn with_intersection(i: &[usize]) -> Self {
Self {
drop: None,
add: None,
intersection: Some(i.to_vec()),
pd: PhantomData,
}
}
pub(crate) fn with_drop(l: &[usize]) -> Self {
Self {
drop: Some(l.to_vec()),
add: None,
intersection: None,
pd: PhantomData,
}
}
pub(crate) fn with_add(l: &[usize]) -> Self {
Self {
drop: None,
add: Some(l.to_vec()),
intersection: None,
pd: PhantomData,
}
}
pub fn get_intersections(&self) -> &Option<Vec<usize>> {
&self.intersection
}
}
fn sweepline_intersection<T: BaseFloat + Sync>(
sweepline: Point2<T>,
other: &linestring_2d::Line2<T>,
) -> Option<(T, T)> {
let y1 = other.start.y;
let y2 = other.end.y;
let x1 = other.start.x;
let x2 = other.end.x;
if ulps_eq!(&y1, &y2) {
return (sweepline.x < x2).then(|| (x2, T::zero()));
}
if ulps_eq!(&x1, &x2) {
return Some((x1, T::infinity()));
}
let slope: T = (y2 - y1) / (x2 - x1);
let d = y1 - slope * x1;
Some(((sweepline.y - d) / slope, slope))
}
pub struct IntersectionData<T: BaseFloat + Sync> {
sweepline_pos: Point2<T>,
stop_at_first_intersection: bool,
pub ignore_end_point_intersections: bool,
site_events: Option<BTreeMap<SiteEventKey<T>, SiteEvent<T>>>,
lines: Vec<linestring_2d::Line2<T>>,
}
impl<T: BaseFloat + Sync> Default for IntersectionData<T> {
fn default() -> Self {
Self {
sweepline_pos: Point2 {
x: -T::max_value(),
y: -T::max_value(),
},
stop_at_first_intersection: false,
ignore_end_point_intersections: false,
site_events: Some(BTreeMap::new()),
lines: Vec::<linestring_2d::Line2<T>>::new(),
}
}
}
impl<T: BaseFloat + Sync> IntersectionData<T> {
pub fn get_sweepline_pos(&self) -> &Point2<T> {
&self.sweepline_pos
}
pub fn get_lines(&self) -> &Vec<linestring_2d::Line2<T>> {
&self.lines
}
pub fn with_stop_at_first_intersection(mut self, value: bool) -> Result<Self, LinestringError> {
self.stop_at_first_intersection = value;
Ok(self)
}
pub fn with_ignore_end_point_intersections(
mut self,
value: bool,
) -> Result<Self, LinestringError> {
self.ignore_end_point_intersections = value;
Ok(self)
}
pub fn with_lines<IT>(mut self, input_iter: IT) -> Result<Self, LinestringError>
where
IT: Iterator<Item = linestring_2d::Line2<T>>,
{
let mut site_events = self.site_events.take().ok_or_else(|| {
LinestringError::InternalError("with_lines() could not take 'site_events'".to_string())
})?;
for (index, mut aline) in input_iter.enumerate() {
if !(aline.start.x.is_finite()
&& aline.start.y.is_finite()
&& aline.end.x.is_finite()
&& aline.end.y.is_finite())
{
return Err(LinestringError::InvalidData(
"Some of the points are infinite".to_string(),
));
}
if !(SiteEventKey { pos: aline.start }).lt(&(SiteEventKey { pos: aline.end })) {
std::mem::swap(&mut aline.start, &mut aline.end);
};
self.lines.push(aline);
let key_start = SiteEventKey { pos: aline.start };
let key_end = SiteEventKey { pos: aline.end };
if let Some(mut event) = site_events.get_mut(&key_start) {
let mut lower = event.add.take().map_or(Vec::<usize>::new(), identity);
lower.push(index);
event.add = Some(lower);
} else {
let event = SiteEvent::<T>::with_add(&[index]);
let _ = site_events.insert(key_start, event);
}
if let Some(mut event) = site_events.get_mut(&key_end) {
let mut upper = event.drop.take().map_or(Vec::<usize>::new(), identity);
upper.push(index);
event.drop = Some(upper);
} else {
let event = SiteEvent::<T>::with_drop(&[index]);
let _ = site_events.insert(key_end, event);
}
}
self.site_events = Some(site_events);
#[cfg(feature = "console_trace")]
self.debug();
Ok(self)
}
#[inline]
fn add_intersection_event(
&self,
site_events: &mut BTreeMap<SiteEventKey<T>, SiteEvent<T>>,
position: &SiteEventKey<T>,
intersecting_lines: &[usize],
) {
if let Some(event) = site_events.get_mut(position) {
let mut intersections_added = 0;
for new_intersection in intersecting_lines.iter() {
let i_line = self.lines[*new_intersection];
if linestring_2d::point_ulps_eq(position.pos, i_line.start)
|| linestring_2d::point_ulps_eq(position.pos, i_line.end)
{
continue;
}
if let Some(prev_intersections) = &mut event.intersection {
prev_intersections.push(*new_intersection);
} else {
let new_vec = vec![*new_intersection];
event.intersection = Some(new_vec);
}
intersections_added += 1;
}
if intersections_added > 0 {
let mut intersections = event.intersection.take().unwrap();
intersections.sort_unstable();
intersections.dedup();
event.intersection = Some(intersections);
}
} else {
let event = SiteEvent::with_intersection(intersecting_lines);
let _ = site_events.insert(*position, event);
}
}
#[allow(unused_assignments)]
#[allow(clippy::type_complexity)]
pub fn compute(
mut self,
) -> Result<impl ExactSizeIterator<Item = (Point2<T>, Vec<usize>)>, LinestringError> {
let mut active_lines = AHashSet::<usize>::default();
let mut site_events = self.site_events.take().ok_or_else(|| {
LinestringError::InternalError("compute() could not take 'site_events'".to_string())
})?;
let mut result: BTreeMap<SiteEventKey<T>, Vec<usize>> = BTreeMap::default();
let mut neighbour_priority = MinMax::<T>::new();
let mut connected_priority = MinMaxSlope::<T>::new();
loop {
if let Some((key, event)) = {
#[cfg(feature = "map_first_last")]
{
site_events.pop_first()
}
#[cfg(not(feature = "map_first_last"))]
{
if let Some((first_key, _)) = site_events.iter().next() {
let first_key = *first_key;
let v = site_events.remove(&first_key).ok_or_else(|| {
LinestringError::InternalError(format!(
"Found a key to pop but could not remove the value. {}:{}",
file!(),
line!()
))
})?;
Some((first_key, v))
} else {
None
}
}
} {
self.handle_event(
&key,
&event,
&mut active_lines,
&mut neighbour_priority,
&mut connected_priority,
&mut site_events,
&mut result,
)?;
if !result.is_empty() && self.stop_at_first_intersection {
break;
}
} else {
self.sweepline_pos = Point2::new(T::max_value(), T::max_value());
break;
}
}
Ok(result.into_iter().map(|x| (x.0.pos, x.1)))
}
#[inline(always)]
#[allow(clippy::too_many_arguments)]
fn handle_event(
&mut self,
key: &SiteEventKey<T>,
event: &SiteEvent<T>,
active_lines: &mut AHashSet<usize>,
neighbour_priority: &mut MinMax<T>,
connected_priority: &mut MinMaxSlope<T>,
site_events: &mut BTreeMap<SiteEventKey<T>, SiteEvent<T>>,
result: &mut BTreeMap<SiteEventKey<T>, Vec<usize>>,
) -> Result<(), LinestringError> {
self.sweepline_pos = key.pos;
let removed_active_lines = event.drop.iter().flatten().count();
let added_active_lines = event.add.iter().flatten().count();
let intersections_found = event.intersection.iter().flatten().count();
#[cfg(feature = "console_trace")]
{
println!("*************************************");
print!(
"handle_event() sweepline=({:?},{:?})",
self.sweepline_pos.x, self.sweepline_pos.y,
);
print!(
", drop={:?}",
event.drop.iter().flatten().collect::<Vec<&usize>>()
);
print!(
", add={:?}",
event.add.iter().flatten().collect::<Vec<&usize>>()
);
print!(
", intersection={:?}",
event.intersection.iter().flatten().collect::<Vec<&usize>>()
);
println!(
", active:{:?}",
active_lines.iter().collect::<Vec<&usize>>()
);
}
if self.ignore_end_point_intersections {
if intersections_found > 0 {
if removed_active_lines > 0 {
self.report_intersections_to_result(
result,
self.sweepline_pos,
event.drop.iter().flatten(),
);
}
if added_active_lines > 0 {
self.report_intersections_to_result(
result,
self.sweepline_pos,
event.add.iter().flatten(),
);
}
self.report_intersections_to_result(
result,
self.sweepline_pos,
event.intersection.iter().flatten(),
);
}
} else if removed_active_lines + added_active_lines + intersections_found > 1 {
if removed_active_lines > 0 {
self.report_intersections_to_result(
result,
self.sweepline_pos,
event.drop.iter().flatten(),
);
}
if added_active_lines > 0 {
self.report_intersections_to_result(
result,
self.sweepline_pos,
event.add.iter().flatten(),
);
}
if intersections_found > 0 {
self.report_intersections_to_result(
result,
self.sweepline_pos,
event.intersection.iter().flatten(),
);
}
}
for line_index in event.drop.iter().flatten() {
let _ = active_lines.remove(line_index);
}
neighbour_priority.clear();
'active_lines: for line_index in active_lines.iter() {
for i in event.intersection.iter().flatten() {
if i == line_index {
continue 'active_lines;
}
}
if let Some((intersection_x, intersection_slope)) =
sweepline_intersection(self.sweepline_pos, self.lines.get(*line_index).unwrap())
{
neighbour_priority.update(
self.sweepline_pos.x,
intersection_x,
intersection_slope,
*line_index,
);
}
}
for l in event.add.iter().flatten() {
let _ = active_lines.insert(*l);
}
if intersections_found + added_active_lines == 0 {
#[cfg(feature = "console_trace")]
{
println!(
"neighbours left: {:?}",
neighbour_priority.slope.candidates_left
);
println!(
"neighbours right: {:?}",
neighbour_priority.slope.candidates_right
);
}
if !neighbour_priority.slope.candidates_left.is_empty()
&& !neighbour_priority.slope.candidates_right.is_empty()
{
self.find_new_events(
&neighbour_priority.slope.candidates_left,
&neighbour_priority.slope.candidates_right,
site_events,
)?;
}
} else {
connected_priority.clear();
for l in event.add.iter().flatten() {
connected_priority.update_both(*l, &self.lines);
}
for l in event.intersection.iter().flatten() {
connected_priority.update_both(*l, &self.lines);
}
#[cfg(feature = "console_trace")]
{
println!(
"left connected_priority candidates {:?}",
connected_priority.candidates_left
);
println!(
"right connected_priority candidates {:?}",
connected_priority.candidates_right
);
}
if !neighbour_priority.slope.candidates_left.is_empty() {
#[cfg(feature = "console_trace")]
println!(
"left neighbour_priority candidates {:?}",
neighbour_priority.slope.candidates_left
);
self.find_new_events(
&neighbour_priority.slope.candidates_left,
&connected_priority.candidates_left,
site_events,
)?;
}
if !neighbour_priority.slope.candidates_right.is_empty() {
#[cfg(feature = "console_trace")]
println!(
"right neighbour_priority candidates {:?}",
neighbour_priority.slope.candidates_right
);
self.find_new_events(
&neighbour_priority.slope.candidates_right,
&connected_priority.candidates_right,
site_events,
)?;
}
}
#[cfg(feature = "console_trace")]
{
println!("Post active lines: {:?}", active_lines);
println!();
}
Ok(())
}
fn find_new_events(
&mut self,
left: &[usize],
right: &[usize],
site_events: &mut BTreeMap<SiteEventKey<T>, SiteEvent<T>>,
) -> Result<(), LinestringError> {
for left_i in left.iter() {
for right_i in right.iter() {
let left_l = self.lines[*left_i];
let right_l = self.lines[*right_i];
if linestring_2d::point_ulps_eq(left_l.end, right_l.end) {
continue;
}
#[cfg(feature = "console_trace")]
print!("testing intersection between {} and {}: ", left_i, right_i);
if let Some(intersection_p) = left_l.intersection_point(right_l) {
let intersection_p = intersection_p.single();
if !intersection_p.x.is_finite() || !intersection_p.y.is_finite() {
return Err(LinestringError::InvalidData(format!(
"Input data has intersection at invalid position x:{:?}, y:{:?}",
intersection_p.x, intersection_p.y
)));
}
if intersection_p.y >= self.sweepline_pos.y
&& !(intersection_p.y == self.sweepline_pos.y
&& intersection_p.x < self.sweepline_pos.x)
&& !linestring_2d::point_ulps_eq(intersection_p, self.sweepline_pos)
{
#[cfg(feature = "console_trace")]
println!(
"Lines {:?} and {:?} intersects at {:?}",
left_i, right_i, intersection_p
);
self.add_intersection_event(
site_events,
&SiteEventKey {
pos: intersection_p,
},
&[*left_i, *right_i],
)
}
} else {
#[cfg(feature = "console_trace")]
println!(" no intersection");
}
}
}
Ok(())
}
fn report_intersections_to_result<'a, I: Iterator<Item = &'a usize>>(
&mut self,
result: &mut BTreeMap<SiteEventKey<T>, Vec<usize>>,
pos: Point2<T>,
intersecting_lines: I,
) {
let key = SiteEventKey { pos };
let value = if let Some(value) = result.get_mut(&key) {
value
} else {
let _ = result.insert(key, Vec::default());
result.get_mut(&key).unwrap()
};
for line in intersecting_lines {
value.push(*line);
#[cfg(feature = "console_trace")]
println!("Reported an intersection {:?} for line #{}", pos, line);
}
value.sort_unstable();
}
#[cfg(feature = "console_trace")]
fn debug(&self) {
println!("Stored item in this order:");
for k in self.site_events.as_ref().unwrap().iter().map(|kv| *kv.0) {
print!("{:?}, ", k);
}
println!();
}
}