// 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-flavored impulse joint set.
struct ImpulseJointSet3 {
  rb_graph_ids : Array[Int]
  rb_graph_generations : Array[Int]
  graph_nodes : Array[RigidBodyHandle]
  graph_edges : Array[JointGraphEdge]
  adjacency : Array[Array[Int]]
  joint_generations : Array[Int]
  joint_edge_ids : Array[Int]
  free_handles : Array[Int]
  to_wake_up : Array[RigidBodyHandle]
  to_join : Array[(RigidBodyHandle, RigidBodyHandle)]
}

///|
pub fn ImpulseJointSet3::ImpulseJointSet3() -> ImpulseJointSet3 {
  {
    rb_graph_ids: [],
    rb_graph_generations: [],
    graph_nodes: [],
    graph_edges: [],
    adjacency: [],
    joint_generations: [],
    joint_edge_ids: [],
    free_handles: [],
    to_wake_up: [],
    to_join: [],
  }
}

///|
pub fn ImpulseJointSet3::as_2d(self : ImpulseJointSet3) -> ImpulseJointSet {
  {
    rb_graph_ids: self.rb_graph_ids,
    rb_graph_generations: self.rb_graph_generations,
    graph_nodes: self.graph_nodes,
    graph_edges: self.graph_edges,
    adjacency: self.adjacency,
    joint_generations: self.joint_generations,
    joint_edge_ids: self.joint_edge_ids,
    free_handles: self.free_handles,
    to_wake_up: self.to_wake_up,
    to_join: self.to_join,
  }
}

///|
pub fn ImpulseJointSet3::take_wake_up(
  self : ImpulseJointSet3,
) -> Array[RigidBodyHandle] {
  let result : Array[RigidBodyHandle] = []
  for i in 0.. Array[(RigidBodyHandle, RigidBodyHandle)] {
  let result : Array[(RigidBodyHandle, RigidBodyHandle)] = []
  for i in 0.. Unit {
  if id < 0 {
    return
  }
  let mut i = self.rb_graph_ids.length()
  while i <= id {
    self.rb_graph_ids.push(-1)
    self.rb_graph_generations.push(0)
    i = i + 1
  }
}

///|
fn ImpulseJointSet3::graph_node_for(
  self : ImpulseJointSet3,
  handle : RigidBodyHandle,
) -> Int? {
  if handle.id < 0 || handle.id >= self.rb_graph_ids.length() {
    return None
  }
  let node_id = self.rb_graph_ids[handle.id]
  if node_id < 0 || node_id >= self.graph_nodes.length() {
    return None
  }
  if self.rb_graph_generations[handle.id] != handle.generation {
    return None
  }
  if !handle_equals(self.graph_nodes[node_id], handle) {
    return None
  }
  Some(node_id)
}

///|
fn ImpulseJointSet3::ensure_graph_node(
  self : ImpulseJointSet3,
  handle : RigidBodyHandle,
) -> Int {
  self.ensure_body_mapping_capacity(handle.id)
  if self.graph_node_for(handle) is Some(node_id) {
    node_id
  } else {
    let node_id = self.graph_nodes.length()
    self.graph_nodes.push(handle)
    self.adjacency.push([])
    self.rb_graph_ids[handle.id] = node_id
    self.rb_graph_generations[handle.id] = handle.generation
    node_id
  }
}

///|
fn ImpulseJointSet3::allocate_joint_handle(
  self : ImpulseJointSet3,
) -> ImpulseJointHandle {
  if self.free_handles.pop() is Some(index) {
    let generation = self.joint_generations[index]
    ImpulseJointHandle(index, generation)
  } else {
    let index = self.joint_generations.length()
    self.joint_generations.push(0)
    self.joint_edge_ids.push(-1)
    ImpulseJointHandle(index, 0)
  }
}

///|
fn ImpulseJointSet3::is_handle_valid(
  self : ImpulseJointSet3,
  handle : ImpulseJointHandle,
) -> Bool {
  if handle.id < 0 || handle.id >= self.joint_generations.length() {
    return false
  }
  if self.joint_generations[handle.id] != handle.generation {
    return false
  }
  let edge_id = self.joint_edge_ids[handle.id]
  edge_id >= 0 && edge_id < self.graph_edges.length()
}

///|
fn ImpulseJointSet3::remove_edge(
  self : ImpulseJointSet3,
  edge_id : Int,
) -> ImpulseJoint? {
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return None
  }
  let removed_edge = self.graph_edges[edge_id]
  remove_edge_index(self.adjacency[removed_edge.node1], edge_id)
  if removed_edge.node2 != removed_edge.node1 {
    remove_edge_index(self.adjacency[removed_edge.node2], edge_id)
  }
  let last_index = self.graph_edges.length() - 1
  if edge_id != last_index {
    let moved_edge = self.graph_edges[last_index]
    self.graph_edges[edge_id] = moved_edge
    replace_edge_index(self.adjacency[moved_edge.node1], last_index, edge_id)
    if moved_edge.node2 != moved_edge.node1 {
      replace_edge_index(self.adjacency[moved_edge.node2], last_index, edge_id)
    }
    let handle_id = moved_edge.joint.handle.id
    if handle_id >= 0 && handle_id < self.joint_edge_ids.length() {
      self.joint_edge_ids[handle_id] = edge_id
    }
  }
  self.graph_edges.pop() |> ignore
  Some(removed_edge.joint)
}

///|
fn ImpulseJointSet3::remove_node(
  self : ImpulseJointSet3,
  node_id : Int,
) -> Unit {
  if node_id < 0 || node_id >= self.graph_nodes.length() {
    return
  }
  let removed_handle = self.graph_nodes[node_id]
  if removed_handle.id >= 0 && removed_handle.id < self.rb_graph_ids.length() {
    self.rb_graph_ids[removed_handle.id] = -1
  }
  let last_index = self.graph_nodes.length() - 1
  if node_id != last_index {
    let moved_handle = self.graph_nodes[last_index]
    self.graph_nodes[node_id] = moved_handle
    self.adjacency[node_id] = self.adjacency[last_index]
    if moved_handle.id >= 0 && moved_handle.id < self.rb_graph_ids.length() {
      self.rb_graph_ids[moved_handle.id] = node_id
      self.rb_graph_generations[moved_handle.id] = moved_handle.generation
    }
    let edges = self.adjacency[node_id]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        let edge = self.graph_edges[edge_id]
        if edge.node1 == last_index {
          edge.node1 = node_id
        }
        if edge.node2 == last_index {
          edge.node2 = node_id
        }
        self.graph_edges[edge_id] = edge
      }
    }
  }
  self.graph_nodes.pop() |> ignore
  self.adjacency.pop() |> ignore
}

///|
pub fn ImpulseJointSet3::insert(
  self : ImpulseJointSet3,
  body1 : RigidBodyHandle,
  body2 : RigidBodyHandle,
  joint : GenericJoint,
  wake_up : Bool,
) -> ImpulseJointHandle {
  let handle = self.allocate_joint_handle()
  if body1.id < 0 || body2.id < 0 {
    return handle
  }
  let node1 = self.ensure_graph_node(body1)
  let node2 = self.ensure_graph_node(body2)
  let joint_data = GenericJoint::{
    local_frame1: joint.local_frame1,
    local_frame2: joint.local_frame2,
    axis3: joint.axis3,
    local_anchor1_3: joint.local_anchor1_3,
    local_anchor2_3: joint.local_anchor2_3,
    local_frame1_rotation: joint.local_frame1_rotation,
    local_frame2_rotation: joint.local_frame2_rotation,
    locked_axes: joint.locked_axes,
    limit_axes: joint.limit_axes,
    motor_axes: joint.motor_axes,
    coupled_axes: joint.coupled_axes,
    limits: joint.limits,
    motors: joint.motors,
    softness: joint.softness,
    contacts_enabled: joint.contacts_enabled,
    enabled: joint.enabled,
    user_data: joint.user_data,
  }
  let impulse_joint = ImpulseJoint::{
    body1,
    body2,
    data: joint_data,
    impulses: @core.Vec2::zero(),
    ang_impulse: 0.0F,
    handle,
  }
  let edge_id = self.graph_edges.length()
  self.graph_edges.push({ node1, node2, joint: impulse_joint })
  self.adjacency[node1].push(edge_id)
  if node2 != node1 {
    self.adjacency[node2].push(edge_id)
  }
  self.joint_edge_ids[handle.id] = edge_id
  if wake_up {
    push_unique_handle(self.to_wake_up, body1)
    push_unique_handle(self.to_wake_up, body2)
  }
  push_unique_pair(self.to_join, (body1, body2))
  handle
}

