use super::linestring_3d;
#[allow(unused_imports)]
use crate::LinestringError;
use cgmath::{ulps_eq, InnerSpace, MetricSpace, Point2, Point3, Transform, Vector2};
use itertools::Itertools;
#[allow(unused_imports)]
use rayon::prelude::*;
use std::collections;
use std::fmt;
pub mod convex_hull;
pub mod intersection;
pub enum Shape2d<T: cgmath::BaseFloat + Sync> {
Line(Line2<T>),
Linestring(LineString2<T>),
ParabolicArc(VoronoiParabolicArc<T>),
}
#[derive(PartialEq, Eq, Copy, Clone, Hash, fmt::Debug)]
pub struct Line2<T: cgmath::BaseFloat + Sync> {
pub start: Point2<T>,
pub end: Point2<T>,
}
impl<T: cgmath::BaseFloat + Sync> Line2<T> {
pub fn new(start: Point2<T>, end: Point2<T>) -> Self {
Self { start, end }
}
#[allow(clippy::many_single_char_names)]
#[allow(clippy::suspicious_operation_groupings)]
pub fn intersection_point(self, other: Self) -> Option<Intersection<T>> {
if self.end.x > other.end.x
&& self.end.x > other.start.x
&& self.start.x > other.end.x
&& self.start.x > other.start.x
{
return None;
}
if self.end.x < other.end.x
&& self.end.x < other.start.x
&& self.start.x < other.end.x
&& self.start.x < other.start.x
{
return None;
}
if self.end.y > other.end.y
&& self.end.y > other.start.y
&& self.start.y > other.end.y
&& self.start.y > other.start.y
{
return None;
}
if self.end.y < other.end.y
&& self.end.y < other.start.y
&& self.start.y < other.end.y
&& self.start.y < other.start.y
{
return None;
}
let p = self.start;
let q = other.start;
let r = self.end - p;
let s = other.end - q;
let r_cross_s = cross_z(r, s);
let q_minus_p = q - p;
let q_minus_p_cross_r = cross_z(q_minus_p, r);
if ulps_eq!(r_cross_s, T::zero()) {
let self_is_a_point = point_ulps_eq(self.start, self.end);
let other_is_a_point = point_ulps_eq(other.start, other.end);
if self_is_a_point || other_is_a_point {
if self_is_a_point && other_is_a_point && point_ulps_eq(self.start, other.start) {
return Some(Intersection::Intersection(self.start));
}
return if self_is_a_point {
intersect_line_point(other, self.start)
} else {
intersect_line_point(self, other.start)
};
}
if ulps_eq!(&q_minus_p_cross_r, &T::zero()) {
let r_dot_r = r.dot(r);
let r_div_r_dot_r = r / r_dot_r;
let s_dot_r = s.dot(r);
let t0 = q_minus_p.dot(r_div_r_dot_r);
let t1 = t0 + s_dot_r / r_dot_r;
Some(Intersection::OverLap(Line2::new(
scale_to_coordinate(p, r, t0),
scale_to_coordinate(p, r, t1),
)))
} else {
None
}
} else {
let t = cross_z(q_minus_p, s / r_cross_s);
let u = cross_z(q_minus_p, r / r_cross_s);
if T::zero() <= t && t <= T::one() && T::zero() <= u && u <= T::one() {
Some(Intersection::Intersection(scale_to_coordinate(p, r, t)))
} else {
None
}
}
}
pub fn intersection_point3(
start: Point2<T>,
middle: Point2<T>,
end: Point2<T>,
) -> Result<Option<Intersection<T>>, LinestringError> {
let middle_sub_start = Vector2 {
x: middle.x - start.x,
y: middle.y - start.x,
};
let middle_sub_end = Vector2 {
x: middle.x - end.x,
y: middle.y - end.y,
};
let start_is_vertical = ulps_eq!(&middle_sub_start.x, &T::zero());
let end_is_vertical = ulps_eq!(&middle_sub_end.x, &T::zero());
if start_is_vertical && end_is_vertical {
if middle_sub_start.y.is_sign_negative() && !middle_sub_end.y.is_sign_negative() {
return Ok(None);
} else {
if (middle_sub_start.x * middle_sub_start.x
+ middle_sub_start.y * middle_sub_start.y)
< (middle_sub_end.x * middle_sub_end.x + middle_sub_end.y * middle_sub_end.y)
{
return Ok(Some(Intersection::OverLap(Line2 { start, end: middle })));
} else {
return Ok(Some(Intersection::OverLap(Line2 { start: middle, end })));
}
}
} else if start_is_vertical || end_is_vertical {
return Ok(None);
}
let start_slope = middle_sub_start.x / middle_sub_start.y;
let end_slope = middle_sub_end.x / middle_sub_end.y;
if !ulps_eq!(&start_slope, &end_slope) {
return Ok(None);
}
if (middle_sub_start.x * middle_sub_start.x + middle_sub_start.y * middle_sub_start.y)
< (middle_sub_end.x * middle_sub_end.x + middle_sub_end.y * middle_sub_end.y)
{
Ok(Some(Intersection::OverLap(Line2 { start, end: middle })))
} else {
Ok(Some(Intersection::OverLap(Line2 { start: middle, end })))
}
}
pub fn triangle_area_squared_times_4(p1: Point2<T>, p2: Point2<T>, p3: Point2<T>) -> T {
let area =
p1.x * p2.y + p2.x * p3.y + p3.x * p1.y - p1.x * p3.y - p2.x * p1.y - p3.x * p2.y;
area * area
}
pub fn copy_to_3d(&self, plane: linestring_3d::Plane) -> linestring_3d::Line3<T> {
linestring_3d::Line3::new(plane.point_to_3d(self.start), plane.point_to_3d(self.end))
}
}
#[allow(clippy::from_over_into)]
impl<T: cgmath::BaseFloat + Sync> Into<[T; 4]> for Line2<T> {
fn into(self) -> [T; 4] {
[self.start.x, self.start.y, self.end.x, self.end.y]
}
}
impl<T, IT> From<[IT; 2]> for Line2<T>
where
T: cgmath::BaseFloat + Sync,
IT: Copy + Into<Point2<T>>,
{
fn from(coordinate: [IT; 2]) -> Line2<T> {
Line2::<T>::new(coordinate[0].into(), coordinate[1].into())
}
}
impl<T: cgmath::BaseFloat + Sync> From<[T; 4]> for Line2<T> {
fn from(coordinate: [T; 4]) -> Line2<T> {
Line2::<T>::new(
[coordinate[0], coordinate[1]].into(),
[coordinate[2], coordinate[3]].into(),
)
}
}
#[derive(Clone, Hash, fmt::Debug)]
pub struct VoronoiParabolicArc<T: cgmath::BaseFloat + Sync> {
pub segment: Line2<T>,
pub cell_point: Point2<T>,
pub start_point: Point2<T>,
pub end_point: Point2<T>,
}
impl<T: cgmath::BaseFloat + Sync> VoronoiParabolicArc<T> {
pub fn new(
segment: Line2<T>,
cell_point: Point2<T>,
start_point: Point2<T>,
end_point: Point2<T>,
) -> Self {
Self {
segment,
cell_point,
start_point,
end_point,
}
}
pub fn discretise_2d(&self, max_dist: T) -> LineString2<T> {
let mut rv = LineString2::default().with_connected(false);
rv.points.push(self.start_point);
let segm_vec_x = self.segment.end.x - self.segment.start.x;
let segm_vec_y = self.segment.end.y - self.segment.start.y;
let sqr_segment_length = segm_vec_x * segm_vec_x + segm_vec_y * segm_vec_y;
let projection_start =
sqr_segment_length * Self::point_projection(self.start_point, self.segment);
let projection_end =
sqr_segment_length * Self::point_projection(self.end_point, self.segment);
let point_vec_x = self.cell_point.x - self.segment.start.x;
let point_vec_y = self.cell_point.y - self.segment.start.y;
let rot_x = segm_vec_x * point_vec_x + segm_vec_y * point_vec_y;
let rot_y = segm_vec_x * point_vec_y - segm_vec_y * point_vec_x;
let mut point_stack = vec![projection_end];
let mut cur_x = projection_start;
let mut cur_y = Self::parabola_y(cur_x, rot_x, rot_y);
let max_dist_transformed = max_dist * max_dist * sqr_segment_length;
while !point_stack.is_empty() {
let new_x = point_stack[point_stack.len() - 1]; let new_y = Self::parabola_y(new_x, rot_x, rot_y);
let mid_x = (new_y - cur_y) / (new_x - cur_x) * rot_y + rot_x;
let mid_y = Self::parabola_y(mid_x, rot_x, rot_y);
let mut dist = (new_y - cur_y) * (mid_x - cur_x) - (new_x - cur_x) * (mid_y - cur_y);
#[allow(clippy::suspicious_operation_groupings)]
{
dist = dist * dist
/ ((new_y - cur_y) * (new_y - cur_y) + (new_x - cur_x) * (new_x - cur_x));
}
if dist <= max_dist_transformed {
let _ = point_stack.pop();
let inter_x = (segm_vec_x * new_x - segm_vec_y * new_y) / sqr_segment_length
+ self.segment.start.x;
let inter_y = (segm_vec_x * new_y + segm_vec_y * new_x) / sqr_segment_length
+ self.segment.start.y;
let p = Point2 {
x: inter_x,
y: inter_y,
};
rv.points.push(p);
cur_x = new_x;
cur_y = new_y;
} else {
point_stack.push(mid_x);
}
}
let last_position = rv.points.len() - 1;
rv.points[last_position] = self.end_point;
rv
}
pub fn discretise_3d_straight_line(&self) -> linestring_3d::LineString3<T> {
let mut rv = linestring_3d::LineString3::default().with_connected(false);
let distance =
-distance_to_line_squared_safe(self.segment.start, self.segment.end, self.start_point)
.sqrt();
rv.points
.push([self.start_point.x, self.start_point.y, distance].into());
let distance = -self.end_point.distance(self.cell_point);
rv.points
.push([self.end_point.x, self.end_point.y, distance].into());
rv
}
pub fn discretise_3d(&self, max_dist: T) -> linestring_3d::LineString3<T> {
let mut rv = linestring_3d::LineString3::default().with_connected(false);
let z_comp = -self.start_point.distance(self.cell_point);
rv.points
.push([self.start_point.x, self.start_point.y, z_comp].into());
let z_comp = -self.end_point.distance(self.cell_point);
rv.points
.push([self.end_point.x, self.end_point.y, z_comp].into());
let segm_vec_x = self.segment.end.x - self.segment.start.x;
let segm_vec_y = self.segment.end.y - self.segment.start.y;
let sqr_segment_length = segm_vec_x * segm_vec_x + segm_vec_y * segm_vec_y;
let projection_start =
sqr_segment_length * Self::point_projection_3d(&rv.points[0], &self.segment);
let projection_end =
sqr_segment_length * Self::point_projection_3d(&rv.points[1], &self.segment);
let point_vec_x = self.cell_point.x - self.segment.start.x;
let point_vec_y = self.cell_point.y - self.segment.start.y;
let rot_x = segm_vec_x * point_vec_x + segm_vec_y * point_vec_y;
let rot_y = segm_vec_x * point_vec_y - segm_vec_y * point_vec_x;
let last_point = (*rv.points)[1];
let _ = rv.points.pop();
let mut point_stack = vec![projection_end];
let mut cur_x = projection_start;
let mut cur_y = Self::parabola_y(cur_x, rot_x, rot_y);
let max_dist_transformed = max_dist * max_dist * sqr_segment_length;
while !point_stack.is_empty() {
let new_x = point_stack[point_stack.len() - 1]; let new_y = Self::parabola_y(new_x, rot_x, rot_y);
let mid_x = (new_y - cur_y) / (new_x - cur_x) * rot_y + rot_x;
let mid_y = Self::parabola_y(mid_x, rot_x, rot_y);
let mut dist = (new_y - cur_y) * (mid_x - cur_x) - (new_x - cur_x) * (mid_y - cur_y);
#[allow(clippy::suspicious_operation_groupings)]
{
dist = dist * dist
/ ((new_y - cur_y) * (new_y - cur_y) + (new_x - cur_x) * (new_x - cur_x));
}
if dist <= max_dist_transformed {
let _ = point_stack.pop();
let inter_x = (segm_vec_x * new_x - segm_vec_y * new_y) / sqr_segment_length
+ self.segment.start.x;
let inter_y = (segm_vec_x * new_y + segm_vec_y * new_x) / sqr_segment_length
+ self.segment.start.y;
let z_comp = -Point2::new(inter_x, inter_y).distance(self.cell_point);
rv.points.push(Point3::new(inter_x, inter_y, z_comp));
cur_x = new_x;
cur_y = new_y;
} else {
point_stack.push(mid_x);
}
}
let last_position = rv.points.len() - 1;
rv.points[last_position] = last_point;
rv
}
#[inline(always)]
#[allow(clippy::suspicious_operation_groupings)]
fn parabola_y(x: T, a: T, b: T) -> T {
((x - a) * (x - a) + b * b) / (b + b)
}
#[inline(always)]
fn point_projection(point: Point2<T>, segment: Line2<T>) -> T {
let segment_vec_x = segment.end.x - segment.start.x;
let segment_vec_y = segment.end.y - segment.start.y;
let point_vec_x = point.x - segment.start.x;
let point_vec_y = point.y - segment.start.y;
let sqr_segment_length = segment_vec_x * segment_vec_x + segment_vec_y * segment_vec_y;
let vec_dot = segment_vec_x * point_vec_x + segment_vec_y * point_vec_y;
vec_dot / sqr_segment_length
}
fn point_projection_3d(point: &Point3<T>, segment: &Line2<T>) -> T {
let segment_vec_x = segment.end.x - segment.start.x;
let segment_vec_y = segment.end.y - segment.start.y;
let point_vec_x = point.x - segment.start.x;
let point_vec_y = point.y - segment.start.y;
let sqr_segment_length = segment_vec_x * segment_vec_x + segment_vec_y * segment_vec_y;
let vec_dot = segment_vec_x * point_vec_x + segment_vec_y * point_vec_y;
vec_dot / sqr_segment_length
}
}
#[derive(PartialEq, Eq, Clone, Hash, fmt::Debug)]
pub struct LineString2<T>
where
T: cgmath::BaseFloat + Sync,
{
pub(crate) points: Vec<Point2<T>>,
pub connected: bool,
}
impl<T> Default for LineString2<T>
where
T: cgmath::BaseFloat + Sync,
{
#[inline]
fn default() -> Self {
Self {
points: Vec::<Point2<T>>::new(),
connected: false,
}
}
}
struct PriorityDistance<T> {
key: T,
index: usize,
}
impl<T> PartialOrd for PriorityDistance<T>
where
T: PartialOrd + PartialEq + cgmath::UlpsEq,
{
#[inline(always)]
fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
Some(self.cmp(other))
}
}
impl<T> Ord for PriorityDistance<T>
where
T: PartialOrd + PartialEq + cgmath::UlpsEq,
{
#[inline(always)]
fn cmp(&self, other: &Self) -> std::cmp::Ordering {
self.key.partial_cmp(&other.key).unwrap().reverse()
}
}
impl<T> PartialEq for PriorityDistance<T>
where
T: PartialOrd + PartialEq + cgmath::UlpsEq,
{
#[inline(always)]
fn eq(&self, other: &Self) -> bool {
ulps_eq!(&self.key, &other.key)
}
}
impl<T> Eq for PriorityDistance<T> where T: PartialOrd + PartialEq + cgmath::UlpsEq {}
impl<T> LineString2<T>
where
T: cgmath::BaseFloat + Sync,
{
pub fn with_capacity(capacity: usize) -> Self {
Self {
points: Vec::<Point2<T>>::with_capacity(capacity),
connected: false,
}
}
pub fn with_points(mut self, points: Vec<Point2<T>>) -> Self {
self.points = points;
self
}
pub fn with_connected(mut self, connected: bool) -> Self {
self.connected = connected;
self
}
pub fn with_iter<'a, I>(iter: I) -> Self
where
T: 'a,
I: Iterator<Item = &'a Point2<T>>,
{
Self {
points: iter.into_iter().copied().collect(),
connected: false,
}
}
pub fn append(&mut self, mut other: Self) {
self.points.append(&mut other.points);
}
pub fn is_self_intersecting(&self) -> Result<bool, LinestringError> {
if self.points.len() <= 2 {
Ok(false)
} else if self.points.len() < 10 {
for l0 in self
.as_lines_iter()
.enumerate()
.take(self.as_lines_iter_len() - 1)
{
for l1 in self.as_lines_iter().enumerate().skip(l0.0 + 1) {
if l0.0 == l1.0 {
continue;
} else if l0.0 + 1 == l1.0 {
if Line2::intersection_point3(l0.1.start, l0.1.end, l1.1.end)?.is_some() {
return Ok(true);
}
} else if let Some(point) = l0.1.intersection_point(l1.1) {
let point = point.single();
if (point_ulps_eq(point, l0.1.start) || point_ulps_eq(point, l0.1.end))
&& (point_ulps_eq(point, l1.1.start) || point_ulps_eq(point, l1.1.end))
{
continue;
} else {
return Ok(true);
}
}
}
}
Ok(false)
} else {
let result = intersection::IntersectionData::<T>::default()
.with_ignore_end_point_intersections(true)?
.with_stop_at_first_intersection(true)?
.with_lines(self.as_lines_iter())?
.compute()?;
Ok(result.len() > 0)
}
}
pub fn points(&self) -> &Vec<Point2<T>> {
&self.points
}
pub fn len(&self) -> usize {
self.points.len()
}
pub fn is_empty(&self) -> bool {
self.points.is_empty()
}
#[inline(always)]
pub fn as_lines(&self) -> Vec<Line2<T>> {
self.as_lines_iter().collect()
}
#[must_use = "iterator adaptors are lazy and do nothing unless consumed"]
pub fn as_lines_iter<'a>(&'a self) -> Box<dyn Iterator<Item = Line2<T>> + 'a> {
if self.connected {
Box::new(
self.points
.iter()
.chain(self.points.first())
.tuple_windows::<(_, _)>()
.map(|(a, b)| Line2 { start: *a, end: *b }),
)
} else {
Box::new(
self.points
.iter()
.tuple_windows::<(_, _)>()
.map(|(a, b)| Line2 { start: *a, end: *b }),
)
}
}
pub fn as_lines_iter_len(&self) -> usize {
if self.points.len() < 2 {
return 0;
}
if self.connected {
self.points.len()
} else {
self.points.len() - 1
}
}
pub fn copy_to_3d(&self, plane: linestring_3d::Plane) -> linestring_3d::LineString3<T> {
let mut rv: linestring_3d::LineString3<T> = match plane {
linestring_3d::Plane::XY => self
.points
.iter()
.map(|p2d| Point3::new(p2d.x, p2d.y, T::zero()))
.collect(),
linestring_3d::Plane::XZ => self
.points
.iter()
.map(|p2d| Point3::new(p2d.x, T::zero(), p2d.y))
.collect(),
linestring_3d::Plane::YZ => self
.points
.iter()
.map(|p2d| Point3::new(T::zero(), p2d.x, p2d.y))
.collect(),
};
rv.connected = self.connected;
rv
}
pub fn push(&mut self, point: Point2<T>) {
self.points.push(point);
}
pub fn transform(&self, matrix3x3: &cgmath::Matrix3<T>) -> Self {
Self {
points: self
.points
.iter()
.map(|x| matrix3x3.transform_point(*x))
.collect(),
connected: self.connected,
}
}
pub fn simplify(&self, distance_predicate: T) -> Self {
if self.points.len() <= 2 {
return self.clone();
}
if self.connected {
let mut points = self.points.clone();
points.push(*points.first().unwrap());
let mut rv: Vec<Point2<T>> = Vec::with_capacity(points.len());
rv.push(*points.first().unwrap());
rv.append(&mut Self::_simplify(
distance_predicate * distance_predicate,
points.as_slice(),
));
let _ = rv.remove(rv.len() - 1);
Self {
points: rv,
connected: true,
}
} else {
let mut rv: Vec<Point2<T>> = Vec::with_capacity(self.points.len());
rv.push(*self.points.first().unwrap());
rv.append(&mut Self::_simplify(
distance_predicate * distance_predicate,
self.points.as_slice(),
));
Self {
points: rv,
connected: false,
}
}
}
fn _simplify(distance_predicate_sq: T, slice: &[Point2<T>]) -> Vec<Point2<T>> {
if slice.len() <= 2 {
return slice[1..].to_vec();
}
let start_point = *slice.first().unwrap();
let end_point = *slice.last().unwrap();
let mut max_dist_sq = (-T::one(), 0_usize);
let mut found_something = false;
for (i, point) in slice.iter().enumerate().take(slice.len() - 1).skip(1) {
let sq_d = distance_to_line_squared_safe(start_point, end_point, *point);
if sq_d > max_dist_sq.0 && sq_d > distance_predicate_sq {
max_dist_sq = (sq_d, i);
found_something = true;
}
}
if !found_something {
return vec![end_point];
}
let mut rv = Self::_simplify(distance_predicate_sq, &slice[..max_dist_sq.1 + 1]);
rv.append(&mut Self::_simplify(
distance_predicate_sq,
&slice[max_dist_sq.1..],
));
rv
}
pub fn simplify_vw(&self, points_to_delete: usize) -> Self {
if (self.connected && self.points.len() <= 1) || (!self.connected && self.points.len() <= 2)
{
return self.clone();
}
let mut search_tree = collections::binary_heap::BinaryHeap::<PriorityDistance<T>>::new();
let mut link_tree = ahash::AHashMap::<usize, (usize, usize, T)>::default();
{
let mut iter_i = self.points.iter().enumerate();
let mut iter_j = self.points.iter().enumerate().skip(1);
for k in self.points.iter().enumerate().skip(2) {
let i = iter_i.next().unwrap();
let j = iter_j.next().unwrap();
let area = Line2::triangle_area_squared_times_4(*i.1, *j.1, *k.1);
let _ = search_tree.push(PriorityDistance {
key: area,
index: j.0,
});
let _ = link_tree.insert(j.0, (i.0, k.0, area));
}
}
if self.connected {
let i = self.points.len() - 2;
let j = self.points.len() - 1;
let k = self.points.len();
let area = Line2::triangle_area_squared_times_4(
self.points[i],
self.points[j],
self.points[0],
);
let _ = search_tree.push(PriorityDistance {
key: area,
index: j,
});
let _ = link_tree.insert(j, (i, k, area));
}
let self_points_len = self.points.len();
let mut deleted_nodes: usize = 0;
loop {
if search_tree.is_empty() || deleted_nodes >= points_to_delete {
break;
}
if let Some(smallest) = search_tree.pop() {
if let Some(old_links) = link_tree.get(&smallest.index).copied() {
let area = old_links.2;
if smallest.key != area {
continue;
} else {
let _ = link_tree.remove(&smallest.index);
}
deleted_nodes += 1;
let prev = old_links.0;
let next = old_links.1;
let prev_prev: Option<usize> = link_tree.get(&prev).map(|link| link.0);
let next_next: Option<usize> = link_tree.get(&next).map(|link| link.1);
if let Some(next_next) = next_next {
if let Some(prev_prev) = prev_prev {
let area = Line2::triangle_area_squared_times_4(
self.points[prev],
self.points[next % self_points_len],
self.points[next_next % self_points_len],
);
let _ = search_tree.push(PriorityDistance {
key: area,
index: next,
});
let _ = link_tree.insert(next, (prev, next_next, area));
let area = Line2::triangle_area_squared_times_4(
self.points[prev_prev],
self.points[prev],
self.points[next % self_points_len],
);
let _ = search_tree.push(PriorityDistance {
key: area,
index: prev,
});
let _ = link_tree.insert(prev, (prev_prev, next, area));
continue;
}
}
if let Some(prev_prev) = prev_prev {
let area = Line2::triangle_area_squared_times_4(
self.points[prev_prev],
self.points[prev],
self.points[next % self_points_len],
);
let _ = search_tree.push(PriorityDistance {
key: area,
index: prev,
});
let _ = link_tree.insert(prev, (prev_prev, next, area));
continue;
};
if let Some(next_next) = next_next {
let area = Line2::triangle_area_squared_times_4(
self.points[prev],
self.points[next % self_points_len],
self.points[next_next % self_points_len],
);
let _ = search_tree.push(PriorityDistance {
key: area,
index: next,
});
let _ = link_tree.insert(next, (prev, next_next, area));
continue;
};
} else {
continue;
}
}
}
if !self.connected {
[0_usize]
.iter()
.copied()
.chain(
link_tree
.keys()
.sorted_unstable()
.copied()
.chain([self.points.len() - 1].iter().copied()),
)
.map(|x| self.points[x])
.collect::<Self>()
.with_connected(false)
} else {
[0_usize]
.iter()
.copied()
.chain(link_tree.keys().sorted_unstable().copied())
.map(|x| self.points[x])
.collect::<Self>()
.with_connected(true)
}
}
pub fn operation<F>(&mut self, f: F)
where
F: Fn(T) -> T,
{
for v in self.points.iter_mut() {
v.x = f(v.x);
v.y = f(v.y);
}
}
}
impl<T> From<Aabb2<T>> for LineString2<T>
where
T: cgmath::BaseFloat + Sync,
{
fn from(other: Aabb2<T>) -> Self {
if let Some(min_max) = other.min_max {
let points = vec![
min_max.0,
Point2 {
x: min_max.1.x,
y: min_max.0.y,
},
min_max.1,
Point2 {
x: min_max.0.x,
y: min_max.1.y,
},
];
Self {
points,
connected: true,
}
} else {
Self {
points: Vec::<Point2<T>>::new(),
connected: false,
}
}
}
}
impl<T, IC> FromIterator<IC> for LineString2<T>
where
T: cgmath::BaseFloat + Sync,
IC: Into<Point2<T>>,
{
fn from_iter<I: IntoIterator<Item = IC>>(iter: I) -> Self {
LineString2 {
points: iter.into_iter().map(|c| c.into()).collect(),
connected: false,
}
}
}
#[derive(PartialEq, Eq, Clone, Hash)]
pub struct LineStringSet2<T>
where
T: cgmath::BaseFloat + Sync,
{
set: Vec<LineString2<T>>,
aabb: Aabb2<T>,
convex_hull: Option<LineString2<T>>,
pub internals: Option<Vec<(Aabb2<T>, LineString2<T>)>>,
}
impl<T> Default for LineStringSet2<T>
where
T: cgmath::BaseFloat + Sync,
{
#[inline]
fn default() -> Self {
Self {
set: Vec::<LineString2<T>>::new(),
aabb: Aabb2::default(),
convex_hull: None,
internals: None,
}
}
}
impl<T: cgmath::BaseFloat + Sync> LineStringSet2<T> {
pub fn steal_from(other: &mut LineStringSet2<T>) -> Self {
let mut set = Vec::<LineString2<T>>::new();
set.append(&mut other.set);
Self {
set,
aabb: other.aabb,
convex_hull: other.convex_hull.take(),
internals: other.internals.take(),
}
}
pub fn set(&self) -> &Vec<LineString2<T>> {
&self.set
}
pub fn with_capacity(capacity: usize) -> Self {
Self {
set: Vec::<LineString2<T>>::with_capacity(capacity),
aabb: Aabb2::default(),
convex_hull: None,
internals: None,
}
}
pub fn get_internals(&self) -> Option<&Vec<(Aabb2<T>, LineString2<T>)>> {
self.internals.as_ref()
}
pub fn is_empty(&self) -> bool {
self.set.is_empty()
}
pub fn push(&mut self, ls: LineString2<T>) {
if !ls.is_empty() {
self.set.push(ls);
for ls in self.set.last().unwrap().points.iter() {
self.aabb.update_point(*ls);
}
}
}
pub fn get_convex_hull(&self) -> &Option<LineString2<T>> {
&self.convex_hull
}
pub fn calculate_convex_hull(&mut self) -> &LineString2<T> {
self.convex_hull = Some(convex_hull::ConvexHull::graham_scan(
self.set.iter().map(|x| x.points()).flatten(),
));
self.convex_hull.as_ref().unwrap()
}
pub fn get_aabb(&self) -> Aabb2<T> {
self.aabb
}
pub fn transform(&self, matrix3x3: &cgmath::Matrix3<T>) -> Self {
let internals = self.internals.as_ref().map(|internals| {
internals
.iter()
.map(|x| (x.0.transform(matrix3x3), x.1.transform(matrix3x3)))
.collect()
});
let convex_hull = self
.convex_hull
.as_ref()
.map(|convex_hull| convex_hull.transform(matrix3x3));
Self {
aabb: self.aabb.transform(matrix3x3),
set: self.set.iter().map(|x| x.transform(matrix3x3)).collect(),
convex_hull,
internals,
}
}
pub fn copy_to_3d(&self, plane: linestring_3d::Plane) -> linestring_3d::LineStringSet3<T> {
let mut rv = linestring_3d::LineStringSet3::with_capacity(self.set.len());
for ls in self.set.iter() {
rv.push(ls.copy_to_3d(plane));
}
rv
}
pub fn take_from(&mut self, mut other: Self) {
self.aabb.update_aabb(other.aabb);
self.set.append(&mut other.set);
}
pub fn take_from_internal(&mut self, other: &mut Self) -> Result<(), LinestringError> {
if other.convex_hull.is_none() {
return Err(LinestringError::InvalidData(
"'other' did not contain a valid 'convex_hull' field".to_string(),
));
}
if self.aabb.get_low().is_none() {
return Err(LinestringError::InvalidData(
"'self' did not contain a valid 'aabb' field".to_string(),
));
}
if other.aabb.get_low().is_none() {
return Err(LinestringError::InvalidData(
"'other' did not contain a valid 'aabb' field".to_string(),
));
}
if !self.aabb.contains_aabb(other.aabb) {
return Err(LinestringError::InvalidData(
"The 'other.aabb' is not contained within 'self.aabb'".to_string(),
));
}
if self.internals.is_none() {
self.internals = Some(Vec::<(Aabb2<T>, LineString2<T>)>::new())
}
self.set.append(&mut other.set);
if let Some(ref mut other_internals) = other.internals {
self.internals.as_mut().unwrap().append(other_internals);
}
self.internals
.as_mut()
.unwrap()
.push((other.aabb, other.convex_hull.take().unwrap()));
Ok(())
}
pub fn operation<F: Fn(T) -> T>(&mut self, f: &F) {
for s in self.set.iter_mut() {
s.operation(f);
}
self.aabb.operation(f);
if let Some(ref mut convex_hull) = self.convex_hull {
convex_hull.operation(f);
}
if let Some(ref mut internals) = self.internals {
for i in internals.iter_mut() {
i.0.operation(f);
i.1.operation(f);
}
}
}
}
#[derive(PartialEq, Eq, Copy, Clone, Hash, fmt::Debug)]
pub struct Aabb2<T: cgmath::BaseFloat + Sync> {
min_max: Option<(Point2<T>, Point2<T>)>,
}
impl<T: cgmath::BaseFloat + Sync, IT: Copy + Into<Point2<T>>> From<[IT; 2]> for Aabb2<T> {
fn from(coordinate: [IT; 2]) -> Aabb2<T> {
let mut rv = Aabb2::<T>::default();
rv.update_point(coordinate[0].into());
rv.update_point(coordinate[1].into());
rv
}
}
impl<T: cgmath::BaseFloat + Sync> From<[T; 4]> for Aabb2<T> {
fn from(coordinate: [T; 4]) -> Aabb2<T> {
let mut rv = Aabb2::default();
rv.update_point(Point2 {
x: coordinate[0],
y: coordinate[1],
});
rv.update_point(Point2 {
x: coordinate[2],
y: coordinate[3],
});
rv
}
}
impl<T: cgmath::BaseFloat + Sync> Default for Aabb2<T> {
#[inline]
fn default() -> Self {
Self { min_max: None }
}
}
impl<T: cgmath::BaseFloat + Sync> Aabb2<T> {
pub fn new(point: Point2<T>) -> Self {
Self {
min_max: Some((point, point)),
}
}
pub fn update_aabb(&mut self, aabb: Aabb2<T>) {
if let Some((min, max)) = aabb.min_max {
self.update_point(min);
self.update_point(max);
}
}
pub fn update_point(&mut self, point: Point2<T>) {
if self.min_max.is_none() {
self.min_max = Some((point, point));
return;
}
let (mut aabb_min, mut aabb_max) = self.min_max.take().unwrap();
if point.x < aabb_min.x {
aabb_min.x = point.x;
}
if point.y < aabb_min.y {
aabb_min.y = point.y;
}
if point.x > aabb_max.x {
aabb_max.x = point.x;
}
if point.y > aabb_max.y {
aabb_max.y = point.y;
}
self.min_max = Some((aabb_min, aabb_max));
}
pub fn get_high(&self) -> Option<Point2<T>> {
if let Some((_, _high)) = self.min_max {
return Some(_high);
}
None
}
pub fn get_low(&self) -> Option<Point2<T>> {
if let Some((_low, _)) = self.min_max {
return Some(_low);
}
None
}
pub fn transform(&self, matrix3x3: &cgmath::Matrix3<T>) -> Self {
if let Some(min_max) = self.min_max {
Self {
min_max: Some((
matrix3x3.transform_point(min_max.0),
matrix3x3.transform_point(min_max.1),
)),
}
} else {
Self { min_max: None }
}
}
#[inline(always)]
pub fn contains_aabb(&self, other: Aabb2<T>) -> bool {
if let Some(self_aabb) = self.min_max {
if let Some(other_aabb) = other.min_max {
return Self::contains_point_inclusive_(self_aabb, other_aabb.0)
&& Self::contains_point_inclusive_(self_aabb, other_aabb.1);
}
}
false
}
#[inline(always)]
pub fn contains_line_inclusive(&self, line: &Line2<T>) -> bool {
if let Some(self_aabb) = self.min_max {
return Self::contains_point_inclusive_(self_aabb, line.start)
&& Self::contains_point_inclusive_(self_aabb, line.end);
}
false
}
#[inline(always)]
pub fn contains_point_inclusive(&self, point: Point2<T>) -> bool {
if let Some(self_aabb) = self.min_max {
return Self::contains_point_inclusive_(self_aabb, point);
}
false
}
#[inline(always)]
fn contains_point_inclusive_(aabb: (Point2<T>, Point2<T>), point: Point2<T>) -> bool {
(aabb.0.x <= point.x || ulps_eq!(&aabb.0.x, &point.x))
&& (aabb.0.y <= point.y || ulps_eq!(&aabb.0.y, &point.y))
&& (aabb.1.x >= point.x || ulps_eq!(&aabb.1.x, &point.x))
&& (aabb.1.y >= point.y || ulps_eq!(&aabb.1.y, &point.y))
}
pub fn operation<F>(&mut self, f: F)
where
F: Fn(T) -> T,
{
if let Some(ref mut min_max) = self.min_max {
min_max.0.x = f(min_max.0.x);
min_max.0.y = f(min_max.0.y);
min_max.1.x = f(min_max.1.x);
min_max.1.y = f(min_max.1.y);
}
}
}
pub fn intersect_line_point<T: cgmath::BaseFloat + Sync>(
line: Line2<T>,
point: Point2<T>,
) -> Option<Intersection<T>> {
if ulps_eq!(line.start.x, point.x) && ulps_eq!(line.start.y, point.y) {
return Some(Intersection::Intersection(point));
}
if ulps_eq!(line.end.x, point.x) && ulps_eq!(line.end.y, point.y) {
return Some(Intersection::Intersection(point));
}
let l0x = line.start.x;
let l1x = line.end.x;
let l0y = line.start.y;
let l1y = line.end.y;
let px = point.x;
let py = point.y;
let ab = ((l1x - l0x) * (l1x - l0x) + (l1y - l0y) * (l1y - l0y)).sqrt();
let ap = ((px - l0x) * (px - l0x) + (py - l0y) * (py - l0y)).sqrt();
let pb = ((l1x - px) * (l1x - px) + (l1y - py) * (l1y - py)).sqrt();
#[cfg(feature = "console_trace")]
println!("ab={:?}, ap={:?}, pb={:?}, ap+pb={:?}", ab, ap, pb, ap + pb);
if ulps_eq!(&ab, &(ap + pb)) {
return Some(Intersection::Intersection(point));
}
None
}
#[allow(dead_code)]
pub enum Intersection<T: cgmath::BaseFloat + Sync> {
Intersection(Point2<T>),
OverLap(Line2<T>),
}
impl<T: cgmath::BaseFloat + Sync> Intersection<T> {
pub fn single(&self) -> Point2<T> {
match self {
Self::OverLap(a) => a.start,
Self::Intersection(a) => *a,
}
}
}
impl<T: cgmath::BaseFloat + Sync> fmt::Debug for Intersection<T> {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::OverLap(a) => a.fmt(f),
Self::Intersection(a) => a.fmt(f),
}
}
}
#[inline(always)]
pub fn scale_to_coordinate<T: cgmath::BaseFloat + Sync>(
point: Point2<T>,
vector: Vector2<T>,
scale: T,
) -> Point2<T> {
Point2::new(point.x + scale * vector.x, point.y + scale * vector.y)
}
#[inline(always)]
fn cross_z<T: cgmath::BaseFloat + Sync>(v: Vector2<T>, w: Vector2<T>) -> T {
v.x * w.y - v.y * w.x
}
#[inline(always)]
#[allow(clippy::suspicious_operation_groupings)]
pub fn distance_to_line_squared<T>(l0: Point2<T>, l1: Point2<T>, p: Point2<T>) -> T
where
T: cgmath::BaseFloat + Sync,
{
let l0_sub_l1 = l0 - l1;
let l0_sub_p = l0 - p;
let dot = (l0_sub_l1.x * l0_sub_p.x + l0_sub_l1.y * l0_sub_p.y)
/ (l0_sub_l1.x * l0_sub_l1.x + l0_sub_l1.y * l0_sub_l1.y);
if dot < T::zero() {
l0_sub_p.x * l0_sub_p.x + l0_sub_p.y * l0_sub_p.y
} else if dot > T::one() {
let b_sub_p = l1 - p;
b_sub_p.x * b_sub_p.x + b_sub_p.y * b_sub_p.y
} else {
let a_sub_p_cross_a_sub_b = cross_z(l0_sub_p, l0_sub_l1);
(a_sub_p_cross_a_sub_b * a_sub_p_cross_a_sub_b)
/ (l0_sub_l1.x * l0_sub_l1.x + l0_sub_l1.y * l0_sub_l1.y)
}
}
#[inline(always)]
pub fn distance_to_line_squared_safe<T: cgmath::BaseFloat + Sync>(
l0: Point2<T>,
l1: Point2<T>,
p: Point2<T>,
) -> T {
if point_ulps_eq(l0, l1) {
l0.distance2(p)
} else {
distance_to_line_squared(l0, l1, p)
}
}
#[inline(always)]
pub fn point_ulps_eq<T: cgmath::BaseFloat + Sync>(a: Point2<T>, b: Point2<T>) -> bool {
ulps_eq!(a.x, b.x) && ulps_eq!(a.y, b.y)
}
#[derive(PartialEq, Clone, fmt::Debug)]
pub struct SimpleAffine<T: cgmath::BaseFloat + Sync> {
pub a_offset: [T; 2],
pub scale: [T; 2],
pub b_offset: [T; 2],
}
impl<T: cgmath::BaseFloat + Sync> Default for SimpleAffine<T> {
#[inline]
fn default() -> Self {
Self {
a_offset: [T::zero(), T::zero()],
scale: [T::one(), T::one()],
b_offset: [T::zero(), T::zero()],
}
}
}
impl<T: cgmath::BaseFloat + Sync + num_traits::cast::NumCast> SimpleAffine<T> {
pub fn new(a_aabb: &Aabb2<T>, b_aabb: &Aabb2<T>) -> Result<Self, LinestringError> {
let min_dim = T::from(1.0).unwrap();
let two = T::from(2.0).unwrap();
if let Some(source_low) = a_aabb.get_low() {
if let Some(source_high) = a_aabb.get_high() {
if let Some(destination_low) = b_aabb.get_low() {
if let Some(destination_high) = b_aabb.get_high() {
let source_aabb_center = [
-(source_low[0] + source_high[0]) / two,
-(source_low[1] + source_high[1]) / two,
];
let source_aabb_size = [
(source_high[0] - source_low[0]).max(min_dim),
(source_high[1] - source_low[1]).max(min_dim),
];
let dest_aabb_center = [
(destination_low[0] + destination_high[0]) / two,
(destination_low[1] + destination_high[1]) / two,
];
let dest_aabb_size = [
(destination_high[0] - destination_low[0]).max(min_dim),
(destination_high[1] - destination_low[1]).max(min_dim),
];
let source_aabb_size = source_aabb_size[0].max(source_aabb_size[1]);
let dest_aabb_size = dest_aabb_size[0].min(dest_aabb_size[1]);
let scale = dest_aabb_size / source_aabb_size;
return Ok(Self {
a_offset: source_aabb_center,
scale: [scale, scale],
b_offset: dest_aabb_center,
});
}
}
}
}
Err(LinestringError::AabbError(
"could not get dimension of the AABB".to_string(),
))
}
#[inline(always)]
pub fn transform_ba(&self, point: Point2<T>) -> Result<Point2<T>, LinestringError> {
let x = (point.x - self.b_offset[0]) / self.scale[0] - self.a_offset[0];
let y = (point.y - self.b_offset[1]) / self.scale[1] - self.a_offset[1];
if x.is_finite() && y.is_finite() {
Ok(Point2 { x, y })
} else {
Err(LinestringError::TransformError(
"Transformation out of bounds".to_string(),
))
}
}
#[inline(always)]
pub fn transform_ab(&self, point: Point2<T>) -> Result<Point2<T>, LinestringError> {
let x = (point.x + self.a_offset[0]) * self.scale[0] + self.b_offset[0];
let y = (point.y + self.a_offset[1]) * self.scale[1] + self.b_offset[1];
if x.is_finite() && y.is_finite() {
Ok(Point2 { x, y })
} else {
Err(LinestringError::TransformError(
"Transformation out of bounds".to_string(),
))
}
}
#[inline(always)]
pub fn transform_ba_a(&self, points: [T; 4]) -> Result<[T; 4], LinestringError> {
let x1 = (points[0] - self.b_offset[0]) / self.scale[0] - self.a_offset[0];
let y1 = (points[1] - self.b_offset[1]) / self.scale[1] - self.a_offset[1];
let x2 = (points[2] - self.b_offset[0]) / self.scale[0] - self.a_offset[0];
let y2 = (points[3] - self.b_offset[1]) / self.scale[1] - self.a_offset[1];
if x1.is_finite() && y1.is_finite() && x2.is_finite() && y2.is_finite() {
Ok([x1, y1, x2, y2])
} else {
Err(LinestringError::TransformError(
"Transformation out of bounds".to_string(),
))
}
}
#[inline(always)]
pub fn transform_ab_a(&self, points: [T; 4]) -> Result<[T; 4], LinestringError> {
let x1 = (points[0] + self.a_offset[0]) * self.scale[0] + self.b_offset[0];
let y1 = (points[1] + self.a_offset[1]) * self.scale[1] + self.b_offset[1];
let x2 = (points[2] + self.a_offset[0]) * self.scale[0] + self.b_offset[0];
let y2 = (points[3] + self.a_offset[1]) * self.scale[1] + self.b_offset[1];
if x1.is_finite() && y1.is_finite() && x2.is_finite() && y2.is_finite() {
Ok([x1, y1, x2, y2])
} else {
Err(LinestringError::TransformError(
"Transformation out of bounds".to_string(),
))
}
}
}