///|
/// A finite three-dimensional vector used for sensor-frame and world-frame
/// calculations.  The type is deliberately small and immutable from callers;
/// every arithmetic method returns a new value.
pub struct Vec3D {
  x : Double
  y : Double
  z : Double
} derive(Debug)

///|
/// Construct a three-dimensional vector.
pub fn Vec3D::new(x : Double, y : Double, z : Double) -> Vec3D {
  { x, y, z }
}

///|
/// Return the x component.
pub fn Vec3D::x(self : Vec3D) -> Double {
  self.x
}

///|
/// Return the y component.
pub fn Vec3D::y(self : Vec3D) -> Double {
  self.y
}

///|
/// Return the z component.
pub fn Vec3D::z(self : Vec3D) -> Double {
  self.z
}

///|
/// Return a zero vector.
pub fn vec3d_zero() -> Vec3D {
  Vec3D::new(0.0, 0.0, 0.0)
}

///|
/// Return a vector with all components equal to `value`.
pub fn vec3d_splat(value : Double) -> Vec3D {
  Vec3D::new(value, value, value)
}

///|
/// Add two vectors.
pub fn Vec3D::add(self : Vec3D, other : Vec3D) -> Vec3D {
  Vec3D::new(self.x + other.x, self.y + other.y, self.z + other.z)
}

///|
/// Subtract two vectors.
pub fn Vec3D::sub(self : Vec3D, other : Vec3D) -> Vec3D {
  Vec3D::new(self.x - other.x, self.y - other.y, self.z - other.z)
}

///|
/// Negate a vector.
pub fn Vec3D::negate(self : Vec3D) -> Vec3D {
  Vec3D::new(-self.x, -self.y, -self.z)
}

///|
/// Scale a vector.
pub fn Vec3D::scale(self : Vec3D, factor : Double) -> Vec3D {
  Vec3D::new(self.x * factor, self.y * factor, self.z * factor)
}

///|
/// Multiply components pairwise.
pub fn Vec3D::hadamard(self : Vec3D, other : Vec3D) -> Vec3D {
  Vec3D::new(self.x * other.x, self.y * other.y, self.z * other.z)
}

///|
/// Compute the dot product.
pub fn Vec3D::dot(self : Vec3D, other : Vec3D) -> Double {
  self.x * other.x + self.y * other.y + self.z * other.z
}

///|
/// Compute the cross product.
pub fn Vec3D::cross(self : Vec3D, other : Vec3D) -> Vec3D {
  Vec3D::new(
    self.y * other.z - self.z * other.y,
    self.z * other.x - self.x * other.z,
    self.x * other.y - self.y * other.x,
  )
}

///|
/// Return the squared Euclidean norm.
pub fn Vec3D::norm_squared(self : Vec3D) -> Double {
  self.dot(self)
}

///|
/// Return the Euclidean norm.
pub fn Vec3D::norm(self : Vec3D) -> Double {
  self.norm_squared().sqrt()
}

///|
/// Normalize the vector, returning `None` for a zero or non-finite vector.
pub fn Vec3D::normalize(self : Vec3D) -> Vec3D? {
  let length = self.norm()
  if length <= 0.0 || length.is_nan() || length.is_inf() {
    None
  } else {
    Some(self.scale(1.0 / length))
  }
}

///|
/// Return the distance between two points.
pub fn Vec3D::distance(self : Vec3D, other : Vec3D) -> Double {
  self.sub(other).norm()
}

///|
/// Linearly interpolate two vectors.
pub fn Vec3D::lerp(self : Vec3D, other : Vec3D, amount : Double) -> Vec3D {
  self.scale(1.0 - amount).add(other.scale(amount))
}

///|
/// Project this vector onto a direction.  A zero direction produces zero.
pub fn Vec3D::project(self : Vec3D, direction : Vec3D) -> Vec3D {
  let denominator = direction.norm_squared()
  if denominator <= 0.0 {
    vec3d_zero()
  } else {
    direction.scale(self.dot(direction) / denominator)
  }
}

///|
/// Remove the component parallel to a direction.
pub fn Vec3D::reject(self : Vec3D, direction : Vec3D) -> Vec3D {
  self.sub(self.project(direction))
}

