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

///|
/// 3D rigid-body velocity: linear and angular.
pub struct RigidBodyVelocity3D {
  linvel : @core.Vec3
  angvel : @core.Vec3
}

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

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

///|
pub fn RigidBodyVelocity3D::zero() -> RigidBodyVelocity3D {
  { linvel: @core.Vec3::zero(), angvel: @core.Vec3::zero() }
}

///|
pub fn RigidBodyVelocity3D::apply_damping(
  self : RigidBodyVelocity3D,
  dt : @core.Real,
  linear_damping : @core.Real,
  angular_damping : @core.Real,
) -> RigidBodyVelocity3D {
  let lin_scale = 1.0F / (1.0F + dt * linear_damping)
  let ang_scale = 1.0F / (1.0F + dt * angular_damping)
  { linvel: self.linvel.scale(lin_scale), angvel: self.angvel.scale(ang_scale) }
}

///|
pub fn RigidBodyVelocity3D::integrate(
  self : RigidBodyVelocity3D,
  dt : @core.Real,
  curr_pos : @core.Isometry3,
  local_com : @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(self.angvel.scale(dt)),
  )
  let translation = self.linvel.scale(dt)
  let unshift = @core.Isometry3::from_translation(com.add(translation))
  unshift.mul(rot).mul(shift).mul(curr_pos)
}

///|
/// 3D rigid-body position state with interpolation support.
pub struct RigidBodyPosition3D {
  position : @core.Isometry3
  next_position : @core.Isometry3
}

///|
pub fn RigidBodyPosition3D::RigidBodyPosition3D(
  position : @core.Isometry3,
  next_position : @core.Isometry3,
) -> RigidBodyPosition3D {
  { position, next_position }
}

///|
pub fn RigidBodyPosition3D::interpolate_velocity(
  self : RigidBodyPosition3D,
  inv_dt : @core.Real,
  local_com : @core.Vec3,
) -> RigidBodyVelocity3D {
  let curr_com = self.position.transform_point(local_com)
  let next_com = self.next_position.transform_point(local_com)
  let linvel = next_com.sub(curr_com).scale(inv_dt)
  let curr_rot = self.position.rotation
  let next_rot = self.next_position.rotation
  let delta_rot = next_rot.mul(curr_rot.inverse())
  let angvel = delta_rot.to_scaled_axis().scale(inv_dt)
  { linvel, angvel }
}

///|
const DEFAULT_SLEEP_LINEAR_THRESHOLD_3D : @core.Real = 0.4F

///|
const DEFAULT_SLEEP_ANGULAR_THRESHOLD_3D : @core.Real = 0.5F

///|
const DEFAULT_TIME_UNTIL_SLEEP_3D : @core.Real = 2.0F

///|
pub struct RigidBodyActivation3D {
  normalized_linear_threshold : @core.Real
  angular_threshold : @core.Real
  time_until_sleep : @core.Real
  mut time_since_can_sleep : @core.Real
  mut sleeping : Bool
}

///|
pub fn RigidBodyActivation3D::active() -> RigidBodyActivation3D {
  {
    normalized_linear_threshold: DEFAULT_SLEEP_LINEAR_THRESHOLD_3D,
    angular_threshold: DEFAULT_SLEEP_ANGULAR_THRESHOLD_3D,
    time_until_sleep: DEFAULT_TIME_UNTIL_SLEEP_3D,
    time_since_can_sleep: 0.0F,
    sleeping: false,
  }
}

///|
pub fn RigidBodyActivation3D::is_sleeping(self : RigidBodyActivation3D) -> Bool {
  self.sleeping
}

///|
pub fn RigidBodyActivation3D::wake_up(
  self : RigidBodyActivation3D,
) -> RigidBodyActivation3D {
  self.sleeping = false
  self.time_since_can_sleep = 0.0F
  self
}

///|
pub fn RigidBodyActivation3D::update(
  self : RigidBodyActivation3D,
  dt : @core.Real,
  length_unit : @core.Real,
  body_type : RigidBodyType,
  vels : RigidBodyVelocity3D,
) -> RigidBodyActivation3D {
  if self.sleeping {
    return self
  }
  let sq_linvel = vels.linvel().length_squared()
  let sq_angvel = vels.angvel().length_squared()
  let can_sleep = match body_type {
    Dynamic => {
      let linear_threshold = self.normalized_linear_threshold * length_unit
      sq_linvel < linear_threshold * linear_threshold &&
      sq_angvel < self.angular_threshold * self.angular_threshold
    }
    KinematicPositionBased => sq_linvel == 0.0F && sq_angvel == 0.0F
    KinematicVelocityBased => sq_linvel == 0.0F && sq_angvel == 0.0F
    Fixed => true
  }
  if can_sleep {
    self.time_since_can_sleep = self.time_since_can_sleep + dt
    if self.time_since_can_sleep >= self.time_until_sleep {
      self.sleeping = true
    }
  } else {
    self.time_since_can_sleep = 0.0F
  }
  self
}

