rscad_csg_bsp/

cleanup.rs

//! Post-CSG polygon cleanup: remove degenerate polygons, collapse collinear
//! vertices, and merge coplanar adjacent polygons.

use crate::{Number, Plane, Polygon};
use std::collections::HashMap;

/// Remove collinear interior vertices from a polygon.
///
/// A vertex `b` between `a` and `c` is collinear when the cross product
/// `(b - a) × (c - a)` has near-zero magnitude relative to the edge lengths.
pub fn remove_collinear_vertices<T: Number>(polygon: Polygon<T, 3>) -> Option<Polygon<T, 3>> {
    let verts = polygon.vertices();
    let n = verts.len();
    if n < 3 {
        return None;
    }

    let eps = T::from(1e-10).expect("BUG: f64 must represent 1e-10");
    let mut keep = Vec::with_capacity(n);

    for i in 0..n {
        let prev = if i == 0 { n - 1 } else { i - 1 };
        let next = if i + 1 < n { i + 1 } else { 0 };
        let a = verts[prev];
        let b = verts[i];
        let c = verts[next];
        let ab = b - a;
        let ac = c - a;
        let cross = ab.cross(&ac);
        if cross.norm_squared() > eps {
            keep.push(verts[i]);
        }
    }

    if keep.len() < 3 {
        return None;
    }

    Some(polygon.clone_with_points(ndarray::Array1::from_vec(keep)))
}

/// Returns true if a polygon is degenerate (near-zero area or duplicate vertices).
pub fn is_degenerate<T: Number>(polygon: &Polygon<T, 3>) -> bool {
    let verts = polygon.vertices();
    if verts.len() < 3 {
        return true;
    }

    // Check area via cross products of edges from vertex 0.
    let eps = T::from(1e-10).expect("BUG: f64 must represent 1e-10");
    let mut area_sq = T::zero();
    let v0 = verts[0];
    for i in 1..verts.len() - 1 {
        let e1 = verts[i] - v0;
        let e2 = verts[i + 1] - v0;
        let cross = e1.cross(&e2);
        area_sq += cross.norm_squared();
    }
    area_sq < eps
}

/// Quantised vertex key for edge-matching. Rounds to 1e-5 grid.
type VertexKey = [i64; 3];

fn vertex_key<T: Number>(p: &nalgebra::Point3<T>) -> VertexKey {
    let scale = T::from(1e5).expect("BUG: f64 must represent 1e5");
    [
        num_traits::cast::<T, f64>(num_traits::Float::round(p.x * scale)).unwrap_or(0.0) as i64,
        num_traits::cast::<T, f64>(num_traits::Float::round(p.y * scale)).unwrap_or(0.0) as i64,
        num_traits::cast::<T, f64>(num_traits::Float::round(p.z * scale)).unwrap_or(0.0) as i64,
    ]
}

/// Quantised plane key for grouping coplanar polygons.
/// Canonicalises the normal direction so that opposite-facing normals don't
/// accidentally merge (they represent different face orientations).
type PlaneKey = [i64; 4];

fn plane_key<T: Number>(plane: &Plane<T, 3>) -> PlaneKey {
    let scale = T::from(1e4).expect("BUG: f64 must represent 1e4");
    let n = plane.normal();
    let d = plane.distance();
    [
        num_traits::cast::<T, f64>(num_traits::Float::round(n.x * scale)).unwrap_or(0.0) as i64,
        num_traits::cast::<T, f64>(num_traits::Float::round(n.y * scale)).unwrap_or(0.0) as i64,
        num_traits::cast::<T, f64>(num_traits::Float::round(n.z * scale)).unwrap_or(0.0) as i64,
        num_traits::cast::<T, f64>(num_traits::Float::round(d * scale)).unwrap_or(0.0) as i64,
    ]
}

type DirectedEdge = (VertexKey, VertexKey);

/// Merge coplanar adjacent polygons that share edges.
///
/// Algorithm:
/// 1. Group polygons by quantised plane.
/// 2. For each coplanar group, build a directed-edge map.
/// 3. Greedily merge polygon pairs that share an edge (opposing directions).
/// 4. Repeat until no more merges are possible in that group.
pub fn merge_coplanar_polygons<T: Number>(polygons: Vec<Polygon<T, 3>>) -> Vec<Polygon<T, 3>> {
    // Group by plane.
    let mut groups: HashMap<PlaneKey, Vec<Polygon<T, 3>>> = HashMap::new();
    for p in polygons {
        groups.entry(plane_key(&p.plane())).or_default().push(p);
    }

    let mut result = Vec::new();
    for (_, group) in groups {
        if group.len() < 2 {
            result.extend(group);
            continue;
        }
        result.extend(merge_group(group));
    }

    result
}

