fexel_hull/src/lib.rs

lib.rs

#![no_std]
extern crate alloc;
use alloc::vec::Vec;
use fexel_num::{vec3x, X19, x19};

const ZERO: x19 = x19::from_bits(0);
const ONE: x19 = X19!(1.0);

/// The object that's used for computing the convex hull. This is basically a
/// bunch of scratch memory so that if you want to avoid allocating a bunch of
/// Vecs for each convex hull computation, you can.
///
/// Usage:
/// ```
/// use fexel_num::*;
/// use fexel_hull::*;
///
/// // A really inefficient way to produce a cube mesh.
/// let mut verts: Vec<vec3x> = vec![];
/// for &x in &[X19!(-1.0), X19!(1.0)] {
///     for &y in &[X19!(-1.0), X19!(1.0)] {
///         for &z in &[X19!(-1.0), X19!(1.0)] {
///             verts.push(vec3x::new(x, y, z));
///         }
///     }
/// }
///
/// let mut api = HullApi::new();
/// let max_vertices = None;
/// let mut triangles = vec![];
/// let used_indices = api.compute_hull(&mut verts, max_vertices, &mut triangles).unwrap();
/// // For this, all indices should be used in the hull. However, for many cases, you won't know this, and
/// // you'll want to do something like `verts.truncate(used_indices)` to trim the excess (unless you need
/// // it for other reasons).
/// assert_eq!(used_indices, verts.len());
/// ```
#[derive(Clone, Debug, Default)]
pub struct HullApi {
    verts: Vec<vec3x>,
    tris: Vec<HullTri>,
    used: Vec<usize>,
    map: Vec<isize>,
    is_extreme: Vec<bool>,
}

impl HullApi {
    /// Equivalent to `HullApi::default()`, but provided for convenience.
    pub fn new() -> Self {
        Self::default()
    }

    /// Clear internal memory. Generally doesn't need to be called explicitly.
    pub fn clear(&mut self) {
        self.verts.clear();
        self.used.clear();
        self.map.clear();
        self.is_extreme.clear();
        self.tris.clear();
    }

    /// Compute the convex hull of the points in `verts`. The triangles making
    /// up the hull are written into `out_triangles`, whose contents are
    /// discarded.
    ///
    /// If `Some(n)` is passed in for max_vertices, It will produce the largest
    /// convex hull that can be constructed using no more than `n` input points.
    ///
    /// Returns `Err(())` on failure. Failure generally indicates a degenerate
    /// mesh (e.g. all points on a single plane, for example), but could
    /// indicate a numerical robustness error, or something. We'll also return
    /// Err if you request nonsense like `max_vertices == 3` or something. Come
    /// on.
    ///
    /// The vertices which are used are moved to the front of `verts`. The
    /// `usize` returned indicates the last index of a vertex which is actually
    /// used. If you don't care about the vertices that are not part of the
    /// convex hull, you might want to truncate the containing vec to this
    /// value.
    pub fn compute_hull(&mut self, verts: &mut [vec3x], max_vertices: Option<usize>, out_triangles: &mut Vec<[u16; 3]>) -> Result<usize, ()> {
        assert!(verts.len() < (i32::max_value() as usize));
        if verts.len() < 4 {
            return Err(());
        }
        let max_vertices = max_vertices.unwrap_or_default();
        // reduced as we add vertices.
        let mut vert_limit: isize = if max_vertices == 0 {
            isize::max_value()
        } else {
            max_vertices as isize
        };
        if vert_limit < 4 {
            return Err(());
        }

        let vert_count = verts.len();
        self.clear();

        {
            let &mut HullApi {
                ref mut tris,
                ref mut is_extreme,
                ..
            } = self;

            is_extreme.resize(vert_count, false);

            let (min_bound, max_bound) = compute_bounds(verts).ok_or(())?;

            // XXXX this is really dodgy...
            let epsilon = (max_bound.dist(min_bound) * X19!(0.1)).max(X19!(0.05));

            let (p0, p1, p2, p3) = find_simplex(verts).ok_or(())?;
            debug_assert!(tris.is_empty());
            tris.extend_from_slice(&build_simplex(p0, p1, p2, p3));

            let center = (verts[p0] + verts[p1] + verts[p2] + verts[p3]) / X19!(4.0);

            is_extreme[p0] = true;
            is_extreme[p1] = true;
            is_extreme[p2] = true;
            is_extreme[p3] = true;

            for t in tris.iter_mut() {
                debug_assert!(t.id >= 0);
                debug_assert!(t.max_v < 0);
                t.update(&verts, None);
            }

            vert_limit -= 4;

            while vert_limit > 0 {
                let te = match find_extrudable(&tris, epsilon) {
                    Some(e) => e,
                    None => break,
                };

                let v = tris[te].max_v as usize;
                debug_assert!(!is_extreme[v]);

                is_extreme[v] = true;

                grow_hull(tris, &verts, v, epsilon);

                fix_degenerate_tris(tris, &verts, center, v, epsilon);

                for tri in tris.iter_mut().rev() {
                    if tri.dead() {
                        continue;
                    }
                    if tri.max_v >= 0 {
                        break;
                    }
                    tri.update(&verts, Some(&is_extreme));
                }

                remove_dead(tris);

                vert_limit -= 1;
            }
        }

        Ok(self.finish_hull(out_triangles, verts))
    }

