linear_sim/collision/proximity/

gjk.rs

use sorted_vec::partial::ReverseSortedVec;

use crate::object;
use crate::math::*;

use super::Proximity;

#[derive(Clone, Copy, Debug, PartialEq)]
struct SimplexMinkowski {
  pub points  : [PointMinkowski; 4],
  pub npoints : u8
}

/// Represents a point in the Minkowski difference A - B of shapes A and B
#[derive(Clone, Copy, Debug, Default, PartialEq)]
struct PointMinkowski {
  pub point_a : Point3 <f64>,
  pub point_b : Point3 <f64>
  // TODO: benchmark if it's more performant to pre-compute point
  //pub point  : Point3 <f64>
}

impl SimplexMinkowski {
  const fn push (&mut self, point : PointMinkowski) {
    self.points[self.npoints as usize] = point;
    self.npoints += 1;
  }
}

impl PointMinkowski {
  /// Point A - point B
  fn point (self) -> Point3 <f64> {
    Point3 (self.point_a - self.point_b)
  }

  fn support <A, B> (direction : NonZero3 <f64>, object_a : &A, object_b : &B)
    -> (Self, f64)
  where A : object::Bounded, B : object::Bounded {
    let (point_a, _) = object_a.to_primitive().support (direction);
    let (point_b, _) = object_b.to_primitive().support (-direction);
    let minkowski = PointMinkowski { point_a, point_b };
    (minkowski, minkowski.point().0.dot (*direction))
  }
}

