//! Collision detection and resolution
use {std, cgmath};
use {component, constraint, event, math, object};

use std::sync::atomic;
use vec_map::VecMap;
use sorted_vec::{SortedVec, ReverseSortedVec};
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.001;   // 1mm
/// Velocity for determination of persistent contacts.
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
const INTERNAL_ID_DYNAMIC_BIT : object::KeyType =
  object::KeyType::max_value() / 2 + 1;

//
//  debug
//
/// Count maximum iterations of collision detect/resolve main loop
static DEBUG_COLLISION_LOOP_MAX_ITERS : atomic::AtomicUsize =
  atomic::AtomicUsize::new(1);
static DEBUG_NARROW_MAX_ITERS         : atomic::AtomicUsize =
  atomic::AtomicUsize::new(1);

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

/// Collision subsystem
#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
#[derive(Clone,Debug,Default,PartialEq)]
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 <cgmath::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        : Persistent
}

/// 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,
  /// Dynamic/static overlaps on all three axes
  pub broad_overlap_pairs_dynamic_static  : SortedVec <(object::Key, object::Key)>,
  /// Dynamic/dynamic overlaps on all three axes
  pub broad_overlap_pairs_dynamic_dynamic : SortedVec <(object::Key, object::Key)>,
  /// Sorted in *reverse order* by start time
  pub mid_toi_pairs                       : ReverseSortedVec <(
    math::Normalized <f64>, math::Normalized <f64>, InternalId, InternalId)>,
  /// Sorted in *reverse order* by TOI
  pub narrow_toi_contacts                 : ReverseSortedVec <(
    math::Normalized <f64>, InternalId, InternalId, contact::Colliding)>,
  pub resolved_collisions                 : Vec <event::CollisionResolve>,
}

/// Track persistent contacts
#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
#[derive(Clone,Debug,Default,PartialEq)]
struct Persistent {
  pub previous_contacts_dynamic_static  : SortedVec <(object::Key, object::Key)>,
  pub previous_contacts_dynamic_dynamic : SortedVec <(object::Key, object::Key)>,
  pub current_contacts_dynamic_static   : SortedVec <(object::Key, object::Key)>,
  pub current_contacts_dynamic_dynamic  : SortedVec <(object::Key, object::Key)>
}

/// 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(Serialize, Deserialize))]
#[derive(Copy,Clone,Debug,Eq,Ord,PartialEq,PartialOrd)]
struct InternalId (object::KeyType);

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