///|
/// Reflect a vector across a plane normal.
pub fn Vec3D::reflect(self : Vec3D, normal : Vec3D) -> Vec3D {
  self.sub(normal.scale(2.0 * self.dot(normal)))
}

///|
/// Clamp each component to the same interval.
pub fn Vec3D::clamp(self : Vec3D, lower : Double, upper : Double) -> Vec3D {
  Vec3D::new(
    self.x.clamp(min=lower, max=upper),
    self.y.clamp(min=lower, max=upper),
    self.z.clamp(min=lower, max=upper),
  )
}

///|
/// Return whether all components are finite.
pub fn Vec3D::is_finite(self : Vec3D) -> Bool {
  !self.x.is_nan() &&
  !self.x.is_inf() &&
  !self.y.is_nan() &&
  !self.y.is_inf() &&
  !self.z.is_nan() &&
  !self.z.is_inf()
}

///|
/// Return the largest absolute component.
pub fn Vec3D::max_abs(self : Vec3D) -> Double {
  let ax = self.x.abs()
  let ay = self.y.abs()
  let az = self.z.abs()
  if ax > ay && ax > az {
    ax
  } else if ay > az {
    ay
  } else {
    az
  }
}

///|
/// Convert to an owned array in x/y/z order.
pub fn Vec3D::to_array(self : Vec3D) -> Array[Double] {
  [self.x, self.y, self.z]
}

///|
/// Construct from the first three entries of an array.
pub fn vec3d_from_array(values : Array[Double]) -> Vec3D? {
  if values.length() < 3 {
    None
  } else {
    Some(Vec3D::new(values[0], values[1], values[2]))
  }
}

///|
/// Return the component-wise minimum.
pub fn vec3d_min(left : Vec3D, right : Vec3D) -> Vec3D {
  Vec3D::new(
    if left.x < right.x {
      left.x
    } else {
      right.x
    },
    if left.y < right.y {
      left.y
    } else {
      right.y
    },
    if left.z < right.z {
      left.z
    } else {
      right.z
    },
  )
}

///|
/// Return the component-wise maximum.
pub fn vec3d_max(left : Vec3D, right : Vec3D) -> Vec3D {
  Vec3D::new(
    if left.x > right.x {
      left.x
    } else {
      right.x
    },
    if left.y > right.y {
      left.y
    } else {
      right.y
    },
    if left.z > right.z {
      left.z
    } else {
      right.z
    },
  )
}

///|
/// A quaternion representing a three-dimensional orientation.
pub struct Quaternion3D {
  w : Double
  x : Double
  y : Double
  z : Double
} derive(Debug)

///|
/// Construct a quaternion from scalar and vector parts.
pub fn Quaternion3D::new(
  w : Double,
  x : Double,
  y : Double,
  z : Double,
) -> Quaternion3D {
  { w, x, y, z }
}

///|
/// Return the identity orientation.
pub fn quaternion3d_identity() -> Quaternion3D {
  Quaternion3D::new(1.0, 0.0, 0.0, 0.0)
}

///|
/// Access the scalar part.
pub fn Quaternion3D::w(self : Quaternion3D) -> Double {
  self.w
}

///|
/// Access the x component.
pub fn Quaternion3D::x(self : Quaternion3D) -> Double {
  self.x
}

///|
/// Access the y component.
pub fn Quaternion3D::y(self : Quaternion3D) -> Double {
  self.y
}

///|
/// Access the z component.
pub fn Quaternion3D::z(self : Quaternion3D) -> Double {
  self.z
}

///|
/// Return the squared quaternion norm.
pub fn Quaternion3D::norm_squared(self : Quaternion3D) -> Double {
  self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z
}

///|
/// Return the quaternion norm.
pub fn Quaternion3D::norm(self : Quaternion3D) -> Double {
  self.norm_squared().sqrt()
}

///|
/// Normalize a quaternion, returning `None` for invalid input.
pub fn Quaternion3D::normalize(self : Quaternion3D) -> Quaternion3D? {
  let length = self.norm()
  if length <= 0.0 || length.is_nan() || length.is_inf() {
    None
  } else {
    Some(
      Quaternion3D::new(
        self.w / length,
        self.x / length,
        self.y / length,
        self.z / length,
      ),
    )
  }
}

