// Copyright 2025 International Digital Economy Academy
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

///|
/// Real dim3 physics pipeline (broadphase + narrowphase + contact solver).
///
/// This is an incremental replacement for the current dim3 compatibility layer.
pub struct PhysicsPipeline3DReal {
  mut contact_cache : ContactSolverCache3D
  priv mut contact_twist_cache : @hashmap.HashMap[
    (Int, Int, Int, Int),
    @core.Real,
  ]
  sensor_pairs : Array[
    (@collision.ColliderHandle3D, @collision.ColliderHandle3D),
  ]
  contact_pairs : Array[
    (@collision.ColliderHandle3D, @collision.ColliderHandle3D),
  ]
  intersection_pairs : Array[
    (@collision.ColliderHandle3D, @collision.ColliderHandle3D),
  ]
}

///|
pub fn PhysicsPipeline3DReal::PhysicsPipeline3DReal() -> PhysicsPipeline3DReal {
  {
    contact_cache: ContactSolverCache3D(),
    contact_twist_cache: HashMap([]),
    sensor_pairs: [],
    contact_pairs: [],
    intersection_pairs: [],
  }
}

///|
fn integrate_lin_ang_3d_real(
  dt : @core.Real,
  curr_pos : @core.Isometry3,
  local_com : @core.Vec3,
  linvel : @core.Vec3,
  angvel : @core.Vec3,
) -> @core.Isometry3 {
  let com = curr_pos.transform_point(local_com)
  let shift = @core.Isometry3::from_translation(Vec3(-com.x, -com.y, -com.z))
  let rot = @core.Isometry3(
    @core.Vec3::zero(),
    @core.rotation_from_scaled_axis(angvel.scale(dt)),
  )
  let translation = linvel.scale(dt)
  let unshift = @core.Isometry3::from_translation(com.add(translation))
  unshift.mul(rot).mul(shift).mul(curr_pos)
}

///|
fn shape_bounding_radius_3d_real(shape : @collision.Shape3D) -> @core.Real {
  match shape {
    HalfSpace(_) => 0.0F
    _ => {
      let aabb = shape.local_aabb()
      let he = aabb.maxs.sub(aabb.mins).scale(0.5F)
      Float::sqrt(he.length_squared())
    }
  }
}

///|
fn shape_ccd_thickness_3d_real(shape : @collision.Shape3D) -> @core.Real {
  match shape {
    HalfSpace(_) => 0.0F
    Triangle(_, _, _) => 0.0F
    Heightfield(_, _, _, _, _) => 0.0F
    TriMesh(_, _) => 0.0F
    Compound(parts) => {
      if parts.length() == 0 {
        return 0.0F
      }
      let mut best = 1.0e30F
      for i in 0..= 1.0e29F {
        0.0F
      } else {
        best
      }
    }
    _ => {
      let aabb = shape.local_aabb()
      let he = aabb.maxs.sub(aabb.mins).scale(0.5F)
      let mut t = he.x
      if he.y < t {
        t = he.y
      }
      if he.z < t {
        t = he.z
      }
      if t > 0.0F {
        t
      } else {
        0.0F
      }
    }
  }
}

///|
fn body_ccd_metrics_3d_real(
  handle : @dynamics.RigidBodyHandle,
  colliders : @collision.ColliderSet3D,
) -> (@core.Real, @core.Real, Bool) {
  let mut ccd_thickness = 1.0e30F
  let mut ccd_max_dist = 0.0F
  let mut found = false
  let collider_handles = colliders.all_handles()
  for i in 0.. ccd_max_dist {
        ccd_max_dist = dist
      }
    }
  }
  if !found {
    (0.0F, 0.0F, false)
  } else if ccd_thickness >= 1.0e29F {
    (0.0F, ccd_max_dist, true)
  } else {
    (ccd_thickness, ccd_max_dist, true)
  }
}

