use std::fmt::{self, Display, Formatter};
use arrayvec::ArrayVec;
use glam::{Affine3A, Mat3A, Vec3A};
use crate::{
bullet::{
collision::narrowphase::{
manifold_point::ManifoldPoint,
persistent_manifold::{MANIFOLD_CACHE_SIZE, PersistentManifold, pair_key},
},
dynamics::rigid_body::CollisionFlags,
},
shared::Aabb,
sim::{Arena, UserInfoType},
};
pub const MAX_CONTACT_POINTS: usize = MANIFOLD_CACHE_SIZE;
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct BodyId(usize);
impl BodyId {
#[inline]
#[must_use]
pub const fn index(self) -> usize {
self.0
}
}
#[derive(Clone, Copy, Debug)]
pub struct BodyInfo {
pub kind: UserInfoType,
pub world_transform: Affine3A,
pub aabb: Aabb,
pub contact_breaking_threshold: f32,
pub contact_processing_threshold: f32,
pub collision_order: usize,
flags: u8,
}
impl BodyInfo {
#[inline]
#[must_use]
pub fn world_point(&self, local_point: Vec3A) -> Vec3A {
if self.flags & CollisionFlags::IdentityTransform != 0 {
local_point
} else if self.flags & CollisionFlags::NoAngularMotion != 0 {
local_point + self.world_transform.translation
} else {
self.world_transform.transform_point3a(local_point)
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct BodyState {
pub inv_inertia_tensor_world: Mat3A,
pub interp_world_basis: Mat3A,
pub linear_sleeping_threshold: f32,
pub angular_sleeping_threshold: f32,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ContactPoint {
pub local_point_a: Vec3A,
pub local_point_b: Vec3A,
pub pos_world_on_a: Vec3A,
pub pos_world_on_b: Vec3A,
pub normal_world_on_b: Vec3A,
pub distance: f32,
pub combined_friction: f32,
pub combined_restitution: f32,
pub applied_impulse: f32,
pub is_special: bool,
}
#[derive(Clone, Debug, PartialEq)]
pub struct ContactManifold {
pub body0: BodyId,
pub body1: BodyId,
pub contact_breaking_threshold: f32,
pub contact_processing_threshold: f32,
pub points: Vec<ContactPoint>,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum StateError {
UnknownBody {
body: usize,
},
UnknownManifoldBody {
manifold: usize,
body: usize,
},
SameBodyPair {
manifold: usize,
},
DuplicateBodyPair {
manifold: usize,
},
TooManyPoints {
manifold: usize,
points: usize,
},
NonFinitePoint {
manifold: usize,
point: usize,
},
NormalNotUnit {
manifold: usize,
point: usize,
},
NegativeAppliedImpulse {
manifold: usize,
point: usize,
},
InvalidBreakingThreshold {
manifold: usize,
},
InvalidProcessingThreshold {
manifold: usize,
},
NonFiniteBodyState {
field: &'static str,
},
NegativeSleepingThreshold {
field: &'static str,
},
}
impl Display for StateError {
fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
match self {
Self::UnknownBody { body } => {
write!(f, "body id {body} is not in this arena")
}
Self::UnknownManifoldBody { manifold, body } => write!(
f,
"manifold {manifold} refers to body id {body}, which is not in this arena"
),
Self::SameBodyPair { manifold } => {
write!(f, "manifold {manifold} uses the same body twice")
}
Self::DuplicateBodyPair { manifold } => {
write!(f, "manifold {manifold} repeats an earlier body pair")
}
Self::TooManyPoints { manifold, points } => {
write!(
f,
"manifold {manifold} has {points} points; the limit is {MANIFOLD_CACHE_SIZE}"
)
}
Self::NonFinitePoint { manifold, point } => {
write!(f, "manifold {manifold} point {point} is not finite")
}
Self::NormalNotUnit { manifold, point } => {
write!(
f,
"manifold {manifold} point {point} normal is not a unit vector"
)
}
Self::NegativeAppliedImpulse { manifold, point } => write!(
f,
"manifold {manifold} point {point} applied impulse is negative"
),
Self::InvalidBreakingThreshold { manifold } => write!(
f,
"manifold {manifold} contact breaking threshold is not finite and nonnegative"
),
Self::InvalidProcessingThreshold { manifold } => write!(
f,
"manifold {manifold} contact processing threshold is not finite and nonnegative"
),
Self::NonFiniteBodyState { field } => {
write!(f, "body state field {field} is not finite")
}
Self::NegativeSleepingThreshold { field } => {
write!(f, "body state field {field} is negative")
}
}
}
}
impl std::error::Error for StateError {}
impl Arena {
pub fn physics_body_ids(&self) -> impl ExactSizeIterator<Item = BodyId> + '_ {
(0..self.bullet_world.bodies().len()).map(BodyId)
}
#[must_use]
pub fn ball_body_id(&self) -> BodyId {
BodyId(self.ball.rigid_body_idx)
}
#[must_use]
pub fn car_body_id(&self, car_idx: usize) -> Option<BodyId> {
self.cars.get(car_idx).map(|car| BodyId(car.rigid_body_idx))
}
pub fn body_info(&self, body: BodyId) -> Result<BodyInfo, StateError> {
let index = body.index();
let rigid_body = self
.bullet_world
.bodies()
.get(index)
.ok_or(StateError::UnknownBody { body: index })?;
Ok(BodyInfo {
kind: rigid_body.user_idx,
world_transform: *rigid_body.get_world_trans(),
aabb: rigid_body
.get_collision_shape()
.get_aabb(rigid_body.get_world_trans()),
contact_breaking_threshold: rigid_body.get_contact_breaking_threshold(),
contact_processing_threshold: rigid_body.contact_processing_threshold,
collision_order: rigid_body.get_broadphase_handle(),
flags: rigid_body.collision_flags,
})
}
pub fn body_state(&self, body: BodyId) -> Result<BodyState, StateError> {
let index = body.index();
let rigid_body = self
.bullet_world
.bodies()
.get(index)
.ok_or(StateError::UnknownBody { body: index })?;
Ok(BodyState {
inv_inertia_tensor_world: rigid_body.inv_inertia_tensor_world,
interp_world_basis: rigid_body.interp_world_trans.matrix3,
linear_sleeping_threshold: rigid_body.linear_sleeping_threshold,
angular_sleeping_threshold: rigid_body.angular_sleeping_threshold,
})
}
pub fn set_body_state(&mut self, body: BodyId, state: BodyState) -> Result<(), StateError> {
validate_body_state(&state)?;
let index = body.index();
let rigid_body = self
.bullet_world
.bodies_mut()
.get_mut(index)
.ok_or(StateError::UnknownBody { body: index })?;
rigid_body.inv_inertia_tensor_world = state.inv_inertia_tensor_world;
rigid_body.interp_world_trans.matrix3 = state.interp_world_basis;
rigid_body.linear_sleeping_threshold = state.linear_sleeping_threshold;
rigid_body.angular_sleeping_threshold = state.angular_sleeping_threshold;
Ok(())
}
pub fn contact_manifolds(&self) -> impl Iterator<Item = ContactManifold> + '_ {
self.bullet_world
.persistent_manifolds()
.iter()
.map(|manifold| ContactManifold {
body0: BodyId(manifold.body0_idx),
body1: BodyId(manifold.body1_idx),
contact_breaking_threshold: manifold.contact_breaking_threshold,
contact_processing_threshold: manifold.contact_processing_threshold,
points: manifold
.point_cache
.iter()
.map(|point| ContactPoint {
local_point_a: point.local_point_a,
local_point_b: point.local_point_b,
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,
distance: point.distance_1,
combined_friction: point.combined_friction,
combined_restitution: point.combined_restitution,
applied_impulse: point.applied_impulse,
is_special: point.is_special,
})
.collect(),
})
}
pub fn set_contact_manifolds(
&mut self,
manifolds: &[ContactManifold],
) -> Result<(), StateError> {
validate_contact_manifolds(manifolds, self.bullet_world.bodies().len())?;
let dispatcher = self.bullet_world.collision_dispatcher_mut();
debug_assert!(
dispatcher.active_manifolds.is_empty(),
"set_contact_manifolds must run between steps"
);
for manifold in manifolds {
let body0 = manifold.body0.index();
let body1 = manifold.body1.index();
let pair = ordered_pair(body0, body1);
let existing_index = dispatcher
.persistent_manifolds
.iter()
.position(|stored| ordered_pair(stored.body0_idx, stored.body1_idx) == pair);
if let Some(index) = existing_index {
let stored = &mut dispatcher.persistent_manifolds[index];
let reversed = stored.body0_idx != body0;
stored.contact_breaking_threshold = manifold.contact_breaking_threshold;
stored.contact_processing_threshold = manifold.contact_processing_threshold;
stored.point_cache.clear();
for point in &manifold.points {
stored
.point_cache
.push(contact_point_to_bullet(point, reversed));
}
} else {
let key = pair_key(body0, body1);
let mut point_cache: ArrayVec<ManifoldPoint, MANIFOLD_CACHE_SIZE> = ArrayVec::new();
for point in &manifold.points {
point_cache.push(contact_point_to_bullet(point, false));
}
let stored = PersistentManifold {
point_cache,
body0_idx: body0,
body1_idx: body1,
pair_key: key,
contact_breaking_threshold: manifold.contact_breaking_threshold,
contact_processing_threshold: manifold.contact_processing_threshold,
};
dispatcher.insert_persistent_manifold(key, stored);
}
}
Ok(())
}
}
#[inline]
fn ordered_pair(body0: usize, body1: usize) -> (usize, usize) {
(body0.min(body1), body0.max(body1))
}
fn contact_point_to_bullet(point: &ContactPoint, reversed: bool) -> ManifoldPoint {
let (local_point_a, local_point_b, pos_world_on_a, pos_world_on_b, normal_world_on_b) =
if reversed {
(
point.local_point_b,
point.local_point_a,
point.pos_world_on_b,
point.pos_world_on_a,
-point.normal_world_on_b,
)
} else {
(
point.local_point_a,
point.local_point_b,
point.pos_world_on_a,
point.pos_world_on_b,
point.normal_world_on_b,
)
};
ManifoldPoint {
local_point_a,
local_point_b,
pos_world_on_a,
pos_world_on_b,
normal_world_on_b,
distance_1: point.distance,
combined_friction: point.combined_friction,
combined_restitution: point.combined_restitution,
applied_impulse: point.applied_impulse,
is_special: point.is_special,
}
}
fn validate_body_state(state: &BodyState) -> Result<(), StateError> {
if !state.inv_inertia_tensor_world.is_finite() {
return Err(StateError::NonFiniteBodyState {
field: "inv_inertia_tensor_world",
});
}
if !state.interp_world_basis.is_finite() {
return Err(StateError::NonFiniteBodyState {
field: "interp_world_basis",
});
}
if !state.linear_sleeping_threshold.is_finite() {
return Err(StateError::NonFiniteBodyState {
field: "linear_sleeping_threshold",
});
}
if state.linear_sleeping_threshold < 0.0 {
return Err(StateError::NegativeSleepingThreshold {
field: "linear_sleeping_threshold",
});
}
if !state.angular_sleeping_threshold.is_finite() {
return Err(StateError::NonFiniteBodyState {
field: "angular_sleeping_threshold",
});
}
if state.angular_sleeping_threshold < 0.0 {
return Err(StateError::NegativeSleepingThreshold {
field: "angular_sleeping_threshold",
});
}
Ok(())
}
fn validate_contact_manifolds(
manifolds: &[ContactManifold],
body_count: usize,
) -> Result<(), StateError> {
let mut pairs = Vec::with_capacity(manifolds.len());
for (manifold_index, manifold) in manifolds.iter().enumerate() {
let body0 = manifold.body0.index();
let body1 = manifold.body1.index();
if body0 >= body_count {
return Err(StateError::UnknownManifoldBody {
manifold: manifold_index,
body: body0,
});
}
if body1 >= body_count {
return Err(StateError::UnknownManifoldBody {
manifold: manifold_index,
body: body1,
});
}
if body0 == body1 {
return Err(StateError::SameBodyPair {
manifold: manifold_index,
});
}
let pair = ordered_pair(body0, body1);
if pairs.contains(&pair) {
return Err(StateError::DuplicateBodyPair {
manifold: manifold_index,
});
}
pairs.push(pair);
if !manifold.contact_breaking_threshold.is_finite()
|| manifold.contact_breaking_threshold < 0.0
{
return Err(StateError::InvalidBreakingThreshold {
manifold: manifold_index,
});
}
if !manifold.contact_processing_threshold.is_finite()
|| manifold.contact_processing_threshold < 0.0
{
return Err(StateError::InvalidProcessingThreshold {
manifold: manifold_index,
});
}
if manifold.points.len() > MANIFOLD_CACHE_SIZE {
return Err(StateError::TooManyPoints {
manifold: manifold_index,
points: manifold.points.len(),
});
}
for (point_index, point) in manifold.points.iter().enumerate() {
validate_contact_point(point, manifold_index, point_index)?;
}
}
Ok(())
}
fn validate_contact_point(
point: &ContactPoint,
manifold: usize,
index: usize,
) -> Result<(), StateError> {
let is_finite = point.local_point_a.is_finite()
&& point.local_point_b.is_finite()
&& point.pos_world_on_a.is_finite()
&& point.pos_world_on_b.is_finite()
&& point.normal_world_on_b.is_finite()
&& point.distance.is_finite()
&& point.combined_friction.is_finite()
&& point.combined_restitution.is_finite()
&& point.applied_impulse.is_finite();
if !is_finite {
return Err(StateError::NonFinitePoint {
manifold,
point: index,
});
}
if !point.normal_world_on_b.is_normalized() {
return Err(StateError::NormalNotUnit {
manifold,
point: index,
});
}
if point.applied_impulse < 0.0 {
return Err(StateError::NegativeAppliedImpulse {
manifold,
point: index,
});
}
Ok(())
}