nsys-math-utils 0.1.0

//! Primitive intersection routines

use approx;

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

pub mod integer;

/// Discrete intersection test of 1D axis-aligned bounding boxes (intervals).
///
/// AABBs that are merely touching are not counted as intersecting:
///
/// ```
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let a = Interval::from_points ( 0.0, 1.0);
/// let b = Interval::from_points (-1.0, 0.0);
/// assert!(!discrete_interval (&a, &b));
/// ```
#[inline]
pub fn discrete_interval <S : Ring> (a : &Interval <S>, b : &Interval <S>)
  -> bool
{
  let (min_a, max_a) = (a.min(), a.max());
  let (min_b, max_b) = (b.min(), b.max());
  // intersection if intervals overlap
  max_a > min_b && min_a < max_b
}

/// Continuous intersection test of 1D axis-aligned bounding boxes (intervals).
///
/// AABBs that are merely touching return no intersection:
///
/// ```
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let a = Interval::from_points ( 0.0, 1.0);
/// let b = Interval::from_points (-1.0, 0.0);
/// assert!(continuous_interval (&a, &b).is_none());
/// ```
#[inline]
pub fn continuous_interval <S> (a : &Interval <S>, b : &Interval <S>)
  -> Option <Interval <S>>
where
  S : Ring + std::fmt::Debug
{
  if discrete_interval (a, b) {
    Some (Interval::with_minmax (
      S::max (a.min(), b.min()),
      S::min (a.max(), b.max())
    ))
  } else {
    None
  }
}

/// Discrete intersection test of 2D axis-aligned bounding boxes.
///
/// AABBs that are merely touching are not counted as intersecting:
///
/// ```
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let a = Aabb2::from_points ([ 0.0, 0.0].into(), [1.0, 1.0].into());
/// let b = Aabb2::from_points ([-1.0, 0.0].into(), [0.0, 1.0].into());
/// assert!(!discrete_aabb2_aabb2 (&a, &b));
/// ```
#[inline]
pub fn discrete_aabb2_aabb2 <S : Ring> (a : &Aabb2 <S>, b : &Aabb2 <S>)
  -> bool
{
  let (min_a, max_a) = (a.min(), a.max());
  let (min_b, max_b) = (b.min(), b.max());
  // intersection if overlap exists on both axes
  max_a.0.x > min_b.0.x && min_a.0.x < max_b.0.x &&
  max_a.0.y > min_b.0.y && min_a.0.y < max_b.0.y
}

/// Continuous intersection test of 2D axis-aligned bounding boxes.
///
/// AABBs that are merely touching return no intersection:
///
/// ```
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let a = Aabb2::from_points ([ 0.0, 0.0].into(), [1.0, 1.0].into());
/// let b = Aabb2::from_points ([-1.0, 0.0].into(), [0.0, 1.0].into());
/// assert!(continuous_aabb2_aabb2 (&a, &b).is_none());
/// ```
#[inline]
pub fn continuous_aabb2_aabb2 <S> (a : &Aabb2 <S>, b : &Aabb2 <S>)
  -> Option <Aabb2 <S>>
where
  S : Ring + std::fmt::Debug
{
  if discrete_aabb2_aabb2 (a, b) {
    Some (Aabb2::with_minmax (
      point2_max (a.min(), b.min()),
      point2_min (a.max(), b.max())
    ))
  } else {
    None
  }
}

/// Discrete intersection test of 3D axis-aligned bounding boxes.
///
/// AABBs that are merely touching are not counted as intersecting:
///
/// ```
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let a = Aabb3::from_points ([ 0.0, 0.0, 0.0].into(), [1.0, 1.0, 1.0].into());
/// let b = Aabb3::from_points ([-1.0, 0.0, 0.0].into(), [0.0, 1.0, 1.0].into());
/// assert!(!discrete_aabb3_aabb3 (&a, &b));
/// ```
#[inline]
pub fn discrete_aabb3_aabb3 <S : Ring> (a : &Aabb3 <S>, b : &Aabb3 <S>)
  -> bool
{
  let (min_a, max_a) = (a.min(), a.max());
  let (min_b, max_b) = (b.min(), b.max());
  // intersection if overlap exists on all three axes
  max_a.0.x > min_b.0.x && min_a.0.x < max_b.0.x &&
  max_a.0.y > min_b.0.y && min_a.0.y < max_b.0.y &&
  max_a.0.z > min_b.0.z && min_a.0.z < max_b.0.z
}

/// Continuous intersection test of 3D axis-aligned bounding boxes.
///
/// AABBs that are merely touching return no intersection:
///
/// ```
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let a = Aabb3::from_points ([ 0.0, 0.0, 0.0].into(), [1.0, 1.0, 1.0].into());
/// let b = Aabb3::from_points ([-1.0, 0.0, 0.0].into(), [0.0, 1.0, 1.0].into());
/// assert!(continuous_aabb3_aabb3 (&a, &b).is_none());
/// ```
#[inline]
pub fn continuous_aabb3_aabb3 <S> (a : &Aabb3 <S>, b : &Aabb3 <S>)
  -> Option <Aabb3 <S>>
where
  S : Ring + std::fmt::Debug
{
  if discrete_aabb3_aabb3 (a, b) {
    Some (Aabb3::with_minmax (
      point3_max (a.min(), b.min()),
      point3_min (a.max(), b.max())
    ))
  } else {
    None
  }
}

