nsys-math-utils 1.0.0

//! Nearest point and distance routines

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

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[expect(clippy::module_name_repetitions)]
pub struct Distance3 <S> {
  distance_squared : NonNegative <S>,
  distance         : Option <NonNegative <S>>,
  nearest_a        : Point3 <S>,
  nearest_b        : Point3 <S>
}

//
//  2D
//

/// Returns the nearest 2D point on 2D triangle together with barycentric coordinates in
/// relation to the triangle edges $ab$ and $ac$
pub fn nearest_triangle2_point2 <S : OrderedField> (
  _triangle : Triangle2 <S>, _point : Point2 <S>
) -> Triangle2Point <S> {
  unimplemented!("TODO: nearest triangle2 point2")
}

/// Nearest 2D point on a 2D line defined by a line segment
pub fn nearest_line2_point2 <S> (line : frame::Line2 <S>, point : Point2 <S>)
  -> Line2Point <S>
where S : OrderedRing {
  let ab        = line.basis;
  let av        = point - line.origin;
  let ab2       = ab.self_dot();
  let ab_dot_av = ab.dot (av);
  let t         = ab_dot_av / *ab2;
  let tab       = *ab * t;
  let at        = line.origin + tab;
  (t, at)
}

/// Returns the nearest point on a 2D segment
pub fn nearest_segment2_point2 <S> (segment : Segment2 <S>, point : Point2 <S>)
  -> Segment2Point <S>
where S : OrderedField {
  use num::One;
  let (t, point) = nearest_line2_point2 (segment.into(), point);
  if t < S::zero() {
    (Normalized::zero(), segment.point_a())
  } else if t > S::one() {
    (Normalized::one(), segment.point_b())
  } else {
    (Normalized::unchecked (t), point)
  }
}

//
//  3D
//

/// Returns the nearest 3D points two 3D triangles together with barycentric coordinates
/// in relation to the triangle edges $ab$ and $ac$
pub fn nearest_triangle3_triangle3 <S> (
  _triangle_a : Triangle3 <S>, _triangle_b : Triangle3 <S>
) -> (Triangle3Point <S>, Triangle3Point <S>) {
  unimplemented!("TODO: nearest triangle3 triangle3")
}

/// Returns the nearest 3D point on 3D triangle together with barycentric coordinates in
/// relation to the triangle edges $ab$ and $ac$, and the nearest point in the 3D line
/// defined by the segment endpoints together with parameter
pub fn nearest_triangle3_line3 <S> (triangle : Triangle3 <S>, line : Segment3 <S>)
  -> (Triangle3Point <S>, Line3Point <S>)