///|
pub struct RigidBody3D {
  body_type : RigidBodyType
  mut user_data : @core.UserData128
  mut position : @core.Isometry3
  // Target pose used by kinematic position-based bodies.
  mut next_position : @core.Isometry3
  mut velocity : RigidBodyVelocity3D
  // Locks/flags (compatible with Rapier's LockedAxes API).
  mut locked_translations : Bool
  mut trans_x_enabled : Bool
  mut trans_y_enabled : Bool
  mut trans_z_enabled : Bool
  mut locked_rotations : Bool
  mut rot_x_enabled : Bool
  mut rot_y_enabled : Bool
  mut rot_z_enabled : Bool
  // CCD/solver-related flags.
  mut ccd_active : Bool
  ccd_enabled : Bool
  soft_ccd_prediction : @core.Real
  gyroscopic_forces_enabled : Bool
  linear_damping : @core.Real
  angular_damping : @core.Real
  mut gravity_scale : @core.Real
  mass_properties : @core.MassProperties3
  mut additional_solver_iterations : Int
  mut activation : RigidBodyActivation3D
  can_sleep : Bool
  mut dominance_group : Int
}

///|
fn rigid_body3d_mask_translation(
  v : @core.Vec3,
  x : Bool,
  y : Bool,
  z : Bool,
) -> @core.Vec3 {
  Vec3(
    if x {
      v.x
    } else {
      0.0F
    },
    if y {
      v.y
    } else {
      0.0F
    },
    if z {
      v.z
    } else {
      0.0F
    },
  )
}

///|
fn rigid_body3d_mask_rotation(
  v : @core.Vec3,
  x : Bool,
  y : Bool,
  z : Bool,
) -> @core.Vec3 {
  Vec3(
    if x {
      v.x
    } else {
      0.0F
    },
    if y {
      v.y
    } else {
      0.0F
    },
    if z {
      v.z
    } else {
      0.0F
    },
  )
}

///|
fn rigid_body3d_locked_axes(
  trans_x_enabled : Bool,
  trans_y_enabled : Bool,
  trans_z_enabled : Bool,
  rot_x_enabled : Bool,
  rot_y_enabled : Bool,
  rot_z_enabled : Bool,
) -> LockedAxes {
  let mut axes = LockedAxes::empty()
  if !trans_x_enabled {
    axes = axes.insert(LockedAxes::translation_locked_x())
  }
  if !trans_y_enabled {
    axes = axes.insert(LockedAxes::translation_locked_y())
  }
  if !trans_z_enabled {
    axes = axes.insert(LockedAxes::translation_locked_z())
  }
  if !rot_x_enabled {
    axes = axes.insert(LockedAxes::rotation_locked_x())
  }
  if !rot_y_enabled {
    axes = axes.insert(LockedAxes::rotation_locked_y())
  }
  if !rot_z_enabled {
    axes = axes.insert(LockedAxes::rotation_locked_z())
  }
  axes
}

///|
pub fn RigidBody3D::translation(self : RigidBody3D) -> @core.Vec3 {
  self.position.translation
}

///|
pub fn RigidBody3D::rotation(self : RigidBody3D) -> @core.Quat {
  self.position.rotation
}

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

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

///|
pub fn RigidBody3D::linvel(self : RigidBody3D) -> @core.Vec3 {
  self.velocity.linvel
}

///|
pub fn RigidBody3D::angvel(self : RigidBody3D) -> @core.Vec3 {
  self.velocity.angvel
}

///|
pub fn RigidBody3D::set_linvel(self : RigidBody3D, v : @core.Vec3) -> Unit {
  let vv = rigid_body3d_mask_translation(
    v,
    self.trans_x_enabled,
    self.trans_y_enabled,
    self.trans_z_enabled,
  )
  self.velocity = { linvel: vv, angvel: self.velocity.angvel }
}

///|
pub fn RigidBody3D::set_angvel(self : RigidBody3D, w : @core.Vec3) -> Unit {
  let ww = rigid_body3d_mask_rotation(
    w,
    self.rot_x_enabled,
    self.rot_y_enabled,
    self.rot_z_enabled,
  )
  self.velocity = { linvel: self.velocity.linvel, angvel: ww }
}

///|
pub fn RigidBody3D::body_type(self : RigidBody3D) -> RigidBodyType {
  self.body_type
}

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

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

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

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

