// 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.

///|
/// Dim3 shape set for the real 3D backend.
pub struct Voxels3DReal {
  voxel_size : @core.Vec3
  columns : @hashmap.HashMap[(Int, Int), Array[Int]] // (ix, iz) -> sorted ys
  dense_origin_x : Int
  dense_origin_z : Int
  dense_nx : Int
  dense_nz : Int
  dense_columns : Array[Array[Int]?]?
  keys : Array[(Int, Int, Int)] // deterministic feature ids
  key_to_id : @hashmap.HashMap[(Int, Int, Int), Int]
  min_key : (Int, Int, Int)
  max_key : (Int, Int, Int)
}

///|
pub fn Voxels3DReal::column_ys(
  self : Voxels3DReal,
  ix : Int,
  iz : Int,
) -> Array[Int]? {
  if self.dense_columns is Some(dense) && self.dense_nx > 0 && self.dense_nz > 0 {
    let dx = ix - self.dense_origin_x
    let dz = iz - self.dense_origin_z
    if dx >= 0 && dz >= 0 && dx < self.dense_nx && dz < self.dense_nz {
      dense[dx + dz * self.dense_nx]
    } else {
      self.columns.get((ix, iz))
    }
  } else {
    self.columns.get((ix, iz))
  }
}

///|
pub(all) enum Shape3D {
  Ball(@core.Real)
  Cuboid(@core.Vec3) // half extents
  CapsuleY(@core.Real, @core.Real) // radius, half_height
  Cylinder(@core.Real, @core.Real) // radius, half_height (aligned with Y)
  RoundCylinder(@core.Real, @core.Real, @core.Real) // radius, half_height, border_radius
  Cone(@core.Real, @core.Real) // radius, half_height (aligned with Y)
  // Infinite half-space bounded by a plane passing through the origin of the shape local-space.
  // The half-space solid is on the side opposite to the normal direction (i.e. points satisfy n·x <= 0).
  HalfSpace(@core.Vec3)
  // A single triangle surface.
  Triangle(@core.Vec3, @core.Vec3, @core.Vec3)
  // Convex hull represented by its support points (may include interior points).
  // The optional border radius represents the Minkowski sum with a ball.
  ConvexHull(Array[@core.Vec3], @core.Real)
  // A compound made of a list of (local pose, shape) pairs.
  Compound(Array[(@core.Isometry3, Shape3D)])
  // A sparse voxel set representing the union of axis-aligned boxes on a regular grid.
  Voxels(Voxels3DReal)
  // Heightfield stored as a triangle mesh plus grid dimensions (rows/cols) for future optimizations.
  // `flags` matches Rapier/Parry `HeightFieldFlags` bitflags (currently only FIX_INTERNAL_EDGES).
  Heightfield(Array[@core.Vec3], Array[(Int, Int, Int)], Int, Int, Int)
  TriMesh(Array[@core.Vec3], Array[(Int, Int, Int)])
}

///|
const HALF_PI_3D : @core.Real = 1.5707964F

///|
pub fn Shape3D::ball(radius : @core.Real) -> Shape3D {
  Ball(radius)
}

///|
pub fn Shape3D::cuboid(
  hx : @core.Real,
  hy : @core.Real,
  hz : @core.Real,
) -> Shape3D {
  Cuboid(Vec3(hx, hy, hz))
}

///|
pub fn Shape3D::capsule_y(
  radius : @core.Real,
  half_height : @core.Real,
) -> Shape3D {
  CapsuleY(radius, half_height)
}

///|
pub fn Shape3D::cylinder(
  radius : @core.Real,
  half_height : @core.Real,
) -> Shape3D {
  Cylinder(radius, half_height)
}

///|
pub fn Shape3D::round_cylinder(
  radius : @core.Real,
  half_height : @core.Real,
  border_radius : @core.Real,
) -> Shape3D {
  RoundCylinder(radius, half_height, border_radius)
}

///|
pub fn Shape3D::cone(radius : @core.Real, half_height : @core.Real) -> Shape3D {
  Cone(radius, half_height)
}

///|
pub fn Shape3D::halfspace(normal : @core.Vec3) -> Shape3D {
  HalfSpace(normal)
}

///|
pub fn Shape3D::triangle(
  a : @core.Vec3,
  b : @core.Vec3,
  c : @core.Vec3,
) -> Shape3D {
  Triangle(a, b, c)
}