impl Proximity {
  /// Return the proximity query using GJK algorithm.
  pub fn query_gjk <A, B> (object_a : &A, object_b : &B) -> Self where
    A : object::Bounded, B : object::Bounded
  {
    // query_gjk function outline:
    // 1. main loop: gjk search for simplex containing the origin in minkowski hull
    // 2. if contained (overlapping):
    //      compute the penetration depth
    //    else (not overlapping):
    //      determine nearest features and calculate distance
    const TOLERANCE         : f64 = 0.000_000_000_01;
    const TOLERANCE_SQUARED : f64 = TOLERANCE * TOLERANCE;
    // initial search direction
    let mut search_dir = NonZero3::unchecked (vector3 (1.0, 1.0, 1.0));
    let (support, _) = PointMinkowski::support (search_dir, object_a, object_b);
    let mut near = support.point();
    let mut distance_squared = near.0.magnitude_squared();
    let mut lowest_distance_sq = distance_squared;
    let mut simplex = SimplexMinkowski {
      points:  [support,
        PointMinkowski::default(), PointMinkowski::default(), PointMinkowski::default()],
      npoints: 1
    };
    // new search direction towards the origin
    // TODO: is this safe ?
    search_dir = -NonZero3::unchecked (support.point().0);
    let mut lowest_unique_count = 0;
    let mut last_distance_sq = f64::MAX;
    let noprogress_limit = 3;
    let mut noprogress_count = 0;
    let mut unique_count = 0;
    loop {
      let (support, _) = PointMinkowski::support (search_dir, object_a, object_b);
      // vector from last nearest point to new support point
      let vnew = support.point() - near;
      let dot_new = vnew.dot (*search_dir);
      if dot_new <= TOLERANCE {
        // no intersection
        break
      }
      // add new support point and check for overlap
      simplex.push (support);
      let mut overlap = false;
      if simplex.npoints == 4 {
        // check for tetrahedron containment, drop point to create face if no containment
        let [a, b, c, d] = simplex.points.map (PointMinkowski::point);
        // check to see if origin is contained
        let ab = b - a;
        let ac = c - a;
        let ad = d - a;
        let da = a - d;
        let db = b - d;
        let dc = c - d;
        let ab_x_ac = ab.cross (ac);
        let ab_x_ac_dot_ad = ab_x_ac.dot (ad);
        let nd = -d.0;
        let (dab, dac, dbc) = if ab_x_ac_dot_ad > 0.0 {
          // winding ab x ac
          ( db.cross (da),
            da.cross (dc),
            dc.cross (db) )
        } else {
          // winding ac x ab
          ( da.cross (db),
            dc.cross (da),
            db.cross (dc) )
        };
        // containment checks
        let check1 = dab.dot (nd) > 0.000000001;
        let check2 = dac.dot (nd) > 0.000000001;
        let check3 = dbc.dot (nd) > 0.000000001;
        if check1 && check2 && check3 {
          overlap = true
        } else {
          // otherwise intersect the base perpendicular (near) with the new faces
          let line = geometry::Segment3::noisy (Point3::<f64>::origin(), near).into();
          // face 1 = dab
          if let Some (triangle) = geometry::Triangle3::new (d, a, b)
            && geometry::intersect::line_triangle (line, triangle).is_some()
          {
            // drop c
            simplex.points[2] = simplex.points[3];
          } else {
            // face 2 = dac
            if let Some (triangle) = geometry::Triangle3::new (d, a, c)
              && geometry::intersect::line_triangle (line, triangle).is_some()
            {
              // drop b
              simplex.points[1] = simplex.points[3];
            } else {
              // face 3 = dbc
              if let Some (triangle) = geometry::Triangle3::new (d, b, c)
                && geometry::intersect::line_triangle (line, triangle).is_some()
              {
                // drop a
                simplex.points[0] = simplex.points[3];
              } else {
                unreachable!("should have intersected new simplex")
              }
            }
          }
          simplex.npoints = 3;
        }
      }
      if !overlap {
        // get new search direction
        match simplex.npoints {
          1 => {
            near = simplex.points[0].point();
            distance_squared = near.0.magnitude_squared();
            if distance_squared < TOLERANCE_SQUARED {
              overlap = true
            } else {
              // new search direction assigned to inverse of this point, i.e. towards origin
              search_dir = NonZero3::new (-near.0).unwrap()
            }
          }
          2 => {
            let t;
            (t, near) = geometry::Segment3::unchecked (
              simplex.points[0].point(), simplex.points[1].point()
            ).nearest_point (Point3::origin());
            distance_squared = near.0.magnitude_squared();
            if distance_squared < TOLERANCE_SQUARED {
              overlap = true
            } else if *t == 1.0 {
              // point 1 becomes the new simplex, new direction point 1 towards origin
              simplex.points[0] = simplex.points[1];
              simplex.npoints -= 1;
              search_dir = NonZero3::unchecked (-simplex.points[0].point().0)
            } else {
              // new direction edge normal towards origin which is equal to -near
              search_dir = NonZero3::unchecked (-near.0)
            }
          }
          3 => {
            let (s, t);
            ([s, t], near) = geometry::Triangle3::unchecked (
              simplex.points[0].point(),
              simplex.points[1].point(),
              simplex.points[2].point()
            ).nearest_point (Point3::origin());
            distance_squared = near.0.magnitude_squared();
            if distance_squared < TOLERANCE_SQUARED {
              overlap = true
            } else {
              // new search direction
              search_dir = NonZero3::unchecked (-near.0);
              if *s == 0.0 && *t == 0.0 {
                // point A, search direction -A
                simplex.npoints = 1;
              } else if *s == 1.0 && *t == 0.0 {
                // point B, search direction -B
                simplex.points[0] = simplex.points[1];
                simplex.npoints = 1;
              } else if *s == 0.0 && *t == 1.0 {
                // point C, search direction -C
                simplex.points[0] = simplex.points[2];
                simplex.npoints = 1;
              } else if (*s + *t - 1.0).abs() < TOLERANCE {
                // edge BC, search direction BC x BO x BC
                simplex.points[0] = simplex.points[1];
                simplex.points[1] = simplex.points[2];
                simplex.npoints = 2;
              } else if *s < TOLERANCE {
                // edge AC, search direction AC x AO x AC
                simplex.points[1] = simplex.points[2];
                simplex.npoints = 2;
              } else if *t < TOLERANCE {
                // edge AB, search direction AB x AO x AB
                simplex.npoints = 2;
              } else {
                // triangle ABC, direction face normal to origin
                debug_assert!(*s > 0.0);
                debug_assert!(*s < 1.0);
                debug_assert!(*t > 0.0);
                debug_assert!(*t < 1.0);
                debug_assert!(*s + *t < 1.0);
              }
            }
          }
          _ => unreachable!()
        }
      }
      if overlap {
        // compute penetration
        /// Represents distance from origin to a face in the polytope
        #[derive(PartialEq)]
        struct PolytopeFace {
          /// Distance squared from origin
          pub distance_squared : NonNegative <f64>,
          /// Distance vector from origin
          pub distance_vector  : Vector3 <f64>,
          pub nearest_a        : Point3 <f64>,
          pub nearest_b        : Point3 <f64>,
          pub points           : [PointMinkowski; 3],
          pub normal           : NonZero3 <f64>,
          pub exterior         : bool
        }
        impl PartialOrd for PolytopeFace {
          fn partial_cmp (&self, other : &Self) -> Option <std::cmp::Ordering> {
            self.distance_squared.partial_cmp (&other.distance_squared)
          }
        }
        let mut node_list = ReverseSortedVec::<PolytopeFace>::new();
        let add_face = |
          node_list : &mut ReverseSortedVec <PolytopeFace>,
          p1 : PointMinkowski, p2 : PointMinkowski, p3 : PointMinkowski
        | {
          let triangle = geometry::Triangle3::noisy (p1.point(), p2.point(), p3.point());
          let ([s, t], nearest) = triangle.nearest_point (Point3::origin());
          let nearest_a = p1.point_a +
            (p2.point_a - p1.point_a) * *s + (p3.point_a - p1.point_a) * *t;
          let nearest_b = p1.point_b +
            (p2.point_b - p1.point_b) * *s + (p3.point_b - p1.point_b) * *t;
          let normal = {
            let mut normal = triangle.normal();
            if normal.dot (triangle.point_a().0) < 0.0 {
              normal = -normal
            }
            normal
          };
          let node = PolytopeFace {
            distance_squared: nearest.0.norm_squared(),
            distance_vector:  -nearest.0,
            points:           [p1, p2, p3],
            exterior:         false,
            nearest_a,
            nearest_b,
            normal
          };
          node_list.insert (node);
        };
        let a  = simplex.points[0];
        let b  = simplex.points[1];
        let c  = simplex.points[2];
        let d  = simplex.points[3];
        match simplex.npoints {
          3 => {
            // add support points in direction of face normals
            let e1 = simplex.points[1].point() - simplex.points[0].point();
            let e2 = simplex.points[2].point() - simplex.points[0].point();
            let n1 = NonZero3::noisy (e1.cross (e2));
            let n2 = -n1;
            // p1
            let (p1, dot) = PointMinkowski::support (n1, object_a, object_b);
            if dot <= TOLERANCE {
              let half_axis = *n1 * TOLERANCE * 0.5;
              return Proximity {
                distance:  -TOLERANCE.sqrt(),
                normal:    n1.normalize(),
                midpoint:  p1.point_a + half_axis,
                half_axis
              }
            }
            add_face (&mut node_list, a, b, p1);
            add_face (&mut node_list, a, c, p1);
            add_face (&mut node_list, b, c, p1);
            // p2
            let (p2, dot) = PointMinkowski::support (n2, object_a, object_b);
            if dot <= TOLERANCE {
              let half_axis = *n2 * TOLERANCE * 0.5;
              return Proximity {
                distance:  -TOLERANCE.sqrt(),
                normal:    n2.normalize(),
                midpoint:  p2.point_a + half_axis,
                half_axis
              }
            }
            add_face (&mut node_list, a, b, p2);
            add_face (&mut node_list, a, c, p2);
            add_face (&mut node_list, b, c, p2);
          }
          4 => {
            add_face (&mut node_list, a, b, c);
            add_face (&mut node_list, a, b, d);
            add_face (&mut node_list, a, c, d);
            add_face (&mut node_list, b, c, d);
          }
          _ => unreachable!()
        }
        // expanding polytope search loop
        // TODO: for improved efficiency it may be desirable to fix winding for all
        // faces in a CW or CCW direction so that shared edges are always in opposite
        // directions and therefore only one edge direction needs to be checked when
        // adding to the edge array when creating a hole for the new support point
        let mut edges = vec![];
        let mut edge_in_list = vec![];
        let mut last_iter_distance = f64::MIN;
        let exterior_node_proximity = |node : PolytopeFace| {
          let half_axis = node.distance_vector * 0.5;
          Proximity {
            half_axis,
            distance:  -*node.distance_squared.sqrt(),
            normal:    half_axis.normalize(), // TODO: verify correct ?
            midpoint:  node.nearest_a + half_axis
          }
        };
        let mut iter = 0;
        let noprogress_limit = 3;
        let mut noprogress_count = 0;
        loop {
          let mut node = node_list.pop().unwrap();
          if approx::relative_eq!(*node.distance_squared, last_iter_distance,
            epsilon = 0.000000001
          ) {
            // no progress
            noprogress_count += 1;
            if noprogress_count == noprogress_limit {
              log::trace!("expanding polytope search no progress after {iter} iterations");
              return exterior_node_proximity (node)
            }
          } else {
            noprogress_count = 0;
          }
          last_iter_distance = *node.distance_squared;
          // calculate support point for face normal
          let (pa, _) = PointMinkowski::support (node.normal, object_a, object_b);
          if node.points.contains (&pa) {
            // support point already in face
            node.exterior = true;
          } else {
            // exterior face check
            let bpa = pa.point() - node.points[0].point();
            // TODO: is it necessary to normalize here?
            let normal_unit = node.normal.normalize();
            if normal_unit.dot (bpa) < TOLERANCE {
              node.exterior = true;
            }
          }
          if node.exterior {
            // exit if face is exterior
            return exterior_node_proximity (node)
          } else {
            // face is not exterior:
            // insert node edges into edge list
            edges.clear();
            edge_in_list.clear();
            edges.extend ([
              [node.points[0], node.points[1]],
              [node.points[0], node.points[2]],
              [node.points[1], node.points[2]]
            ]);
            edge_in_list.extend ([true, true, true]);
            // remove all faces visible to new support point and fill holes with new
            // faces
            node_list.retain (|node|{
              let p0_pa = pa.point() - node.points[0].point();
              if TOLERANCE < node.normal.dot (p0_pa) {
                // pa visible: add edges of this face to edge array
                let mut add_edge = |i : usize, j : usize| {
                  let mut found = false;
                  // filter: skip excluded edges
                  for (k, edge) in edges.iter().enumerate()
                    .filter (|(k, _)| edge_in_list[*k])
                  {
                    if node.points[i].point() == edge[0].point()
                      && node.points[j].point() == edge[1].point()
                      || node.points[i].point() == edge[1].point()
                      && node.points[j].point() == edge[0].point()
                    {
                      found = true;
                      edge_in_list[k] = false;  // exclude shared edge
                      break
                    }
                  }
                  if !found {
                    // insert
                    edge_in_list.push (true);
                    edges.push ([node.points[i], node.points[j]]);
                  }
                };
                add_edge (0, 1); // edge0: 0->1
                add_edge (0, 2); // edge1: 0->2
                add_edge (1, 2); // edge2: 1->2
                false // remove
              } else {
                true  // retain
              }
            });
            // add new faces connected to support point and "hole" edges in edge list
            // filter: skip excluded edges
            for (_, edge) in edges.iter().enumerate()
              .filter (|(i, _)| edge_in_list[*i])
            {
              if geometry::Triangle3::new (pa.point(), edge[0].point(), edge[1].point())
                .is_none()
              {
                // skip faces where points are colinear
                continue
              }
              add_face (&mut node_list, pa, edge[0], edge[1]);
            }
          }
          iter += 1;
        }
      } // end penetration
      // count unique points
      if simplex.points[0].point_a == simplex.points[1].point_a
        && simplex.points[1].point_a == simplex.points[2].point_a
      {
        unique_count += 1;
      } else if simplex.points[0].point_a == simplex.points[1].point_a
        || simplex.points[0].point_a == simplex.points[2].point_a
        || simplex.points[1].point_a == simplex.points[2].point_a
      {
        unique_count += 2;
      } else {
        debug_assert!(
          simplex.points[0].point_a != simplex.points[1].point_a &&
          simplex.points[0].point_a != simplex.points[2].point_a &&
          simplex.points[1].point_a != simplex.points[2].point_a);
        unique_count += 3;
      }
      if simplex.points[0].point_b == simplex.points[1].point_b
        && simplex.points[1].point_b == simplex.points[2].point_b
      {
        unique_count += 1;
      } else if simplex.points[0].point_b == simplex.points[1].point_b
        || simplex.points[0].point_b == simplex.points[2].point_b
        || simplex.points[1].point_b == simplex.points[2].point_b
      {
        unique_count += 2;
      } else {
        debug_assert!(
          simplex.points[0].point_b != simplex.points[1].point_b &&
          simplex.points[0].point_b != simplex.points[2].point_b &&
          simplex.points[1].point_b != simplex.points[2].point_b);
        unique_count += 3;
      }
      // new lowest
      if distance_squared < lowest_distance_sq ||
        distance_squared == lowest_distance_sq && unique_count < lowest_unique_count
      {
        lowest_distance_sq = distance_squared;
        lowest_unique_count = unique_count;
      } else if distance_squared >= last_distance_sq {
        if noprogress_count < noprogress_limit {
          noprogress_count += 1;
        } else {
          // no progress limit reached: break
          break
        }
      }
      last_distance_sq = distance_squared;
    }
    // distance squared to nearest feature
    // check duplicate vertices
    let mut dupe_a_01 = false;
    let mut dupe_a_02 = false;
    let mut dupe_a_12 = false;
    let mut dupe_b_01 = false;
    let mut dupe_b_02 = false;
    let mut dupe_b_12 = false;
    if simplex.npoints >= 2 {
      dupe_a_01 = simplex.points[0].point_a == simplex.points[1].point_a;
      dupe_b_01 = simplex.points[0].point_b == simplex.points[1].point_b;
    }
    if simplex.npoints >= 3 {
      dupe_a_02 = simplex.points[0].point_a == simplex.points[2].point_a;
      dupe_a_12 = simplex.points[1].point_a == simplex.points[2].point_a;
      dupe_b_02 = simplex.points[0].point_b == simplex.points[2].point_b;
      dupe_b_12 = simplex.points[1].point_b == simplex.points[2].point_b;
    }
    // fix some degenerate cases
    // simplex.points[0].point() == simplex.points[1].point()
    debug_assert!(!(simplex.npoints == 2 && unique_count == 2),
      "only one unique point: not possible");
    // calculate final distance and nearest points
    match simplex.npoints {
      1 => {
        // point simplex
        let nearest_a = simplex.points[0].point_a;
        let nearest_b = simplex.points[0].point_b;
        let a_to_b = nearest_b - nearest_a;
        let half_axis = a_to_b * 0.5;
        let midpoint = nearest_a + half_axis;
        let normal = if !a_to_b.is_approx_zero() {
          Unit3::normalize (-a_to_b)
        } else {
          // points are touching: use object centers
          Unit3::normalize (object_a.position().0 - object_b.position().0)
        };
        let distance = a_to_b.magnitude();
        Proximity { distance, half_axis, midpoint, normal }
      }
      2 => {
        // line simplex
        let point0_a = simplex.points[0].point_a;
        let point1_a = simplex.points[1].point_a;
        let point0_b = simplex.points[0].point_b;
        let point1_b = simplex.points[1].point_b;
        if !dupe_a_01 && !dupe_b_01 {
          // edge vs edge
          let edge_a = geometry::Segment3::noisy (point0_a, point1_a);
          let edge_b = geometry::Segment3::noisy (point0_b, point1_b);
          Proximity::query_segment_segment (&edge_a, &edge_b)
        } else {
          // point vs edge
          if dupe_a_01 {
            // point in A vs edge in B
            debug_assert!(!dupe_b_01);
            debug_assert_eq!(point0_a, point1_a);
            let point_a = point0_a;
            let edge_b  = geometry::Segment3::noisy (point0_b, point1_b);
            // flip direction so proximity query points from A -> B
            Proximity::query_segment_point (&edge_b, &point_a).flip()
          } else {
            // point in B vs edge in A
            debug_assert!(dupe_b_01);
            let point_b = point0_b;
            let edge_a  = geometry::Segment3::noisy (point0_a, point1_a);
            Proximity::query_segment_point (&edge_a, &point_b)
          }
        }
      }
      3 => {
        // triangle simplex
        let is_edge = |
          p0 : &mut Point3 <f64>, p1 : &mut Point3 <f64>, p2 : &Point3 <f64>
        | {
          let p01 = *p1 - *p0;
          let p02 = *p2 - *p0;
          let p01_cross_p02 = p01.cross (p02);
          if p01_cross_p02.is_approx_zero() {
            // points are colinear
            let p12 = *p2 - *p1;
            let dot_p01 = p01.magnitude_squared();
            let dot_p02 = p02.magnitude_squared();
            let dot_p12 = p12.magnitude_squared();
            // swap so that longest edge is always p0->p1
            if dot_p01 > dot_p02 && dot_p01 > dot_p12 {
              // do nothing: already p0->p1
            } else if dot_p02 > dot_p01 && dot_p02 > dot_p12 {
              *p1 = *p2;
            } else {
              debug_assert!(dot_p12 > dot_p01 && dot_p12 > dot_p02);
              *p0 = *p1;
              *p1 = *p2;
            }
            true
          } else {
            false
          }
        };
        // point vs point not possible
        debug_assert!(!((dupe_a_01 && dupe_a_12) && (dupe_b_01 && dupe_b_12)));
        if (!dupe_a_01 && !dupe_a_02 && !dupe_a_12) &&
           (!dupe_b_01 && !dupe_b_02 && !dupe_b_12)
        {
          // face vs face
          let mut a0 = simplex.points[0].point_a;
          let mut a1 = simplex.points[1].point_a;
          let a2 = simplex.points[2].point_a;
          let mut b0 = simplex.points[0].point_b;
          let mut b1 = simplex.points[1].point_b;
          let b2 = simplex.points[2].point_b;
          let edge_a = is_edge (&mut a0, &mut a1, &a2);
          let edge_b = is_edge (&mut b0, &mut b1, &b2);
          if edge_a && edge_b {
            // edge vs edge
            let edge_a = geometry::Segment3::noisy (a0, a1);
            let edge_b = geometry::Segment3::noisy (b0, b1);
            Proximity::query_segment_segment (&edge_a, &edge_b)
          } else if edge_a && !edge_b {
            // edge vs face
            let edge_a = geometry::Segment3::noisy (a0, a1);
            let triangle_b = geometry::Triangle3::noisy (b0, b1, b2);
            // flip direction so proximity query points from A -> B
            Proximity::query_triangle_segment (&triangle_b, &edge_a).flip()
          } else if !edge_a && edge_b {
            // face vs edge
            let edge_b = geometry::Segment3::noisy (b0, b1);
            let triangle_a = geometry::Triangle3::noisy (a0, a1, a2);
            Proximity::query_triangle_segment (&triangle_a, &edge_b)
          } else {
            debug_assert!(!edge_a);
            debug_assert!(!edge_b);
            // face vs face
            let triangle_a = geometry::Triangle3::noisy (a0, a1, a2);
            let triangle_b = geometry::Triangle3::noisy (b0, b1, b2);
            Proximity::query_triangle_triangle (&triangle_a, &triangle_b)
          }
        } else if (dupe_a_01 != dupe_a_02 || dupe_a_01 != dupe_a_12)
          && (!dupe_b_01 && !dupe_b_02 && !dupe_b_12)
        {
          // edge vs face
          // need to look at distance_sq between edge and face in objects: there is
          // probably not a general way to get the contact points from the simplex as
          // the 3-simplex doesn't fully determine the intersection of an edge+face this
          // code makes use of a segment/triangle distance_sq function from eberly
          // geometrictools which is probably slower than the other (simpler) cases
          let a0 = simplex.points[0].point_a;
          let mut a1 = simplex.points[1].point_a;
          let mut b0 = simplex.points[0].point_b;
          let mut b1 = simplex.points[1].point_b;
          let b2 = simplex.points[2].point_b;
          if dupe_a_01 || dupe_a_12 {
            a1 = simplex.points[2].point_a;
          }
          let edge_b = is_edge (&mut b0, &mut b1, &b2);
          let edge_a = geometry::Segment3::noisy (a0, a1);
          if edge_b {
            // edge vs edge
            let edge_b = geometry::Segment3::noisy (b0, b1);
            Proximity::query_segment_segment (&edge_a, &edge_b)
          } else {
            // edge vs triangle
            let triangle_b = geometry::Triangle3::noisy (b0, b1, b2);
            // flip direction so proximity query points from A -> B
            Proximity::query_triangle_segment (&triangle_b, &edge_a).flip()
          }
        } else if (dupe_b_01 != dupe_b_02 || dupe_b_01 != dupe_b_12)
          && (!dupe_a_01 && !dupe_a_02 && !dupe_a_12)
        {
          // face vs edge
          let mut a0 = simplex.points[0].point_a;
          let mut a1 = simplex.points[1].point_a;
          let a2 = simplex.points[2].point_a;
          let b0 = simplex.points[0].point_b;
          let b1 = if dupe_b_01 || dupe_b_12 {
            simplex.points[2].point_b
          } else {
            simplex.points[1].point_b
          };
          let edge_a = is_edge (&mut a0, &mut a1, &a2);
          if edge_a {
            // edge vs edge
            let edge_a = geometry::Segment3::noisy (a0, a1);
            let edge_b = geometry::Segment3::noisy (b0, b1);
            Proximity::query_segment_segment (&edge_a, &edge_b)
          } else {
            // triangle vs edge
            let triangle_a = geometry::Triangle3::noisy (a0, a1, a2);
            let edge_b = geometry::Segment3::noisy (b0, b1);
            Proximity::query_triangle_segment (&triangle_a, &edge_b)
          }
        } else {
          if (dupe_a_01 != dupe_a_02 || dupe_a_01 != dupe_a_12)
            && !(dupe_a_01 && dupe_a_02 && dupe_a_12)  // XOR of 3 values
          {
            // edge vs edge
            // B also has exactly one duplicate pair
            debug_assert!(dupe_b_01 != dupe_b_02 || dupe_b_01 != dupe_b_12);
            debug_assert!(!(dupe_b_01 && dupe_b_02 && dupe_b_12));
            debug_assert!(!(dupe_a_01 && dupe_b_01));
            debug_assert!(!(dupe_a_02 && dupe_b_02));
            debug_assert!(!(dupe_a_12 && dupe_b_12));
            // before making the distance call, the base point of the triangle simplex
            // has to be the intersection of the 'duplicate' points in each object and
            // the edge 01 of the simplex triangle should be the edge corresponding to
            // the edge in A, and likewise edge 02 for B
            if (dupe_a_01 != dupe_a_02) && (dupe_b_01 != dupe_b_02) {
              // 0 -> 0
              if dupe_a_02 {
                // 1 -> 1
                // 2 -> 2
                debug_assert!(dupe_b_01);
                // no action needed
              } else {
                // 1 -> 2
                // 2 -> 1
                debug_assert!(dupe_a_01);
                debug_assert!(dupe_b_02);
                simplex.points.swap(1, 2);
              }
            } else if (dupe_a_01 != dupe_a_12) && (dupe_b_01 != dupe_b_12) {
              // 0 -> 1
              let temp = simplex.points[0];
              simplex.points[0] = simplex.points[1];
              if dupe_a_01 {
                // 1 -> 2
                // 2 -> 0
                debug_assert!(dupe_b_12);
                simplex.points[1] = simplex.points[2];
                simplex.points[2] = temp;
              } else {
                // 1 -> 0
                // 2 -> 2
                debug_assert!(dupe_a_12);
                debug_assert!(dupe_b_01);
                simplex.points[1] = temp;
              }
            } else {
              // 0 -> 2
              debug_assert!(dupe_a_02 != dupe_a_12);
              debug_assert!(dupe_b_02 != dupe_b_12);
              let temp = simplex.points[0];
              simplex.points[0] = simplex.points[2];
              if dupe_a_02 {
                // 1 -> 1
                // 2 -> 0
                debug_assert!(dupe_b_12);
                simplex.points[2] = temp;
              } else {
                // 1 -> 0
                // 2 -> 1
                debug_assert!(dupe_a_12);
                debug_assert!(dupe_b_02);
                simplex.points[2] = simplex.points[1];
                simplex.points[1] = temp;
              }
            }
            let a0 = simplex.points[0].point_a;
            let a1 = simplex.points[1].point_a;
            let b0 = simplex.points[0].point_b;
            // somewhat confusing, but edge B is always along simplex points 0->2
            let b2 = simplex.points[2].point_b;
            let edge_a = geometry::Segment3::noisy (a0, a1);
            let edge_b = geometry::Segment3::noisy (b0, b2);
            Proximity::query_segment_segment (&edge_a, &edge_b)
          } else {
            // point vs face
            // XOR: point in one of the objects but not both
            debug_assert!((dupe_a_01 && dupe_a_12) != (dupe_b_01 && dupe_b_12));
            if dupe_a_01 && dupe_a_12 {
              // point in A, face in B
              debug_assert!(dupe_a_02); // implied
              let point_a = simplex.points[0].point_a;
              let b0 = simplex.points[0].point_b;
              let b1 = simplex.points[1].point_b;
              let b2 = simplex.points[2].point_b;
              let triangle_b = geometry::Triangle3::noisy (b0, b1, b2);
              // flip direction so proximity query points from A -> B
              Proximity::query_triangle_point (&triangle_b, &point_a).flip()
            } else {
              // point in B, face in A
              debug_assert!(dupe_b_01 && dupe_b_12);
              debug_assert!(dupe_b_02); // implied
              let point_b = simplex.points[0].point_b;
              let a0 = simplex.points[0].point_a;
              let a1 = simplex.points[1].point_a;
              let a2 = simplex.points[2].point_a;
              let triangle_a = geometry::Triangle3::noisy (a0, a1, a2);
              Proximity::query_triangle_point (&triangle_a, &point_b)
            }
          }
        }
      }
      _ => unreachable!()
    }
  }
}