///|
pub fn RigidBody3D::locked_translations(self : RigidBody3D) -> Bool {
  self.locked_translations
}

///|
pub fn RigidBody3D::locked_rotations(self : RigidBody3D) -> Bool {
  self.locked_rotations
}

///|
pub fn RigidBody3D::locked_axes(self : RigidBody3D) -> LockedAxes {
  rigid_body3d_locked_axes(
    self.trans_x_enabled,
    self.trans_y_enabled,
    self.trans_z_enabled,
    self.rot_x_enabled,
    self.rot_y_enabled,
    self.rot_z_enabled,
  )
}

///|
pub fn RigidBody3D::set_locked_axes(
  self : RigidBody3D,
  locked_axes : LockedAxes,
  wake_up : Bool,
) -> Unit {
  self.trans_x_enabled = !locked_axes.contains(
    LockedAxes::translation_locked_x(),
  )
  self.trans_y_enabled = !locked_axes.contains(
    LockedAxes::translation_locked_y(),
  )
  self.trans_z_enabled = !locked_axes.contains(
    LockedAxes::translation_locked_z(),
  )
  self.rot_x_enabled = !locked_axes.contains(LockedAxes::rotation_locked_x())
  self.rot_y_enabled = !locked_axes.contains(LockedAxes::rotation_locked_y())
  self.rot_z_enabled = !locked_axes.contains(LockedAxes::rotation_locked_z())
  self.locked_translations = !(self.trans_x_enabled ||
    self.trans_y_enabled ||
    self.trans_z_enabled)
  self.locked_rotations = !(self.rot_x_enabled ||
    self.rot_y_enabled ||
    self.rot_z_enabled)
  self.set_linvel(self.velocity.linvel)
  self.set_angvel(self.velocity.angvel)
  if wake_up && self.body_type.is_dynamic_or_kinematic() {
    self.wake_up()
  }
}

///|
pub fn RigidBody3D::set_enabled_translations(
  self : RigidBody3D,
  allow_x : Bool,
  allow_y : Bool,
  allow_z : Bool,
  wake_up : Bool,
) -> Unit {
  let mut axes = self.locked_axes()
  axes = if allow_x {
    axes.remove(LockedAxes::translation_locked_x())
  } else {
    axes.insert(LockedAxes::translation_locked_x())
  }
  axes = if allow_y {
    axes.remove(LockedAxes::translation_locked_y())
  } else {
    axes.insert(LockedAxes::translation_locked_y())
  }
  axes = if allow_z {
    axes.remove(LockedAxes::translation_locked_z())
  } else {
    axes.insert(LockedAxes::translation_locked_z())
  }
  self.set_locked_axes(axes, wake_up)
}

///|
pub fn RigidBody3D::set_enabled_rotations(
  self : RigidBody3D,
  allow_x : Bool,
  allow_y : Bool,
  allow_z : Bool,
  wake_up : Bool,
) -> Unit {
  let mut axes = self.locked_axes()
  axes = if allow_x {
    axes.remove(LockedAxes::rotation_locked_x())
  } else {
    axes.insert(LockedAxes::rotation_locked_x())
  }
  axes = if allow_y {
    axes.remove(LockedAxes::rotation_locked_y())
  } else {
    axes.insert(LockedAxes::rotation_locked_y())
  }
  axes = if allow_z {
    axes.remove(LockedAxes::rotation_locked_z())
  } else {
    axes.insert(LockedAxes::rotation_locked_z())
  }
  self.set_locked_axes(axes, wake_up)
}

///|
pub fn RigidBody3D::restrict_translations(
  self : RigidBody3D,
  allow_x : Bool,
  allow_y : Bool,
  allow_z : Bool,
  wake_up : Bool,
) -> Unit {
  self.set_enabled_translations(allow_x, allow_y, allow_z, wake_up)
}

///|
pub fn RigidBody3D::restrict_rotations(
  self : RigidBody3D,
  allow_x : Bool,
  allow_y : Bool,
  allow_z : Bool,
  wake_up : Bool,
) -> Unit {
  self.set_enabled_rotations(allow_x, allow_y, allow_z, wake_up)
}

///|
pub fn RigidBody3D::dominance_group(self : RigidBody3D) -> Int {
  self.dominance_group
}

///|
pub fn RigidBody3D::effective_dominance_group(self : RigidBody3D) -> Int {
  RigidBodyDominance(self.dominance_group).effective_group(self.body_type)
}

///|
pub fn RigidBody3D::set_dominance_group(
  self : RigidBody3D,
  group : Int,
) -> Unit {
  self.dominance_group = group
}