/// Merge a group of coplanar polygons by greedily combining pairs that share edges.
fn merge_group<T: Number>(mut polys: Vec<Polygon<T, 3>>) -> Vec<Polygon<T, 3>> {
    // Iterate until no more merges happen.
    let mut changed = true;
    while changed {
        changed = false;
        // Build edge → polygon-index map for the reverse edge direction.
        // For polygon i with directed edge (A→B), we want to find polygon j
        // with directed edge (B→A).
        let mut edge_to_poly: HashMap<DirectedEdge, usize> = HashMap::new();
        for (idx, poly) in polys.iter().enumerate() {
            let verts = poly.vertices();
            let n = verts.len();
            for i in 0..n {
                let j = if i + 1 < n { i + 1 } else { 0 };
                let a = vertex_key(&verts[i]);
                let b = vertex_key(&verts[j]);
                edge_to_poly.insert((a, b), idx);
            }
        }

        // Find first mergeable pair.
        let mut merge = None;
        'outer: for (idx_a, poly_a) in polys.iter().enumerate() {
            let verts_a = poly_a.vertices();
            let na = verts_a.len();
            for i in 0..na {
                let j = if i + 1 < na { i + 1 } else { 0 };
                let a_key = vertex_key(&verts_a[i]);
                let b_key = vertex_key(&verts_a[j]);
                // Look for a polygon with the reverse edge (B→A).
                if let Some(&idx_b) = edge_to_poly.get(&(b_key, a_key))
                    && idx_b != idx_a
                {
                    merge = Some((idx_a, i, idx_b));
                    break 'outer;
                }
            }
        }

        if let Some((idx_a, edge_start_a, idx_b)) = merge {
            // Remove both polygons (remove higher index first to keep lower valid).
            let (ia, ib) = if idx_a < idx_b {
                (idx_a, idx_b)
            } else {
                (idx_b, idx_a)
            };
            let poly_hi = polys.remove(ib);
            let poly_lo = polys.remove(ia);
            let (poly_a, edge_a, poly_b) = if idx_a < idx_b {
                (poly_lo, edge_start_a, poly_hi)
            } else {
                (poly_hi, edge_start_a, poly_lo)
            };
            if let Some(merged) = merge_two_polygons(&poly_a, edge_a, &poly_b) {
                polys.push(merged);
                changed = true;
            } else {
                // Put them back if merge failed.
                polys.push(poly_a);
                polys.push(poly_b);
            }
        }
    }
    polys
}

/// Merge two polygons along a shared edge.
///
/// `poly_a` has a directed edge starting at vertex index `edge_a_start` (i.e.
/// edge `verts_a[edge_a_start] → verts_a[edge_a_start+1]`).
/// `poly_b` has the reverse of that edge somewhere.
///
/// The merged polygon walks along poly_a, and where the shared edge would be,
/// detours through poly_b instead.
fn merge_two_polygons<T: Number>(
    poly_a: &Polygon<T, 3>,
    edge_a_start: usize,
    poly_b: &Polygon<T, 3>,
) -> Option<Polygon<T, 3>> {
    let va = poly_a.vertices();
    let vb = poly_b.vertices();
    let na = va.len();
    let nb = vb.len();

    let a_key = vertex_key(&va[edge_a_start]);
    let b_key = vertex_key(&va[(edge_a_start + 1) % na]);

    // Find the shared edge in poly_b: looking for B→A.
    let edge_b_start = (0..nb).find(|&i| {
        let j = (i + 1) % nb;
        vertex_key(&vb[i]) == b_key && vertex_key(&vb[j]) == a_key
    })?;

    // Build merged vertex list:
    // 1. poly_a vertices from 0 up to and including edge_a_start (= vertex A)
    // 2. poly_b's non-shared vertices (the detour between A and B), excluding the
    //    shared edge's endpoints in poly_b that duplicate A and B
    // 3. poly_a vertices from edge_a_start + 1 (= vertex B) through the end
    //
    // Vertex B appears exactly once:
    //   - when the shared edge is poly_a's wrap-around edge, edge_a_start == na - 1
    //     so B == va[0] is already emitted by part 1 and part 3 is empty;
    //   - otherwise B == va[edge_a_start + 1] and is emitted as part 3's first vertex.
    // Part 2 never emits B, so neither case duplicates or drops it.

    let mut merged = Vec::with_capacity(na + nb - 2);

    // Part 1: poly_a up to and including A
    for i in 0..=edge_a_start {
        merged.push(va[i]);
    }

    // Part 2: poly_b detour (from after A to before B, exclusive of both shared edge endpoints)
    let b_detour_count = nb - 2; // we skip the two shared-edge vertices
    for k in 0..b_detour_count {
        let idx = (edge_b_start + 2 + k) % nb;
        merged.push(vb[idx]);
    }

    // Part 3: poly_a from B onward (B is at edge_a_start + 1), continuing to the end.
    // In the wrap-around case (edge_a_start == na - 1) this range is empty and B was
    // already emitted by part 1; otherwise B is this loop's first vertex.
    for k in 0..na.saturating_sub(edge_a_start + 1) {
        let idx = edge_a_start + 1 + k;
        merged.push(va[idx]);
    }

    if merged.len() < 3 {
        return None;
    }

    Some(poly_a.clone_with_points(ndarray::Array1::from_vec(merged)))
}