///|
pub fn ImpulseJointSet3::len(self : ImpulseJointSet3) -> Int {
  self.graph_edges.length()
}

///|
pub fn ImpulseJointSet3::is_empty(self : ImpulseJointSet3) -> Bool {
  self.graph_edges.length() == 0
}

///|
pub fn ImpulseJointSet3::joint_graph(
  self : ImpulseJointSet3,
) -> ImpulseJointGraph {
  let nodes : Array[RigidBodyHandle] = []
  for i in 0..= 0 && edge.node1 < self.graph_nodes.length() {
      self.graph_nodes[edge.node1]
    } else {
      RigidBodyHandle::invalid()
    }
    let body2 = if edge.node2 >= 0 && edge.node2 < self.graph_nodes.length() {
      self.graph_nodes[edge.node2]
    } else {
      RigidBodyHandle::invalid()
    }
    edges.push((body1, body2, edge.joint))
  }
  { nodes, edges }
}

///|
pub fn ImpulseJointSet3::contains(
  self : ImpulseJointSet3,
  handle : ImpulseJointHandle,
) -> Bool {
  self.is_handle_valid(handle)
}

///|
pub fn ImpulseJointSet3::get(
  self : ImpulseJointSet3,
  handle : ImpulseJointHandle,
) -> ImpulseJoint? {
  if !self.is_handle_valid(handle) {
    return None
  }
  let edge_id = self.joint_edge_ids[handle.id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return None
  }
  Some(self.graph_edges[edge_id].joint)
}

///|
pub fn ImpulseJointSet3::get_mut(
  self : ImpulseJointSet3,
  handle : ImpulseJointHandle,
  wake_up : Bool,
) -> ImpulseJoint? {
  if !self.is_handle_valid(handle) {
    return None
  }
  let edge_id = self.joint_edge_ids[handle.id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return None
  }
  let joint = self.graph_edges[edge_id].joint
  if wake_up {
    push_unique_handle(self.to_wake_up, joint.body1)
    push_unique_handle(self.to_wake_up, joint.body2)
  }
  Some(joint)
}

///|
pub fn ImpulseJointSet3::set_enabled(
  self : ImpulseJointSet3,
  handle : ImpulseJointHandle,
  enabled : Bool,
) -> Unit {
  if !self.is_handle_valid(handle) {
    return
  }
  let edge_id = self.joint_edge_ids[handle.id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return
  }
  let edge = self.graph_edges[edge_id]
  self.graph_edges[edge_id] = {
    node1: edge.node1,
    node2: edge.node2,
    joint: edge.joint.set_enabled(enabled),
  }
}

///|
pub fn ImpulseJointSet3::set_impulses(
  self : ImpulseJointSet3,
  handle : ImpulseJointHandle,
  impulses : @core.Vec2,
) -> Unit {
  if !self.is_handle_valid(handle) {
    return
  }
  let edge_id = self.joint_edge_ids[handle.id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return
  }
  let edge = self.graph_edges[edge_id]
  let joint = edge.joint
  joint.impulses = impulses
  self.graph_edges[edge_id] = { node1: edge.node1, node2: edge.node2, joint }
}

///|
pub fn ImpulseJointSet3::set_solver_impulses(
  self : ImpulseJointSet3,
  handle : ImpulseJointHandle,
  impulses : @core.Vec2,
  ang_impulse : @core.Real,
  motor_lin_x : @core.Real,
  motor_lin_y : @core.Real,
  motor_ang_z : @core.Real,
  limit_lin_x : @core.Real,
  limit_lin_y : @core.Real,
  limit_ang_z : @core.Real,
) -> Unit {
  if !self.is_handle_valid(handle) {
    return
  }
  let edge_id = self.joint_edge_ids[handle.id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return
  }
  let edge = self.graph_edges[edge_id]
  let joint = edge.joint
  let angular_axis = impulse_joint_solver_angular_axis(joint.data)
  joint.impulses = impulses
  joint.ang_impulse = ang_impulse
  let motors = joint.data.motors
  let updated_motors = JointMotors3::{
    lin_x: motors.lin_x.set_impulse(motor_lin_x),
    lin_y: motors.lin_y.set_impulse(motor_lin_y),
    lin_z: motors.lin_z,
    ang_x: motors.ang_x,
    ang_y: motors.ang_y,
    ang_z: motors.ang_z,
  }
  let updated_motor = joint_motor_for_axis(updated_motors, angular_axis).set_impulse(
    motor_ang_z,
  )
  joint.data.motors = joint_motors_set_axis(
    updated_motors, angular_axis, updated_motor,
  )
  let limits = joint.data.limits
  let updated_limits = JointLimits3::{
    lin_x: limits.lin_x.set_impulse(limit_lin_x),
    lin_y: limits.lin_y.set_impulse(limit_lin_y),
    lin_z: limits.lin_z,
    ang_x: limits.ang_x,
    ang_y: limits.ang_y,
    ang_z: limits.ang_z,
  }
  let updated_limit = joint_limits_for_axis(updated_limits, angular_axis).set_impulse(
    limit_ang_z,
  )
  joint.data.limits = joint_limits_set_axis(
    updated_limits, angular_axis, updated_limit,
  )
  self.graph_edges[edge_id] = { node1: edge.node1, node2: edge.node2, joint }
}

///|
pub fn ImpulseJointSet3::get_unknown_gen(
  self : ImpulseJointSet3,
  id : Int,
) -> (ImpulseJoint, ImpulseJointHandle)? {
  if id < 0 || id >= self.joint_edge_ids.length() {
    return None
  }
  let edge_id = self.joint_edge_ids[id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return None
  }
  let handle = ImpulseJointHandle(id, self.joint_generations[id])
  Some((self.graph_edges[edge_id].joint, handle))
}

///|
pub fn ImpulseJointSet3::get_unknown_gen_mut(
  self : ImpulseJointSet3,
  id : Int,
) -> (ImpulseJoint, ImpulseJointHandle)? {
  self.get_unknown_gen(id)
}

///|
pub fn ImpulseJointSet3::iter(
  self : ImpulseJointSet3,
) -> Array[(ImpulseJointHandle, ImpulseJoint)] {
  let result : Array[(ImpulseJointHandle, ImpulseJoint)] = []
  for i in 0.. Array[(ImpulseJointHandle, ImpulseJoint)] {
  self.iter()
}

///|
pub fn ImpulseJointSet3::remove(
  self : ImpulseJointSet3,
  handle : ImpulseJointHandle,
  wake_up : Bool,
) -> ImpulseJoint? {
  if !self.is_handle_valid(handle) {
    return None
  }
  let edge_id = self.joint_edge_ids[handle.id]
  let removed = self.remove_edge(edge_id)
  if removed is Some(joint) {
    self.joint_edge_ids[handle.id] = -1
    self.joint_generations[handle.id] = self.joint_generations[handle.id] + 1
    self.free_handles.push(handle.id)
    if wake_up {
      push_unique_handle(self.to_wake_up, joint.body1)
      push_unique_handle(self.to_wake_up, joint.body2)
    }
    Some(joint)
  } else {
    None
  }
}

///|
pub fn ImpulseJointSet3::remove_joints_attached_to_rigid_body(
  self : ImpulseJointSet3,
  handle : RigidBodyHandle,
) -> Array[ImpulseJointHandle] {
  if self.graph_node_for(handle) is Some(node_index) {
    let to_remove : Array[ImpulseJointHandle] = []
    let edges_snapshot = self.adjacency[node_index]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        to_remove.push(self.graph_edges[edge_id].joint.handle)
      }
    }
    let removed : Array[ImpulseJointHandle] = []
    for i in 0..= 0 &&
      node_index < self.adjacency.length() &&
      self.adjacency[node_index].length() == 0 {
      self.remove_node(node_index)
    }
    removed
  } else {
    []
  }
}