where S : Real + approx::RelativeEq <Epsilon=S> {
  // method from Eberly GeometricTools:
  // <https://github.com/davideberly/GeometricTools/blob/a9a2b149485857273ea1f1cfa5416b392f495882/GTE/Mathematics/DistLine3Triangle3.h>
  let edge1     = triangle.point_b() - triangle.point_a();
  let edge2     = triangle.point_c() - triangle.point_a();
  let n         = edge1.cross (edge2);
  let dir       = line.vector();
  let n_dot_dir = n.dot (*dir);
  if n_dot_dir.abs() > S::zero() {
    // line and triangle are not parallel
    let diff             = line.point_a() - triangle.point_a();
    let n_dot_diff       = n.dot (diff);
    let intersect        = -n_dot_diff / n_dot_dir;
    let y                = line.point_a() + *dir * intersect;
    let tri0_to_y        = y - triangle.point_a();
    let e1_dot_e1        = edge1.dot (edge1);
    let e1_dot_e2        = edge1.dot (edge2);
    let e2_dot_e2        = edge2.dot (edge2);
    let e1_dot_tri0_to_y = edge1.dot (tri0_to_y);
    let e2_dot_tri0_to_y = edge2.dot (tri0_to_y);
    let det = e1_dot_e1 * e2_dot_e2 - e1_dot_e2 * e1_dot_e2;
    let b1  = (e2_dot_e2 * e1_dot_tri0_to_y - e1_dot_e2 * e2_dot_tri0_to_y) / det;
    let b2  = (e1_dot_e1 * e2_dot_tri0_to_y - e1_dot_e2 * e1_dot_tri0_to_y) / det;
    let b0  = S::one() - b1 - b2;
    if b0 >= S::zero() && b1 >= S::zero() && b2 >= S::zero() {
      // line and triangle intersect
      if cfg!(debug_assertions) {
        approx::assert_relative_eq!(triangle.point_a() + edge1 * b1 + edge2 * b2, y,
          epsilon = S::default_epsilon() * S::two().powi (36),
          max_relative = S::default_max_relative() * S::two().powi (36));
      }
      return (
        ([b1, b2].map (Normalized::unchecked), y),
        (intersect, y) )
    }
  }
  // either parallel or intersection outside triangle: nearest point is on an edge
  let mut i0 = 2;
  let mut i1 = 0;
  let mut i2 = 1;
  let mut lowest_dist_sq = None;
  let mut p0 = S::zero();
  let mut barycentric = [Normalized::zero(), Normalized::zero(), Normalized::zero()];
  let triangle_points = triangle.points();
  while i1 < 3 {
    let segment = Segment3::noisy (triangle_points[i0], triangle_points[i1]);
    let ((s0, p_line), (s1, p_seg)) = nearest_line3_segment3 (line, segment);
    let dist_sq = (p_seg - p_line).self_dot();
    if lowest_dist_sq.is_none_or (|lowest| dist_sq < lowest) {
      lowest_dist_sq  = Some (dist_sq);
      p0 = s0;
      // NOTE: in the reference implementation these indices are in a different order,
      // but the following gives the correct results in tests
      barycentric[i0] = s1;
      barycentric[i1] = Normalized::zero();
      barycentric[i2] = Normalized::unchecked (S::one() - *s1);
    }
    i2 = i0;
    i0 = i1;
    i1 += 1;
  }
  let point_tri = triangle.point_a() + edge1 * *barycentric[0] + edge2 * *barycentric[1];
  let point_line = line.point_a() + *dir * p0;
  ( ([barycentric[0], barycentric[1]], point_tri),
    (p0, point_line))
}

/// Returns the nearest 3D point on 3D triangle together with barycentric coordinates in
/// relation to the triangle edges $ab$ and $ac$, and the nearest point in the 3D
/// segment
pub fn nearest_triangle3_segment3 <S> (triangle : Triangle3 <S>, segment : Segment3 <S>)
  -> (Triangle3Point <S>, Segment3Point <S>)
where S : Real + approx::RelativeEq <Epsilon=S> {
  use num::One;
  let (out_tri, (r, near_line)) = nearest_triangle3_line3 (triangle, segment);
  if r < S::zero() {
    let point_a = segment.point_a();
    let out_tri = nearest_triangle3_point3 (triangle, point_a);
    (out_tri, (Normalized::zero(), point_a))
  } else if r > S::one() {
    let point_b = segment.point_b();
    let out_tri = nearest_triangle3_point3 (triangle, point_b);
    (out_tri, (Normalized::one(), point_b))
  } else {
    (out_tri, (Normalized::unchecked (r), near_line))
  }
}

/// Returns the nearest 3D point on 3D triangle together with barycentric coordinates in
/// relation to the triangle edges $ab$ and $ac$
pub fn nearest_triangle3_point3 <S> (triangle : Triangle3 <S>, point : Point3 <S>)
  -> Triangle3Point <S>