///|
/// Return the conjugate quaternion.
pub fn Quaternion3D::conjugate(self : Quaternion3D) -> Quaternion3D {
  Quaternion3D::new(self.w, -self.x, -self.y, -self.z)
}

///|
/// Return the inverse quaternion, when it exists.
pub fn Quaternion3D::inverse(self : Quaternion3D) -> Quaternion3D? {
  let norm = self.norm_squared()
  if norm <= 0.0 || norm.is_nan() || norm.is_inf() {
    None
  } else {
    Some(self.conjugate().scale(1.0 / norm))
  }
}

///|
/// Scale all quaternion components.
pub fn Quaternion3D::scale(
  self : Quaternion3D,
  factor : Double,
) -> Quaternion3D {
  Quaternion3D::new(
    self.w * factor,
    self.x * factor,
    self.y * factor,
    self.z * factor,
  )
}

///|
/// Hamilton product of two quaternions.
pub fn Quaternion3D::multiply(
  self : Quaternion3D,
  other : Quaternion3D,
) -> Quaternion3D {
  Quaternion3D::new(
    self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
    self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
    self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
    self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w,
  )
}

///|
/// Dot product of quaternion components.
pub fn Quaternion3D::dot(self : Quaternion3D, other : Quaternion3D) -> Double {
  self.w * other.w + self.x * other.x + self.y * other.y + self.z * other.z
}

///|
/// Return whether all components are finite.
pub fn Quaternion3D::is_finite(self : Quaternion3D) -> Bool {
  !self.w.is_nan() &&
  !self.w.is_inf() &&
  !self.x.is_nan() &&
  !self.x.is_inf() &&
  !self.y.is_nan() &&
  !self.y.is_inf() &&
  !self.z.is_nan() &&
  !self.z.is_inf()
}

///|
/// Rotate a vector using this quaternion.  Non-unit inputs are normalized.
pub fn Quaternion3D::rotate(self : Quaternion3D, value : Vec3D) -> Vec3D {
  let unit = match self.normalize() {
    Some(q) => q
    None => return value
  }
  let pure = Quaternion3D::new(0.0, value.x(), value.y(), value.z())
  let result = unit.multiply(pure).multiply(unit.conjugate())
  Vec3D::new(result.x, result.y, result.z)
}

///|
/// Convert a quaternion to a 3x3 rotation matrix.
pub fn Quaternion3D::to_matrix(self : Quaternion3D) -> Matrix {
  let unit = match self.normalize() {
    Some(q) => q
    None => return Matrix::identity(3)
  }
  let xx = unit.x * unit.x
  let yy = unit.y * unit.y
  let zz = unit.z * unit.z
  let xy = unit.x * unit.y
  let xz = unit.x * unit.z
  let yz = unit.y * unit.z
  let wx = unit.w * unit.x
  let wy = unit.w * unit.y
  let wz = unit.w * unit.z
  Matrix::from_rows([
    [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)],
    [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)],
    [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)],
  ])
}

///|
/// Construct a quaternion from a 3x3 rotation matrix using the trace branch.
/// This is intended for matrices close to a proper rotation.
pub fn quaternion3d_from_matrix(matrix : Matrix) -> Quaternion3D {
  if matrix.rows() != 3 || matrix.cols() != 3 {
    return quaternion3d_identity()
  }
  let trace = matrix.get(0, 0) + matrix.get(1, 1) + matrix.get(2, 2)
  if trace > 0.0 {
    let scale = (trace + 1.0).sqrt() * 2.0
    if scale <= 0.0 {
      return quaternion3d_identity()
    }
    Quaternion3D::new(
      0.25 * scale,
      (matrix.get(2, 1) - matrix.get(1, 2)) / scale,
      (matrix.get(0, 2) - matrix.get(2, 0)) / scale,
      (matrix.get(1, 0) - matrix.get(0, 1)) / scale,
    )
  } else {
    let xx = (1.0 + matrix.get(0, 0) - matrix.get(1, 1) - matrix.get(2, 2)).sqrt()
    if xx <= 0.0 {
      return quaternion3d_identity()
    }
    let x = 0.5 * xx
    let divisor = 4.0 * x
    Quaternion3D::new(
      (matrix.get(2, 1) - matrix.get(1, 2)) / divisor,
      x,
      (matrix.get(0, 1) + matrix.get(1, 0)) / divisor,
      (matrix.get(0, 2) + matrix.get(2, 0)) / divisor,
    )
  }
}

