use std::mem;
use crate::{
bullet::{
collision::{
dispatch::internal_edge_utility::adjust_internal_edge_contacts,
narrowphase::{
manifold_point::ManifoldPoint,
persistent_manifold::{ContactAddedCallback, PersistentManifold},
},
},
dynamics::rigid_body::RigidBody,
},
consts,
sim::UserInfoType,
};
#[derive(Debug, Default, Copy, Clone)]
pub(crate) struct ContactGeometry {
pub pos_world_on_a: glam::Vec3A,
pub pos_world_on_b: glam::Vec3A,
pub normal_world_on_b: glam::Vec3A,
}
impl ContactGeometry {
fn from_point(point: &ManifoldPoint) -> Self {
Self {
pos_world_on_a: point.pos_world_on_a,
pos_world_on_b: point.pos_world_on_b,
normal_world_on_b: point.normal_world_on_b,
}
}
}
#[derive(Debug, Copy, Clone)]
pub(crate) struct ContactRecord {
pub is_swap: bool,
pub rb_idx_a: usize,
pub rb_idx_b: usize,
pub geometry: ContactGeometry,
}
#[derive(Debug, Copy, Clone)]
pub(crate) struct BallWorldContactRecord {
pub rb_idx: usize,
pub contact_point: glam::Vec3A,
pub contact_normal: glam::Vec3A,
}
#[derive(Debug, Copy, Clone)]
pub(crate) struct WorldContactNormalGroup {
pub car_idx: usize,
pub object_idx: usize,
pub sum: glam::Vec3A,
}
#[derive(Clone)]
pub(crate) struct ArenaContactTracker {
collision_records: Vec<ContactRecord>,
ball_world_records: Vec<BallWorldContactRecord>,
world_normal_groups: Vec<WorldContactNormalGroup>,
ball_only: bool,
}
impl ArenaContactTracker {
pub fn new() -> Self {
Self {
collision_records: Vec::with_capacity(4), ball_world_records: Vec::with_capacity(4),
world_normal_groups: Vec::with_capacity(4),
ball_only: false,
}
}
pub(super) fn new_empty() -> Self {
Self {
collision_records: Vec::new(),
ball_world_records: Vec::new(),
world_normal_groups: Vec::new(),
ball_only: false,
}
}
pub fn set_ball_only(&mut self, ball_only: bool) {
self.ball_only = ball_only;
}
pub const fn num_records(&self) -> usize {
self.collision_records.len()
}
pub fn get_record(&self, idx: usize) -> &ContactRecord {
&self.collision_records[idx]
}
pub const fn num_ball_world_records(&self) -> usize {
self.ball_world_records.len()
}
pub fn get_ball_world_record(&self, idx: usize) -> &BallWorldContactRecord {
&self.ball_world_records[idx]
}
pub fn clear_records(&mut self) {
self.collision_records.clear();
self.ball_world_records.clear();
self.world_normal_groups.clear();
}
pub(crate) fn reset_transient(&mut self) {
self.clear_records();
self.ball_only = false;
}
pub fn add_world_contact_normal(
&mut self,
car_idx: usize,
object_idx: usize,
normal: glam::Vec3A,
) {
if let Some(group) = self
.world_normal_groups
.iter_mut()
.find(|group| group.car_idx == car_idx && group.object_idx == object_idx)
{
group.sum += normal;
} else {
self.world_normal_groups.push(WorldContactNormalGroup {
car_idx,
object_idx,
sum: normal,
});
}
}
pub fn world_contact_normal_groups(&self) -> &[WorldContactNormalGroup] {
&self.world_normal_groups
}
}
impl ContactAddedCallback for ArenaContactTracker {
fn callback<'a>(
&mut self,
manifold_point: &mut ManifoldPoint,
mut body_a: &'a RigidBody,
mut body_b: &'a RigidBody,
idx: Option<usize>,
) {
debug_assert!(body_a.has_contact_response() || body_b.has_contact_response());
let should_swap =
if body_a.user_idx != UserInfoType::None && body_b.user_idx != UserInfoType::None {
body_a.user_idx > body_b.user_idx
} else {
body_b.user_idx != UserInfoType::None
};
if should_swap {
mem::swap(&mut body_a, &mut body_b);
}
let user_idx_a = body_a.user_idx;
let user_idx_b = body_b.user_idx;
if user_idx_a == UserInfoType::Car {
let hit_coefs = match user_idx_b {
UserInfoType::Ball => consts::car::HIT_BALL_COEFS,
UserInfoType::Car => consts::car::HIT_CAR_COEFS,
_ => consts::car::HIT_WORLD_COEFS,
};
manifold_point.combined_friction = hit_coefs.friction;
manifold_point.combined_restitution = hit_coefs.restitution;
} else if user_idx_a == UserInfoType::Ball
&& user_idx_b == UserInfoType::None
&& body_b.is_static_obj()
{
manifold_point.is_special = true;
}
if self.ball_only
&& user_idx_a == UserInfoType::Ball
&& user_idx_b == UserInfoType::None
&& body_b.is_static_obj()
{
self.ball_world_records.push(BallWorldContactRecord {
rb_idx: body_a.world_array_idx,
contact_point: manifold_point.pos_world_on_b,
contact_normal: manifold_point.normal_world_on_b,
});
} else {
self.collision_records.push(ContactRecord {
is_swap: should_swap,
rb_idx_a: body_a.world_array_idx,
rb_idx_b: body_b.world_array_idx,
geometry: ContactGeometry::from_point(manifold_point),
});
}
if let Some(idx) = idx {
adjust_internal_edge_contacts(manifold_point, body_b, idx);
}
}
fn contact_processed<'a>(
&mut self,
manifold_point: &ManifoldPoint,
mut body_a: &'a RigidBody,
mut body_b: &'a RigidBody,
) {
let should_swap =
if body_a.user_idx != UserInfoType::None && body_b.user_idx != UserInfoType::None {
body_a.user_idx > body_b.user_idx
} else {
body_b.user_idx != UserInfoType::None
};
if should_swap {
mem::swap(&mut body_a, &mut body_b);
}
if body_a.user_idx != UserInfoType::Car
|| body_b.user_idx == UserInfoType::Ball
|| body_b.user_idx == UserInfoType::Car
{
return;
}
self.collision_records.push(ContactRecord {
is_swap: should_swap,
rb_idx_a: body_a.world_array_idx,
rb_idx_b: body_b.world_array_idx,
geometry: ContactGeometry::from_point(manifold_point),
});
}
fn manifold_processed<'a>(
&mut self,
manifold: &PersistentManifold,
mut body_a: &'a RigidBody,
mut body_b: &'a RigidBody,
) {
let should_swap =
if body_a.user_idx != UserInfoType::None && body_b.user_idx != UserInfoType::None {
body_a.user_idx > body_b.user_idx
} else {
body_b.user_idx != UserInfoType::None
};
if should_swap {
mem::swap(&mut body_a, &mut body_b);
}
if body_a.user_idx != UserInfoType::Car
|| body_b.user_idx == UserInfoType::Ball
|| body_b.user_idx == UserInfoType::Car
{
return;
}
for point in manifold.point_cache.iter() {
self.add_world_contact_normal(
body_a.user_pointer,
body_b.world_array_idx,
point.normal_world_on_b,
);
}
}
}