///|
pub fn Shape3D::local_aabb(self : Shape3D) -> @core.Aabb3 {
  match self {
    Ball(r) => Aabb3(Vec3(-r, -r, -r), Vec3(r, r, r))
    Cuboid(he) => Aabb3(Vec3(-he.x, -he.y, -he.z), Vec3(he.x, he.y, he.z))
    CapsuleY(r, hh) => Aabb3(Vec3(-r, -(hh + r), -r), Vec3(r, hh + r, r))
    Cylinder(r, hh) => Aabb3(Vec3(-r, -hh, -r), Vec3(r, hh, r))
    RoundCylinder(r, hh, br) => {
      let rr = r + br
      let yy = hh + br
      Aabb3(Vec3(-rr, -yy, -rr), Vec3(rr, yy, rr))
    }
    Cone(r, hh) => Aabb3(Vec3(-r, -hh, -r), Vec3(r, hh, r))
    HalfSpace(_) => {
      // The BVH broad-phase treats half-spaces specially (they are infinite). We still provide
      // a very large AABB as a defensive fallback for any code-path that expects a finite AABB.
      let m = 1.0e6F
      Aabb3(Vec3(-m, -m, -m), Vec3(m, m, m))
    }
    Triangle(a, b, c) => {
      let mut min = a
      let mut max = a
      for p in [b, c] {
        if p.x < min.x {
          min = Vec3(p.x, min.y, min.z)
        }
        if p.y < min.y {
          min = Vec3(min.x, p.y, min.z)
        }
        if p.z < min.z {
          min = Vec3(min.x, min.y, p.z)
        }
        if p.x > max.x {
          max = Vec3(p.x, max.y, max.z)
        }
        if p.y > max.y {
          max = Vec3(max.x, p.y, max.z)
        }
        if p.z > max.z {
          max = Vec3(max.x, max.y, p.z)
        }
      }
      Aabb3(min, max)
    }
    ConvexHull(points, br) =>
      if points.length() == 0 {
        Aabb3(@core.Vec3::zero(), @core.Vec3::zero())
      } else {
        let mut min = points[0]
        let mut max = points[0]
        for i in 1.. max.x {
            max = Vec3(p.x, max.y, max.z)
          }
          if p.y > max.y {
            max = Vec3(max.x, p.y, max.z)
          }
          if p.z > max.z {
            max = Vec3(max.x, max.y, p.z)
          }
        }
        let pad = @core.Vec3(br, br, br)
        Aabb3(min.sub(pad), max.add(pad))
      }
    Compound(parts) =>
      if parts.length() == 0 {
        Aabb3(@core.Vec3::zero(), @core.Vec3::zero())
      } else {
        // Union all child AABBs transformed into the compound's local space.
        let mut aabb = aabb3_transform(parts[0].0, parts[0].1.local_aabb())
        for i in 1.. {
      let (ix0, iy0, iz0) = v.min_key
      let (ix1, iy1, iz1) = v.max_key
      let sx = v.voxel_size.x
      let sy = v.voxel_size.y
      let sz = v.voxel_size.z
      let min = @core.Vec3(
        Float::from_int(ix0) * sx,
        Float::from_int(iy0) * sy,
        Float::from_int(iz0) * sz,
      )
      let max = @core.Vec3(
        Float::from_int(ix1 + 1) * sx,
        Float::from_int(iy1 + 1) * sy,
        Float::from_int(iz1 + 1) * sz,
      )
      Aabb3(min, max)
    }
    Heightfield(vertices, _, _, _, _) =>
      if vertices.length() == 0 {
        Aabb3(@core.Vec3::zero(), @core.Vec3::zero())
      } else {
        let mut min = vertices[0]
        let mut max = vertices[0]
        for i in 1.. max.x {
            max = Vec3(v.x, max.y, max.z)
          }
          if v.y > max.y {
            max = Vec3(max.x, v.y, max.z)
          }
          if v.z > max.z {
            max = Vec3(max.x, max.y, v.z)
          }
        }
        Aabb3(min, max)
      }
    TriMesh(vertices, _) =>
      if vertices.length() == 0 {
        Aabb3(@core.Vec3::zero(), @core.Vec3::zero())
      } else {
        let mut min = vertices[0]
        let mut max = vertices[0]
        for i in 1.. max.x {
            max = Vec3(v.x, max.y, max.z)
          }
          if v.y > max.y {
            max = Vec3(max.x, v.y, max.z)
          }
          if v.z > max.z {
            max = Vec3(max.x, max.y, v.z)
          }
        }
        Aabb3(min, max)
      }
  }
}

///|
fn abs_mat3(m : @core.Mat3) -> @core.Mat3 {
  Mat3(
    @core.abs(m.m00),
    @core.abs(m.m01),
    @core.abs(m.m02),
    @core.abs(m.m10),
    @core.abs(m.m11),
    @core.abs(m.m12),
    @core.abs(m.m20),
    @core.abs(m.m21),
    @core.abs(m.m22),
  )
}

///|
fn aabb3_transform(iso : @core.Isometry3, aabb : @core.Aabb3) -> @core.Aabb3 {
  // Conservative (and exact for OBBs) transform using center+extents.
  let center = aabb.mins.add(aabb.maxs).scale(0.5F)
  let half = aabb.maxs.sub(aabb.mins).scale(0.5F)
  let rot = abs_mat3(iso.rotation.to_mat3())
  let ext = rot.mul_vec3(half)
  let world_center = iso.transform_point(center)
  Aabb3(world_center.sub(ext), world_center.add(ext))
}