/// Linear-interpolate an object with the given pseudovelocity
pub fn lerp_object <O : object::Temporal> (
  object : &mut O, pseudovelocity : &cgmath::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;
}

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 {
  //
  //  Collision::try_add_object_static
  //
  /// 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.
  pub fn try_add_object_static (&mut self,
    objects_dynamic : &VecMap <object::Dynamic>,
    object          : &object::Static,
    key             : object::Key
  ) -> Result <(), Vec <(object::Identifier, Intersection)>> {
    let mut intersections = Vec::new();
    for (key, dynamic_object) in objects_dynamic.iter() {
      let proximity = Proximity::query (object, dynamic_object);
      if proximity.relation() == proximity::Relation::Intersect {
        use std::convert::TryFrom;
        let object_id     = object::Identifier {
          kind: object::Kind::Dynamic,
          key:  object::Key::from (key)
        };
        intersections.push ((
          // safe to unwrap: checked for intersection
          object_id, Intersection::try_from (proximity).unwrap()
        ));
      }
    }
    if intersections.is_empty() {
      self.add_object_static (object, key);
      Ok  (())
    } else {
      Err (intersections)
    }
  }
  //  end Collision::try_add_object_static

  #[inline]
  pub fn remove_object_static (&mut self, object_key : object::Key) {
    debug_assert!(self.pipeline.is_empty());
    self.broad.remove_object_static (object_key);
    println!("TODO: remove persistent contacts");
  }

  //
  //  Collision::try_add_object_dynamic
  //
  /// 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.
  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::Identifier, Intersection)>> {
    let mut intersections = Vec::new();
    for (key, static_object) in objects_static.iter() {
      let proximity = Proximity::query (object, static_object);
      if proximity.relation() == proximity::Relation::Intersect {
        use std::convert::TryFrom;
        let object_id     = object::Identifier {
          kind: object::Kind::Static,
          key:  object::Key::from (key)
        };
        intersections.push ((
          // safe to unwrap: checked for intersection
          object_id, Intersection::try_from (proximity).unwrap()
        ));
      }
    }

    for (key, dynamic_object) in objects_dynamic.iter() {
      let proximity = Proximity::query (object, dynamic_object);
      if proximity.relation() == proximity::Relation::Intersect {
        use std::convert::TryFrom;
        let object_id   = object::Identifier {
          kind: object::Kind::Dynamic,
          key:  object::Key::from (key)
        };
        intersections.push ((
          // safe to unwrap: checked for intersection
          object_id, Intersection::try_from (proximity).unwrap()
        ));
      }
    }

    if intersections.is_empty() {
      self.add_object_dynamic (object, key);
      Ok  (())
    } else {
      Err (intersections)
    }
  } // end Collision::try_add_object_dynamic

  #[inline]
  pub fn remove_object_dynamic (&mut self, object_key : object::Key) {
    debug_assert!(self.pipeline.is_empty());
    self.broad.remove_object_dynamic (object_key);
    self.pseudo_velocities.remove (object_key.index()).unwrap();
    println!("TODO: remove persistent contacts");
  }

  //
  //  Collision::detect_resolve_loop
  //
  /// Main collision loop.
  ///
  /// Before the loop starts, `begin_step()`:
  ///
  /// 1. Update broad-phase AABB data and re-sort
  /// 2. Zero pseudo-velocities
  ///
  /// Inside the collision loop:
  ///
  /// 1. Perform continuous detection
  /// 2. Resolve earliest detected collision
  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>
  ) {
    use colored::Colorize;
    trace!("detect/resolve loop step [{}]", step.to_string().red().bold());

    // initialize collision for the current step
    self.begin_step (objects_static, objects_dynamic, step);
    debug_assert_eq!(self.pipeline.detect_resolve_iter, 0);
    // main detect/resolve loop: resolve one collision per iteration
    loop {
      trace!("detect/resolve loop iter [{}]",
        self.pipeline.detect_resolve_iter.to_string().yellow().bold());

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

    trace!("detect/resolve loop complete in [{}] iters",
      self.pipeline.detect_resolve_iter+1);
    if cfg!(debug_assertions) {
      if DEBUG_COLLISION_LOOP_MAX_ITERS.load (atomic::Ordering::SeqCst)
        < (self.pipeline.detect_resolve_iter + 1) as usize
      {
        DEBUG_COLLISION_LOOP_MAX_ITERS.store (
          self.pipeline.detect_resolve_iter as usize + 1,
          atomic::Ordering::SeqCst)
      }
      let loop_max_iters =
        DEBUG_COLLISION_LOOP_MAX_ITERS.load (atomic::Ordering::SeqCst);
      trace!("detect/resolve loop max iters: ({})", loop_max_iters);
    }

    // output collision events
    for collision_resolve in self.pipeline.resolved_collisions.iter() {
      output.push (collision_resolve.clone().into());
    }

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

  //
  //  private methods
  //

  //
  //  Collision::begin_step
  //
  /// Prepare collision state for the next step based on current object states
  /// at t=0.0.
  ///
  /// - updates broad phase AABB state and perform full insertion sort
  /// - zeros pseudovelocities
  fn begin_step (&mut self,
    objects_static  : &VecMap <object::Static>,
    objects_dynamic : &VecMap <object::Dynamic>,
    step            : u64
  ) {
    use cgmath::Zero;
    debug_assert!(self.pipeline.is_empty());
    self.pipeline.detect_resolve_iter = 0;

    // broad: update AABBs based on the given objects state and re-sort
    //
    // the continuous (swept) AABBs are calculated from min/max of the previous
    // 'discrete' (instantaneous) AABB and the new "current" discrete AABB and
    // the broad phase performs a full insertion sort on the sorted axes
    self.broad.begin_step (&objects_static, &objects_dynamic, step);

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

  //
  //  Collision::detect_continuous
  //
  /// 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);
  }
  //  end Collision::detect_continuous

  //
  //  Collision::resolve
  //
  /// 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  : &mut VecMap <object::Static>,
    objects_dynamic : &mut VecMap <object::Dynamic>
  ) -> bool {
    use cgmath::{InnerSpace, Zero};

    if let Some((toi, object_id_a, object_id_b, contact)) =
      self.pipeline.narrow_toi_contacts.pop()
    {
      use geometry::shape::Aabb;

      let toi_f64 : f64 = *toi;   // [0.0,1.0]
      let kind_a  = object_id_a.kind();
      let kind_b  = object_id_b.kind();
      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 collision_resolve = match (kind_a, kind_b) {
        (object::Kind::Dynamic, object::Kind::Static)  => {
          // safe to unwrap: objects should not be destroyed during collision
          let dynamic_object = objects_dynamic.get_mut (index_a).unwrap();
          let static_object  = objects_static.get (index_b).unwrap();
          let dynamic_pseudovelocity = self.pseudo_velocities
            .get (index_a).unwrap().clone();
          let dynamic_velocity_effective =
            dynamic_object.derivatives.velocity + dynamic_pseudovelocity;
          let t_reverse = -1.0 + toi_f64;
          let t_forward =  1.0 - toi_f64;
          // lerp to TOI
          lerp_object (dynamic_object, &dynamic_pseudovelocity, t_reverse);
          let toi_aabb = dynamic_object.aabb();

          // normal impulse
          let velocity_contact_normal  =
            dynamic_velocity_effective.dot (*contact.constraint.normal()) *
              *contact.constraint.normal();
          let impulse_normal   = -velocity_contact_normal -
            contact.restitution * velocity_contact_normal;
          dynamic_object.derivatives.velocity += impulse_normal;

          // friction (tangent) impulse
          let velocity_contact_tangent =
            dynamic_velocity_effective - velocity_contact_normal;
          let velocity_contact_tangent_magnitude2 =
            velocity_contact_tangent.magnitude2();
          let impulse_tangent = if velocity_contact_tangent_magnitude2 > 0.0 {
            let velocity_contact_tangent_magnitude =
              f64::sqrt (velocity_contact_tangent_magnitude2);
            let velocity_contact_tangent_magnitude_reciprocal =
              1.0 / velocity_contact_tangent_magnitude;
            let velocity_contact_tangent_unit =
              velocity_contact_tangent_magnitude_reciprocal *
              velocity_contact_tangent;
            let friction_coefficient = 0.5 * (
              dynamic_object.material.friction + static_object.material.friction);
            let impulse_tangent_potential = -friction_coefficient *
              impulse_normal.magnitude() * velocity_contact_tangent_unit;
            let impulse_tangent = if impulse_tangent_potential.magnitude2() <
              velocity_contact_tangent_magnitude2
            {
              impulse_tangent_potential
            } else {
              -velocity_contact_tangent
            };
            dynamic_object.derivatives.velocity += impulse_tangent;
            impulse_tangent
          } else {
            cgmath::Vector3::zero()
          };

          // lerp to new final position
          lerp_object (dynamic_object, &dynamic_pseudovelocity, t_forward);
          // post impulse position correction
          let proximity      = Proximity::query (dynamic_object, static_object);
          trace!("post impulse distance: {}", proximity.distance);
          let pseudo_impulse = if proximity.distance < 0.0 {
            let dynamic_pseudovelocity = self.pseudo_velocities
              .get_mut (index_a).unwrap();
            lerp_object (dynamic_object, &dynamic_pseudovelocity, t_reverse);
            let pseudo_impulse = t_forward *
              ( 2.0 * proximity.half_axis +
                0.5 * CONTACT_DISTANCE * *proximity.normal);
            *dynamic_pseudovelocity += pseudo_impulse;
            lerp_object (dynamic_object, dynamic_pseudovelocity, t_forward);
            if cfg!(debug_assertions) {
              let proximity = Proximity::query (dynamic_object, static_object);
              trace!("position corrected distance: {}", proximity.distance);
              debug_assert!(proximity.distance >= 0.0);
            }
            pseudo_impulse
          } else {
            cgmath::Vector3::zero()
          };

          // update AABB data
          let new_aabb = dynamic_object.aabb();
          self.broad.update_aabb_dynamic (
            new_aabb.clone(), Aabb3::union (&toi_aabb, &new_aabb), key_a);
          self.broad.add_resort_key_static (key_b);
          self.pipeline.remove_resort_keys (&self.broad.resort_keys().dynamics);

          event::CollisionResolve {
            toi, contact,
            object_id_a:       object_id_a.into(),
            object_id_b:       object_id_b.into(),
            impulse_normal_a:  impulse_normal,
            impulse_normal_b:  cgmath::Vector3::zero(),
            impulse_tangent_a: impulse_tangent,
            impulse_tangent_b: cgmath::Vector3::zero(),
            pseudo_impulse_a:  pseudo_impulse,
            pseudo_impulse_b:  cgmath::Vector3::zero()
          }
        }

        (object::Kind::Dynamic, object::Kind::Dynamic) => {
           // safe to unwrap: objects should not be destroyed during collision
          let mut object_a = objects_dynamic.get (index_a).unwrap().clone();
          let mut object_b = objects_dynamic.get (index_b).unwrap().clone();
          let mut pseudovelocity_a = self.pseudo_velocities.get (index_a)
            .unwrap().clone();
          let mut pseudovelocity_b = self.pseudo_velocities.get (index_b)
            .unwrap().clone();
          let velocity_effective_a =
            object_a.derivatives.velocity + pseudovelocity_a;
          let velocity_effective_b =
            object_b.derivatives.velocity + pseudovelocity_b;
          let velocity_effective_relative =
            velocity_effective_a - velocity_effective_b;
          let t_reverse = -1.0 + *toi;
          let t_forward =  1.0 - *toi;

          // lerp to TOI
          lerp_object (&mut object_a, &pseudovelocity_a, t_reverse);
          lerp_object (&mut object_b, &pseudovelocity_b, t_reverse);
          let aabb_toi_a = object_a.aabb();
          let aabb_toi_b = object_b.aabb();

          // normal impulse
          let velocity_contact_normal =
            velocity_effective_relative.dot (*contact.constraint.normal()) *
              *contact.constraint.normal();
          let impulse_normal   = -velocity_contact_normal -
            contact.restitution * velocity_contact_normal;
          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 impulse_normal_a = mass_relative_b * impulse_normal;
          let impulse_normal_b = mass_relative_a * impulse_normal;
          object_a.derivatives.velocity += impulse_normal_a.clone();
          object_b.derivatives.velocity -= impulse_normal_b.clone();

          // friction (tangent) impulse
          let velocity_contact_tangent =
            velocity_effective_relative - velocity_contact_normal;
          let velocity_contact_tangent_magnitude2 =
            velocity_contact_tangent.magnitude2();
          let (impulse_tangent_a, impulse_tangent_b) = if
            velocity_contact_tangent_magnitude2 > 0.0
          {
            let velocity_contact_tangent_magnitude =
              f64::sqrt (velocity_contact_tangent_magnitude2);
            let velocity_contact_tangent_magnitude_reciprocal =
              1.0 / velocity_contact_tangent_magnitude;
            let velocity_contact_tangent_unit =
              velocity_contact_tangent_magnitude_reciprocal *
              velocity_contact_tangent;
            let friction_coefficient = 0.5 * (
              object_a.material.friction + object_b.material.friction);
            let impulse_tangent  = -friction_coefficient *
              impulse_normal.magnitude() * velocity_contact_tangent_unit;
            let (impulse_tangent_a, impulse_tangent_b) = if
              impulse_tangent.magnitude2() < velocity_contact_tangent_magnitude2
            {
              ( mass_relative_b * impulse_tangent,
                -mass_relative_a * impulse_tangent )
            } else {
              ( mass_relative_b * velocity_contact_tangent,
                -mass_relative_a * velocity_contact_tangent )
            };
            object_a.derivatives.velocity += impulse_tangent_a;
            object_b.derivatives.velocity += impulse_tangent_b;
            (impulse_tangent_a, impulse_tangent_b)
          } else {
            (cgmath::Vector3::zero(), cgmath::Vector3::zero())
          };

          // lerp to new end of step
          lerp_object (&mut object_a, &pseudovelocity_a, t_forward);
          lerp_object (&mut object_b, &pseudovelocity_b, t_forward);

          // post impulse position correction
          let proximity = Proximity::query (&object_a, &object_b);
          trace!("collsion resolve post impulse distance: {}",
            proximity.distance);
          let (pseudo_impulse_a, pseudo_impulse_b) = if
            proximity.distance < 0.0
          {
            lerp_object (&mut object_a, &pseudovelocity_a, t_reverse);
            lerp_object (&mut object_b, &pseudovelocity_b, t_reverse);
            let pseudo_impulse = t_forward *
              ( 2.0 * proximity.half_axis +
                0.5 * CONTACT_DISTANCE * *proximity.normal);
            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 (&mut object_a, &pseudovelocity_a, t_forward);
            lerp_object (&mut object_b, &pseudovelocity_b, t_forward);

            if cfg!(debug_assertions) {
              let proximity = Proximity::query (&object_a, &object_b);
              trace!("collsion resolve position corrected distance: {}",
                proximity.distance);
              debug_assert!(proximity.distance >= 0.0);
            }

            *self.pseudo_velocities.get_mut (index_a).unwrap() =
              pseudovelocity_a;
            *self.pseudo_velocities.get_mut (index_b).unwrap() =
              pseudovelocity_b;
            (pseudo_impulse_a, pseudo_impulse_b)
          } else {
            (cgmath::Vector3::zero(), cgmath::Vector3::zero())
          };

          {  // update AABB data
            let mut update_aabbs = |
              object   : &object::Dynamic,
              aabb_toi : &Aabb3 <f64>,
              key      : object::Key
            | {
              let aabb_discrete   = object.aabb();
              let aabb_continuous = Aabb3::union (&aabb_toi, &aabb_discrete);
              self.broad.update_aabb_dynamic (aabb_discrete, aabb_continuous, key);
            };
            update_aabbs (&object_a, &aabb_toi_a, key_a);
            update_aabbs (&object_b, &aabb_toi_b, key_b);
          }
          self.pipeline.remove_resort_keys (&self.broad.resort_keys().dynamics);

          *objects_dynamic.get_mut (index_a).unwrap() = object_a;
          *objects_dynamic.get_mut (index_b).unwrap() = object_b;

          // these should both be true unless we have infinite masses
          debug_assert!(!impulse_normal_a.is_zero());
          debug_assert!(!impulse_normal_b.is_zero());

          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
          }
        }
        // TODO: compiler unreachable hint ?
        _ => unreachable!()
      };
      trace!("resolved collision: {:?}", collision_resolve);
      self.pipeline.resolved_collisions.push (collision_resolve);
      true
    } else {
      debug_assert!(self.pipeline.mid_toi_pairs.is_empty());
      false
    }
  }
  //  end Collision::resolve

  /// Detects AABB overlap pairs of dynamic/static and dynamic/dynamic objects.
  ///
  /// Output overlap pair arrays are are sorted on keys.
  ///
  /// In dynamic/dynamic output, keypairs are in sorted order (key A < key B).
  #[inline]
  fn broad_overlap_pairs_continuous (&mut self) {
    self.broad.overlap_pairs_continuous (
      &mut self.pipeline.broad_overlap_pairs_dynamic_static,
      &mut self.pipeline.broad_overlap_pairs_dynamic_dynamic,
      self.pipeline.detect_resolve_iter);
    trace!("broad overlap pairs dynamic/static: {:?}",
      self.pipeline.broad_overlap_pairs_dynamic_static);
    trace!("broad overlap pairs dynamic/dynamic: {:?}",
      self.pipeline.broad_overlap_pairs_dynamic_dynamic);
  }

  /// Collects swept AABB TOIs from broad overlap pairs.
  ///
  /// 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>) {
    let last_toi = self.pipeline.resolved_collisions.last().map_or (
      0.0, |collision_resolve| *collision_resolve.toi);
    //
    //  dynamic v. static
    //
    for (dynamic_key, static_key) in
      self.pipeline.broad_overlap_pairs_dynamic_static.iter()
    {
      let dynamic_index = dynamic_key.index();
      let _static_index  = static_key.index();
      // should be safe to unwrap
      let dynamic_object         = objects_dynamic.get (dynamic_index).unwrap();
      let dynamic_velocity       = dynamic_object.derivatives.velocity;
      let dynamic_pseudovelocity = self.pseudo_velocities.get (dynamic_index)
        .unwrap();
      let dynamic_velocity_effective = dynamic_velocity + dynamic_pseudovelocity;
      let static_aabb            = self.broad.get_aabb_static (*static_key);
      let dynamic_aabb_current   = self.broad.get_aabb_dynamic_discrete (
        *dynamic_key);
      let _dynamic_aabb_swept    = self.broad.get_aabb_dynamic_continuous (
        *dynamic_key);
      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 =
        1.0 / dynamic_velocity_effective;
      // 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().x > static_aabb.min().x &&
          static_aabb.max().x > dynamic_aabb_current.min().x);
        debug_assert!(
          dynamic_aabb_previous.max().x > static_aabb.min().x &&
          static_aabb.max().x > dynamic_aabb_previous.min().x);
        (std::f64::NEG_INFINITY, std::f64::INFINITY)
      } else if dynamic_velocity_effective.x > 0.0 {
        ( (static_aabb.min().x - dynamic_aabb_previous.max().x) *
            dynamic_velocity_effective_reciprocal.x - t_margin_x,
          (static_aabb.max().x - dynamic_aabb_previous.min().x) *
            dynamic_velocity_effective_reciprocal.x + t_margin_x
        )
      } else {
        debug_assert!(dynamic_velocity_effective.x < 0.0);
        ( (static_aabb.max().x - dynamic_aabb_previous.min().x) *
            dynamic_velocity_effective_reciprocal.x - t_margin_x,
          (static_aabb.min().x - dynamic_aabb_previous.max().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().y > static_aabb.min().y &&
          static_aabb.max().y > dynamic_aabb_current.min().y);
        debug_assert!(
          dynamic_aabb_previous.max().y > static_aabb.min().y &&
          static_aabb.max().y > dynamic_aabb_previous.min().y);
        (std::f64::NEG_INFINITY, std::f64::INFINITY)
      } else if dynamic_velocity_effective.y > 0.0 {
        ( dynamic_velocity_effective_reciprocal.y *
            (static_aabb.min().y - dynamic_aabb_previous.max().y) - t_margin_y,
          dynamic_velocity_effective_reciprocal.y *
            (static_aabb.max().y - dynamic_aabb_previous.min().y) + t_margin_y
        )
      } else {
        debug_assert!(dynamic_velocity_effective.y < 0.0);
        ( dynamic_velocity_effective_reciprocal.y *
            (static_aabb.max().y - dynamic_aabb_previous.min().y) - t_margin_y,
          dynamic_velocity_effective_reciprocal.y *
            (static_aabb.min().y - dynamic_aabb_previous.max().y) + t_margin_y
        )
      };

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

      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 = (
            math::Normalized::clamp (f64::max (interval_start, last_toi)),
            math::Normalized::clamp (interval_end),
            InternalId::new_dynamic (*dynamic_key),
            InternalId::new_static  (*static_key)
          );
          match self.pipeline.mid_toi_pairs.insert (mid_toi_pair) {
            Ok  (_) => {}
            // TODO: compiler hint ?
            Err (_) => unreachable!()
          }
        }
      }
    }

    //
    //  dynamic v. dynamic
    //
    for (key_a, key_b) in
      self.pipeline.broad_overlap_pairs_dynamic_dynamic.iter()
    {
      // should be safe to unwrap: objects should not be destroyed during
      // collision detection
      let object_a = objects_dynamic.get (key_a.index()).unwrap();
      let object_b = objects_dynamic.get (key_b.index()).unwrap();
      let velocity_a        = object_a.derivatives.velocity;
      let velocity_b        = object_b.derivatives.velocity;
      let pseudovelocity_a  = self.pseudo_velocities.get (key_a.index()).unwrap();
      let pseudovelocity_b  = self.pseudo_velocities.get (key_b.index()).unwrap();
      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 =
        1.0 / velocity_effective_relative;
      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);
      // NB: div by zero will result in `inf` values
      // these values adjust start/end times to avoid getting too close
      let t_margin_x =
        (0.5 * CONTACT_DISTANCE * velocity_effective_relative_reciprocal.x).abs();
      let t_margin_y =
        (0.5 * CONTACT_DISTANCE * velocity_effective_relative_reciprocal.y).abs();
      let t_margin_z =
        (0.5 * CONTACT_DISTANCE * velocity_effective_relative_reciprocal.z).abs();

      let (interval_start_x, interval_end_x) = if
        velocity_effective_relative.x == 0.0
      {
        debug_assert!(
          aabb_current_a.max().x > aabb_previous_b.min().x &&
          aabb_previous_b.max().x > aabb_current_a.min().x);
        debug_assert!(
          aabb_previous_a.max().x > aabb_previous_b.min().x &&
          aabb_previous_b.max().x > aabb_previous_a.min().x);
        (std::f64::NEG_INFINITY, std::f64::INFINITY)
      } else if velocity_effective_relative.x > 0.0 {
        ( (aabb_previous_b.min().x - aabb_previous_a.max().x) *
            velocity_effective_relative_reciprocal.x - t_margin_x,
          (aabb_previous_b.max().x - aabb_previous_a.min().x) *
            velocity_effective_relative_reciprocal.x + t_margin_x
        )
      } else {
        debug_assert!(velocity_effective_relative.x < 0.0);
        ( (aabb_previous_b.max().x - aabb_previous_a.min().x) *
            velocity_effective_relative_reciprocal.x - t_margin_x,
          (aabb_previous_b.min().x - aabb_previous_a.max().x) *
            velocity_effective_relative_reciprocal.x + t_margin_x
        )
      };

      let (interval_start_y, interval_end_y) = if
        velocity_effective_relative.y == 0.0
      {
        debug_assert!(
          aabb_current_a.max().y > aabb_previous_b.min().y &&
          aabb_previous_b.max().y > aabb_current_a.min().y);
        debug_assert!(
          aabb_previous_a.max().y > aabb_previous_b.min().y &&
          aabb_previous_b.max().y > aabb_previous_a.min().y);
        (std::f64::NEG_INFINITY, std::f64::INFINITY)
      } else if velocity_effective_relative.y > 0.0 {
        ( velocity_effective_relative_reciprocal.y *
            (aabb_previous_b.min().y - aabb_previous_a.max().y) - t_margin_y,
          velocity_effective_relative_reciprocal.y *
            (aabb_previous_b.max().y - aabb_previous_a.min().y) + t_margin_y
        )
      } else {
        debug_assert!(velocity_effective_relative.y < 0.0);
        ( velocity_effective_relative_reciprocal.y *
            (aabb_previous_b.max().y - aabb_previous_a.min().y) - t_margin_y,
          velocity_effective_relative_reciprocal.y *
            (aabb_previous_b.min().y - aabb_previous_a.max().y) + t_margin_y
        )
      };

      let (interval_start_z, interval_end_z) = if
        velocity_effective_relative.z == 0.0
      {
        debug_assert!(
          aabb_current_a.max().z > aabb_previous_b.min().z &&
          aabb_previous_b.max().z > aabb_current_a.min().z);
        debug_assert!(
          aabb_previous_a.max().z > aabb_previous_b.min().z &&
          aabb_previous_b.max().z > aabb_previous_a.min().z);
        (std::f64::NEG_INFINITY, std::f64::INFINITY)
      } else if velocity_effective_relative.z > 0.0 {
        ( velocity_effective_relative_reciprocal.z *
            (aabb_previous_b.min().z - aabb_previous_a.max().z) - t_margin_z,
          velocity_effective_relative_reciprocal.z *
            (aabb_previous_b.max().z - aabb_previous_a.min().z) + t_margin_z
        )
      } else {
        debug_assert!(velocity_effective_relative.z < 0.0);
        ( velocity_effective_relative_reciprocal.z *
            (aabb_previous_b.max().z - aabb_previous_a.min().z) - t_margin_z,
          velocity_effective_relative_reciprocal.z *
            (aabb_previous_b.min().z - aabb_previous_a.max().z) + t_margin_z
        )
      };

      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 = (
            math::Normalized::clamp (f64::max (interval_start, last_toi)),
            math::Normalized::clamp (interval_end),
            InternalId::new_dynamic (*key_a),
            InternalId::new_dynamic (*key_b)
          );
          match self.pipeline.mid_toi_pairs.insert (mid_toi_pair) {
            Ok  (_) => {}
            // TODO: compiler hint ?
            Err (_) => unreachable!()
          }
        }
      }
    }

    trace!("mid TOI pairs: {:#?}", self.pipeline.mid_toi_pairs);

    self.pipeline.broad_overlap_pairs_dynamic_static.clear();
    self.pipeline.broad_overlap_pairs_dynamic_dynamic.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>
  ) {
    let narrow_toi_contacts = &mut self.pipeline.narrow_toi_contacts;

    while let Some (mid_toi_pair) = self.pipeline.mid_toi_pairs.last()
      .map (Clone::clone)
    {
      let earliest_toi_contact = if !narrow_toi_contacts.is_empty() {
        narrow_toi_contacts[0].0
      } else {
        math::Normalized::clamp (1.0)
      };

      if mid_toi_pair.0 < earliest_toi_contact {
        use cgmath::InnerSpace;

        let (mid_toi_start, _mid_toi_end, object_id_a, object_id_b) =
          mid_toi_pair.clone();
        let kind_a  = object_id_a.kind();
        let kind_b  = object_id_b.kind();
        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();
        match (kind_a, kind_b) {
          (object::Kind::Dynamic, object::Kind::Static)  => {
            // safe to unwrap
            let mut dynamic_object = objects_dynamic.get (index_a).unwrap()
              .clone();
            let dynamic_velocity   = dynamic_object.derivatives.velocity;
            let dynamic_pseudovelocity = self.pseudo_velocities.get (index_a)
              .unwrap();
            let dynamic_velocity_effective =
              dynamic_velocity - dynamic_pseudovelocity;
            let static_object      = objects_static.get (index_b)
              .unwrap().clone();
            let mut narrow_toi     = *mid_toi_start;
            lerp_object (&mut dynamic_object, dynamic_pseudovelocity, -1.0 + narrow_toi);
            let mut t_remaining    = 1.0 - narrow_toi;
            let mut iter           = 0;
            loop {
              // debug count max iters
              if cfg!(debug_assertions) {
                if DEBUG_NARROW_MAX_ITERS
                  .load (atomic::Ordering::SeqCst) - 1 < iter
                {
                  DEBUG_NARROW_MAX_ITERS.store (iter + 1,
                    atomic::Ordering::SeqCst)
                }
              }
              let proximity = Proximity::query (&dynamic_object, &static_object);
              debug_assert!(proximity.distance >= 0.0);
              let dynamic_velocity_effective_normal =
                dynamic_velocity_effective.dot (*proximity.normal);
              let dynamic_velocity_effective_normal_reciprocal =
                1.0 / dynamic_velocity_effective_normal;
              if dynamic_velocity_effective_normal >= 0.0 {
                // no collision: separating velocity zero or positive
                break
              } else if proximity.distance < CONTACT_DISTANCE {
                debug_assert!(dynamic_velocity_effective_normal < 0.0);
                // collision: toi contact
                let constraint  = constraint::Planar::new (
                  proximity.midpoint, proximity.normal);
                let contact     = Contact { constraint };
                let restitution = if narrow_toi == 0.0 {
                  0.0
                } else {
                  dynamic_object.material.restitution *
                    static_object.material.restitution
                };
                let collision = contact::Colliding { contact, restitution };
                let narrow_toi_contact = (math::Normalized::noisy (narrow_toi),
                  object_id_a, object_id_b, collision);
                match narrow_toi_contacts.insert (narrow_toi_contact) {
                  Ok  (_) => {}
                  // TODO: compiler hint ?
                  Err (_) => unreachable!()
                }
                break
              }
              let toi_axis = narrow_toi +
                (proximity.distance - 0.5 * CONTACT_DISTANCE) *
                  (-dynamic_velocity_effective_normal_reciprocal);
              debug_assert!(toi_axis > narrow_toi);
              if toi_axis > 1.0 {
                // no collision this step
                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);
            }
          }

          (object::Kind::Dynamic, object::Kind::Dynamic) => {
             // safe to unwrap: objects should not be destroyed during collision
             // detection
            let mut object_a     = objects_dynamic.get (index_a).unwrap()
              .clone();
            let mut object_b     = objects_dynamic.get (index_b).unwrap()
              .clone();
            let pseudovelocity_a = self.pseudo_velocities.get (index_a)
              .unwrap();
            let pseudovelocity_b = self.pseudo_velocities.get (index_b)
              .unwrap();
            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);
              debug_assert!(proximity.distance >= 0.0);
              let velocity_effective_relative_normal =
                velocity_effective_relative.dot (*proximity.normal);
              let velocity_effective_relative_normal_reciprocal =
                1.0 / velocity_effective_relative_normal;
              if velocity_effective_relative_normal >= 0.0 {
                // no collision: separating velocity zero or positive
                break
              } else if proximity.distance < CONTACT_DISTANCE {
                debug_assert!(velocity_effective_relative_normal < 0.0);
                // collision: toi contact
                let constraint  = constraint::Planar::new (
                  proximity.midpoint, proximity.normal);
                let contact     = Contact { constraint };
                let restitution = if narrow_toi == 0.0 {
                  0.0
                } else {
                  object_a.material.restitution * object_b.material.restitution
                };
                let collision   = contact::Colliding { contact, restitution };
                let narrow_toi_contact = (math::Normalized::noisy (narrow_toi),
                  object_id_a, object_id_b, collision);
                match narrow_toi_contacts.insert (narrow_toi_contact) {
                  Ok  (_) => {}
                  // TODO: compiler hint ?
                  Err (_) => unreachable!()
                }
                break
              }
              let toi_axis = narrow_toi +
                (proximity.distance - 0.5 * CONTACT_DISTANCE) *
                  (-velocity_effective_relative_normal_reciprocal);
              debug_assert!(toi_axis > narrow_toi);
              if toi_axis > 1.0 {
                // no collision this step
                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 }
    }

    if cfg!(debug_assertions) {
      let narrow_max_iters =
        DEBUG_NARROW_MAX_ITERS.load (atomic::Ordering::SeqCst);
      trace!("narrow TOI max iters: {}", narrow_max_iters);
    }

    trace!("narrow TOI contacts: {:#?}", narrow_toi_contacts);
  }

  #[inline]
  fn add_object_static (&mut self,
    object     : &object::Static,
    object_key : object::Key,
  ) {
    use geometry::shape::Aabb;
    self.broad.add_object_static (object.aabb(), object_key);
  }

  #[inline]
  fn add_object_dynamic (&mut self,
    object     : &object::Dynamic,
    object_key : object::Key
  ) {
    use cgmath::Zero;
    use geometry::shape::Aabb;
    let aabb_discrete   = object.aabb();
    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(), cgmath::Vector3::zero()).is_none());
  }

}

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

  /// After a collision is resolved, all modified objects should be removed from
  /// intermediate results in the pipeline:
  ///
  /// - `mid_toi_pairs`
  /// - `narrow_toi_contacts`
  ///
  /// These objects will be checked for any new collisions in the broad phase of
  /// the next detect/resolve loop iteration.
  #[inline]
  pub fn remove_resort_keys (&mut self, resort_keys : &SortedVec <object::Key>) {
    debug_assert!(self.broad_overlap_pairs_dynamic_static.is_empty());
    debug_assert!(self.broad_overlap_pairs_dynamic_dynamic.is_empty());

    let (mut i, mut len);

    // remove intermediate mid TOI pairs
    i   = 0;
    len = self.mid_toi_pairs.len();
    while i < len {
      let (_,_, id_a, id_b) = self.mid_toi_pairs[i].clone();
      debug_assert_eq!(id_a.kind(), object::Kind::Dynamic);
      if resort_keys.binary_search (&id_a.key()).is_ok() {
        self.mid_toi_pairs.remove_index (i);
        len -= 1;
        continue
      }
      if id_b.kind() == object::Kind::Dynamic {
        if resort_keys.binary_search (&id_b.key()).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 (_, id_a, id_b, _) = self.narrow_toi_contacts[i].clone();
      debug_assert_eq!(id_a.kind(), object::Kind::Dynamic);
      if resort_keys.binary_search (&id_a.key()).is_ok() {
        self.narrow_toi_contacts.remove_index (i);
        len -= 1;
        continue
      }
      if id_b.kind() == object::Kind::Dynamic {
        if resort_keys.binary_search (&id_b.key()).is_ok() {
          self.narrow_toi_contacts.remove_index (i);
          len -= 1;
          continue
        }
      }
      i += 1;
    }
  }
}

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::Identifier {
  fn from (id : InternalId) -> Self {
    object::Identifier {
      kind: id.kind(),
      key:  id.key()
    }
  }
}