    fn finish_hull(&mut self, out: &mut Vec<[u16; 3]>, verts: &mut [vec3x]) -> usize {
        let HullApi { used, map, tris, .. } = self;
        used.clear();
        map.clear();
        used.resize(verts.len(), 0usize);
        map.resize(verts.len(), 0isize);

        for t in tris.iter() {
            for ti in &t.vi.at {
                used[*ti as usize] += 1;
            }
        }

        let mut n = 0usize;
        for (i, use_count) in used.iter().enumerate() {
            if *use_count > 0 {
                map[i] = n as isize;
                verts.swap(n, i);
                n += 1;
            } else {
                map[i] = -1;
            }
        }

        for tri in tris.iter_mut() {
            for j in 0..3 {
                let old_pos = tri.vi[j];
                tri.vi[j] = map[old_pos as usize] as i32;
            }
        }
        out.clear();
        out.reserve(tris.len());
        let mut max_idx = 0;
        out.extend(tris.iter().map(|t| {
            let tmaxi = t.vi[0].max(t.vi[1]).max(t.vi[2]) as usize;
            assert!(tmaxi < u16::max_value() as usize, "overflowed u16 for tri index");
            if tmaxi > max_idx {
                max_idx = tmaxi;
            }
            [t.vi[0] as u16, t.vi[1] as u16, t.vi[2] as u16]
        }));

        max_idx + 1
    }
}

#[inline]
fn x_axis() -> vec3x {
    vec3x::new(ONE, ZERO, ZERO)
}

#[inline]
fn y_axis() -> vec3x {
    vec3x::new(ZERO, ONE, ZERO)
}

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
struct I3 {
    at: [i32; 3],
}

#[inline]
fn int3(a: i32, b: i32, c: i32) -> I3 {
    I3 { at: [a, b, c] }
}

#[inline]
fn int3u(a: usize, b: usize, c: usize) -> I3 {
    debug_assert!(
        a <= (i32::max_value() as usize) &&
        b <= (i32::max_value() as usize) &&
        c <= (i32::max_value() as usize)
    );
    int3(a as i32, b as i32, c as i32)
}

impl core::ops::Index<usize> for I3 {
    type Output = i32;
    #[inline]
    fn index(&self, i: usize) -> &i32 {
        &self.at[i]
    }
}

impl core::ops::IndexMut<usize> for I3 {
    #[inline]
    fn index_mut(&mut self, i: usize) -> &mut i32 {
        &mut self.at[i]
    }
}

impl I3 {
    #[inline]
    fn has_vert(self, a: i32) -> bool {
        self[0] == a || self[1] == a || self[2] == a
    }
}

#[inline]
fn tri(verts: &[vec3x], t: I3) -> (vec3x, vec3x, vec3x) {
    (verts[t[0] as usize], verts[t[1] as usize], verts[t[2] as usize])
}

// I don't love this returing Option<T>...
#[inline]
fn try_norm(v: vec3x) -> Option<vec3x> {
    let len = v.len();
    // Should we just compare v == vec3x::zero()? Seems like the len could round
    // down to zero even for a nonzero vec.
    if len == ZERO {
        None
    } else {
        Some(v.norm())
    }
}

#[inline]
fn tri_area(v0: vec3x, v1: vec3x, v2: vec3x) -> x19 {
    (v1 - v0).cross(v2 - v1).len()
}

#[inline]
fn tri_normal(v0: vec3x, v1: vec3x, v2: vec3x) -> Option<vec3x> {
    try_norm((v1 - v0).cross(v2 - v1))
}

fn above(verts: &[vec3x], t: I3, p: vec3x, epsilon: x19) -> bool {
    let (v0, v1, v2) = tri(verts, t);
    if let Some(n) = tri_normal(v0, v1, v2) {
        n.dot(p - v0) > epsilon
    } else {
        false
    }
}