///|
pub fn RigidBody3D::additional_solver_iterations(self : RigidBody3D) -> Int {
  self.additional_solver_iterations
}

///|
pub fn RigidBody3D::set_additional_solver_iterations(
  self : RigidBody3D,
  additional_iterations : Int,
) -> Unit {
  self.additional_solver_iterations = additional_iterations
}

///|
pub fn RigidBody3D::is_ccd_active(self : RigidBody3D) -> Bool {
  self.ccd_active
}

///|
pub fn RigidBody3D::is_ccd_enabled(self : RigidBody3D) -> Bool {
  self.ccd_enabled
}

///|
pub fn RigidBody3D::set_ccd_active(self : RigidBody3D, active : Bool) -> Unit {
  self.ccd_active = active
}

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

///|
pub fn RigidBody3D::gyroscopic_forces_enabled(self : RigidBody3D) -> Bool {
  self.gyroscopic_forces_enabled
}

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

///|
pub fn RigidBody3D::set_gravity_scale(
  self : RigidBody3D,
  scale : @core.Real,
  wake_up : Bool,
) -> Unit {
  self.gravity_scale = scale
  if wake_up {
    self.wake_up()
  }
}

///|
pub fn RigidBody3D::mass_properties(
  self : RigidBody3D,
) -> @core.MassProperties3 {
  self.mass_properties
}

///|
pub fn RigidBody3D::world_com(self : RigidBody3D) -> @core.Vec3 {
  self.position.transform_point(self.mass_properties.center_of_mass)
}

///|
pub fn RigidBody3D::is_active(self : RigidBody3D) -> Bool {
  !self.activation.is_sleeping()
}

///|
pub fn RigidBody3D::is_sleeping(self : RigidBody3D) -> Bool {
  self.activation.is_sleeping()
}

///|
pub fn RigidBody3D::wake_up(self : RigidBody3D) -> Unit {
  self.activation = self.activation.wake_up()
}

///|
pub fn RigidBody3D::apply_impulse_at_point(
  self : RigidBody3D,
  impulse : @core.Vec3,
  point : @core.Vec3,
  wake_up : Bool,
) -> Unit {
  if !self.body_type.is_dynamic() {
    return
  }
  if wake_up {
    self.wake_up()
  }
  if self.activation.is_sleeping() {
    return
  }
  let inv_m = self.mass_properties.inv_mass
  if inv_m > 0.0F && !self.locked_translations {
    let delta_v = rigid_body3d_mask_translation(
      impulse.scale(inv_m),
      self.trans_x_enabled,
      self.trans_y_enabled,
      self.trans_z_enabled,
    )
    self.set_linvel(self.linvel().add(delta_v))
  }
  if self.locked_rotations {
    return
  }
  let r = point.sub(self.world_com())
  let torque_impulse = r.cross(impulse)
  let rot = self.rotation().to_mat3()
  let inv_i_local = self.mass_properties.inv_inertia.into_mat3()
  let inv_i_world = rot.mul(inv_i_local).mul(rot.transpose())
  let dw = inv_i_world.mul_vec3(torque_impulse)
  self.set_angvel(self.angvel().add(dw))
}

///|
pub fn RigidBody3D::update_sleep(
  self : RigidBody3D,
  dt : @core.Real,
  length_unit : @core.Real,
) -> Unit {
  if !self.can_sleep {
    return
  }
  self.activation = self.activation.update(
    dt,
    length_unit,
    self.body_type,
    self.velocity,
  )
}

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

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

///|
pub fn RigidBody3D::set_next_kinematic_translation(
  self : RigidBody3D,
  t : @core.Vec3,
) -> Unit {
  self.next_position = Isometry3(t, self.next_position.rotation)
}

///|
pub fn RigidBody3D::set_next_kinematic_rotation(
  self : RigidBody3D,
  r : @core.Quat,
) -> Unit {
  self.next_position = Isometry3(self.next_position.translation, r)
}

///|
pub fn RigidBody3D::update_kinematic_velocities(
  self : RigidBody3D,
  dt : @core.Real,
) -> Unit {
  if self.body_type is KinematicPositionBased {
    if dt <= 0.0F {
      self.set_linvel(@core.Vec3::zero())
      self.set_angvel(@core.Vec3::zero())
      return
    }
    let inv_dt = 1.0F / dt
    let linvel = self.next_position.translation
      .sub(self.position.translation)
      .scale(inv_dt)
    let delta_rot = self.next_position.rotation.mul(
      self.position.rotation.inverse(),
    )
    let angvel = delta_rot.to_scaled_axis().scale(inv_dt)
    self.set_linvel(linvel)
    self.set_angvel(angvel)
  }
}

