linear-sim 0.12.0

//! Collision detection and resolution

use std;
#[cfg(feature = "debug_dump")]
use std::sync::atomic;
use std::cmp::Reverse;
use either::Either;
use log;
use sorted_vec::{SortedSet, ReverseSortedSet};
use vec_map::VecMap;
#[cfg(feature = "derive_serdes")]
use serde::{Deserialize, Serialize};

use geometry::Aabb3;
use math::*;

use crate::{constraint, event, geometry, math, Constraint};
use crate::constraint::contact::CONTACT_DISTANCE;
use crate::object::{self, InternalId, ObjectPair};

pub mod proximity;
pub mod intersection;
mod broad;

pub use self::proximity::Proximity;
pub use self::intersection::Intersection;
use self::broad::Broad;

////////////////////////////////////////////////////////////////////////////////////////
//  constants                                                                         //
////////////////////////////////////////////////////////////////////////////////////////

#[cfg(feature = "debug_dump")]
#[cfg_attr(docsrs, doc(cfg(feature="debug_dump")))]
pub static DEBUG_DUMP : atomic::AtomicBool = atomic::AtomicBool::new (false);
#[cfg(feature = "debug_dump")]
#[cfg_attr(docsrs, doc(cfg(feature="debug_dump")))]
pub const DEBUG_DUMP_DETECT_RESOLVE_MAX_ITERS : u64 = 200;

////////////////////////////////////////////////////////////////////////////////////////
//  structs                                                                           //
////////////////////////////////////////////////////////////////////////////////////////

/// Collision subsystem
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug, Default)]
pub(crate) struct Collision {
  /// Broad-phase detection subsystem
  broad : Broad,
  /// Collision pipeline data: broad, mid, and narrow phase results.
  ///
  /// The pipeline should always be empty at the beginning and end of each collision
  /// step and is skipped when serializing the collision state resulting in a default
  /// initialized (empty) pipeline when deserialized.
  #[cfg_attr(feature = "derive_serdes", serde(skip))]
  pipeline : Pipeline,
  /// Count maximum iterations of collision detect/resolve main loop
  #[cfg(debug_assertions)]
  detect_resolve_max_iter_count : u64,
  /// Count maximum iterations of narrow phase loop
  #[cfg(debug_assertions)]
  narrow_toi_max_iter_count : u64
}

/// Collision pipeline data.
///
/// A collision detection step should begin and end with all vectors empty.
#[derive(Clone, Debug, Default, PartialEq)]
struct Pipeline {
  /// Counts iterations of the `detect_resolve` loop; reset to zero at the beginning of
  /// a collision step
  pub detect_resolve_iter : u64,
  /// Broad phase AABB overlaps on all three axes
  pub broad_overlap_pairs : Vec <ObjectPair>,
  /// Sorted in *reverse order* by start time
  pub mid_toi_pairs       :
    ReverseSortedSet <(Normalized <f64>, Normalized <f64>, ObjectPair)>,
  /// Sorted in *reverse order* by TOI
  pub narrow_toi_contacts :
    ReverseSortedSet <(Normalized <f64>, ObjectPair, constraint::Contact)>,
  pub resolved_collisions : Vec <event::CollisionResolve>,
  /// Nocollide intersections
  pub overlaps            : Vec <event::Overlap>,
}

////////////////////////////////////////////////////////////////////////////////////////
//  functions                                                                         //
////////////////////////////////////////////////////////////////////////////////////////

pub fn report_sizes() {
  use std::mem::size_of;
  println!("collision report sizes...");

  println!("  size of Collision: {}", size_of::<Collision>());

  println!("...collision report sizes");
}

////////////////////////////////////////////////////////////////////////////////////////
//  impls                                                                             //
////////////////////////////////////////////////////////////////////////////////////////

impl Collision {
  pub(crate) const fn broad (&self) -> &Broad {
    &self.broad
  }
  /// Attempt to add a new static object to the collision subsystem.
  ///
  /// Queries distance to each dynamic object and if any intersections are
  /// found, the object is not added and the intersections are returned.
  ///
  /// Only counts intersections if both objects are collidable.
  pub(crate) fn try_add_object_static (&mut self,
    objects_dynamic : &VecMap <object::Dynamic>,
    object          : &object::Static,
    key             : object::Key
  ) -> Result <(), Vec <(object::Id, Intersection)>> {
    let mut intersections = Vec::new();
    if object.collidable {
      let aabb_discrete = object.aabb_dilated();
      let overlaps : Vec <object::Id> = self.broad.overlaps_discrete (aabb_discrete)
        .into_iter().map (object::Id::from).collect::<Vec <_>>();
      for object_id in overlaps {
        let proximity = match object_id.kind {
          object::Kind::Static  => continue,  // skip static/static checks
          object::Kind::Dynamic => {
            let object_dynamic = &objects_dynamic[object_id.key.index()];
            if !object_dynamic.collidable {
              continue                    // skip non-collidable dynamic objects
            }
            Proximity::query (object_dynamic, object)
          }
          object::Kind::Nodetect => unreachable!()
        };
        if let Ok (intersection) = proximity.try_into() {
          intersections.push ((object_id, intersection));
        }
      }
    }
    if intersections.is_empty() {
      self.add_object_static (object, key);
      Ok  (())
    } else {
      Err (intersections)
    }
  }

  #[inline]
  pub(crate) fn remove_object_static (&mut self, object_key : object::Key) {
    debug_assert!(self.pipeline.is_empty());
    let id = InternalId::new_static (object_key);
    self.broad.remove_object (id);
  }

  /// Attempts to add a new dynamic object.
  ///
  /// Queries distance to each static and dynamic object and if any intersections are
  /// found, the object is not added and the intersections are returned.
  ///
  /// Only counts intersections if both objects are collidable.
  pub(crate) fn try_add_object_dynamic (&mut self,
    pseudo_velocities : &mut VecMap <Vector3 <f64>>,
    objects_static    : &VecMap <object::Static>,
    objects_dynamic   : &VecMap <object::Dynamic>,
    object            : &object::Dynamic,
    key               : object::Key
  ) -> Result <(), Vec <(object::Id, Intersection)>> {
    let mut intersections = Vec::new();
    if object.collidable {
      let aabb_discrete     = object.aabb_dilated();
      let overlaps : Vec <object::Id> = self.broad.overlaps_discrete (aabb_discrete)
        .into_iter().map (object::Id::from).collect::<Vec <_>>();
      for object_id in overlaps {
        let proximity = match object_id.kind {
          object::Kind::Static  => {
            let object_static = &objects_static[object_id.key.index()];
            if !object_static.collidable {
              continue  // skip non-collidable static objects
            }
            Proximity::query (object_static, object)
          }
          object::Kind::Dynamic => {
            let object_dynamic = &objects_dynamic[object_id.key.index()];
            if !object_dynamic.collidable {
              continue  // skip non-collidable dynamic objects
            }
            Proximity::query (object_dynamic, object)
          }
          object::Kind::Nodetect => unreachable!()
        };
        if let Ok (intersection) = proximity.try_into() {
          intersections.push ((object_id, intersection));
        }
      }
    }
    if intersections.is_empty() {
      self.add_object_dynamic (pseudo_velocities, object, key);
      Ok  (())
    } else {
      Err (intersections)
    }
  }

