#![deny(
rust_2018_compatibility,
rust_2018_idioms,
nonstandard_style,
unused,
future_incompatible,
non_camel_case_types,
unused_parens,
non_upper_case_globals,
unused_qualifications,
unused_results,
unused_imports,
unused_variables
)]
#![cfg_attr(feature = "hash_drain_filter", feature(hash_drain_filter))]
#![cfg_attr(feature = "map_first_last", feature(map_first_last))]
use boostvoronoi as BV;
use cgmath::{InnerSpace, MetricSpace, SquareMatrix, Transform};
use linestring::linestring_2d::{self, convex_hull};
use linestring::linestring_3d::{self, Line3, LineString3, LineStringSet3};
use ordered_float::OrderedFloat;
use rayon::iter::{IntoParallelIterator, ParallelIterator};
use std::collections::VecDeque;
use std::line;
use thiserror::Error;
#[macro_use]
extern crate bitflags;
#[derive(Error, Debug)]
pub enum CenterlineError {
#[error("Something is wrong with the internal logic")]
InternalError(String),
#[error("Something is wrong with the input data")]
CouldNotCalculateInverseMatrix,
#[error("Your line-strings are self-intersecting.")]
SelfIntersectingData,
#[error("The input data is not 2D")]
InputNotPLane,
#[error("Invalid data")]
InvalidData(String),
#[error(transparent)]
BvError(#[from] BV::BvError),
#[error("Error from .obj file handling")]
ObjError(String),
#[error(transparent)]
IoError(#[from] std::io::Error),
#[error(transparent)]
LinestringError(#[from] linestring::LinestringError),
}
bitflags! {
pub struct ColorFlag: BV::ColorType {
const EXTERNAL = 0b00000001;
const SECONDARY = 0b00000010;
const INFINITE = 0b00000100;
const DOTLIMIT = 0b00001000;
}
}
type Vob32 = vob::Vob<u32>;
pub trait GrowingVob {
fn fill(initial_size: usize) -> Self;
fn set_grow(&mut self, bit: usize, state: bool);
fn get_f(&self, bit: usize) -> bool;
}
impl<T: num_traits::PrimInt + std::fmt::Debug> GrowingVob for vob::Vob<T> {
#[inline]
fn fill(initial_size: usize) -> Self {
let mut v = Self::new_with_storage_type(0);
v.resize(initial_size, false);
v
}
#[inline]
fn set_grow(&mut self, bit: usize, state: bool) {
if bit >= self.len() {
self.resize(bit + std::mem::size_of::<T>(), false);
}
let _ = self.set(bit, state);
}
#[inline]
fn get_f(&self, bit: usize) -> bool {
self.get(bit).unwrap_or(false)
}
}
#[derive(Debug)]
struct Vertices {
id: usize, connected_vertices: Vec<usize>, shape: Option<usize>, }
fn paint_every_connected_vertex(
vertices: &mut ahash::AHashMap<usize, Vertices>,
already_painted: &mut Vob32,
vertex_id: usize,
color: usize,
) -> Result<(), CenterlineError> {
let mut queue = VecDeque::<usize>::new();
queue.push_back(vertex_id);
while !queue.is_empty() {
let current_vertex = queue.pop_front().unwrap();
if already_painted.get_f(current_vertex) {
continue;
}
if let Some(vertex_obj) = vertices.get_mut(¤t_vertex) {
if vertex_obj.shape.is_none() {
vertex_obj.shape = Some(color);
let _ = already_painted.set(current_vertex, true);
} else {
continue;
}
for v in vertex_obj.connected_vertices.iter() {
if !already_painted.get_f(*v) {
queue.push_back(*v);
}
}
} else {
return Err(CenterlineError::InternalError(format!(
"Vertex with id:{} dissapeared. {}:{}",
current_vertex,
file!(),
line!()
)));
};
}
Ok(())
}
#[cfg(feature = "obj-rs")]
#[allow(clippy::type_complexity)]
pub fn remove_internal_edges(
obj: obj::raw::RawObj,
) -> Result<(ahash::AHashSet<(usize, usize)>, Vec<cgmath::Point3<f32>>), CenterlineError> {
for p in obj.points.iter() {
println!("Ignored point:{:?}", p);
}
let mut all_edges = ahash::AHashSet::<(usize, usize)>::default();
let mut internal_edges = ahash::AHashSet::<(usize, usize)>::default();
for i in 0..obj.lines.len() {
let v = match &obj.lines[i] {
obj::raw::object::Line::P(a) => a.clone(),
obj::raw::object::Line::PT(a) => a.iter().map(|x| x.0).collect::<Vec<usize>>(),
};
let mut i1 = v.iter();
for i in v.iter().skip(1) {
let i1_v = *i1.next().unwrap();
let i2_v = *i;
let key = (*std::cmp::min(&i1_v, &i2_v), *std::cmp::max(&i1_v, &i2_v));
if all_edges.contains(&key) {
let _ = internal_edges.insert(key);
} else {
let _ = all_edges.insert(key);
}
}
}
for i in 0..obj.polygons.len() {
let v = match &obj.polygons[i] {
obj::raw::object::Polygon::P(a) => {
let mut v = a.clone();
v.push(a[0]);
v
}
obj::raw::object::Polygon::PT(a) => {
let mut v = a.iter().map(|x| x.0).collect::<Vec<usize>>();
v.push(a[0].0);
v
}
obj::raw::object::Polygon::PN(a) => {
let mut v = a.iter().map(|x| x.0).collect::<Vec<usize>>();
v.push(a[0].0);
v
}
obj::raw::object::Polygon::PTN(a) => {
let mut v = a.iter().map(|x| x.0).collect::<Vec<usize>>();
v.push(a[0].0);
v
}
};
let mut i1 = v.iter();
for i in v.iter().skip(1) {
let i1_v = *i1.next().unwrap();
let i2_v = *i;
let key = (*std::cmp::min(&i1_v, &i2_v), *std::cmp::max(&i1_v, &i2_v));
if all_edges.contains(&key) {
let _ = internal_edges.insert(key);
} else {
let _ = all_edges.insert(key);
}
}
}
#[cfg(feature = "hash_drain_filter")]
{
let _ = all_edges.drain_filter(|x| internal_edges.contains(x));
}
#[cfg(not(feature = "hash_drain_filter"))]
{
let kept_edges = all_edges
.into_iter()
.filter(|x| !internal_edges.contains(&x))
.collect();
all_edges = kept_edges;
}
let vertices: Vec<cgmath::Point3<f32>> = obj
.positions
.into_iter()
.map(|x| cgmath::Point3 {
x: x.0,
y: x.1,
z: x.2,
})
.collect();
Ok((all_edges, vertices))
}
pub fn divide_into_shapes<T: cgmath::BaseFloat + Sync + Send>(
edge_set: ahash::AHashSet<(usize, usize)>,
points: Vec<cgmath::Point3<T>>,
) -> Result<Vec<LineStringSet3<T>>, CenterlineError> {
let mut vertices = ahash::AHashMap::<usize, Vertices>::default();
for e in edge_set.iter() {
let id = e.0;
let other = e.1;
vertices
.entry(id)
.or_insert_with_key(|key| Vertices {
id: *key,
connected_vertices: Vec::<usize>::new(),
shape: None,
})
.connected_vertices
.push(other);
let id = e.1;
let other = e.0;
vertices
.entry(id)
.or_insert_with_key(|key| Vertices {
id: *key,
connected_vertices: Vec::<usize>::new(),
shape: None,
})
.connected_vertices
.push(other);
}
let mut unique_shape_id_generator = 0..usize::MAX;
let mut already_painted = Vob32::fill(vertices.len());
for vertex_id in 0..vertices.len() {
if already_painted.get_f(vertex_id) {
continue;
}
paint_every_connected_vertex(
&mut vertices,
&mut already_painted,
vertex_id,
unique_shape_id_generator.next().unwrap(),
)?;
}
let highest_shape_id_plus_one = unique_shape_id_generator.next().unwrap();
if highest_shape_id_plus_one == 0 {
return Err(CenterlineError::InternalError(format!(
"Could not find any shapes to separate. {}:{}",
file!(),
line!()
)));
}
let mut shape_separation = Vec::<ahash::AHashMap<usize, Vertices>>::new();
for current_shape in 0..highest_shape_id_plus_one {
if vertices.is_empty() {
println!("vertices:{:?}", vertices);
println!("current_shape:{}", current_shape);
println!("shape_separation:{:?}", shape_separation);
return Err(CenterlineError::InternalError(format!(
"Could not separate all shapes, ran out of vertices. {}:{}",
file!(),
line!()
)));
}
#[cfg(feature = "hash_drain_filter")]
{
let drained = vertices
.drain_filter(|_, x| {
if let Some(shape) = x.shape {
shape == current_shape
} else {
false
}
})
.collect();
shape_separation.push(drained);
}
#[cfg(not(feature = "hash_drain_filter"))]
{
let mut drained = ahash::AHashMap::<usize, Vertices>::default();
let mut new_vertices = ahash::AHashMap::<usize, Vertices>::default();
for (x0, x1) in vertices.into_iter() {
if x1.shape.map_or(false, |shape| shape == current_shape) {
let _ = drained.insert(x0, x1);
} else {
let _ = new_vertices.insert(x0, x1);
};
}
vertices = new_vertices;
shape_separation.push(drained);
}
}
drop(vertices);
let shape_separation = shape_separation;
shape_separation
.into_par_iter()
.map(|rvi| -> Result<LineStringSet3<T>, CenterlineError> {
if rvi.is_empty() {
return Err(CenterlineError::InternalError(
format!("rvi.is_empty() Seems like the shape separation failed. {}:{}", file!(),line!()),
));
}
let mut loops = 0;
let mut rvs = linestring_3d::LineStringSet3::<T>::with_capacity(rvi.len());
let mut als = linestring_3d::LineString3::<T>::with_capacity(rvi.len());
let started_with: usize = rvi.iter().next().unwrap().1.id;
let mut prev: usize;
let mut current: usize = started_with;
let mut next: usize = started_with;
let mut first_loop = true;
loop {
prev = current;
current = next;
if let Some(current_vertex) = rvi.get(¤t) {
als.push(points[current]);
next = *current_vertex.connected_vertices.iter().find(|x| **x != prev).ok_or_else(||
CenterlineError::InvalidData(
"Could not find next vertex. All lines must form unconnected loops".to_string(),
),
)?;
} else {
break;
}
if !first_loop && current == started_with {
break;
}
first_loop = false;
loops += 1;
if loops > rvi.len() + 1 {
return Err(CenterlineError::InvalidData(
"It seems like one (or more) of the line strings does not form an unconnected loop."
.to_string(),
));
}
}
if als.points().last() != als.points().first() {
println!(
"Linestring is not connected ! {:?} {:?}",
als.points().first(),
als.points().last()
);
println!("Linestring is not connected ! {:?}", als.points());
}
rvs.push(als);
Ok(rvs)
})
.collect()
}
#[inline(always)]
pub fn get_transform<F: cgmath::BaseFloat + Sync>(
total_aabb: linestring_3d::Aabb3<F>,
desired_voronoi_dimension: F,
) -> Result<
(
linestring_3d::Plane,
cgmath::Matrix4<F>,
linestring::linestring_2d::Aabb2<F>,
),
CenterlineError,
> {
get_transform_relaxed(
total_aabb,
desired_voronoi_dimension,
F::default_epsilon(),
F::default_max_ulps(),
)
}
pub fn get_transform_relaxed<F: cgmath::BaseFloat + Sync>(
total_aabb: linestring_3d::Aabb3<F>,
desired_voronoi_dimension: F,
epsilon: F,
max_ulps: u32,
) -> Result<
(
linestring_3d::Plane,
cgmath::Matrix4<F>,
linestring::linestring_2d::Aabb2<F>,
),
CenterlineError,
> {
let plane = if let Some(plane) =
linestring_3d::Plane::get_plane_relaxed(total_aabb, epsilon, max_ulps)
{
plane
} else {
return Err(CenterlineError::InputNotPLane);
};
println!(
"get_transform_relaxed desired_voronoi_dimension:{:?}",
desired_voronoi_dimension
);
let low = total_aabb.get_low().unwrap();
let high = total_aabb.get_high().unwrap();
let delta = high - low;
let center = cgmath::point3(
(high.x + low.x) / F::from(2.0).unwrap(),
(high.y + low.y) / F::from(2.0).unwrap(),
(high.z + low.z) / F::from(2.0).unwrap(),
);
println!(
"Input data AABB: Center:({:?}, {:?}, {:?})",
center.x, center.y, center.z,
);
println!(
" high:({:?}, {:?}, {:?})",
high.x, high.y, high.z,
);
println!(
" low:({:?}, {:?}, {:?})",
low.x, low.y, low.z,
);
println!(
" delta:({:?}, {:?}, {:?})",
delta.x, delta.y, delta.z,
);
let scale_transform = {
let scale = desired_voronoi_dimension
/ std::cmp::max(
std::cmp::max(OrderedFloat(delta.x), OrderedFloat(delta.y)),
OrderedFloat(delta.z),
)
.into_inner();
cgmath::Matrix4::from_scale(scale)
};
let center = scale_transform.transform_point(center);
let center_transform: cgmath::Matrix4<F> =
cgmath::Matrix4::from_translation(cgmath::Vector3::new(-center.x, -center.y, -center.z));
let plane_transform: cgmath::Matrix4<F> = {
let x = cgmath::Vector4::<F>::new(F::one(), F::zero(), F::zero(), F::zero());
let y = cgmath::Vector4::<F>::new(F::zero(), F::one(), F::zero(), F::zero());
let z = cgmath::Vector4::<F>::new(F::zero(), F::zero(), F::one(), F::zero());
let w = cgmath::Vector4::<F>::new(F::zero(), F::zero(), F::zero(), F::one());
match plane {
linestring_3d::Plane::XY => cgmath::Matrix4::from_cols(x, y, z, w),
linestring_3d::Plane::XZ => cgmath::Matrix4::from_cols(x, z, y, w),
linestring_3d::Plane::YZ => cgmath::Matrix4::from_cols(z, y, x, w),
}
};
let total_transform = plane_transform * center_transform * scale_transform;
let high0 = total_aabb.get_high().unwrap();
let low0 = total_aabb.get_low().unwrap();
let low0 = total_transform.transform_point(low0);
let high0 = total_transform.transform_point(high0);
let delta0 = high0 - low0;
let center0 = cgmath::point3(
(high0.x + low0.x) / F::from(2.0).unwrap(),
(high0.y + low0.y) / F::from(2.0).unwrap(),
(high0.z + low0.z) / F::from(2.0).unwrap(),
);
#[cfg(feature = "console_debug")]
let center0 = total_transform.transform_point(center0);
#[cfg(feature = "console_debug")]
println!(
"Voronoi input AABB: Center {:?} low:{:?}, high:{:?}",
center0, low0, high0
);
let mut voronoi_input_aabb =
linestring::linestring_2d::Aabb2::new(cgmath::Point2::new(low0.x, low0.y));
voronoi_input_aabb.update_point(cgmath::Point2::new(high0.x, high0.y));
println!(
"Voronoi input AABB: Center:({:?}, {:?}, {:?})",
center0.x, center0.y, center0.z,
);
println!(
" high:({:?}, {:?}, {:?})",
high0.x, high0.y, high0.z,
);
println!(
" low:({:?}, {:?}, {:?})",
low0.x, low0.y, low0.z,
);
println!(
" delta:({:?}, {:?}, {:?})",
delta0.x, delta0.y, delta0.z,
);
let inverse_total = total_transform.invert();
if inverse_total.is_none() {
return Err(CenterlineError::CouldNotCalculateInverseMatrix);
}
Ok((plane, total_transform, voronoi_input_aabb))
}
pub fn consolidate_shapes<F: cgmath::BaseFloat + Sync>(
mut raw_data: Vec<linestring::linestring_2d::LineStringSet2<F>>,
) -> Result<Vec<linestring::linestring_2d::LineStringSet2<F>>, CenterlineError> {
'outer_loop: loop {
for i in 0..raw_data.len() {
for j in i + 1..raw_data.len() {
if raw_data[i].get_aabb().contains_aabb(raw_data[j].get_aabb())
&& linestring::linestring_2d::convex_hull::ConvexHull::contains(
raw_data[i].get_convex_hull().as_ref().unwrap(),
raw_data[j].get_convex_hull().as_ref().unwrap(),
F::default_epsilon() * F::from(2.0).unwrap(),
F::default_max_ulps() * 2,
)
{
let mut stolen_line_j = linestring::linestring_2d::LineStringSet2::steal_from(
raw_data.get_mut(j).unwrap(),
);
let line_i = raw_data.get_mut(i).unwrap();
line_i.take_from_internal(&mut stolen_line_j)?;
let _ = raw_data.remove(j);
continue 'outer_loop;
} else if raw_data[j].get_aabb().contains_aabb(raw_data[i].get_aabb())
&& linestring::linestring_2d::convex_hull::ConvexHull::contains(
raw_data[j].get_convex_hull().as_ref().unwrap(),
raw_data[i].get_convex_hull().as_ref().unwrap(),
F::default_epsilon() * F::from(2.0).unwrap(),
F::default_max_ulps() * 2,
)
{
let mut stolen_line_i = linestring::linestring_2d::LineStringSet2::steal_from(
raw_data.get_mut(i).unwrap(),
);
let line_j = raw_data.get_mut(j).unwrap();
line_j.take_from_internal(&mut stolen_line_i)?;
let _ = raw_data.remove(i);
continue 'outer_loop;
}
}
}
break 'outer_loop;
}
Ok(raw_data)
}
pub struct Centerline<I: BV::InputType, F: cgmath::BaseFloat + BV::OutputType> {
pub segments: Vec<BV::Line<I>>,
pub diagram: BV::SyncDiagram<F>,
pub lines: Option<Vec<Line3<F>>>,
pub line_strings: Option<Vec<LineString3<F>>>,
rejected_edges: Option<Vob32>,
ignored_edges: Option<Vob32>,
#[cfg(feature = "console_debug")]
pub debug_edges: Option<ahash::AHashMap<usize, [F; 4]>>,
}
impl<I: BV::InputType, F: cgmath::BaseFloat + BV::OutputType> Centerline<I, F> {
pub fn default() -> Self {
Self {
diagram: BV::SyncDiagram::default(),
segments: Vec::<BV::Line<I>>::default(),
lines: Some(Vec::<Line3<F>>::new()),
line_strings: Some(Vec::<LineString3<F>>::new()),
rejected_edges: None,
ignored_edges: None,
#[cfg(feature = "console_debug")]
debug_edges: None,
}
}
pub fn with_segments(segments: Vec<BV::Line<I>>) -> Self {
Self {
diagram: BV::SyncDiagram::default(),
segments,
lines: Some(Vec::<Line3<F>>::new()),
line_strings: Some(Vec::<LineString3<F>>::new()),
rejected_edges: None,
ignored_edges: None,
#[cfg(feature = "console_debug")]
debug_edges: None,
}
}
pub fn build_voronoi(&mut self) -> Result<(), CenterlineError> {
self.diagram = {
#[cfg(feature = "console_debug")]
{
print!("build_voronoi()-> input segments:[");
for s in self.segments.iter() {
print!("[{},{},{},{}],", s.start.x, s.start.y, s.end.x, s.end.y);
}
println!("];");
}
BV::Builder::default()
.with_segments(self.segments.iter())?
.build()?
.into()
};
self.reject_external_edges()?;
#[cfg(feature = "console_debug")]
println!(
"build_voronoi()-> Rejected edges:{:?} {}",
self.rejected_edges.as_ref(),
&self.rejected_edges.as_ref().unwrap().get_f(0)
);
Ok(())
}
pub fn calculate_centerline(
&mut self,
cos_angle: F,
discrete_limit: F,
ignored_regions: Option<
&Vec<(
linestring::linestring_2d::Aabb2<F>,
linestring::linestring_2d::LineString2<F>,
)>,
>,
) -> Result<(), CenterlineError> {
self.angle_test(cos_angle)?;
if let Some(ignored_regions) = ignored_regions {
self.traverse_edges(discrete_limit, ignored_regions)?;
} else {
let ignored_regions = Vec::<(
linestring::linestring_2d::Aabb2<F>,
linestring::linestring_2d::LineString2<F>,
)>::with_capacity(0);
self.traverse_edges(discrete_limit, &ignored_regions)?;
}
Ok(())
}
pub fn calculate_centerline_mesh(
&mut self,
discrete_limit: F,
ignored_regions: Option<
&Vec<(
linestring::linestring_2d::Aabb2<F>,
linestring::linestring_2d::LineString2<F>,
)>,
>,
) -> Result<(), CenterlineError> {
self.ignored_edges = self.rejected_edges.clone();
if let Some(ignored_regions) = ignored_regions {
self.traverse_cells(discrete_limit, ignored_regions)?;
} else {
let ignored_regions = Vec::<(
linestring::linestring_2d::Aabb2<F>,
linestring::linestring_2d::LineString2<F>,
)>::with_capacity(0);
self.traverse_cells(discrete_limit, &ignored_regions)?;
}
Ok(())
}
pub fn ignored_edges(&self) -> Option<Vob32> {
self.ignored_edges.to_owned()
}
pub fn rejected_edges(&self) -> Option<Vob32> {
self.rejected_edges.to_owned()
}
pub fn retrieve_point(&self, cell_id: BV::CellIndex) -> Result<BV::Point<I>, CenterlineError> {
let (index, category) = self.diagram.cell_get(cell_id)?.source_index_2();
match category {
BV::SourceCategory::SinglePoint => panic!("No points in the input data"),
BV::SourceCategory::SegmentStart => Ok(self.segments[index].start),
BV::SourceCategory::Segment | BV::SourceCategory::SegmentEnd => {
Ok(self.segments[index].end)
}
}
}
pub fn retrieve_segment(&self, cell_id: BV::CellIndex) -> Result<BV::Line<I>, CenterlineError> {
let cell = self.diagram.cell_get(cell_id)?;
Ok(self.segments[cell.source_index()])
}
pub fn diagram(&self) -> &BV::SyncDiagram<F> {
&self.diagram
}
fn reject_external_edges(&mut self) -> Result<(), CenterlineError> {
let mut rejected_edges = Vob32::fill(self.diagram.edges().len());
for edge in self.diagram.edges().iter() {
let edge_id = edge.id();
if edge.is_secondary() {
let _ = rejected_edges.set(edge_id.0, true);
let twin_id = self.diagram.edge_get_twin(edge_id)?;
let _ = rejected_edges.set(twin_id.0, true);
}
if !self.diagram.edge_is_finite(edge_id)? {
self.mark_connected_edges(edge_id, &mut rejected_edges, true)?;
let _ = rejected_edges.set(edge_id.0, true);
}
}
self.rejected_edges = Some(rejected_edges);
Ok(())
}
fn angle_test(&mut self, cos_angle: F) -> Result<(), CenterlineError> {
let mut ignored_edges = self.rejected_edges.clone().take().unwrap();
for cell in self.diagram.cells().iter() {
let cell_id = cell.id();
if !cell.contains_segment() {
continue;
}
let segment = self.retrieve_segment(cell_id)?;
let point0 = cgmath::Point2 {
x: Self::i2f(segment.start.x),
y: Self::i2f(segment.start.y),
};
let point1 = cgmath::Point2 {
x: Self::i2f(segment.end.x),
y: Self::i2f(segment.end.y),
};
if let Some(incident_e) = cell.get_incident_edge() {
let mut e = incident_e;
loop {
e = self.diagram.edge_get_next(e)?;
if !ignored_edges.get_f(e.0) {
if let Some(Ok(vertex0)) = self
.diagram
.edge_get_vertex0(e)?
.map(|x| self.diagram.vertex_get(x))
{
let vertex0 = cgmath::Point2::<F>::from(vertex0);
if let Some(Ok(vertex1)) = self
.diagram
.edge_get_vertex1(e)?
.map(|x| self.diagram.vertex_get(x))
{
let vertex1 = cgmath::Point2::<F>::from(vertex1);
let _ = self.angle_test_6(
cos_angle,
&mut ignored_edges,
e,
vertex0,
vertex1,
point0,
point1,
)? || self.angle_test_6(
cos_angle,
&mut ignored_edges,
e,
vertex0,
vertex1,
point1,
point0,
)? || self.angle_test_6(
cos_angle,
&mut ignored_edges,
e,
vertex1,
vertex0,
point0,
point1,
)? || self.angle_test_6(
cos_angle,
&mut ignored_edges,
e,
vertex1,
vertex0,
point1,
point0,
)?;
}
}
}
if e == incident_e {
break;
}
}
}
}
self.ignored_edges = Some(ignored_edges);
Ok(())
}
#[allow(clippy::too_many_arguments)]
fn angle_test_6(
&self,
cos_angle: F,
ignored_edges: &mut Vob32,
edge_id: BV::EdgeIndex,
vertex0: cgmath::Point2<F>,
vertex1: cgmath::Point2<F>,
s_point_0: cgmath::Point2<F>,
s_point_1: cgmath::Point2<F>,
) -> Result<bool, CenterlineError> {
if cgmath::ulps_eq!(vertex0.x, s_point_0.x) && cgmath::ulps_eq!(vertex0.y, s_point_0.y) {
let segment_v = (s_point_1 - s_point_0).normalize();
let vertex_v = (vertex1 - vertex0).normalize();
if segment_v.dot(vertex_v).abs() < cos_angle {
let twin = self.diagram.edge_get_twin(edge_id)?;
let _ = ignored_edges.set(twin.0, true);
let _ = ignored_edges.set(edge_id.0, true);
return Ok(true);
}
}
Ok(false)
}
fn mark_connected_edges(
&self,
edge_id: BV::EdgeIndex,
marked_edges: &mut Vob32,
initial: bool,
) -> Result<(), CenterlineError> {
if marked_edges.get_f(edge_id.0) {
return Ok(());
}
let mut initial = initial;
let mut queue = VecDeque::<BV::EdgeIndex>::new();
queue.push_back(edge_id);
'outer: while !queue.is_empty() {
let edge_id = queue.pop_front().unwrap();
if marked_edges.get_f(edge_id.0) {
initial = false;
continue 'outer;
}
let v1 = self.diagram.edge_get_vertex1(edge_id)?;
if self.diagram.edge_get_vertex0(edge_id)?.is_some() && v1.is_none() {
let _ = marked_edges.set(edge_id.0, true);
initial = false;
continue 'outer;
}
let _ = marked_edges.set(edge_id.0, true);
#[allow(unused_assignments)]
if initial {
initial = false;
queue.push_back(self.diagram.edge_get_twin(edge_id)?);
} else {
let _ = marked_edges.set(self.diagram.edge_get_twin(edge_id)?.0, true);
}
if v1.is_none() || !self.diagram.edge_get(edge_id)?.is_primary() {
initial = false;
continue 'outer;
}
if let Some(v1) = v1 {
let v1 = self.diagram.vertex_get(v1)?;
if v1.is_site_point() {
initial = false;
continue 'outer;
}
let mut this_edge = v1.get_incident_edge()?;
let v_incident_edge = this_edge;
loop {
if !marked_edges.get_f(this_edge.0) {
queue.push_back(this_edge);
}
this_edge = self.diagram.edge_rot_next(this_edge)?;
if this_edge == v_incident_edge {
break;
}
}
}
initial = false;
}
Ok(())
}
fn edges_are_inside_ignored_region(
&self,
edges: &Vob32,
ignored_regions: &[(
linestring::linestring_2d::Aabb2<F>,
linestring::linestring_2d::LineString2<F>,
)],
) -> Result<bool, CenterlineError> {
let is_inside_region = |edge: BV::EdgeIndex,
region: &(
linestring::linestring_2d::Aabb2<F>,
linestring::linestring_2d::LineString2<F>,
)|
-> Result<bool, CenterlineError> {
let v0 = self.diagram.edge_get_vertex0(edge)?.unwrap();
let v0 = self.diagram.vertex_get(v0).unwrap();
let v0 = cgmath::Point2 {
x: v0.x(),
y: v0.y(),
};
let v1 = self.diagram.edge_get_vertex0(edge)?.unwrap();
let v1 = self.diagram.vertex_get(v1).unwrap();
let v1 = cgmath::Point2 {
x: v1.x(),
y: v1.y(),
};
Ok(region.0.contains_point_inclusive(v0)
&& region.0.contains_point_inclusive(v1)
&& convex_hull::ConvexHull::contains_point_inclusive(®ion.1, v0)
&& convex_hull::ConvexHull::contains_point_inclusive(®ion.1, v1))
};
'outer: for region in ignored_regions.iter().enumerate() {
for edge in edges.iter_set_bits(..) {
if !is_inside_region(BV::EdgeIndex(edge), region.1)? {
continue 'outer;
}
}
return Ok(true);
}
Ok(false)
}
fn traverse_edges(
&mut self,
maxdist: F,
ignored_regions: &[(
linestring::linestring_2d::Aabb2<F>,
linestring::linestring_2d::LineString2<F>,
)],
) -> Result<(), CenterlineError> {
let mut lines = self.lines.take().ok_or_else(|| {
CenterlineError::InternalError(format!(
"traverse_edges(): could not take lines. {}:{}",
file!(),
line!()
))
})?;
let mut linestrings = self.line_strings.take().ok_or_else(|| {
CenterlineError::InternalError(format!(
"traverse_edges(): could not take linestrings. {}:{}",
file!(),
line!()
))
})?;
let mut ignored_edges = self
.ignored_edges
.take()
.unwrap_or_else(|| Vob32::fill(self.diagram.edges().len()));
#[cfg(feature = "console_debug")]
let edge_lines = ahash::AHashMap::<usize, [F; 4]>::default();
linestrings.clear();
lines.clear();
if !ignored_regions.is_empty() {
let mut searched_edges_v = Vec::<Vob32>::new();
let mut searched_edges_s = ignored_edges.clone();
for it in self.diagram.edges().iter().enumerate() {
if searched_edges_s.get_f(it.0) {
continue;
}
let mut edges = Vob32::fill(self.diagram.edges().len());
self.mark_connected_edges(BV::EdgeIndex(it.0), &mut edges, true)?;
let _ = searched_edges_s.or(&edges);
searched_edges_v.push(edges);
}
for edges in searched_edges_v.iter() {
if self.edges_are_inside_ignored_region(edges, ignored_regions)? {
let _ = ignored_edges.or(edges);
continue;
} else {
}
}
}
let mut used_edges = ignored_edges.clone();
for it in self.diagram.edges().iter().enumerate() {
if used_edges.get_f(it.0) {
continue;
}
let edge_id = BV::EdgeIndex(it.0);
self.traverse_edge(
edge_id,
false,
&ignored_edges,
&mut used_edges,
&mut lines,
&mut linestrings,
maxdist,
)?;
}
for it in self.diagram.edges().iter().enumerate() {
if used_edges.get_f(it.0) {
continue;
}
let edge_id = BV::EdgeIndex(it.0);
#[cfg(feature = "console_debug")]
println!(
"Did not use all edges, forcing the use of edge:{}",
edge_id.0
);
self.traverse_edge(
edge_id,
true,
&ignored_edges,
&mut used_edges,
&mut lines,
&mut linestrings,
maxdist,
)?;
}
#[cfg(feature = "console_debug")]
{
println!("Got {} single lines", lines.len());
println!("Got {} linestrings", linestrings.len());
println!(
" ignored_edges {:?}",
ignored_edges
.iter_storage()
.map(|s| format!("{:#02X} ", s)[2..].to_string())
.collect::<String>()
);
println!(
" used_edges {:?}",
used_edges
.iter_storage()
.map(|s| format!("{:#02X} ", s)[2..].to_string())
.collect::<String>()
);
}
self.lines = Some(lines);
self.line_strings = Some(linestrings);
#[cfg(feature = "console_debug")]
{
self.debug_edges = Some(edge_lines);
}
Ok(())
}
fn traverse_cells(
&mut self,
maxdist: F,
ignored_regions: &[(
linestring::linestring_2d::Aabb2<F>,
linestring::linestring_2d::LineString2<F>,
)],
) -> Result<(), CenterlineError> {
let mut lines = self.lines.take().ok_or_else(|| {
CenterlineError::InternalError(format!(
"traverse_edges(): could not take lines. {}:{}",
file!(),
line!()
))
})?;
let mut linestrings = self.line_strings.take().ok_or_else(|| {
CenterlineError::InternalError(format!(
"traverse_edges(): could not take linestrings. {}:{}",
file!(),
line!()
))
})?;
let mut ignored_edges = self.ignored_edges.take().unwrap_or_else(|| Vob32::fill(0));
#[cfg(feature = "console_debug")]
let edge_lines = ahash::AHashMap::<usize, [F; 4]>::default();
linestrings.clear();
lines.clear();
if !ignored_regions.is_empty() {
let mut searched_edges_v = Vec::<Vob32>::new();
let mut searched_edges_s = ignored_edges.clone();
for it in self.diagram.edges().iter().enumerate() {
if searched_edges_s.get_f(it.0) {
continue;
}
let mut edges = Vob32::fill(self.diagram.edges().len());
self.mark_connected_edges(BV::EdgeIndex(it.0), &mut edges, true)?;
let _ = searched_edges_s.or(&edges);
searched_edges_v.push(edges);
}
for edges in searched_edges_v.iter() {
if self.edges_are_inside_ignored_region(edges, ignored_regions)? {
let _ = ignored_edges.or(edges);
continue;
} else {
}
}
}
let mut used_edges = ignored_edges.clone();
for it in self.diagram.edges().iter().enumerate() {
if used_edges.get_f(it.0) {
continue;
}
let edge_id = BV::EdgeIndex(it.0);
self.traverse_edge(
edge_id,
false,
&ignored_edges,
&mut used_edges,
&mut lines,
&mut linestrings,
maxdist,
)?;
}
for it in self.diagram.edges().iter().enumerate() {
if used_edges.get_f(it.0) {
continue;
}
let edge_id = BV::EdgeIndex(it.0);
#[cfg(feature = "console_debug")]
println!(
"Did not use all edges, forcing the use of edge:{}",
edge_id.0
);
self.traverse_edge(
edge_id,
true,
&ignored_edges,
&mut used_edges,
&mut lines,
&mut linestrings,
maxdist,
)?;
}
#[cfg(feature = "console_debug")]
{
println!("Got {} single lines", lines.len());
println!("Got {} linestrings", linestrings.len());
println!(
" ignored_edges {}",
ignored_edges
.iter_storage()
.map(|s| format!("{:#02X} ", s)[2..].to_string())
.collect::<String>()
);
println!(
" used_edges {}",
used_edges
.iter_storage()
.map(|s| format!("{:#02X} ", s)[2..].to_string())
.collect::<String>()
);
}
self.lines = Some(lines);
self.line_strings = Some(linestrings);
#[cfg(feature = "console_debug")]
{
self.debug_edges = Some(edge_lines);
}
Ok(())
}
#[inline(always)]
fn mark_edge_and_twin_as_used(
&self,
edge_id: BV::EdgeIndex,
used_edges: &mut Vob32,
) -> Result<(), CenterlineError> {
let _ = used_edges.set(edge_id.0, true);
#[cfg(feature = "console_debug")]
print!("marking {}", edge_id.0);
{
let twin = self.diagram.edge_get_twin(edge_id)?;
#[cfg(feature = "console_debug")]
print!(" & {}", twin.0);
if used_edges.get_f(twin.0) {
eprintln!(" TWIN was already used!!!!! edge id:{}", twin.0);
}
let _ = used_edges.set(twin.0, true);
}
Ok(())
}
#[allow(clippy::too_many_arguments)]
fn traverse_edge(
&self,
seed_edge: BV::EdgeIndex,
force_seed_edge: bool,
ignored_edges: &Vob32,
used_edges: &mut Vob32,
lines: &mut Vec<Line3<F>>,
linestrings: &mut Vec<LineString3<F>>,
maxdist: F,
) -> Result<(), CenterlineError> {
#[cfg(feature = "console_debug")]
{
println!();
println!("->traverse_edge({})", seed_edge.0);
}
#[cfg(feature = "console_debug")]
let mut mockup = Vec::<Vec<BV::EdgeIndex>>::default();
let found_edge = force_seed_edge
|| self
.diagram
.edge_rot_next_iterator(seed_edge)
.filter(|x| !ignored_edges.get_f(x.0))
.take(2) .count()
== 1;
if found_edge {
let mut start_points = VecDeque::<BV::EdgeIndex>::default();
let mut current_edge_set = Vec::<BV::EdgeIndex>::new();
start_points.push_front(seed_edge);
while !start_points.is_empty() {
#[cfg(feature = "console_debug")]
println!();
let edge = start_points.pop_front().unwrap();
if ignored_edges.get_f(edge.0) {
return Err(CenterlineError::InternalError(format!(
"should never happen: edge {} already in ignore list. {}:{}",
edge.0,
file!(),
line!()
)));
}
if used_edges.get_f(edge.0) {
#[cfg(feature = "console_debug")]
print!(" skip");
continue;
}
#[cfg(feature = "console_debug")]
println!();
current_edge_set.push(edge);
self.mark_edge_and_twin_as_used(edge, used_edges)?;
let mut next_edge = self.diagram.edge_get(edge)?.next()?;
loop {
#[cfg(feature = "console_debug")]
print!("Inner loop next_edge={} ", next_edge.0);
let next_edges: Vec<BV::EdgeIndex> = self
.diagram
.edge_rot_next_iterator(next_edge)
.filter(|x| !ignored_edges.get_f(x.0))
.collect();
#[cfg(feature = "console_debug")]
{
print!("candidates[");
for ne in next_edges.iter() {
if used_edges.get_f(ne.0) {
print!("!");
}
print!("{},", ne.0);
}
println!("]");
}
match next_edges.len() {
1 | 2 => {
let next_edges: Vec<BV::EdgeIndex> = next_edges
.into_iter()
.filter(|x| !used_edges.get_f(x.0))
.collect();
if next_edges.len() == 1 {
let e = next_edges.first().unwrap().to_owned();
current_edge_set.push(e);
self.mark_edge_and_twin_as_used(e, used_edges)?;
next_edge = self.diagram.edge_get(e)?.next()?;
} else {
let _ = self.convert_edges_to_lines(
¤t_edge_set,
lines,
linestrings,
maxdist,
)?;
#[cfg(feature = "console_debug")]
mockup.push(current_edge_set.clone());
current_edge_set.clear();
if !next_edges.is_empty() {
#[cfg(feature = "console_debug")]
print!("1|2 Pushing new start points: [");
for e in next_edges.iter() {
if !ignored_edges.get_f(e.0) && !used_edges.get_f(e.0) {
#[cfg(feature = "console_debug")]
print!("{},", e.0);
start_points.push_back(*e);
}
}
}
#[cfg(feature = "console_debug")]
{
println!("]");
println!("1|2 Starting new set");
}
break;
}
continue;
}
_ => {
let _ = self.convert_edges_to_lines(
¤t_edge_set,
lines,
linestrings,
maxdist,
)?;
if !next_edges.is_empty() {
#[cfg(feature = "console_debug")]
print!("0|_ Pushing new start points: [");
for e in next_edges.iter() {
if !ignored_edges.get_f(e.0) && !used_edges.get_f(e.0) {
#[cfg(feature = "console_debug")]
print!("{},", e.0);
start_points.push_back(*e);
}
}
#[cfg(feature = "console_debug")]
println!("]");
}
#[cfg(feature = "console_debug")]
mockup.push(current_edge_set.clone());
current_edge_set.clear();
#[cfg(feature = "console_debug")]
println!("0|_ Starting new set");
break;
}
}
}
}
} else {
#[cfg(feature = "console_debug")]
println!(
"<-traverse_edge({}) ignoring start edge, {:?}",
seed_edge.0,
self.diagram
.edge_rot_next_iterator(seed_edge)
.filter(|x| !ignored_edges.get_f(x.0))
.map(|x| x.0)
.collect::<Vec<usize>>()
);
}
Ok(())
}
fn convert_edges_to_lines(
&self,
edges: &[BV::EdgeIndex],
lines: &mut Vec<Line3<F>>,
linestrings: &mut Vec<LineString3<F>>,
maxdist: F,
) -> Result<(), CenterlineError> {
#[cfg(feature = "console_debug")]
{
println!();
println!(
"Converting {:?} to lines",
edges.iter().map(|x| x.0).collect::<Vec<usize>>()
);
}
match edges.len() {
0 => panic!(),
1 => {
let edge_id = edges.first().unwrap();
let edge = self.diagram.edge_get(*edge_id)?;
match self.convert_edge_to_shape(edge) {
Ok(linestring_3d::Shape3d::Line(l)) => lines.push(l),
Ok(linestring_3d::Shape3d::ParabolicArc(a)) => {
linestrings.push(a.discretise_3d(maxdist));
}
Ok(linestring_3d::Shape3d::Linestring(_s)) => {
panic!();
}
Err(_) => {
println!("Error :{:?}", edge);
}
}
}
_ => {
let mut ls = LineString3::<F>::default();
for edge_id in edges.iter() {
let edge = self.diagram.edge_get(*edge_id)?;
match self.convert_edge_to_shape(edge)? {
linestring_3d::Shape3d::Line(l) => {
ls.push(l.start);
ls.push(l.end);
}
linestring_3d::Shape3d::ParabolicArc(a) => {
ls.append(a.discretise_3d(maxdist));
}
linestring_3d::Shape3d::Linestring(_s) => {
return Err(CenterlineError::InternalError(format!(
"convert_edges_to_lines() got an unexpected linestring. {}:{}",
file!(),
line!()
)))
}
}
}
linestrings.push(ls);
}
}
Ok(())
}
fn convert_edge_to_shape(
&self,
edge: &BV::Edge,
) -> Result<linestring_3d::Shape3d<F>, CenterlineError> {
let edge_id = edge.id();
let edge_twin_id = self.diagram.edge_get_twin(edge_id)?;
let vertex0 = self.diagram.vertex_get(edge.vertex0().ok_or_else(|| {
CenterlineError::InternalError(format!(
"Could not find vertex 0. {}:{}",
file!(),
line!()
))
})?)?;
let vertex1 = self.diagram.edge_get_vertex1(edge_id)?.ok_or_else(|| {
CenterlineError::InternalError(format!(
"Could not find vertex 1. {}:{}",
file!(),
line!()
))
})?;
let vertex1 = self.diagram.vertex_get(vertex1)?;
#[cfg(feature = "console_debug")]
println!(
"Converting e:{:?} to line v0:{} v1:{}",
edge.id().0,
vertex0.get_id().0,
vertex1.get_id().0,
);
let start_point = cgmath::Point2 {
x: vertex0.x(),
y: vertex0.y(),
};
let end_point = cgmath::Point2 {
x: vertex1.x(),
y: vertex1.y(),
};
let cell_id = self.diagram.edge_get(edge_id)?.cell().unwrap();
let cell = self.diagram.cell_get(cell_id)?;
let twin_cell_id = self.diagram.edge_get(edge_twin_id)?.cell().unwrap();
let cell_point = if cell.contains_point() {
#[cfg(feature = "console_debug")]
println!("cell c:{}", cell_id.0);
self.retrieve_point(cell_id)?
} else {
#[cfg(feature = "console_debug")]
println!("twin cell c:{}", twin_cell_id.0);
self.retrieve_point(twin_cell_id)?
};
let segment = if cell.contains_point() {
#[cfg(feature = "console_debug")]
println!("twin segment c:{}", twin_cell_id.0);
self.retrieve_segment(twin_cell_id)?
} else {
#[cfg(feature = "console_debug")]
println!("segment c:{}", cell_id.0);
self.retrieve_segment(cell_id)?
};
let segment_start_point = cgmath::Point2 {
x: Self::i2f(segment.start.x),
y: Self::i2f(segment.start.y),
};
let segment_end_point = cgmath::Point2 {
x: Self::i2f(segment.end.x),
y: Self::i2f(segment.end.y),
};
let cell_point = cgmath::Point2 {
x: Self::i2f(cell_point.x),
y: Self::i2f(cell_point.y),
};
#[cfg(feature = "console_debug")]
{
println!("sp:[{},{}]", start_point.x, start_point.y);
println!("ep:[{},{}]", end_point.x, end_point.y);
println!(
"cp:[{},{}] sg:[{},{},{},{}]",
cell_point.x,
cell_point.y,
segment_start_point.x,
segment_start_point.y,
segment_end_point.x,
segment_end_point.y
);
}
if edge.is_curved() {
let arc = linestring_2d::VoronoiParabolicArc::new(
linestring_2d::Line2 {
start: segment_start_point,
end: segment_end_point,
},
cell_point,
start_point,
end_point,
);
#[cfg(feature = "console_debug")]
println!("Converted {:?} to {:?}", edge.id().0, arc);
Ok(linestring_3d::Shape3d::ParabolicArc(arc))
} else {
let distance_to_start = {
if vertex0.is_site_point() {
F::zero()
} else if cell.contains_point() {
let cell_point = self.retrieve_point(cell_id)?;
let cell_point = cgmath::Point2 {
x: Self::i2f(cell_point.x),
y: Self::i2f(cell_point.y),
};
-cell_point.distance(start_point)
} else {
let segment = self.retrieve_segment(cell_id)?;
let segment_start_point = cgmath::Point2 {
x: Self::i2f(segment.start.x),
y: Self::i2f(segment.start.y),
};
let segment_end_point = cgmath::Point2 {
x: Self::i2f(segment.end.x),
y: Self::i2f(segment.end.y),
};
-linestring::linestring_2d::distance_to_line_squared_safe(
segment_start_point,
segment_end_point,
start_point,
)
.sqrt()
}
};
let distance_to_end = {
if vertex1.is_site_point() {
F::zero()
} else {
let cell_id = self
.diagram
.edge_get(vertex1.get_incident_edge().unwrap())?
.cell()
.unwrap();
let cell = self.diagram.cell_get(cell_id)?;
if cell.contains_point() {
let cell_point = self.retrieve_point(cell_id)?;
let cell_point = cgmath::Point2 {
x: Self::i2f(cell_point.x),
y: Self::i2f(cell_point.y),
};
-cell_point.distance(end_point)
} else {
let segment = self.retrieve_segment(cell_id)?;
let segment_start_point = cgmath::Point2 {
x: Self::i2f(segment.start.x),
y: Self::i2f(segment.start.y),
};
let segment_end_point = cgmath::Point2 {
x: Self::i2f(segment.end.x),
y: Self::i2f(segment.end.y),
};
-linestring::linestring_2d::distance_to_line_squared_safe(
segment_start_point,
segment_end_point,
end_point,
)
.sqrt()
}
}
};
let line = Line3 {
start: cgmath::Point3 {
x: start_point.x,
y: start_point.y,
z: distance_to_start,
},
end: cgmath::Point3 {
x: end_point.x,
y: end_point.y,
z: distance_to_end,
},
};
#[cfg(feature = "console_debug")]
println!("Converted {:?} to {:?}", edge.id().0, line);
Ok(linestring_3d::Shape3d::Line(line))
}
}
#[inline(always)]
pub fn i2f(input: I) -> F {
num::cast::<I, F>(input).unwrap()
}
}