nsys-math-utils 0.2.0

//! Convex hulls

use std;
use std::collections::{BTreeSet, VecDeque};
use approx;

use crate::*;
use super::*;

use geometry::mesh::VertexEdgeTriangleMesh;
use geometry::mesh::edge_triangle::{self, TriangleKey};

/// 2D convex hull
#[derive(Clone, Debug, Eq, PartialEq)]
pub struct Hull2 <S> {
  /// Set of unique points sorted in counter-clockwise order for efficient
  /// minimum bounding box algorithm
  points : Vec <Point2 <S>>
}

/// 3D convex hull
#[derive(Clone, Debug, Eq, PartialEq)]
pub struct Hull3 <S> {
  points : Vec <Point3 <S>>
}

impl <S> Hull2 <S> {
  /// Create a new 2D convex hull from a bag of points.
  ///
  /// Uses Graham scan algorithm.
  ///
  /// Input must contain at least 1 point:
  /// ```
  /// # use math_utils::geometry::Hull2;
  /// assert!(Hull2::<f32>::from_points (&[]).is_none());
  /// ```
  pub fn from_points (points : &[Point2 <S>]) -> Option <Self> where S : Real {
    Self::from_points_indices (points).map (|indices|{
      let points = indices.iter().map (|i| points[*i as usize]).collect();
      Hull2 { points }
    })
  }

  /// Points sorted in counter-clockwise order
  pub fn points (&self) -> &[Point2 <S>] {
    &self.points
  }

  #[expect(clippy::missing_asserts_for_indexing)]
  fn from_points_indices (points : &[Point2 <S>]) -> Option <Vec <u32>> where S : Real {
    // code adapted from scirs2-spatial:
    // <https://github.com/cool-japan/scirs/blob/a176b462aca55e73bd4b25eea83aad10a9f4a2b0/scirs2-spatial/src/convex_hull.rs>
    use std::cmp::Ordering;
    match points.len() {
      0 => return None,
      1 => return Some (vec![0]),
      2 => {
        let v = if points[0] == points[1] {
          vec![0]
        } else {
          vec![0, 1]
        };
        return Some (v)
      }
      _ => {} // continue
    }
    let mut indexed_points = points.iter().copied().enumerate()
      .collect::<Vec <(usize, Point2 <S>)>>();
    // find bottom-most (lowest y-coordinate), then left-most x
    let start_index = indexed_points.iter().min_by (|a, b|{
      let cmp = a.1.0.y.partial_cmp (&b.1.0.y).unwrap();
      if cmp == Ordering::Equal {
        a.1.0.x.partial_cmp (&b.1.0.x).unwrap()
      } else {
        cmp
      }
    }).unwrap().0;
    let start_point = indexed_points[start_index].1;
    // sort points by polar angle with respect to start point
    indexed_points.sort_by (|a, b| {
      if a.0 == start_index {
        return Ordering::Less
      }
      if b.0 == start_index {
        return Ordering::Greater
      }
      let angle_a = (a.1.0.y - start_point.0.y)
        .atan2 (a.1.0.x - start_point.0.x);
      let angle_b = (b.1.0.y - start_point.0.y)
        .atan2 (b.1.0.x - start_point.0.x);
      let angle_cmp = angle_a.partial_cmp (&angle_b).unwrap();
      if angle_cmp == Ordering::Equal {
        // if angles are equal sort by distance
        let dist_a = (a.1.0.x - start_point.0.x).powi (2) +
          (a.1.0.y - start_point.0.y).powi (2);
        let dist_b = (b.1.0.x - start_point.0.x).powi (2) +
          (b.1.0.y - start_point.0.y).powi (2);
        dist_a.partial_cmp (&dist_b).unwrap()
      } else {
        angle_cmp
      }
    });
    // graham scan
    let mut stack : Vec <u32> = Vec::new();
    for (point_index, point) in indexed_points {
      while stack.len() >= 2 {
        let top = stack[stack.len() - 1];
        let second = stack[stack.len() -2];
        let p1 = points[second as usize];
        let p2 = points[top as usize];
        let p3 = point;
        let det = Matrix2 {
          cols: vector2 (p2 - p1, p3 - p1)
        }.determinant();
        if det <= S::zero() {
          stack.pop();
        } else {
          break
        }
      }
      #[expect(clippy::cast_possible_truncation)]
      stack.push (point_index as u32)
    }
    Some (stack)
  }
}