///|
pub struct Collider3D {
  mut position : @core.Isometry3
  mut local_position : @core.Isometry3
  mut parent : @dynamics.RigidBodyHandle?
  mut shape : Shape3D
  // Optional per-feature metadata for voxel colliders created with `voxels_from_points`.
  // This maps `RayIntersection3Feature.feature_id` back to a voxel key.
  voxel_tri_map : Array[(Int, Int, Int)]?
  mut enabled : Bool
  sensor : Bool
  // Solver-only features used by a few examples/tests (e.g. one-way platforms).
  mut surface_velocity : @core.Vec3
  mut one_way_offset : @core.Real?
  mut one_way_direction : Int // 1 = allow from above, -1 = allow from below, 0 = disabled
  mut active_collision_types : ActiveCollisionTypes
  mut active_hooks : ActiveHooks
  mut active_events : ActiveEvents
  mut contact_force_event_threshold : @core.Real
  mut user_data : @core.UserData128
  mut collision_groups : @dynamics.InteractionGroups
  mut solver_groups : @dynamics.InteractionGroups
  mut contact_skin : @core.Real
  mut friction : @core.Real
  mut friction_combine_rule : @dynamics.CoefficientCombineRule
  mut restitution : @core.Real
  mut restitution_combine_rule : @dynamics.CoefficientCombineRule
}

///|
pub fn Collider3D::position(self : Collider3D) -> @core.Isometry3 {
  self.position
}

///|
pub fn Collider3D::set_position(
  self : Collider3D,
  pos : @core.Isometry3,
) -> Unit {
  self.position = pos
}

///|
pub fn Collider3D::local_position(self : Collider3D) -> @core.Isometry3 {
  self.local_position
}

///|
pub fn Collider3D::set_local_position(
  self : Collider3D,
  pos : @core.Isometry3,
) -> Unit {
  self.local_position = pos
}

///|
pub fn Collider3D::parent(self : Collider3D) -> @dynamics.RigidBodyHandle? {
  self.parent
}

///|
pub fn Collider3D::set_parent(
  self : Collider3D,
  parent : @dynamics.RigidBodyHandle?,
) -> Unit {
  self.parent = parent
}

///|
pub fn Collider3D::shape(self : Collider3D) -> Shape3D {
  self.shape
}

///|
pub fn Collider3D::set_shape(self : Collider3D, shape : Shape3D) -> Unit {
  self.shape = shape
}

///|
pub fn Collider3D::voxel_key_for_triangle(
  self : Collider3D,
  feature_id : Int,
) -> (Int, Int, Int)? {
  if self.voxel_tri_map is Some(map) {
    if feature_id >= 0 && feature_id < map.length() {
      Some(map[feature_id])
    } else {
      None
    }
  } else {
    None
  }
}

///|
pub fn Collider3D::is_sensor(self : Collider3D) -> Bool {
  self.sensor
}

///|
pub fn Collider3D::surface_velocity(self : Collider3D) -> @core.Vec3 {
  self.surface_velocity
}

///|
pub fn Collider3D::set_surface_velocity(
  self : Collider3D,
  v : @core.Vec3,
) -> Unit {
  self.surface_velocity = v
}

///|
pub fn Collider3D::one_way(self : Collider3D) -> (@core.Real, Int)? {
  if self.one_way_direction == 0 || self.one_way_offset is None {
    None
  } else if self.one_way_offset is Some(offset) {
    Some((offset, self.one_way_direction))
  } else {
    None
  }
}

///|
pub fn Collider3D::set_one_way_above(
  self : Collider3D,
  offset : @core.Real,
) -> Unit {
  self.one_way_offset = Some(offset)
  self.one_way_direction = 1
}

///|
pub fn Collider3D::set_one_way_below(
  self : Collider3D,
  offset : @core.Real,
) -> Unit {
  self.one_way_offset = Some(offset)
  self.one_way_direction = -1
}

///|
pub fn Collider3D::clear_one_way(self : Collider3D) -> Unit {
  self.one_way_offset = None
  self.one_way_direction = 0
}

///|
pub fn Collider3D::active_events(self : Collider3D) -> ActiveEvents {
  self.active_events
}

///|
pub fn Collider3D::set_active_events(
  self : Collider3D,
  active_events : ActiveEvents,
) -> Collider3D {
  self.active_events = active_events
  self
}

///|
pub fn Collider3D::active_collision_types(
  self : Collider3D,
) -> ActiveCollisionTypes {
  self.active_collision_types
}

///|
pub fn Collider3D::set_active_collision_types(
  self : Collider3D,
  types : ActiveCollisionTypes,
) -> Collider3D {
  self.active_collision_types = types
  self
}

///|
pub fn Collider3D::active_hooks(self : Collider3D) -> ActiveHooks {
  self.active_hooks
}

///|
pub fn Collider3D::set_active_hooks(
  self : Collider3D,
  hooks : ActiveHooks,
) -> Collider3D {
  self.active_hooks = hooks
  self
}

///|
pub fn Collider3D::contact_force_event_threshold(
  self : Collider3D,
) -> @core.Real {
  self.contact_force_event_threshold
}

///|
pub fn Collider3D::set_contact_force_event_threshold(
  self : Collider3D,
  threshold : @core.Real,
) -> Collider3D {
  self.contact_force_event_threshold = threshold
  self
}

///|
pub fn Collider3D::user_data(self : Collider3D) -> Int {
  self.user_data.to_int_truncate()
}

///|
pub fn Collider3D::user_data128(self : Collider3D) -> @core.UserData128 {
  self.user_data
}

///|
pub fn Collider3D::set_user_data(self : Collider3D, data : Int) -> Collider3D {
  self.user_data = @core.UserData128::from_int_truncate(data)
  self
}