  #[inline]
  pub(crate) fn remove_object_dynamic (&mut self, key : object::Key) {
    debug_assert!(self.pipeline.is_empty());
    let id = InternalId::new_dynamic (key);
    self.broad.remove_object (id);
  }

  #[inline]
  pub(crate) fn update_object_static (&mut self,
    object : &object::Static, key : object::Key
  ) {
    debug_assert!(self.pipeline.is_empty());
    self.broad.update_aabb_static (object.aabb_dilated(), key);
    unimplemented!("TODO: collision detect and resolve any overlaps caused by \
      updating the static object")
  }

  #[inline]
  pub(crate) fn update_object_dynamic (&mut self,
    object : &object::Dynamic, key : object::Key
  ) {
    debug_assert!(self.pipeline.is_empty());
    let aabb = object.aabb_dilated();
    self.broad.update_aabb_dynamic_discrete (aabb, key);
    // TODO: why is the continuous AABB the same as discrete here?
    self.broad.update_aabb_dynamic_continuous (aabb, key);
    log::warn!("TODO: collision detect and resolve any overlaps caused by \
      updating the dynamic object")
  }

  //
  //  Collision::detect_resolve_loop
  //
  /// Main collision loop.
  ///
  /// Before the loop starts, `begin_step()`:
  ///
  /// 1. Update broad-phase AABB data
  ///
  /// Inside the collision loop:
  ///
  /// 1. Perform continuous detection
  /// 2. Resolve earliest detected collision
  ///
  /// After collision loop:
  ///
  /// 1. Drain pipeline outputs (collisions, overlaps, contacts)
  /// 2. Zero pseudovelocities
  pub(crate) fn detect_resolve_loop (&mut self,
    constraint_manager : &mut constraint::Manager,
    objects_static     : &VecMap <object::Static>,
    objects_dynamic    : &mut VecMap <object::Dynamic>,
    step               : u64,
    output             : &mut Vec <event::Output>
  ) {
    log::trace!(step; "detect/resolve loop");

    // initialize collision for the current step
    debug_assert!(self.pipeline.is_empty());
    self.pipeline.detect_resolve_iter = 0;

    // broad: update AABBs based on the given dynamic objects state
    //
    // the continuous (swept) AABBs are calculated from min/max of the previous
    // 'discrete' (instantaneous) AABB and the new "current" discrete AABB
    self.broad.begin_step (objects_dynamic, step);

    // main detect/resolve loop: resolve one collision per iteration
    loop {
      log::debug!(iter=self.pipeline.detect_resolve_iter;
        "detect/resolve loop iteration");
      // detect collisions (TOI contacts)
      self.detect_continuous (constraint_manager, objects_static, objects_dynamic);
      // resolve earliest collision: resolve a velocity constraint at TOI and a
      // position constraint at t==1.0
      if !self.resolve_first_collision (
        constraint_manager, objects_static, objects_dynamic
      ) {
        debug_assert!(self.pipeline.mid_toi_pairs.is_empty());
        debug_assert!(self.pipeline.narrow_toi_contacts.is_empty());
        break
      }
      self.pipeline.detect_resolve_iter += 1;
    }

    log::trace!(iters=self.pipeline.detect_resolve_iter+1;
      "detect/resolve loop complete");
    #[cfg(debug_assertions)]
    {
      if self.detect_resolve_max_iter_count
        < (self.pipeline.detect_resolve_iter + 1)
      {
        #[cfg(feature = "debug_dump")]
        if self.pipeline.detect_resolve_iter >
          DEBUG_DUMP_DETECT_RESOLVE_MAX_ITERS
        {
          DEBUG_DUMP.store (true, atomic::Ordering::SeqCst);
        }
        self.detect_resolve_max_iter_count = self.pipeline.detect_resolve_iter;
      }
      log::debug!(max_iter_count=self.detect_resolve_max_iter_count;
        "detect/resolve loop max iters");
    }

    // output collision events
    for collision_resolve in self.pipeline.resolved_collisions.drain (..) {
      output.push (collision_resolve.into());
    }
    for overlap in self.pipeline.overlaps.drain (..) {
      output.push (overlap.into());
    }

    // output contacts
    constraint_manager.output_contacts (output);

    // zero pseudovelocities
    for (_, pseudovelocity) in constraint_manager.pseudo_velocities.iter_mut() {
      *pseudovelocity = Vector3::zero();
    }

    // done
    debug_assert!(self.pipeline.is_empty());
  }

  //
  //  private methods
  //

  /// Detect TOI (time-of-impact) contacts, i.e. *collisions*.
  fn detect_continuous (&mut self,
    constraint_manager : &constraint::Manager,
    objects_static     : &VecMap <object::Static>,
    objects_dynamic    : &VecMap <object::Dynamic>
  ) {
    // collect broad overlap pairs
    self.broad_overlap_pairs_continuous();
    // mid-phase TOI pairs
    self.mid_toi_pairs (constraint_manager, objects_dynamic);
    // narrow-phase TOI pairs
    self.narrow_toi_contacts (objects_static, objects_dynamic,
      &constraint_manager.pseudo_velocities);
  }

  /// Resolve the first collision (TOI contact pair) in the pipeline, if any, and
  /// returns true, otherwise returns false.
  ///
  /// If impulses do not prevent intersection at t==1.0, post-impulse position
  /// correction will move objects to a distance of `0.5 * CONTACT_DISTANCE` by applying
  /// a pseudo-velocity impulse at the collision TOI.
  ///
  /// This function will be called multiple times in the `detect_resolve_loop()`.
  fn resolve_first_collision (&mut self,
    constraint_manager : &mut constraint::Manager,
    objects_static     : &VecMap <object::Static>,
    objects_dynamic    : &mut VecMap <object::Dynamic>
  ) -> bool {
    if let Some (Reverse ((toi, object_pair, contact))) =
      self.pipeline.narrow_toi_contacts.pop()
    {
      // process the first narrow toi contact (collision): the container is
      // reverse-sorted so we use pop
      let (object_id_a, object_id_b) = object_pair.into();
      let kind_a  = object_id_a.kind();
      let kind_b  = object_id_b.kind();
      let index_a = object_id_a.key().index();
      let index_b = object_id_b.key().index();
      // if one or both objects are not collidable, we report overlap and return
      let collidable = {
        let collidable_a = match kind_a {
          object::Kind::Static   => objects_static[index_a].collidable,
          object::Kind::Dynamic  => objects_dynamic[index_a].collidable,
          object::Kind::Nodetect => unreachable!()
        };
        debug_assert_eq!(kind_b, object::Kind::Dynamic);
        let collidable_b = objects_dynamic[index_b].collidable;
        collidable_a && collidable_b
      };
      if !collidable {
        let overlap = event::Overlap {
          object_id_a: object_id_a.into(),
          object_id_b: object_id_b.into()
        };
        log::debug!(toi=*toi, overlap:?; "nocollide overlap");
        self.pipeline.overlaps.push (overlap);
        return true
      }
      log::debug!(toi=*toi, object_pair:?=(object_id_a, object_id_b);
        "resolve collision");
      // get persistant contact islands
      let maybe_island_a = constraint_manager.get_island_key (object_id_a);
      let maybe_island_b = {
        let maybe_island_b = constraint_manager.get_island_key (object_id_b);
        if maybe_island_a == maybe_island_b {
          // if the islands are the same, then just use island a
          None
        } else {
          maybe_island_b
        }
      };
      // resolve collision
      let collision_resolve = self.resolve_collision (
        constraint_manager, objects_static, objects_dynamic, toi, object_id_a,
        object_id_b, contact, maybe_island_a, maybe_island_b);
      log::trace!(collision_resolve:?; "resolved collision");
      self.pipeline.resolved_collisions.push (collision_resolve);
      true
    } else {
      // no narrow toi contact to process, return false
      debug_assert!(self.pipeline.mid_toi_pairs.is_empty());
      false
    }
  }