impl <S> Hull3 <S> {
  pub fn from_points (points : &[Point3 <S>]) -> Option <Self> where
    S : Real + approx::RelativeEq
  {
    Self::from_points_with_mesh (points).map (|x| x.0)
  }

  pub fn from_points_with_mesh (points : &[Point3 <S>])
    -> Option <(Self, VertexEdgeTriangleMesh)>
  where S : Real + approx::RelativeEq {
    // code adapted from GeometricTools:
    // <https://github.com/davideberly/GeometricTools/blob/f78dd0b65bc3a0a4723586de6991dd2c339b08ad/GTE/Mathematics/ConvexHull3.h>
    // TODO: multi-threaded
    let point_hull = |points : Vec <Point3 <S>>| {
      debug_assert_eq!(points.len(), 1);
      ( Hull3 { points },
        VertexEdgeTriangleMesh::with_vertex (edge_triangle::Vertex::new (0))
      )
    };
    let line_hull = |points : Vec <Point3 <S>>| {
      debug_assert_eq!(points.len(), 2);
      ( Hull3 { points },
        VertexEdgeTriangleMesh::with_edge (edge_triangle::Edge::new (0, 1))
      )
    };
    match points.len() {
      0 => return None,
      n if n < 3 => {
        let mut points = points.to_vec();
        points.dedup();
        match points.len() {
          1 => return Some (point_hull (points)),
          2 => return Some (line_hull (points)),
          _ => unreachable!()
        }
      }
      _ => {}
    }
    let get_point = |i : u32| points[i as usize];
    let collect_points = |indices : &[u32]|
      indices.iter().copied().map (get_point).collect();
    let sorted = {
      #[expect(clippy::cast_possible_truncation)]
      let mut sorted = (0..points.len() as u32).collect::<Vec<_>>();
      sorted.sort_by (|a, b|
        Point3::partial_cmp (&get_point (*a), &get_point (*b)).unwrap());
      sorted.dedup_by_key (|i| get_point(*i));
      sorted
    };
    let mut hull = Vec::with_capacity (sorted.len());
    hull.push (sorted[0]);  // hull[0]
    let mut current = 1;
    let mut dimension = 0;
    // point hull
    for i in &sorted[current..] {
      if get_point (hull[0]) != get_point (*i) {
        dimension = 1;
        break
      }
      current += 1;
    }
    debug_assert_eq!(hull.len(), 1);
    if dimension == 0 {
      let points = collect_points (hull.as_slice());
      return Some (point_hull (points))
    }
    // linear hull
    hull.push (sorted[current]);  // hull[1]
    current += 1;
    for i in &sorted[current..] {
      if !colinear_3d (&get_point (hull[0]), &get_point (hull[1]), &get_point (*i)) {
        dimension = 2;
        break
      }
      hull.push (sorted[current]);
      current += 1;
    }
    if hull.len() > 2 {
      // keep endpoints
      hull.sort_by (|a, b|
        Point3::partial_cmp (&get_point (*a), &get_point (*b)).unwrap());
      hull.drain (1..hull.len()-1);
    }
    debug_assert_eq!(hull.len(), 2);
    if dimension == 1 {
      let points = collect_points (hull.as_slice());
      return Some (line_hull (points))
    }
    // planar hull
    hull.push (sorted[current]);  // hull[2]
    current += 1;
    while current < sorted.len() {
      if !coplanar_3d (
        &get_point (hull[0]), &get_point (hull[1]), &get_point (hull[2]),
        &get_point (sorted[current])
      ) {
        dimension = 3;
        break
      }
      hull.push (sorted[current]);
      current += 1;
    }
    if hull.len() > 3 {
      // compute planar convex hull
      let v0     = get_point (hull[0]);
      let v1     = get_point (hull[1]);
      let v2     = get_point (hull[2]);
      let diff1  = v1 - v0;
      let diff2  = v2 - v0;
      let mut normal = diff1.cross (diff2);
      let signs  = normal.sigvec();
      let c;
      debug_assert_eq!(signs.len(), 3);
      normal = normal.map (|s| s.abs());
      #[expect(clippy::collapsible_else_if)]
      if normal.x > normal.y {
        if normal.x > normal.z {
          if signs[0] > S::zero() {
            c = [1, 2];
          } else {
            c = [2, 1];
          }
        } else {
          if signs[2] > S::zero() {
            c = [0, 1];
          } else {
            c = [1, 0];
          }
        }
      } else {
        if normal.y > normal.z {
          if signs[1] > S::zero() {
            c = [2, 0];
          } else {
            c = [0, 2];
          }
        } else {
          if signs[2] > S::zero() {
            c = [0, 1];
          } else {
            c = [1, 0];
          }
        }
      }
      let projections = hull.iter().copied()
        .map (|i| point2 (get_point (i).0[c[0]], get_point (i).0[c[1]]))
        .collect::<Vec <_>>();
      let hull2_indices = Hull2::from_points_indices (projections.as_slice()).unwrap();
      hull = hull2_indices.iter().map (|i| hull[*i as usize]).collect::<Vec <_>>();
    }
    if dimension == 2 {
      let points = collect_points (hull.as_slice());
      // TODO: the following mesh creation relies on the fact that the points in a 2d
      // hull are sorted in counter-clockwise order, but it is not necessarily a good
      // triangulation
      debug_assert!(points.len() >= 3);
      let mut mesh = VertexEdgeTriangleMesh::default();
      #[expect(clippy::cast_possible_truncation)]
      for i in 1u32..points.len() as u32 - 1 {
        mesh.insert (0, i, i+1);
      }
      return Some ((Hull3 { points }, mesh))
    }
    // 3-dimensional hull
    let plane_side = |a, b, c, d| {
      let [s0, s1, s2, s3] = [a, b, c, d].map (get_point);
      let diff1 = s1 - s0;
      let diff2 = s2 - s0;
      let diff3 = s3 - s0;
      diff1.dot (diff2.cross (diff3)).signum_or_zero()
    };
    let sign = plane_side (hull[0], hull[1], hull[2], sorted[current]);
    let mut mesh = VertexEdgeTriangleMesh::default();
    let mut h0 = hull[0];
    let mut h1;
    if sign > S::zero() {
      for i in 1..hull.len()-1 {
        h1 = hull[i];
        let h2 = hull[i+1];
        let inserted = mesh.insert (h0, h2, h1);
        debug_assert!(inserted.is_some());
      }
      h0 = sorted[current];
      let mut i1 = hull.len() - 1;
      let mut i2 = 0;
      while i2 < hull.len() {
        h1 = hull[i1];
        let h2 = hull[i2];
        let inserted = mesh.insert (h0, h1, h2);
        debug_assert!(inserted.is_some());
        // iter
        i1 = i2;
        i2 += 1;
      }
    } else {
      for i in 1..hull.len()-1 {
        h1 = hull[i];
        let h2 = hull[i+1];
        let inserted = mesh.insert (h0, h1, h2);
        debug_assert!(inserted.is_some());
      }
      h0 = sorted[current];
      let mut i1 = hull.len() - 1;
      let mut i2 = 0;
      while i2 < hull.len() {
        h1 = hull[i1];
        let h2 = hull[i2];
        let inserted = mesh.insert (h0, h2, h1);
        debug_assert!(inserted.is_some());
        // iter
        i1 = i2;
        i2 += 1;
      }
    }
    let mut terminator : Vec <[u32; 2]> = Vec::new();
    current += 1;
    while current < sorted.len() {
      let vertex = mesh.get_vertex (h0).unwrap();
      h1 = sorted[current];
      let mut visible = VecDeque::<TriangleKey>::new();
      let mut visited = BTreeSet::<TriangleKey>::new();
      for triangle_key in vertex.adjacent_triangles().iter() {
        let triangle = mesh.get_triangle (triangle_key).unwrap();
        let sign = plane_side (
          triangle.vertices()[0], triangle.vertices()[1], triangle.vertices()[2], h1);
        if sign > S::zero() {
          visible.push_back (*triangle_key);
          visited.insert (*triangle_key);
          break
        }
      }
      debug_assert!(visible.len() > 0);
      debug_assert!(terminator.is_empty());
      while visible.len() > 0 {
        let triangle_key = visible.pop_front().unwrap();
        let triangle = mesh.get_triangle (&triangle_key).copied().unwrap();
        for (i, adjacent_key) in triangle.adjacent_triangles().iter().enumerate() {
          if let Some (key) = adjacent_key {
            let adjacent = mesh.get_triangle (key).unwrap();
            if plane_side (
              adjacent.vertices()[0], adjacent.vertices()[1], adjacent.vertices()[2], h1
            ) <= S::zero() {
              terminator.push (
                [triangle.vertices()[i], triangle.vertices()[(i + 1) % 3]]);
            } else if visited.insert (*key) {
              visible.push_back (*key);
            }
          }
        }
        visited.remove (&triangle_key);
        let removed = mesh.remove (
          triangle.vertices()[0], triangle.vertices()[1], triangle.vertices()[2]);
        debug_assert!(removed);
      }
      // TODO: remove expect if clippy issue #15119 is resolved
      #[expect(clippy::iter_with_drain)]
      for edge in terminator.drain(..) {
        let inserted = mesh.insert (edge[0], edge[1], h1);
        debug_assert!(inserted.is_some());
      }
      h0 = h1;
      current += 1;
    }
    let points = mesh.vertices().keys().copied().map (get_point).collect();
    Some ((Hull3 { points }, mesh))
  }