///|
/// Interpolate quaternion components and renormalize.  This avoids a
/// trigonometric dependency while remaining stable for telemetry smoothing.
pub fn Quaternion3D::lerp(
  self : Quaternion3D,
  other : Quaternion3D,
  amount : Double,
) -> Quaternion3D {
  let sign = if self.dot(other) < 0.0 { -1.0 } else { 1.0 }
  let mixed = Quaternion3D::new(
    self.w * (1.0 - amount) + other.w * sign * amount,
    self.x * (1.0 - amount) + other.x * sign * amount,
    self.y * (1.0 - amount) + other.y * sign * amount,
    self.z * (1.0 - amount) + other.z * sign * amount,
  )
  match mixed.normalize() {
    Some(value) => value
    None => quaternion3d_identity()
  }
}

///|
/// A rigid pose mapping local-frame points into a parent frame.
pub struct Pose3D {
  translation : Vec3D
  rotation : Quaternion3D
} derive(Debug)

///|
/// Construct a pose.
pub fn Pose3D::new(translation : Vec3D, rotation : Quaternion3D) -> Pose3D {
  { translation, rotation }
}

///|
/// Return the identity pose.
pub fn pose3d_identity() -> Pose3D {
  Pose3D::new(vec3d_zero(), quaternion3d_identity())
}

///|
/// Return the translation component.
pub fn Pose3D::translation(self : Pose3D) -> Vec3D {
  self.translation
}

///|
/// Return the rotation component.
pub fn Pose3D::rotation(self : Pose3D) -> Quaternion3D {
  self.rotation
}

///|
/// Transform a point from local to parent coordinates.
pub fn Pose3D::transform_point(self : Pose3D, point : Vec3D) -> Vec3D {
  self.rotation.rotate(point).add(self.translation)
}

///|
/// Transform a direction without applying translation.
pub fn Pose3D::transform_vector(self : Pose3D, value : Vec3D) -> Vec3D {
  self.rotation.rotate(value)
}

///|
/// Compose this pose with a child pose.
pub fn Pose3D::compose(self : Pose3D, child : Pose3D) -> Pose3D {
  Pose3D::new(
    self.transform_point(child.translation),
    self.rotation.multiply(child.rotation),
  )
}

///|
/// Return the inverse mapping.
pub fn Pose3D::inverse(self : Pose3D) -> Pose3D {
  let rotation = match self.rotation.inverse() {
    Some(value) => value
    None => quaternion3d_identity()
  }
  Pose3D::new(rotation.rotate(self.translation.negate()), rotation)
}

///|
/// Interpolate translation and orientation.
pub fn Pose3D::interpolate(
  self : Pose3D,
  other : Pose3D,
  amount : Double,
) -> Pose3D {
  Pose3D::new(
    self.translation.lerp(other.translation, amount),
    self.rotation.lerp(other.rotation, amount),
  )
}

///|
/// Return the translation distance between poses.
pub fn Pose3D::translation_distance(self : Pose3D, other : Pose3D) -> Double {
  self.translation.distance(other.translation)
}

///|
/// Return whether the pose contains finite values.
pub fn Pose3D::is_finite(self : Pose3D) -> Bool {
  self.translation.is_finite() && self.rotation.is_finite()
}

///|
/// Convert the pose to a homogeneous 4x4 transform matrix.
pub fn Pose3D::to_matrix(self : Pose3D) -> Matrix {
  let rotation = self.rotation.to_matrix()
  Matrix::from_rows([
    [
      rotation.get(0, 0),
      rotation.get(0, 1),
      rotation.get(0, 2),
      self.translation.x(),
    ],
    [
      rotation.get(1, 0),
      rotation.get(1, 1),
      rotation.get(1, 2),
      self.translation.y(),
    ],
    [
      rotation.get(2, 0),
      rotation.get(2, 1),
      rotation.get(2, 2),
      self.translation.z(),
    ],
    [0.0, 0.0, 0.0, 1.0],
  ])
}