///|
pub fn ImpulseJointSet3::map_attached_joints_mut(
  self : ImpulseJointSet3,
  handle : RigidBodyHandle,
  f : (RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, ImpulseJoint) -> Unit,
) -> Unit {
  if self.graph_node_for(handle) is Some(node_index) {
    let edges = self.adjacency[node_index]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        let joint = self.graph_edges[edge_id].joint
        f(joint.body1, joint.body2, joint.handle, joint)
      }
    }
  }
}

///|
pub fn ImpulseJointSet3::attached_joints(
  self : ImpulseJointSet3,
  handle : RigidBodyHandle,
) -> Array[(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, ImpulseJoint)] {
  if self.graph_node_for(handle) is Some(node_index) {
    let result : Array[
      (RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, ImpulseJoint),
    ] = []
    let edges = self.adjacency[node_index]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        let joint = self.graph_edges[edge_id].joint
        result.push((joint.body1, joint.body2, joint.handle, joint))
      }
    }
    result
  } else {
    []
  }
}

///|
pub fn ImpulseJointSet3::attached_enabled_joints(
  self : ImpulseJointSet3,
  handle : RigidBodyHandle,
) -> Array[(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, ImpulseJoint)] {
  let attached = self.attached_joints(handle)
  let result : Array[
    (RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, ImpulseJoint),
  ] = []
  for i in 0.. Array[(ImpulseJointHandle, ImpulseJoint)] {
  if self.graph_node_for(body1) is Some(node1_index) &&
    self.graph_node_for(body2) is Some(node2_index) {
    let result : Array[(ImpulseJointHandle, ImpulseJoint)] = []
    let edges = self.adjacency[node1_index]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        let edge = self.graph_edges[edge_id]
        if edge.node1 == node2_index || edge.node2 == node2_index {
          result.push((edge.joint.handle, edge.joint))
        }
      }
    }
    result
  } else {
    []
  }
}

///|
pub fn ImpulseJointSet3::select_active_interactions(
  self : ImpulseJointSet3,
  islands : IslandManager,
  bodies : RigidBodySet,
  out : Array[Array[Int]],
) -> Unit {
  let awake_len = islands.awake_islands.length()
  for i in 0..= islands.islands.length() {
          continue
        }
        if islands.islands[island_id] is Some(island) {
          if island.id_in_awake_list is Some(awake_index) {
            island_awake_index = awake_index
          }
        }
      } else {
        let island_id = body1.ids.active_island_id
        if island_id < 0 || island_id >= islands.islands.length() {
          continue
        }
        if islands.islands[island_id] is Some(island) {
          if island.id_in_awake_list is Some(awake_index) {
            island_awake_index = awake_index
          }
        }
      }
      if island_awake_index < 0 {
        continue
      }
      if island_awake_index >= 0 && island_awake_index < out.length() {
        out[island_awake_index].push(i)
      }
    }
  }
}

///|
/// 3D-flavored multibody joint set.
struct MultibodyJointSet3 {
  multibodies : @data.Arena[Multibody]
  rb2mb : @data.Coarena[MultibodyLinkId]
  graph_nodes : Array[RigidBodyHandle]
  graph_edges : Array[MultibodyGraphEdge]
  adjacency : Array[Array[Int]]
  to_wake_up : Array[RigidBodyHandle]
  to_join : Array[(RigidBodyHandle, RigidBodyHandle)]
}

///|
pub fn MultibodyJointSet3::MultibodyJointSet3() -> MultibodyJointSet3 {
  {
    multibodies: Arena(),
    rb2mb: Coarena(),
    graph_nodes: [],
    graph_edges: [],
    adjacency: [],
    to_wake_up: [],
    to_join: [],
  }
}

///|
pub fn MultibodyJointSet3::as_2d(
  self : MultibodyJointSet3,
) -> MultibodyJointSet {
  {
    multibodies: self.multibodies,
    rb2mb: self.rb2mb,
    graph_nodes: self.graph_nodes,
    graph_edges: self.graph_edges,
    adjacency: self.adjacency,
    to_wake_up: self.to_wake_up,
    to_join: self.to_join,
  }
}

///|
fn MultibodyJointSet3::set_link(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
  link : MultibodyLinkId,
) -> Unit {
  self.rb2mb.set(rb_handle_index(handle), link, MultibodyLinkId::default())
}

///|
fn MultibodyJointSet3::graph_node_for(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
) -> Int? {
  if self.rb2mb.get(rb_handle_index(handle)) is Some(link) {
    let graph_id = link.graph_id
    if graph_id >= 0 &&
      graph_id < self.graph_nodes.length() &&
      RigidBodyHandle::equals(self.graph_nodes[graph_id], handle) {
      Some(graph_id)
    } else {
      None
    }
  } else {
    None
  }
}

///|
fn MultibodyJointSet3::update_graph_id(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
  graph_id : Int,
) -> Unit {
  let key = rb_handle_index(handle)
  if self.rb2mb.get(key) is Some(link) {
    link.graph_id = graph_id
    self.rb2mb.set(key, link, MultibodyLinkId::default())
  }
}

///|
fn MultibodyJointSet3::ensure_graph_node(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
) -> Int {
  if self.graph_node_for(handle) is Some(node_id) {
    node_id
  } else {
    let node_id = self.graph_nodes.length()
    self.graph_nodes.push(handle)
    self.adjacency.push([])
    node_id
  }
}

///|
fn MultibodyJointSet3::insert_graph_edge(
  self : MultibodyJointSet3,
  parent : RigidBodyHandle,
  child : RigidBodyHandle,
) -> Unit {
  let node1 = self.ensure_graph_node(parent)
  let node2 = self.ensure_graph_node(child)
  let edge_id = self.graph_edges.length()
  self.graph_edges.push({ node1, node2, parent, child })
  self.adjacency[node1].push(edge_id)
  self.adjacency[node2].push(edge_id)
}

///|
fn MultibodyJointSet3::remove_graph_edge(
  self : MultibodyJointSet3,
  edge_id : Int,
) -> MultibodyGraphEdge? {
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return None
  }
  let removed_edge = self.graph_edges[edge_id]
  remove_graph_edge_index(self.adjacency[removed_edge.node1], edge_id)
  if removed_edge.node2 != removed_edge.node1 {
    remove_graph_edge_index(self.adjacency[removed_edge.node2], edge_id)
  }
  let last_index = self.graph_edges.length() - 1
  if edge_id != last_index {
    let moved_edge = self.graph_edges[last_index]
    self.graph_edges[edge_id] = moved_edge
    replace_graph_edge_index(
      self.adjacency[moved_edge.node1],
      last_index,
      edge_id,
    )
    if moved_edge.node2 != moved_edge.node1 {
      replace_graph_edge_index(
        self.adjacency[moved_edge.node2],
        last_index,
        edge_id,
      )
    }
  }
  self.graph_edges.pop() |> ignore
  Some(removed_edge)
}

///|
fn MultibodyJointSet3::remove_graph_node(
  self : MultibodyJointSet3,
  node_id : Int,
) -> Unit {
  if node_id < 0 || node_id >= self.graph_nodes.length() {
    return
  }
  while self.adjacency[node_id].length() > 0 {
    let last_edge = self.adjacency[node_id].length() - 1
    self.remove_graph_edge(self.adjacency[node_id][last_edge]) |> ignore
  }
  let last_index = self.graph_nodes.length() - 1
  if node_id != last_index {
    let moved_handle = self.graph_nodes[last_index]
    self.graph_nodes[node_id] = moved_handle
    self.adjacency[node_id] = self.adjacency[last_index]
    self.update_graph_id(moved_handle, node_id)
    let edges = self.adjacency[node_id]
    for i in 0..= self.graph_edges.length() {
        continue
      }
      let edge = self.graph_edges[edge_id]
      self.graph_edges[edge_id] = {
        node1: if edge.node1 == last_index {
          node_id
        } else {
          edge.node1
        },
        node2: if edge.node2 == last_index {
          node_id
        } else {
          edge.node2
        },
        parent: edge.parent,
        child: edge.child,
      }
    }
  }
  self.graph_nodes.pop() |> ignore
  self.adjacency.pop() |> ignore
}

///|
fn MultibodyJointSet3::remove_graph_node_for_handle(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
) -> Unit {
  if self.graph_node_for(handle) is Some(node_id) {
    self.remove_graph_node(node_id)
  }
}