  #[expect(clippy::too_many_arguments)]
  fn resolve_collision (
    &mut self,
    constraint_manager : &mut constraint::Manager,
    objects_static     : &VecMap <object::Static>,
    objects_dynamic    : &mut VecMap <object::Dynamic>,
    toi                : Normalized <f64>,
    object_id_a        : InternalId,
    object_id_b        : InternalId,
    collision          : constraint::Contact,
    island_key_a       : Option <constraint::island::KeyType>,
    island_key_b       : Option <constraint::island::KeyType>
  ) -> event::CollisionResolve {
    // 1. lerp all involved dynamic objects to the TOI
    // 2. constrain velocities
    // 3. lerp to T==1.0
    // 4. constrain positions
    // 5. create or destroy persistent contacts from final proximity check
    // 6. update AABBs in broad pipeline
    use num::Zero;
    let mut dynamic_ids = SortedSet::new();
    if object_id_a.kind() == object::Kind::Dynamic {
      dynamic_ids.push (object_id_a);
    }
    dynamic_ids.push (object_id_b);
    let mut islands = constraint_manager.islands.take().unwrap();
    let island_a    = island_key_a.map (|key| &islands[key as usize]);
    let island_b    = island_key_b.map (|key| &islands[key as usize]);
    island_a.map (|island| dynamic_ids.extend (island.dynamic_ids().into_vec()));
    island_b.map (|island| dynamic_ids.extend (island.dynamic_ids().into_vec()));
    // track whether a dynamic object was modified; if so it will need to be removed
    // from intermediate results in the pipeline and checked for new collisions
    let mut dynamic_modified = SortedSet::new();
    // 1. lerp to TOI
    let lerp_objects = |objects_dynamic : &mut VecMap <object::Dynamic>, time_delta|
      for id in dynamic_ids.iter() {
        let index          = id.key().index();
        let object         = &mut objects_dynamic[index];
        let pseudovelocity = constraint_manager.pseudo_velocities[index];
        object::lerp (object, pseudovelocity, time_delta);
      };
    let (t_reverse, t_forward) = {
      let toi_f64 : f64 = *toi;   // [0.0, 1.0]
      (-1.0 + toi_f64, 1.0 - toi_f64)
    };
    lerp_objects (objects_dynamic, t_reverse);
    let toi_aabbs = dynamic_ids.iter().map (|id|{
      let index = id.key().index();
      objects_dynamic[index].aabb_dilated()
    }).collect::<Vec <_>>();
    // 2. constrain velocities
    let constrain_velocity = |
      objects_dynamic  : &mut VecMap <object::Dynamic>,
      pseudovelocities : &VecMap <Vector3 <f64>>,
      constraint       : constraint::Variant,
      colliding        : bool,
      object_id_a      : InternalId,
      object_id_b      : InternalId
    | {
      let key_a    = object_id_a.key();
      let key_b    = object_id_b.key();
      let index_a  = key_a.index();
      let index_b  = key_b.index();
      let (mut object_a, object_a_restitution) = match object_id_a.kind() {
        object::Kind::Static   => {
          let object      = objects_static[index_a].clone();
          let restitution = object.material.restitution;
          (Either::Left (object), restitution)
        }
        object::Kind::Dynamic  => {
          let object      = objects_dynamic[index_a].clone();
          let restitution = object.material.restitution;
          (Either::Right (object), restitution)
        }
        object::Kind::Nodetect => unreachable!()
      };
      debug_assert_eq!(object_id_b.kind(), object::Kind::Dynamic);
      let mut object_b = objects_dynamic[index_b].clone();
      let restitution  = if colliding {
        object_a_restitution * object_b.material.restitution
      } else {
        Normalized::zero()
      };
      let pseudovelocity_a = if object_id_a.kind() == object::Kind::Dynamic {
        Some (pseudovelocities[index_a])
      } else {
        None
      };
      let pseudovelocity_b = pseudovelocities[index_b];
      let maybe_impulses = match constraint {
        constraint::Variant::Contact (contact) =>
          contact.constrain_velocity_with_restitution (
            object_a.as_mut().map_left (|object_static| &*object_static),
            &mut object_b, pseudovelocity_a, pseudovelocity_b, restitution),
        constraint::Variant::Distance (distance) => distance.constrain_velocity (
          object_a.as_mut().map_left (|object_static| &*object_static),
          &mut object_b, pseudovelocity_a, pseudovelocity_b)
      };
      if maybe_impulses.is_some() {
        match object_a {
          Either::Right (object_a) => objects_dynamic[index_a] = object_a,
          Either::Left  (_)        => {}
        }
        objects_dynamic[index_b] = object_b;
      }
      maybe_impulses
    };
    let mut impulse_normal_a  = Vector3::zero();
    let mut impulse_normal_b  = Vector3::zero();
    let mut impulse_tangent_a = Vector3::zero();
    let mut impulse_tangent_b = Vector3::zero();
    let mut iter = 0;
    loop {
      let mut satisfied = true;
      if let Some (impulses) = constrain_velocity (
        objects_dynamic, &constraint_manager.pseudo_velocities, collision.clone().into(),
        true, object_id_a, object_id_b
      ) {
        impulse_normal_a  += impulses.impulse_normal_a;
        impulse_normal_b  += impulses.impulse_normal_b;
        impulse_tangent_a += impulses.impulse_tangent_a;
        impulse_tangent_b += impulses.impulse_tangent_b;
      }
      let mut constrain_island = |island : &constraint::Island| {
        for (object_pair, constraint) in island.constraints.iter().cloned() {
          let (object_id_a, object_id_b) = object_pair.into();
          if constrain_velocity (
            objects_dynamic, &constraint_manager.pseudo_velocities, constraint, false,
            object_id_a, object_id_b
          ).is_some() {
            if object_id_a.kind() == object::Kind::Dynamic {
              dynamic_modified.push (object_id_a);
            }
            dynamic_modified.push (object_id_b);
            satisfied = false;
          }
        }
      };
      if let Some (island) = island_a {
        constrain_island (island);
      }
      if let Some (island) = island_b {
        constrain_island (island);
      }
      iter += 1;
      if satisfied {
        log::debug!(iters=iter; "collision velocity constraint satisfied");
        break
      }
    }
    // 3. lerp to T==1.0
    lerp_objects (objects_dynamic, t_forward);
    // 4. constrain positions
    let constrain_position = |
      objects_dynamic  : &mut VecMap <object::Dynamic>,
      pseudovelocities : &mut VecMap <Vector3 <f64>>,
      constraint       : &mut constraint::Variant,
      object_id_a      : InternalId,
      object_id_b      : InternalId
    | {
      let key_a    = object_id_a.key();
      let key_b    = object_id_b.key();
      let index_a  = key_a.index();
      let index_b  = key_b.index();
      let mut object_a = match object_id_a.kind() {
        object::Kind::Static   => Either::Left  (objects_static[index_a].clone()),
        object::Kind::Dynamic  => Either::Right (objects_dynamic[index_a].clone()),
        object::Kind::Nodetect => unreachable!()
      };
      debug_assert_eq!(object_id_b.kind(), object::Kind::Dynamic);
      let mut object_b = objects_dynamic[index_b].clone();
      let mut pseudovelocity_a = if object_id_a.kind() == object::Kind::Dynamic {
        Some (pseudovelocities[index_a])
      } else {
        None
      };
      let mut pseudovelocity_b = pseudovelocities[index_b];
      let maybe_impulses = constraint.constrain_position (
        t_reverse, object_a.as_mut().map_left (|object_static| &*object_static),
        &mut object_b, pseudovelocity_a.as_mut(), &mut pseudovelocity_b);
      if maybe_impulses.is_some() {
        match object_a {
          Either::Right (object_a) => {
            objects_dynamic[index_a]  = object_a;
            pseudovelocities[index_a] = pseudovelocity_a.unwrap();
          }
          Either::Left  (_) => {}
        }
        objects_dynamic[index_b]  = object_b;
        pseudovelocities[index_b] = pseudovelocity_b;
      }
      maybe_impulses
    };
    // contact index and final proximity result
    let mut pseudo_impulse_a  = Vector3::zero();
    let mut pseudo_impulse_b  = Vector3::zero();
    let mut iter = 0;
    let mut collision_constraint = constraint::Variant::from (collision.clone());
    loop {
      let mut satisfied = true;
      let maybe_impulses = constrain_position (
        objects_dynamic, &mut constraint_manager.pseudo_velocities,
        &mut collision_constraint, object_id_a, object_id_b);
      if let Some (impulses) = maybe_impulses {
        pseudo_impulse_a += impulses.pseudo_impulse_a;
        pseudo_impulse_b += impulses.pseudo_impulse_b;
      }
      let mut constrain_island = |island : &mut constraint::Island|
        for (object_pair, constraint) in island.constraints.iter_mut() {
          let (object_id_a, object_id_b) = (*object_pair).into();
          let maybe_impulses = constrain_position (
            objects_dynamic, &mut constraint_manager.pseudo_velocities, constraint,
            object_id_a, object_id_b
          );
          if maybe_impulses.is_some() {
            if object_id_a.kind() == object::Kind::Dynamic {
              dynamic_modified.push (object_id_a);
            }
            dynamic_modified.push (object_id_b);
            satisfied = false;
          }
        };
      if let Some (key) = island_key_a {
        constrain_island (&mut islands[key as usize]);
      }
      if let Some (key) = island_key_b {
        constrain_island (&mut islands[key as usize]);
      }
      iter += 1;
      if satisfied {
        log::debug!(iters=iter; "collision position constraint satisfied");
        break
      }
    }
    constraint_manager.islands = Some (islands);
    // 5. create or destroy persistent contacts from final proximity check;
    // also add resolved pairs to the broad phase here since we are iterating over
    // constraint islands one last time
    let object_pair = (object_id_a, object_id_b).into();
    if !impulse_normal_b.is_zero() || !impulse_tangent_b.is_zero() ||
      !pseudo_impulse_b.is_zero()
    {
      if object_id_a.kind() == object::Kind::Dynamic {
        debug_assert!(!impulse_normal_a.is_zero() ||
          !impulse_tangent_a.is_zero() || !pseudo_impulse_a.is_zero());
        dynamic_modified.push (object_id_a);
      }
      dynamic_modified.push (object_id_b);
    }
    self.broad.add_resolved (object_pair);
    // wait until after updating islands to create the new contact in case the islands
    // are merged by adding the contact, which would invalidate one of the old island
    // keys
    let mut update_island = |island_key : constraint::island::KeyType| {
      let mut islands = constraint_manager.islands.take().unwrap();
      let island = &mut islands[island_key as usize];
      let mut remove_contacts = vec![];
      for (constraint_index, (object_pair, contact)) in island.constraints.iter()
        .enumerate().filter_map (|(index, (pair, constraint))|
          if let constraint::Variant::Contact (contact) = constraint {
            Some ((index, (pair, contact)))
          } else {
            None
          }
        )
      {
        self.broad.add_resolved (*object_pair);
        if contact.distance >= CONTACT_DISTANCE {
          remove_contacts.push (constraint_index as u32)
        }
      }
      if !remove_contacts.is_empty() {
        constraint_manager.remove_constraints (island, remove_contacts.as_slice());
        if island.constraints.is_empty() {
          let _ = islands.take (island_key as usize).unwrap();
        } else {
          let partitions = island.partition();
          if !partitions.is_empty() {
            let _ = islands.take (island_key as usize).unwrap();
            for new_island in partitions {
              let dynamic_ids = new_island.dynamic_ids();
              let island_key  = islands.put (new_island) as constraint::island::KeyType;
              for dynamic_id in dynamic_ids.into_vec() {
                constraint_manager.change_dynamic_island_key (dynamic_id, island_key);
              }
            }
          }
        }
      }
      constraint_manager.islands = Some (islands);
    };
    if let Some (island_key) = island_key_a {
      update_island (island_key);
    }
    if let Some (island_key) = island_key_b {
      update_island (island_key);
    }
    let constraint::Variant::Contact (contact) = collision_constraint
      else { unreachable!() };
    if contact.distance < CONTACT_DISTANCE {
      debug_assert!(contact.distance >= 0.0);
      log::trace!(object_pair:?, contact:?; "creating contact @ T==1.0");
      constraint_manager.add_constraint (object_pair, contact.into());
    }
    // 6. update AABBs in broad pipeline;
    // also remove resolved dynamic objects from intermediate pipeline results
    for id in dynamic_modified.iter() {
      let i = dynamic_ids.binary_search_by_key (&id, |x| x).unwrap();
      let key    = id.key();
      let index  = key.index();
      let object = &objects_dynamic[index];
      let aabb_discrete   = object.aabb_dilated();
      let aabb_continuous = Aabb3::union (toi_aabbs[i], aabb_discrete);
      self.broad.update_aabb_dynamic_discrete (aabb_discrete, key);
      self.broad.update_aabb_dynamic_continuous (aabb_continuous, key);
    }
    self.pipeline.remove_resolved_dynamic (&dynamic_modified);
    // TODO: update the modified set in-place ?
    self.broad.set_modified (dynamic_modified);
    // result
    let object_id_a = object_id_a.into();
    let object_id_b = object_id_b.into();
    event::CollisionResolve {
      toi, collision,
      object_id_a,       object_id_b,
      impulse_normal_a,  impulse_normal_b,
      impulse_tangent_a, impulse_tangent_b,
      pseudo_impulse_a,  pseudo_impulse_b
    }
  }