///|
pub fn Collider3D::set_user_data128(
  self : Collider3D,
  data : @core.UserData128,
) -> Collider3D {
  self.user_data = data
  self
}

///|
pub fn Collider3D::collision_groups(
  self : Collider3D,
) -> @dynamics.InteractionGroups {
  self.collision_groups
}

///|
pub fn Collider3D::set_collision_groups(
  self : Collider3D,
  groups : @dynamics.InteractionGroups,
) -> Collider3D {
  self.collision_groups = groups
  self
}

///|
pub fn Collider3D::solver_groups(
  self : Collider3D,
) -> @dynamics.InteractionGroups {
  self.solver_groups
}

///|
pub fn Collider3D::set_solver_groups(
  self : Collider3D,
  groups : @dynamics.InteractionGroups,
) -> Collider3D {
  self.solver_groups = groups
  self
}

///|
pub fn Collider3D::contact_skin(self : Collider3D) -> @core.Real {
  self.contact_skin
}

///|
pub fn Collider3D::set_contact_skin(
  self : Collider3D,
  skin : @core.Real,
) -> Collider3D {
  self.contact_skin = skin
  self
}

///|
pub fn Collider3D::friction(self : Collider3D) -> @core.Real {
  self.friction
}

///|
pub fn Collider3D::set_friction(
  self : Collider3D,
  friction : @core.Real,
) -> Collider3D {
  self.friction = friction
  self
}

///|
pub fn Collider3D::friction_combine_rule(
  self : Collider3D,
) -> @dynamics.CoefficientCombineRule {
  self.friction_combine_rule
}

///|
pub fn Collider3D::set_friction_combine_rule(
  self : Collider3D,
  rule : @dynamics.CoefficientCombineRule,
) -> Collider3D {
  self.friction_combine_rule = rule
  self
}

///|
pub fn Collider3D::restitution(self : Collider3D) -> @core.Real {
  self.restitution
}

///|
pub fn Collider3D::set_restitution(
  self : Collider3D,
  restitution : @core.Real,
) -> Collider3D {
  self.restitution = restitution
  self
}

///|
pub fn Collider3D::restitution_combine_rule(
  self : Collider3D,
) -> @dynamics.CoefficientCombineRule {
  self.restitution_combine_rule
}

///|
pub fn Collider3D::set_restitution_combine_rule(
  self : Collider3D,
  rule : @dynamics.CoefficientCombineRule,
) -> Collider3D {
  self.restitution_combine_rule = rule
  self
}

///|
pub fn Collider3D::compute_aabb(self : Collider3D) -> @core.Aabb3 {
  aabb3_transform(self.position, self.shape.local_aabb())
}

///|
pub fn Collider3D::compute_collision_aabb(
  self : Collider3D,
  prediction_distance : @core.Real,
) -> @core.Aabb3 {
  self.compute_aabb().dilated(prediction_distance + self.contact_skin)
}

///|
pub fn Collider3D::enabled(self : Collider3D) -> Bool {
  self.enabled
}

///|
pub fn Collider3D::set_enabled(self : Collider3D, enabled : Bool) -> Collider3D {
  self.enabled = enabled
  self
}

///|
pub struct ColliderBuilder3D {
  mut position : @core.Isometry3
  mut local_position : @core.Isometry3
  parent : @dynamics.RigidBodyHandle?
  shape : Shape3D
  voxel_tri_map : Array[(Int, Int, Int)]?
  mut enabled : Bool
  mut sensor : Bool
  mut surface_velocity : @core.Vec3
  mut one_way_offset : @core.Real?
  mut one_way_direction : Int
  mut active_collision_types : ActiveCollisionTypes
  mut active_hooks : ActiveHooks
  mut active_events : ActiveEvents
  mut contact_force_event_threshold : @core.Real
  mut user_data : @core.UserData128
  mut collision_groups : @dynamics.InteractionGroups
  mut solver_groups : @dynamics.InteractionGroups
  mut contact_skin : @core.Real
  mut friction : @core.Real
  mut friction_combine_rule : @dynamics.CoefficientCombineRule
  mut restitution : @core.Real
  mut restitution_combine_rule : @dynamics.CoefficientCombineRule
}

///|
fn collider_builder3d_default(
  shape : Shape3D,
  voxel_tri_map : Array[(Int, Int, Int)]?,
) -> ColliderBuilder3D {
  {
    position: @core.Isometry3::identity(),
    local_position: @core.Isometry3::identity(),
    parent: None,
    shape,
    voxel_tri_map,
    enabled: true,
    sensor: false,
    surface_velocity: @core.Vec3::zero(),
    one_way_offset: None,
    one_way_direction: 0,
    active_collision_types: ActiveCollisionTypes::default(),
    active_hooks: ActiveHooks::empty(),
    active_events: ActiveEvents::empty(),
    contact_force_event_threshold: 0.0F,
    user_data: @core.UserData128::zero(),
    collision_groups: @dynamics.InteractionGroups::all(),
    solver_groups: @dynamics.InteractionGroups::all(),
    contact_skin: 0.0F,
    friction: ColliderBuilder3D::default_friction(),
    friction_combine_rule: Average,
    restitution: 0.0F,
    restitution_combine_rule: Average,
  }
}

