use std::cmp::Ordering;
use rustc_hash::FxHashMap as HashMap;
use crate::disjoint_sets::DisjointSets;
use crate::linalg::Vec3;
use super::exact::rational::R3;
use super::exact::Sign;
use super::intersection_graph::{edge_key, EdgeKey, IntersectionGraph};
pub use super::cells_extract::{extract, in_result};
pub const NORMAL: usize = 0;
pub const ANTI: usize = 1;
#[inline]
fn node(piece: usize, side: usize) -> u32 {
(2 * piece + side) as u32
}
pub struct CellComplex {
pub cell_of: Vec<u32>,
pub num_cells: usize,
pub walls: Vec<Wall>,
}
impl CellComplex {
#[inline]
pub fn cell(&self, piece: usize, side: usize) -> usize {
self.cell_of[node(piece, side) as usize] as usize
}
}
#[derive(Clone, Copy)]
pub struct VertTables<'a> {
pub verts: &'a [R3],
pub verts_f64: &'a [Vec3],
}
impl<'a> VertTables<'a> {
pub fn of(graph: &'a IntersectionGraph) -> Self {
VertTables {
verts: &graph.verts,
verts_f64: &graph.verts_f64,
}
}
}
pub struct Inc {
pub id: usize,
pub forward: bool,
pub apex: u32,
}
#[inline]
fn ccw_side(forward: bool) -> usize {
if forward {
NORMAL
} else {
ANTI
}
}
#[inline]
fn cw_side(forward: bool) -> usize {
1 - ccw_side(forward)
}
impl Inc {
#[inline]
fn ccw_side(&self) -> usize {
ccw_side(self.forward)
}
#[inline]
fn cw_side(&self) -> usize {
cw_side(self.forward)
}
}
pub fn build_cells(graph: &IntersectionGraph) -> CellComplex {
build_cells_with_token(graph, None).expect("uncancellable build_cells cannot cancel")
}
pub fn build_cells_with_token(
graph: &IntersectionGraph,
token: Option<&crate::cancel::CancelToken>,
) -> Option<CellComplex> {
build_cells_with_progress(graph, token, None)
}
pub fn build_cells_with_progress(
graph: &IntersectionGraph,
token: Option<&crate::cancel::CancelToken>,
progress: Option<&crate::progress::ProgressReporter>,
) -> Option<CellComplex> {
let n = graph.pieces.len();
let vt = VertTables::of(graph);
let ds = DisjointSets::new((2 * n).max(1) as u32);
let mut incident: Vec<(EdgeKey, usize, bool, u32)> = Vec::with_capacity(3 * n);
for (pi, piece) in graph.pieces.iter().enumerate() {
let vi = piece.vi;
for e in 0..3 {
let (a, b) = (vi[e], vi[(e + 1) % 3]);
incident.push((edge_key(a, b), pi, a < b, vi[(e + 2) % 3]));
}
}
incident.sort_unstable();
crate::progress::begin_phase(progress, crate::progress::Phase::Cells, incident.len() as u64);
let mut at = 0;
while at < incident.len() {
if crate::cancel::is_cancelled(token) {
return None;
}
let key = &incident[at].0;
let mut end = at + 1;
while end < incident.len() && incident[end].0 == *key {
end += 1;
}
let raw = &incident[at..end];
if let Some(pr) = progress {
pr.advance((end - at) as u64);
}
at = end;
if raw.len() < 2 {
continue; }
if raw.len() == 2 {
let pt = |v: u32| {
let p = graph.verts_f64[v as usize];
[p.x, p.y, p.z]
};
let sign =
super::exact::approx::orient3d_a(pt(key.0), pt(key.1), pt(raw[0].3), pt(raw[1].3));
if matches!(sign, Some(Sign::Neg) | Some(Sign::Pos)) {
let (p0, fw0) = (raw[0].1, raw[0].2);
let (p1, fw1) = (raw[1].1, raw[1].2);
ds.unite(node(p0, ccw_side(fw0)), node(p1, cw_side(fw1)));
ds.unite(node(p1, ccw_side(fw1)), node(p0, cw_side(fw0)));
continue;
}
}
let Some((incs, groups)) = radial_fan(key.0, key.1, raw, vt) else {
continue;
};
for gi in 0..groups.len() {
let (s, e) = groups[gi];
for k in s..e {
ds.unite(
node(incs[s].id, incs[s].ccw_side()),
node(incs[k].id, incs[k].ccw_side()),
);
ds.unite(
node(incs[s].id, incs[s].cw_side()),
node(incs[k].id, incs[k].cw_side()),
);
}
let (ns, _) = groups[(gi + 1) % groups.len()];
if groups.len() > 1 {
ds.unite(
node(incs[s].id, incs[s].ccw_side()),
node(incs[ns].id, incs[ns].cw_side()),
);
}
}
}
let mut cell_of = vec![0u32; 2 * n];
let mut remap = vec![u32::MAX; 2 * n];
let mut num_cells = 0u32;
for i in 0..(2 * n) {
let root = ds.find(i as u32) as usize;
if remap[root] == u32::MAX {
remap[root] = num_cells;
num_cells += 1;
}
cell_of[i] = remap[root];
}
Some(CellComplex {
num_cells: num_cells as usize,
cell_of,
walls: walls(graph),
})
}
fn orient_edge(vt: VertTables, k0: u32, k1: u32, a: u32, b: u32) -> Sign {
let pt = |v: u32| {
let p = vt.verts_f64[v as usize];
[p.x, p.y, p.z]
};
match super::exact::approx::orient3d_a(pt(k0), pt(k1), pt(a), pt(b)) {
Some(s @ (Sign::Pos | Sign::Neg)) => s,
_ => super::exact::predicates::orient3d_r(
&vt.verts[k0 as usize],
&vt.verts[k1 as usize],
&vt.verts[a as usize],
&vt.verts[b as usize],
),
}
}
fn radial_cross(vt: VertTables, k0: u32, k1: u32, a: u32) -> R3 {
let w = vt.verts[k1 as usize].sub(&vt.verts[k0 as usize]);
let d = vt.verts[a as usize].sub(&vt.verts[k0 as usize]);
w.cross(&d)
}
#[derive(Clone, Copy)]
struct Approx {
v: f64,
err: f64,
}
const U: f64 = f64::EPSILON;
impl Approx {
fn input(v: f64) -> Self {
Approx { v, err: U * v.abs() }
}
fn sub(self, o: Approx) -> Self {
let v = self.v - o.v;
Approx { v, err: self.err + o.err + U * v.abs() }
}
fn mul(self, o: Approx) -> Self {
let v = self.v * o.v;
Approx {
v,
err: self.v.abs() * o.err + self.err * o.v.abs() + self.err * o.err + U * v.abs(),
}
}
fn add(self, o: Approx) -> Self {
let v = self.v + o.v;
Approx { v, err: self.err + o.err + U * v.abs() }
}
fn sign(self) -> Option<Sign> {
if self.v.abs() > self.err {
Some(if self.v > 0.0 { Sign::Pos } else { Sign::Neg })
} else {
None
}
}
}
fn radial_cross_a(vt: VertTables, k0: u32, k1: u32, a: u32) -> [Approx; 3] {
let p = |v: u32| {
let p = vt.verts_f64[v as usize];
[Approx::input(p.x), Approx::input(p.y), Approx::input(p.z)]
};
let (p0, p1, pa) = (p(k0), p(k1), p(a));
let w = [p1[0].sub(p0[0]), p1[1].sub(p0[1]), p1[2].sub(p0[2])];
let d = [pa[0].sub(p0[0]), pa[1].sub(p0[1]), pa[2].sub(p0[2])];
[
w[1].mul(d[2]).sub(w[2].mul(d[1])),
w[2].mul(d[0]).sub(w[0].mul(d[2])),
w[0].mul(d[1]).sub(w[1].mul(d[0])),
]
}
fn on_axis(vt: VertTables, k0: u32, k1: u32, a: u32) -> bool {
if radial_cross_a(vt, k0, k1, a)
.iter()
.any(|c| c.sign().is_some())
{
return false;
}
radial_cross(vt, k0, k1, a).is_zero()
}
fn same_ray_sign(vt: VertTables, k0: u32, k1: u32, a: u32, b: u32) -> Sign {
let ca = radial_cross_a(vt, k0, k1, a);
let cb = radial_cross_a(vt, k0, k1, b);
let dot = ca[0]
.mul(cb[0])
.add(ca[1].mul(cb[1]))
.add(ca[2].mul(cb[2]));
if let Some(s) = dot.sign() {
return s;
}
let ea = radial_cross(vt, k0, k1, a);
let eb = radial_cross(vt, k0, k1, b);
let exact = &ea.x * &eb.x + &ea.y * &eb.y + &ea.z * &eb.z;
Sign::of_rat(&exact)
}
pub fn radial_fan(
k0: u32,
k1: u32,
raw: &[(EdgeKey, usize, bool, u32)],
vt: VertTables,
) -> Option<(Vec<Inc>, Vec<(usize, usize)>)> {
let mut incs: Vec<Inc> = raw
.iter()
.filter(|&&(_, _, _, apex)| !on_axis(vt, k0, k1, apex))
.map(|&(_, id, forward, apex)| Inc { id, forward, apex })
.collect();
if incs.len() < 2 {
return None;
}
let r = incs[0].apex;
let class = |apex: u32| -> u8 {
if apex == r {
return 0;
}
match orient_edge(vt, k0, k1, r, apex) {
Sign::Pos => 1,
Sign::Neg => 3,
Sign::Zero => match same_ray_sign(vt, k0, k1, r, apex) {
Sign::Pos => 0,
Sign::Neg => 2,
Sign::Zero => unreachable!("parallel nonzero radial rays have nonzero dot"),
},
}
};
let classes: HashMap<u32, u8> = incs.iter().map(|i| (i.apex, class(i.apex))).collect();
incs.sort_by(|a, b| {
let (ca, cb) = (classes[&a.apex], classes[&b.apex]);
ca.cmp(&cb)
.then_with(|| {
if ca != 1 && ca != 3 || a.apex == b.apex {
Ordering::Equal } else {
match orient_edge(vt, k0, k1, a.apex, b.apex) {
Sign::Pos => Ordering::Less,
Sign::Neg => Ordering::Greater,
Sign::Zero => Ordering::Equal,
}
}
})
.then_with(|| a.id.cmp(&b.id))
});
let mut groups = Vec::new();
let mut i = 0;
while i < incs.len() {
let mut j = i + 1;
while j < incs.len() && {
let (ci, cj) = (classes[&incs[i].apex], classes[&incs[j].apex]);
ci == cj
&& (ci == 0
|| ci == 2
|| incs[i].apex == incs[j].apex
|| orient_edge(vt, k0, k1, incs[i].apex, incs[j].apex) == Sign::Zero)
} {
j += 1;
}
groups.push((i, j));
i = j;
}
Some((incs, groups))
}
pub struct Windings {
pub w: Vec<[i32; 2]>,
pub known: Vec<bool>,
}
impl Windings {
pub fn complete(&self) -> bool {
self.known.iter().all(|&k| k)
}
}
pub fn windings(
graph: &IntersectionGraph,
complex: &CellComplex,
tris: [&[[Vec3; 3]]; 2],
) -> Windings {
let rat = [to_rational(tris[0]), to_rational(tris[1])];
let bx = [tri_boxes(tris[0]), tri_boxes(tris[1])];
let mut out = Windings {
w: vec![[0i32; 2]; complex.num_cells],
known: vec![false; complex.num_cells],
};
seed_unreached(
graph,
complex,
&mut out,
tris,
[&rat[0], &rat[1]],
[&bx[0], &bx[1]],
);
out
}
fn to_rational(tris: &[[Vec3; 3]]) -> Vec<[R3; 3]> {
tris.iter()
.map(|t| [R3::from_vec3(t[0]), R3::from_vec3(t[1]), R3::from_vec3(t[2])])
.collect()
}
fn tri_boxes(tris: &[[Vec3; 3]]) -> Vec<crate::types::Box> {
tris.iter()
.map(|t| {
let mut b = crate::types::Box::from_points(t[0], t[1]);
b.union_point(t[2]);
b
})
.collect()
}
pub fn seed_unreached(
graph: &IntersectionGraph,
complex: &CellComplex,
out: &mut Windings,
tris_f64: [&[[Vec3; 3]]; 2],
tris_r: [&[[R3; 3]]; 2],
boxes: [&[crate::types::Box]; 2],
) {
if out.complete() {
return;
}
let adj = cell_adjacency(complex);
let mut rep: Vec<Option<(usize, usize)>> = vec![None; complex.num_cells];
for pi in 0..graph.pieces.len() {
for side in [NORMAL, ANTI] {
let c = complex.cell(pi, side);
if rep[c].is_none() {
rep[c] = Some((pi, side));
}
}
}
for c in 0..complex.num_cells {
if out.known[c] {
continue;
}
let Some((pi, side)) = rep[c] else { continue };
let pv = graph.piece_verts(pi);
let point = super::ray_shoot::piece_centroid(pv);
let n = pv[1].sub(pv[0]).cross(&pv[2].sub(pv[0]));
let outward = if side == NORMAL {
n
} else {
R3::new(-&n.x, -&n.y, -&n.z)
};
let mut w = [0i32; 2];
for m in 0..2 {
w[m] = super::ray_shoot::winding_off_surface(
&point,
&outward,
tris_r[m],
tris_f64[m],
boxes[m],
);
}
seed(out, c, w);
bfs(&adj, out, c);
}
}
fn seed(out: &mut Windings, cell: usize, w: [i32; 2]) {
out.w[cell] = w;
out.known[cell] = true;
}
fn cell_adjacency(complex: &CellComplex) -> Vec<Vec<(usize, [i32; 2])>> {
let mut adj: Vec<Vec<(usize, [i32; 2])>> = vec![Vec::new(); complex.num_cells];
for &Wall { rep, delta } in &complex.walls {
let (cn, ca) = (complex.cell(rep, NORMAL), complex.cell(rep, ANTI));
if cn == ca {
continue; }
adj[cn].push((ca, delta));
adj[ca].push((cn, [-delta[0], -delta[1]]));
}
for a in adj.iter_mut() {
a.sort_unstable();
a.dedup();
}
adj
}
pub fn inconsistent_walls(
complex: &CellComplex,
wind: &Windings,
) -> Vec<(usize, [i32; 2], [i32; 2])> {
let mut bad = Vec::new();
for &Wall { rep, delta } in &complex.walls {
let (cn, ca) = (complex.cell(rep, NORMAL), complex.cell(rep, ANTI));
if cn == ca || !wind.known[cn] || !wind.known[ca] {
continue;
}
let actual = [
wind.w[ca][0] - wind.w[cn][0],
wind.w[ca][1] - wind.w[cn][1],
];
if actual != delta {
bad.push((rep, delta, actual));
}
}
bad
}
#[derive(Clone, Copy)]
pub struct Wall {
pub rep: usize,
pub delta: [i32; 2],
}
fn walls(graph: &IntersectionGraph) -> Vec<Wall> {
let mut by_tri: HashMap<[u32; 3], (usize, bool, [i32; 2])> = HashMap::default();
for (pi, piece) in graph.pieces.iter().enumerate() {
let (key, parity) = canonical(piece.vi);
let m = piece.mesh as usize;
let entry = by_tri.entry(key).or_insert((pi, parity, [0; 2]));
entry.2[m] += if parity == entry.1 { 1 } else { -1 };
}
let mut out: Vec<Wall> = by_tri
.into_values()
.map(|(rep, _, delta)| Wall { rep, delta })
.collect();
out.sort_unstable_by_key(|w| w.rep);
out
}
fn canonical(vi: [u32; 3]) -> ([u32; 3], bool) {
let mut sorted = vi;
sorted.sort_unstable();
let i = (0..3).min_by_key(|&i| vi[i]).unwrap_or(0);
let rotated = [vi[i], vi[(i + 1) % 3], vi[(i + 2) % 3]];
(sorted, rotated == sorted)
}
fn bfs(adj: &[Vec<(usize, [i32; 2])>], out: &mut Windings, start: usize) {
let mut queue = std::collections::VecDeque::from([start]);
while let Some(c) = queue.pop_front() {
let base = out.w[c];
for &(next, d) in &adj[c] {
if out.known[next] {
continue;
}
seed(out, next, [base[0] + d[0], base[1] + d[1]]);
queue.push_back(next);
}
}
}
#[cfg(test)]
#[path = "cells_tests.rs"]
mod tests;