/// Full cleanup pass: remove degenerates, collapse collinear vertices, merge coplanars.
pub fn cleanup_polygons<T: Number>(polygons: Vec<Polygon<T, 3>>) -> Vec<Polygon<T, 3>> {
    // Step 1: Remove collinear vertices and degenerate polygons.
    let cleaned: Vec<Polygon<T, 3>> = polygons
        .into_iter()
        .filter(|p| !is_degenerate(p))
        .filter_map(remove_collinear_vertices)
        .collect();

    // Step 2: Merge coplanar adjacent polygons.
    merge_coplanar_polygons(cleaned)
}

#[cfg(test)]
mod tests {
    use super::*;
    use nalgebra::Point3;

    fn total_vertices<T: Number>(polys: &[Polygon<T, 3>]) -> usize {
        polys.iter().map(|p| p.vertices().len()).sum()
    }

    /// Shoelace area of a planar polygon, projected onto the xy-plane.
    /// (All test polygons here live on z = 0.)
    fn polygon_area_xy(poly: &Polygon<f64, 3>) -> f64 {
        let v = poly.vertices();
        let n = v.len();
        let mut sum = 0.0;
        for i in 0..n {
            let j = (i + 1) % n;
            sum += v[i].x * v[j].y - v[j].x * v[i].y;
        }
        0.5 * sum.abs()
    }

    /// Collect the quantised vertex keys of a polygon (order-independent identity).
    fn vertex_keys(poly: &Polygon<f64, 3>) -> std::collections::BTreeSet<VertexKey> {
        poly.vertices().iter().map(vertex_key).collect()
    }

    #[test]
    fn collinear_vertex_removal() {
        // Triangle with an extra collinear vertex on one edge.
        let p = Polygon::from_points(vec![
            Point3::new(0.0f64, 0.0, 0.0),
            Point3::new(5.0, 0.0, 0.0),
            Point3::new(10.0, 0.0, 0.0), // collinear with prev and next edge direction
            Point3::new(5.0, 10.0, 0.0),
        ])
        .unwrap();
        let cleaned = remove_collinear_vertices(p).unwrap();
        assert_eq!(
            cleaned.vertices().len(),
            3,
            "collinear vertex should be removed"
        );
    }

    #[test]
    fn degenerate_polygon_filtered() {
        // Zero-area polygon (all points collinear).
        let p = Polygon::from_points_and_plane(
            vec![
                Point3::new(0.0f64, 0.0, 0.0),
                Point3::new(1.0, 0.0, 0.0),
                Point3::new(2.0, 0.0, 0.0),
            ],
            Plane::xy_plane(),
        )
        .unwrap();
        assert!(is_degenerate(&p));
    }

    #[test]
    fn coplanar_merge_two_triangles() {
        // Two triangles sharing edge B-C, forming a quad.
        let a = Point3::new(0.0f64, 0.0, 0.0);
        let b = Point3::new(1.0, 0.0, 0.0);
        let c = Point3::new(1.0, 1.0, 0.0);
        let d = Point3::new(0.0, 1.0, 0.0);

        let p1 = Polygon::from_points(vec![a, b, c]).unwrap();
        let p2 = Polygon::from_points(vec![a, c, d]).unwrap();

        assert_eq!(total_vertices(&[p1.clone(), p2.clone()]), 6);

        let merged = merge_coplanar_polygons(vec![p1, p2]);
        let merged_verts = total_vertices(&merged);
        assert!(
            merged_verts <= 4,
            "two triangles sharing an edge should merge to <= 4 vertices, got {merged_verts}"
        );
    }