///|
pub fn ColliderBuilder3D::default_density() -> @core.Real {
  1.0F
}

///|
pub fn ColliderBuilder3D::default_friction() -> @core.Real {
  0.5F
}

///|
pub fn ColliderBuilder3D::ball(radius : @core.Real) -> ColliderBuilder3D {
  collider_builder3d_default(Shape3D::ball(radius), None)
}

///|
pub fn ColliderBuilder3D::cuboid(
  hx : @core.Real,
  hy : @core.Real,
  hz : @core.Real,
) -> ColliderBuilder3D {
  collider_builder3d_default(Shape3D::cuboid(hx, hy, hz), None)
}

///|
pub fn ColliderBuilder3D::capsule_y(
  radius : @core.Real,
  half_height : @core.Real,
) -> ColliderBuilder3D {
  collider_builder3d_default(Shape3D::capsule_y(radius, half_height), None)
}

///|
pub fn ColliderBuilder3D::capsule_x(
  radius : @core.Real,
  half_height : @core.Real,
) -> ColliderBuilder3D {
  ColliderBuilder3D::capsule_y(radius, half_height).rotation(
    @core.rotation_from_scaled_axis(Vec3(0.0F, 0.0F, HALF_PI_3D)),
  )
}

///|
pub fn ColliderBuilder3D::capsule_z(
  radius : @core.Real,
  half_height : @core.Real,
) -> ColliderBuilder3D {
  // Rotate +Y into +Z (rotate around +X by -pi/2).
  ColliderBuilder3D::capsule_y(radius, half_height).rotation(
    @core.rotation_from_scaled_axis(Vec3(-HALF_PI_3D, 0.0F, 0.0F)),
  )
}

///|
pub fn ColliderBuilder3D::cylinder(
  radius : @core.Real,
  half_height : @core.Real,
) -> ColliderBuilder3D {
  collider_builder3d_default(Shape3D::cylinder(radius, half_height), None)
}

///|
pub fn ColliderBuilder3D::round_cylinder(
  radius : @core.Real,
  half_height : @core.Real,
  border_radius : @core.Real,
) -> ColliderBuilder3D {
  collider_builder3d_default(
    Shape3D::round_cylinder(radius, half_height, border_radius),
    None,
  )
}

///|
pub fn ColliderBuilder3D::cone(
  radius : @core.Real,
  half_height : @core.Real,
) -> ColliderBuilder3D {
  collider_builder3d_default(Shape3D::cone(radius, half_height), None)
}

///|
pub fn ColliderBuilder3D::round_cone(
  radius : @core.Real,
  half_height : @core.Real,
  border_radius : @core.Real,
) -> ColliderBuilder3D {
  // This port doesn't have a dedicated round-cone primitive yet.
  // Approximate by inflating the cone dimensions.
  ColliderBuilder3D::cone(radius + border_radius, half_height + border_radius)
}

///|
pub fn ColliderBuilder3D::halfspace(normal : @core.Vec3) -> ColliderBuilder3D {
  collider_builder3d_default(Shape3D::halfspace(normal), None)
}

///|
pub fn ColliderBuilder3D::triangle(
  a : @core.Vec3,
  b : @core.Vec3,
  c : @core.Vec3,
) -> ColliderBuilder3D {
  collider_builder3d_default(Shape3D::triangle(a, b, c), None)
}

///|
pub fn ColliderBuilder3D::trimesh(
  vertices : Array[@core.Vec3],
  indices : Array[(Int, Int, Int)],
) -> ColliderBuilder3D? {
  if vertices.length() == 0 || indices.length() == 0 {
    return None
  }
  let valid : Array[(Int, Int, Int)] = []
  for i in 0..= vertices.length() ||
      i1 >= vertices.length() ||
      i2 >= vertices.length() {
      continue
    }
    if i0 == i1 || i1 == i2 || i2 == i0 {
      continue
    }
    let a = vertices[i0]
    let b = vertices[i1]
    let c = vertices[i2]
    let area2 = b.sub(a).cross(c.sub(a)).length_squared()
    if area2 <= 1.0e-12F {
      continue
    }
    valid.push((i0, i1, i2))
  }
  if valid.length() == 0 {
    return None
  }
  Some(collider_builder3d_default(TriMesh(vertices, valid), None))
}

///|
pub fn ColliderBuilder3D::convex_mesh(
  vertices : Array[@core.Vec3],
  indices : Array[(Int, Int, Int)],
) -> ColliderBuilder3D? {
  // This port uses support-mapping convex hulls; indices are accepted for API parity.
  indices |> ignore
  ColliderBuilder3D::round_convex_hull(vertices, 0.0F)
}

///|
pub fn ColliderBuilder3D::round_convex_mesh(
  vertices : Array[@core.Vec3],
  indices : Array[(Int, Int, Int)],
  border_radius : @core.Real,
) -> ColliderBuilder3D? {
  indices |> ignore
  ColliderBuilder3D::round_convex_hull(vertices, border_radius)
}