  /// Detects AABB overlap pairs of static/dynamic and dynamic/dynamic objects.
  ///
  /// Excludes Nocollide pairs that were already detected but does not filter
  /// object pairs that are already in persistent contacts.
  #[inline]
  fn broad_overlap_pairs_continuous (&mut self) {
    self.broad.overlap_pairs_continuous (
      &mut self.pipeline.broad_overlap_pairs, self.pipeline.detect_resolve_iter);
    // exclude nocollide pairs that were already detected
    self.pipeline.broad_overlap_pairs.retain (
      |pair|{
        let (id_a, id_b) = (*pair).into();
        let id_a = object::Id::from (id_a);
        let id_b = object::Id::from (id_b);
        for event::Overlap { object_id_a, object_id_b } in
          self.pipeline.overlaps.iter()
        {
          if object_id_a == &id_a && object_id_b == &id_b {
            return false
          }
        }
        true
      }
    );
    log::trace!(overlap_pairs:?=self.pipeline.broad_overlap_pairs;
      "broad overlap pairs");
  }

  /// Collects swept AABB TOIs from broad overlap pairs.
  ///
  /// Filters pairs that are already in persistent contact.
  ///
  /// Note that the algorithm will aim for a distance of `0.5 * CONTACT_DISTANCE` at
  /// each TOI.
  fn mid_toi_pairs (&mut self,
    constraint_manager : &constraint::Manager,
    objects_dynamic    : &VecMap <object::Dynamic>
  ) {
    use sorted_vec::FindOrInsert;
    let last_toi = self.pipeline.resolved_collisions.last()
      .map_or (0.0, |collision_resolve| *collision_resolve.toi);
    'outer: for object_pair in self.pipeline.broad_overlap_pairs.drain (..) {
      let (object_id_a, object_id_b) = object_pair.into();
      debug_assert_eq!(object_id_b.kind(), object::Kind::Dynamic);
      match object_id_a.kind() {
        //
        //  static v. dynamic
        //
        object::Kind::Static => {
          let id_static     = object::Id::from (object_id_a);
          let id_dynamic    = object::Id::from (object_id_b);
          let key_static    = id_static.key;
          let key_dynamic   = id_dynamic.key;
          let _index_static = key_static.index();
          let index_dynamic = key_dynamic.index();
          if let (Some (static_contact_count), Some (dynamic_island_key)) = (
            constraint_manager.get_constraint_count (object_id_a),
            constraint_manager.get_island_key (object_id_b)
          ) {
            debug_assert!(static_contact_count > 0);
            let island = constraint_manager.get_island (dynamic_island_key).unwrap();
            for (pair, _) in island.constraints.iter() {
              if *pair == object_pair {
                continue 'outer
              }
            }
          }
          // should be safe to unwrap
          let dynamic_object         = &objects_dynamic[index_dynamic];
          let dynamic_velocity       = dynamic_object.derivatives.velocity;
          let dynamic_pseudovelocity = constraint_manager.pseudo_velocities[index_dynamic];
          let dynamic_velocity_effective = dynamic_velocity + dynamic_pseudovelocity;
          let static_aabb            = self.broad.get_aabb_static (key_static);
          let dynamic_aabb_current   = self.broad
            .get_aabb_dynamic_discrete (key_dynamic);
          let _dynamic_aabb_swept    = self.broad
            .get_aabb_dynamic_continuous (key_dynamic);
          let dynamic_aabb_previous  = Aabb3::with_minmax_unchecked (
            dynamic_aabb_current.min() - dynamic_velocity_effective,
            dynamic_aabb_current.max() - dynamic_velocity_effective);
          // NB: div by zero will result in `inf` values
          let dynamic_velocity_effective_reciprocal = dynamic_velocity_effective.recip();
          // these values adjust start/end times to avoid getting too close
          let t_margin_x =
            (0.5 * CONTACT_DISTANCE * dynamic_velocity_effective_reciprocal.x).abs();
          let t_margin_y =
            (0.5 * CONTACT_DISTANCE * dynamic_velocity_effective_reciprocal.y).abs();
          let t_margin_z =
            (0.5 * CONTACT_DISTANCE * dynamic_velocity_effective_reciprocal.z).abs();

          let (interval_start_x, interval_end_x) = if
            dynamic_velocity_effective.x == 0.0
          {
            debug_assert!(
              dynamic_aabb_current.max().0.x > static_aabb.min().0.x &&
              static_aabb.max().0.x > dynamic_aabb_current.min().0.x);
            debug_assert!(
              dynamic_aabb_previous.max().0.x > static_aabb.min().0.x &&
              static_aabb.max().0.x > dynamic_aabb_previous.min().0.x);
            (f64::NEG_INFINITY, f64::INFINITY)
          } else if dynamic_velocity_effective.x > 0.0 {
            ( //(static_aabb.min().0.x - dynamic_aabb_previous.max().0.x) *
              //  dynamic_velocity_effective_reciprocal.x - t_margin_x,
              (static_aabb.min().0.x - dynamic_aabb_previous.max().0.x)
                .mul_add (dynamic_velocity_effective_reciprocal.x, -t_margin_x),
              //(static_aabb.max().0.x - dynamic_aabb_previous.min().0.x) *
              //  dynamic_velocity_effective_reciprocal.x + t_margin_x
              (static_aabb.max().0.x - dynamic_aabb_previous.min().0.x)
                .mul_add (dynamic_velocity_effective_reciprocal.x, t_margin_x)
            )
          } else {
            debug_assert!(dynamic_velocity_effective.x < 0.0);
            ( //(static_aabb.max().0.x - dynamic_aabb_previous.min().0.x) *
              //  dynamic_velocity_effective_reciprocal.x - t_margin_x,
              (static_aabb.max().0.x - dynamic_aabb_previous.min().0.x)
                .mul_add (dynamic_velocity_effective_reciprocal.x, -t_margin_x),
              //(static_aabb.min().0.x - dynamic_aabb_previous.max().0.x) *
              //  dynamic_velocity_effective_reciprocal.x + t_margin_x
              (static_aabb.min().0.x - dynamic_aabb_previous.max().0.x)
                .mul_add (dynamic_velocity_effective_reciprocal.x, t_margin_x)
            )
          };

          let (interval_start_y, interval_end_y) = if
            dynamic_velocity_effective.y == 0.0
          {
            debug_assert!(
              dynamic_aabb_current.max().0.y > static_aabb.min().0.y &&
              static_aabb.max().0.y > dynamic_aabb_current.min().0.y);
            debug_assert!(
              dynamic_aabb_previous.max().0.y > static_aabb.min().0.y &&
              static_aabb.max().0.y > dynamic_aabb_previous.min().0.y);
            (f64::NEG_INFINITY, f64::INFINITY)
          } else if dynamic_velocity_effective.y > 0.0 {
            ( //dynamic_velocity_effective_reciprocal.y *
              //  (static_aabb.min().0.y - dynamic_aabb_previous.max().0.y) - t_margin_y,
              dynamic_velocity_effective_reciprocal.y.mul_add (
                static_aabb.min().0.y - dynamic_aabb_previous.max().0.y, -t_margin_y),
              //dynamic_velocity_effective_reciprocal.y *
              //  (static_aabb.max().0.y - dynamic_aabb_previous.min().0.y) + t_margin_y
              dynamic_velocity_effective_reciprocal.y.mul_add (
                static_aabb.max().0.y - dynamic_aabb_previous.min().0.y, t_margin_y)
            )
          } else {
            debug_assert!(dynamic_velocity_effective.y < 0.0);
            ( //dynamic_velocity_effective_reciprocal.y *
              //  (static_aabb.max().0.y - dynamic_aabb_previous.min().0.y) - t_margin_y,
              dynamic_velocity_effective_reciprocal.y.mul_add (
                static_aabb.max().0.y - dynamic_aabb_previous.min().0.y, -t_margin_y),
              //dynamic_velocity_effective_reciprocal.y *
              //  (static_aabb.min().0.y - dynamic_aabb_previous.max().0.y) + t_margin_y
              dynamic_velocity_effective_reciprocal.y.mul_add (
                static_aabb.min().0.y - dynamic_aabb_previous.max().0.y, t_margin_y)
            )
          };

          let (interval_start_z, interval_end_z) = if
            dynamic_velocity_effective.z == 0.0
          {
            debug_assert!(
              dynamic_aabb_current.max().0.z > static_aabb.min().0.z &&
              static_aabb.max().0.z > dynamic_aabb_current.min().0.z);
            debug_assert!(
              dynamic_aabb_previous.max().0.z > static_aabb.min().0.z &&
              static_aabb.max().0.z > dynamic_aabb_previous.min().0.z);
            (f64::NEG_INFINITY, f64::INFINITY)
          } else if dynamic_velocity_effective.z > 0.0 {
            ( //dynamic_velocity_effective_reciprocal.z *
              //  (static_aabb.min().0.z - dynamic_aabb_previous.max().0.z) - t_margin_z,
              dynamic_velocity_effective_reciprocal.z.mul_add (
                static_aabb.min().0.z - dynamic_aabb_previous.max().0.z, -t_margin_z),
              //dynamic_velocity_effective_reciprocal.z *
              //  (static_aabb.max().0.z - dynamic_aabb_previous.min().0.z) + t_margin_z
              dynamic_velocity_effective_reciprocal.z.mul_add (
                static_aabb.max().0.z - dynamic_aabb_previous.min().0.z, t_margin_z)
            )
          } else {
            debug_assert!(dynamic_velocity_effective.z < 0.0);
            ( //dynamic_velocity_effective_reciprocal.z *
              //  (static_aabb.max().0.z - dynamic_aabb_previous.min().0.z) - t_margin_z,
              dynamic_velocity_effective_reciprocal.z.mul_add (
                static_aabb.max().0.z - dynamic_aabb_previous.min().0.z, -t_margin_z),
              //dynamic_velocity_effective_reciprocal.z *
              //  (static_aabb.min().0.z - dynamic_aabb_previous.max().0.z) + t_margin_z
              dynamic_velocity_effective_reciprocal.z.mul_add (
                static_aabb.min().0.z - dynamic_aabb_previous.max().0.z, t_margin_z)
            )
          };

          let interval = {
            let interval_xy =
              if interval_start_x < interval_end_y && interval_start_y < interval_end_x {
                Some ((
                  f64::max (interval_start_x, interval_start_y),
                  f64::min (interval_end_x, interval_end_y)
                ))
              } else {
                None
              };
            if let Some ((interval_start_xy, interval_end_xy)) = interval_xy {
              if interval_start_xy < interval_end_z &&
                 interval_start_z  < interval_end_xy
              {
                Some ((
                  f64::max (interval_start_xy, interval_start_z),
                  f64::min (interval_end_xy, interval_end_z)
                ))
              } else {
                None
              }
            } else {
              None
            }
          };

          if let Some ((interval_start, interval_end)) = interval &&
            interval_start < 1.0 && 0.0 < interval_end
          {
            let mid_toi_pair = (
              Normalized::clamp (f64::max (interval_start, last_toi)),
              Normalized::clamp (interval_end),
              object_pair
            );
            match self.pipeline.mid_toi_pairs.find_or_insert (Reverse (mid_toi_pair)) {
              FindOrInsert::Inserted (_) => {}
              FindOrInsert::Found    (_) => unreachable!()
            }
          }
        }
        //
        //  dynamic v. dynamic
        //
        object::Kind::Dynamic => {
          let key_a   = object_id_a.key();
          let key_b   = object_id_b.key();
          let index_a = key_a.index();
          let index_b = key_b.index();
          if let (Some (island_key_a), Some (island_key_b)) = (
            constraint_manager.get_island_key (object_id_a),
            constraint_manager.get_island_key (object_id_b)
          ) && island_key_a == island_key_b {
            let island = constraint_manager.get_island (island_key_a).unwrap();
            for (pair, _) in island.constraints.iter() {
              if *pair == object_pair {
                continue 'outer
              }
            }
          }
          // should be safe to unwrap: objects should not be destroyed during
          // collision detection
          let object_a          = &objects_dynamic[index_a];
          let object_b          = &objects_dynamic[index_b];
          let velocity_a        = object_a.derivatives.velocity;
          let velocity_b        = object_b.derivatives.velocity;
          let pseudovelocity_a  = constraint_manager.pseudo_velocities[key_a.index()];
          let pseudovelocity_b  = constraint_manager.pseudo_velocities[key_b.index()];
          let velocity_effective_a = velocity_a + pseudovelocity_a;
          let velocity_effective_b = velocity_b + pseudovelocity_b;
          let velocity_effective_relative =
            velocity_effective_a - velocity_effective_b;
          let velocity_effective_relative_reciprocal =
            velocity_effective_relative.recip();
          let aabb_current_a    = self.broad.get_aabb_dynamic_discrete   (key_a);
          let aabb_current_b    = self.broad.get_aabb_dynamic_discrete   (key_b);
          let _aabb_swept_a     = self.broad.get_aabb_dynamic_continuous (key_a);
          let _aabb_swept_b     = self.broad.get_aabb_dynamic_continuous (key_b);
          let aabb_previous_a   = Aabb3::with_minmax_unchecked (
            aabb_current_a.min() - velocity_effective_a,
            aabb_current_a.max() - velocity_effective_a);
          let aabb_previous_b   = Aabb3::with_minmax_unchecked (
            aabb_current_b.min() - velocity_effective_b,
            aabb_current_b.max() - velocity_effective_b);

          // compute overlap time start/end interval for the given axis
          let overlap_interval_axis = |axis : Axis3| {
            let axis_index = axis as usize;
            // div by zero will result in `inf` values; t_margin adjusts
            // start/end times to avoid getting too close
            let t_margin = (0.5 * CONTACT_DISTANCE *
              velocity_effective_relative_reciprocal[axis_index]).abs();
            let velocity_relative = velocity_effective_relative[axis_index];
            let velocity_relative_reciprocal =
              velocity_effective_relative_reciprocal[axis_index];
            let prev_a_min = aabb_previous_a.min().0[axis_index];
            let prev_a_max = aabb_previous_a.max().0[axis_index];
            let prev_b_min = aabb_previous_b.min().0[axis_index];
            let prev_b_max = aabb_previous_b.max().0[axis_index];
            if velocity_relative == 0.0 {
              // NOTE: even though the relative velocity is zero, the previous AABBs may
              // not be overlapping due to rounding errors; we will still consider the
              // begin/end time to be infinite
              geometry::Interval::with_minmax_unchecked (
                f64::NEG_INFINITY, f64::INFINITY)
            } else if velocity_relative > 0.0 {
              geometry::Interval::with_minmax_unchecked (
                //(prev_b_min - prev_a_max) * velocity_relative_reciprocal - t_margin,
                (prev_b_min - prev_a_max)
                  .mul_add (velocity_relative_reciprocal, -t_margin),
                //(prev_b_max - prev_a_min) * velocity_relative_reciprocal + t_margin
                (prev_b_max - prev_a_min)
                  .mul_add (velocity_relative_reciprocal, t_margin)
              )
            } else {
              debug_assert!(velocity_relative < 0.0);
              geometry::Interval::with_minmax_unchecked (
                //(prev_b_max - prev_a_min) * velocity_relative_reciprocal - t_margin,
                (prev_b_max - prev_a_min)
                  .mul_add (velocity_relative_reciprocal, -t_margin),
                //(prev_b_min - prev_a_max) * velocity_relative_reciprocal + t_margin
                (prev_b_min - prev_a_max)
                  .mul_add (velocity_relative_reciprocal, t_margin)
              )
            }
          };
          let overlap_interval_x = overlap_interval_axis (Axis3::X);
          let overlap_interval_y = overlap_interval_axis (Axis3::Y);
          let overlap_interval_z = overlap_interval_axis (Axis3::Z);

          if let Some (interval) = overlap_interval_x.intersection (overlap_interval_y)
            .and_then (|overlap_interval_xy|
              overlap_interval_xy.intersection (overlap_interval_z))
            && interval.min() < 1.0 && 0.0 < interval.max()
          {
            let mid_toi_pair = (
              Normalized::clamp (f64::max (interval.min(), last_toi)),
              Normalized::clamp (interval.max()),
              object_pair
            );
            match self.pipeline.mid_toi_pairs.find_or_insert (Reverse (mid_toi_pair)) {
              FindOrInsert::Inserted (_) => {}
              FindOrInsert::Found    (_) => unreachable!()
            }
          }
        }
        object::Kind::Nodetect => unreachable!()
      }
    }