///|
fn update_ccd_active_flags_3d_real(
  dt : @core.Real,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  _include_forces : Bool,
) -> Bool {
  if dt <= 0.0F {
    return false
  }
  let mut any_active = false
  let body_handles = bodies.all_handles()
  for i in 0.. 0.0F
      if !ccd_candidate ||
        !rb.body_type().is_dynamic_or_kinematic() ||
        !rb.is_active() {
        rb.set_ccd_active(false)
        continue
      }
      let (ccd_thickness, ccd_max_dist, has_attached_collider) = body_ccd_metrics_3d_real(
        bh, colliders,
      )
      if !has_attached_collider {
        rb.set_ccd_active(false)
        continue
      }
      let max_point_velocity = rb.linvel().length() +
        rb.angvel().length() * ccd_max_dist
      let threshold = ccd_thickness / 10.0F
      let moving_fast = max_point_velocity * dt > threshold
      rb.set_ccd_active(moving_fast)
      if moving_fast {
        any_active = true
      }
    }
  }
  any_active
}

///|
fn find_first_impact_3d_real(
  dt : @core.Real,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
) -> @core.Real? {
  if dt <= 0.0F {
    return None
  }
  let base_query = @collision.QueryPipeline3DReal(
    QueryFilter3DReal(),
    bodies,
    colliders,
  )
  let mut best : @core.Real? = None
  let collider_handles = colliders.all_handles()
  for i in 0..= 0.0F && toi < dt {
          if best is Some(curr) {
            if toi < curr {
              best = Some(toi)
            }
          } else {
            best = Some(toi)
          }
        }
      }
    }
  }
  best
}

///|
fn clamp_fast_ccd_body_motions_3d_real(
  dt : @core.Real,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  require_ccd_active : Bool,
) -> Unit {
  if dt <= 0.0F {
    return
  }
  let base_query = @collision.QueryPipeline3DReal(
    QueryFilter3DReal(),
    bodies,
    colliders,
  )
  let best_toi_by_body : @hashmap.HashMap[(Int, Int), @core.Real] = HashMap([])
  let collider_handles = colliders.all_handles()
  for i in 0..= 0.0F && toi < dt {
          let key = bh.into_raw_parts()
          if best_toi_by_body.get(key) is Some(best) {
            if toi < best {
              best_toi_by_body.set(key, toi)
            }
          } else {
            best_toi_by_body.set(key, toi)
          }
        }
      }
    }
  }
  let body_handles = bodies.all_handles()
  for i in 0.. 0.0F {
          let value = ccd_thickness * 0.15F / max_point_velocity
          if value < dt {
            value
          } else {
            dt
          }
        } else {
          dt
        }
        effective_toi = if toi > min_toi { toi } else { min_toi }
      }
      let s = effective_toi / dt
      if s < 1.0F {
        rb.set_linvel(rb.linvel().scale(s))
        rb.set_angvel(rb.angvel().scale(s))
      }
    }
  }
}

///|
fn pair_key_3d(
  a : @collision.ColliderHandle3D,
  b : @collision.ColliderHandle3D,
) -> (Int, Int, Int, Int) {
  let (ai, ag) = a.into_raw_parts()
  let (bi, bg) = b.into_raw_parts()
  if ai < bi || (ai == bi && ag <= bg) {
    (ai, ag, bi, bg)
  } else {
    (bi, bg, ai, ag)
  }
}

///|
fn canonical_pair_3d(
  a : @collision.ColliderHandle3D,
  b : @collision.ColliderHandle3D,
) -> (@collision.ColliderHandle3D, @collision.ColliderHandle3D) {
  let (ai, ag, bi, bg) = pair_key_3d(a, b)
  (
    @collision.ColliderHandle3D::from_raw_parts(ai, ag),
    @collision.ColliderHandle3D::from_raw_parts(bi, bg),
  )
}

///|
fn pair_less_3d(
  a : (@collision.ColliderHandle3D, @collision.ColliderHandle3D),
  b : (@collision.ColliderHandle3D, @collision.ColliderHandle3D),
) -> Bool {
  let ka = pair_key_3d(a.0, a.1)
  let kb = pair_key_3d(b.0, b.1)
  if ka.0 != kb.0 {
    return ka.0 < kb.0
  }
  if ka.1 != kb.1 {
    return ka.1 < kb.1
  }
  if ka.2 != kb.2 {
    return ka.2 < kb.2
  }
  ka.3 < kb.3
}