where S : OrderedField {
  // method from Eberly GeometricTools:
  // <https://github.com/davideberly/GeometricTools/blob/87e5d3924200515fd49812844812acf232da26aa/GTE/Mathematics/DistPointTriangle.h>
  let d = triangle.point_a() - point;
  let edge0 = triangle.point_b() - triangle.point_a();
  let edge1 = triangle.point_c() - triangle.point_a();
  let e00 = edge0.magnitude_squared();
  let e01 = edge1.dot (edge0);
  let e11 = edge1.magnitude_squared();
  let de0 = d.dot (edge0);
  let de1 = d.dot (edge1);
  let det = (e00 * e11 - e01 * e01).abs();
  let mut s = e01 * de1 - e11 * de0;
  let mut t = e01 * de0 - e00 * de1;
  // NOTE: in a previous version of this function this was a strict inequality
  if s + t <= det {
    if s < S::zero() {
      if t < S::zero() {
        // region 4
        if de0 < S::zero() {
          t = S::zero();
          if -de0 > e00 {
            s = S::one()
          } else {
            s = -de0 / e00
          }
        } else {
          s = S::zero();
          if de1 > S::zero() {
            t = S::zero()
          } else if -de1 >= e11 {
            t = S::one()
          } else {
            t = -de1 / e11
          }
        }
      } else {
        // region 3
        s = S::zero();
        if de1 > S::zero() {
          t = S::zero()
        } else if -de1 >= e11 {
          t = S::one()
        } else {
          t = -de1 / e11
        }
      }
    } else if t < S::zero() {
      // region 5
      t = S::zero();
      if de0 >= S::zero() {
        s = S::zero()
      } else if -de0 >= e00 {
        s = S::one()
      } else {
        s = -de0 / e00
      }
    } else {
      // region 0
      // minimum at interior point
      let idet = S::one() / det;
      s *= idet;
      t *= idet;
    }
  } else if s < S::zero() {
    // region 2
    let tmp0 = e01 + de0;
    let tmp1 = e11 + de1;
    if tmp1 > tmp0 {
      let numer = tmp1 - tmp0;
      let denom = e00 - S::two() * e01 + e11;
      if numer >= denom {
        s = S::one();
        t = S::zero();
      } else {
        s = numer / denom;
        t = S::one() - s;
      }
    } else {
      s = S::zero();
      if tmp1 <= S::zero() {
        t = S::one()
      } else if de1 >= S::zero() {
        t = S::zero()
      } else {
        t = -de1 / e11
      }
    }
  } else if t < S::zero() {
    // region 6
    let tmp0 = e01 + de1;
    let tmp1 = e00 + de0;
    if tmp1 > tmp0 {
      let numer = tmp1 - tmp0;
      let denom = e00 - S::two() * e01 + e11;
      if numer >= denom {
        t = S::one();
        s = S::zero();
      } else {
        t = numer / denom;
        s = S::one() - t;
      }
    } else {
      t = S::zero();
      if tmp1 <= S::zero() {
        s = S::one()
      } else if de0 >= S::zero() {
        s = S::zero()
      } else {
        s = -de0 / e00
      }
    }
  } else {
    // region 1
    let numer = e11 + de1 - e01 - de0;
    if numer <= S::zero() {
      s = S::zero();
      t = S::one();
    } else {
      let denom = e00 - S::two() * e01 + e11;
      if numer >= denom {
        s = S::one();
        t = S::zero();
      } else {
        s = numer / denom;
        t = S::one() - s;
      }
    }
  }
  let nearest = triangle.point_a() + edge0 * s + edge1 * t;
  ([s, t].map (Normalized::unchecked), nearest)
}

/// Returns the nearest points on given 3D lines
pub fn nearest_line3_line3 <S> (line_a : Segment3 <S>, line_b : Segment3 <S>)
  -> (Line3Point <S>, Line3Point <S>)
where S : OrderedField {
  // method from Eberly GeometricTools:
  // <https://github.com/davideberly/GeometricTools/blob/e095b84018766274f1546a7baa12c257fd18f7d8/GTE/Mathematics/DistLineLine.h>
  let line_a_vec = line_a.vector();
  let line_b_vec = line_b.vector();
  let diff       = line_a.point_a() - line_b.point_a();
  let a00        = line_a_vec.magnitude_squared();
  let a01        = -line_a_vec.dot (*line_b_vec);
  let a11        = line_b_vec.magnitude_squared();
  let b0         = line_a_vec.dot (diff);
  let det        = S::max (a00 * a11 - a01 * a01, S::zero());
  let s0;
  let s1;
  if det > S::zero() {
    let b1 = -line_b_vec.dot (diff);
    s0 = (a01 * b1 - a11 * b0) / det;
    s1 = (a01 * b0 - a00 * b1) / det;
  } else {
    s0 = -b0 / a00;
    s1 = S::zero();
  }
  let nearest_a = line_a.point_a() + *line_a_vec * s0;
  let nearest_b = line_b.point_a() + *line_b_vec * s1;
  ((s0, nearest_a), (s1, nearest_b))
}

