use std::collections::HashSet;
use rayon::prelude::*;
use crate::cluster::{connected_components, dijkstra, knn_graph, radius_graph};
use crate::filters::{planarity_linearity, statistical_outlier_mask, voxel_downsample_indices};
use crate::spatial::KdTree;
use crate::Point;
#[derive(Debug, Clone)]
pub struct WoodParams {
pub k: usize,
pub high_threshold: f64,
pub medium_threshold: f64,
pub scale_radius: f64,
pub graph_k: usize,
pub max_edge: f64,
pub base_height: f64,
pub target_res: f64,
pub min_passage: usize,
pub assign_dist: f64,
pub assign_scale: f64,
pub component_res: f64,
pub component_min: usize,
pub sor_k: usize,
pub sor_std: f64,
pub dilate_dist: f64,
pub passage: bool,
}
impl Default for WoodParams {
fn default() -> Self {
WoodParams { k: 20, high_threshold: 0.85, medium_threshold: 0.75, scale_radius: 0.0, graph_k: 10, max_edge: 1.0, base_height: 0.25, target_res: 0.2, min_passage: 3, assign_dist: 0.05, assign_scale: 0.0, component_res: 0.05, component_min: 200, sor_k: 50, sor_std: 1.0, dilate_dist: 0.03, passage: true }
}
}
fn anisotropy(points: &[Point], nbrs: &[(usize, f64)]) -> f64 {
if nbrs.len() < 4 {
return 0.0;
}
let step = nbrs.len().div_ceil(64);
let sel: Vec<usize> = nbrs.iter().step_by(step).map(|&(i, _)| i).collect();
let n = sel.len() as f64;
let mut c = [0.0; 3];
for &i in &sel {
for a in 0..3 {
c[a] += points[i][a] / n;
}
}
let mut cov = nalgebra::Matrix3::<f64>::zeros();
for &i in &sel {
let d = [points[i][0] - c[0], points[i][1] - c[1], points[i][2] - c[2]];
for a in 0..3 {
for b in 0..3 {
cov[(a, b)] += d[a] * d[b];
}
}
}
let e = cov.symmetric_eigen().eigenvalues;
let (lo, hi) = (e.min(), e.max());
if hi > 1e-14 { 1.0 - (lo / hi).max(0.0) } else { 0.0 }
}
pub fn wood_mask(points: &[Point], p: &WoodParams) -> Vec<bool> {
let n = points.len();
if n == 0 {
return Vec::new();
}
let (pl, li) = planarity_linearity(points, p.k);
let mut like: Vec<f64> = pl.iter().zip(&li).map(|(a, b)| a + b).collect();
if p.scale_radius > 0.0 {
let tree = KdTree::new(points);
let wide: Vec<f64> = points.par_iter().map(|q| anisotropy(points, &tree.within(q, p.scale_radius))).collect();
for (l, w) in like.iter_mut().zip(wide) {
*l = l.min(w);
}
}
let mut wood = vec![false; n];
if p.passage && n > p.graph_k {
let graph = knn_graph(points, p.graph_k, p.max_edge);
let (_, comp) = connected_components(&graph);
let main = super::model::largest_component(&comp);
let z0 = points.iter().zip(&comp).filter(|(_, &c)| c == main).map(|(q, _)| q[2]).fold(f64::INFINITY, f64::min);
let sources: Vec<usize> = (0..n).filter(|&i| comp[i] == main && points[i][2] <= z0 + p.base_height).collect();
let (dist, _, pred) = dijkstra(&graph, &sources);
let targets: HashSet<usize> = voxel_downsample_indices(points, p.target_res).into_iter().collect();
let mut order: Vec<usize> = (0..n).filter(|&i| dist[i].is_finite()).collect();
order.sort_by(|&a, &b| dist[b].partial_cmp(&dist[a]).unwrap());
let mut passage = vec![0usize; n];
for &i in &order {
if targets.contains(&i) {
passage[i] += 1;
}
if pred[i] != usize::MAX {
passage[pred[i]] += passage[i];
}
}
let z1 = points.iter().zip(&comp).filter(|(_, &c)| c == main).map(|(q, _)| q[2]).fold(f64::NEG_INFINITY, f64::max);
let height = (z1 - z0).max(0.0);
let n_targets = targets.len().max(1) as f64;
let tree = KdTree::new(points);
let mut seed = vec![false; n];
for i in 0..n {
if passage[i] >= p.min_passage {
seed[i] = true;
let share = (passage[i] as f64 / n_targets).min(1.0);
let reach = p.assign_dist + p.assign_scale * height * share.sqrt();
for (j, _) in tree.within(&points[i], reach) {
seed[j] = true;
}
}
}
wood = seed;
}
let high: Vec<usize> = (0..n).filter(|&i| wood[i] || like[i] > p.high_threshold).collect();
wood = keep_components(points, &high, &wood, p.component_res, p.component_min, n);
if p.medium_threshold < p.high_threshold {
let mut medium: Vec<usize> = (0..n).filter(|&i| wood[i] || like[i] > p.medium_threshold).collect();
if p.sor_k > 0 {
let sub: Vec<Point> = medium.iter().map(|&i| points[i]).collect();
let keep = statistical_outlier_mask(&sub, p.sor_k, p.sor_std);
medium = medium.iter().zip(&keep).filter(|(&i, &k)| k || wood[i]).map(|(&i, _)| i).collect();
}
wood = keep_components(points, &medium, &wood, p.component_res, usize::MAX, n);
}
if p.dilate_dist > 0.0 {
let tree = KdTree::new(points);
let mut out = wood.clone();
for i in 0..n {
if wood[i] {
for (j, _) in tree.within(&points[i], p.dilate_dist) {
out[j] = true;
}
}
}
wood = out;
}
wood
}
fn keep_components(points: &[Point], subset: &[usize], wood: &[bool], res: f64, min_size: usize, n: usize) -> Vec<bool> {
let mut out = vec![false; n];
if subset.is_empty() {
return out;
}
let sub: Vec<Point> = subset.iter().map(|&i| points[i]).collect();
let graph = radius_graph(&sub, res * 3f64.sqrt());
let (count, label) = connected_components(&graph);
let mut size = vec![0usize; count];
let mut has_wood = vec![false; count];
for (k, &i) in subset.iter().enumerate() {
size[label[k]] += 1;
if wood[i] {
has_wood[label[k]] = true;
}
}
for (k, &i) in subset.iter().enumerate() {
if has_wood[label[k]] || size[label[k]] >= min_size {
out[i] = true;
}
}
out
}
#[derive(Debug, Clone)]
pub struct GbsParams {
pub graph_k: usize,
pub max_edge: f64,
pub base_height: f64,
pub intervals: Vec<f64>,
pub max_angle: f64,
pub linearity: f64,
pub circle_error: f64,
pub min_points: usize,
}
impl Default for GbsParams {
fn default() -> Self {
GbsParams { graph_k: 8, max_edge: 1.0, base_height: 0.25, intervals: vec![0.1, 0.2, 0.3, 0.5, 1.0], max_angle: std::f64::consts::FRAC_PI_4, linearity: 0.9, circle_error: 0.2, min_points: 10 }
}
}
fn find(parent: &mut [usize], mut i: usize) -> usize {
while parent[i] != i {
parent[i] = parent[parent[i]];
i = parent[i];
}
i
}
fn classify_piece(points: &[Point], members: &[usize], step: &[Point], interval: f64, p: &GbsParams) -> Option<f64> {
use crate::transform::{cross, dot, norm, normalize, scale, sub};
let mut axis = [0.0; 3];
for &i in members {
for a in 0..3 {
axis[a] += step[i][a];
}
}
if norm(&axis) < 1e-12 {
return None;
}
let axis = normalize(&axis);
let n = members.len() as f64;
let mut c = [0.0; 3];
for &i in members {
for a in 0..3 {
c[a] += points[i][a] / n;
}
}
let helper = if axis[0].abs() < 0.9 { [1.0, 0.0, 0.0] } else { [0.0, 1.0, 0.0] };
let u = normalize(&cross(&axis, &helper));
let v = cross(&axis, &u);
let (mut lo, mut hi) = (f64::INFINITY, f64::NEG_INFINITY);
let (mut s_ax, mut s_u, mut s_v) = (0.0, 0.0, 0.0);
let mut xy: Vec<[f64; 2]> = Vec::with_capacity(members.len());
for &i in members {
let d = sub(&points[i], &c);
let (t, a, b) = (dot(&d, &axis), dot(&d, &u), dot(&d, &v));
lo = lo.min(t);
hi = hi.max(t);
s_ax += t * t;
s_u += a * a;
s_v += b * b;
xy.push([a, b]);
}
let extent = hi - lo;
if extent < 0.75 * interval || extent > 1.25 * interval {
return None;
}
let total = s_ax + s_u + s_v;
if total <= 0.0 {
return None;
}
let spread = s_u.min(s_v) / total;
if spread > 0.01 {
if let Some((cx, cy, r)) = crate::stems::fit_circle_refined(&xy) {
if r > 0.0 {
let rmse = (xy.iter().map(|q| ((q[0] - cx).hypot(q[1] - cy) - r).powi(2)).sum::<f64>() / n).sqrt();
if rmse / r < p.circle_error {
return Some(r);
}
}
}
}
let linearity = s_ax / total;
let _ = scale;
(linearity > p.linearity).then_some(-linearity)
}
pub fn gbs_mask(points: &[Point], p: &GbsParams) -> Vec<bool> {
use crate::transform::{dot, norm, sub};
let n = points.len();
if n <= p.graph_k {
return vec![false; n];
}
let graph = knn_graph(points, p.graph_k, p.max_edge);
let (_, comp) = connected_components(&graph);
let main = super::model::largest_component(&comp);
let z0 = points.iter().zip(&comp).filter(|(_, &c)| c == main).map(|(q, _)| q[2]).fold(f64::INFINITY, f64::min);
let sources: Vec<usize> = (0..n).filter(|&i| comp[i] == main && points[i][2] <= z0 + p.base_height).collect();
let (dist, _, pred) = dijkstra(&graph, &sources);
let step: Vec<Point> = (0..n).map(|i| if pred[i] != usize::MAX { sub(&points[i], &points[pred[i]]) } else { [0.0, 0.0, 0.0] }).collect();
let step_len: Vec<f64> = step.iter().map(norm).collect();
let keep_edge = |i: usize, j: usize, w: f64| -> bool {
if pred[i] == usize::MAX || pred[j] == usize::MAX {
return true; }
if w > 2.0 * step_len[i].min(step_len[j]) {
return false;
}
let c = (dot(&step[i], &step[j]) / (step_len[i] * step_len[j]).max(1e-18)).clamp(-1.0, 1.0);
let a = c.acos();
a.min(std::f64::consts::PI - a) <= p.max_angle
};
let min_piece = p.min_points.max(n / 20_000);
let mut init = vec![false; n];
for &interval in &p.intervals {
let bin: Vec<i64> = dist.iter().map(|d| if d.is_finite() { (d / interval).floor() as i64 } else { -1 }).collect();
let mut parent: Vec<usize> = (0..n).collect();
for i in 0..n {
if bin[i] < 0 {
continue;
}
for (j, w) in graph.edges(i) {
if j > i && bin[j] == bin[i] && keep_edge(i, j, w) {
let (a, b) = (find(&mut parent, i), find(&mut parent, j));
if a != b {
parent[a] = b;
}
}
}
}
let mut pieces: std::collections::HashMap<usize, Vec<usize>> = std::collections::HashMap::new();
for i in 0..n {
if bin[i] >= 0 {
let r = find(&mut parent, i);
pieces.entry(r).or_default().push(i);
}
}
let mut pieces: Vec<Vec<usize>> = pieces.into_values().filter(|m| m.len() >= min_piece).collect();
pieces.sort_unstable_by_key(|m| m[0]);
let mut class: Vec<Option<f64>> = pieces.par_iter().map(|m| classify_piece(points, m, &step, interval, p)).collect();
let mut piece_of = vec![usize::MAX; n];
for (k, m) in pieces.iter().enumerate() {
for &i in m {
piece_of[i] = k;
}
}
for _ in 0..3 {
for k in 0..pieces.len() {
let Some(c) = class[k] else { continue };
let mut i = pieces[k][0];
while pred[i] != usize::MAX {
i = pred[i];
let q = piece_of[i];
if q != usize::MAX && q != k {
if let Some(below) = class[q] {
if c > below {
class[k] = None;
}
break;
}
}
}
}
}
for (k, m) in pieces.iter().enumerate() {
if class[k].is_some() {
for &i in m {
init[i] = true;
}
}
}
}
let mut wood = vec![false; n];
for i in 0..n {
if init[i] {
let mut j = i;
while !wood[j] {
wood[j] = true;
if pred[j] == usize::MAX {
break;
}
j = pred[j];
}
}
}
let mut frontier: Vec<usize> = (0..n).filter(|&i| wood[i]).collect();
for _ in 0..100 {
let mut next = Vec::new();
for &i in &frontier {
for (j, _) in graph.edges(i) {
if !wood[j] && dist[j] <= dist[i] {
wood[j] = true;
next.push(j);
}
}
}
if next.is_empty() {
break;
}
frontier = next;
}
let seed: Vec<usize> = (0..n).filter(|&i| wood[i] && pred[i] != usize::MAX).collect();
for i in seed {
for (j, w) in graph.edges(i) {
if !wood[j] && w < 2.0 * step_len[i] {
wood[j] = true;
}
}
}
for &s in &sources {
wood[s] = true;
}
wood
}