// note: `s` can't be empty.
fn max_dir(dir: vec3x, s: &[vec3x]) -> (vec3x, usize) {
    assert!(!s.is_empty());
    let initial = s[0];

    let mut best = initial;
    let mut best_dot = initial.dot(dir);
    let mut best_index = 0;
    for (i, &item) in s[1..].iter().enumerate() {
        let d = dir.dot(item);
        if d > best_dot {
            best = item;
            best_dot = d;
            best_index = i + 1;
        }
    }
    (best, best_index)
}

#[inline]
fn next_mod3(i: usize) -> (usize, usize) {
    debug_assert!(i < 3);
    ((i + 1) % 3, (i + 2) % 3)
}

#[derive(Copy, Clone, Debug)]
struct HullTri {
    vi: I3,
    ni: I3,
    id: i32,
    max_v: i32,
    rise: x19,
}

impl HullTri {
    #[inline]
    const fn new(vi: I3, ni: I3, id: i32) -> HullTri {
        HullTri {
            vi,
            ni,
            id,
            max_v: -1,
            rise: ZERO,
        }
    }

    #[inline]
    fn dead(&self) -> bool {
        self.ni[0] == -1
    }

    fn neib_idx(&self, va: i32, vb: i32) -> usize {
        for i in 0..3 {
            let (i1, i2) = next_mod3(i);
            if (self.vi[i] == va && self.vi[i1] == vb) || (self.vi[i] == vb && self.vi[i1] == va) {
                return i2;
            }
        }
        unreachable!(
            "Fell through neib loop v={:?} n={:?} va={} vb={}",
            self.vi, self.ni, va, vb
        );
    }

    #[inline]
    fn get_neib(&self, va: i32, vb: i32) -> i32 {
        let idx = self.neib_idx(va, vb);
        self.ni[idx]
    }

    #[inline]
    fn set_neib(&mut self, va: i32, vb: i32, new_value: i32) {
        let idx = self.neib_idx(va, vb);
        self.ni[idx] = new_value;
    }

    fn update(&mut self, verts: &[vec3x], extreme_map: Option<&[bool]>) {
        let (v0, v1, v2) = tri(verts, self.vi);
        let n = tri_normal(v0, v1, v2).unwrap_or_else(|| vec3x::new(ONE, ZERO, ZERO));

        let (vmax, vmax_i) = max_dir(n, verts);
        self.max_v = vmax_i as i32;

        if extreme_map.map_or(false, |m| m[vmax_i]) {
            self.max_v = -1; // already did this one
        } else {
            self.rise = n.dot(vmax - v0);
        }
    }
}

fn neib_fix(tris: &mut [HullTri], k_id: i32) {
    let k = k_id as usize;
    if tris[k].id == -1 {
        return;
    }
    debug_assert!(tris[k].id == k_id);
    for i in 0..3 {
        let (i1, i2) = next_mod3(i);
        if tris[k].ni[i] != -1 {
            let va = tris[k].vi[i2];
            let vb = tris[k].vi[i1];
            let ni = tris[k].ni[i] as usize;
            tris[ni].set_neib(va, vb, k_id);
        }
    }
}

fn swap_neib(tris: &mut [HullTri], a: usize, b: usize) {
    tris.swap(a, b);
    {
        let id = tris[a].id;
        tris[a].id = tris[b].id;
        tris[b].id = id;
    }
    neib_fix(tris, a as i32);
    neib_fix(tris, b as i32);
}


fn fix_back_to_back(tris: &mut [HullTri], s: usize, t: usize) {
    for i in 0..3 {
        let (i1, i2) = next_mod3(i);
        let (va, vb) = (tris[s].vi[i1], tris[s].vi[i2]);
        debug_assert_eq!(tris[tris[s].get_neib(va, vb) as usize].get_neib(vb, va), tris[s].id);
        debug_assert_eq!(tris[tris[t].get_neib(va, vb) as usize].get_neib(vb, va), tris[t].id);

        let t_neib = tris[t].get_neib(vb, va);
        tris[tris[s].get_neib(va, vb) as usize].set_neib(vb, va, t_neib);

        let s_neib = tris[s].get_neib(va, vb);
        tris[tris[t].get_neib(vb, va) as usize].set_neib(va, vb, s_neib);
    }
    // cleaned up later
    tris[s].ni = int3(-1, -1, -1);
    tris[t].ni = int3(-1, -1, -1);
}