/// Returns the nearest points in given 3D line and segment
pub fn nearest_line3_segment3 <S> (line : Segment3 <S>, segment : Segment3 <S>)
  -> (Line3Point <S>, Segment3Point <S>)
where S : OrderedField {
  // method from Eberly GeometricTools:
  // <https://github.com/davideberly/GeometricTools/blob/2339413089df7c2b21c2ec403729d9d72f75f2de/GTE/Mathematics/DistLineSegment.h>
  let line_dir = line.vector();
  let seg_dir  = segment.vector();
  let diff     = line.point_a() - segment.point_a();
  let a00      = line_dir.magnitude_squared();
  let a01      = -line_dir.dot (*seg_dir);
  let a11      = seg_dir.magnitude_squared();
  let b0       = line_dir.dot (diff);
  let det      = S::max (a00 * a11 - a01 * a01, S::zero());
  let s0;
  let mut s1;
  if det > S::zero() {
    // line and segment are not parallel
    let b1 = -seg_dir.dot (diff);
    s1 = a01 * b0 - a00 * b1;
    if s1 >= S::zero() {
      if s1 <= det {
        s0 = (a01 * b1 - a11 * b0) / det;
        s1 /= det;
      } else {
        s0 = -(a01 + b0) / a00;
        s1 = S::one();
      }
    } else {
      s0 = -b0 / a00;
      s1 = S::zero();
    }
  } else {
    // line and segment are parallel
    s0 = -b0 / a00;
    s1 = S::zero();
  }
  let p_line = line.point_a() + *line_dir * s0;
  let p_seg  = segment.point_a() + *seg_dir * s1;
  ((s0, p_line), (Normalized::unchecked (s1), p_seg))
}

/// Nearest 3D point on a 3D line defined by a line segment
pub fn nearest_line3_point3 <S> (line : Segment3 <S>, point : Point3 <S>)
  -> Line3Point <S>
where S : OrderedRing {
  let ab = line.vector();
  let av = point.0 - line.point_a().0;
  let ab2 = ab.magnitude_squared();
  let ab_dot_av = ab.dot (av);
  let t = ab_dot_av / ab2;
  let tab = *ab * t;
  let at = line.point_a() + tab;
  (t, at)
}

/// Returns the nearest points on given 3D segments
pub fn nearest_segment3_segment3 <S : Real> (
  _segment_a : Segment3 <S>, _segment_b : Segment3 <S>
) -> (Segment3Point <S>, Segment3Point <S>) {
  unimplemented!("TODO: nearest segment3 segment3")
}

/// Returns the nearest point on a 3D segment
pub fn nearest_segment3_point3 <S> (segment : Segment3 <S>, point : Point3 <S>)
  -> Segment3Point <S>
where S : OrderedField {
  use num::One;
  let (t, point) = nearest_line3_point3 (segment, point);
  if t < S::zero() {
    (Normalized::zero(), segment.point_a())
  } else if t > S::one() {
    (Normalized::one(), segment.point_b())
  } else {
    (Normalized::unchecked (t), point)
  }
}

impl <S : Ring> Distance3 <S> {
  pub const fn nearest_a (&self) -> Point3 <S> {
    self.nearest_a
  }
  pub const fn nearest_b (&self) -> Point3 <S> {
    self.nearest_b
  }
  pub const fn distance_squared (&self) -> NonNegative <S> {
    self.distance_squared
  }
  pub fn distance (&mut self) -> NonNegative <S> where S : Sqrt {
    self.distance.unwrap_or_else (|| {
      let distance = self.distance_squared.sqrt();
      self.distance = Some (distance);
      distance
    })
  }

