1186 lines
38 KiB
Rust
1186 lines
38 KiB
Rust
use crate::density::{DensityMap, SurfaceType};
|
|
use crate::mesh::Vec3;
|
|
use crate::sparse_grid::{SparseGrid, SurfaceSample};
|
|
use crate::DecompileConfig;
|
|
|
|
#[derive(Debug, Clone)]
|
|
pub struct DetectedPrimitive {
|
|
pub kind: PrimitiveKind,
|
|
pub support: Vec<usize>,
|
|
pub fit_error: f64,
|
|
}
|
|
|
|
#[derive(Debug, Clone)]
|
|
pub enum PrimitiveKind {
|
|
Plane { point: Vec3, normal: Vec3 },
|
|
Sphere { center: Vec3, radius: f64 },
|
|
Cylinder { point: Vec3, axis: Vec3, radius: f64 },
|
|
Box {
|
|
center: Vec3,
|
|
half_extents: [f64; 3],
|
|
rotation_axis: Vec3,
|
|
rotation_angle: f64,
|
|
},
|
|
}
|
|
|
|
pub fn detect_primitives(
|
|
grid: &SparseGrid,
|
|
density_map: &DensityMap,
|
|
config: &DecompileConfig,
|
|
) -> Vec<DetectedPrimitive> {
|
|
let samples = grid.surface_samples();
|
|
if samples.is_empty() {
|
|
return Vec::new();
|
|
}
|
|
|
|
let mut remaining: Vec<bool> = vec![true; samples.len()];
|
|
let mut primitives = Vec::new();
|
|
let min_support = (samples.len() as f64 * config.min_support_ratio).max(3.0) as usize;
|
|
|
|
// Iterative RANSAC: detect largest primitive, remove its inliers, repeat
|
|
for _ in 0..20 {
|
|
let active_count = remaining.iter().filter(|&&r| r).count();
|
|
if active_count < min_support {
|
|
break;
|
|
}
|
|
|
|
let best = ransac_detect(
|
|
&samples,
|
|
&remaining,
|
|
density_map,
|
|
config,
|
|
);
|
|
|
|
if let Some(mut prim) = best {
|
|
if prim.support.len() < min_support {
|
|
break;
|
|
}
|
|
refine_and_expand(&mut prim, &samples, &remaining, config);
|
|
for &idx in &prim.support {
|
|
remaining[idx] = false;
|
|
}
|
|
prim.support.sort_unstable();
|
|
primitives.push(prim);
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Post-detection: try to consolidate clusters of primitives into boxes.
|
|
// Group primitives by spatial proximity and test if each cluster is a box.
|
|
consolidate_into_boxes(&mut primitives, &samples, config);
|
|
|
|
primitives
|
|
}
|
|
|
|
/// Try to replace non-box primitives with box detections.
|
|
///
|
|
/// For each non-box primitive, check if its support points form a box.
|
|
/// If the box fit is better (higher coverage), replace the original.
|
|
/// Then try merging overlapping box-candidates and neighboring primitives.
|
|
fn consolidate_into_boxes(
|
|
primitives: &mut Vec<DetectedPrimitive>,
|
|
samples: &[SurfaceSample],
|
|
config: &DecompileConfig,
|
|
) {
|
|
// Pass 1: for each non-box primitive, split its support into spatial clusters
|
|
// and try fitting a box to each cluster.
|
|
let mut new_prims = Vec::new();
|
|
let mut any_replaced = false;
|
|
|
|
for prim in primitives.iter() {
|
|
if matches!(prim.kind, PrimitiveKind::Box { .. }) || prim.support.len() < 6 {
|
|
new_prims.push(prim.clone());
|
|
continue;
|
|
}
|
|
|
|
let clusters = spatial_cluster(&prim.support, samples, config.distance_threshold * 5.0);
|
|
let mut box_replacements: Vec<DetectedPrimitive> = Vec::new();
|
|
|
|
for cluster in &clusters {
|
|
if cluster.len() < 6 { continue; }
|
|
if let Some(box_prim) = try_fit_box_relaxed(samples, cluster, config) {
|
|
// Box must capture at least 60% of the cluster's points
|
|
let coverage = box_prim.support.len() as f64 / cluster.len() as f64;
|
|
// Box mean error must be lower than original
|
|
let box_mean_err = box_prim.fit_error / box_prim.support.len().max(1) as f64;
|
|
let orig_mean_err = prim.fit_error / prim.support.len().max(1) as f64;
|
|
if coverage > 0.6 && box_mean_err <= orig_mean_err {
|
|
box_replacements.push(box_prim);
|
|
}
|
|
}
|
|
}
|
|
|
|
if box_replacements.is_empty() {
|
|
new_prims.push(prim.clone());
|
|
} else {
|
|
new_prims.extend(box_replacements);
|
|
any_replaced = true;
|
|
}
|
|
}
|
|
|
|
if any_replaced {
|
|
*primitives = new_prims;
|
|
}
|
|
|
|
// Pass 2: merge overlapping primitives into boxes
|
|
if primitives.len() < 2 {
|
|
return;
|
|
}
|
|
|
|
let bboxes: Vec<(Vec3, Vec3)> = primitives.iter().map(|prim| {
|
|
let mut lo = Vec3::new(f64::MAX, f64::MAX, f64::MAX);
|
|
let mut hi = Vec3::new(f64::MIN, f64::MIN, f64::MIN);
|
|
for &idx in &prim.support {
|
|
if idx >= samples.len() { continue; }
|
|
let p = samples[idx].position;
|
|
lo.x = lo.x.min(p.x); lo.y = lo.y.min(p.y); lo.z = lo.z.min(p.z);
|
|
hi.x = hi.x.max(p.x); hi.y = hi.y.max(p.y); hi.z = hi.z.max(p.z);
|
|
}
|
|
(lo, hi)
|
|
}).collect();
|
|
|
|
let mut used = vec![false; primitives.len()];
|
|
let mut replacements: Vec<(Vec<usize>, DetectedPrimitive)> = Vec::new();
|
|
|
|
for i in 0..primitives.len() {
|
|
if used[i] { continue; }
|
|
let mut group = vec![i];
|
|
for j in (i + 1)..primitives.len() {
|
|
if used[j] { continue; }
|
|
if bboxes_overlap(&bboxes[i], &bboxes[j]) {
|
|
group.push(j);
|
|
}
|
|
}
|
|
if group.len() < 2 { continue; }
|
|
|
|
let mut all_indices: Vec<usize> = Vec::new();
|
|
for &gi in &group {
|
|
all_indices.extend_from_slice(&primitives[gi].support);
|
|
}
|
|
all_indices.sort_unstable();
|
|
all_indices.dedup();
|
|
|
|
if let Some(box_prim) = try_fit_box(samples, &all_indices, config) {
|
|
let box_coverage = box_prim.support.len() as f64 / all_indices.len() as f64;
|
|
if box_coverage > 0.5 {
|
|
for &gi in &group {
|
|
used[gi] = true;
|
|
}
|
|
replacements.push((group, box_prim));
|
|
}
|
|
}
|
|
}
|
|
|
|
if !replacements.is_empty() {
|
|
let mut new_prims = Vec::new();
|
|
for (i, prim) in primitives.drain(..).enumerate() {
|
|
if !used[i] {
|
|
new_prims.push(prim);
|
|
}
|
|
}
|
|
for (_, box_prim) in replacements {
|
|
new_prims.push(box_prim);
|
|
}
|
|
*primitives = new_prims;
|
|
}
|
|
}
|
|
|
|
/// Split a set of sample indices into spatially connected clusters.
|
|
/// Uses a simple grid-based spatial hash for O(n) clustering.
|
|
fn spatial_cluster(
|
|
indices: &[usize],
|
|
samples: &[SurfaceSample],
|
|
cell_size: f64,
|
|
) -> Vec<Vec<usize>> {
|
|
use std::collections::HashMap;
|
|
|
|
if indices.len() < 2 || cell_size < 1e-10 {
|
|
return vec![indices.to_vec()];
|
|
}
|
|
|
|
let inv = 1.0 / cell_size;
|
|
let mut grid: HashMap<(i64, i64, i64), Vec<usize>> = HashMap::new();
|
|
|
|
for &idx in indices {
|
|
if idx >= samples.len() { continue; }
|
|
let p = samples[idx].position;
|
|
let key = (
|
|
(p.x * inv).floor() as i64,
|
|
(p.y * inv).floor() as i64,
|
|
(p.z * inv).floor() as i64,
|
|
);
|
|
grid.entry(key).or_default().push(idx);
|
|
}
|
|
|
|
// BFS to find connected components
|
|
let keys: Vec<(i64, i64, i64)> = grid.keys().cloned().collect();
|
|
let mut visited: HashMap<(i64, i64, i64), bool> = HashMap::new();
|
|
let mut clusters = Vec::new();
|
|
|
|
for key in &keys {
|
|
if visited.get(key).copied().unwrap_or(false) { continue; }
|
|
|
|
let mut cluster_indices = Vec::new();
|
|
let mut queue = vec![*key];
|
|
visited.insert(*key, true);
|
|
|
|
while let Some(k) = queue.pop() {
|
|
if let Some(pts) = grid.get(&k) {
|
|
cluster_indices.extend_from_slice(pts);
|
|
}
|
|
// Check 26 neighbors
|
|
for dz in -1..=1i64 {
|
|
for dy in -1..=1i64 {
|
|
for dx in -1..=1i64 {
|
|
if dx == 0 && dy == 0 && dz == 0 { continue; }
|
|
let nk = (k.0 + dx, k.1 + dy, k.2 + dz);
|
|
if grid.contains_key(&nk) && !visited.get(&nk).copied().unwrap_or(false) {
|
|
visited.insert(nk, true);
|
|
queue.push(nk);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if !cluster_indices.is_empty() {
|
|
clusters.push(cluster_indices);
|
|
}
|
|
}
|
|
|
|
clusters
|
|
}
|
|
|
|
fn bboxes_overlap(a: &(Vec3, Vec3), b: &(Vec3, Vec3)) -> bool {
|
|
let (a_lo, a_hi) = a;
|
|
let (b_lo, b_hi) = b;
|
|
a_lo.x <= b_hi.x && a_hi.x >= b_lo.x
|
|
&& a_lo.y <= b_hi.y && a_hi.y >= b_lo.y
|
|
&& a_lo.z <= b_hi.z && a_hi.z >= b_lo.z
|
|
}
|
|
|
|
/// Re-fit primitive parameters from support set, then expand inliers.
|
|
/// Runs 2 iterations of refine→expand to converge on the best fit.
|
|
fn refine_and_expand(
|
|
prim: &mut DetectedPrimitive,
|
|
samples: &[SurfaceSample],
|
|
remaining: &[bool],
|
|
config: &DecompileConfig,
|
|
) {
|
|
for _ in 0..2 {
|
|
match &mut prim.kind {
|
|
PrimitiveKind::Sphere { center, radius } => {
|
|
// Re-estimate center as mean of support positions minus their radial direction
|
|
if prim.support.len() < 4 { return; }
|
|
let mut sum_c = Vec3::zero();
|
|
let mut count = 0.0;
|
|
for &idx in &prim.support {
|
|
if idx >= samples.len() { continue; }
|
|
let p = samples[idx].position;
|
|
let n = samples[idx].normal;
|
|
let c = p - n * *radius;
|
|
sum_c = sum_c + c;
|
|
count += 1.0;
|
|
}
|
|
if count < 1.0 { return; }
|
|
*center = sum_c * (1.0 / count);
|
|
|
|
// Re-estimate radius
|
|
let mut sum_r = 0.0;
|
|
for &idx in &prim.support {
|
|
if idx >= samples.len() { continue; }
|
|
sum_r += (samples[idx].position - *center).length();
|
|
}
|
|
*radius = sum_r / count;
|
|
|
|
// Expand: re-scan all remaining points
|
|
let thresh = config.distance_threshold * 1.5;
|
|
prim.support.clear();
|
|
prim.fit_error = 0.0;
|
|
for (idx, &active) in remaining.iter().enumerate() {
|
|
if !active || idx >= samples.len() { continue; }
|
|
let dist = ((samples[idx].position - *center).length() - *radius).abs();
|
|
if dist < thresh {
|
|
prim.support.push(idx);
|
|
prim.fit_error += dist;
|
|
}
|
|
}
|
|
}
|
|
PrimitiveKind::Cylinder { point, axis, radius } => {
|
|
if prim.support.len() < 5 { return; }
|
|
// Re-estimate radius
|
|
let mut sum_r = 0.0;
|
|
let mut count = 0.0;
|
|
for &idx in &prim.support {
|
|
if idx >= samples.len() { continue; }
|
|
let p = samples[idx].position;
|
|
let v = p - *point;
|
|
let along = *axis * v.dot(*axis);
|
|
let radial = (v - along).length();
|
|
sum_r += radial;
|
|
count += 1.0;
|
|
}
|
|
if count < 1.0 { return; }
|
|
*radius = sum_r / count;
|
|
|
|
let thresh = config.distance_threshold * 1.5;
|
|
prim.support.clear();
|
|
prim.fit_error = 0.0;
|
|
for (idx, &active) in remaining.iter().enumerate() {
|
|
if !active || idx >= samples.len() { continue; }
|
|
let p = samples[idx].position;
|
|
let v = p - *point;
|
|
let along = *axis * v.dot(*axis);
|
|
let dist = ((v - along).length() - *radius).abs();
|
|
if dist < thresh {
|
|
prim.support.push(idx);
|
|
prim.fit_error += dist;
|
|
}
|
|
}
|
|
}
|
|
PrimitiveKind::Plane { point, normal } => {
|
|
if prim.support.len() < 3 { return; }
|
|
// Re-estimate normal and offset from support mean
|
|
let mut sum_n = Vec3::zero();
|
|
let mut sum_p = Vec3::zero();
|
|
let mut count = 0.0;
|
|
for &idx in &prim.support {
|
|
if idx >= samples.len() { continue; }
|
|
sum_n = sum_n + samples[idx].normal;
|
|
sum_p = sum_p + samples[idx].position;
|
|
count += 1.0;
|
|
}
|
|
if count < 1.0 { return; }
|
|
*normal = sum_n.normalized();
|
|
*point = sum_p * (1.0 / count);
|
|
|
|
let d = normal.dot(*point);
|
|
let thresh = config.distance_threshold * 1.5;
|
|
prim.support.clear();
|
|
prim.fit_error = 0.0;
|
|
for (idx, &active) in remaining.iter().enumerate() {
|
|
if !active || idx >= samples.len() { continue; }
|
|
let dist = (normal.dot(samples[idx].position) - d).abs();
|
|
let agree = normal.dot(samples[idx].normal).abs();
|
|
if dist < thresh && agree > config.normal_threshold * 0.9 {
|
|
prim.support.push(idx);
|
|
prim.fit_error += dist;
|
|
}
|
|
}
|
|
}
|
|
PrimitiveKind::Box { .. } => {}
|
|
}
|
|
}
|
|
}
|
|
|
|
fn ransac_detect(
|
|
samples: &[SurfaceSample],
|
|
remaining: &[bool],
|
|
density_map: &DensityMap,
|
|
config: &DecompileConfig,
|
|
) -> Option<DetectedPrimitive> {
|
|
let active: Vec<usize> = remaining.iter().enumerate()
|
|
.filter(|(_, &r)| r)
|
|
.map(|(i, _)| i)
|
|
.collect();
|
|
|
|
if active.len() < 3 {
|
|
return None;
|
|
}
|
|
|
|
let mut best: Option<DetectedPrimitive> = None;
|
|
let mut rng = SimpleRng::new(42);
|
|
|
|
// Global geometric detection (box, sphere) before random RANSAC
|
|
for candidate in [
|
|
try_fit_box(samples, &active, config),
|
|
try_fit_global_sphere(samples, &active, config),
|
|
].into_iter().flatten() {
|
|
let score = fit_score(&candidate);
|
|
let dominated = match &best {
|
|
Some(b) => score > fit_score(b),
|
|
None => true,
|
|
};
|
|
if dominated {
|
|
best = Some(candidate);
|
|
}
|
|
}
|
|
|
|
for _ in 0..config.ransac_iterations {
|
|
let candidates = [
|
|
try_fit_plane(samples, &active, &mut rng, config),
|
|
try_fit_sphere(samples, &active, &mut rng, config),
|
|
try_fit_cylinder(samples, &active, density_map, &mut rng, config),
|
|
];
|
|
|
|
for candidate in candidates.into_iter().flatten() {
|
|
let dominated = match &best {
|
|
Some(b) => {
|
|
let c_score = fit_score(&candidate);
|
|
let b_score = fit_score(b);
|
|
c_score > b_score
|
|
}
|
|
None => true,
|
|
};
|
|
if dominated {
|
|
best = Some(candidate);
|
|
}
|
|
}
|
|
}
|
|
|
|
best
|
|
}
|
|
|
|
/// Quality-weighted score for primitive ranking.
|
|
/// Planes get a strong bonus (simpler parametric form, merge into boxes later).
|
|
/// Score = support_count * quality_factor / (1 + mean_error)
|
|
fn fit_score(prim: &DetectedPrimitive) -> f64 {
|
|
let support = prim.support.len() as f64;
|
|
let mean_err = if prim.support.is_empty() {
|
|
1.0
|
|
} else {
|
|
prim.fit_error / support
|
|
};
|
|
let complexity_bonus = match &prim.kind {
|
|
PrimitiveKind::Plane { .. } => 1.5,
|
|
PrimitiveKind::Sphere { .. } => 1.0,
|
|
PrimitiveKind::Cylinder { .. } => 0.8,
|
|
PrimitiveKind::Box { .. } => 1.3,
|
|
};
|
|
support * complexity_bonus / (1.0 + mean_err)
|
|
}
|
|
|
|
fn try_fit_plane(
|
|
samples: &[SurfaceSample],
|
|
active: &[usize],
|
|
rng: &mut SimpleRng,
|
|
config: &DecompileConfig,
|
|
) -> Option<DetectedPrimitive> {
|
|
if active.len() < 3 {
|
|
return None;
|
|
}
|
|
|
|
let i0 = active[rng.next_usize(active.len())];
|
|
let i1 = active[rng.next_usize(active.len())];
|
|
let i2 = active[rng.next_usize(active.len())];
|
|
if i0 == i1 || i1 == i2 || i0 == i2 {
|
|
return None;
|
|
}
|
|
|
|
let p0 = samples[i0].position;
|
|
let p1 = samples[i1].position;
|
|
let p2 = samples[i2].position;
|
|
|
|
let normal = (p1 - p0).cross(p2 - p0).normalized();
|
|
if normal.length() < 0.5 {
|
|
return None;
|
|
}
|
|
|
|
let d = normal.dot(p0);
|
|
|
|
let mut support = Vec::new();
|
|
let mut total_error = 0.0;
|
|
|
|
for &idx in active {
|
|
let dist = (normal.dot(samples[idx].position) - d).abs();
|
|
let normal_agree = normal.dot(samples[idx].normal).abs();
|
|
if dist < config.distance_threshold && normal_agree > config.normal_threshold {
|
|
total_error += dist;
|
|
support.push(idx);
|
|
}
|
|
}
|
|
|
|
if support.len() < 3 {
|
|
return None;
|
|
}
|
|
|
|
Some(DetectedPrimitive {
|
|
kind: PrimitiveKind::Plane { point: p0, normal },
|
|
support,
|
|
fit_error: total_error,
|
|
})
|
|
}
|
|
|
|
fn try_fit_sphere(
|
|
samples: &[SurfaceSample],
|
|
active: &[usize],
|
|
rng: &mut SimpleRng,
|
|
config: &DecompileConfig,
|
|
) -> Option<DetectedPrimitive> {
|
|
if active.len() < 4 {
|
|
return None;
|
|
}
|
|
|
|
// Pick two points; use their normals to estimate center.
|
|
// If normals point radially from a common center, the intersection
|
|
// of the two normal rays gives the center.
|
|
let i0 = active[rng.next_usize(active.len())];
|
|
let i1 = active[rng.next_usize(active.len())];
|
|
if i0 == i1 { return None; }
|
|
|
|
let p0 = samples[i0].position;
|
|
let n0 = samples[i0].normal;
|
|
let p1 = samples[i1].position;
|
|
let n1 = samples[i1].normal;
|
|
|
|
// Closest point between two rays: p0 + t*n0 and p1 + s*n1
|
|
let center = closest_point_two_rays(p0, n0, p1, n1)?;
|
|
|
|
let r0 = (center - p0).length();
|
|
let r1 = (center - p1).length();
|
|
if (r0 - r1).abs() > config.distance_threshold * 10.0 {
|
|
return None;
|
|
}
|
|
let radius = (r0 + r1) * 0.5;
|
|
if radius < 1e-6 {
|
|
return None;
|
|
}
|
|
|
|
let mut support = Vec::new();
|
|
let mut total_error = 0.0;
|
|
|
|
for &idx in active {
|
|
let dist = ((samples[idx].position - center).length() - radius).abs();
|
|
if dist < config.distance_threshold {
|
|
total_error += dist;
|
|
support.push(idx);
|
|
}
|
|
}
|
|
|
|
if support.len() < 4 {
|
|
return None;
|
|
}
|
|
|
|
Some(DetectedPrimitive {
|
|
kind: PrimitiveKind::Sphere { center, radius },
|
|
support,
|
|
fit_error: total_error,
|
|
})
|
|
}
|
|
|
|
fn try_fit_cylinder(
|
|
samples: &[SurfaceSample],
|
|
active: &[usize],
|
|
density_map: &DensityMap,
|
|
rng: &mut SimpleRng,
|
|
config: &DecompileConfig,
|
|
) -> Option<DetectedPrimitive> {
|
|
if active.len() < 5 {
|
|
return None;
|
|
}
|
|
|
|
// Cylinder detection: pick two points with curved surface type,
|
|
// cross their normals to get the axis direction.
|
|
let curved: Vec<usize> = active.iter()
|
|
.filter(|&&idx| {
|
|
density_map.cells.get(&samples[idx].cell_key)
|
|
.map_or(false, |d| d.surface_type == SurfaceType::Curved)
|
|
})
|
|
.copied()
|
|
.collect();
|
|
|
|
let source = if curved.len() >= 2 { &curved } else { active };
|
|
|
|
let i0 = source[rng.next_usize(source.len())];
|
|
let i1 = source[rng.next_usize(source.len())];
|
|
if i0 == i1 { return None; }
|
|
|
|
let n0 = samples[i0].normal;
|
|
let n1 = samples[i1].normal;
|
|
|
|
let axis = n0.cross(n1).normalized();
|
|
if axis.length() < 0.3 {
|
|
return None;
|
|
}
|
|
|
|
// Project points onto the plane perpendicular to the axis.
|
|
// The cylinder's cross-section in that plane is a circle.
|
|
let p0_proj = project_onto_plane(samples[i0].position, axis);
|
|
let p1_proj = project_onto_plane(samples[i1].position, axis);
|
|
|
|
// Use the normals (also projected) to find the center
|
|
let n0_proj = project_onto_plane(n0, axis).normalized();
|
|
let n1_proj = project_onto_plane(n1, axis).normalized();
|
|
|
|
let center_2d = closest_point_two_rays(p0_proj, n0_proj, p1_proj, n1_proj)?;
|
|
|
|
let r0 = (p0_proj - center_2d).length();
|
|
let r1 = (p1_proj - center_2d).length();
|
|
if (r0 - r1).abs() > config.distance_threshold * 10.0 {
|
|
return None;
|
|
}
|
|
let radius = (r0 + r1) * 0.5;
|
|
if radius < 1e-6 {
|
|
return None;
|
|
}
|
|
|
|
// center_2d is in the projected plane; reconstruct 3D point on axis
|
|
let axis_point = center_2d; // The projection zeroed the axis component
|
|
|
|
let mut support = Vec::new();
|
|
let mut total_error = 0.0;
|
|
|
|
for &idx in active {
|
|
let p = samples[idx].position;
|
|
let p_proj = project_onto_plane(p, axis);
|
|
let dist = ((p_proj - center_2d).length() - radius).abs();
|
|
if dist >= config.distance_threshold { continue; }
|
|
|
|
// Normal check: the point's normal projected onto the cross-section plane
|
|
// should be roughly radial from the center (for side surface points)
|
|
let n = samples[idx].normal;
|
|
let n_along_axis = axis * n.dot(axis);
|
|
let n_radial = (n - n_along_axis).normalized();
|
|
let expected_radial = (p_proj - center_2d).normalized();
|
|
let normal_agree = n_radial.dot(expected_radial).abs();
|
|
|
|
// Accept if normal is radial (side surface) or axial (cap)
|
|
let is_axial = n.dot(axis).abs() > 0.8;
|
|
if normal_agree > 0.5 || is_axial {
|
|
total_error += dist;
|
|
support.push(idx);
|
|
}
|
|
}
|
|
|
|
if support.len() < 5 {
|
|
return None;
|
|
}
|
|
|
|
Some(DetectedPrimitive {
|
|
kind: PrimitiveKind::Cylinder { point: axis_point, axis, radius },
|
|
support,
|
|
fit_error: total_error,
|
|
})
|
|
}
|
|
|
|
/// Axis-aligned box detection.
|
|
///
|
|
/// Global sphere detection using normal convergence.
|
|
///
|
|
/// For each point, the estimated center is `p - r*n`. If many points
|
|
/// produce a consistent center, they form a sphere. Estimates center
|
|
/// from a subsample, then scores all active points.
|
|
fn try_fit_global_sphere(
|
|
samples: &[SurfaceSample],
|
|
active: &[usize],
|
|
config: &DecompileConfig,
|
|
) -> Option<DetectedPrimitive> {
|
|
if active.len() < 10 {
|
|
return None;
|
|
}
|
|
|
|
// Estimate a candidate radius from the bounding extent of active points
|
|
let mut min_p = Vec3::new(f64::MAX, f64::MAX, f64::MAX);
|
|
let mut max_p = Vec3::new(f64::MIN, f64::MIN, f64::MIN);
|
|
for &idx in active {
|
|
let p = samples[idx].position;
|
|
min_p.x = min_p.x.min(p.x); min_p.y = min_p.y.min(p.y); min_p.z = min_p.z.min(p.z);
|
|
max_p.x = max_p.x.max(p.x); max_p.y = max_p.y.max(p.y); max_p.z = max_p.z.max(p.z);
|
|
}
|
|
let extent = max_p - min_p;
|
|
let max_dim = extent.x.max(extent.y).max(extent.z);
|
|
let min_dim = extent.x.min(extent.y).min(extent.z);
|
|
|
|
// Sphere test: all three dimensions should be similar
|
|
if min_dim < max_dim * 0.75 {
|
|
return None;
|
|
}
|
|
|
|
let r_est = max_dim / 2.0;
|
|
|
|
// Estimate center: for each point, center = position - normal * radius
|
|
// Use a subsample for speed
|
|
let step = (active.len() / 200).max(1);
|
|
let mut sum_c = Vec3::zero();
|
|
let mut count = 0.0;
|
|
for i in (0..active.len()).step_by(step) {
|
|
let idx = active[i];
|
|
let p = samples[idx].position;
|
|
let n = samples[idx].normal;
|
|
let c = p - n * r_est;
|
|
sum_c = sum_c + c;
|
|
count += 1.0;
|
|
}
|
|
if count < 3.0 { return None; }
|
|
let mut center = sum_c * (1.0 / count);
|
|
|
|
// Refine radius from estimated center
|
|
let mut sum_r = 0.0;
|
|
let mut r_count = 0.0;
|
|
for i in (0..active.len()).step_by(step) {
|
|
let idx = active[i];
|
|
let r = (samples[idx].position - center).length();
|
|
sum_r += r;
|
|
r_count += 1.0;
|
|
}
|
|
let mut radius = sum_r / r_count;
|
|
|
|
// Second pass: refine center using all points near the sphere
|
|
let thresh = config.distance_threshold * 2.0;
|
|
for _ in 0..3 {
|
|
let mut new_center = Vec3::zero();
|
|
let mut new_count = 0.0;
|
|
let mut new_radius = 0.0;
|
|
for &idx in active {
|
|
let p = samples[idx].position;
|
|
let dist = ((p - center).length() - radius).abs();
|
|
if dist < thresh {
|
|
let n = samples[idx].normal;
|
|
new_center = new_center + (p - n * radius);
|
|
new_radius += (p - center).length();
|
|
new_count += 1.0;
|
|
}
|
|
}
|
|
if new_count < 3.0 { break; }
|
|
center = new_center * (1.0 / new_count);
|
|
radius = new_radius / new_count;
|
|
}
|
|
|
|
// Score
|
|
let mut support = Vec::new();
|
|
let mut total_error = 0.0;
|
|
for &idx in active {
|
|
let dist = ((samples[idx].position - center).length() - radius).abs();
|
|
if dist < thresh {
|
|
support.push(idx);
|
|
total_error += dist;
|
|
}
|
|
}
|
|
|
|
let min_support = (active.len() as f64 * 0.3).max(10.0) as usize;
|
|
if support.len() < min_support {
|
|
return None;
|
|
}
|
|
|
|
Some(DetectedPrimitive {
|
|
kind: PrimitiveKind::Sphere { center, radius },
|
|
support,
|
|
fit_error: total_error,
|
|
})
|
|
}
|
|
|
|
/// Axis-aligned box detection.
|
|
///
|
|
/// Groups active points by which axis-aligned face they belong to
|
|
/// (based on normal direction), then checks if 4+ groups describe
|
|
/// a consistent box. Requires points on at least 4 of the 6 faces.
|
|
fn try_fit_box(
|
|
samples: &[SurfaceSample],
|
|
active: &[usize],
|
|
config: &DecompileConfig,
|
|
) -> Option<DetectedPrimitive> {
|
|
if active.len() < 6 {
|
|
return None;
|
|
}
|
|
|
|
let normal_threshold = 0.95;
|
|
|
|
// Bucket points by face normal: +X, -X, +Y, -Y, +Z, -Z
|
|
let mut face_points: [Vec<usize>; 6] = Default::default();
|
|
|
|
for &idx in active {
|
|
let n = samples[idx].normal;
|
|
let ax = n.x.abs();
|
|
let ay = n.y.abs();
|
|
let az = n.z.abs();
|
|
let max_comp = ax.max(ay).max(az);
|
|
if max_comp < normal_threshold { continue; }
|
|
|
|
if ax == max_comp {
|
|
if n.x > 0.0 { face_points[0].push(idx); }
|
|
else { face_points[1].push(idx); }
|
|
} else if ay == max_comp {
|
|
if n.y > 0.0 { face_points[2].push(idx); }
|
|
else { face_points[3].push(idx); }
|
|
} else {
|
|
if n.z > 0.0 { face_points[4].push(idx); }
|
|
else { face_points[5].push(idx); }
|
|
}
|
|
}
|
|
|
|
// Need points on at least 4 faces to identify a box
|
|
let populated_faces = face_points.iter().filter(|f| f.len() >= 3).count();
|
|
if populated_faces < 4 {
|
|
return None;
|
|
}
|
|
|
|
// Axis-aligned points should be a majority of all active points.
|
|
// For a true box, most surface points have axis-aligned normals.
|
|
// For a cylinder, only cap faces are axis-aligned.
|
|
let aligned_total: usize = face_points.iter().map(|f| f.len()).sum();
|
|
if (aligned_total as f64) < active.len() as f64 * 0.4 {
|
|
return None;
|
|
}
|
|
|
|
// Verify each face is actually planar: low variance along the face normal axis
|
|
for (i, points) in face_points.iter().enumerate() {
|
|
if points.len() < 3 { continue; }
|
|
let axis_idx = i / 2;
|
|
let vals: Vec<f64> = points.iter().map(|&idx| {
|
|
let p = samples[idx].position;
|
|
match axis_idx { 0 => p.x, 1 => p.y, _ => p.z }
|
|
}).collect();
|
|
let mean = vals.iter().sum::<f64>() / vals.len() as f64;
|
|
let variance = vals.iter().map(|v| (v - mean).powi(2)).sum::<f64>() / vals.len() as f64;
|
|
let std_dev = variance.sqrt();
|
|
// If variance is high relative to the threshold, this isn't a flat face
|
|
if std_dev > config.distance_threshold * 3.0 {
|
|
return None;
|
|
}
|
|
}
|
|
|
|
// Compute face positions (average coordinate along the face normal axis)
|
|
let mut face_pos = [f64::NAN; 6];
|
|
for (i, points) in face_points.iter().enumerate() {
|
|
if points.len() < 3 { continue; }
|
|
let axis_idx = i / 2;
|
|
let sum: f64 = points.iter().map(|&idx| {
|
|
let p = samples[idx].position;
|
|
match axis_idx { 0 => p.x, 1 => p.y, _ => p.z }
|
|
}).sum();
|
|
face_pos[i] = sum / points.len() as f64;
|
|
}
|
|
|
|
// Verify consistency: opposing faces should bracket the box
|
|
let mut center = Vec3::zero();
|
|
let mut half = [0.0f64; 3];
|
|
let mut axes_found = 0;
|
|
|
|
for axis_idx in 0..3 {
|
|
let pos_face = axis_idx * 2;
|
|
let neg_face = axis_idx * 2 + 1;
|
|
if face_pos[pos_face].is_nan() && face_pos[neg_face].is_nan() {
|
|
continue;
|
|
}
|
|
|
|
if !face_pos[pos_face].is_nan() && !face_pos[neg_face].is_nan() {
|
|
let hi = face_pos[pos_face].max(face_pos[neg_face]);
|
|
let lo = face_pos[pos_face].min(face_pos[neg_face]);
|
|
let mid = (hi + lo) / 2.0;
|
|
let h = (hi - lo) / 2.0;
|
|
match axis_idx {
|
|
0 => center.x = mid,
|
|
1 => center.y = mid,
|
|
_ => center.z = mid,
|
|
}
|
|
half[axis_idx] = h;
|
|
axes_found += 1;
|
|
} else {
|
|
// Single face found: estimate from point extent along this axis
|
|
let face_idx = if !face_pos[pos_face].is_nan() { pos_face } else { neg_face };
|
|
let all_box_points: Vec<usize> = face_points.iter().flat_map(|f| f.iter().copied()).collect();
|
|
let (lo, hi) = extent_along_axis(&all_box_points, samples, axis_idx);
|
|
let mid = (hi + lo) / 2.0;
|
|
let h = (hi - lo) / 2.0;
|
|
if h < 1e-6 { continue; }
|
|
match axis_idx {
|
|
0 => center.x = mid,
|
|
1 => center.y = mid,
|
|
_ => center.z = mid,
|
|
}
|
|
half[axis_idx] = h;
|
|
let _ = face_idx;
|
|
axes_found += 1;
|
|
}
|
|
}
|
|
|
|
if axes_found < 2 {
|
|
return None;
|
|
}
|
|
|
|
// If one axis is missing, estimate from the extent of support points
|
|
if axes_found == 2 {
|
|
for axis_idx in 0..3 {
|
|
if half[axis_idx] > 1e-6 { continue; }
|
|
let all_box_points: Vec<usize> = face_points.iter().flat_map(|f| f.iter().copied()).collect();
|
|
let (lo, hi) = extent_along_axis(&all_box_points, samples, axis_idx);
|
|
let mid = (hi + lo) / 2.0;
|
|
let h = (hi - lo) / 2.0;
|
|
if h < 1e-6 { continue; }
|
|
match axis_idx {
|
|
0 => center.x = mid,
|
|
1 => center.y = mid,
|
|
_ => center.z = mid,
|
|
}
|
|
half[axis_idx] = h;
|
|
}
|
|
}
|
|
|
|
if half[0] < 1e-6 || half[1] < 1e-6 || half[2] < 1e-6 {
|
|
return None;
|
|
}
|
|
|
|
// Score: use wider threshold for box scoring to capture edge/corner samples
|
|
let mut support = Vec::new();
|
|
let mut total_error = 0.0;
|
|
let thresh = config.distance_threshold * 2.0;
|
|
|
|
for &idx in active {
|
|
let p = samples[idx].position;
|
|
let local = Vec3::new(
|
|
(p.x - center.x).abs(),
|
|
(p.y - center.y).abs(),
|
|
(p.z - center.z).abs(),
|
|
);
|
|
// SDF of a box at origin with half_extents
|
|
let q = Vec3::new(
|
|
local.x - half[0],
|
|
local.y - half[1],
|
|
local.z - half[2],
|
|
);
|
|
let outside = Vec3::new(q.x.max(0.0), q.y.max(0.0), q.z.max(0.0));
|
|
let inside_dist = q.x.max(q.y).max(q.z).min(0.0);
|
|
let dist = (outside.length() + inside_dist).abs();
|
|
|
|
if dist < thresh {
|
|
total_error += dist;
|
|
support.push(idx);
|
|
}
|
|
}
|
|
|
|
let min_support = (active.len() as f64 * 0.1).max(6.0) as usize;
|
|
if support.len() < min_support {
|
|
return None;
|
|
}
|
|
|
|
Some(DetectedPrimitive {
|
|
kind: PrimitiveKind::Box {
|
|
center,
|
|
half_extents: half,
|
|
rotation_axis: Vec3::new(1.0, 0.0, 0.0),
|
|
rotation_angle: 0.0,
|
|
},
|
|
support,
|
|
fit_error: total_error,
|
|
})
|
|
}
|
|
|
|
/// Relaxed box detection for consolidation pass.
|
|
/// Lower normal threshold and aligned fraction requirement.
|
|
fn try_fit_box_relaxed(
|
|
samples: &[SurfaceSample],
|
|
active: &[usize],
|
|
config: &DecompileConfig,
|
|
) -> Option<DetectedPrimitive> {
|
|
if active.len() < 6 {
|
|
return None;
|
|
}
|
|
|
|
let normal_threshold = 0.85;
|
|
let mut face_points: [Vec<usize>; 6] = Default::default();
|
|
|
|
for &idx in active {
|
|
if idx >= samples.len() { continue; }
|
|
let n = samples[idx].normal;
|
|
let ax = n.x.abs();
|
|
let ay = n.y.abs();
|
|
let az = n.z.abs();
|
|
let max_comp = ax.max(ay).max(az);
|
|
if max_comp < normal_threshold { continue; }
|
|
|
|
if ax == max_comp {
|
|
if n.x > 0.0 { face_points[0].push(idx); }
|
|
else { face_points[1].push(idx); }
|
|
} else if ay == max_comp {
|
|
if n.y > 0.0 { face_points[2].push(idx); }
|
|
else { face_points[3].push(idx); }
|
|
} else {
|
|
if n.z > 0.0 { face_points[4].push(idx); }
|
|
else { face_points[5].push(idx); }
|
|
}
|
|
}
|
|
|
|
let populated_faces = face_points.iter().filter(|f| f.len() >= 3).count();
|
|
if populated_faces < 4 {
|
|
return None;
|
|
}
|
|
|
|
// Relaxed: only 25% axis-aligned required
|
|
let aligned_total: usize = face_points.iter().map(|f| f.len()).sum();
|
|
if (aligned_total as f64) < active.len() as f64 * 0.25 {
|
|
return None;
|
|
}
|
|
|
|
// Face planarity check
|
|
for (i, points) in face_points.iter().enumerate() {
|
|
if points.len() < 3 { continue; }
|
|
let axis_idx = i / 2;
|
|
let vals: Vec<f64> = points.iter().map(|&idx| {
|
|
let p = samples[idx].position;
|
|
match axis_idx { 0 => p.x, 1 => p.y, _ => p.z }
|
|
}).collect();
|
|
let mean = vals.iter().sum::<f64>() / vals.len() as f64;
|
|
let variance = vals.iter().map(|v| (v - mean).powi(2)).sum::<f64>() / vals.len() as f64;
|
|
if variance.sqrt() > config.distance_threshold * 5.0 {
|
|
return None;
|
|
}
|
|
}
|
|
|
|
let mut face_pos = [f64::NAN; 6];
|
|
for (i, points) in face_points.iter().enumerate() {
|
|
if points.len() < 3 { continue; }
|
|
let axis_idx = i / 2;
|
|
let sum: f64 = points.iter().map(|&idx| {
|
|
let p = samples[idx].position;
|
|
match axis_idx { 0 => p.x, 1 => p.y, _ => p.z }
|
|
}).sum();
|
|
face_pos[i] = sum / points.len() as f64;
|
|
}
|
|
|
|
let mut center = Vec3::zero();
|
|
let mut half = [0.0f64; 3];
|
|
let mut axes_found = 0;
|
|
|
|
for axis_idx in 0..3 {
|
|
let pos_face = axis_idx * 2;
|
|
let neg_face = axis_idx * 2 + 1;
|
|
if face_pos[pos_face].is_nan() && face_pos[neg_face].is_nan() {
|
|
continue;
|
|
}
|
|
if !face_pos[pos_face].is_nan() && !face_pos[neg_face].is_nan() {
|
|
let hi = face_pos[pos_face].max(face_pos[neg_face]);
|
|
let lo = face_pos[pos_face].min(face_pos[neg_face]);
|
|
match axis_idx {
|
|
0 => center.x = (hi + lo) / 2.0,
|
|
1 => center.y = (hi + lo) / 2.0,
|
|
_ => center.z = (hi + lo) / 2.0,
|
|
}
|
|
half[axis_idx] = (hi - lo) / 2.0;
|
|
axes_found += 1;
|
|
} else {
|
|
let all_pts: Vec<usize> = face_points.iter().flat_map(|f| f.iter().copied()).collect();
|
|
let (lo, hi) = extent_along_axis(&all_pts, samples, axis_idx);
|
|
if hi - lo < 1e-6 { continue; }
|
|
match axis_idx {
|
|
0 => center.x = (hi + lo) / 2.0,
|
|
1 => center.y = (hi + lo) / 2.0,
|
|
_ => center.z = (hi + lo) / 2.0,
|
|
}
|
|
half[axis_idx] = (hi - lo) / 2.0;
|
|
axes_found += 1;
|
|
}
|
|
}
|
|
|
|
if axes_found < 2 || half[0] < 1e-6 || half[1] < 1e-6 || half[2] < 1e-6 {
|
|
// Estimate missing dimensions from overall extent
|
|
for axis_idx in 0..3 {
|
|
if half[axis_idx] >= 1e-6 { continue; }
|
|
let (lo, hi) = extent_along_axis(active, samples, axis_idx);
|
|
if hi - lo < 1e-6 { return None; }
|
|
match axis_idx {
|
|
0 => center.x = (hi + lo) / 2.0,
|
|
1 => center.y = (hi + lo) / 2.0,
|
|
_ => center.z = (hi + lo) / 2.0,
|
|
}
|
|
half[axis_idx] = (hi - lo) / 2.0;
|
|
}
|
|
}
|
|
|
|
if half[0] < 1e-6 || half[1] < 1e-6 || half[2] < 1e-6 {
|
|
return None;
|
|
}
|
|
|
|
let mut support = Vec::new();
|
|
let mut total_error = 0.0;
|
|
let thresh = config.distance_threshold * 2.0;
|
|
|
|
for &idx in active {
|
|
if idx >= samples.len() { continue; }
|
|
let p = samples[idx].position;
|
|
let local = Vec3::new(
|
|
(p.x - center.x).abs(),
|
|
(p.y - center.y).abs(),
|
|
(p.z - center.z).abs(),
|
|
);
|
|
let q = Vec3::new(local.x - half[0], local.y - half[1], local.z - half[2]);
|
|
let outside = Vec3::new(q.x.max(0.0), q.y.max(0.0), q.z.max(0.0));
|
|
let inside_dist = q.x.max(q.y).max(q.z).min(0.0);
|
|
let dist = (outside.length() + inside_dist).abs();
|
|
if dist < thresh {
|
|
support.push(idx);
|
|
total_error += dist;
|
|
}
|
|
}
|
|
|
|
let min_support = (active.len() as f64 * 0.3).max(6.0) as usize;
|
|
if support.len() < min_support {
|
|
return None;
|
|
}
|
|
|
|
Some(DetectedPrimitive {
|
|
kind: PrimitiveKind::Box {
|
|
center,
|
|
half_extents: half,
|
|
rotation_axis: Vec3::new(1.0, 0.0, 0.0),
|
|
rotation_angle: 0.0,
|
|
},
|
|
support,
|
|
fit_error: total_error,
|
|
})
|
|
}
|
|
|
|
fn extent_along_axis(points: &[usize], samples: &[SurfaceSample], axis: usize) -> (f64, f64) {
|
|
let mut lo = f64::MAX;
|
|
let mut hi = f64::MIN;
|
|
for &idx in points {
|
|
let p = samples[idx].position;
|
|
let v = match axis { 0 => p.x, 1 => p.y, _ => p.z };
|
|
lo = lo.min(v);
|
|
hi = hi.max(v);
|
|
}
|
|
(lo, hi)
|
|
}
|
|
|
|
fn project_onto_plane(v: Vec3, normal: Vec3) -> Vec3 {
|
|
v - normal * v.dot(normal)
|
|
}
|
|
|
|
fn closest_point_two_rays(p0: Vec3, d0: Vec3, p1: Vec3, d1: Vec3) -> Option<Vec3> {
|
|
let w0 = p0 - p1;
|
|
let a = d0.dot(d0);
|
|
let b = d0.dot(d1);
|
|
let c = d1.dot(d1);
|
|
let d = d0.dot(w0);
|
|
let e = d1.dot(w0);
|
|
|
|
let denom = a * c - b * b;
|
|
if denom.abs() < 1e-10 {
|
|
return None;
|
|
}
|
|
|
|
let t = (b * e - c * d) / denom;
|
|
let s = (a * e - b * d) / denom;
|
|
|
|
let closest_on_0 = p0 + d0 * t;
|
|
let closest_on_1 = p1 + d1 * s;
|
|
|
|
Some(Vec3 {
|
|
x: (closest_on_0.x + closest_on_1.x) * 0.5,
|
|
y: (closest_on_0.y + closest_on_1.y) * 0.5,
|
|
z: (closest_on_0.z + closest_on_1.z) * 0.5,
|
|
})
|
|
}
|
|
|
|
/// Xorshift64 RNG for RANSAC sampling.
|
|
struct SimpleRng {
|
|
state: u64,
|
|
}
|
|
|
|
impl SimpleRng {
|
|
fn new(seed: u64) -> Self {
|
|
Self { state: seed.wrapping_add(1) }
|
|
}
|
|
|
|
fn next_u64(&mut self) -> u64 {
|
|
self.state ^= self.state << 13;
|
|
self.state ^= self.state >> 7;
|
|
self.state ^= self.state << 17;
|
|
self.state
|
|
}
|
|
|
|
fn next_usize(&mut self, max: usize) -> usize {
|
|
(self.next_u64() % max as u64) as usize
|
|
}
|
|
}
|