use glam::{Affine3A, Vec3A};
#[cfg(not(feature = "box-triangle-gjk"))]
use crate::bullet::collision::narrowphase::box_triangle_sat::box_triangle_sat;
#[cfg(feature = "box-triangle-gjk")]
use crate::bullet::collision::narrowphase::gjk::{ClosestPointInput, GjkPairDetector, GjkResult};
#[cfg(feature = "box-triangle-gjk")]
use crate::bullet::collision::shapes::collision_shape::CollisionShapes;
use crate::{
bullet::{
collision::{
narrowphase::persistent_manifold::{ContactAddedCallback, PersistentManifold},
shapes::{
bvh_triangle_mesh_shape::BvhTriangleMeshShape, compound_shape::CompoundShape,
triangle_callback::ProcessTriangle, triangle_shape::TriangleShape,
},
},
dynamics::rigid_body::{CollisionFlags, RigidBody},
linear_math::AffineExt,
},
shared::Aabb,
};
#[derive(Clone, Copy)]
struct MeshTransformFlags {
no_angular_motion: bool,
identity: bool,
}
impl MeshTransformFlags {
fn from_body(body: &RigidBody) -> Self {
let flags = body.collision_flags;
Self {
no_angular_motion: flags & CollisionFlags::NoAngularMotion != 0,
identity: flags & CollisionFlags::IdentityTransform != 0,
}
}
#[cfg(not(feature = "box-triangle-gjk"))]
const fn is_flat(self) -> bool {
self.no_angular_motion || self.identity
}
}
#[inline]
fn mesh_pair_transform(
mesh_trans: &Affine3A,
flags: MeshTransformFlags,
box_world_trans: Affine3A,
) -> Affine3A {
if flags.identity {
box_world_trans
} else if flags.no_angular_motion {
Affine3A {
matrix3: box_world_trans.matrix3,
translation: box_world_trans.translation - mesh_trans.translation,
}
} else {
let mesh_to_local = mesh_trans.transpose();
Affine3A {
matrix3: mesh_to_local.matrix3 * box_world_trans.matrix3,
translation: mesh_to_local.transform_point3a(box_world_trans.translation),
}
}
}
#[inline]
#[cfg(not(feature = "box-triangle-gjk"))]
fn mesh_to_box_transform(
box_trans: &Affine3A,
mesh_trans: &Affine3A,
flags: MeshTransformFlags,
) -> Affine3A {
let box_to_local = box_trans.transpose();
let matrix3 = if flags.is_flat() {
box_to_local.matrix3
} else {
box_to_local.matrix3 * mesh_trans.matrix3
};
Affine3A {
matrix3,
translation: box_to_local.transform_point3a(mesh_trans.translation),
}
}
#[derive(Clone, Copy, Debug)]
struct BoxTriangleContact {
point_on_b_world: Vec3A,
distance: f32,
}
#[cfg(feature = "box-triangle-gjk")]
const GJK_LOOP_MAX_DISTANCE_BT: f32 = 10.0;
#[cfg(not(feature = "box-triangle-gjk"))]
#[inline]
fn box_triangle_contact(
box_trans: &Affine3A,
half: Vec3A,
margin: f32,
q: &[Vec3A; 3],
maximum_distance: f32,
) -> Option<BoxTriangleContact> {
box_triangle_sat(box_trans, half, margin, q, maximum_distance).map(|contact| {
BoxTriangleContact {
point_on_b_world: contact.point_on_b_world,
distance: contact.distance,
}
})
}
#[cfg(feature = "box-triangle-gjk")]
fn box_triangle_contact(
box_shape: &CollisionShapes,
box_trans: &Affine3A,
triangle: &TriangleShape,
tri_trans: &Affine3A,
maximum_distance: f32,
) -> Option<BoxTriangleContact> {
let triangle_shape = CollisionShapes::Triangle(*triangle);
let margin_a = box_shape.get_margin();
let input = ClosestPointInput::new(box_trans, tri_trans, GJK_LOOP_MAX_DISTANCE_BT);
let detector = GjkPairDetector::new(margin_a, triangle_shape.get_margin());
let mut sink = GjkContactSink(None);
detector.get_closest_points(&input, box_shape, &triangle_shape, &mut sink);
sink.0.filter(|contact| contact.distance < maximum_distance)
}
#[cfg(feature = "box-triangle-gjk")]
struct GjkContactSink(Option<BoxTriangleContact>);
#[cfg(feature = "box-triangle-gjk")]
impl GjkResult for GjkContactSink {
fn add_contact_point(&mut self, _normal_on_b: Vec3A, point_on_b_world: Vec3A, depth: f32) {
self.0 = Some(BoxTriangleContact {
point_on_b_world,
distance: depth,
});
}
}
struct ConvexTriangleCallback<'a, T: ContactAddedCallback> {
manifold: Option<PersistentManifold>,
convex_obj: &'a RigidBody,
tri_obj: &'a RigidBody,
local_convex_aabb: Aabb,
#[cfg(not(feature = "box-triangle-gjk"))]
half: Vec3A,
#[cfg(not(feature = "box-triangle-gjk"))]
margin: f32,
maximum_distance: f32,
box_trans: Affine3A,
tri_trans: &'a Affine3A,
#[cfg(not(feature = "box-triangle-gjk"))]
mesh_to_box: Option<Affine3A>,
#[cfg(not(feature = "box-triangle-gjk"))]
tri_flags: MeshTransformFlags,
#[cfg(feature = "box-triangle-gjk")]
box_shape: CollisionShapes,
contact_added_callback: &'a mut T,
}
impl<T: ContactAddedCallback> ProcessTriangle for ConvexTriangleCallback<'_, T> {
fn process_triangle(&mut self, triangle: &TriangleShape, triangle_idx: usize) {
let tri_min = triangle.points[0]
.min(triangle.points[1])
.min(triangle.points[2]);
let tri_max = triangle.points[0]
.max(triangle.points[1])
.max(triangle.points[2]);
if !(tri_min.cmple(self.local_convex_aabb.max).all()
&& tri_max.cmpge(self.local_convex_aabb.min).all())
{
return;
}
#[cfg(not(feature = "box-triangle-gjk"))]
let contact = {
let mesh_to_box = match self.mesh_to_box {
Some(mesh_to_box) => mesh_to_box,
None => {
let mesh_to_box =
mesh_to_box_transform(&self.box_trans, self.tri_trans, self.tri_flags);
self.mesh_to_box = Some(mesh_to_box);
mesh_to_box
}
};
let q = [
mesh_to_box.transform_point3a(triangle.points[0]),
mesh_to_box.transform_point3a(triangle.points[1]),
mesh_to_box.transform_point3a(triangle.points[2]),
];
box_triangle_contact(
&self.box_trans,
self.half,
self.margin,
&q,
self.maximum_distance,
)
};
#[cfg(feature = "box-triangle-gjk")]
let contact = box_triangle_contact(
&self.box_shape,
&self.box_trans,
triangle,
self.tri_trans,
self.maximum_distance,
);
let Some(contact) = contact else {
return;
};
let emit_normal = self.tri_trans.matrix3 * triangle.normal;
let (convex_obj, tri_obj) = (self.convex_obj, self.tri_obj);
let manifold = self
.manifold
.get_or_insert_with(|| PersistentManifold::new(convex_obj, tri_obj));
let callback = &mut *self.contact_added_callback;
manifold.add_contact_point(
convex_obj,
tri_obj,
emit_normal,
contact.point_on_b_world,
contact.distance,
Some(triangle_idx),
callback,
);
}
}
pub(super) fn process_box_mesh_collision<T: ContactAddedCallback>(
compound_obj: &RigidBody,
compound_shape: &CompoundShape,
box_world_trans: Affine3A,
mesh_obj: &RigidBody,
tri_mesh: &BvhTriangleMeshShape,
contact_added_callback: &mut T,
) -> Option<PersistentManifold> {
let box_shape = &compound_shape.child_shape;
let mesh_trans = mesh_obj.get_world_trans();
let tri_flags = MeshTransformFlags::from_body(mesh_obj);
let box_in_mesh_space = mesh_pair_transform(mesh_trans, tri_flags, box_world_trans);
let aabb_in_mesh = box_shape.get_aabb(&box_in_mesh_space);
#[cfg(not(feature = "box-triangle-gjk"))]
let half = box_shape.get_half_extents();
let margin = box_shape.get_margin();
let breaking = compound_obj
.get_contact_breaking_threshold()
.min(mesh_obj.get_contact_breaking_threshold());
let maximum_distance = margin + breaking;
let mut callback = ConvexTriangleCallback {
manifold: None,
convex_obj: compound_obj,
tri_obj: mesh_obj,
local_convex_aabb: aabb_in_mesh,
#[cfg(not(feature = "box-triangle-gjk"))]
half,
#[cfg(not(feature = "box-triangle-gjk"))]
margin,
maximum_distance,
box_trans: box_world_trans,
tri_trans: mesh_trans,
#[cfg(not(feature = "box-triangle-gjk"))]
tri_flags,
#[cfg(not(feature = "box-triangle-gjk"))]
mesh_to_box: None,
#[cfg(feature = "box-triangle-gjk")]
box_shape: CollisionShapes::Compound(*compound_shape),
contact_added_callback,
};
tri_mesh.process_all_triangles(&mut callback, &aabb_in_mesh);
let mut manifold = callback.manifold?;
manifold.refresh_contact_points(compound_obj, mesh_obj);
if manifold.point_cache.is_empty() {
None
} else {
Some(manifold)
}
}