///|
fn MultibodyJointSet3::find_graph_edge_by_child(
  self : MultibodyJointSet3,
  child : RigidBodyHandle,
) -> Int? {
  if self.graph_node_for(child) is Some(node_id) {
    let edges = self.adjacency[node_id]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        let edge = self.graph_edges[edge_id]
        if RigidBodyHandle::equals(edge.child, child) {
          return Some(edge_id)
        }
      }
    }
  }
  None
}

///|
fn MultibodyJointSet3::register_body(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
) -> MultibodyLinkId {
  let mb_handle = self.multibodies.insert(Multibody::new_with_root(handle))
  let graph_id = self.ensure_graph_node(handle)
  let link = MultibodyLinkId(graph_id, MultibodyIndex(mb_handle), 0)
  self.set_link(handle, link)
  link
}

///|
fn MultibodyJointSet3::lookup_body(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
) -> MultibodyLinkId {
  if self.rb2mb.get(rb_handle_index(handle)) is Some(link) {
    link
  } else {
    self.register_body(handle)
  }
}

///|
fn MultibodyJointSet3::update_body_links_for_multibody(
  self : MultibodyJointSet3,
  mb_index : MultibodyIndex,
  multibody : Multibody,
) -> Unit {
  for i in 0.. Array[RigidBodyHandle] {
  let result : Array[RigidBodyHandle] = []
  for i in 0.. Array[(RigidBodyHandle, RigidBodyHandle)] {
  let result : Array[(RigidBodyHandle, RigidBodyHandle)] = []
  for i in 0.. Array[(RigidBodyHandle, RigidBodyHandle)] {
  let result : Array[(RigidBodyHandle, RigidBodyHandle)] = []
  let items = self.multibodies.iter()
  for i in 0..= mb.links.length() {
        continue
      }
      if !link.joint.data.contacts_enabled() {
        let parent = mb.links[parent_id].rigid_body
        let child = link.rigid_body
        result.push((parent, child))
      }
    }
  }
  result
}

///|
pub fn MultibodyJointSet3::interaction_pairs(
  self : MultibodyJointSet3,
) -> Array[(RigidBodyHandle, RigidBodyHandle)] {
  let result : Array[(RigidBodyHandle, RigidBodyHandle)] = []
  let items = self.multibodies.iter()
  for i in 0..= mb.links.length() {
        continue
      }
      let parent = mb.links[parent_id].rigid_body
      let child = link.rigid_body
      result.push((parent, child))
    }
  }
  result
}

///|
pub fn MultibodyJointSet3::sync_rigid_bodies_from_multibodies(
  self : MultibodyJointSet3,
  bodies : RigidBodySet3,
) -> Unit {
  let items = self.multibodies.iter()
  for i in 0.. ignore
  }
}

///|
pub fn MultibodyJointSet3::revolute_joint_descriptors(
  self : MultibodyJointSet3,
) -> Array[MultibodyRevoluteJointDesc] {
  let result : Array[MultibodyRevoluteJointDesc] = []
  let items = self.multibodies.iter()
  for i in 0..= mb.links.length() || parent_id == link_id {
        continue
      }
      if !link.joint.is_revolute() {
        continue
      }
      let parent_link = mb.links[parent_id]
      let graph_id = if self.rb2mb.get(rb_handle_index(link.rigid_body))
        is Some(ids) {
        ids.graph_id
      } else {
        -1
      }
      result.push({
        link: MultibodyLinkId(graph_id, mb_index, link_id),
        parent: parent_link.rigid_body,
        child: link.rigid_body,
        joint: link.joint.as_revolute_joint(),
        solver_lin_impulses: link.joint.solver_lin_impulses,
      })
    }
  }
  result
}

///|
pub fn MultibodyJointSet3::set_revolute_joint_solver_impulses(
  self : MultibodyJointSet3,
  link : MultibodyLinkId,
  lin_impulses : @core.Vec2,
  motor_impulse : @core.Real,
  limit_impulse : @core.Real,
) -> Unit {
  if self.multibodies.get(link.multibody.arena_index()) is Some(mb) {
    if link.id <= 0 || link.id >= mb.links.length() {
      return
    }
    let lnk = mb.links[link.id]
    lnk.joint.solver_lin_impulses = lin_impulses
    let motors = lnk.joint.data.motors
    lnk.joint.data.motors = {
      lin_x: motors.lin_x,
      lin_y: motors.lin_y,
      lin_z: motors.lin_z,
      ang_x: motors.ang_x,
      ang_y: motors.ang_y,
      ang_z: motors.ang_z.set_impulse(motor_impulse),
    }
    let limits = lnk.joint.data.limits
    lnk.joint.data.limits = {
      lin_x: limits.lin_x,
      lin_y: limits.lin_y,
      lin_z: limits.lin_z,
      ang_x: limits.ang_x,
      ang_y: limits.ang_y,
      ang_z: limits.ang_z.set_impulse(limit_impulse),
    }
    mb.links[link.id] = lnk
    self.multibodies.set(link.multibody.arena_index(), mb) |> ignore
  }
}

