///|
/// Baumgarte bias factor: how aggressively penetration is pushed out.
const BIAS_FACTOR : Double = 0.2

///|
/// Relative normal speed above which restitution is applied.
const RESTITUTION_THRESHOLD : Double = 1.0

///|
/// Persistent contact constraints between two bodies.
///
/// Impulses are accumulated and clamped, and are carried over to the next
/// frame by matching contact feature ids, which is what makes stacks stable.
pub struct Arbiter {
  body_a : Int
  body_b : Int
  mut contacts : Array[ContactPoint]
  mut normal_impulses : Array[Double]
  mut tangent_impulses : Array[Double]
  mut mass_normal : Array[Double]
  mut mass_tangent : Array[Double]
  mut bias : Array[Double]
  mut restitution_bias : Array[Double]
  mut friction : Double
  mut restitution : Double
}

///|
pub fn Arbiter::new(
  body_a : Int,
  body_b : Int,
  contacts : Array[ContactPoint],
  friction : Double,
  restitution : Double,
) -> Arbiter {
  let n = contacts.length()
  {
    body_a,
    body_b,
    contacts,
    normal_impulses: Array::make(n, 0.0),
    tangent_impulses: Array::make(n, 0.0),
    mass_normal: Array::make(n, 0.0),
    mass_tangent: Array::make(n, 0.0),
    bias: Array::make(n, 0.0),
    restitution_bias: Array::make(n, 0.0),
    friction,
    restitution,
  }
}

///|
pub fn Arbiter::body_a(self : Arbiter) -> Int {
  self.body_a
}

///|
pub fn Arbiter::body_b(self : Arbiter) -> Int {
  self.body_b
}

///|
pub fn Arbiter::contacts(self : Arbiter) -> Array[ContactPoint] {
  self.contacts
}

///|
pub fn Arbiter::is_touching(self : Arbiter) -> Bool {
  self.contacts.length() > 0
}

///|
/// Refresh the manifold, keeping impulses of surviving points.
pub fn Arbiter::update(
  self : Arbiter,
  contacts : Array[ContactPoint],
  friction : Double,
  restitution : Double,
) -> Unit {
  let n = contacts.length()
  let normal_impulses = Array::make(n, 0.0)
  let tangent_impulses = Array::make(n, 0.0)
  for i in 0.. Unit {
  body_a.velocity = body_a.velocity - impulse.scale(body_a.inv_mass)
  body_a.angular_velocity = body_a.angular_velocity -
    body_a.inv_inertia * r_a.cross(impulse)
  body_b.velocity = body_b.velocity + impulse.scale(body_b.inv_mass)
  body_b.angular_velocity = body_b.angular_velocity +
    body_b.inv_inertia * r_b.cross(impulse)
}

///|
/// Compute effective masses and bias terms, then warm start.
pub fn Arbiter::pre_step(
  self : Arbiter,
  bodies : Array[Body],
  inv_dt : Double,
) -> Unit {
  let body_a = bodies[self.body_a]
  let body_b = bodies[self.body_b]
  let n = self.contacts.length()
  for i in 0.. K_EPSILON {
      1.0 / k_normal
    } else {
      0.0
    }
    let tangent = normal.perp()
    let rt_a = r_a.cross(tangent)
    let rt_b = r_b.cross(tangent)
    let k_tangent = body_a.inv_mass +
      body_b.inv_mass +
      body_a.inv_inertia * rt_a * rt_a +
      body_b.inv_inertia * rt_b * rt_b
    self.mass_tangent[i] = if k_tangent > K_EPSILON {
      1.0 / k_tangent
    } else {
      0.0
    }
    self.bias[i] = -BIAS_FACTOR *
      inv_dt *
      (contact.separation + LINEAR_SLOP).min(0.0)
    let relative = body_b.point_velocity(contact.position) -
      body_a.point_velocity(contact.position)
    let vn = relative.dot(normal)
    self.restitution_bias[i] = if vn < -RESTITUTION_THRESHOLD {
      -self.restitution * vn
    } else {
      0.0
    }
  }
  for i in 0.. Unit {
  let body_a = bodies[self.body_a]
  let body_b = bodies[self.body_b]
  let n = self.contacts.length()
  for i in 0.. Double {
  let mut sum = 0.0
  for i in 0..