linear_sim/collision/

mod.rs

//! Collision detection and resolution

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

use crate::{component, event, geometry, math, object};
use math::*;
use geometry::Aabb3;

pub mod contact;
pub mod proximity;
pub mod intersection;
mod broad;

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

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

/// Distance criterea for contacts and collisions (TOI contacts).
///
/// Continuous collision detection will aim to return the TOI for when the
/// objects are at a distance approximately `0.5 * CONTACT_DISTANCE` in order to
/// prevent intersection due to numerical error.
///
/// Post-impulse position correction will also move objects apart to a distance
/// of `0.5 * CONTACT_DISTANCE`.
pub const CONTACT_DISTANCE : f64 = 0.005;   // 5mm
/// Velocity for determination of whether a normal impulse should be applied
pub const RESTING_VELOCITY : f64 = (1.0/120.0) * CONTACT_DISTANCE;
/// The value of the dynamic bit also gives the maximum key value allowed for
/// objects in the collision system.
pub const OBJECT_KEY_MAX   : object::KeyType = INTERNAL_ID_DYNAMIC_BIT - 1;

/// Used by 'InternalId' to indicate a dynamic object identifier, otherwise if
/// this bit is zero the object identifier is for a static object
pub (crate) const INTERNAL_ID_DYNAMIC_BIT : object::KeyType =
  1 << (object::KeyType::BITS - 1);   // most significant bit

#[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 struct Collision {
  /// Broad-phase detection subsystem
  broad             : Broad,
  /// Pseudo-velocities.
  ///
  /// Post-impulse position corrections utilize this velocity which is reset to
  /// zero at the beginning of each collision step.
  pseudo_velocities : VecMap <Vector3 <f64>>,
  /// 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,
  /// Persistent contact groups
  persistent        : contact::Manager,
  /// 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, contact::Colliding)>,
  pub resolved_collisions : Vec <event::CollisionResolve>,
  /// Nocollide intersections
  pub overlaps            : Vec <event::Overlap>,
}

/// (Sorted) static/dynamic or dynamic/dynamic pair
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd, Into)]
pub (crate) struct ObjectPair (InternalId, InternalId);

/// Represents a static or dynamic object identifier by using the most
/// significant bit of the object key type (0 for static, 1 for dynamic).
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Copy, Clone, Debug, Eq, Ord, PartialEq, PartialOrd)]
pub (crate) struct InternalId (object::KeyType);

#[derive(Debug)]
struct ContactImpulses {
  pub impulse_normal_a  : Vector3 <f64>,
  pub impulse_normal_b  : Vector3 <f64>,
  pub impulse_tangent_a : Vector3 <f64>,
  pub impulse_tangent_b : Vector3 <f64>
}

#[derive(Debug)]
struct ContactPostImpulses {
  pub pseudo_impulse_a : Vector3 <f64>,
  pub pseudo_impulse_b : Vector3 <f64>
}

////////////////////////////////////////////////////////////////////////////////
//  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");
}

/// Linear-interpolate an object with the given pseudovelocity
fn lerp_object <O : object::Temporal> (
  object : &mut O, pseudovelocity : Vector3 <f64>, t_relative : f64
) {
  let component::Position (position) = object.position().clone();
  object.position_mut().0 = position +
    t_relative * object.derivatives().velocity + t_relative * pseudovelocity;
}