///|
fn sort_pairs_3d(
  pairs : Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)],
) -> Unit {
  // Insertion sort (small N in tests; replace with faster sort later).
  for i in 1.. 0 && pair_less_3d(pairs[j], pairs[j - 1]) {
      let tmp = pairs[j - 1]
      pairs[j - 1] = pairs[j]
      pairs[j] = tmp
      j = j - 1
    }
  }
}

///|
fn collect_sensor_pairs(
  narrow_phase : @collision.NarrowPhase3D,
  colliders : @collision.ColliderSet3D,
) -> Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)] {
  let out : Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)] = []
  let all = narrow_phase.all_intersection_pairs()
  for i in 0.. Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)] {
  let out : Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)] = []
  let all = narrow_phase.all_intersection_pairs()
  for i in 0.. Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)] {
  let out : Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)] = []
  let all = narrow_phase.all_contact_pairs()
  for i in 0.. Unit {
  let all = narrow_phase.all_contact_pairs()
  for i in 0.. Unit {
  fn stopped_flags(
    colliders : @collision.ColliderSet3D,
    a : @collision.ColliderHandle3D,
    b : @collision.ColliderHandle3D,
    flags : @collision.CollisionEventFlags,
  ) -> @collision.CollisionEventFlags {
    let mut out = flags
    if colliders.get(a) is None || colliders.get(b) is None {
      out = out.insert(@collision.CollisionEventFlags::removed())
    }
    out
  }

  sort_pairs_3d(prev_pairs)
  let mut i = 0
  let mut j = 0
  while i < prev_pairs.length() || j < next_pairs.length() {
    if i >= prev_pairs.length() {
      let p = next_pairs[j]
      handler.push_collision_event(Started(p.0, p.1, flags))
      j = j + 1
      continue
    }
    if j >= next_pairs.length() {
      let p = prev_pairs[i]
      let event_flags = stopped_flags(colliders, p.0, p.1, flags)
      handler.push_collision_event(Stopped(p.0, p.1, event_flags))
      i = i + 1
      continue
    }
    let a = prev_pairs[i]
    let b = next_pairs[j]
    if pair_less_3d(a, b) {
      let event_flags = stopped_flags(colliders, a.0, a.1, flags)
      handler.push_collision_event(Stopped(a.0, a.1, event_flags))
      i = i + 1
    } else if pair_less_3d(b, a) {
      handler.push_collision_event(Started(b.0, b.1, flags))
      j = j + 1
    } else {
      i = i + 1
      j = j + 1
    }
  }
}

///|
fn emit_intersection_events_for_pair_diffs(
  handler : EventHandler3D,
  prev_pairs : Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)],
  next_pairs : Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)],
) -> Unit {
  sort_pairs_3d(prev_pairs)
  let mut i = 0
  let mut j = 0
  while i < prev_pairs.length() || j < next_pairs.length() {
    if i >= prev_pairs.length() {
      let p = next_pairs[j]
      handler.push_intersection_event(IntersectionEvent3D(p.0, p.1, true))
      j = j + 1
      continue
    }
    if j >= next_pairs.length() {
      let p = prev_pairs[i]
      handler.push_intersection_event(IntersectionEvent3D(p.0, p.1, false))
      i = i + 1
      continue
    }
    let a = prev_pairs[i]
    let b = next_pairs[j]
    if pair_less_3d(a, b) {
      handler.push_intersection_event(IntersectionEvent3D(a.0, a.1, false))
      i = i + 1
    } else if pair_less_3d(b, a) {
      handler.push_intersection_event(IntersectionEvent3D(b.0, b.1, true))
      j = j + 1
    } else {
      i = i + 1
      j = j + 1
    }
  }
}

///|
fn rb_pair_key_3d(
  a : @dynamics.RigidBodyHandle,
  b : @dynamics.RigidBodyHandle,
) -> (Int, Int, Int, Int) {
  let (ai, ag) = a.into_raw_parts()
  let (bi, bg) = b.into_raw_parts()
  if ai < bi || (ai == bi && ag <= bg) {
    (ai, ag, bi, bg)
  } else {
    (bi, bg, ai, ag)
  }
}