///|
/// Construct a pose from a homogeneous 4x4 transform matrix.
pub fn pose3d_from_matrix(matrix : Matrix) -> Pose3D {
  if matrix.rows() != 4 || matrix.cols() != 4 {
    return pose3d_identity()
  }
  let rotation = Matrix::from_rows([
    matrix.row(0)[:3].to_owned(),
    matrix.row(1)[:3].to_owned(),
    matrix.row(2)[:3].to_owned(),
  ])
  Pose3D::new(
    Vec3D::new(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3)),
    quaternion3d_from_matrix(rotation),
  )
}

///|
/// A bounded sequence of timestamped poses for calibration and replay.
pub struct Pose3DSeries {
  entries : Array[(Int, Pose3D)]
  capacity : Int
  mut rejected : Int
} derive(Debug)

///|
/// Construct an empty pose series.
pub fn Pose3DSeries::new(capacity : Int) -> Pose3DSeries {
  {
    entries: [],
    capacity: if capacity < 0 {
      0
    } else {
      capacity
    },
    rejected: 0,
  }
}

///|
/// Append a pose when its timestamp is monotonic and its values are finite.
pub fn Pose3DSeries::push(
  self : Pose3DSeries,
  timestamp : Int,
  pose : Pose3D,
) -> Bool {
  if self.capacity == 0 || !pose.is_finite() {
    self.rejected = self.rejected + 1
    return false
  }
  if self.entries.length() > 0 &&
    timestamp < self.entries[self.entries.length() - 1].0 {
    self.rejected = self.rejected + 1
    return false
  }
  if self.entries.length() >= self.capacity {
    for i in 1.. Int {
  self.entries.length()
}

///|
/// Return the configured capacity.
pub fn Pose3DSeries::capacity(self : Pose3DSeries) -> Int {
  self.capacity
}

///|
/// Return the number of rejected poses.
pub fn Pose3DSeries::rejected(self : Pose3DSeries) -> Int {
  self.rejected
}

///|
/// Return a copy of stored timestamp/pose pairs.
pub fn Pose3DSeries::entries(self : Pose3DSeries) -> Array[(Int, Pose3D)] {
  self.entries.copy()
}

///|
/// Return the oldest entry.
pub fn Pose3DSeries::first(self : Pose3DSeries) -> (Int, Pose3D)? {
  if self.entries.length() == 0 {
    None
  } else {
    Some(self.entries[0])
  }
}

///|
/// Return the newest entry.
pub fn Pose3DSeries::last(self : Pose3DSeries) -> (Int, Pose3D)? {
  if self.entries.length() == 0 {
    None
  } else {
    Some(self.entries[self.entries.length() - 1])
  }
}

///|
/// Return the timestamp span.
pub fn Pose3DSeries::duration(self : Pose3DSeries) -> Int {
  match (self.first(), self.last()) {
    (Some(first), Some(last)) => last.0 - first.0
    _ => 0
  }
}

///|
/// Remove all stored poses and rejection history.
pub fn Pose3DSeries::clear(self : Pose3DSeries) -> Unit {
  self.entries.clear()
  self.rejected = 0
}

///|
/// Interpolate a pose series at a timestamp.  Extrapolation is not performed.
pub fn pose3d_series_interpolate(
  series : Pose3DSeries,
  timestamp : Int,
) -> Pose3D? {
  let entries = series.entries()
  if entries.length() == 0 {
    return None
  }
  if timestamp < entries[0].0 || timestamp > entries[entries.length() - 1].0 {
    return None
  }
  for i in 1.. Vec3D {
  let entries = series.entries()
  if entries.length() == 0 {
    return vec3d_zero()
  }
  let mut sum = vec3d_zero()
  for entry in entries {
    sum = sum.add(entry.1.translation())
  }
  sum.scale(1.0 / entries.length().to_double())
}

///|
/// Compute the maximum translation step between adjacent poses.
pub fn pose3d_series_max_step(series : Pose3DSeries) -> Double {
  let entries = series.entries()
  let mut result = 0.0
  if entries.length() < 2 {
    return result
  }
  for i in 1.. result {
      result = distance
    }
  }
  result
}

///|
/// Compute a rigid-body transform from a local vector and pose.
pub fn transform_point3d(pose : Pose3D, point : Vec3D) -> Vec3D {
  pose.transform_point(point)
}

///|
/// Apply the inverse pose to a parent-frame point.
pub fn inverse_transform_point3d(pose : Pose3D, point : Vec3D) -> Vec3D {
  pose.inverse().transform_point(point)
}

///|
/// Transform an array of points without modifying the input.
pub fn transform_points3d(pose : Pose3D, points : Array[Vec3D]) -> Array[Vec3D] {
  Array::makei(points.length(), i => pose.transform_point(points[i]))
}

///|
/// Compute the axis-aligned bounds of a point cloud.
pub fn point_cloud_bounds3d(points : Array[Vec3D]) -> (Vec3D, Vec3D)? {
  if points.length() == 0 {
    return None
  }
  let mut lower = points[0]
  let mut upper = points[0]
  for point in points[1:] {
    lower = vec3d_min(lower, point)
    upper = vec3d_max(upper, point)
  }
  Some((lower, upper))
}

///|
/// Compute the centroid of a finite point cloud.
pub fn point_cloud_centroid3d(points : Array[Vec3D]) -> Vec3D? {
  if points.length() == 0 {
    return None
  }
  let mut sum = vec3d_zero()
  let mut count = 0
  for point in points {
    if point.is_finite() {
      sum = sum.add(point)
      count = count + 1
    }
  }
  if count == 0 {
    None
  } else {
    Some(sum.scale(1.0 / count.to_double()))
  }
}

///|
/// Compute the covariance matrix of a 3D point cloud.
pub fn point_cloud_covariance3d(points : Array[Vec3D]) -> Matrix {
  guard point_cloud_centroid3d(points) is Some(mean) else {
    return Matrix::zeros(3, 3)
  }
  let result = Matrix::zeros(3, 3)
  let mut count = 0
  for point in points {
    if point.is_finite() {
      let delta = point.sub(mean)
      let values = delta.to_array()
      for i in 0..<3 {
        for j in 0..<3 {
          result.set(i, j, result.get(i, j) + values[i] * values[j]) |> ignore
        }
      }
      count = count + 1
    }
  }
  if count > 1 {
    result.scale(1.0 / (count - 1).to_double())
  } else {
    result
  }
}

///|
/// Build a diagonal covariance matrix from component standard deviations.
pub fn diagonal_covariance3d(deviations : Vec3D) -> Matrix {
  Matrix::from_rows([
    [deviations.x() * deviations.x(), 0.0, 0.0],
    [0.0, deviations.y() * deviations.y(), 0.0],
    [0.0, 0.0, deviations.z() * deviations.z()],
  ])
}

///|
/// Return the trace of a 3D covariance matrix.
pub fn covariance_trace3d(covariance : Matrix) -> Double {
  if covariance.rows() != 3 || covariance.cols() != 3 {
    0.0
  } else {
    covariance.get(0, 0) + covariance.get(1, 1) + covariance.get(2, 2)
  }
}

///|
/// Inflate a covariance matrix by a non-negative factor.
pub fn inflate_covariance3d(covariance : Matrix, factor : Double) -> Matrix {
  let safe = if factor < 0.0 { 0.0 } else { factor }
  covariance.scale(safe)
}

///|
/// Compute a Mahalanobis-like diagonal distance for a 3D residual.
pub fn diagonal_mahalanobis3d(
  residual : Vec3D,
  standard_deviation : Vec3D,
) -> Double {
  let x = if standard_deviation.x().abs() > 0.0 {
    residual.x() / standard_deviation.x()
  } else {
    0.0
  }
  let y = if standard_deviation.y().abs() > 0.0 {
    residual.y() / standard_deviation.y()
  } else {
    0.0
  }
  let z = if standard_deviation.z().abs() > 0.0 {
    residual.z() / standard_deviation.z()
  } else {
    0.0
  }
  (x * x + y * y + z * z).sqrt()
}

///|
/// Project a point onto a plane defined by a point and a normal.
pub fn project_point_to_plane3d(
  point : Vec3D,
  plane_point : Vec3D,
  plane_normal : Vec3D,
) -> Vec3D {
  point.sub(point.sub(plane_point).project(plane_normal))
}

///|
/// Return the signed distance from a point to a plane.
pub fn signed_plane_distance3d(
  point : Vec3D,
  plane_point : Vec3D,
  plane_normal : Vec3D,
) -> Double {
  let length = plane_normal.norm()
  if length <= 0.0 {
    0.0
  } else {
    point.sub(plane_point).dot(plane_normal) / length
  }
}

///|
/// Build a quaternion that rotates one vector to another using a stable
/// half-way construction.  Opposing vectors fall back to a deterministic axis.
pub fn quaternion3d_between(from : Vec3D, to : Vec3D) -> Quaternion3D {
  guard from.normalize() is Some(left) else { return quaternion3d_identity() }
  guard to.normalize() is Some(right) else { return quaternion3d_identity() }
  let dot = left.dot(right)
  if dot > 0.999999 {
    return quaternion3d_identity()
  }
  if dot < -0.999999 {
    let axis = if left.x().abs() < left.y().abs() {
      left.cross(Vec3D::new(1.0, 0.0, 0.0))
    } else {
      left.cross(Vec3D::new(0.0, 1.0, 0.0))
    }
    guard axis.normalize() is Some(unit_axis) else {
      return quaternion3d_identity()
    }
    return Quaternion3D::new(0.0, unit_axis.x(), unit_axis.y(), unit_axis.z())
  }
  let cross = left.cross(right)
  match
    Quaternion3D::new(1.0 + dot, cross.x(), cross.y(), cross.z()).normalize() {
    Some(result) => result
    None => quaternion3d_identity()
  }
}

///|
/// Compute the relative pose from `parent` to `child`.
pub fn relative_pose3d(parent : Pose3D, child : Pose3D) -> Pose3D {
  parent.inverse().compose(child)
}

///|
/// Average a sequence of poses using translation averaging and quaternion
/// component averaging with sign correction.
pub fn average_pose3d(poses : Array[Pose3D]) -> Pose3D? {
  if poses.length() == 0 {
    return None
  }
  let mut translation = vec3d_zero()
  let first_rotation = poses[0].rotation()
  let mut rotation = Quaternion3D::new(0.0, 0.0, 0.0, 0.0)
  for pose in poses {
    translation = translation.add(pose.translation())
    let sign = if first_rotation.dot(pose.rotation()) < 0.0 {
      -1.0
    } else {
      1.0
    }
    rotation = rotation.add_quaternion(pose.rotation().scale(sign))
  }
  guard rotation.normalize() is Some(unit) else { return None }
  Some(Pose3D::new(translation.scale(1.0 / poses.length().to_double()), unit))
}

///|
/// Add two quaternions component-wise for averaging.
pub fn Quaternion3D::add_quaternion(
  self : Quaternion3D,
  other : Quaternion3D,
) -> Quaternion3D {
  Quaternion3D::new(
    self.w + other.w,
    self.x + other.x,
    self.y + other.y,
    self.z + other.z,
  )
}

///|
/// Convert a pose to a compact vector `[tx, ty, tz, qw, qx, qy, qz]`.
pub fn Pose3D::to_vector(self : Pose3D) -> Array[Double] {
  [
    self.translation.x(),
    self.translation.y(),
    self.translation.z(),
    self.rotation.w(),
    self.rotation.x(),
    self.rotation.y(),
    self.rotation.z(),
  ]
}

///|
/// Construct a pose from a seven-element vector.
pub fn pose3d_from_vector(values : Array[Double]) -> Pose3D? {
  if values.length() < 7 {
    return None
  }
  Some(
    Pose3D::new(
      Vec3D::new(values[0], values[1], values[2]),
      Quaternion3D::new(values[3], values[4], values[5], values[6]),
    ),
  )
}

///|
/// Compute the sum of squared point residuals after applying a pose.
pub fn transformed_cloud_error3d(
  pose : Pose3D,
  points : Array[Vec3D],
  expected : Array[Vec3D],
) -> Double {
  let count = if points.length() < expected.length() {
    points.length()
  } else {
    expected.length()
  }
  let mut result = 0.0
  for i in 0.. Double {
  if series.capacity() == 0 {
    return 0.0
  }
  let occupancy = series.length().to_double() / series.capacity().to_double()
  let rejection = series.rejected().to_double() /
    (series.length() + series.rejected() + 1).to_double()
  (occupancy * (1.0 - rejection)).clamp(min=0.0, max=1.0)
}