#[inline]
fn check_tri(tris: &[HullTri], t: &HullTri) {
    if cfg!(debug_assertions) {
        debug_assert!(tris[t.id as usize].id == t.id);
        debug_assert!(tris[t.id as usize].id == t.id);
        for i in 0..3 {
            let (i1, i2) = next_mod3(i);
            let (a, b) = (t.vi[i1], t.vi[i2]);
            debug_assert!(a != b);
            debug_assert!(tris[t.ni[i] as usize].get_neib(b, a) == t.id);
        }
    }
}

fn extrude(tris: &mut Vec<HullTri>, t0: usize, v: usize) {
    let bu = tris.len();
    let b = bu as i32;

    let n = tris[t0].ni;
    let t = tris[t0].vi;

    let vi = v as i32;
    tris.reserve(3);

    tris.push(HullTri::new(int3(vi, t[1], t[2]), int3(n[0], b + 1, b + 2), b));
    tris[n[0] as usize].set_neib(t[1], t[2], b);

    tris.push(HullTri::new(int3(vi, t[2], t[0]), int3(n[1], b + 2, b), b + 1));
    tris[n[1] as usize].set_neib(t[2], t[0], b + 1);

    tris.push(HullTri::new(int3(vi, t[0], t[1]), int3(n[2], b, b + 1), b + 2));
    tris[n[2] as usize].set_neib(t[0], t[1], b + 2);

    tris[t0].ni = int3(-1, -1, -1);

    // @@TODO: disable in debug?
    check_tri(&tris, &tris[bu]);
    check_tri(&tris, &tris[bu + 1]);
    check_tri(&tris, &tris[bu + 2]);

    if tris[n[0] as usize].vi.has_vert(vi) {
        fix_back_to_back(&mut tris[..], bu, n[0] as usize);
    }
    if tris[n[1] as usize].vi.has_vert(vi) {
        fix_back_to_back(&mut tris[..], bu + 1, n[1] as usize);
    }
    if tris[n[2] as usize].vi.has_vert(vi) {
        fix_back_to_back(&mut tris[..], bu + 2, n[2] as usize);
    }
}

fn find_extrudable(tris: &[HullTri], epsilon: x19) -> Option<usize> {
    assert_ne!(tris.len(), 0);
    let mut best = 0usize;
    for (idx, tri) in tris.iter().enumerate() {
        debug_assert!(tri.id >= 0);
        debug_assert_eq!(tri.id, (idx as i32));
        debug_assert!(!tri.dead());
        if best != idx && tris[best].rise < tri.rise {
            best = idx
        }
    }
    if tris[best].rise > epsilon {
        Some(best)
    } else {
        None
    }
}

fn find_simplex(verts: &[vec3x]) -> Option<(usize, usize, usize, usize)> {
    assert!(verts.len() >= 4);
    let b0 = vec3x::new(X19!(0.01), X19!(0.02), X19!(1.0));

    let (p0v, p0i) = max_dir(b0, verts);
    let (p1v, p1i) = max_dir(-b0, verts);

    let b0v = p0v - p1v;

    if p0i == p1i || b0 == vec3x::zero() {
        return None;
    }
    // lazy way to create a basis, whatever.
    let b1 = x_axis().cross(b0v);
    let b2 = y_axis().cross(b0v);

    let b1 = (if b1.len_sq() > b2.len_sq() { b1 } else { b2 }).norm();

    let (p2v, p2i) = max_dir(b1, verts);

    let (p2v, p2i) = if p2i == p0i || p2i == p1i {
        max_dir(-b1, verts)
    } else {
        (p2v, p2i)
    };

    if p2i == p0i || p2i == p1i {
        return None;
    }

    let b1 = p2v - p0v;

    let b2 = b1.cross(b0);

    let (p3v, p3i) = max_dir(b2, verts);

    let (p3v, p3i) = if p3i == p0i || p3i == p1i || p3i == p2i {
        max_dir(-b2, verts)
    } else {
        (p3v, p3i)
    };

    if p3i == p0i || p3i == p1i || p3i == p2i {
        return None;
    }

    debug_assert!(p0i != p1i && p0i != p2i && p0i != p3i && p1i != p2i && p1i != p3i && p2i != p3i);
    // adjust winding order
    if (p3v - p0v).dot((p1v - p0v).cross(p2v - p0v)) < ZERO {
        Some((p0i, p1i, p3i, p2i))
    } else {
        Some((p0i, p1i, p2i, p3i))
    }
}