    #[test]
    fn coplanar_merge_nonwraparound_edge_keeps_all_corners() {
        // Two triangles sharing edge B-C, where the shared edge is NOT poly_a's
        // wrap-around edge (regression for the dropped shared-edge endpoint).
        //
        // poly_a = [a, b, c]  -> shared edge is b->c (edge_a_start == 1, non-wrap)
        // poly_b = [b, d, c]  -> reverse edge c->b
        //
        // The merge must produce the quad [a, b, d, c] with all four distinct
        // corners; the pre-fix code dropped corner c and returned only [a, b, d].
        let a = Point3::new(0.0f64, 0.0, 0.0);
        let b = Point3::new(1.0, 0.0, 0.0);
        let c = Point3::new(1.0, 1.0, 0.0);
        let d = Point3::new(2.0, 0.5, 0.0);

        let p1 = Polygon::from_points(vec![a, b, c]).unwrap();
        let p2 = Polygon::from_points(vec![b, d, c]).unwrap();
        let expected_area = polygon_area_xy(&p1) + polygon_area_xy(&p2);

        let merged = merge_coplanar_polygons(vec![p1, p2]);

        assert_eq!(
            merged.len(),
            1,
            "the two triangles must merge into a single polygon"
        );
        let quad = &merged[0];
        assert_eq!(
            quad.vertices().len(),
            4,
            "merged polygon must be a quad (4 vertices), no dropped/duplicated vertex, got {}",
            quad.vertices().len()
        );

        // All four distinct corners must survive the merge (c is the one the bug dropped).
        let got = vertex_keys(quad);
        for (name, p) in [("a", a), ("b", b), ("c", c), ("d", d)] {
            assert!(
                got.contains(&vertex_key(&p)),
                "merged quad is missing corner {name}"
            );
        }

        // Area must equal the sum of the two triangles (dropping c would halve it).
        assert!(
            (polygon_area_xy(quad) - expected_area).abs() < 1e-9,
            "merged quad area {} should equal the summed triangle area {expected_area}",
            polygon_area_xy(quad)
        );
    }

    #[test]
    fn coplanar_merge_wraparound_edge_keeps_all_corners() {
        // Two triangles where the shared edge IS poly_a's wrap-around edge
        // (edge_a_start == na - 1). This case worked before the fix and must
        // keep working: verify all four corners and correct area.
        //
        // poly_a = [a, b, c]  -> shared edge c->a is the wrap-around edge (index 2->0)
        // poly_b = [a, c, d]  -> reverse edge a->c
        let a = Point3::new(0.0f64, 0.0, 0.0);
        let b = Point3::new(1.0, 0.0, 0.0);
        let c = Point3::new(1.0, 1.0, 0.0);
        let d = Point3::new(0.0, 1.0, 0.0);

        let p1 = Polygon::from_points(vec![a, b, c]).unwrap();
        let p2 = Polygon::from_points(vec![a, c, d]).unwrap();
        let expected_area = polygon_area_xy(&p1) + polygon_area_xy(&p2);

        let merged = merge_coplanar_polygons(vec![p1, p2]);

        assert_eq!(merged.len(), 1, "triangles must merge into one polygon");
        let quad = &merged[0];
        assert_eq!(
            quad.vertices().len(),
            4,
            "wrap-around merge must stay a quad, got {}",
            quad.vertices().len()
        );

        let got = vertex_keys(quad);
        for (name, p) in [("a", a), ("b", b), ("c", c), ("d", d)] {
            assert!(
                got.contains(&vertex_key(&p)),
                "merged quad is missing corner {name}"
            );
        }

        assert!(
            (polygon_area_xy(quad) - expected_area).abs() < 1e-9,
            "merged quad area {} should equal the summed triangle area {expected_area}",
            polygon_area_xy(quad)
        );
    }

    #[test]
    fn cube_union_reduces_polygon_count() {
        // Two overlapping cubes — cleanup after union should produce fewer
        // polygons than the raw polygon count.
        let a = crate::cuboid(10.0, 10.0, 10.0);
        let b = crate::cuboid(10.0, 10.0, 10.0).translate(nalgebra::Vector3::new(5.0, 0.0, 0.0));
        let raw_total = a.polygons().len() + b.polygons().len();
        let result = a.union(b).cleanup();
        let result_count = result.polygons().len();
        // After cleanup, the union must produce fewer polygons than just
        // concatenating both cubes.
        assert!(
            result_count < raw_total,
            "union+cleanup should reduce polygon count: {result_count} should be < {raw_total}"
        );
    }
}