///|
pub fn RigidBody3D::apply_gravity(
  self : RigidBody3D,
  gravity : @core.Vec3,
  dt : @core.Real,
) -> Unit {
  if !self.body_type.is_dynamic() {
    return
  }
  if self.activation.is_sleeping() {
    return
  }
  if self.locked_translations {
    self.set_linvel(@core.Vec3::zero())
    return
  }
  self.set_linvel(
    self.velocity.linvel.add(gravity.scale(self.gravity_scale * dt)),
  )
}

///|
pub fn RigidBody3D::apply_damping(self : RigidBody3D, dt : @core.Real) -> Unit {
  if !self.body_type.is_dynamic() {
    return
  }
  if self.activation.is_sleeping() {
    return
  }
  self.velocity = self.velocity.apply_damping(
    dt,
    self.linear_damping,
    self.angular_damping,
  )
  self.set_linvel(self.velocity.linvel())
  self.set_angvel(self.velocity.angvel())
}

///|
pub fn RigidBody3D::apply_gyroscopic_forces(
  self : RigidBody3D,
  dt : @core.Real,
) -> Unit {
  if !self.body_type.is_dynamic() {
    return
  }
  if self.activation.is_sleeping() {
    return
  }
  if !self.gyroscopic_forces_enabled {
    return
  }
  if self.locked_rotations {
    self.set_angvel(@core.Vec3::zero())
    return
  }
  let w_world = self.angvel()
  if w_world.length_squared() <= 1.0e-12F {
    return
  }
  let rot = self.rotation().to_mat3()
  let w_local = rot.transpose().mul_vec3(w_world)
  let inv_i_local = self.mass_properties.inv_inertia.into_mat3()
  let i_local = inv_i_local.inverse()
  // Singular inertia: no meaningful gyroscopic effects.
  if i_local.determinant() == 0.0F {
    return
  }
  let l_local = i_local.mul_vec3(w_local)
  // Euler equation with no external torque: dw/dt = -I^{-1}(w x (I w)).
  let gyro = w_local.cross(l_local)
  let dw_local = inv_i_local.mul_vec3(gyro).scale(-dt)
  let w_local2 = w_local.add(dw_local)
  let w_world2 = rot.mul_vec3(w_local2)
  self.set_angvel(w_world2)
}

///|
pub fn RigidBody3D::advance_position(
  self : RigidBody3D,
  dt : @core.Real,
) -> Unit {
  match self.body_type {
    Dynamic => {
      if self.activation.is_sleeping() {
        return
      }
      let linvel = rigid_body3d_mask_translation(
        self.velocity.linvel(),
        self.trans_x_enabled,
        self.trans_y_enabled,
        self.trans_z_enabled,
      )
      let angvel = rigid_body3d_mask_rotation(
        self.velocity.angvel(),
        self.rot_x_enabled,
        self.rot_y_enabled,
        self.rot_z_enabled,
      )
      let vels : RigidBodyVelocity3D = { linvel, angvel }
      self.position = vels.integrate(
        dt,
        self.position,
        self.mass_properties.center_of_mass,
      )
    }
    KinematicVelocityBased => {
      let linvel = rigid_body3d_mask_translation(
        self.velocity.linvel(),
        self.trans_x_enabled,
        self.trans_y_enabled,
        self.trans_z_enabled,
      )
      let angvel = rigid_body3d_mask_rotation(
        self.velocity.angvel(),
        self.rot_x_enabled,
        self.rot_y_enabled,
        self.rot_z_enabled,
      )
      let vels : RigidBodyVelocity3D = { linvel, angvel }
      self.position = vels.integrate(
        dt,
        self.position,
        self.mass_properties.center_of_mass,
      )
    }
    KinematicPositionBased => {
      let mut next = self.next_position
      let curr_t = self.position.translation
      let next_t = next.translation
      next = Isometry3(
        Vec3(
          if self.trans_x_enabled {
            next_t.x
          } else {
            curr_t.x
          },
          if self.trans_y_enabled {
            next_t.y
          } else {
            curr_t.y
          },
          if self.trans_z_enabled {
            next_t.z
          } else {
            curr_t.z
          },
        ),
        next.rotation,
      )
      if self.locked_rotations {
        next = Isometry3(next.translation, self.position.rotation)
      }
      self.position = next
    }
    Fixed => ()
  }
}

///|
pub fn RigidBody3D::integrate(
  self : RigidBody3D,
  gravity : @core.Vec3,
  dt : @core.Real,
) -> Unit {
  self.apply_gravity(gravity, dt)
  self.apply_damping(dt)
  self.apply_gyroscopic_forces(dt)
  self.advance_position(dt)
}