fn remove_dead(tris: &mut Vec<HullTri>) {
    for j in (0..tris.len()).rev() {
        if !tris[j].dead() {
            continue;
        }
        let last = tris.len() - 1;
        swap_neib(tris, j, last);
        tris.pop();
    }
}

fn fix_degenerate_tris(tris: &mut Vec<HullTri>, verts: &[vec3x], center: vec3x, vert_id: usize, epsilon: x19) {
    let mut i: isize = tris.len() as isize;
    loop {
        i -= 1;
        if i < 0 {
            break;
        }
        let iu = i as usize;
        if tris[iu].dead() {
            continue;
        }

        if !tris[iu].vi.has_vert(vert_id as i32) {
            break;
        }

        let nt = tris[iu].vi;
        let (nv0, nv1, nv2) = tri(verts, nt);

        let is_flipped = above(verts, nt, center, epsilon * X19!(0.01));
        // XXX this should be epsilon * epsilon * 0.1, but that seems guaranteed
        // to underflow... should be able to rewrite to avoid that issue but i
        // must be tired or something, since I tried and got bustage.
        if is_flipped || tri_area(nv0, nv1, nv2) < epsilon * epsilon {
            debug_assert!(tris[iu].ni[0] >= 0);
            let nb = tris[iu].ni[0] as usize;

            debug_assert!(!tris[nb].dead());
            debug_assert!(!tris[nb].vi.has_vert(vert_id as i32));
            debug_assert!(tris[nb].id < (i as i32));

            extrude(tris, nb, vert_id);
            i = tris.len() as isize;
        }
    }
}

// extrude any triangles who would be below the hull containing the new vertex
fn grow_hull(tris: &mut Vec<HullTri>, verts: &[vec3x], vert_id: usize, epsilon: x19) {
    for j in (0..tris.len()).rev() {
        if tris[j].dead() {
            continue;
        }
        let t = tris[j].vi;
        if above(verts, t, verts[vert_id], epsilon * X19!(0.01)) {
            extrude(tris, j, vert_id);
        }
    }
}

fn build_simplex(p0: usize, p1: usize, p2: usize, p3: usize) -> [HullTri; 4] {
    let tris = [
        HullTri::new(int3u(p2, p3, p1), int3(2, 3, 1), 0),
        HullTri::new(int3u(p3, p2, p0), int3(3, 2, 0), 1),
        HullTri::new(int3u(p0, p1, p3), int3(0, 1, 3), 2),
        HullTri::new(int3u(p1, p0, p2), int3(1, 0, 2), 3),
    ];

    check_tri(&tris, &tris[0]);
    check_tri(&tris, &tris[1]);
    check_tri(&tris, &tris[2]);
    check_tri(&tris, &tris[3]);

    tris
}

fn compute_bounds(v: &[vec3x]) -> Option<(vec3x, vec3x)> {
    let (&b, rest) = v.split_first()?;
    let mut min_bound = b;
    let mut max_bound = b;
    for &item in rest {
        min_bound = min_bound.min(item);
        max_bound = max_bound.max(item);
    }
    Some((min_bound, max_bound))
}

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

    #[test]
    fn test_basic() {
        // A more fleshed-out version of the doctest.
        let mut verts: Vec<vec3x> = Vec::with_capacity(8);
        for &x in &[X19!(-1.0), X19!(1.0)] {
            for &y in &[X19!(-1.0), X19!(1.0)] {
                for &z in &[X19!(-1.0), X19!(1.0)] {
                    verts.push(vec3x::new(x, y, z));
                    // Add some garbage points the algo has to discard.
                    verts.push(vec3x::new(x / X19!(2.0), y / X19!(2.0), z / X19!(2.0)));
                }
            }
        }

        verts.push(vec3x::zero());

        let mut api = HullApi::new();
        let max_vertices = None;
        let mut triangles = Vec::with_capacity(12);
        let used_indices = api.compute_hull(&mut verts, max_vertices, &mut triangles).unwrap();

        assert_eq!(triangles.len(), 12);
        assert_eq!(used_indices, 8);
        verts.truncate(used_indices);
        for &[a, b, c] in &triangles {
            assert!(a != b && b != c && a != c);
            let v0 = verts[a as usize];
            let v1 = verts[b as usize];
            let v2 = verts[c as usize];
            assert!(v0 != v1 && v1 != v2 && v2 != v0);
            // only the extreme points should be present.
            for v in &[v0, v1, v2] {
                assert_eq!(v.x().abs(), X19!(1.0));
                assert_eq!(v.y().abs(), X19!(1.0));
                assert_eq!(v.z().abs(), X19!(1.0));
            }
        }
    }
}