  pub fn triangle_point (triangle : Triangle3 <S>, point : Point3 <S>) -> Self
    where S : OrderedField
  {
    let (_, nearest_a) = nearest_triangle3_point3 (triangle, point);
    let distance_squared = (point - nearest_a).norm_squared();
    Distance3 { distance_squared, distance: None, nearest_a, nearest_b: point }
  }
}

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

  #[test]
  fn nearest_3d_line_segment() {
    let line = Segment3::noisy (
      [0.0, 0.0, 0.0].into(),
      [1.0, 1.0, 0.0].into());
    let segment = Segment3::noisy (
      [0.0, -1.0, 0.0].into(),
      [1.0, -1.0, 0.0].into());
    let ((s0, p_line), (s1, p_seg)) = nearest_line3_segment3 (line, segment);
    assert_eq!(s0, -0.5);
    assert_eq!(*s1, 0.0);
    assert_eq!(p_line, [-0.5, -0.5, 0.0].into());
    assert_eq!(p_seg, [0.0, -1.0, 0.0].into());
    let segment = Segment3::noisy (
      [0.0, -1.0, 0.0].into(),
      [1.0,  0.0, 0.0].into());
    let ((s0, p_line), (s1, p_seg)) = nearest_line3_segment3 (line, segment);
    assert_eq!(s0, -0.5);
    assert_eq!(*s1, 0.0);
    assert_eq!(p_line, [-0.5, -0.5, 0.0].into());
    assert_eq!(p_seg, [0.0, -1.0, 0.0].into());
  }

  #[test]
  fn nearest_3d_triangle_line() {
    let triangle = Triangle3::noisy (
      [-1.0, -1.0, 0.0].into(),
      [ 1.0, -1.0, 0.0].into(),
      [ 0.0,  1.0, 0.0].into());
    let line = Segment3::noisy (
      [-2.0, -2.0, 0.0].into(),
      [-2.0, -2.0, 1.0].into());
    let (([t0, t1], p_tri), (s0, p_line)) =
      nearest_triangle3_line3 (triangle, line);
    assert_eq!((*t0, *t1), (0.0, 0.0));
    assert_eq!(p_tri, [-1.0, -1.0, 0.0].into());
    assert_eq!(s0, 0.0);
    assert_eq!(p_line, [-2.0, -2.0, 0.0].into());
    let line = Segment3::noisy (
      [ 2.0, -2.0, 0.0].into(),
      [ 2.0, -2.0, 1.0].into());
    let (([t0, t1], p_tri), (s0, p_line)) =
      nearest_triangle3_line3 (triangle, line);
    assert_eq!((*t0, *t1), (1.0, 0.0));
    assert_eq!(p_tri, [1.0, -1.0, 0.0].into());
    assert_eq!(s0, 0.0);
    assert_eq!(p_line, [2.0, -2.0, 0.0].into());
    let line = Segment3::noisy (
      [0.0, 2.0, 0.0].into(),
      [0.0, 2.0, 1.0].into());
    let (([t0, t1], p_tri), (s0, p_line)) =
      nearest_triangle3_line3 (triangle, line);
    assert_eq!((*t0, *t1), (0.0, 1.0));
    assert_eq!(p_tri, [0.0, 1.0, 0.0].into());
    assert_eq!(s0, 0.0);
    assert_eq!(p_line, [0.0, 2.0, 0.0].into());
    let line = Segment3::noisy (
      [0.0, -2.0, 0.0].into(),
      [0.0, -2.0, 1.0].into());
    let (([t0, t1], p_tri), (s0, p_line)) =
      nearest_triangle3_line3 (triangle, line);
    assert_eq!((*t0, *t1), (0.5, 0.0));
    assert_eq!(p_tri, [0.0, -1.0, 0.0].into());
    assert_eq!(s0, 0.0);
    assert_eq!(p_line, [0.0, -2.0, 0.0].into());
  }
}