/// Compute the intersection of a 2D line with a 2D AABB (rectangle).
///
/// If the line and AABB intersect, the two intersection points are returned
/// with the scalar parameter corresponding to that point along the line.
///
/// ```
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let aabb = Aabb2::with_minmax ([-1.0, -1.0].into(), [1.0, 1.0].into());
/// let line = Line2::new  ([0.0, 0.0].into(), Unit2::axis_x());
/// assert_eq!(
///   continuous_line2_aabb2 (&line, &aabb).unwrap(),
///   ( (-1.0, [-1.0, 0.0].into()),
///     ( 1.0, [ 1.0, 0.0].into())
///   )
/// );
/// ```
///
/// Returns `None` if the line and AABB are tangent:
///
/// ```
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let aabb = Aabb2::with_minmax ([-1.0, -1.0].into(), [1.0, 1.0].into());
/// let line = Line2::new  ([0.0, 1.0].into(), Unit2::axis_x());
/// assert_eq!(continuous_line2_aabb2 (&line, &aabb), None);
/// ```
#[allow(clippy::type_complexity)]
pub fn continuous_line2_aabb2 <S> (line : &Line2 <S>, aabb : &Aabb2 <S>)
  -> Option <((S, Point2 <S>), (S, Point2 <S>))>
where
  S : Real + std::fmt::Debug
{
  let aabb_min = aabb.min();
  let aabb_max = aabb.max();
  if line.direction.x == S::zero() {
    // parallel x-axis
    if aabb_min.0.x < line.base.0.x && line.base.0.x < aabb_max.0.x {
      let out = if line.direction.y > S::zero() {
        let (t0, t1) = (aabb_min.0.y - line.base.0.y, aabb_max.0.y - line.base.0.y);
        ( (t0, [line.base.0.x, aabb_min.0.y].into()),
          (t1, [line.base.0.x, aabb_max.0.y].into())
        )
      } else {
        let (t0, t1) = (line.base.0.y - aabb_max.0.y, line.base.0.y - aabb_min.0.y);
        ( (t0, [line.base.0.x, aabb_max.0.y].into()),
          (t1, [line.base.0.x, aabb_min.0.y].into())
        )
      };
      Some (out)
    } else {
      None
    }
  } else if line.direction.y == S::zero() {
    // parallel y-axis
    if aabb_min.0.y < line.base.0.y && line.base.0.y < aabb_max.0.y {
      let out = if line.direction.x > S::zero() {
        let (t0, t1) = (aabb_min.0.x - line.base.0.x, aabb_max.0.x - line.base.0.x);
        ( (t0, [aabb_min.0.x, line.base.0.y].into()),
          (t1, [aabb_max.0.x, line.base.0.y].into())
        )
      } else {
        let (t0, t1) = (line.base.0.x - aabb_max.0.x, line.base.0.x - aabb_min.0.x);
        ( (t0, [aabb_max.0.x, line.base.0.y].into()),
          (t1, [aabb_min.0.x, line.base.0.y].into())
        )
      };
      Some (out)
    } else {
      None
    }
  } else {
    let dir_reciprocal = line.direction.map (|s| S::one() / s);
    let (t0_x, t1_x)   = {
      let (near_x, far_x) = if line.direction.x.is_positive() {
        (aabb_min.0.x, aabb_max.0.x)
      } else {
        (aabb_max.0.x, aabb_min.0.x)
      };
      ( (near_x - line.base.0.x) * dir_reciprocal.x,
        (far_x  - line.base.0.x) * dir_reciprocal.x
      )
    };
    let (t0_y, t1_y) = {
      let (near_y, far_y) = if line.direction.y.is_positive() {
        (aabb_min.0.y, aabb_max.0.y)
      } else {
        (aabb_max.0.y, aabb_min.0.y)
      };
      ( (near_y - line.base.0.y) * dir_reciprocal.y,
        (far_y  - line.base.0.y) * dir_reciprocal.y
      )
    };
    let interval_x = Interval::with_minmax (t0_x, t1_x);
    let interval_y = Interval::with_minmax (t0_y, t1_y);
    continuous_interval (&interval_x, &interval_y).map (|interval|{
      let start = line.point (interval.min());
      let end   = line.point (interval.max());
      ( (interval.min(), start), (interval.max(), end) )
    })
  }
}

/// Compute the intersection of a 2D line with a 2D sphere (circle).
///
/// If the line and circle intersect, the two intersection points are returned
/// with the scalar parameter corresponding to that point along the line.
///
/// ```
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let sphere = Sphere2::unit();
/// let line   = Line2::new  ([0.0, 0.0].into(), Unit2::axis_x());
/// assert_eq!(
///   continuous_line2_sphere2 (&line, &sphere).unwrap(),
///   ( (-1.0, [-1.0, 0.0].into()),
///     ( 1.0, [ 1.0, 0.0].into())
///   )
/// );
/// ```
///
/// Returns `None` if the line and circle are tangent:
///
/// ```
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let sphere = Sphere2::unit();
/// let line   = Line2::new  ([0.0, 1.0].into(), Unit2::axis_x());
/// assert_eq!(continuous_line2_sphere2 (&line, &sphere), None);
/// ```
#[allow(clippy::type_complexity)]
pub fn continuous_line2_sphere2 <S : Real> (line : &Line2 <S>, sphere : &Sphere2 <S>)
  -> Option <((S, Point2 <S>), (S, Point2 <S>))>
{
  // intersect the line with the cylinder circle in the XY plane
  let two  = S::two();
  let four = S::four();
  // defining intersection points by the equation at^2 + bt + c = 0,
  // solve for t using quadratic formula:
  //
  //  t = (-b +- sqrt (b^2 -4ac)) / 2a
  //
  // where a, b, and c are defined in terms of points p1, p2 of the line
  // segment and the sphere center p3, and sphere radius r
  let p1   = line.base;
  let _p2   = line.base + *line.direction;
  let p3   = sphere.center;
  let r    = *sphere.radius;
  let p1p2 = &line.direction;
  let p3p1 = p1 - p3;
  let a    = S::one();
  let b    = two * p1p2.dot (p3p1);
  let c    = p3p1.norm_squared() - r * r;
  // this is the portion of the quadratic equation inside the square root
  // that determines whether the intersection is none, a tangent point, or
  // a segment
  let discriminant = b * b - four * a * c;
  if discriminant <= S::zero() {
    None
  } else {
    let discriminant_sqrt = discriminant.sqrt();
    let frac_2a           = S::one() / (two * a);
    let t1                = (-b - discriminant_sqrt) * frac_2a;
    let t2                = (-b + discriminant_sqrt) * frac_2a;
    let first  = p1 + (**p1p2) * t1;
    let second = p1 + (**p1p2) * t2;
    Some (((t1, first), (t2, second)))
  }
}