///|
/// Build a compound collider from multiple (local pose, child shape) pairs.
///
/// This matches Rapier's `ColliderBuilder::compound` pattern: the compound has a single set of
/// material/flags, while each child contributes its local geometry.
///
/// Note: we accept child shapes as `ColliderBuilder3D` so callers can reuse existing constructor
/// helpers (`ball`, `cuboid`, ...). Only the child's `shape` and `position` are used; all other
/// fields (friction, events, etc.) are ignored.
pub fn ColliderBuilder3D::compound(
  parts : Array[(@core.Isometry3, ColliderBuilder3D)],
) -> ColliderBuilder3D? {
  if parts.length() == 0 {
    return None
  }
  let shapes : Array[(@core.Isometry3, Shape3D)] = []
  for i in 0.. ColliderBuilder3D? {
  if points.length() == 0 {
    return None
  }
  let pts : Array[@core.Vec3] = []
  for i in 0.. ColliderBuilder3D? {
  ColliderBuilder3D::round_convex_hull(points, 0.0F)
}

///|
pub fn ColliderBuilder3D::convex_decomposition(
  vertices : Array[@core.Vec3],
  indices : Array[(Int, Int, Int)],
) -> ColliderBuilder3D? {
  ColliderBuilder3D::round_convex_decomposition(vertices, indices, 0.0F)
}

///|
pub fn ColliderBuilder3D::round_convex_decomposition(
  vertices : Array[@core.Vec3],
  indices : Array[(Int, Int, Int)],
  border_radius : @core.Real,
) -> ColliderBuilder3D? {
  if vertices.length() == 0 {
    return None
  }
  if indices.length() == 0 {
    return ColliderBuilder3D::round_convex_hull(vertices, border_radius)
  }
  // Very small, deterministic decomposition: split triangles along the dominant axis of triangle
  // centroids and build one convex part per chunk.
  let tris = indices
  let mut min = vertices[0]
  let mut max = vertices[0]
  for i in 1.. max.x {
      max = Vec3(p.x, max.y, max.z)
    }
    if p.y > max.y {
      max = Vec3(max.x, p.y, max.z)
    }
    if p.z > max.z {
      max = Vec3(max.x, max.y, p.z)
    }
  }
  let ext = max.sub(min)
  let axis_i = if ext.x >= ext.y && ext.x >= ext.z {
    0
  } else if ext.y >= ext.z {
    1
  } else {
    2
  }
  fn axis_value(v : @core.Vec3, axis_i : Int) -> @core.Real {
    if axis_i == 0 {
      v.x
    } else if axis_i == 1 {
      v.y
    } else {
      v.z
    }
  }

  let tri_ids : Array[Int] = []
  for i in 0..= 0 {
      let idj = tri_ids[j]
      let (b0, b1, b2) = tris[idj]
      let cb = vertices[b0]
        .add(vertices[b1])
        .add(vertices[b2])
        .scale(1.0F / 3.0F)
      let vj = axis_value(cb, axis_i)
      if key_val >= vj {
        break
      }
      tri_ids[j + 1] = tri_ids[j]
      j = j - 1
    }
    tri_ids[j + 1] = key
  }
  let tri_count = tri_ids.length()
  let num_parts = if tri_count >= 24 {
    4
  } else if tri_count >= 12 {
    2
  } else {
    1
  }
  if num_parts <= 1 {
    return ColliderBuilder3D::round_convex_hull(vertices, border_radius)
  }
  let parts : Array[(@core.Isometry3, Shape3D)] = []
  let chunk = (tri_count + num_parts - 1) / num_parts
  for p in 0..= tri_count {
      continue
    }
    let end = if start + chunk < tri_count { start + chunk } else { tri_count }
    let used : @hashset.HashSet[Int] = HashSet([], capacity=chunk * 3)
    for ii in start.. 0 {
      parts.push((@core.Isometry3::identity(), ConvexHull(pts, border_radius)))
    }
  }
  if parts.length() <= 1 {
    ColliderBuilder3D::round_convex_hull(vertices, border_radius)
  } else {
    Some(collider_builder3d_default(Compound(parts), None))
  }
}