    log::trace!(mid_toi_pairs:?=self.pipeline.mid_toi_pairs; "mid TOI pairs");

    self.pipeline.broad_overlap_pairs.clear();
  }

  /// Returns narrow TOI pairs.
  ///
  /// Note that the algorithm will aim for a distance of `0.5 * CONTACT_DISTANCE` at
  /// each TOI.
  fn narrow_toi_contacts (&mut self,
    objects_static    : &VecMap <object::Static>,
    objects_dynamic   : &VecMap <object::Dynamic>,
    pseudo_velocities : &VecMap <Vector3 <f64>>
  ) {
    use sorted_vec::FindOrInsert;

    let narrow_toi_contacts = &mut self.pipeline.narrow_toi_contacts;
    while let Some (mid_toi_pair) = self.pipeline.mid_toi_pairs.last().copied() {
      let earliest_toi_contact = if !narrow_toi_contacts.is_empty() {
        narrow_toi_contacts[0].0.0
      } else {
        Normalized::clamp (1.0)
      };

      if mid_toi_pair.0.0 < earliest_toi_contact {
        let (mid_toi_start, _mid_toi_end, object_pair) = mid_toi_pair.0;
        let (object_id_a, object_id_b) = object_pair.into();
        let kind_a  = object_id_a.kind();
        let kind_b  = object_id_b.kind();
        match (kind_a, kind_b) {
          (object::Kind::Static, object::Kind::Dynamic) => {
            let key_static    = object_id_a.key();
            let key_dynamic   = object_id_b.key();
            let index_static  = key_static.index();
            let index_dynamic = key_dynamic.index();
            // safe to unwrap
            let mut dynamic_object         = objects_dynamic[index_dynamic].clone();
            let dynamic_velocity           = dynamic_object.derivatives.velocity;
            let dynamic_pseudovelocity     = pseudo_velocities[index_dynamic];
            let dynamic_velocity_effective = dynamic_velocity - dynamic_pseudovelocity;
            let static_object              = objects_static[index_static].clone();
            let mut narrow_toi             = *mid_toi_start;
            // rewind object to the narrow_toi: the object is currently at
            // t==1.0, so we are lerping a negative value
            object::lerp (&mut dynamic_object, dynamic_pseudovelocity, -1.0 + narrow_toi);
            let mut t_remaining = 1.0 - narrow_toi;
            #[expect(clippy::used_underscore_binding)]
            let mut _iter       = 0;
            loop {
              #[cfg(debug_assertions)]
              if self.narrow_toi_max_iter_count < _iter {
                self.narrow_toi_max_iter_count = _iter;
              }
              let proximity = Proximity::query (&static_object, &dynamic_object);
              if cfg!(debug_assertions) &&
                dynamic_object.collidable && static_object.collidable
              {
                debug_assert!(proximity.distance >= 0.0,
                  "objects should not be overlapping, distance: {}", proximity.distance);
              }
              let dynamic_velocity_effective_normal =
                dynamic_velocity_effective.dot (-*proximity.normal);
              if dynamic_velocity_effective_normal > 0.0 &&
                dynamic_object.collidable && static_object.collidable
              {
                // no collision: separating velocity zero or positive
                log::trace!(
                  t=narrow_toi, velocity=dynamic_velocity_effective_normal;
                  "collision rejected, separating velocity");
                break
              } else if proximity.distance < CONTACT_DISTANCE {
                debug_assert!(dynamic_velocity_effective_normal <= 0.0 ||
                  !dynamic_object.collidable || !static_object.collidable);
                // collision: toi contact
                log::trace!(t=narrow_toi, proximity:?; "collision detected");
                // TODO: it appears that sometimes the proximity distance can be
                // negative here, is that correct?
                // let collision = constraint::Contact::try_from (proximity).unwrap();
                let collision = constraint::Contact {
                  normal:   proximity.normal,
                  distance: proximity.distance
                };
                let narrow_toi_contact = Reverse ((
                  Normalized::noisy (narrow_toi), object_pair, collision
                ));
                match narrow_toi_contacts.find_or_insert (narrow_toi_contact) {
                  FindOrInsert::Inserted (_) => {}
                  FindOrInsert::Found    (_) => unreachable!()
                }
                break
              } else if dynamic_velocity_effective_normal == 0.0 {
                log::trace!(
                  t=narrow_toi, velocity=dynamic_velocity_effective_normal;
                  "collision rejected, zero relative velocity and objects not \
                  in contact");
                break
              }
              let dynamic_velocity_effective_normal_reciprocal =
                1.0 / dynamic_velocity_effective_normal;
              let toi_axis =
                //narrow_toi +
                //(proximity.distance - 0.5 * CONTACT_DISTANCE) *
                //(-dynamic_velocity_effective_normal_reciprocal);
                0.5f64.mul_add (-CONTACT_DISTANCE, proximity.distance)
                  .mul_add (-dynamic_velocity_effective_normal_reciprocal, narrow_toi);
              debug_assert!(toi_axis > narrow_toi ||
                !dynamic_object.collidable || !static_object.collidable);
              if toi_axis > 1.0 || toi_axis < 0.0 {
                // no collision this step
                log::trace!(t=narrow_toi, proximity:?;
                  "collision rejected, final proximity");
                break
              }
              let t_advance = toi_axis - narrow_toi;
              object::lerp (&mut dynamic_object, dynamic_pseudovelocity, t_advance);
              narrow_toi    = toi_axis;
              t_remaining   -= t_advance;
              debug_assert!(t_remaining > 0.0);
              _iter += 1;
            }
          }

          (object::Kind::Dynamic, object::Kind::Dynamic) => {
            let key_a   = object_id_a.key();
            let key_b   = object_id_b.key();
            let index_a = key_a.index();
            let index_b = key_b.index();
             // safe to unwrap: objects should not be destroyed during collision
             // detection
            let mut object_a         = objects_dynamic[index_a].clone();
            let mut object_b         = objects_dynamic[index_b].clone();
            let pseudovelocity_a     = pseudo_velocities[index_a];
            let pseudovelocity_b     = pseudo_velocities[index_b];
            let velocity_a           = object_a.derivatives.velocity;
            let velocity_b           = object_b.derivatives.velocity;
            let velocity_effective_a = velocity_a + pseudovelocity_a;
            let velocity_effective_b = velocity_b + pseudovelocity_b;
            let velocity_effective_relative =
              velocity_effective_a - velocity_effective_b;
            let mut narrow_toi  = *mid_toi_start;
            let mut t_remaining = 1.0 - narrow_toi;
            object::lerp (&mut object_a, pseudovelocity_a, -1.0 + narrow_toi);
            object::lerp (&mut object_b, pseudovelocity_b, -1.0 + narrow_toi);
            loop {
              debug_assert!(narrow_toi <= 1.0);
              debug_assert!(t_remaining >= 0.0);
              let proximity = Proximity::query (&object_a, &object_b);
              if cfg!(debug_assertions) && object_a.collidable && object_b.collidable {
                debug_assert!(proximity.distance >= 0.0);
              }
              let velocity_effective_relative_normal =
                velocity_effective_relative.dot (*proximity.normal);
              if velocity_effective_relative_normal > 0.0 &&
                object_a.collidable && object_b.collidable
              {
                // no collision: separating velocity zero or positive
                log::trace!(
                  t=narrow_toi, velocity=velocity_effective_relative_normal;
                  "collision rejected, separating velocity");
                break
              } else if proximity.distance < CONTACT_DISTANCE {
                debug_assert!(velocity_effective_relative_normal <= 0.0 ||
                  !object_a.collidable || !object_b.collidable);
                // collision: toi contact
                log::trace!(t=narrow_toi, proximity:?; "collision detected");
                let contact = constraint::Contact::try_from (proximity).unwrap();
                let narrow_toi_contact = Reverse ((
                  Normalized::noisy (narrow_toi), object_pair, contact
                ));
                match narrow_toi_contacts.find_or_insert (narrow_toi_contact) {
                  FindOrInsert::Inserted (_) => {}
                  FindOrInsert::Found    (_) => unreachable!()
                }
                break
              } else if velocity_effective_relative_normal == 0.0 {
                log::trace!(
                  t=narrow_toi, velocity=velocity_effective_relative_normal;
                  "collision rejected, zero relative velocity and objects not in \
                  contact");
                break
              }
              let velocity_effective_relative_normal_reciprocal =
                1.0 / velocity_effective_relative_normal;
              let toi_axis =
                //narrow_toi +
                //(proximity.distance - 0.5 * CONTACT_DISTANCE) *
                //(-velocity_effective_relative_normal_reciprocal);
                0.5f64.mul_add (-CONTACT_DISTANCE, proximity.distance)
                  .mul_add (-velocity_effective_relative_normal_reciprocal, narrow_toi);
              debug_assert!(toi_axis > narrow_toi ||
                !object_a.collidable || !object_b.collidable);
              if toi_axis > 1.0 || toi_axis < 0.0 {
                // no collision this step
                log::trace!(t=narrow_toi, proximity:?;
                  "collision rejected, final proximity");
                break
              }
              let t_advance = toi_axis - narrow_toi;
              object::lerp (&mut object_a, pseudovelocity_a, t_advance);
              object::lerp (&mut object_b, pseudovelocity_b, t_advance);
              narrow_toi    = toi_axis;
              t_remaining   -= t_advance;
            }
          }
          _ => unreachable!()
        }
        self.pipeline.mid_toi_pairs.pop();
      } else { break }
    }
    #[cfg(debug_assertions)]
    log::debug!(max_iter_count=self.narrow_toi_max_iter_count;
      "narrow TOI max iters");
    log::trace!(contacts:?=narrow_toi_contacts; "narrow TOI contacts");
  }

  #[inline]
  fn add_object_static (&mut self,
    object     : &object::Static,
    object_key : object::Key,
  ) {
    self.broad.add_object_static (object.aabb_dilated(), object_key);
  }

  #[inline]
  fn add_object_dynamic (&mut self,
    pseudo_velocities : &mut VecMap <Vector3 <f64>>,
    object            : &object::Dynamic,
    object_key        : object::Key
  ) {
    let aabb_discrete   = object.aabb_dilated();
    let aabb_continuous = Aabb3::with_minmax_unchecked (
      aabb_discrete.min() - object.derivatives.velocity,
      aabb_discrete.max() - object.derivatives.velocity);
    // NOTE: this swept AABB should not be seen by the next continuous collision
    // detection/resolution loop since `begin_step()` will re-calculate for the next
    // frame
    self.broad.add_object_dynamic (aabb_discrete, aabb_continuous, object_key);
    assert!(pseudo_velocities.insert (object_key.index(), Vector3::zero()).is_none());
  }

}