///|
pub fn MultibodyJointSet3::insert(
  self : MultibodyJointSet3,
  parent : RigidBodyHandle,
  child : RigidBodyHandle,
  joint : GenericJoint,
  wake_up : Bool,
) -> MultibodyJointHandle? {
  let multibody_joint = MultibodyJoint(joint, false)
  let parent_link = self.lookup_body(parent)
  let child_link = self.lookup_body(child)
  if parent_link.multibody.equals(child_link.multibody) || child_link.id != 0 {
    return None
  }
  if self.multibodies.get(parent_link.multibody.arena_index())
    is Some(parent_mb) {
    if self.multibodies.remove(child_link.multibody.arena_index())
      is Some(child_mb) {
      let offset = parent_mb.links.length()
      for i in 0.. ignore
      self.insert_graph_edge(parent, child)
      for i in 0.. MultibodyJointHandle? {
  let multibody_joint = MultibodyJoint(joint, true)
  let parent_link = self.lookup_body(parent)
  let child_link = self.lookup_body(child)
  if parent_link.multibody.equals(child_link.multibody) || child_link.id != 0 {
    return None
  }
  if self.multibodies.get(parent_link.multibody.arena_index())
    is Some(parent_mb) {
    if self.multibodies.remove(child_link.multibody.arena_index())
      is Some(child_mb) {
      let offset = parent_mb.links.length()
      for i in 0.. ignore
      self.insert_graph_edge(parent, child)
      for i in 0.. Int {
  mb3_logical_ndofs(self.inner)
}

///|
pub fn MultibodyJointSet3::get(
  self : MultibodyJointSet3,
  handle : MultibodyJointHandle,
) -> (Multibody3, LinkId)? {
  if self.rb2mb.get(handle.arena_index()) is Some(link) {
    if self.multibodies.get(link.multibody.arena_index()) is Some(multibody) {
      Some(({ inner: multibody }, LinkId(link.id)))
    } else {
      None
    }
  } else {
    None
  }
}

///|
pub fn MultibodyJointSet3::get_mut(
  self : MultibodyJointSet3,
  handle : MultibodyJointHandle,
) -> (Multibody3, LinkId)? {
  self.get(handle)
}

///|
pub fn MultibodyJointSet3::get_mut_internal(
  self : MultibodyJointSet3,
  handle : MultibodyJointHandle,
) -> (Multibody3, LinkId)? {
  self.get(handle)
}

///|
pub fn MultibodyJointSet3::remove(
  self : MultibodyJointSet3,
  handle : MultibodyJointHandle,
  wake_up : Bool,
) -> Unit {
  if self.rb2mb.get(handle.arena_index()) is Some(removed) {
    if removed.id <= 0 {
      return
    }
    if self.multibodies.remove(removed.multibody.arena_index())
      is Some(multibody) {
      let removed_link = multibody.links[removed.id]
      if self.find_graph_edge_by_child(removed_link.rigid_body) is Some(edge_id) {
        self.remove_graph_edge(edge_id) |> ignore
      }
      let subtree = collect_subtree_indices(multibody, removed.id)
      if subtree.length() == 0 || subtree.length() == multibody.links.length() {
        self.multibodies.insert(multibody) |> ignore
        return
      }
      let remaining : Array[Int] = []
      for i in 0..= 0 && parent_id < multibody.links.length() {
          let parent_link = multibody.links[parent_id]
          push_unique_handle(self.to_wake_up, parent_link.rigid_body)
        }
      }
      let remaining_mb = build_multibody_from_indices(multibody, remaining, 0)
      let subtree_mb = build_multibody_from_indices(
        multibody,
        subtree,
        removed.id,
      )
      if remaining_mb.links.length() <= 1 {
        let link = remaining_mb.links[0]
        self.rb2mb.remove(
          rb_handle_index(link.rigid_body),
          MultibodyLinkId::default(),
        )
        |> ignore
        self.remove_graph_node_for_handle(link.rigid_body)
      } else {
        let remaining_handle = self.multibodies.insert(remaining_mb)
        self.update_body_links_for_multibody(
          MultibodyIndex(remaining_handle),
          remaining_mb,
        )
      }
      if subtree_mb.links.length() <= 1 {
        let link = subtree_mb.links[0]
        self.rb2mb.remove(
          rb_handle_index(link.rigid_body),
          MultibodyLinkId::default(),
        )
        |> ignore
        self.remove_graph_node_for_handle(link.rigid_body)
      } else {
        let subtree_handle = self.multibodies.insert(subtree_mb)
        self.update_body_links_for_multibody(
          MultibodyIndex(subtree_handle),
          subtree_mb,
        )
      }
    }
  }
}

///|
pub fn MultibodyJointSet3::remove_multibody_articulations(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
  wake_up : Bool,
) -> Unit {
  if self.rb2mb.get(rb_handle_index(handle)) is Some(link) {
    if self.multibodies.remove(link.multibody.arena_index()) is Some(multibody) {
      for i in 0.. ignore
        if graph_id >= 0 {
          self.remove_graph_node(graph_id)
        }
      }
    }
  }
}

///|
pub fn MultibodyJointSet3::rigid_body_link(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
) -> MultibodyLinkId? {
  self.rb2mb.get(rb_handle_index(handle))
}

///|
pub fn MultibodyJointSet3::get_multibody(
  self : MultibodyJointSet3,
  index : MultibodyIndex,
) -> Multibody3? {
  if self.multibodies.get(index.arena_index()) is Some(multibody) {
    Some({ inner: multibody })
  } else {
    None
  }
}

///|
pub fn MultibodyJointSet3::get_multibody_mut(
  self : MultibodyJointSet3,
  index : MultibodyIndex,
) -> Multibody3? {
  self.get_multibody(index)
}

///|
pub fn MultibodyJointSet3::get_multibody_mut_internal(
  self : MultibodyJointSet3,
  index : MultibodyIndex,
) -> Multibody3? {
  self.get_multibody(index)
}

///|
pub fn MultibodyJointSet3::multibodies(
  self : MultibodyJointSet3,
) -> Array[Multibody3] {
  let result : Array[Multibody3] = []
  let items = self.multibodies.iter()
  for i in 0.. (Multibody3, LinkId, MultibodyJointHandle)? {
  if self.rb2mb.get_unknown_gen(id) is Some(link) {
    if self.multibodies.get(link.multibody.arena_index()) is Some(multibody) {
      if self.rb2mb.get_gen(id) is Some(generation) {
        Some(
          (
            { inner: multibody },
            LinkId(link.id),
            MultibodyJointHandle::from_raw_parts(id, generation),
          ),
        )
      } else {
        None
      }
    } else {
      None
    }
  } else {
    None
  }
}

///|
pub fn MultibodyJointSet3::joint_between(
  self : MultibodyJointSet3,
  body1 : RigidBodyHandle,
  body2 : RigidBodyHandle,
) -> (MultibodyJointHandle, Multibody3, MultibodyLink)? {
  if self.rb2mb.get(rb_handle_index(body1)) is Some(link1) &&
    self.rb2mb.get(rb_handle_index(body2)) is Some(link2) {
    if !link1.multibody.equals(link2.multibody) {
      return None
    }
    if self.multibodies.get(link1.multibody.arena_index()) is Some(multibody) {
      if link1.id < 0 ||
        link1.id >= multibody.links.length() ||
        link2.id < 0 ||
        link2.id >= multibody.links.length() {
        return None
      }
      let child1 = multibody.links[link1.id]
      let child2 = multibody.links[link2.id]
      if child1.parent_internal_id == link2.id {
        Some(
          (
            MultibodyJointHandle(rb_handle_index(body1)),
            { inner: multibody },
            child1,
          ),
        )
      } else if child2.parent_internal_id == link1.id {
        Some(
          (
            MultibodyJointHandle(rb_handle_index(body2)),
            { inner: multibody },
            child2,
          ),
        )
      } else {
        None
      }
    } else {
      None
    }
  } else {
    None
  }
}

///|
pub fn MultibodyJointSet3::attached_joints(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
) -> Array[(RigidBodyHandle, RigidBodyHandle, MultibodyJointHandle)] {
  if self.graph_node_for(handle) is Some(node_id) {
    let result : Array[(RigidBodyHandle, RigidBodyHandle, MultibodyJointHandle)] = []
    let edges = self.adjacency[node_id]
    for i in 0..= self.graph_edges.length() {
        continue
      }
      let edge = self.graph_edges[edge_id]
      result.push(
        (
          edge.parent,
          edge.child,
          MultibodyJointHandle(rb_handle_index(edge.child)),
        ),
      )
    }
    result
  } else {
    []
  }
}

///|
pub fn MultibodyJointSet3::attached_bodies(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
) -> Array[RigidBodyHandle] {
  if self.graph_node_for(handle) is Some(node_id) {
    let result : Array[RigidBodyHandle] = []
    let edges = self.adjacency[node_id]
    for i in 0..= self.graph_edges.length() {
        continue
      }
      let edge = self.graph_edges[edge_id]
      if RigidBodyHandle::equals(edge.parent, handle) {
        result.push(edge.child)
      } else {
        result.push(edge.parent)
      }
    }
    result
  } else {
    []
  }
}

///|
pub fn MultibodyJointSet3::bodies_attached_with_enabled_joint(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
) -> Array[RigidBodyHandle] {
  self.attached_bodies(handle)
}

///|
pub fn MultibodyJointSet3::iter(
  self : MultibodyJointSet3,
) -> Array[(MultibodyJointHandle, MultibodyLinkId, Multibody3, MultibodyLink)] {
  let result : Array[
    (MultibodyJointHandle, MultibodyLinkId, Multibody3, MultibodyLink),
  ] = []
  let links = self.rb2mb.iter()
  for i in 0..= 0 && link.id < multibody.links.length() {
        result.push(
          (
            MultibodyJointHandle(joint_handle),
            link,
            { inner: multibody },
            multibody.links[link.id],
          ),
        )
      }
    }
  }
  result
}

///|
fn MultibodyJointSet3::remove_joints_attached_to_rigid_body(
  self : MultibodyJointSet3,
  handle : RigidBodyHandle,
) -> Unit {
  let attached = self.attached_joints(handle)
  if attached.length() == 0 {
    return
  }
  let to_remove : Array[MultibodyJointHandle] = []
  for i in 0.. Jacobian3 {
  let data : Array[@core.Real] = []
  for _ in 0..<(size * 6) {
    data.push(0.0F)
  }
  { cols_: size, data }
}

///|
pub fn Jacobian3::cols(self : Jacobian3) -> Int {
  self.cols_
}

///|
pub fn Jacobian3::value_at(
  self : Jacobian3,
  row : Int,
  col : Int,
) -> @core.Real {
  if row < 0 || row >= 6 || col < 0 || col >= self.cols_ {
    0.0F
  } else {
    self.data[col * 6 + row]
  }
}

///|
fn Jacobian3::ensure_cols(self : Jacobian3, cols : Int) -> Unit {
  if self.cols_ != cols {
    self.cols_ = cols
    self.data.clear()
    for _ in 0..<(cols * 6) {
      self.data.push(0.0F)
    }
  } else {
    for i in 0.. Unit {
  if row >= 0 && row < 6 && col >= 0 && col < self.cols_ {
    self.data[col * 6 + row] = value
  }
}

///|
fn Jacobian3::clear_column(self : Jacobian3, col : Int) -> Unit {
  if col < 0 || col >= self.cols_ {
    return
  }
  let base = col * 6
  for row in 0..<6 {
    self.data[base + row] = 0.0F
  }
}

///|
fn mb3_logical_ndofs(multibody : Multibody) -> Int {
  if multibody.root_is_dynamic {
    multibody.ndofs() + 3
  } else {
    multibody.ndofs()
  }
}

///|
fn mb3_joint_logical_column(multibody : Multibody, assembly_id : Int) -> Int {
  if multibody.root_is_dynamic && assembly_id >= 3 {
    assembly_id + 3
  } else {
    assembly_id
  }
}

///|
fn mb3_set_spatial_column(
  jacobian : Jacobian3,
  col : Int,
  linear : @core.Vec3,
  angular : @core.Vec3,
) -> Unit {
  jacobian.set_value(0, col, linear.x)
  jacobian.set_value(1, col, linear.y)
  jacobian.set_value(2, col, linear.z)
  jacobian.set_value(3, col, angular.x)
  jacobian.set_value(4, col, angular.y)
  jacobian.set_value(5, col, angular.z)
}

///|
fn mb3_project_pose_to_2d(pose : @core.Isometry3) -> @core.Isometry2 {
  let axis = pose.rotation.to_scaled_axis()
  Isometry2(
    Vec2(pose.translation.x, pose.translation.y),
    @core.Rot2::from_angle(axis.z),
  )
}

///|
fn mb3_cached_root_pose(multibody : Multibody) -> @core.Isometry3 {
  if multibody.links.length() > 0 {
    Isometry3(
      Vec3(
        multibody.root_translation.x,
        multibody.root_translation.y,
        multibody.root_translation3_z,
      ),
      multibody.root_rotation3_extra.mul(
        @core.rotation_from_scaled_axis(Vec3(0.0F, 0.0F, multibody.root_angle)),
      ),
    )
  } else {
    @core.Isometry3::identity()
  }
}

///|
fn mb3_update_root_from_bodies(
  multibody : Multibody,
  bodies : RigidBodySet3,
) -> Unit {
  if multibody.links.length() > 0 {
    let root = multibody.links[0].rigid_body
    let translation = bodies.translation(root)
    multibody.root_translation = Vec2(translation.x, translation.y)
    multibody.root_angle = bodies.rotation(root).to_scaled_axis().z
  }
}

///|
fn mb3_update_ndofs_with_root(
  multibody : Multibody,
  bodies : RigidBodySet3,
) -> Unit {
  if multibody.links.length() <= 0 {
    multibody.ndofs = 0
    return
  }
  let root = multibody.links[0]
  let is_dynamic = if bodies.get(root.rigid_body) is Some(body) {
    !(body.body_type is Fixed)
  } else {
    true
  }
  let root_dofs = if is_dynamic { 6 } else { 0 }
  let mut joint_ndofs = 0
  for i in 1.. @core.Isometry3 {
  let mut pose = if multibody.links.length() > 0 {
    let root = multibody.links[0].rigid_body
    let body_translation = bodies.translation(root)
    @core.Isometry3(
      Vec3(
        body_translation.x,
        body_translation.y,
        multibody.root_translation3_z,
      ),
      multibody.root_rotation3_extra.mul(bodies.rotation(root)),
    )
  } else {
    @core.Isometry3::identity()
  }

  if multibody.root_is_dynamic && displacements is Some(disp) {
    let dx = if disp.length() > 0 { disp[0] } else { 0.0F }
    let dy = if disp.length() > 1 { disp[1] } else { 0.0F }
    let dz = if disp.length() > 2 { disp[2] } else { 0.0F }
    let rot_delta = @core.rotation_from_scaled_axis(
      Vec3(
        if disp.length() > 3 {
          disp[3]
        } else {
          0.0F
        },
        if disp.length() > 4 {
          disp[4]
        } else {
          0.0F
        },
        if disp.length() > 5 {
          disp[5]
        } else {
          0.0F
        },
      ),
    )
    pose = Isometry3(
      pose.translation.add(Vec3(dx, dy, dz)),
      rot_delta.mul(pose.rotation),
    )
  }

  pose
}

///|
fn mb3_store_projected_link_poses(
  multibody : Multibody,
  root_pose : @core.Isometry3,
) -> Unit {
  if multibody.links.length() <= 0 {
    return
  }

  let world_poses : Array[@core.Isometry3] = []
  for _ in 0.. Unit {
  mb3_update_ndofs_with_root(multibody, bodies)
  if multibody.links.length() <= 0 {
    update_dofs |> ignore
    return
  }
  if update_dofs {
    mb3_update_root_from_bodies(multibody, bodies)
  }
  mb3_store_projected_link_poses(multibody, mb3_cached_root_pose(multibody))
}

///|
fn mb3_update_rigid_bodies(
  multibody : Multibody,
  bodies : RigidBodySet3,
) -> Unit {
  for i in 0.. ignore
    }
  }
}

///|
fn mb3_multibody_body_to_parent(
  joint : MultibodyJoint,
  displacements : Array[@core.Real],
) -> @core.Isometry3 {
  let locked = joint.data.locked_axes().bits()
  let frame1 = @core.Isometry3(
    joint.local_anchor1_3,
    joint.local_frame1_rotation,
  )
  let frame2 = @core.Isometry3(
    joint.local_anchor2_3,
    joint.local_frame2_rotation,
  )
  let mut linear = joint.lin_coords3
  let mut angular = joint.ang_coords3
  let mut curr = 0
  if (locked & (1 << 0)) == 0 && curr < displacements.length() {
    linear = Vec3(linear.x + displacements[curr], linear.y, linear.z)
    curr = curr + 1
  }
  if (locked & (1 << 1)) == 0 && curr < displacements.length() {
    linear = Vec3(linear.x, linear.y + displacements[curr], linear.z)
    curr = curr + 1
  }
  if (locked & (1 << 2)) == 0 && curr < displacements.length() {
    linear = Vec3(linear.x, linear.y, linear.z + displacements[curr])
    curr = curr + 1
  }
  for ang_index in 0..<3 {
    let bit = 3 + ang_index
    if (locked & (1 << bit)) == 0 && curr < displacements.length() {
      angular = match ang_index {
        0 => Vec3(angular.x + displacements[curr], angular.y, angular.z)
        1 => Vec3(angular.x, angular.y + displacements[curr], angular.z)
        _ => Vec3(angular.x, angular.y, angular.z + displacements[curr])
      }
      curr = curr + 1
    }
  }

  let mut transform = @core.Isometry3(
    @core.Vec3::zero(),
    @core.rotation_from_scaled_axis(angular),
  ).mul(frame2.inverse())

  if (locked & (1 << 0)) == 0 {
    transform = @core.Isometry3::from_translation(Vec3(linear.x, 0.0F, 0.0F)).mul(
      transform,
    )
  }
  if (locked & (1 << 1)) == 0 {
    transform = @core.Isometry3::from_translation(Vec3(0.0F, linear.y, 0.0F)).mul(
      transform,
    )
  }
  if (locked & (1 << 2)) == 0 {
    transform = @core.Isometry3::from_translation(Vec3(0.0F, 0.0F, linear.z)).mul(
      transform,
    )
  }

  frame1.mul(transform)
}

///|
fn mb3_compute_link_pose(
  multibody : Multibody,
  id : LinkId,
  root_pose : @core.Isometry3,
  displacements : Array[@core.Real]?,
  out_jacobian : Jacobian3?,
) -> @core.Isometry3 {
  let branch = multibody.branch_ids(id)
  if branch.length() <= 0 {
    return @core.Isometry3::identity()
  }

  if out_jacobian is Some(jacobian) {
    jacobian.ensure_cols(mb3_logical_ndofs(multibody))
  }

  let dof_positions : Array[@core.Vec3] = []
  let dof_axes : Array[@core.Vec3] = []
  let dof_cols : Array[Int] = []
  let dof_is_angular : Array[Bool] = []
  let root_position = root_pose.translation
  let mut current_pose = root_pose

  for i in 1.. Array[@core.Real]? {
  if data.length() != 36 {
    return None
  }

  let augmented : Array[@core.Real] = []
  for row in 0..<6 {
    for col in 0..<6 {
      augmented.push(data[row * 6 + col])
    }
    for col in 0..<6 {
      augmented.push(if row == col { 1.0F } else { 0.0F })
    }
  }

  for pivot in 0..<6 {
    let mut best_row = pivot
    let mut best_abs = @core.abs(augmented[pivot * 12 + pivot])
    for row in (pivot + 1)..<6 {
      let candidate = @core.abs(augmented[row * 12 + pivot])
      if candidate > best_abs {
        best_abs = candidate
        best_row = row
      }
    }

    if best_abs <= 1.0e-8F {
      return None
    }

    if best_row != pivot {
      for col in 0..<12 {
        let temp = augmented[pivot * 12 + col]
        augmented[pivot * 12 + col] = augmented[best_row * 12 + col]
        augmented[best_row * 12 + col] = temp
      }
    }

    let pivot_value = augmented[pivot * 12 + pivot]
    for col in 0..<12 {
      augmented[pivot * 12 + col] = augmented[pivot * 12 + col] / pivot_value
    }

    for row in 0..<6 {
      if row == pivot {
        continue
      }
      let factor = augmented[row * 12 + pivot]
      if @core.abs(factor) <= 1.0e-12F {
        continue
      }
      for col in 0..<12 {
        augmented[row * 12 + col] = augmented[row * 12 + col] -
          factor * augmented[pivot * 12 + col]
      }
    }
  }

  let inverse : Array[@core.Real] = []
  for row in 0..<6 {
    for col in 0..<6 {
      inverse.push(augmented[row * 12 + 6 + col])
    }
  }
  Some(inverse)
}

///|
fn mb3_inverse_kinematics_delta_with_jacobian(
  jacobian : Jacobian3,
  desired_movement : Array[@core.Real],
  damping : @core.Real,
  displacements : DVector,
) -> Unit {
  let jj : Array[@core.Real] = []
  for _ in 0..<36 {
    jj.push(0.0F)
  }

  for row in 0..<6 {
    for col in 0..<6 {
      let mut value = 0.0F
      for j in 0.. Unit {
  mb3_forward_kinematics(self.inner, bodies, update_dofs)
}

///|
pub fn Multibody3::apply_displacements(
  self : Multibody3,
  displacements : Array[@core.Real],
) -> Unit {
  let projected : Array[@core.Real] = []
  if self.inner.root_is_dynamic {
    if displacements.length() > 0 {
      projected.push(displacements[0])
    }
    if displacements.length() > 1 {
      projected.push(displacements[1])
    }
    if displacements.length() > 2 {
      self.inner.root_translation3_z = self.inner.root_translation3_z +
        displacements[2]
    }
    let rot_delta = @core.rotation_from_scaled_axis(
      Vec3(
        if displacements.length() > 3 {
          displacements[3]
        } else {
          0.0F
        },
        if displacements.length() > 4 {
          displacements[4]
        } else {
          0.0F
        },
        if displacements.length() > 5 {
          displacements[5]
        } else {
          0.0F
        },
      ),
    )
    self.inner.root_rotation3_extra = rot_delta.mul(
      self.inner.root_rotation3_extra,
    )
    if displacements.length() > 5 {
      projected.push(displacements[5])
    }
    for i in 6.. @core.Isometry3? {
  if self.inner.link(id) is Some(_) {
    Some(
      mb3_compute_link_pose(
        self.inner,
        id,
        mb3_cached_root_pose(self.inner),
        None,
        None,
      ),
    )
  } else {
    None
  }
}

///|
pub fn Multibody3::body_jacobian(self : Multibody3, id : LinkId) -> Jacobian3 {
  let jacobian = Jacobian3::zeros(0)
  mb3_compute_link_pose(
    self.inner,
    id,
    mb3_cached_root_pose(self.inner),
    None,
    Some(jacobian),
  )
  |> ignore
  jacobian
}

///|
pub fn Multibody3::forward_kinematics_single_link(
  self : Multibody3,
  bodies : RigidBodySet3,
  id : LinkId,
  displacements : Array[@core.Real]?,
  jacobian : Jacobian3?,
) -> @core.Isometry3 {
  mb3_update_ndofs_with_root(self.inner, bodies)
  mb3_compute_link_pose(
    self.inner,
    id,
    mb3_root_pose_from_bodies(self.inner, bodies, displacements),
    displacements,
    jacobian,
  )
}

///|
pub fn Multibody3::inverse_kinematics(
  self : Multibody3,
  bodies : RigidBodySet3,
  id : LinkId,
  options : InverseKinematicsOption,
  target : @core.Isometry3,
  can_move : (MultibodyLink) -> Bool,
  displacements : DVector,
) -> Unit {
  let branch = self.inner.branch_ids(id)
  let can_move_branch : Array[Bool] = []
  for branch_id in branch {
    can_move_branch.push(can_move(self.inner.links[branch_id]))
  }

  let jacobian = Jacobian3::zeros(0)
  for _ in 0.. (@core.Real, @core.Quat) {
  let yaw = rotation.to_scaled_axis().z
  let extra = rotation.mul(
    @core.rotation_from_scaled_axis(Vec3(0.0F, 0.0F, -yaw)),
  )
  (yaw, extra)
}

///|
fn mb3_update_root_from_bodies_real(
  multibody : Multibody,
  bodies : RigidBodySet3D,
) -> Unit {
  if multibody.links.length() > 0 {
    let root = multibody.links[0].rigid_body
    if bodies.get(root) is Some(body) {
      let translation = body.translation()
      let rotation = body.rotation()
      let (yaw, extra) = mb3_split_root_rotation_real(rotation)
      multibody.root_translation = Vec2(translation.x, translation.y)
      multibody.root_translation3_z = translation.z
      multibody.root_angle = yaw
      multibody.root_rotation3_extra = extra
    }
  }
}

///|
fn mb3_update_ndofs_with_root_real(
  multibody : Multibody,
  bodies : RigidBodySet3D,
) -> Unit {
  if multibody.links.length() <= 0 {
    multibody.ndofs = 0
    return
  }
  let root = multibody.links[0]
  let is_dynamic = if bodies.get(root.rigid_body) is Some(body) {
    !(body.body_type() is Fixed)
  } else {
    true
  }
  let root_dofs = if is_dynamic { 6 } else { 0 }
  let mut joint_ndofs = 0
  for i in 1.. @core.Isometry3 {
  let mut pose = if multibody.links.length() > 0 {
    let root = multibody.links[0].rigid_body
    if bodies.get(root) is Some(body) {
      body.position()
    } else {
      mb3_cached_root_pose(multibody)
    }
  } else {
    @core.Isometry3::identity()
  }

  if multibody.root_is_dynamic && displacements is Some(disp) {
    let dx = if disp.length() > 0 { disp[0] } else { 0.0F }
    let dy = if disp.length() > 1 { disp[1] } else { 0.0F }
    let dz = if disp.length() > 2 { disp[2] } else { 0.0F }
    let rot_delta = @core.rotation_from_scaled_axis(
      Vec3(
        if disp.length() > 3 {
          disp[3]
        } else {
          0.0F
        },
        if disp.length() > 4 {
          disp[4]
        } else {
          0.0F
        },
        if disp.length() > 5 {
          disp[5]
        } else {
          0.0F
        },
      ),
    )
    pose = Isometry3(
      pose.translation.add(Vec3(dx, dy, dz)),
      rot_delta.mul(pose.rotation),
    )
  }

  pose
}

///|
fn mb3_forward_kinematics_real(
  multibody : Multibody,
  bodies : RigidBodySet3D,
  update_dofs : Bool,
) -> Unit {
  mb3_update_ndofs_with_root_real(multibody, bodies)
  if multibody.links.length() <= 0 {
    update_dofs |> ignore
    return
  }
  if update_dofs {
    mb3_update_root_from_bodies_real(multibody, bodies)
  }
  mb3_store_projected_link_poses(multibody, mb3_cached_root_pose(multibody))
}

///|
fn mb3_update_rigid_bodies_real(
  multibody : Multibody,
  bodies : RigidBodySet3D,
) -> Unit {
  let root_pose = mb3_cached_root_pose(multibody)
  for i in 0.. Int {
  mb3_logical_ndofs(self.inner)
}

///|
pub fn Multibody3DReal::forward_kinematics(
  self : Multibody3DReal,
  bodies : RigidBodySet3D,
  update_dofs : Bool,
) -> Unit {
  mb3_forward_kinematics_real(self.inner, bodies, update_dofs)
}

///|
pub fn Multibody3DReal::apply_displacements(
  self : Multibody3DReal,
  displacements : Array[@core.Real],
) -> Unit {
  Multibody3::{ inner: self.inner }.apply_displacements(displacements)
}

///|
pub fn Multibody3DReal::link_pose(
  self : Multibody3DReal,
  id : LinkId,
) -> @core.Isometry3? {
  Multibody3::{ inner: self.inner }.link_pose(id)
}

///|
pub fn Multibody3DReal::body_jacobian(
  self : Multibody3DReal,
  id : LinkId,
) -> Jacobian3 {
  Multibody3::{ inner: self.inner }.body_jacobian(id)
}

///|
pub fn Multibody3DReal::forward_kinematics_single_link(
  self : Multibody3DReal,
  bodies : RigidBodySet3D,
  id : LinkId,
  displacements : Array[@core.Real]?,
  jacobian : Jacobian3?,
) -> @core.Isometry3 {
  mb3_update_ndofs_with_root_real(self.inner, bodies)
  mb3_compute_link_pose(
    self.inner,
    id,
    mb3_root_pose_from_bodies_real(self.inner, bodies, displacements),
    displacements,
    jacobian,
  )
}

///|
pub fn Multibody3DReal::inverse_kinematics(
  self : Multibody3DReal,
  bodies : RigidBodySet3D,
  id : LinkId,
  options : InverseKinematicsOption,
  target : @core.Isometry3,
  can_move : (MultibodyLink) -> Bool,
  displacements : DVector,
) -> Unit {
  let branch = self.inner.branch_ids(id)
  let can_move_branch : Array[Bool] = []
  for branch_id in branch {
    can_move_branch.push(can_move(self.inner.links[branch_id]))
  }

  let jacobian = Jacobian3::zeros(0)
  for _ in 0.. MultibodyJointSet3DReal {
  { inner: MultibodyJointSet3() }
}

///|
pub fn MultibodyJointSet3DReal::take_wake_up(
  self : MultibodyJointSet3DReal,
) -> Array[RigidBodyHandle] {
  self.inner.take_wake_up()
}

///|
pub fn MultibodyJointSet3DReal::take_to_join(
  self : MultibodyJointSet3DReal,
) -> Array[(RigidBodyHandle, RigidBodyHandle)] {
  self.inner.take_to_join()
}

///|
pub fn MultibodyJointSet3DReal::disabled_contact_pairs(
  self : MultibodyJointSet3DReal,
) -> Array[(RigidBodyHandle, RigidBodyHandle)] {
  self.inner.disabled_contact_pairs()
}

///|
pub fn MultibodyJointSet3DReal::interaction_pairs(
  self : MultibodyJointSet3DReal,
) -> Array[(RigidBodyHandle, RigidBodyHandle)] {
  self.inner.interaction_pairs()
}

///|
pub fn MultibodyJointSet3DReal::insert(
  self : MultibodyJointSet3DReal,
  parent : RigidBodyHandle,
  child : RigidBodyHandle,
  joint : GenericJoint3DReal,
  wake_up : Bool,
) -> MultibodyJointHandle? {
  self.inner.insert(parent, child, GenericJoint::from_3d_real(joint), wake_up)
}

///|
pub fn MultibodyJointSet3DReal::insert_kinematic(
  self : MultibodyJointSet3DReal,
  parent : RigidBodyHandle,
  child : RigidBodyHandle,
  joint : GenericJoint3DReal,
  wake_up : Bool,
) -> MultibodyJointHandle? {
  self.inner.insert_kinematic(
    parent,
    child,
    GenericJoint::from_3d_real(joint),
    wake_up,
  )
}

///|
pub fn MultibodyJointSet3DReal::get(
  self : MultibodyJointSet3DReal,
  handle : MultibodyJointHandle,
) -> (Multibody3DReal, LinkId)? {
  if self.inner.get(handle) is Some((multibody, link)) {
    Some(({ inner: multibody.inner }, link))
  } else {
    None
  }
}

///|
pub fn MultibodyJointSet3DReal::get_mut(
  self : MultibodyJointSet3DReal,
  handle : MultibodyJointHandle,
) -> (Multibody3DReal, LinkId)? {
  if self.inner.get_mut(handle) is Some((multibody, link)) {
    Some(({ inner: multibody.inner }, link))
  } else {
    None
  }
}

///|
pub fn MultibodyJointSet3DReal::remove(
  self : MultibodyJointSet3DReal,
  handle : MultibodyJointHandle,
  wake_up : Bool,
) -> Unit {
  self.inner.remove(handle, wake_up)
}

///|
pub fn MultibodyJointSet3DReal::remove_multibody_articulations(
  self : MultibodyJointSet3DReal,
  handle : RigidBodyHandle,
  wake_up : Bool,
) -> Unit {
  self.inner.remove_multibody_articulations(handle, wake_up)
}

///|
pub fn MultibodyJointSet3DReal::rigid_body_link(
  self : MultibodyJointSet3DReal,
  handle : RigidBodyHandle,
) -> MultibodyLinkId? {
  self.inner.rigid_body_link(handle)
}

///|
pub fn MultibodyJointSet3DReal::get_multibody(
  self : MultibodyJointSet3DReal,
  index : MultibodyIndex,
) -> Multibody3DReal? {
  if self.inner.get_multibody(index) is Some(multibody) {
    Some({ inner: multibody.inner })
  } else {
    None
  }
}

///|
pub fn MultibodyJointSet3DReal::get_multibody_mut(
  self : MultibodyJointSet3DReal,
  index : MultibodyIndex,
) -> Multibody3DReal? {
  if self.inner.get_multibody_mut(index) is Some(multibody) {
    Some({ inner: multibody.inner })
  } else {
    None
  }
}

///|
pub fn MultibodyJointSet3DReal::multibodies(
  self : MultibodyJointSet3DReal,
) -> Array[Multibody3DReal] {
  let result : Array[Multibody3DReal] = []
  let items = self.inner.multibodies()
  for i in 0.. (Multibody3DReal, LinkId, MultibodyJointHandle)? {
  if self.inner.get_unknown_gen(id) is Some((multibody, link, handle)) {
    Some(({ inner: multibody.inner }, link, handle))
  } else {
    None
  }
}

///|
pub fn MultibodyJointSet3DReal::joint_between(
  self : MultibodyJointSet3DReal,
  body1 : RigidBodyHandle,
  body2 : RigidBodyHandle,
) -> (MultibodyJointHandle, Multibody3DReal, MultibodyLink)? {
  if self.inner.joint_between(body1, body2) is Some((handle, multibody, link)) {
    Some((handle, { inner: multibody.inner }, link))
  } else {
    None
  }
}

///|
pub fn MultibodyJointSet3DReal::attached_joints(
  self : MultibodyJointSet3DReal,
  handle : RigidBodyHandle,
) -> Array[(RigidBodyHandle, RigidBodyHandle, MultibodyJointHandle)] {
  self.inner.attached_joints(handle)
}

///|
pub fn MultibodyJointSet3DReal::attached_bodies(
  self : MultibodyJointSet3DReal,
  handle : RigidBodyHandle,
) -> Array[RigidBodyHandle] {
  self.inner.attached_bodies(handle)
}

///|
pub fn MultibodyJointSet3DReal::bodies_attached_with_enabled_joint(
  self : MultibodyJointSet3DReal,
  handle : RigidBodyHandle,
) -> Array[RigidBodyHandle] {
  self.inner.bodies_attached_with_enabled_joint(handle)
}

///|
pub fn MultibodyJointSet3DReal::iter(
  self : MultibodyJointSet3DReal,
) -> Array[
  (MultibodyJointHandle, MultibodyLinkId, Multibody3DReal, MultibodyLink),
] {
  let result : Array[
    (MultibodyJointHandle, MultibodyLinkId, Multibody3DReal, MultibodyLink),
  ] = []
  let items = self.inner.iter()
  for i in 0.. Array[MultibodyRevoluteJointDesc] {
  self.inner.revolute_joint_descriptors()
}

///|
pub fn MultibodyJointSet3DReal::set_revolute_joint_solver_impulses(
  self : MultibodyJointSet3DReal,
  link : MultibodyLinkId,
  lin_impulses : @core.Vec2,
  motor_impulse : @core.Real,
  limit_impulse : @core.Real,
) -> Unit {
  self.inner.set_revolute_joint_solver_impulses(
    link, lin_impulses, motor_impulse, limit_impulse,
  )
}

///|
pub fn MultibodyJointSet3DReal::sync_rigid_bodies_from_multibodies(
  self : MultibodyJointSet3DReal,
  bodies : RigidBodySet3D,
) -> Unit {
  let items = self.inner.multibodies.iter()
  for i in 0.. ignore
  }
}