///|
/// Build a voxel mesh from a point cloud by snapping points to a regular grid.
///
/// This creates a sparse voxel set where each occupied cell is an axis-aligned box on a regular
/// grid. The returned collider stores a deterministic voxel id mapping so ray-casts can return the
/// hit voxel key.
pub fn ColliderBuilder3D::voxels_from_points(
  voxel_size : @core.Vec3,
  points : Array[@core.Vec3],
) -> ColliderBuilder3D? {
  if points.length() == 0 {
    return None
  }
  if voxel_size.x == 0.0F || voxel_size.y == 0.0F || voxel_size.z == 0.0F {
    return None
  }
  fn floor_div_to_int(value : @core.Real, denom : @core.Real) -> Int {
    if denom == 0.0F {
      0
    } else {
      @math.floor((value / denom).to_double()).to_int()
    }
  }

  let occupied : @hashset.HashSet[(Int, Int, Int)] = HashSet(
    [],
    capacity=points.length(),
  )
  for i in 0..voxel metadata.
  fn less(a : (Int, Int, Int), b : (Int, Int, Int)) -> Bool {
    if a.0 != b.0 {
      a.0 < b.0
    } else if a.1 != b.1 {
      a.1 < b.1
    } else {
      a.2 < b.2
    }
  }

  fn sort_keys(keys : Array[(Int, Int, Int)]) -> Unit {
    if keys.length() <= 1 {
      return
    }
    fn swap(arr : Array[(Int, Int, Int)], i : Int, j : Int) -> Unit {
      let tmp = arr[i]
      arr[i] = arr[j]
      arr[j] = tmp
    }

    let stack : Array[(Int, Int)] = [(0, keys.length() - 1)]
    while stack.length() > 0 {
      let range = stack[stack.length() - 1]
      stack.pop() |> ignore
      let lo = range.0
      let hi = range.1
      if lo >= hi {
        continue
      }
      let mid = lo + (hi - lo) / 2
      let pivot = keys[mid]
      let mut i = lo
      let mut j = hi
      while i <= j {
        while less(keys[i], pivot) {
          i = i + 1
        }
        while less(pivot, keys[j]) {
          j = j - 1
        }
        if i <= j {
          swap(keys, i, j)
          i = i + 1
          j = j - 1
        }
      }
      if lo < j {
        stack.push((lo, j))
      }
      if i < hi {
        stack.push((i, hi))
      }
    }
  }

  sort_keys(voxels)
  let mut min_key = voxels[0]
  let mut max_key = voxels[0]
  for i in 1.. max_key.0 {
      max_key = (ix, max_key.1, max_key.2)
    }
    if iy > max_key.1 {
      max_key = (max_key.0, iy, max_key.2)
    }
    if iz > max_key.2 {
      max_key = (max_key.0, max_key.1, iz)
    }
  }
  let key_to_id : @hashmap.HashMap[(Int, Int, Int), Int] = HashMap(
    [],
    capacity=voxels.length(),
  )
  for i in 0.. Unit {
    let mut j = 0
    while j < ys.length() && ys[j] < y {
      j = j + 1
    }
    if j < ys.length() && ys[j] == y {
      return
    }
    ys.push(y)
    let mut k = ys.length() - 1
    while k > j {
      ys[k] = ys[k - 1]
      k = k - 1
    }
    ys[j] = y
  }

  for i in 0.. 0 && nz > 0 && nx * nz <= 262144 {
    let dense : Array[Array[Int]?] = []
    for _ in 0..<(nx * nz) {
      dense.push(None)
    }
    for i in 0.. ColliderBuilder3D? {
  heightfield_impl(heights, nrows, ncols, scale, 0)
}

///|
fn heightfield_impl(
  heights : Array[@core.Real],
  nrows : Int,
  ncols : Int,
  scale : @core.Vec3,
  flags : Int,
) -> ColliderBuilder3D? {
  if nrows <= 1 || ncols <= 1 {
    return None
  }
  if heights.length() != nrows * ncols {
    return None
  }
  let dx = scale.x / Float::from_int(nrows - 1)
  let dz = scale.z / Float::from_int(ncols - 1)
  let origin_x = -scale.x / 2.0F
  let origin_z = -scale.z / 2.0F
  let vertices : Array[@core.Vec3] = []
  for i in 0.. Int {
    i * ncols + j
  }

  let indices : Array[(Int, Int, Int)] = []
  for i in 0..<(nrows - 1) {
    for j in 0..<(ncols - 1) {
      let a = vid(i, j, ncols)
      let b = vid(i + 1, j, ncols)
      let c = vid(i + 1, j + 1, ncols)
      let d = vid(i, j + 1, ncols)
      indices.push((a, c, b))
      indices.push((a, d, c))
    }
  }
  if ColliderBuilder3D::trimesh(vertices, indices) is Some(builder) {
    match builder.shape {
      TriMesh(vtx, idx) =>
        Some(
          collider_builder3d_default(
            Heightfield(vtx, idx, nrows, ncols, flags),
            None,
          ),
        )
      _ => None
    }
  } else {
    None
  }
}

///|
pub fn ColliderBuilder3D::heightfield_with_flags(
  heights : Array[@core.Real],
  nrows : Int,
  ncols : Int,
  scale : @core.Vec3,
  flags : Int,
) -> ColliderBuilder3D? {
  heightfield_impl(heights, nrows, ncols, scale, flags)
}

///|
pub fn ColliderBuilder3D::position(
  self : ColliderBuilder3D,
  pos : @core.Isometry3,
) -> ColliderBuilder3D {
  self.position = pos
  self
}

///|
pub fn ColliderBuilder3D::translation(
  self : ColliderBuilder3D,
  t : @core.Vec3,
) -> ColliderBuilder3D {
  self.position = Isometry3(t, self.position.rotation)
  self
}

///|
pub fn ColliderBuilder3D::rotation(
  self : ColliderBuilder3D,
  r : @core.Quat,
) -> ColliderBuilder3D {
  self.position = Isometry3(self.position.translation, r)
  self
}

///|
pub fn ColliderBuilder3D::delta(
  self : ColliderBuilder3D,
  delta : @core.Isometry3,
) -> ColliderBuilder3D {
  self.position = delta
  self
}

///|
pub fn ColliderBuilder3D::local_position(
  self : ColliderBuilder3D,
  pos : @core.Isometry3,
) -> ColliderBuilder3D {
  self.local_position = pos
  self
}

///|
pub fn ColliderBuilder3D::enabled(
  self : ColliderBuilder3D,
  enabled : Bool,
) -> ColliderBuilder3D {
  self.enabled = enabled
  self
}

///|
pub fn ColliderBuilder3D::sensor(
  self : ColliderBuilder3D,
  sensor : Bool,
) -> ColliderBuilder3D {
  self.sensor = sensor
  self
}

///|
pub fn ColliderBuilder3D::surface_velocity(
  self : ColliderBuilder3D,
  v : @core.Vec3,
) -> ColliderBuilder3D {
  self.surface_velocity = v
  self
}

///|
pub fn ColliderBuilder3D::one_way_above(
  self : ColliderBuilder3D,
  offset : @core.Real,
) -> ColliderBuilder3D {
  self.one_way_offset = Some(offset)
  self.one_way_direction = 1
  self
}

///|
pub fn ColliderBuilder3D::one_way_below(
  self : ColliderBuilder3D,
  offset : @core.Real,
) -> ColliderBuilder3D {
  self.one_way_offset = Some(offset)
  self.one_way_direction = -1
  self
}

///|
pub fn ColliderBuilder3D::active_events(
  self : ColliderBuilder3D,
  active_events : ActiveEvents,
) -> ColliderBuilder3D {
  self.active_events = active_events
  self
}

///|
pub fn ColliderBuilder3D::active_collision_types(
  self : ColliderBuilder3D,
  types : ActiveCollisionTypes,
) -> ColliderBuilder3D {
  self.active_collision_types = types
  self
}

///|
pub fn ColliderBuilder3D::active_hooks(
  self : ColliderBuilder3D,
  hooks : ActiveHooks,
) -> ColliderBuilder3D {
  self.active_hooks = hooks
  self
}

///|
pub fn ColliderBuilder3D::contact_force_event_threshold(
  self : ColliderBuilder3D,
  threshold : @core.Real,
) -> ColliderBuilder3D {
  self.contact_force_event_threshold = threshold
  self
}

///|
pub fn ColliderBuilder3D::user_data(
  self : ColliderBuilder3D,
  data : Int,
) -> ColliderBuilder3D {
  self.user_data = @core.UserData128::from_int_truncate(data)
  self
}

///|
pub fn ColliderBuilder3D::user_data128(
  self : ColliderBuilder3D,
  data : @core.UserData128,
) -> ColliderBuilder3D {
  self.user_data = data
  self
}

///|
pub fn ColliderBuilder3D::density(
  self : ColliderBuilder3D,
  density : @core.Real,
) -> ColliderBuilder3D {
  density |> ignore
  self
}

///|
pub fn ColliderBuilder3D::collision_groups(
  self : ColliderBuilder3D,
  groups : @dynamics.InteractionGroups,
) -> ColliderBuilder3D {
  self.collision_groups = groups
  self
}

///|
pub fn ColliderBuilder3D::solver_groups(
  self : ColliderBuilder3D,
  groups : @dynamics.InteractionGroups,
) -> ColliderBuilder3D {
  self.solver_groups = groups
  self
}

///|
pub fn ColliderBuilder3D::contact_skin(
  self : ColliderBuilder3D,
  skin : @core.Real,
) -> ColliderBuilder3D {
  self.contact_skin = skin
  self
}

///|
pub fn ColliderBuilder3D::friction(
  self : ColliderBuilder3D,
  friction : @core.Real,
) -> ColliderBuilder3D {
  self.friction = friction
  self
}

///|
pub fn ColliderBuilder3D::friction_combine_rule(
  self : ColliderBuilder3D,
  rule : @dynamics.CoefficientCombineRule,
) -> ColliderBuilder3D {
  self.friction_combine_rule = rule
  self
}

///|
pub fn ColliderBuilder3D::restitution(
  self : ColliderBuilder3D,
  restitution : @core.Real,
) -> ColliderBuilder3D {
  self.restitution = restitution
  self
}

///|
pub fn ColliderBuilder3D::restitution_combine_rule(
  self : ColliderBuilder3D,
  rule : @dynamics.CoefficientCombineRule,
) -> ColliderBuilder3D {
  self.restitution_combine_rule = rule
  self
}

///|
pub fn ColliderBuilder3D::build(self : ColliderBuilder3D) -> Collider3D {
  {
    position: self.position,
    local_position: self.local_position,
    parent: self.parent,
    shape: self.shape,
    voxel_tri_map: self.voxel_tri_map,
    enabled: self.enabled,
    sensor: self.sensor,
    surface_velocity: self.surface_velocity,
    one_way_offset: self.one_way_offset,
    one_way_direction: self.one_way_direction,
    active_collision_types: self.active_collision_types,
    active_hooks: self.active_hooks,
    active_events: self.active_events,
    contact_force_event_threshold: self.contact_force_event_threshold,
    user_data: self.user_data,
    collision_groups: self.collision_groups,
    solver_groups: self.solver_groups,
    contact_skin: self.contact_skin,
    friction: self.friction,
    friction_combine_rule: self.friction_combine_rule,
    restitution: self.restitution,
    restitution_combine_rule: self.restitution_combine_rule,
  }
}