use super::mint_2d;
use itertools::Itertools;
use std::fmt;
pub enum Shape3d<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
Line(Line3<T>),
Linestring(LineString3<T>),
ParabolicArc(crate::mint_2d::VoronoiParabolicArc<T>),
}
#[allow(clippy::upper_case_acronyms)]
#[derive(fmt::Debug, Copy, Clone)]
pub enum Plane {
XY,
XZ,
ZY,
}
impl Plane {
pub fn get_plane<T>(aabb: &Aabb3<T>) -> Option<Plane>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
if let Some(low_bound) = aabb.get_low() {
if let Some(high_bound) = aabb.get_high() {
let dx = high_bound.x - low_bound.x;
let dy = high_bound.y - low_bound.y;
let dz = high_bound.z - low_bound.z;
let max_delta = num_traits::Float::max(num_traits::Float::max(dx, dy), dz);
let dx = T::zero().ulps_eq(
&(dx / max_delta),
T::default_epsilon(),
T::default_max_ulps(),
);
let dy = T::zero().ulps_eq(
&&(dy / max_delta),
T::default_epsilon(),
T::default_max_ulps(),
);
let dz = T::zero().ulps_eq(
&&(dz / max_delta),
T::default_epsilon(),
T::default_max_ulps(),
);
if dx && !dy && !dz {
return Some(Plane::XY);
}
if dy && !dx && !dz {
return Some(Plane::XZ);
}
if dz && !dx && !dy {
return Some(Plane::ZY);
}
}
}
None
}
}
#[derive(PartialEq, Eq, Copy, Clone, Hash, fmt::Debug)]
pub struct Line3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
pub start: mint::Point3<T>,
pub end: mint::Point3<T>,
}
impl<T> Line3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
pub fn new(start: mint::Point3<T>, end: mint::Point3<T>) -> Self {
Self { start, end }
}
pub fn triangle_area_squared_times_4(
p1: &mint::Point3<T>,
p2: &mint::Point3<T>,
p3: &mint::Point3<T>,
) -> T {
let v1_x = p1.x - p2.x;
let v1_y = p1.y - p2.y;
let v1_z = p1.z - p2.z;
let v2_x = p3.x - p2.x;
let v2_y = p3.y - p2.y;
let v2_z = p3.z - p2.z;
let x = v1_y * v2_z - v2_y * v1_z;
let y = v1_x * v2_z - v2_x * v1_z;
let z = v1_x * v2_y - v2_x * v1_y;
x * x + y * y + z * z
}
}
#[allow(clippy::from_over_into)]
impl<T> Into<[T; 6]> for Line3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
fn into(self) -> [T; 6] {
[
self.start.x,
self.start.y,
self.start.z,
self.end.x,
self.end.y,
self.end.z,
]
}
}
impl<T, IT> From<[IT; 2]> for Line3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
IT: Copy + Into<mint::Point3<T>>,
{
fn from(pos: [IT; 2]) -> Line3<T> {
Line3::<T>::new(pos[0].into(), pos[1].into())
}
}
#[derive(PartialEq, Eq, Clone, Hash, fmt::Debug)]
pub struct LineString3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
pub(crate) points: Vec<mint::Point3<T>>,
pub connected: bool,
}
#[derive(PartialEq, Eq, Clone, Hash)]
pub struct LineStringSet3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
pub set: Vec<LineString3<T>>,
pub aabb: Aabb3<T>,
}
#[derive(PartialEq, Eq, Copy, Clone, Hash, fmt::Debug)]
pub struct Aabb3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
min_max: Option<(mint::Point3<T>, mint::Point3<T>)>,
}
impl<T> LineString3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
pub fn default() -> Self {
Self {
points: Vec::<mint::Point3<T>>::new(),
connected: false,
}
}
pub fn with_capacity(capacity: usize) -> Self {
Self {
points: Vec::<mint::Point3<T>>::with_capacity(capacity),
connected: false,
}
}
pub fn with_points(mut self, points: Vec<mint::Point3<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 mint::Point3<T>>,
{
Self {
points: iter.into_iter().copied().collect(),
connected: false,
}
}
pub fn copy_to_2d(&self, plane: Plane) -> mint_2d::LineString2<T> {
let mut rv: mint_2d::LineString2<T> = match plane {
Plane::XY => self
.points
.iter()
.map(|p3d| mint::Point2 { x: p3d.x, y: p3d.y })
.collect(),
Plane::XZ => self
.points
.iter()
.map(|p3d| mint::Point2 { x: p3d.x, y: p3d.z })
.collect(),
Plane::ZY => self
.points
.iter()
.map(|p3d| mint::Point2 { x: p3d.z, y: p3d.y })
.collect(),
};
rv.connected = self.connected;
rv
}
pub fn points(&self) -> &Vec<mint::Point3<T>> {
&self.points
}
pub fn len(&self) -> usize {
self.points.len()
}
pub fn is_empty(&self) -> bool {
self.points.is_empty()
}
pub fn as_lines(&self) -> Vec<Line3<T>> {
if self.points.is_empty() {
return vec![];
} else if self.points.len() == 1 {
return vec![Line3 {
start: *self.points.first().unwrap(),
end: *self.points.first().unwrap(),
}];
}
let iter1 = self.points.iter().skip(1);
let iter2 = self.points.iter();
if self.connected && self.points.last() != self.points.first() {
iter1
.zip(iter2)
.map(|(a, b)| Line3 { start: *b, end: *a })
.chain(
Some(Line3 {
start: *self.points.last().unwrap(),
end: *self.points.first().unwrap(),
})
.into_iter(),
)
.collect()
} else {
iter1
.zip(iter2)
.map(|(a, b)| Line3 { start: *b, end: *a })
.collect()
}
}
pub fn push(&mut self, point: mint::Point3<T>) {
self.points.push(point);
}
#[cfg(not(feature = "impl-mint"))]
pub fn transform(&self, matrix4x4: &mint::ColumnMatrix4<T>) -> Self {
Self {
points: self
.points
.iter()
.map(|x| matrix4x4.transform_point(*x))
.collect(),
connected: self.connected,
}
}
pub fn append(&mut self, mut other: Self) {
self.points.append(&mut other.points);
}
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<mint::Point3<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<mint::Point3<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: &[mint::Point3<T>]) -> Vec<mint::Point3<T>> {
if slice.len() <= 2 {
return slice[1..].to_vec();
}
let start_point = slice.first().unwrap();
let end_point = slice.last().unwrap();
let identical_points = point_ulps_eq(&start_point, &end_point);
let mut max_dist_sq = (-T::one(), 0_usize);
for (i, point) in slice.iter().enumerate().take(slice.len() - 1).skip(1) {
let sq_d = if identical_points {
distance_to_point_squared(start_point, point)
} else {
distance_to_line_squared(start_point, end_point, point)
};
if sq_d > max_dist_sq.0 && sq_d > distance_predicate_sq {
max_dist_sq = (sq_d, i);
}
}
if max_dist_sq.1 == 0 {
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 = rb_tree::RBTree::<(T, usize)>::new();
let mut link_tree = fnv::FnvHashMap::<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 = Line3::triangle_area_squared_times_4(i.1, j.1, k.1);
let _ = search_tree.insert((area, 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 = Line3::triangle_area_squared_times_4(
&self.points[i],
&self.points[j],
&self.points[0],
);
let _ = search_tree.insert((area, 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.1).copied() {
let area = old_links.2;
if smallest.0 != area {
continue;
} else {
let _ = link_tree.remove(&smallest.1);
}
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 = Line3::triangle_area_squared_times_4(
&self.points[prev],
&self.points[next % self_points_len],
&self.points[next_next % self_points_len],
);
let _ = search_tree.insert((area, next));
let _ = link_tree.insert(next, (prev, next_next, area));
let area = Line3::triangle_area_squared_times_4(
&self.points[prev_prev],
&self.points[prev],
&self.points[next % self_points_len],
);
let _ = search_tree.insert((area, prev));
let _ = link_tree.insert(prev, (prev_prev, next, area));
continue;
}
}
if let Some(prev_prev) = prev_prev {
let area = Line3::triangle_area_squared_times_4(
&self.points[prev_prev],
&self.points[prev],
&self.points[next % self_points_len],
);
let _ = search_tree.insert((area, prev));
let _ = link_tree.insert(prev, (prev_prev, next, area));
continue;
};
if let Some(next_next) = next_next {
let area = Line3::triangle_area_squared_times_4(
&self.points[prev],
&self.points[next % self_points_len],
&self.points[next_next % self_points_len],
);
let _ = search_tree.insert((area, 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)
}
}
}
impl<T, IC: Into<mint::Point3<T>>> std::iter::FromIterator<IC> for LineString3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
fn from_iter<I: IntoIterator<Item = IC>>(iter: I) -> Self {
LineString3 {
points: iter.into_iter().map(|c| c.into()).collect(),
connected: false,
}
}
}
impl<T> LineStringSet3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
pub fn default() -> Self {
Self {
set: Vec::<LineString3<T>>::new(),
aabb: Aabb3::default(),
}
}
pub fn with_capacity(capacity: usize) -> Self {
Self {
set: Vec::<LineString3<T>>::with_capacity(capacity),
aabb: Aabb3::default(),
}
}
pub fn set(&self) -> &Vec<LineString3<T>> {
&self.set
}
pub fn is_empty(&self) -> bool {
self.set.is_empty()
}
pub fn push(&mut self, ls: LineString3<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_aabb(&self) -> &Aabb3<T> {
&self.aabb
}
#[cfg(not(feature = "impl-mint"))]
pub fn transform(&self, mat: &mint::ColumnMatrix4<T>) -> Self {
Self {
set: self.set.iter().map(|x| x.transform(mat)).collect(),
aabb: self.aabb.transform(mat),
}
}
pub fn copy_to_2d(&self, plane: Plane) -> mint_2d::LineStringSet2<T> {
let mut rv = mint_2d::LineStringSet2::with_capacity(self.set.len());
for ls in self.set.iter() {
rv.push(ls.copy_to_2d(plane));
}
rv
}
pub fn take_from(&mut self, other: &mut Self) {
self.aabb.update_aabb(&other.aabb);
self.set.append(&mut other.set);
}
}
impl<T, IT> From<[IT; 2]> for Aabb3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
IT: Copy + Into<mint::Point3<T>>,
{
fn from(coordinate: [IT; 2]) -> Aabb3<T> {
Aabb3 {
min_max: Some((coordinate[0].into(), coordinate[1].into())),
}
}
}
impl<T> From<[T; 6]> for Aabb3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
fn from(coordinate: [T; 6]) -> Aabb3<T> {
Aabb3 {
min_max: Some((
mint::Point3 {
x: coordinate[0],
y: coordinate[1],
z: coordinate[2],
},
mint::Point3 {
x: coordinate[3],
y: coordinate[4],
z: coordinate[5],
},
)),
}
}
}
impl<T> Aabb3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
pub fn default() -> Self {
Self { min_max: None }
}
pub fn update_aabb(&mut self, aabb: &Aabb3<T>) {
if let Some((min, max)) = &aabb.min_max {
self.update_point(min);
self.update_point(max);
}
}
pub fn update_point(&mut self, point: &mint::Point3<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.z < aabb_min.z {
aabb_min.z = point.z;
}
if point.x > aabb_max.x {
aabb_max.x = point.x;
}
if point.y > aabb_max.y {
aabb_max.y = point.y;
}
if point.z > aabb_max.z {
aabb_max.z = point.z;
}
self.min_max = Some((aabb_min, aabb_max));
}
pub fn get_high(&self) -> Option<mint::Point3<T>> {
if let Some((_, _high)) = self.min_max {
return Some(_high);
}
None
}
pub fn get_low(&self) -> Option<mint::Point3<T>> {
if let Some((_low, _)) = self.min_max {
return Some(_low);
}
None
}
#[cfg(not(feature = "impl-mint"))]
pub fn transform(&self, matrix4x4: &mint::ColumnMatrix4<T>) -> Self {
if let Some(min_max) = self.min_max {
Self {
min_max: Some((
matrix4x4.transform_point(min_max.0),
matrix4x4.transform_point(min_max.1),
)),
}
} else {
Self { min_max: None }
}
}
#[inline(always)]
pub fn contains_aabb(&self, other: &Self) -> bool {
if let Some(self_aabb) = other.min_max {
if let Some(o_aabb) = other.min_max {
return Self::contains_point_(&self_aabb, &o_aabb.0)
&& Self::contains_point_(&self_aabb, &o_aabb.1);
}
}
false
}
#[inline(always)]
pub fn contains_line(&self, line: &Line3<T>) -> bool {
if let Some(self_aabb) = self.min_max {
return Self::contains_point_(&self_aabb, &line.start)
&& Self::contains_point_(&self_aabb, &line.end);
}
false
}
#[inline(always)]
pub fn contains_point(&self, point: &mint::Point3<T>) -> bool {
if let Some(self_aabb) = self.min_max {
return Self::contains_point_(&self_aabb, point);
}
false
}
#[inline(always)]
fn contains_point_(aabb: &(mint::Point3<T>, mint::Point3<T>), point: &mint::Point3<T>) -> bool {
aabb.0.x <= point.x
&& aabb.0.y <= point.y
&& aabb.0.z <= point.z
&& aabb.1.x >= point.x
&& aabb.1.y >= point.y
&& aabb.1.z >= point.z
}
}
#[inline(always)]
pub fn point_ulps_eq<T>(a: &mint::Point3<T>, b: &mint::Point3<T>) -> bool
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
mint_2d::ulps_eq(&a.x, &b.x) && mint_2d::ulps_eq(&a.y, &b.y) && mint_2d::ulps_eq(&a.z, &b.z)
}
#[inline(always)]
fn sub<T>(a: &mint::Point3<T>, b: &mint::Point3<T>) -> mint::Vector3<T>
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
mint::Vector3 {
x: a.x - b.x,
y: a.y - b.y,
z: a.z - b.z,
}
}
#[allow(clippy::many_single_char_names)]
#[inline(always)]
fn cross_abs_squared<T>(a: &mint::Vector3<T>, b: &mint::Vector3<T>) -> T
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
let x = a.y * b.z - a.z * b.y;
let y = a.z * b.x - a.x * b.z;
let z = a.x * b.y - a.y * b.x;
x * x + y * y + z * z
}
#[inline(always)]
pub fn distance_to_line_squared<T>(
a: &mint::Point3<T>,
b: &mint::Point3<T>,
p: &mint::Point3<T>,
) -> T
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
let a_sub_b = sub(a, b);
let a_sub_p = sub(a, p);
let a_sub_p_cross_a_sub_b_squared = cross_abs_squared(&a_sub_p, &a_sub_b);
a_sub_p_cross_a_sub_b_squared
/ (a_sub_b.x * a_sub_b.x + a_sub_b.y * a_sub_b.y + a_sub_b.z * a_sub_b.z)
}
#[inline(always)]
pub fn distance_to_point_squared<T>(a: &mint::Point3<T>, b: &mint::Point3<T>) -> T
where
T: num_traits::Float + std::fmt::Debug + approx::AbsDiffEq + approx::UlpsEq,
{
let v = sub(a, b);
v.x * v.x + v.y * v.y + v.z * v.z
}