impl Pipeline {
  #[inline]
  fn is_empty (&self) -> bool {
    self.broad_overlap_pairs.is_empty() &&
    self.mid_toi_pairs.is_empty() &&
    self.narrow_toi_contacts.is_empty() &&
    self.resolved_collisions.is_empty() &&
    self.overlaps.is_empty()
  }

  /// After a collision is resolved, all modified (dynamic) objects should be removed
  /// from intermediate results in the pipeline:
  ///
  /// - `mid_toi_pairs`
  /// - `narrow_toi_contacts`
  ///
  /// These objects will be checked for overlaps in the broad phase of the next
  /// detect/resolve loop iteration.
  // TODO: because we are only removing resolved dynamic objects and the resolved list
  // is sorted, we could be slicing the resolved set to only contain the dynamic ids and
  // exclude the static ids from binary search
  #[inline]
  fn remove_resolved_dynamic (&mut self, resolved : &SortedSet <InternalId>) {
    debug_assert!(self.broad_overlap_pairs.is_empty());

    let (mut i, mut len);

    // remove intermediate mid TOI pairs
    i   = 0;
    len = self.mid_toi_pairs.len();
    while i < len {
      let (_, _, object_pair) = self.mid_toi_pairs[i].0;
      let (id_a, id_b) = object_pair.into();
      if id_a.kind() == object::Kind::Dynamic && resolved.binary_search (&id_a).is_ok() {
        self.mid_toi_pairs.remove_index (i);
        len -= 1;
        continue
      }
      debug_assert_eq!(id_b.kind(), object::Kind::Dynamic);
      if resolved.binary_search (&id_b).is_ok() {
        self.mid_toi_pairs.remove_index (i);
        len -= 1;
        continue
      }
      i += 1;
    }

    // remove intermediate narrow TOI contacts
    i   = 0;
    len = self.narrow_toi_contacts.len();
    while i < len {
      let (_, object_pair, _) = self.narrow_toi_contacts[i].clone().0;
      let (id_a, id_b) = object_pair.into();
      if id_a.kind() == object::Kind::Dynamic && resolved.binary_search (&id_a).is_ok() {
        self.narrow_toi_contacts.remove_index (i);
        len -= 1;
        continue
      }
      debug_assert_eq!(id_b.kind(), object::Kind::Dynamic);
      if resolved.binary_search (&id_b).is_ok() {
        self.narrow_toi_contacts.remove_index (i);
        len -= 1;
        continue
      }
      i += 1;
    }
  }
}