///|
fn filter_broad_phase_pairs_3d(
  pairs : Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)],
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  joints : @dynamics.JointSet3DReal?,
  multibody_joints : @dynamics.MultibodyJointSet3DReal?,
  hooks : PhysicsHooks3D?,
) -> Array[(@collision.ColliderHandle3D, @collision.ColliderHandle3D)] {
  let mut disabled : @hashmap.HashMap[(Int, Int, Int, Int), Bool]? = None
  let disabled_body_pairs : Array[
    (@dynamics.RigidBodyHandle, @dynamics.RigidBodyHandle),
  ] = []
  if joints is Some(js) {
    disabled_body_pairs.append(js.disabled_contact_pairs()[:])
  }
  if multibody_joints is Some(multibody) {
    disabled_body_pairs.append(multibody.disabled_contact_pairs()[:])
  }
  if disabled_body_pairs.length() > 0 {
    let out : @hashmap.HashMap[(Int, Int, Int, Int), Bool] = HashMap(
      [],
      capacity=disabled_body_pairs.length() * 2,
    )
    for p in disabled_body_pairs {
      out.set(rb_pair_key_3d(p.0, p.1), true)
    }
    disabled = Some(out)
  }
  let filtered : Array[
    (@collision.ColliderHandle3D, @collision.ColliderHandle3D),
  ] = []
  for pair in pairs {
    let c1 = pair.0
    let c2 = pair.1
    if colliders.get(c1) is Some(co1) && colliders.get(c2) is Some(co2) {
      if disabled is Some(set) &&
        co1.parent() is Some(p1) &&
        co2.parent() is Some(p2) &&
        set.get(rb_pair_key_3d(p1, p2)) is Some(true) {
        continue
      }
      if hooks is Some(physics_hooks) {
        let keep = if co1.is_sensor() || co2.is_sensor() {
          let hook = @collision.ActiveHooks::filter_intersection_pair()
          if co1.active_hooks().contains(hook) ||
            co2.active_hooks().contains(hook) {
            physics_hooks.test_intersection_pair(bodies, colliders, c1, c2)
          } else {
            true
          }
        } else {
          let hook = @collision.ActiveHooks::filter_contact_pairs()
          if co1.active_hooks().contains(hook) ||
            co2.active_hooks().contains(hook) {
            physics_hooks.test_contact_pair(bodies, colliders, c1, c2)
          } else {
            true
          }
        }
        if !keep {
          continue
        }
      }
      filtered.push((c1, c2))
    }
  }
  filtered
}