///|
pub struct RigidBodyBuilder3D {
  mut body_type : RigidBodyType
  mut translation : @core.Vec3
  mut rotation : @core.Quat
  mut linvel : @core.Vec3
  mut angvel : @core.Vec3
  mut locked_translations : Bool
  mut trans_x_enabled : Bool
  mut trans_y_enabled : Bool
  mut trans_z_enabled : Bool
  mut locked_rotations : Bool
  mut rot_x_enabled : Bool
  mut rot_y_enabled : Bool
  mut rot_z_enabled : Bool
  mut ccd_enabled : Bool
  mut soft_ccd_prediction : @core.Real
  mut gyroscopic_forces_enabled : Bool
  mut linear_damping : @core.Real
  mut angular_damping : @core.Real
  mut gravity_scale : @core.Real
  mut mass_properties : @core.MassProperties3
  mut additional_solver_iterations : Int
  mut can_sleep : Bool
  mut sleeping : Bool
  mut user_data : @core.UserData128
  mut dominance_group : Int
}

///|
pub fn RigidBodyBuilder3D::dynamic() -> RigidBodyBuilder3D {
  {
    body_type: Dynamic,
    translation: @core.Vec3::zero(),
    rotation: @core.Quat::identity(),
    linvel: @core.Vec3::zero(),
    angvel: @core.Vec3::zero(),
    locked_translations: false,
    trans_x_enabled: true,
    trans_y_enabled: true,
    trans_z_enabled: true,
    locked_rotations: false,
    rot_x_enabled: true,
    rot_y_enabled: true,
    rot_z_enabled: true,
    ccd_enabled: false,
    soft_ccd_prediction: 0.0F,
    gyroscopic_forces_enabled: false,
    linear_damping: 0.0F,
    angular_damping: 0.0F,
    gravity_scale: 1.0F,
    mass_properties: MassProperties3(
      1.0F,
      @core.SdpMat3::from_diagonal(Vec3(1.0F, 1.0F, 1.0F)),
      @core.Vec3::zero(),
    ),
    additional_solver_iterations: 0,
    can_sleep: true,
    sleeping: false,
    user_data: @core.UserData128::zero(),
    dominance_group: 0,
  }
}

///|
pub fn RigidBodyBuilder3D::new_static() -> RigidBodyBuilder3D {
  RigidBodyBuilder3D::fixed()
}

///|
pub fn RigidBodyBuilder3D::new_kinematic_position_based() -> RigidBodyBuilder3D {
  RigidBodyBuilder3D::kinematic_position_based()
}

///|
pub fn RigidBodyBuilder3D::new_kinematic_velocity_based() -> RigidBodyBuilder3D {
  RigidBodyBuilder3D::kinematic_velocity_based()
}

///|
pub fn RigidBodyBuilder3D::fixed() -> RigidBodyBuilder3D {
  let b = RigidBodyBuilder3D::dynamic()
  b.body_type = Fixed
  b.mass_properties = @core.MassProperties3::default()
  b.can_sleep = false
  b
}

///|
pub fn RigidBodyBuilder3D::kinematic_position_based() -> RigidBodyBuilder3D {
  let b = RigidBodyBuilder3D::dynamic()
  b.body_type = KinematicPositionBased
  b.mass_properties = @core.MassProperties3::default()
  b.can_sleep = true
  b
}

///|
pub fn RigidBodyBuilder3D::kinematic_velocity_based() -> RigidBodyBuilder3D {
  let b = RigidBodyBuilder3D::dynamic()
  b.body_type = KinematicVelocityBased
  b.mass_properties = @core.MassProperties3::default()
  b.can_sleep = true
  b
}

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

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

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

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

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

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

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

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

///|
pub fn RigidBodyBuilder3D::rotation_scaled_axis(
  self : RigidBodyBuilder3D,
  scaled_axis : @core.Vec3,
) -> RigidBodyBuilder3D {
  self.rotation = @core.rotation_from_scaled_axis(scaled_axis)
  self
}

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

///|
pub fn RigidBodyBuilder3D::angvel(
  self : RigidBodyBuilder3D,
  w : @core.Vec3,
) -> RigidBodyBuilder3D {
  self.angvel = w
  self
}

///|
pub fn RigidBodyBuilder3D::linear_damping(
  self : RigidBodyBuilder3D,
  damping : @core.Real,
) -> RigidBodyBuilder3D {
  self.linear_damping = damping
  self
}

///|
pub fn RigidBodyBuilder3D::angular_damping(
  self : RigidBodyBuilder3D,
  damping : @core.Real,
) -> RigidBodyBuilder3D {
  self.angular_damping = damping
  self
}