#[cfg(test)]
mod tests {
  use approx;
  use gl_utils::{mesh, Mesh};
  use rand;
  use rand_distr;
  use rand_xorshift::XorShiftRng;
  use crate::math::geometry::*;
  use crate::component;
  use super::*;

  #[test]
  fn query_gjk() {
    use rand::SeedableRng;
    use rand_distr::Distribution;

    let tetrahedron = |p1 : [f64; 3], p2 : [f64; 3], p3 : [f64; 3], p4 : [f64; 3]|
      -> (component::Position, component::Bound)
    {
      ( component::Position::origin(),
        Hull3::from_points_with_mesh (
          &simplex3::Tetrahedron::<f64>::noisy (
            p1.into(), p2.into(), p3.into(), p4.into()
          ).points()
        ).unwrap().into()
      )
    };
    let a = tetrahedron (
      [-2.0,  2.0, -1.0],
      [ 2.0,  2.0, -1.0],
      [ 0.0, -2.0, -1.0],
      [ 0.0,  0.0,  2.0]);
    let b = tetrahedron (
      [-2.0,  2.0,  3.5],
      [ 2.0,  2.0,  3.5],
      [ 0.0, -2.0,  3.5],
      [ 0.0,  0.0,  6.5]);
    let proximity = Proximity::query_gjk (&a, &b);
    assert_eq!(proximity.distance, 1.5);
    assert_eq!(proximity.half_axis, [0.0, 0.0, 0.75].into());
    assert_eq!(proximity.midpoint, [0.0, 0.0, 2.75].into());
    assert_eq!(proximity.normal, -Unit3::axis_z());
    let a = tetrahedron (
      [-2.0,  2.0, -1.0],
      [ 2.0,  2.0, -1.0],
      [ 0.0, -2.0, -1.0],
      [ 0.0,  0.0,  2.0]);
    let b = tetrahedron (
      [-2.0,  2.0,  1.75],
      [ 2.0,  2.0,  1.75],
      [ 0.0, -2.0,  1.75],
      [ 0.0,  0.0,  4.75]);
    let proximity = Proximity::query_gjk (&a, &b);
    assert_eq!(proximity.distance, -0.25);
    assert_eq!(proximity.half_axis, [0.0, 0.0, -0.125].into());
    assert_eq!(proximity.midpoint, [0.0, 0.0, 1.875].into());
    assert_eq!(proximity.normal, -Unit3::axis_z());
    let a = tetrahedron (
      [ 0.0,  0.0,  3.5],
      [-2.0,  2.0,  0.5],
      [ 2.0,  2.0,  0.5],
      [ 0.0, -2.0,  0.5]);
    let b = tetrahedron (
      [-2.0,  2.0,  1.0],
      [ 2.0,  2.0,  1.0],
      [ 0.0, -2.0,  1.0],
      [ 0.0,  0.0, -2.0]);
    let proximity = Proximity::query_gjk (&a, &b);
    assert_eq!(proximity.distance, -0.5);
    approx::assert_abs_diff_eq!(proximity.half_axis, [0.0, 0.0, 0.25].into());
    assert_eq!(proximity.midpoint, [0.0, 2.0/3.0, 0.75].into());
    approx::assert_abs_diff_eq!(*proximity.normal, *Unit3::axis_z(),
      epsilon = f64::EPSILON * 4.0);
    let hull = |points : &[Point3 <f64>]| -> (component::Position, component::Bound) {
      ( component::Position::origin(),
        Hull3::from_points_with_mesh (points).unwrap().into()
      )
    };
    let a = hull (&[
      [ 0.0,  0.0,  3.5],
      [ 0.0,  0.0,  0.5],
      [-2.0,  2.0,  0.5],
      [ 2.0,  2.0,  0.5],
      [ 0.0, -2.0,  0.5]].map (Into::into));
    let b = hull (&[
      [-2.0,  2.0,  1.0],
      [ 2.0,  2.0,  1.0],
      [ 0.0, -2.0,  1.0],
      [ 0.0,  0.0,  1.0],
      [ 0.0,  0.0, -2.0]].map (Into::into));
    let proximity = Proximity::query_gjk (&a, &b);
    assert_eq!(proximity.distance, -0.5);
    approx::assert_abs_diff_eq!(proximity.half_axis, [0.0, 0.0, 0.25].into());
    assert_eq!(proximity.midpoint, [0.0, 2.0/3.0, 0.75].into());
    approx::assert_abs_diff_eq!(*proximity.normal, *Unit3::axis_z(),
      epsilon = f64::EPSILON * 4.0);

    {
      const NPOINTS : usize = 4;
      let mut rng    = XorShiftRng::seed_from_u64 (0);
      let cauchy     = rand_distr::Cauchy::new (0.0, 1.0).unwrap();
      let randf      = |rng : &mut XorShiftRng| cauchy.sample (rng);
      let aabb = Aabb3::with_minmax (
        [-5.0, -5.0, -5.0].into(),
        [ 5.0,  5.0,  5.0].into()).unwrap();
      let rand_point = |rng : &mut XorShiftRng|
        aabb.clamp (point3 (randf (rng), randf (rng), randf (rng)));
      let object = |hull : shape::Hull <f64>|
        (component::Position::origin(), component::Bound::from (hull));
      #[expect(unused_variables)]
      let write_gltf = |
        (hull, mesh) : &shape::Hull <f64>,
        filename     : &str
      | {
        let mut m = mesh::Triangles3dBuilder::empty();
        for triangle in mesh.triangles().values() {
          let triangle = hull.triangle (triangle);
          m.push_face (triangle.points());
        }
        Mesh::from (m.build()).write_gltf (filename);
      };
      for _i in 0..1000 {
        //println!("ITER: {i}");
        let a = {
          let points = std::iter::repeat_with (|| rand_point (&mut rng)).take (NPOINTS)
            .collect::<Vec<_>>();
          let hull = Hull3::from_points_with_mesh (&points).unwrap();
          //write_gltf (&hull, mesh, "hull_a.gltf");
          object (hull)
        };
        let b = {
          let points = std::iter::repeat_with (|| rand_point (&mut rng)).take (NPOINTS)
            .collect::<Vec<_>>();
          let hull = Hull3::from_points_with_mesh (&points).unwrap();
          //write_gltf (&hull, mesh, "hull_b.gltf");
          object (hull)
        };
        //show!(a);
        //show!(b);
        let _proximity = Proximity::query_gjk (&a, &b);
      }
    }
  }
}