mod closest_features;
use glam::{Affine3A, Mat3A, Vec3A};
use super::{CarToiBoxShape, CarToiContact, MAXIMUM_DISTANCE_SQUARED};
const MIN_AXIS_LEN_SQ: f32 = 1.0e-12;
const NORMAL_MIN_CORE_DISTANCE_SQ: f32 = 1.0e-12;
struct CoreBox {
center: Vec3A,
rotation: Mat3A,
half_extents: Vec3A,
}
impl CoreBox {
fn new(shape: &CarToiBoxShape, trans: &Affine3A) -> Self {
let compound = shape.compound();
let local = compound.child_trans;
Self {
center: trans.transform_point3a(local.translation),
rotation: trans.matrix3 * local.matrix3,
half_extents: compound.child_shape.get_half_extents(),
}
}
}
struct AxisSweep {
max_gap: f32,
max_gap_axis: Vec3A,
}
#[must_use]
pub(crate) fn closest_points(
shape_a: &CarToiBoxShape,
trans_a: &Affine3A,
shape_b: &CarToiBoxShape,
trans_b: &Affine3A,
) -> Option<CarToiContact> {
let core_a = CoreBox::new(shape_a, trans_a);
let core_b = CoreBox::new(shape_b, trans_b);
let offset = (trans_a.translation + trans_b.translation) * 0.5;
let center_a = core_a.center - offset;
let center_b = core_b.center - offset;
let rotation = core_a.rotation.transpose() * core_b.rotation;
let center_rel = core_a.rotation.transpose() * (center_b - center_a);
let sweep = sat_sweep(
core_a.half_extents,
core_b.half_extents,
rotation,
center_rel,
);
let margin_sum = shape_a.margin() + shape_b.margin();
let (distance, normal_local) = if sweep.max_gap > 0.0 {
let nearest = closest_features::nearest(
core_a.half_extents,
core_b.half_extents,
rotation,
center_rel,
);
let witness_delta = nearest.point_a - nearest.point_b;
let witness_len_sq = witness_delta.length_squared();
let normal = if witness_len_sq > NORMAL_MIN_CORE_DISTANCE_SQ {
witness_delta * (1.0 / witness_len_sq.sqrt())
} else {
normal_toward_a(sweep.max_gap_axis, center_rel)
};
(nearest.distance_sq.sqrt() - margin_sum, normal)
} else {
(
-(-sweep.max_gap + margin_sum),
normal_toward_a(sweep.max_gap_axis, center_rel),
)
};
let normal_world = core_a.rotation * normal_local;
let normal_len_sq = normal_world.length_squared();
if normal_len_sq <= f32::MIN_POSITIVE {
return None;
}
let normal = normal_world * (1.0 / normal_len_sq.sqrt());
if distance < 0.0 || distance * distance < MAXIMUM_DISTANCE_SQUARED {
Some(CarToiContact { distance, normal })
} else {
None
}
}
fn sat_sweep(half_a: Vec3A, half_b: Vec3A, rotation: Mat3A, center: Vec3A) -> AxisSweep {
let mut sweep = AxisSweep {
max_gap: f32::NEG_INFINITY,
max_gap_axis: Vec3A::X,
};
let a_axes = [Vec3A::X, Vec3A::Y, Vec3A::Z];
let b_axes = [rotation.x_axis, rotation.y_axis, rotation.z_axis];
let a_half = [half_a.x, half_a.y, half_a.z];
let a_center = [center.x, center.y, center.z];
for ((axis, half), projection) in a_axes.into_iter().zip(a_half).zip(a_center) {
consider_axis(
&mut sweep,
axis,
projection,
half + projected_radius_b(half_b, b_axes, axis),
);
}
for raw_axis in b_axes {
let Some(axis) = unit_axis(raw_axis) else {
continue;
};
consider_axis(
&mut sweep,
axis,
center.dot(axis),
projected_radius_a(half_a, axis) + projected_radius_b(half_b, b_axes, axis),
);
}
for axis_a in a_axes {
for axis_b in b_axes {
let Some(axis) = unit_axis(axis_a.cross(axis_b)) else {
continue;
};
consider_axis(
&mut sweep,
axis,
center.dot(axis),
projected_radius_a(half_a, axis) + projected_radius_b(half_b, b_axes, axis),
);
}
}
sweep
}
fn consider_axis(sweep: &mut AxisSweep, axis: Vec3A, projection: f32, radius: f32) {
let gap = projection.abs() - radius;
if gap > sweep.max_gap {
sweep.max_gap = gap;
sweep.max_gap_axis = axis;
}
}
fn unit_axis(v: Vec3A) -> Option<Vec3A> {
let len_sq = v.length_squared();
if len_sq < MIN_AXIS_LEN_SQ {
return None;
}
Some(v * (1.0 / len_sq.sqrt()))
}
fn projected_radius_a(half: Vec3A, axis: Vec3A) -> f32 {
half.x * axis.x.abs() + half.y * axis.y.abs() + half.z * axis.z.abs()
}
fn projected_radius_b(half: Vec3A, b_axes: [Vec3A; 3], axis: Vec3A) -> f32 {
let x = half.x * b_axes[0].dot(axis).abs();
let y = half.y * b_axes[1].dot(axis).abs();
let z = half.z * b_axes[2].dot(axis).abs();
(x + y) + z
}
fn normal_toward_a(axis: Vec3A, center_rel: Vec3A) -> Vec3A {
if axis.dot(center_rel) > 0.0 {
-axis
} else {
axis
}
}