/// Compute the continuous intersection of a 3D line with a 3D plane.
///
/// Returns `None` if the line and plane are parallel:
///
/// ```
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let plane = Plane3::new ([0.0, 0.0,  0.0].into(), Unit3::axis_z());
/// let line  = Line3::new  ([0.0, 0.0,  0.0].into(),
///   Unit3::normalize ([1.0, 1.0, 0.0].into()));
/// assert_eq!(continuous_line3_plane3 (&line, &plane), None);
/// ```
pub fn continuous_line3_plane3 <S> (line : &Line3 <S>, plane : &Plane3 <S>)
  -> Option <(S, Point3 <S>)>
where
  S : Real + approx::RelativeEq
{
  let normal_dot_direction = plane.normal.dot (*line.direction);
  if approx::relative_eq!(normal_dot_direction, S::zero()) {
    None
  } else {
    let plane_to_line = line.base - plane.base;
    let t     = -plane.normal.dot (plane_to_line) / normal_dot_direction;
    let point = line.point (t);
    Some ((t, point))
  }
}

/// Compute the intersection of a 3D line with a 3D AABB.
///
/// If the line and AABB intersect, the two intersection points are returned
/// with the scalar parameter corresponding to that point along the line.
///
/// ```
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let aabb = Aabb3::with_minmax (
///   [-1.0, -1.0, -1.0].into(), [1.0, 1.0, 1.0].into());
/// let line = Line3::new  ([0.0, 0.0, 0.0].into(), Unit3::axis_x());
/// assert_eq!(
///   continuous_line3_aabb3 (&line, &aabb).unwrap(),
///   ( (-1.0, [-1.0, 0.0, 0.0].into()),
///     ( 1.0, [ 1.0, 0.0, 0.0].into())
///   )
/// );
/// ```
///
/// Returns `None` if the line and AABB are tangent:
///
/// ```
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let aabb = Aabb3::with_minmax (
///   [-1.0, -1.0, -1.0].into(), [1.0, 1.0, 1.0].into());
/// let line = Line3::new  ([0.0, 1.0, 0.0].into(), Unit3::axis_x());
/// assert_eq!(continuous_line3_aabb3 (&line, &aabb), None);
/// ```
#[allow(clippy::type_complexity)]
pub fn continuous_line3_aabb3 <S> (line : &Line3 <S>, aabb : &Aabb3 <S>)
  -> Option <((S, Point3 <S>), (S, Point3 <S>))>
where
  S : Real + num_traits::Float + approx::RelativeEq <Epsilon=S> + std::fmt::Debug
{
  let aabb_min = aabb.min();
  let aabb_max = aabb.max();
  if line.direction.x == S::zero() {
    if aabb_min.0.x < line.base.0.x && line.base.0.x < aabb_max.0.x {
      let line2 = Line2::new ([line.base.0.y, line.base.0.z].into(),
        Unit2::unchecked_approx ([line.direction.y, line.direction.z].into()));
      let aabb2 = Aabb2::with_minmax (
        [aabb_min.0.y, aabb_min.0.z].into(),
        [aabb_max.0.y, aabb_max.0.z].into());
      continuous_line2_aabb2 (&line2, &aabb2).map (|((t0, p0), (t1, p1))|
        ( (t0, [line.base.0.x, p0.0.x, p0.0.y].into()),
          (t1, [line.base.0.x, p1.0.x, p1.0.y].into())
        )
      )
    } else {
      None
    }
  } else if line.direction.y == S::zero() {
    if aabb_min.0.y < line.base.0.y && line.base.0.y < aabb_max.0.y {
      let line2 = Line2::new ([line.base.0.x, line.base.0.z].into(),
        Unit2::unchecked_approx ([line.direction.x, line.direction.z].into()));
      let aabb2 = Aabb2::with_minmax (
        [aabb_min.0.x, aabb_min.0.z].into(),
        [aabb_max.0.x, aabb_max.0.z].into());
      continuous_line2_aabb2 (&line2, &aabb2).map (|((t0, p0), (t1, p1))|
        ( (t0, [p0.0.x, line.base.0.y, p0.0.y].into()),
          (t1, [p1.0.x, line.base.0.y, p1.0.y].into())
        )
      )
    } else {
      None
    }
  } else if line.direction.z == S::zero() {
    if aabb_min.0.z < line.base.0.z && line.base.0.z < aabb_max.0.z {
      let line2 = Line2::new ([line.base.0.x, line.base.0.y].into(),
        Unit2::unchecked_approx ([line.direction.x, line.direction.y].into()));
      let aabb2 = Aabb2::with_minmax (
        [aabb_min.0.x, aabb_min.0.y].into(),
        [aabb_max.0.x, aabb_max.0.y].into());
      continuous_line2_aabb2 (&line2, &aabb2).map (|((t0, p0), (t1, p1))|
        ( (t0, [p0.0.x, p0.0.y, line.base.0.z].into()),
          (t1, [p1.0.x, p1.0.y, line.base.0.z].into())
        )
      )
    } else {
      None
    }
  } else {
    let dir_reciprocal = line.direction.map (|s| S::one() / s);
    let (t0_x, t1_x)   = {
      let (near_x, far_x) = if line.direction.x.is_positive() {
        (aabb_min.0.x, aabb_max.0.x)
      } else {
        (aabb_max.0.x, aabb_min.0.x)
      };
      ( (near_x - line.base.0.x) * dir_reciprocal.x,
        (far_x  - line.base.0.x) * dir_reciprocal.x
      )
    };
    let (t0_y, t1_y) = {
      let (near_y, far_y) = if line.direction.y.is_positive() {
        (aabb_min.0.y, aabb_max.0.y)
      } else {
        (aabb_max.0.y, aabb_min.0.y)
      };
      ( (near_y - line.base.0.y) * dir_reciprocal.y,
        (far_y  - line.base.0.y) * dir_reciprocal.y
      )
    };
    let (t0_z, t1_z) = {
      let (near_z, far_z) = if line.direction.z.is_positive() {
        (aabb_min.0.z, aabb_max.0.z)
      } else {
        (aabb_max.0.z, aabb_min.0.z)
      };
      ( (near_z - line.base.0.z) * dir_reciprocal.z,
        (far_z  - line.base.0.z) * dir_reciprocal.z
      )
    };
    let interval_x = Interval::with_minmax (t0_x, t1_x);
    let interval_y = Interval::with_minmax (t0_y, t1_y);
    continuous_interval (&interval_x, &interval_y).and_then (|interval| {
      let interval_z = Interval::with_minmax (t0_z, t1_z);
      continuous_interval (&interval, &interval_z).map (|interval|{
        let start = line.point (interval.min());
        let end   = line.point (interval.max());
        ( (interval.min(), start), (interval.max(), end) )
      })
    })
  }
}

/// Compute the continuous intersection of a 3D line with a 3D sphere.
///
/// If the line and sphere intersect, the two intersection points are returned
/// with the scalar parameter corresponding to that point along the line.
///
/// ```
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let sphere = Sphere3::unit();
/// let line   = Line3::new  ([0.0, 0.0, 0.0].into(), Unit3::axis_x());
/// assert_eq!(
///   continuous_line3_sphere3 (&line, &sphere).unwrap(),
///   ( (-1.0, [-1.0, 0.0, 0.0].into()),
///     ( 1.0, [ 1.0, 0.0, 0.0].into())
///   )
/// );
/// ```
///
/// Returns `None` if the line and sphere are tangent:
///
/// ```
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let sphere = Sphere3::unit();
/// let line   = Line3::new  ([0.0, 0.0, 1.0].into(), Unit3::axis_x());
/// assert_eq!(continuous_line3_sphere3 (&line, &sphere), None);
/// ```
#[allow(clippy::type_complexity)]
pub fn continuous_line3_sphere3 <S : Real> (line : &Line3 <S>, sphere : &Sphere3 <S>)
  -> Option <((S, Point3 <S>), (S, Point3 <S>))>
{
  let two  = S::two();
  let four = S::four();
  // defining intersection points by the equation at^2 + bt + c = 0,
  // solve for t using quadratic formula:
  //
  //  t = (-b +- sqrt (b^2 -4ac)) / 2a
  //
  // where a, b, and c are defined in terms of points p1, p2 of the line
  // segment and the sphere center p3, and sphere radius r
  let p1   = line.base;
  let _p2   = line.base + *line.direction;
  let p3   = sphere.center;
  let r    = *sphere.radius;
  let p1p2 = &line.direction;
  let p3p1 = p1 - p3;
  let a    = S::one();
  let b    = two * p1p2.dot (p3p1);
  let c    = p3p1.norm_squared() - r * r;
  // this is the portion of the quadratic equation inside the square root
  // that determines whether the intersection is none, a tangent point, or
  // a segment
  let discriminant = b * b - four * a * c;
  if discriminant <= S::zero() {
    None
  } else {
    let discriminant_sqrt = discriminant.sqrt();
    let frac_2a           = S::one() / (two * a);
    let t1                = (-b - discriminant_sqrt) * frac_2a;
    let t2                = (-b + discriminant_sqrt) * frac_2a;
    let first  = p1 + (**p1p2) * t1;
    let second = p1 + (**p1p2) * t2;
    Some (((t1, first), (t2, second)))
  }
}

/// Compute continuous intersection of a 2D line segment with a 2D AABB
/// (rectangle).
///
/// The points are returned in the order given by the ordering of the points
/// in the segment.
///
/// Note that a segment that is tangent to the AABB returns no intersection.
#[allow(clippy::type_complexity)]
pub fn continuous_segment2_aabb2 <S> (segment : &Segment2 <S>, aabb : &Aabb2 <S>)
  -> Option <((S, Point2 <S>), (S, Point2 <S>))>
where S : Real + std::fmt::Debug {
  let vector = *segment.point_b() - segment.point_a();
  let length = vector.norm();
  let line   = Line2::new (
    *segment.point_a(), Unit2::unchecked (vector / length));
  if let Some (((t0, _p0), (t1, _p1))) = continuous_line2_aabb2 (&line, aabb) {
    let interval = Interval::with_minmax (S::zero(), length);
    continuous_interval (&interval, &Interval::with_minmax (t0, t1)).map (
      |interval|
      ( (interval.min() / length, line.point (interval.min())),
        (interval.max() / length, line.point (interval.max()))
      )
    )
  } else {
    None
  }
}

/// Compute continuous intersection of a 2D line segment with a 2D sphere
/// (circle).
///
/// The points are returned in the order given by the ordering of the points
/// in the segment.
///
/// Note that a segment that is tangent to the surface of the circle returns no
/// intersection.
#[allow(clippy::type_complexity)]
pub fn continuous_segment2_sphere2 <S> (segment : &Segment2 <S>, sphere : &Sphere2 <S>)
  -> Option <((S, Point2 <S>), (S, Point2<S>))>
where S : Real + std::fmt::Debug {
  let vector = *segment.point_b() - segment.point_a();
  let length = vector.norm();
  let line   = Line2::new (
    *segment.point_a(), Unit2::unchecked (vector / length));
  if let Some (((t0, _p0), (t1, _p1))) = continuous_line2_sphere2 (&line, sphere) {
    let interval = Interval::with_minmax (S::zero(), length);
    continuous_interval (&interval, &Interval::with_minmax (t0, t1)).map (
      |interval|
      ( (interval.min() / length, line.point (interval.min())),
        (interval.max() / length, line.point (interval.max()))
      )
    )
  } else {
    None
  }
}

/// Compute continuous intersection of a 3D line segment with an AABB.
///
/// The points are returned in the order given by the ordering of the points
/// in the segment.
///
/// Note that a segment that is tangent to the AABB returns no intersection.
#[allow(clippy::type_complexity)]
pub fn continuous_segment3_aabb3 <S> (segment : &Segment3 <S>, aabb : &Aabb3 <S>)
  -> Option <((S, Point3 <S>), (S, Point3<S>))>
where S : Real + num_traits::Float + approx::RelativeEq <Epsilon=S> + std::fmt::Debug {
  let vector = *segment.point_b() - segment.point_a();
  let length = vector.norm();
  let line   = Line3::new (
    *segment.point_a(), Unit3::unchecked_approx (vector / length));
  if let Some (((t0, _p0), (t1, _p1))) = continuous_line3_aabb3 (&line, aabb) {
    let interval = Interval::with_minmax (S::zero(), length);
    continuous_interval (&interval, &Interval::with_minmax (t0, t1)).map (
      |interval|
      ( (interval.min() / length, line.point (interval.min())),
        (interval.max() / length, line.point (interval.max()))
      )
    )
  } else {
    None
  }
}

/// Compute continuous intersection of a 3D line segment with a sphere.
///
/// The points are returned in the order given by the ordering of the points
/// in the segment.
///
/// Note that a segment that is tangent to the surface of the sphere returns no
/// intersection.
#[allow(clippy::type_complexity)]
pub fn continuous_segment3_sphere3 <S> (segment : &Segment3 <S>, sphere : &Sphere3 <S>)
  -> Option <((S, Point3 <S>), (S, Point3<S>))>
where S : Field + Sqrt {
  let two  = S::two();
  let four = S::four();
  // defining intersection points by the equation at^2 + bt + c = 0,
  // solve for t using quadratic formula:
  //
  //  t = (-b +- sqrt (b^2 -4ac)) / 2a
  //
  // where a, b, and c are defined in terms of points p1, p2 of the line
  // segment and the sphere center p3, and sphere radius r
  let p1   = segment.point_a();
  let p2   = segment.point_b();
  let p3   = sphere.center;
  let r    = *sphere.radius;
  let p1p2 = *p2-p1;
  let p3p1 = *p1-p3;
  let a    = p1p2.norm_squared();
  let b    = two * p1p2.dot (p3p1);
  let c    = p3p1.norm_squared() - r * r;
  // this is the portion of the quadratic equation inside the square root
  // that determines whether the intersection is none, a tangent point, or
  // a segment
  let discriminant = b * b - four * a * c;
  if discriminant <= S::zero() {
    None
  } else {
    let discriminant_sqrt = discriminant.sqrt();
    let frac_2a        = S::one() / (two * a);
    let t1             = S::max (
      (-b - discriminant_sqrt) * frac_2a,
      S::zero());
    let t2             = S::min (
      (-b + discriminant_sqrt) * frac_2a,
      S::one());
    if t2 <= S::zero() || S::one() <= t1 {
      None
    } else {
      let first  = *p1 + p1p2 * t1;
      let second = *p1 + p1p2 * t2;
      Some (((t1, first), (t2, second)))
    }
  }
}

/// Compute continuous intersection of a 3D line segment with an axis-aligned
/// cylinder.
///
/// The points are returned in the order given by the ordering of the points
/// in the segment.
///
/// Note that a segment that is tangent to the surface of the cylinder returns
/// no intersection.
#[allow(clippy::type_complexity)]
pub fn continuous_segment3_cylinder3 <S> (
  segment : &Segment3 <S>, cylinder : &Cylinder3 <S>
) -> Option <((S, Point3 <S>), (S, Point3<S>))> where
  S : Real + std::fmt::Debug
{
  let segment_aabb  = segment.aabb3();
  let cylinder_aabb = cylinder.aabb3();
  if !discrete_aabb3_aabb3 (&segment_aabb, &cylinder_aabb) {
    None
  } else {
    let p1       = segment.point_a();
    let p2       = segment.point_b();
    let p3       = cylinder.center;
    let r        = *cylinder.radius;
    let r2       = r * r;
    let p1p2     = *p2 - p1;
    let p1_xy    = Point2::from ([p1.0.x, p1.0.y]);
    let p2_xy    = Point2::from ([p2.0.x, p2.0.y]);
    let p3_xy    = Point2::from ([p3.0.x, p3.0.y]);
    let p3_z_max = cylinder_aabb.max().0.z;
    let p3_z_min = cylinder_aabb.min().0.z;
    if p1_xy == p2_xy {   // segment is aligned vertically (Z axis)
      let d2 = (p1_xy - p3_xy).norm_squared();
      if d2 >= r2 {
        None
      } else {
        let (t1, begin_z) = if p1.0.z >= p3_z_max {
          ((p3_z_max - p1.0.z) / p1p2.z, p3_z_max)
        } else if p1.0.z <= p3_z_min {
          ((p3_z_min - p1.0.z) / p1p2.z, p3_z_min)
        } else {
          (S::zero(), p1.0.z)
        };
        let (t2, end_z)   = if p2.0.z >= p3_z_max {
          ((p3_z_max - p1.0.z) / p1p2.z, p3_z_max)
        } else if p2.0.z <= p3_z_min {
          ((p3_z_min - p1.0.z) / p1p2.z, p3_z_min)
        } else {
          (S::one(), p2.0.z)
        };
        let begin = [p1_xy.0.x, p1_xy.0.y, begin_z].into();
        let end   = [p1_xy.0.x, p1_xy.0.y, end_z  ].into();
        Some (((t1, begin), (t2, end)))
      }
    } else {    // segment is not aligned vertically
      // intersect the line with the cylinder circle in the XY plane
      let two     = S::two();
      let four    = S::four();
      let p1p2_xy = p1p2.xy();
      let p3p1_xy = p1_xy - p3_xy;
      let a       = p1p2_xy.norm_squared();
      let b       = two * p1p2_xy.dot (p3p1_xy);
      let c       = p3p1_xy.norm_squared() - r * r;
      // this is the portion of the quadratic equation inside the square root
      // that determines whether the intersection is none, a tangent point, or
      // a segment
      let discriminant = b * b - four * a * c;
      if discriminant <= S::zero() {
        None
      } else {
        let discriminant_sqrt = discriminant.sqrt();
        let frac_2a           = S::one() / (two * a);
        let t1_xy             = S::max (
          (-b - discriminant_sqrt) * frac_2a,
          S::zero());
        let t2_xy             = S::min (
          (-b + discriminant_sqrt) * frac_2a,
          S::one());
        if t2_xy <= S::zero() || S::one() <= t1_xy {
          None
        } else if let Some ((t1, t2)) = if p1.0.z == p2.0.z { // segment aligned horizontally
          Some ((t1_xy, t2_xy))
        } else {                         // segment not aligned horizontally
          // intersect the line with the top and bottom of the cylinder
          let p1p3_z_max = p3_z_max - p1.0.z;
          let p1p3_z_min = p3_z_min - p1.0.z;
          let t_z_max    = S::max (
            S::min (p1p3_z_max / p1p2.z, S::one()),
            S::zero());
          let t_z_min    = S::max (
            S::min (p1p3_z_min / p1p2.z, S::one()),
            S::zero());
          let t1_z       = S::min (t_z_max, t_z_min);
          let t2_z       = S::max (t_z_max, t_z_min);
          let aabb_xy    = Interval::with_minmax (t1_xy, t2_xy);
          let aabb_z     = Interval::with_minmax ( t1_z,  t2_z);
          if !aabb_xy.intersects (&aabb_z) {
            None
          } else {
            Some ((S::max (t1_xy, t1_z), S::min (t2_xy, t2_z)))
          }
        } /*then*/ {
          debug_assert!(t1 < t2);
          debug_assert!(t1 >= S::zero());
          debug_assert!(t1 <  S::one());
          debug_assert!(t2 >  S::zero());
          debug_assert!(t2 <= S::one());
          let first  = *p1 + p1p2 * t1;
          let second = *p1 + p1p2 * t2;
          Some (((t1, first), (t2, second)))
        } else {
          None
        }
      }
    }
  }
}

/// Compute continuous intersection of a line segment with an axis-aligned
/// capsule.
///
/// The points are returned in the order given by the ordering of the points
/// in the segment.
///
/// Note that a line that is tangent to the surface of the capsule returns no
/// intersection.
#[allow(clippy::type_complexity)]
pub fn continuous_segment3_capsule3 <S> (
  segment : &Segment3 <S>, capsule : &Capsule3 <S>
) -> Option <((S, Point3 <S>), (S, Point3 <S>))> where
  S : Real + std::fmt::Debug
{
  let segment_aabb = segment.aabb3();
  let capsule_aabb = capsule.aabb3();
  if !discrete_aabb3_aabb3 (&segment_aabb, &capsule_aabb) {
    None
  } else {
    // decompose the capsule into spheres and cylinder
    let (upper_sphere, cylinder, lower_sphere) = capsule.decompose();
    let cylinder_result = cylinder.and_then (
      |cylinder| segment.intersect_cylinder (&cylinder));
    let upper_result    = segment.intersect_sphere   (&upper_sphere);
    let lower_result    = segment.intersect_sphere   (&lower_sphere);
    match (upper_result, cylinder_result, lower_result) {
      (None, None, None) => None,
      (one,  None, None) |
      (None,  one, None) |
      (None, None,  one) => one,
      (Some (((t1,p1), (t2,p2))), Some (((u1,q1), (u2,q2))), None) |
      (Some (((t1,p1), (t2,p2))), None, Some (((u1,q1), (u2,q2)))) |
      (None, Some (((t1,p1), (t2,p2))), Some (((u1,q1), (u2,q2)))) => {
        let first = if t1 < u1 {
          (t1,p1)
        } else {
          (u1,q1)
        };
        let second = if t2 > u2 {
          (t2,p2)
        } else {
          (u2,q2)
        };
        Some ((first, second))
      }
      ( Some (((t1,p1), (t2,p2))),
        Some (((u1,q1), (u2,q2))),
        Some (((v1,r1), (v2,r2)))
      ) => {
        let min1 = S::min (S::min (t1, u1), v1);
        let max2 = S::max (S::max (t2, u2), v2);
        let first = if min1 == t1 {
          (t1,p1)
        } else if min1 == u1 {
          (u1,q1)
        } else {
          debug_assert_eq!(min1, v1);
          (v1,r1)
        };
        let second = if max2 == t2 {
          (t2,p2)
        } else if max2 == u2 {
          (u2,q2)
        } else {
          debug_assert_eq!(max2, v2);
          (v2,r2)
        };
        Some ((first, second))
      }
    }
  }
}

/// Discrete intersection test of 2D spheres.
///
/// Spheres that are merely touching are not counted as intersecting:
///
/// ```
/// # use math_utils::num_traits::One;
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let r = Positive::one();
/// let a = Sphere2 { center: [ 1.0, 0.0].into(), radius: r };
/// let b = Sphere2 { center: [-1.0, 0.0].into(), radius: r };
/// assert!(!discrete_sphere2_sphere2 (&a, &b));
/// ```
#[inline]
pub fn discrete_sphere2_sphere2 <S : Ring> (
  a : &Sphere2 <S>, b : &Sphere2 <S>
) -> bool {
  let r_ab = *(a.radius + b.radius);
  (b.center - a.center).self_dot() < r_ab * r_ab
}

/// Discrete intersection test of 3D spheres.
///
/// Spheres that are merely touching are not counted as intersecting:
///
/// ```
/// # use math_utils::num_traits::One;
/// # use math_utils::*;
/// # use math_utils::geometry::*;
/// # use math_utils::geometry::intersect::*;
/// let r = Positive::one();
/// let a = Sphere3 { center: [ 1.0, 0.0,  0.0].into(), radius: r };
/// let b = Sphere3 { center: [-1.0, 0.0,  0.0].into(), radius: r };
/// assert!(!discrete_sphere3_sphere3 (&a, &b));
/// ```
#[inline]
pub fn discrete_sphere3_sphere3 <S : Ring> (
  a : &Sphere3 <S>, b : &Sphere3 <S>
) -> bool {
  let r_ab = *(a.radius + b.radius);
  (b.center - a.center).self_dot() < r_ab * r_ab
}

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

  #[test]
  fn test_line2_aabb2() {
    use std::f64::consts::SQRT_2;
    let aabb = Aabb2::with_minmax ([-1.0, -1.0].into(), [1.0, 1.0].into());
    let line = Line2::new ([ 0.0, 0.0].into(), Unit2::axis_y());
    assert_eq!(
      continuous_line2_aabb2 (&line, &aabb).unwrap(),
      ((-1.0, [ 0.0, -1.0].into()), (1.0, [ 0.0, 1.0].into())));
    let line = Line2::new ([ 0.0, 0.0].into(), Unit2::axis_x());
    assert_eq!(
      continuous_line2_aabb2 (&line, &aabb).unwrap(),
      ((-1.0, [-1.0,  0.0].into()), (1.0, [ 1.0, 0.0].into())));
    let line = Line2::new ([ 0.0, 0.0].into(),
      Unit2::normalize ([1.0, 1.0].into()));
    assert_eq!(
      continuous_line2_aabb2 (&line, &aabb).unwrap(),
      ((-SQRT_2, [-1.0, -1.0].into()), (SQRT_2, [ 1.0,  1.0].into())));
    let line = Line2::new ([ 0.0, 0.0].into(),
      Unit2::normalize ([-1.0, -1.0].into()));
    assert_eq!(
      continuous_line2_aabb2 (&line, &aabb).unwrap(),
      ((-SQRT_2, [ 1.0,  1.0].into()), (SQRT_2, [-1.0, -1.0].into())));
    let line = Line2::new ([ 0.0, 3.0].into(),
      Unit2::normalize ([-1.0, -1.0].into()));
    assert!(continuous_line2_aabb2 (&line, &aabb).is_none());
    let line = Line2::new ([ 0.0, -3.0].into(),
      Unit2::normalize ([ 1.0,  1.0].into()));
    assert!(continuous_line2_aabb2 (&line, &aabb).is_none());
  }

  #[test]
  fn test_line3_aabb3() {
    use approx::assert_ulps_eq;
    let aabb = Aabb3::with_minmax (
      [-1.0, -1.0, -1.0].into(), [1.0, 1.0, 1.0].into());
    let line = Line3::new ([ 0.0, 0.0, 0.0].into(), Unit3::axis_z());
    assert_eq!(
      continuous_line3_aabb3 (&line, &aabb).unwrap(),
      ((-1.0, [ 0.0, 0.0, -1.0].into()), (1.0, [ 0.0, 0.0, 1.0].into())));
    let line = Line3::new ([ 0.0, 0.0, 0.0].into(), Unit3::axis_y());
    assert_eq!(
      continuous_line3_aabb3 (&line, &aabb).unwrap(),
      ((-1.0, [0.0, -1.0,  0.0].into()), (1.0, [ 0.0, 1.0, 0.0].into())));
    {
      let line = Line3::new ([ 0.0, 0.0, 0.0].into(),
        Unit3::normalize ([1.0, 1.0, 1.0].into()));
      let result = continuous_line3_aabb3 (&line, &aabb).unwrap();
      assert_ulps_eq!((result.0).0, -f64::sqrt_3());
      assert_ulps_eq!((result.1).0, f64::sqrt_3());
      assert_eq!((result.0).1, [-1.0, -1.0, -1.0].into());
      assert_eq!((result.1).1, [ 1.0,  1.0,  1.0].into());
    }
    {
      let line = Line3::new ([ 0.0, 0.0, 0.0].into(),
        Unit3::normalize ([-1.0, -1.0, -1.0].into()));
      let result = continuous_line3_aabb3 (&line, &aabb).unwrap();
      assert_ulps_eq!((result.0).0, -f64::sqrt_3());
      assert_ulps_eq!((result.1).0, f64::sqrt_3());
      assert_eq!((result.0).1, [ 1.0,  1.0,  1.0].into());
      assert_eq!((result.1).1, [-1.0, -1.0, -1.0].into());
    }
    let line = Line3::new ([ 0.0, 0.0, 3.0].into(),
      Unit3::normalize ([-1.0, -1.0, -1.0].into()));
    assert!(continuous_line3_aabb3 (&line, &aabb).is_none());
    let line = Line3::new ([0.0, 0.0, -3.0].into(),
      Unit3::normalize ([1.0, 1.0, 1.0].into()));
    assert!(continuous_line3_aabb3 (&line, &aabb).is_none());
  }

  #[test]
  fn test_segment3_sphere3() {
    let sphere  = shape::Sphere::unit().sphere3 (Point3::origin());
    let segment = Segment3::new (
      [-2.0, 0.0, 0.0].into(), [ 2.0, 0.0, 0.0].into());
    assert_eq!(
      continuous_segment3_sphere3 (&segment, &sphere).unwrap(),
      ((0.25, [-1.0, 0.0, 0.0].into()), (0.75, [ 1.0, 0.0, 0.0].into())));
    let segment = Segment3::new (
      [ 2.0, 0.0, 0.0].into(), [-2.0, 0.0, 0.0].into());
    assert_eq!(
      continuous_segment3_sphere3 (&segment, &sphere).unwrap(),
      ((0.25, [ 1.0, 0.0, 0.0].into()), (0.75, [-1.0, 0.0, 0.0].into())));
    let segment = Segment3::new (
      [ 0.0, 0.0, 0.0].into(), [ 0.0, 0.0, 2.0].into());
    assert_eq!(
      continuous_segment3_sphere3 (&segment, &sphere).unwrap(),
      ((0.0, [ 0.0, 0.0, 0.0].into()), (0.5, [ 0.0, 0.0, 1.0].into())));
    let segment = Segment3::new (
      [ 0.0, 0.0, -2.0].into(), [ 0.0, 0.0, 0.0].into());
    assert_eq!(
      continuous_segment3_sphere3 (&segment, &sphere).unwrap(),
      ((0.5, [ 0.0, 0.0, -1.0].into()), (1.0, [ 0.0, 0.0, 0.0].into())));
  }

  #[test]
  fn test_segment3_cylinder3() {
    let cylinder = shape::Cylinder::unit().cylinder3 (Point3::origin());
    let segment = Segment3::new (
      [-2.0, 0.0, 0.0].into(), [ 2.0, 0.0, 0.0].into());
    assert_eq!(
      continuous_segment3_cylinder3 (&segment, &cylinder).unwrap(),
      ((0.25, [-1.0, 0.0, 0.0].into()), (0.75, [ 1.0, 0.0, 0.0].into())));
    let segment = Segment3::new (
      [ 2.0, 0.0, 0.0].into(), [-2.0, 0.0, 0.0].into());
    assert_eq!(
      continuous_segment3_cylinder3 (&segment, &cylinder).unwrap(),
      ((0.25, [ 1.0, 0.0, 0.0].into()), (0.75, [-1.0, 0.0, 0.0].into())));
    let segment = Segment3::new (
      [ 0.0, 0.0, 0.0].into(), [ 0.0, 0.0, 2.0].into());
    assert_eq!(
      continuous_segment3_cylinder3 (&segment, &cylinder).unwrap(),
      ((0.0, [ 0.0, 0.0, 0.0].into()), (0.5, [ 0.0, 0.0, 1.0].into())));
    let segment = Segment3::new (
      [ 0.0, 0.0, -2.0].into(), [ 0.0, 0.0, 0.0].into());
    assert_eq!(
      continuous_segment3_cylinder3 (&segment, &cylinder).unwrap(),
      ((0.5, [ 0.0, 0.0, -1.0].into()), (1.0, [ 0.0, 0.0, 0.0].into())));
  }

  #[test]
  fn test_segment3_capsule3() {
    let capsule = shape::Capsule::noisy (1.0, 1.0)
      .capsule3 (Point3::origin());
    let segment = Segment3::new (
      [-2.0, 0.0, 0.0].into(), [ 2.0, 0.0, 0.0].into());
    assert_eq!(
      continuous_segment3_capsule3 (&segment, &capsule).unwrap(),
      ((0.25, [-1.0, 0.0, 0.0].into()), (0.75, [ 1.0, 0.0, 0.0].into())));
    let segment = Segment3::new (
      [ 2.0, 0.0, 0.0].into(), [-2.0, 0.0, 0.0].into());
    assert_eq!(
      continuous_segment3_capsule3 (&segment, &capsule).unwrap(),
      ((0.25, [ 1.0, 0.0, 0.0].into()), (0.75, [-1.0, 0.0, 0.0].into())));
    let segment = Segment3::new (
      [ 0.0, 0.0, 0.0].into(), [ 0.0, 0.0, 4.0].into());
    assert_eq!(
      continuous_segment3_capsule3 (&segment, &capsule).unwrap(),
      ((0.0, [ 0.0, 0.0, 0.0].into()), (0.5, [ 0.0, 0.0, 2.0].into())));
    let segment = Segment3::new (
      [ 0.0, 0.0, -4.0].into(), [ 0.0, 0.0, 0.0].into());
    assert_eq!(
      continuous_segment3_capsule3 (&segment, &capsule).unwrap(),
      ((0.5, [ 0.0, 0.0, -2.0].into()), (1.0, [ 0.0, 0.0, 0.0].into())));
  }

  #[test]
  fn test_segment3_aabb3() {
    let aabb    = Aabb3::with_minmax (
      [1.0, -0.5, 0.0].into(), [2.0, 0.5, 1.0].into()
    );
    let segment = Segment3::new (
      [-1.0, 0.0, 0.5].into(), [ 2.0, 0.0, 0.5].into());
    assert_eq!(
      continuous_segment3_aabb3 (&segment, &aabb).unwrap(),
      ((2.0/3.0, [1.0, 0.0, 0.5].into()), (1.0, [ 2.0, 0.0, 0.5].into())));
  }

} // end tests