///|
pub fn RigidBodyBuilder3D::gravity_scale(
  self : RigidBodyBuilder3D,
  scale : @core.Real,
) -> RigidBodyBuilder3D {
  self.gravity_scale = scale
  self
}

///|
pub fn RigidBodyBuilder3D::additional_mass(
  self : RigidBodyBuilder3D,
  mass : @core.Real,
) -> RigidBodyBuilder3D {
  self.mass_properties = self.mass_properties.set_mass(mass, true)
  self
}

///|
pub fn RigidBodyBuilder3D::additional_solver_iterations(
  self : RigidBodyBuilder3D,
  additional_iterations : Int,
) -> RigidBodyBuilder3D {
  self.additional_solver_iterations = additional_iterations
  self
}

///|
pub fn RigidBodyBuilder3D::additional_mass_properties(
  self : RigidBodyBuilder3D,
  props : @core.MassProperties3,
) -> RigidBodyBuilder3D {
  self.mass_properties = props
  self
}

///|
pub fn RigidBodyBuilder3D::dominance_group(
  self : RigidBodyBuilder3D,
  group : Int,
) -> RigidBodyBuilder3D {
  self.dominance_group = group
  self
}

///|
pub fn RigidBodyBuilder3D::lock_translations(
  self : RigidBodyBuilder3D,
) -> RigidBodyBuilder3D {
  self.trans_x_enabled = false
  self.trans_y_enabled = false
  self.trans_z_enabled = false
  self.locked_translations = true
  self
}

///|
pub fn RigidBodyBuilder3D::locked_axes(
  self : RigidBodyBuilder3D,
  locked_axes : LockedAxes,
) -> RigidBodyBuilder3D {
  self.trans_x_enabled = !locked_axes.contains(
    LockedAxes::translation_locked_x(),
  )
  self.trans_y_enabled = !locked_axes.contains(
    LockedAxes::translation_locked_y(),
  )
  self.trans_z_enabled = !locked_axes.contains(
    LockedAxes::translation_locked_z(),
  )
  self.rot_x_enabled = !locked_axes.contains(LockedAxes::rotation_locked_x())
  self.rot_y_enabled = !locked_axes.contains(LockedAxes::rotation_locked_y())
  self.rot_z_enabled = !locked_axes.contains(LockedAxes::rotation_locked_z())
  self.locked_translations = !(self.trans_x_enabled ||
    self.trans_y_enabled ||
    self.trans_z_enabled)
  self.locked_rotations = !(self.rot_x_enabled ||
    self.rot_y_enabled ||
    self.rot_z_enabled)
  self
}

///|
pub fn RigidBodyBuilder3D::enabled_translations(
  self : RigidBodyBuilder3D,
  x : Bool,
  y : Bool,
  z : Bool,
) -> RigidBodyBuilder3D {
  self.trans_x_enabled = x
  self.trans_y_enabled = y
  self.trans_z_enabled = z
  self.locked_translations = !(x || y || z)
  self
}

///|
pub fn RigidBodyBuilder3D::lock_rotations(
  self : RigidBodyBuilder3D,
) -> RigidBodyBuilder3D {
  self.locked_rotations = true
  self.rot_x_enabled = false
  self.rot_y_enabled = false
  self.rot_z_enabled = false
  self
}

///|
pub fn RigidBodyBuilder3D::enabled_rotations(
  self : RigidBodyBuilder3D,
  x : Bool,
  y : Bool,
  z : Bool,
) -> RigidBodyBuilder3D {
  self.locked_rotations = !(x || y || z)
  self.rot_x_enabled = x
  self.rot_y_enabled = y
  self.rot_z_enabled = z
  self
}

///|
pub fn RigidBodyBuilder3D::restrict_translations(
  self : RigidBodyBuilder3D,
  x : Bool,
  y : Bool,
  z : Bool,
) -> RigidBodyBuilder3D {
  self.enabled_translations(x, y, z)
}

///|
pub fn RigidBodyBuilder3D::restrict_rotations(
  self : RigidBodyBuilder3D,
  x : Bool,
  y : Bool,
  z : Bool,
) -> RigidBodyBuilder3D {
  self.enabled_rotations(x, y, z)
}

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

///|
pub fn RigidBodyBuilder3D::soft_ccd_prediction(
  self : RigidBodyBuilder3D,
  prediction : @core.Real,
) -> RigidBodyBuilder3D {
  self.soft_ccd_prediction = prediction
  self
}

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

///|
pub fn RigidBodyBuilder3D::mass_properties(
  self : RigidBodyBuilder3D,
  props : @core.MassProperties3,
) -> RigidBodyBuilder3D {
  self.mass_properties = props
  self
}