fn contact_constrain_velocity (
  contact          : contact::Contact,
  restitution      : f64,   // 0.0 for a resting contact
  mut object_a     : Either <&object::Static, &mut object::Dynamic>,
  object_b         : &mut object::Dynamic,
  pseudovelocity_a : Option <Vector3 <f64>>,
  pseudovelocity_b : Vector3 <f64>
) -> Option <ContactImpulses> {
  let velocity_effective_a = match &object_a {
    Either::Left (_) => {
      debug_assert!(pseudovelocity_a.is_none());
      Vector3::zero()
    }
    Either::Right (object_a) => object_a.derivatives.velocity +
      pseudovelocity_a.unwrap_or (Vector3::zero())
  };
  let velocity_effective_b =
    object_b.derivatives.velocity + pseudovelocity_b;
  let velocity_effective_relative =
    velocity_effective_a - velocity_effective_b;
  let velocity_normal_component =
    velocity_effective_relative.dot (*contact.constraint.normal());
  let mut impulses        = None;
  let mut mass_relative_a = 0.0;
  let mut mass_relative_b = 0.0;
  if velocity_normal_component < 0.0 {
    let velocity_normal =
      velocity_normal_component * *contact.constraint.normal();
    // normal impulse
    let impulse_normal = velocity_normal + restitution * velocity_normal;
    let impulse_normal_magnitude2 = impulse_normal.magnitude_squared();
    if impulse_normal_magnitude2 < RESTING_VELOCITY * RESTING_VELOCITY {
      return None
    }
    let (impulse_normal_a, impulse_normal_b) = match &mut object_a {
      Either::Right (object_a) => {
        let mass_combined    = object_a.mass.clone() + object_b.mass.clone();
        mass_relative_a      =
          object_a.mass.mass() * mass_combined.mass_reciprocal();
        mass_relative_b      =
          object_b.mass.mass() * mass_combined.mass_reciprocal();
        let impulse_normal_a = -mass_relative_b * impulse_normal;
        let impulse_normal_b = mass_relative_a * impulse_normal;
        object_a.derivatives.velocity += impulse_normal_a;
        object_b.derivatives.velocity += impulse_normal_b;
        (impulse_normal_a, impulse_normal_b)
      }
      Either::Left (_) => {
        object_b.derivatives.velocity += impulse_normal;
        (Vector3::zero(), impulse_normal)
      }
    };
    // friction (tangent) impulse
    let (impulse_tangent_a, impulse_tangent_b) = {
      let velocity_tangent = velocity_effective_relative - velocity_normal;
      let velocity_tangent_magnitude2 = velocity_tangent.magnitude_squared();
      if velocity_tangent_magnitude2 > 0.0 {
        let friction_a = match &object_a {
          Either::Left  (x) => x.material.friction,
          Either::Right (x) => x.material.friction
        };
        let friction_coefficient = friction_a * object_b.material.friction;
        let impulse_tangent_potential_magnitude2 =
          friction_coefficient * impulse_normal_magnitude2;
        let impulse_tangent = if impulse_tangent_potential_magnitude2 <
          velocity_tangent_magnitude2
        {
          (impulse_tangent_potential_magnitude2.sqrt()
          / velocity_tangent_magnitude2.sqrt()) * velocity_tangent
        } else {
          velocity_tangent
        };
        match object_a {
          Either::Right (object_a) => {
            let impulse_tangent_a = -mass_relative_b * impulse_tangent;
            let impulse_tangent_b = mass_relative_a * impulse_tangent;
            object_a.derivatives.velocity += impulse_tangent_a;
            object_b.derivatives.velocity += impulse_tangent_b;
            (impulse_tangent_a, impulse_tangent_b)
          }
          Either::Left (_) => {
            object_b.derivatives.velocity += impulse_tangent;
            (Vector3::zero(), impulse_tangent)
          }
        }
      } else {
        (Vector3::zero(), Vector3::zero())
      }
    };
    impulses = Some (ContactImpulses {
      impulse_normal_a, impulse_normal_b,
      impulse_tangent_a, impulse_tangent_b
    });
  }
  impulses
}