///|
fn collect_solver_interactions_3d_real(
  narrow_phase : @collision.NarrowPhase3D,
  colliders : @collision.ColliderSet3D,
  rope_joints : @dynamics.RopeJointSet3DReal?,
  joints : @dynamics.JointSet3DReal?,
  multibody_joints : @dynamics.MultibodyJointSet3DReal?,
) -> Array[(@dynamics.RigidBodyHandle, @dynamics.RigidBodyHandle)] {
  let interactions : Array[
    (@dynamics.RigidBodyHandle, @dynamics.RigidBodyHandle),
  ] = []
  let interaction_keys : @hashmap.HashMap[(Int, Int, Int, Int), Bool] = HashMap([],
  )
  fn push_unique_interaction(
    interactions : Array[(@dynamics.RigidBodyHandle, @dynamics.RigidBodyHandle)],
    interaction_keys : @hashmap.HashMap[(Int, Int, Int, Int), Bool],
    body1 : @dynamics.RigidBodyHandle,
    body2 : @dynamics.RigidBodyHandle,
  ) -> Unit {
    let key = rb_pair_key_3d(body1, body2)
    if interaction_keys.get(key) is Some(true) {
      return
    }
    interaction_keys.set(key, true)
    interactions.push((body1, body2))
  }

  let pairs = narrow_phase.all_contact_pairs()
  for i in 0.. Int {
  let base_iters = if parameters.num_solver_iterations > 0 {
    parameters.num_solver_iterations
  } else {
    1
  }
  let extra_iters = islands.island_additional_solver_iterations(island_id)
  if extra_iters > 0 {
    base_iters + extra_iters
  } else {
    base_iters
  }
}

///|
pub fn PhysicsPipeline3DReal::step(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    None,
    None,
    None,
    None,
    None,
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_hooks(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  hooks : PhysicsHooks3D,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    None,
    None,
    None,
    None,
    Some(hooks),
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_events(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  events : EventHandler3D,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    Some(events),
    None,
    None,
    None,
    None,
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_events_and_hooks(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  events : EventHandler3D,
  hooks : PhysicsHooks3D,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    Some(events),
    None,
    None,
    None,
    Some(hooks),
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_events_and_joints(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  events : EventHandler3D,
  rope_joints : @dynamics.RopeJointSet3DReal,
  joints : @dynamics.JointSet3DReal,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    Some(events),
    Some(rope_joints),
    Some(joints),
    None,
    None,
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_events_joints_and_hooks(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  events : EventHandler3D,
  rope_joints : @dynamics.RopeJointSet3DReal,
  joints : @dynamics.JointSet3DReal,
  hooks : PhysicsHooks3D,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    Some(events),
    Some(rope_joints),
    Some(joints),
    None,
    Some(hooks),
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_events_and_joint_sets(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  events : EventHandler3D,
  rope_joints : @dynamics.RopeJointSet3DReal,
  joints : @dynamics.JointSet3DReal,
  multibody_joints : @dynamics.MultibodyJointSet3DReal,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    Some(events),
    Some(rope_joints),
    Some(joints),
    Some(multibody_joints),
    None,
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_events_joint_sets_and_hooks(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  events : EventHandler3D,
  rope_joints : @dynamics.RopeJointSet3DReal,
  joints : @dynamics.JointSet3DReal,
  multibody_joints : @dynamics.MultibodyJointSet3DReal,
  hooks : PhysicsHooks3D,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    Some(events),
    Some(rope_joints),
    Some(joints),
    Some(multibody_joints),
    Some(hooks),
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_rope_joints(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  rope_joints : @dynamics.RopeJointSet3DReal,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    None,
    Some(rope_joints),
    None,
    None,
    None,
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_rope_joints_and_hooks(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  rope_joints : @dynamics.RopeJointSet3DReal,
  hooks : PhysicsHooks3D,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    None,
    Some(rope_joints),
    None,
    None,
    Some(hooks),
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_joints(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  joints : @dynamics.JointSet3DReal,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    None,
    None,
    Some(joints),
    None,
    None,
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_joints_and_hooks(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  joints : @dynamics.JointSet3DReal,
  hooks : PhysicsHooks3D,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    None,
    None,
    Some(joints),
    None,
    Some(hooks),
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_multibody_joints(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  multibody_joints : @dynamics.MultibodyJointSet3DReal,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    None,
    None,
    None,
    Some(multibody_joints),
    None,
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_multibody_joints_and_hooks(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  multibody_joints : @dynamics.MultibodyJointSet3DReal,
  hooks : PhysicsHooks3D,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    None,
    None,
    None,
    Some(multibody_joints),
    Some(hooks),
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_events_and_multibody_joints(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  events : EventHandler3D,
  multibody_joints : @dynamics.MultibodyJointSet3DReal,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    Some(events),
    None,
    None,
    Some(multibody_joints),
    None,
  )
}

///|
pub fn PhysicsPipeline3DReal::step_with_events_multibody_joints_and_hooks(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  events : EventHandler3D,
  multibody_joints : @dynamics.MultibodyJointSet3DReal,
  hooks : PhysicsHooks3D,
) -> Unit {
  PhysicsPipeline3DReal::step_impl(
    self,
    gravity,
    parameters,
    islands,
    broad_phase,
    narrow_phase,
    bodies,
    colliders,
    Some(events),
    None,
    None,
    Some(multibody_joints),
    Some(hooks),
  )
}

///|
fn PhysicsPipeline3DReal::step_impl(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  events : EventHandler3D?,
  rope_joints : @dynamics.RopeJointSet3DReal?,
  joints : @dynamics.JointSet3DReal?,
  multibody_joints : @dynamics.MultibodyJointSet3DReal?,
  hooks : PhysicsHooks3D?,
) -> Unit {
  let dt = parameters.dt
  if dt <= 0.0F {
    return
  }
  if multibody_joints is Some(multibody) {
    let to_wake_up = multibody.take_wake_up()
    for handle in to_wake_up {
      if bodies.get(handle) is Some(body) {
        body.wake_up()
      }
    }
    multibody.take_to_join() |> ignore
    multibody.sync_rigid_bodies_from_multibodies(bodies)
    colliders.sync_with_bodies(bodies)
  }
  let ccd_enabled = parameters.max_ccd_substeps > 0
  let mut remaining_time = dt
  let mut remaining_substeps = if ccd_enabled {
    parameters.max_ccd_substeps
  } else {
    1
  }
  while remaining_substeps > 0 && remaining_time > 0.0F {
    let mut sub_dt = remaining_time
    if ccd_enabled && remaining_substeps > 1 {
      let ccd_active = update_ccd_active_flags_3d_real(
        remaining_time, bodies, colliders, true,
      )
      let first_impact = if ccd_active {
        find_first_impact_3d_real(remaining_time, bodies, colliders)
      } else {
        None
      }
      if first_impact is Some(toi) {
        let interval = remaining_time / Float::from_int(remaining_substeps)
        sub_dt = if toi < interval {
          interval
        } else {
          toi + (remaining_time - toi) / Float::from_int(remaining_substeps)
        }
        remaining_substeps = remaining_substeps - 1
      } else {
        sub_dt = remaining_time
        remaining_substeps = 0
      }
      remaining_time = remaining_time - sub_dt
      if remaining_time <= parameters.min_ccd_dt {
        sub_dt = sub_dt + remaining_time
        remaining_time = 0.0F
        remaining_substeps = 0
      }
    } else {
      remaining_time = 0.0F
      remaining_substeps = 0
    }
    if sub_dt > 0.0F {
      let sub_params = parameters.with_dt(sub_dt)
      PhysicsPipeline3DReal::step_impl_one(
        self, gravity, sub_params, islands, broad_phase, narrow_phase, bodies, colliders,
        events, rope_joints, joints, multibody_joints, hooks, ccd_enabled,
      )
    }
  }
}

///|
fn PhysicsPipeline3DReal::step_impl_one(
  self : PhysicsPipeline3DReal,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3D,
  broad_phase : @collision.BroadPhase3D,
  narrow_phase : @collision.NarrowPhase3D,
  bodies : @dynamics.RigidBodySet3D,
  colliders : @collision.ColliderSet3D,
  events : EventHandler3D?,
  rope_joints : @dynamics.RopeJointSet3DReal?,
  joints : @dynamics.JointSet3DReal?,
  multibody_joints : @dynamics.MultibodyJointSet3DReal?,
  hooks : PhysicsHooks3D?,
  ccd_enabled : Bool,
) -> Unit {
  let dt = parameters.dt
  bodies.apply_gravity_all(gravity, dt)
  bodies.apply_damping_all(dt)
  bodies.apply_gyroscopic_forces_all(dt)
  bodies.update_kinematic_velocities_all(dt)
  if multibody_joints is Some(multibody) {
    multibody.sync_rigid_bodies_from_multibodies(bodies)
  }

  // Sync colliders to body motion before detecting contacts.
  colliders.sync_with_bodies(bodies)
  let prediction_distance = parameters.prediction_distance()
  broad_phase.update(prediction_distance, colliders)
  let pairs = filter_broad_phase_pairs_3d(
    broad_phase.pairs(),
    bodies,
    colliders,
    joints,
    multibody_joints,
    hooks,
  )
  narrow_phase.update(pairs, bodies, colliders, prediction_distance, dt)
  let solver_interactions = collect_solver_interactions_3d_real(
    narrow_phase, colliders, rope_joints, joints, multibody_joints,
  )
  islands.update(bodies, solver_interactions)
  let active_island_ids = islands.active_islands()
  let island_stamps : Array[Int] = []
  for _ in 0..= 0 && id < island_stamps.length() {
        island_stamps[id] = stamp
      }
    }
  }
  let has_joint_constraints = rope_joints is Some(_) ||
    joints is Some(_) ||
    multibody_joints is Some(_)
  let coupling_passes = if has_joint_constraints { 2 } else { 1 }
  let mut cache_in = self.contact_cache
  let mut twist_cache_in = self.contact_twist_cache
  for pass in 0.. 0 {
      for island_idx in 0..