///|
pub fn RigidBodyBuilder3D::build(self : RigidBodyBuilder3D) -> RigidBody3D {
  let pos = @core.Isometry3(self.translation, self.rotation)
  let activation = RigidBodyActivation3D::active()
  if self.sleeping {
    activation.sleeping = true
    activation.time_since_can_sleep = activation.time_until_sleep
  }
  {
    body_type: self.body_type,
    user_data: self.user_data,
    position: pos,
    next_position: pos,
    velocity: {
      linvel: rigid_body3d_mask_translation(
        self.linvel,
        self.trans_x_enabled,
        self.trans_y_enabled,
        self.trans_z_enabled,
      ),
      angvel: rigid_body3d_mask_rotation(
        self.angvel,
        self.rot_x_enabled,
        self.rot_y_enabled,
        self.rot_z_enabled,
      ),
    },
    locked_translations: self.locked_translations,
    trans_x_enabled: self.trans_x_enabled,
    trans_y_enabled: self.trans_y_enabled,
    trans_z_enabled: self.trans_z_enabled,
    locked_rotations: self.locked_rotations,
    rot_x_enabled: self.rot_x_enabled,
    rot_y_enabled: self.rot_y_enabled,
    rot_z_enabled: self.rot_z_enabled,
    ccd_active: false,
    ccd_enabled: self.ccd_enabled,
    soft_ccd_prediction: self.soft_ccd_prediction,
    gyroscopic_forces_enabled: self.gyroscopic_forces_enabled,
    linear_damping: self.linear_damping,
    angular_damping: self.angular_damping,
    gravity_scale: self.gravity_scale,
    mass_properties: self.mass_properties,
    additional_solver_iterations: self.additional_solver_iterations,
    activation,
    can_sleep: self.can_sleep,
    dominance_group: self.dominance_group,
  }
}

///|
pub struct RigidBodySet3D {
  bodies : Array[RigidBody3D?]
  generations : Array[Int]
  free_list : Array[Int]
}

///|
pub fn RigidBodySet3D::RigidBodySet3D() -> RigidBodySet3D {
  { bodies: [], generations: [], free_list: [] }
}

///|
pub fn RigidBodySet3D::len(self : RigidBodySet3D) -> Int {
  let mut count = 0
  for i in 0.. RigidBodyHandle {
  if self.free_list.pop() is Some(index) {
    self.bodies[index] = Some(body)
    let generation = self.generations[index]
    RigidBodyHandle::from_raw_parts(index, generation)
  } else {
    let index = self.bodies.length()
    self.bodies.push(Some(body))
    self.generations.push(0)
    RigidBodyHandle::from_raw_parts(index, 0)
  }
}

///|
pub fn RigidBodySet3D::all_handles(
  self : RigidBodySet3D,
) -> Array[RigidBodyHandle] {
  let out : Array[RigidBodyHandle] = []
  for i in 0.. RigidBody3D? {
  let (id, gen) = handle.into_raw_parts()
  if id < 0 || id >= self.bodies.length() {
    return None
  }
  if self.generations[id] != gen {
    return None
  }
  let removed = self.bodies[id]
  if removed is Some(_) {
    self.bodies[id] = None
    self.free_list.push(id)
    self.generations[id] = self.generations[id] + 1
  }
  removed
}

///|
pub fn RigidBodySet3D::get(
  self : RigidBodySet3D,
  handle : RigidBodyHandle,
) -> RigidBody3D? {
  let (id, gen) = handle.into_raw_parts()
  if id < 0 || id >= self.bodies.length() {
    return None
  }
  if self.generations[id] != gen {
    return None
  }
  self.bodies[id]
}

///|
pub fn RigidBodySet3D::get_mut(
  self : RigidBodySet3D,
  handle : RigidBodyHandle,
) -> RigidBody3D? {
  self.get(handle)
}

///|
pub fn RigidBodySet3D::integrate_all(
  self : RigidBodySet3D,
  gravity : @core.Vec3,
  dt : @core.Real,
) -> Unit {
  for i in 0.. Unit {
  for i in 0.. Unit {
  for i in 0.. Unit {
  for i in 0.. Unit {
  for i in 0.. Bool {
  for i in 0.. @core.Real {
  let mut best2 = 0.0F
  for i in 0.. best2 {
        best2 = v2
      }
    }
  }
  if best2 <= 0.0F {
    0.0F
  } else {
    Float::sqrt(best2)
  }
}

///|
pub fn RigidBodySet3D::advance_positions_all(
  self : RigidBodySet3D,
  dt : @core.Real,
) -> Unit {
  for i in 0.. Unit {
  for i in 0..