fn contact_constrain_position (
  t_reverse        : f64,
  mut object_a     : Either <&object::Static, &mut object::Dynamic>,
  object_b         : &mut object::Dynamic,
  pseudovelocity_a : Option <&mut Vector3 <f64>>,
  pseudovelocity_b : &mut Vector3 <f64>
) -> (Option <ContactPostImpulses>, Proximity) {
  let t_forward = -t_reverse;
  let mut impulses  = None;
  let mut proximity = match &object_a {
    Either::Left  (object_a) => Proximity::query (*object_a, object_b),
    Either::Right (object_a) => Proximity::query (*object_a, object_b)
  };
  log::trace!(distance=proximity.distance; "post impulse contact @ T==1.0");
  if proximity.distance < 0.0 {
    let (pseudo_impulse_a, pseudo_impulse_b) = match &mut object_a {
      Either::Left  (object_a) => {
        lerp_object (object_b, *pseudovelocity_b, t_reverse);
        let pseudo_impulse_b = -(2.0 * proximity.half_axis +
          0.5 * CONTACT_DISTANCE * *proximity.normal) / t_forward;
        *pseudovelocity_b += pseudo_impulse_b;
        lerp_object (object_b, *pseudovelocity_b, t_forward);
        proximity = Proximity::query (*object_a, object_b);
        (Vector3::zero(), pseudo_impulse_b)
      }
      Either::Right (object_a) => {
        let pseudovelocity_a = pseudovelocity_a.unwrap();
        lerp_object (*object_a, *pseudovelocity_a, t_reverse);
        lerp_object (object_b, *pseudovelocity_b, t_reverse);
        let mass_combined    = object_a.mass.clone() + object_b.mass.clone();
        let mass_relative_a  =
          object_a.mass.mass() * mass_combined.mass_reciprocal();
        let mass_relative_b  =
          object_b.mass.mass() * mass_combined.mass_reciprocal();
        let pseudo_impulse   = (2.0 * proximity.half_axis +
          0.5 * CONTACT_DISTANCE * *proximity.normal) / t_forward;
        let pseudo_impulse_a = mass_relative_b * pseudo_impulse;
        let pseudo_impulse_b = -mass_relative_a * pseudo_impulse;
        *pseudovelocity_a += pseudo_impulse_a;
        *pseudovelocity_b += pseudo_impulse_b;
        lerp_object (*object_a, *pseudovelocity_a, t_forward);
        lerp_object (object_b, *pseudovelocity_b, t_forward);
        proximity = Proximity::query (*object_a, object_b);
        (pseudo_impulse_a, pseudo_impulse_b)
      }
    };
    log::trace!(distance=proximity.distance;
      "position corrected distance @ T==1.0");
    debug_assert!(proximity.distance >= 0.0);
    if cfg!(debug_assertions) {
      math::approx::assert_abs_diff_eq!(
        proximity.distance, 0.5 * CONTACT_DISTANCE, epsilon=0.000000001);
    }
    impulses = Some (ContactPostImpulses { pseudo_impulse_a, pseudo_impulse_b });
  }
  (impulses, proximity)
}

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