  pub fn points (&self) -> &[Point3 <S>] {
    &self.points
  }
}

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

  #[test]
  fn hull2() {
    use crate::*;
    // dedup unique points
    let points : Vec <Point2 <f32>> = [
      [ 1.0,  1.0],
      [ 1.0,  1.0]
    ].map (Point2::from).into_iter().collect();
    let hull = Hull2::from_points (points.as_slice()).unwrap();
    assert_eq!(hull.points(), &[[1.0, 1.0].into()]);
    // interior point at origin is excluded
    let points : Vec <Point2 <f32>> = [
      [ 0.0,  0.0],
      [ 1.0,  3.0],
      [ 2.0, -3.0],
      [-3.0, -1.0]
    ].map (Point2::from).into_iter().collect();
    let hull = Hull2::from_points (points.as_slice()).unwrap();
    // points are in counter-clockwise order
    let points : Vec <Point2 <f32>> = [
      [ 2.0, -3.0],
      [ 1.0,  3.0],
      [-3.0, -1.0]
    ].map (Point2::from).into_iter().collect();
    assert_eq!(hull.points(), points);
    // colinear point on edge is excluded
    let points : Vec <Point2 <f32>> = [
      [ 0.0,  2.0],
      [-2.0, -2.0],
      [ 0.0, -2.0],
      [ 2.0, -2.0]
    ].map (Point2::from).into_iter().collect();
    let hull = Hull2::from_points (points.as_slice()).unwrap();
    // points are in counter-clockwise order
    let points : Vec <Point2 <f32>> = [
      [-2.0, -2.0],
      [ 2.0, -2.0],
      [ 0.0,  2.0]
    ].map (Point2::from).into_iter().collect();
    assert_eq!(hull.points(), points);
    // multiple edge and interior points are excluded
    let points : Vec <Point2 <f32>> = [
      // perimeter points
      [ 0.0,  6.0],
      [ 1.0,  5.0],
      [ 2.0,  4.0],
      [ 3.0,  3.0],
      [ 3.0,  1.0],
      [ 3.0, -1.0],
      [ 3.0, -3.0],
      [ 1.0, -3.0],
      [-1.0, -3.0],
      [-3.0, -3.0],
      [-3.0, -1.0],
      [-3.0,  1.0],
      [-3.0,  3.0],
      [-2.0,  4.0],
      [-1.0,  5.0],
      // interior points
      [-1.0,  2.0],
      [ 2.0, -1.0],
      [ 0.0,  3.0],
      [-2.0, -2.0]
    ].map (Point2::from).into_iter().collect();
    let hull = Hull2::from_points (points.as_slice()).unwrap();
    // points are in counter-clockwise order
    let points : Vec <Point2 <f32>> = [
      [-3.0, -3.0],
      [ 3.0, -3.0],
      [ 3.0,  3.0],
      [ 0.0,  6.0],
      [-3.0,  3.0]
    ].map (Point2::from).into_iter().collect();
    assert_eq!(hull.points(), points);
  }

  #[test]
  fn hull3() {
    // dedup 0 dimensional
    let points = [
      [-1.0, -4.0, -1.0],
      [-1.0, -4.0, -1.0]
    ].map (Point3::<f32>::from);
    let hull = Hull3::from_points (points.as_slice()).unwrap();
    assert_eq!(hull.points(), &[
      [-1.0, -4.0, -1.0]
    ].map (Into::into));
    // 1 dimensional endpoints
    let points = [
      [-1.0, -4.0, -1.0],
      [ 0.0,  0.0,  0.0],
      [ 1.0,  4.0,  1.0]
    ].map (Point3::<f32>::from);
    let hull = Hull3::from_points (points.as_slice()).unwrap();
    assert_eq!(hull.points(), &[
      [-1.0, -4.0, -1.0],
      [ 1.0,  4.0,  1.0]
    ].map (Into::into));
    // 2 dimensional
    let points = [
      [-1.0, -4.0, 0.0],
      [ 2.0,  2.0, 0.0],
      [-1.0, -1.0, 0.0],
      [-4.0, -1.0, 0.0],
      [ 0.0,  2.0, 0.0]
    ].map (Point3::<f32>::from);
    let hull = Hull3::from_points (points.as_slice()).unwrap();
    assert_eq!(hull.points(), &[
      [-1.0, -4.0, 0.0],
      [ 2.0,  2.0, 0.0],
      [ 0.0,  2.0, 0.0],
      [-4.0, -1.0, 0.0]
    ].map (Into::into));
    // already a hull
    let points = [
      [-1.0, -4.0, -1.0],
      [ 2.0,  2.0,  2.0],
      [-4.0, -1.0,  2.0],
      [ 0.0,  2.0, -3.0]
    ].map (Point3::<f32>::from);
    let hull = Hull3::from_points (points.as_slice()).unwrap();
    assert_eq!(hull.points(), &[
      [-1.0, -4.0, -1.0],
      [ 2.0,  2.0,  2.0],
      [-4.0, -1.0,  2.0],
      [ 0.0,  2.0, -3.0]
    ].map (Into::into));
    let points = [
      [ 1.0, -1.0, -1.0],
      [ 1.0, -1.0,  1.0],
      [ 1.0,  1.0, -1.0],
      [ 1.0,  1.0,  1.0],
      [-1.0, -1.0, -1.0],
      [-1.0, -1.0,  1.0],
      [-1.0,  1.0, -1.0],
      [-1.0,  1.0,  1.0]
    ].map (Point3::<f32>::from);
    let hull = Hull3::from_points (points.as_slice()).unwrap();
    assert_eq!(hull.points(), points.as_slice());
    // removes interior points
    let points = [
      [-1.0, -4.0, -1.0],
      [ 2.0,  2.0,  2.0],
      [-4.0, -1.0,  2.0],
      [ 0.0,  2.0, -3.0],
      [ 0.1,  0.2,  0.3],
      [-0.1, -0.2, -0.3],
      [-0.1,  0.2,  0.3]
    ].map (Point3::<f32>::from);
    let hull = Hull3::from_points (points.as_slice()).unwrap();
    assert_eq!(hull.points(), &[
      [-1.0, -4.0, -1.0],
      [ 2.0,  2.0,  2.0],
      [-4.0, -1.0,  2.0],
      [ 0.0,  2.0, -3.0]
    ].map (Into::into));
  }
}