impl Collision {
  pub (crate) 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 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 (|id| id.into())
        .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)
          }
          _ => 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 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);
    let _ = self.persistent.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 fn try_add_object_dynamic (&mut self,
    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 (|id| id.into())
        .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 static objects
            }
            Proximity::query (object_dynamic, object)
          }
          _ => unreachable!()
        };
        if let Ok (intersection) = proximity.try_into() {
          intersections.push ((object_id, intersection));
        }
      }
    }
    if intersections.is_empty() {
      self.add_object_dynamic (object, key);
      Ok  (())
    } else {
      Err (intersections)
    }
  }

  #[inline]
  pub 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);
    self.pseudo_velocities.remove (key.index()).unwrap();
    let _ = self.persistent.remove_object (id);
  }

  #[inline]
  pub 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 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 fn detect_resolve_loop (&mut self,
    objects_static  : &mut 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 (objects_static, objects_dynamic);
      // resolve earliest collision: resolve a velocity constraint at TOI and a
      // position constraint at t==1.0
      if !self.resolve_collision (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
    self.persistent.output_contacts (output);

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

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

  /// Solve positions constraints for persistent contact groups
  pub fn constrain_contact_positions (&mut self,
    objects_static  : &VecMap <object::Static>,
    objects_dynamic : &mut VecMap <object::Dynamic>
  ) {
    let mut remove_groups  = vec![];
    let mut new_groups     = vec![];
    let mut contact_groups = self.persistent.contact_groups.take().unwrap();
    for (group_index, group) in contact_groups.iter_mut() {
      log::debug!(group_index, contacts_count=group.contacts.len();
        "group constrain positions");
      log::trace!(group_index, contacts:?=group.contacts;
        "group constrain positions contacts");
      let mut iter = 0;
      let mut remove_contacts = vec![];
      loop {
        remove_contacts.clear();
        let mut satisfied = true;
        for (contact_index, (object_pair, contact)) in
          group.contacts.iter_mut().enumerate()
        {
          let (object_id_a, object_id_b) = (*object_pair).into();
          let index_a = object_id_a.key().index();
          let index_b = object_id_b.key().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()),
            _ => unreachable!()
          };
          debug_assert_eq!(object_id_b.kind(), object::Kind::Dynamic);
          let mut object_b = objects_dynamic[index_b].clone();
          let mut pseudovelocity_a = match object_a {
            Either::Left  (_) => None,
            Either::Right (_) => Some (self.pseudo_velocities[index_a])
          };
          let mut pseudovelocity_b = self.pseudo_velocities[index_b];
          let (maybe_impulses, proximity) = contact_constrain_position (
            -1.0,
            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() {
            satisfied = false;
            match object_a {
              Either::Right (object_a) => {
                objects_dynamic[index_a]        = object_a;
                self.pseudo_velocities[index_a] = pseudovelocity_a.unwrap();
              }
              Either::Left  (_) => {}
            }
            objects_dynamic[index_b]        = object_b;
            self.pseudo_velocities[index_b] = pseudovelocity_b;
          }
          // NOTE: checking the contact distance here instead of the try_into
          // result so we don't have to clone to use the proximity object in the
          // debug log statement
          if proximity.distance >= CONTACT_DISTANCE {
            log::debug!(object_pair:?, contact:?, proximity:?;
              "removing contact @ T==1.0");
            remove_contacts.push (contact_index as u32);
          } else {
            // update contact
            *contact = proximity.try_into().unwrap();
          }
        }
        iter += 1;
        if satisfied {
          // group satisfied
          log::debug!(group_index, iters=iter;
            "group position constraint satisfied");
          if !remove_contacts.is_empty() {
            self.persistent.remove_contacts (group, remove_contacts.as_slice());
            if group.contacts.is_empty() {
              remove_groups.push (group_index as contact::group::KeyType);
            } else {
              let partitions = group.partition();
              if !partitions.is_empty() {
                remove_groups.push (group_index as contact::group::KeyType);
                new_groups.extend (partitions);
              }
            }
          }
          break
        }
      }
    }
    for group_key in remove_groups {
      let _ = contact_groups.take (group_key as usize).unwrap();
    }
    for new_group in new_groups {
      let dynamic_ids = new_group.dynamic_ids();
      let group_key = contact_groups.put (new_group) as contact::group::KeyType;
      for dynamic_id in dynamic_ids.into_vec() {
        self.persistent.change_dynamic_group_key (dynamic_id, group_key);
      }
    }
    self.persistent.contact_groups = Some (contact_groups);
  }

  /// Solve velocity constraints for persistent contact groups
  pub fn constrain_contact_velocities (&self,
    objects_static  : &VecMap <object::Static>,
    objects_dynamic : &mut VecMap <object::Dynamic>
  ) {
    for (group_index, group) in self.contact_groups().iter() {
      log::debug!(group_index, contacts_count=group.contacts.len();
        "group constrain velocities");
      log::trace!(group_index, contacts:?=group.contacts;
        "group constrain velocities contacts");
      let mut iter = 0;
      loop {
        let mut satisfied = true;
        for (object_pair, contact) in group.contacts.iter() {
          let (object_id_a, object_id_b) = (*object_pair).into();
          let index_a = object_id_a.key().index();
          let index_b = object_id_b.key().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()),
            _ => unreachable!()
          };
          debug_assert_eq!(object_id_b.kind(), object::Kind::Dynamic);
          let mut object_b = objects_dynamic[index_b].clone();
          // NOTE: at this stage in the pipeline, pseudovelocities are zero
          if contact_constrain_velocity (
            contact.clone(), 0.0,
            object_a.as_mut().map_left (|object_static| &*object_static),
            &mut object_b, None, Vector3::zero()
          ).is_some() {
            match object_a {
              Either::Right (object_a) => objects_dynamic[index_a] = object_a,
              Either::Left  (_) => {}
            }
            objects_dynamic[index_b] = object_b;
            satisfied = false;
          }
        }
        iter += 1;
        if satisfied {
          // group satisfied
          log::debug!(group_index, iters=iter;
            "group velocity constraint satisfied");
          break
        }
      }
    }
  }

  pub (crate) fn contact_groups (&self) -> &Stash <contact::Group> {
    self.persistent.contact_groups.as_ref().unwrap()
  }

  //
  //  private methods
  //

  /// Detect TOI (time-of-impact) contacts, i.e. *collisions*.
  fn detect_continuous (&mut self,
    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 (objects_dynamic);
    // narrow-phase TOI pairs
    self.narrow_toi_contacts (objects_static, objects_dynamic);
  }

  /// 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_collision (&mut self,
    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,
          _ => 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 groups
      let maybe_group_a = self.persistent.get_group_key (object_id_a);
      let maybe_group_b = {
        let maybe_group_b = self.persistent.get_group_key (object_id_b);
        if maybe_group_a == maybe_group_b {
          // if the groups are the same, then just use group a
          None
        } else {
          maybe_group_b
        }
      };
      // resolve collision
      let collision_resolve = self.resolve (objects_static, objects_dynamic,
        toi, object_id_a, object_id_b, contact.clone(), maybe_group_a,
        maybe_group_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
    }
  }

  #[allow(clippy::too_many_arguments)]
  fn resolve (
    &mut self,
    objects_static  : &VecMap <object::Static>,
    objects_dynamic : &mut VecMap <object::Dynamic>,
    toi             : Normalized <f64>,
    object_id_a     : InternalId,
    object_id_b     : InternalId,
    contact         : contact::Colliding,
    group_key_a     : Option <contact::group::KeyType>,
    group_key_b     : Option <contact::group::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 math::num_traits::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 group_a = group_key_a
      .map (|group_key| self.persistent.get_group (group_key).unwrap());
    let group_b = group_key_b
      .map (|group_key| self.persistent.get_group (group_key).unwrap());
    group_a.map (|group| dynamic_ids.extend (group.dynamic_ids().into_vec()));
    group_b.map (|group| dynamic_ids.extend (group.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 = self.pseudo_velocities[index];
        lerp_object (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>>,
      contact          : Contact,
      restitution      : f64,
      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()),
        _ => unreachable!()
      };
      debug_assert_eq!(object_id_b.kind(), object::Kind::Dynamic);
      let mut object_b     = objects_dynamic[index_b].clone();
      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   = contact_constrain_velocity (
        contact, restitution,
        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, &self.pseudo_velocities, contact.contact.clone(),
        contact.restitution, object_id_a, object_id_b
      ) {
        // if there are no groups, then the collision is isolated and satisfied
        satisfied = group_a.is_none() && group_b.is_none();
        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_group = |group : &contact::Group|
        for (object_pair, contact) in group.contacts.iter().cloned() {
          let (object_id_a, object_id_b) = object_pair.into();
          if constrain_velocity (
            objects_dynamic, &self.pseudo_velocities, contact, 0.0,
            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 (group) = group_a {
        constrain_group (group);
      }
      if let Some (group) = group_b {
        constrain_group (group);
      }
      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>>,
      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()),
        _ => 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, proximity) = contact_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, proximity)
    };
    let mut final_proximities = vec![
      Proximity {
        distance:  0.0,
        half_axis: Vector3::zero(),
        midpoint:  Point::origin(),
        normal:    Unit3::axis_z()
      };
      1 + group_a.map (|group| group.contacts.len()).unwrap_or (0) +
      group_b.map (|group| group.contacts.len()).unwrap_or (0)
    ];
    let mut pseudo_impulse_a = Vector3::zero();
    let mut pseudo_impulse_b = Vector3::zero();
    let mut iter = 0;
    loop {
      let mut satisfied     = true;
      let mut contact_index = 0;
      let (maybe_impulses, proximity) = constrain_position (
        objects_dynamic, &mut self.pseudo_velocities, object_id_a, object_id_b
      );
      final_proximities[contact_index] = proximity;
      contact_index += 1;
      if let Some (impulses) = maybe_impulses {
        // if there are no groups, then the collision is isolated and satisfied
        satisfied = group_a.is_none() && group_b.is_none();
        pseudo_impulse_a += impulses.pseudo_impulse_a;
        pseudo_impulse_b += impulses.pseudo_impulse_b;
      }
      let mut constrain_group = |group : &contact::Group|
        for (object_pair, _) in group.contacts.iter() {
          let (object_id_a, object_id_b)  = (*object_pair).into();
          let (maybe_impulses, proximity) = constrain_position (
            objects_dynamic, &mut self.pseudo_velocities,
            object_id_a, object_id_b
          );
          final_proximities[contact_index] = proximity;
          contact_index += 1;
          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 (group) = group_a {
        constrain_group (group);
      }
      if let Some (group) = group_b {
        constrain_group (group);
      }
      iter += 1;
      if satisfied {
        log::debug!(iters=iter; "collision position constraint satisfied");
        break
      }
    }
    // 5. create or destroy persistent contacts from final proximity check;
    // also add resolved pairs to the broad phase here since we are iterating
    // over contact groups 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);
    let mut final_proximities     = final_proximities.into_iter();
    let collision_final_proximity = final_proximities.next().unwrap();
    // wait until after updating groups to create any new contacts in case the
    // groups are merged which would invalidate one of the old group keys
    let mut update_group = |
      final_proximities : &mut std::vec::IntoIter <Proximity>,
      group_key         : contact::group::KeyType
    | {
      let mut contact_groups = self.persistent.contact_groups.take().unwrap();
      let group = &mut contact_groups[group_key as usize];
      let mut remove_contacts = vec![];
      for (i, proximity) in
        final_proximities.take (group.contacts.len()).enumerate()
      {
        let contact = &mut group.contacts[i];
        self.broad.add_resolved (contact.0);
        if let Ok (update_contact) = Contact::try_from (proximity) {
          contact.1 = update_contact;
        } else {
          remove_contacts.push (i as u32);
        }
      }
      if !remove_contacts.is_empty() {
        self.persistent.remove_contacts (group, remove_contacts.as_slice());
        if group.contacts.is_empty() {
          let _ = contact_groups.take (group_key as usize).unwrap();
        } else {
          let partitions = group.partition();
          if !partitions.is_empty() {
            let _ = contact_groups.take (group_key as usize).unwrap();
            for new_group in partitions {
              let dynamic_ids = new_group.dynamic_ids();
              let group_key   = contact_groups.put (new_group)
                as contact::group::KeyType;
              for dynamic_id in dynamic_ids.into_vec() {
                self.persistent.change_dynamic_group_key (dynamic_id, group_key);
              }
            }
          }
        }
      }
      self.persistent.contact_groups = Some (contact_groups);
    };
    if let Some (group_key) = group_key_a {
      update_group (&mut final_proximities, group_key);
    }
    if let Some (group_key) = group_key_b {
      update_group (&mut final_proximities, group_key);
    }
    if let Ok (contact) = collision_final_proximity.try_into() {
      log::trace!(object_pair:?, contact:?; "creating contact @ T==1.0");
      self.persistent.add_contact (object_pair, contact);
    }
    // 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, contact,
      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, 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_group_key)) = (
            self.persistent.get_contact_count (object_id_a),
            self.persistent.get_group_key (object_id_b)
          ) {
            debug_assert!(static_contact_count > 0);
            let group = self.persistent.get_group (dynamic_group_key).unwrap();
            for (pair, _) in group.contacts.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 = self.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 (
            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.max().0.x - dynamic_aabb_previous.min().0.x) *
                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.min().0.x - dynamic_aabb_previous.max().0.x) *
                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 *
                (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 *
                (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 *
                (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 *
                (static_aabb.min().0.z - dynamic_aabb_previous.max().0.z)
                + t_margin_z
            )
          };

          // TODO: can we write this more clearly?
          #[allow(clippy::collapsible_if)]
          if let Some ((interval_start, interval_end)) =
            if let Some ((interval_start_xy, interval_end_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 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 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 (group_key_a), Some (group_key_b)) = (
            self.persistent.get_group_key (object_id_a),
            self.persistent.get_group_key (object_id_b)
          ) && group_key_a == group_key_b {
            let group = self.persistent.get_group (group_key_a).unwrap();
            for (pair, _) in group.contacts.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  = self.pseudo_velocities[key_a.index()];
          let pseudovelocity_b  = self.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 (
            aabb_current_a.min() - velocity_effective_a,
            aabb_current_a.max() - velocity_effective_a);
          let aabb_previous_b   = Aabb3::with_minmax (
            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 (f64::NEG_INFINITY, f64::INFINITY)
            } else if velocity_relative > 0.0 {
              geometry::Interval::with_minmax (
                (prev_b_min - prev_a_max) * velocity_relative_reciprocal -
                  t_margin,
                (prev_b_max - prev_a_min) * velocity_relative_reciprocal +
                  t_margin
              )
            } else {
              debug_assert!(velocity_relative < 0.0);
              geometry::Interval::with_minmax (
                (prev_b_max - prev_a_min) * velocity_relative_reciprocal -
                  t_margin,
                (prev_b_min - prev_a_max) * 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!()
            }
          }
        }
        _ => 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>
  ) {
    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().cloned() {
      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 = self.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
            lerp_object (&mut dynamic_object, dynamic_pseudovelocity,
              -1.0 + narrow_toi);
            let mut t_remaining    = 1.0 - narrow_toi;
            #[allow(unused_variables)]
            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);
              }
              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");
                let contact     = Contact { constraint: proximity.into() };
                let restitution = dynamic_object.material.restitution *
                  static_object.material.restitution;
                let collision   = contact::Colliding { contact, restitution };
                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);
              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;
              lerp_object (&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 = self.pseudo_velocities[index_a];
            let pseudovelocity_b = self.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;
            lerp_object (&mut object_a, pseudovelocity_a, -1.0 + narrow_toi);
            lerp_object (&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     = Contact { constraint: proximity.into() };
                let restitution =
                  object_a.material.restitution * object_b.material.restitution;
                let collision   = contact::Colliding { contact, restitution };
                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 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);
              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;
              lerp_object (&mut object_a, pseudovelocity_a, t_advance);
              lerp_object (&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,
    object     : &object::Dynamic,
    object_key : object::Key
  ) {
    let aabb_discrete   = object.aabb_dilated();
    let aabb_continuous = Aabb3::with_minmax (
      aabb_discrete.min() - object.derivatives.velocity,
      aabb_discrete.max() - object.derivatives.velocity);
    // NB: 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!(self.pseudo_velocities
      .insert (object_key.index(), Vector3::zero()).is_none());
  }

}

impl Pipeline {
  #[inline]
  pub 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]
  pub 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;
    }
  }
}

impl ObjectPair {
  /// Create a new object pair.
  ///
  /// One object must be dynamic. Internally the object IDs will be stored in
  /// sorted order, and a static object will always be first.
  pub fn new (id_a : InternalId, id_b : InternalId) -> Self {
    debug_assert!(id_a.kind() == object::Kind::Dynamic ||
      id_b.kind() == object::Kind::Dynamic);
    if id_a < id_b {
      ObjectPair (id_a, id_b)
    } else {
      debug_assert!(id_a > id_b, "overlap ids should not be identical");
      ObjectPair (id_b, id_a)
    }
  }
}

impl From <(InternalId, InternalId)> for ObjectPair {
  fn from ((id_a, id_b) : (InternalId, InternalId)) -> Self {
    ObjectPair::new (id_a, id_b)
  }
}

impl InternalId {
  #[inline]
  pub fn new_static (key : object::Key) -> Self {
    debug_assert!(key.value() < OBJECT_KEY_MAX);
    InternalId (key.value())
  }
  #[inline]
  pub fn new_dynamic (key : object::Key) -> Self {
    debug_assert!(key.value() < OBJECT_KEY_MAX);
    InternalId (key.value() | INTERNAL_ID_DYNAMIC_BIT)
  }
  #[inline]
  pub fn kind (&self) -> object::Kind {
    match self.0 & INTERNAL_ID_DYNAMIC_BIT > 0 {
      true  => object::Kind::Dynamic,
      false => object::Kind::Static
    }
  }
  #[inline]
  pub fn key (&self) -> object::Key {
    object::Key::from (self.0 & !INTERNAL_ID_DYNAMIC_BIT)
  }
}
impl From <InternalId> for object::Id {
  fn from (id : InternalId) -> Self {
    object::Id {
      kind: id.kind(),
      key:  id.key()
    }
  }
}