use crate::alloc_prelude::*;
use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet};
use crate::geometry::{Collider, ColliderHandle, ColliderSet};
use crate::math::{Pose, Real, Vector};
use crate::parry::bounding_volume::Aabb;
use crate::parry::bounding_volume::BoundingVolume;
use crate::parry::partitioning::Bvh;
use crate::pipeline::{ActiveHooks, PairFilterContext, PhysicsHooks};
#[cfg(feature = "dim2")]
use parry::query::sweep_toi::CORE_FRACTION;
use parry::query::sweep_toi::{
Sweep, SweepCompositeFastShape, SweepToiStatus, ToiProxy, sweep_time_of_impact,
sweep_time_of_impact_composite,
};
use parry::query::{NonlinearRigidMotion, QueryDispatcher};
use parry::shape::{Shape, TypedShape};
fn is_fixed_target(rb: Option<&RigidBody>) -> bool {
rb.map(|b| b.is_fixed()).unwrap_or(true)
}
pub(super) fn is_bullet(rb: &RigidBody) -> bool {
rb.is_dynamic() && rb.ccd.ccd_enabled
}
fn tier_allows(rb1: &RigidBody, rb2: Option<&RigidBody>) -> bool {
if is_bullet(rb1) {
!rb2.map(is_bullet).unwrap_or(false)
} else {
is_fixed_target(rb2)
}
}
#[inline]
#[allow(clippy::too_many_arguments)]
fn pair_filtered_out_by_hooks(
hooks: &dyn PhysicsHooks,
bodies: &RigidBodySet,
colliders: &ColliderSet,
co1: &Collider,
co2: &Collider,
ch1: ColliderHandle,
ch2: ColliderHandle,
bh1: RigidBodyHandle,
bh2: Option<RigidBodyHandle>,
) -> bool {
let active_hooks = co1.flags.active_hooks | co2.flags.active_hooks;
if !active_hooks.contains(ActiveHooks::FILTER_CONTACT_PAIRS) {
return false;
}
let context = PairFilterContext {
bodies,
colliders,
rigid_body1: Some(bh1),
rigid_body2: bh2,
collider1: ch1,
collider2: ch2,
};
hooks.filter_contact_pair(&context).is_none()
}
fn is_composite_shape(shape: &dyn Shape) -> bool {
matches!(
shape.as_typed_shape(),
TypedShape::TriMesh(_)
| TypedShape::Polyline(_)
| TypedShape::HeightField(_)
| TypedShape::Compound(_)
)
}
pub(crate) fn shape_never_ccd_swept(shape: &dyn Shape) -> bool {
matches!(
shape.as_typed_shape(),
TypedShape::TriMesh(_)
| TypedShape::Polyline(_)
| TypedShape::HeightField(_)
| TypedShape::Voxels(_)
)
}
fn target_collider_pose(co: &Collider, rb: Option<&RigidBody>) -> Pose {
match (rb, co.parent.as_ref()) {
(Some(rb), Some(parent)) => rb.pos.next_position * parent.pos_wrt_parent,
_ => co.pos.0,
}
}
fn intersect_swept_aabb<'a>(
bvh: &'a Bvh,
colliders: &'a ColliderSet,
aabb: Aabb,
) -> impl Iterator<Item = (ColliderHandle, &'a Collider)> + 'a {
bvh.leaves(move |node| node.aabb().intersects(&aabb))
.filter_map(move |leaf| {
let (co, ch) = colliders.get_unknown_gen(leaf)?;
Some((ch, co))
})
}
#[derive(Copy, Clone)]
pub(super) enum CcdTargets<'a> {
FixedList(&'a [(ColliderHandle, Aabb)]),
FullBvh(&'a Bvh),
}
const FIXED_TARGETS_LIST_MAX: usize = 512;
pub(super) fn collect_fixed_targets(
bodies: &RigidBodySet,
colliders: &ColliderSet,
prediction_distance: Real,
) -> Option<Vec<(ColliderHandle, Aabb)>> {
let mut fixed = Vec::new();
for (ch, co) in colliders.iter_enabled() {
let rb = co.parent.and_then(|p| bodies.get(p.handle));
if is_fixed_target(rb) {
if fixed.len() == FIXED_TARGETS_LIST_MAX {
return None;
}
let aabb = co.shape.compute_aabb(&co.pos).loosened(prediction_distance);
fixed.push((ch, aabb));
}
}
Some(fixed)
}
struct FastSubShape<'a> {
proxy: ToiProxy<'a>,
sweep: Sweep,
local_centroid: Vector,
min_extent: Real,
}
#[allow(clippy::large_enum_variant)]
enum FastShapeKind<'a> {
Convex(FastSubShape<'a>),
Compound(Vec<FastSubShape<'a>>),
Nonlinear,
}
struct FastColliderInfo<'a> {
collider: &'a Collider,
body: &'a RigidBody,
kind: FastShapeKind<'a>,
}
impl<'a> FastColliderInfo<'a> {
fn new(
collider: &'a Collider,
body: &'a RigidBody,
start: &Pose,
end: &Pose,
local_com: Vector,
) -> Option<Self> {
let shape = collider.shape.as_ref();
if shape_never_ccd_swept(shape) {
return None;
}
let kind = if let Some(proxy) = ToiProxy::from_shape(shape) {
FastShapeKind::Convex(FastSubShape {
proxy,
sweep: Sweep::from_poses(start, end, local_com),
local_centroid: shape.mass_properties(1.0).local_com,
min_extent: shape.ccd_thickness(),
})
} else if let TypedShape::Compound(compound) = shape.as_typed_shape() {
let children: Vec<_> = compound
.shapes()
.iter()
.filter_map(|(child_pose, child_shape)| {
let proxy = ToiProxy::from_shape(child_shape.as_ref())?;
Some(FastSubShape {
proxy,
sweep: Sweep::from_poses(
&(*start * *child_pose),
&(*end * *child_pose),
child_pose.inverse_transform_point(local_com),
),
local_centroid: child_shape.mass_properties(1.0).local_com,
min_extent: child_shape.ccd_thickness(),
})
})
.collect();
if children.is_empty() {
return None;
}
FastShapeKind::Compound(children)
} else {
FastShapeKind::Nonlinear
};
Some(Self {
collider,
body,
kind,
})
}
}
#[allow(clippy::large_enum_variant)]
enum TargetKind<'a> {
Proxy(ToiProxy<'a>),
Composite,
}
pub(super) struct PseudoHit {
pub(super) ch1: ColliderHandle,
pub(super) ch2: ColliderHandle,
pub(super) fraction: Real,
}
pub(super) struct BodyContinuousResult {
pub(super) handle: RigidBodyHandle,
pub(super) fraction: Real,
pub(super) pseudo_hits: Vec<PseudoHit>,
}
#[allow(clippy::too_many_arguments)]
fn cast_collider_pair(
dispatcher: &dyn QueryDispatcher,
fast: &FastColliderInfo,
co2: &Collider,
rb2: Option<&RigidBody>,
max_fraction: Real,
dt: Real,
linear_slop: Real,
is_pseudo: bool,
) -> Option<Real> {
let target_pose = target_collider_pose(co2, rb2);
let sub_shapes: &[FastSubShape] = match &fast.kind {
FastShapeKind::Convex(sub) => core::slice::from_ref(sub),
FastShapeKind::Compound(children) => children,
FastShapeKind::Nonlinear => {
return fallback_nonlinear_fraction(
dispatcher,
fast,
co2,
&target_pose,
max_fraction,
dt,
is_pseudo,
);
}
};
let shape2 = co2.shape.as_ref();
let target = match ToiProxy::from_shape(shape2) {
Some(proxy) => TargetKind::Proxy(proxy),
None if is_composite_shape(shape2) => TargetKind::Composite,
None => {
return fallback_nonlinear_fraction(
dispatcher,
fast,
co2,
&target_pose,
max_fraction,
dt,
is_pseudo,
);
}
};
let mut best = None;
let mut max_fraction = max_fraction;
for sub in sub_shapes {
if let Some(fraction) = cast_sub_shape(
sub,
&target,
shape2,
&target_pose,
max_fraction,
linear_slop,
is_pseudo,
) {
best = Some(fraction);
max_fraction = fraction;
}
}
best
}
fn cast_sub_shape(
sub: &FastSubShape,
target: &TargetKind,
shape2: &dyn Shape,
target_pose: &Pose,
max_fraction: Real,
linear_slop: Real,
is_pseudo: bool,
) -> Option<Real> {
let output = match target {
TargetKind::Proxy(target_proxy) => {
let target_sweep = Sweep::constant(target_pose, Vector::ZERO);
sweep_time_of_impact(
target_proxy,
&target_sweep,
&sub.proxy,
&sub.sweep,
max_fraction,
linear_slop,
)
}
TargetKind::Composite => {
#[cfg(feature = "dim2")]
let one_sided = false;
#[cfg(feature = "dim3")]
let one_sided = matches!(shape2.as_typed_shape(), TypedShape::HeightField(_));
let fast_desc = SweepCompositeFastShape {
proxy: &sub.proxy,
sweep: &sub.sweep,
local_centroid: sub.local_centroid,
min_extent: sub.min_extent,
};
sweep_time_of_impact_composite(
shape2,
target_pose,
fast_desc,
one_sided,
is_pseudo,
max_fraction,
linear_slop,
)?
}
};
if is_pseudo {
return match output.status {
SweepToiStatus::Hit | SweepToiStatus::Failed | SweepToiStatus::Overlapped
if output.fraction <= max_fraction =>
{
Some(output.fraction)
}
_ => None,
};
}
if 0.0 < output.fraction && output.fraction < max_fraction {
return Some(output.fraction);
}
#[cfg(feature = "dim2")]
if output.fraction == 0.0 {
if let TargetKind::Proxy(target_proxy) = target {
let core = ToiProxy::point(sub.local_centroid, CORE_FRACTION * sub.min_extent);
let target_sweep = Sweep::constant(target_pose, Vector::ZERO);
let output = sweep_time_of_impact(
target_proxy,
&target_sweep,
&core,
&sub.sweep,
max_fraction,
linear_slop,
);
if 0.0 < output.fraction && output.fraction < max_fraction {
return Some(output.fraction);
}
}
}
None
}
fn fallback_nonlinear_fraction(
dispatcher: &dyn QueryDispatcher,
fast: &FastColliderInfo,
co2: &Collider,
target_pose: &Pose,
max_fraction: Real,
dt: Real,
is_pseudo: bool,
) -> Option<Real> {
if dt == 0.0 {
return None;
}
let rb1 = fast.body;
let parent1 = fast.collider.parent.as_ref()?;
let motion1 = NonlinearRigidMotion::new(
rb1.pos.position,
rb1.mprops.local_mprops.local_com,
rb1.ccd_vels.linvel,
rb1.ccd_vels.angvel,
)
.prepend(parent1.pos_wrt_parent);
let motion2 = NonlinearRigidMotion::constant_position(*target_pose);
let hit = dispatcher
.cast_shapes_nonlinear(
&motion1,
fast.collider.shape.as_ref(),
&motion2,
co2.shape.as_ref(),
0.0,
dt,
is_pseudo, )
.ok()??;
let fraction = hit.time_of_impact / dt;
if is_pseudo {
(fraction <= max_fraction).then_some(fraction)
} else {
(0.0 < fraction && fraction < max_fraction).then_some(fraction)
}
}
#[derive(Copy, Clone, PartialEq, Eq)]
pub(super) enum PseudoHitMode {
Record,
Ignore,
}
#[allow(clippy::too_many_arguments)]
pub(super) fn sweep_fast_body(
handle: RigidBodyHandle,
bodies: &RigidBodySet,
colliders: &ColliderSet,
end_body_pose: Pose,
targets: CcdTargets,
dispatcher: &dyn QueryDispatcher,
hooks: &dyn PhysicsHooks,
dt: Real,
linear_slop: Real,
pseudo_mode: PseudoHitMode,
) -> BodyContinuousResult {
let rb1 = &bodies[handle];
let mut fraction: Real = 1.0;
let mut pseudo_hits = Vec::new();
for ch1 in &rb1.colliders.0 {
let co1 = &colliders[*ch1];
let Some(parent1) = co1.parent.as_ref() else {
continue;
};
if pseudo_mode == PseudoHitMode::Ignore && co1.is_sensor() {
continue; }
let start = rb1.pos.position * parent1.pos_wrt_parent;
let end = end_body_pose * parent1.pos_wrt_parent;
let local_com = parent1
.pos_wrt_parent
.inverse_transform_point(rb1.mprops.local_mprops.local_com);
let Some(fast) = FastColliderInfo::new(co1, rb1, &start, &end, local_com) else {
continue;
};
let start_aabb = co1.shape.compute_aabb(&start);
let end_aabb = co1.shape.compute_aabb(&end);
let swept_aabb = start_aabb.merged(&end_aabb);
let mut handle_candidate = |ch2: ColliderHandle, co2: &Collider| {
if ch2 == *ch1 {
return;
}
let bh2 = co2.parent.map(|p| p.handle);
if bh2 == Some(handle) {
return; }
let rb2 = bh2.and_then(|h| bodies.get(h));
if !tier_allows(rb1, rb2) {
return;
}
if !co1.flags.collision_groups.test(co2.flags.collision_groups) {
return;
}
let is_pseudo = co1.is_sensor()
|| co2.is_sensor()
|| !co1.flags.solver_groups.test(co2.flags.solver_groups);
if is_pseudo && pseudo_mode == PseudoHitMode::Ignore {
return;
}
if pair_filtered_out_by_hooks(
hooks, bodies, colliders, co1, co2, *ch1, ch2, handle, bh2,
) {
return;
}
if let Some(hit_fraction) = cast_collider_pair(
dispatcher,
&fast,
co2,
rb2,
fraction,
dt,
linear_slop,
is_pseudo,
) {
if is_pseudo {
pseudo_hits.push(PseudoHit {
ch1: *ch1,
ch2,
fraction: hit_fraction,
});
} else {
fraction = hit_fraction;
}
}
};
match targets {
CcdTargets::FixedList(fixed) => {
for (ch2, aabb2) in fixed {
if aabb2.intersects(&swept_aabb) {
handle_candidate(*ch2, &colliders[*ch2]);
}
}
}
CcdTargets::FullBvh(bvh) => {
for (ch2, co2) in intersect_swept_aabb(bvh, colliders, swept_aabb) {
handle_candidate(ch2, co2);
}
}
}
}
BodyContinuousResult {
handle,
fraction,
pseudo_hits,
}
}
#[cfg(all(feature = "parallel", feature = "unsync-callbacks"))]
#[derive(Default)]
struct HookProbe(core::sync::atomic::AtomicBool);
#[cfg(all(feature = "parallel", feature = "unsync-callbacks"))]
impl PhysicsHooks for HookProbe {
fn filter_contact_pair(
&self,
_: &crate::pipeline::PairFilterContext,
) -> Option<crate::geometry::SolverFlags> {
self.0.store(true, core::sync::atomic::Ordering::Relaxed);
Some(crate::geometry::SolverFlags::COMPUTE_IMPULSES)
}
}
#[cfg(feature = "parallel")]
pub(super) fn map_bodies_parallel<T: Send>(
handles: &[RigidBodyHandle],
hooks: &dyn PhysicsHooks,
f: impl Fn(RigidBodyHandle, &dyn PhysicsHooks) -> T + Sync + Send,
) -> Vec<T> {
let map_with = |hooks: &(dyn PhysicsHooks + Sync)| {
if handles.len() >= 64 {
use rayon::prelude::*;
return handles.par_iter().map(|h| f(*h, hooks)).collect();
}
handles.iter().map(|h| f(*h, hooks)).collect()
};
#[cfg(not(feature = "unsync-callbacks"))]
return map_with(hooks);
#[cfg(feature = "unsync-callbacks")]
{
let probe = HookProbe::default();
let swept = map_with(&probe);
if !probe.0.load(core::sync::atomic::Ordering::Relaxed) {
return swept;
}
drop(swept);
handles.iter().map(|h| f(*h, hooks)).collect()
}
}
#[cfg(not(feature = "parallel"))]
pub(super) fn map_bodies_parallel<T>(
handles: &[RigidBodyHandle],
hooks: &dyn PhysicsHooks,
f: impl Fn(RigidBodyHandle, &dyn PhysicsHooks) -> T,
) -> Vec<T> {
handles.iter().map(|h| f(*h, hooks)).collect()
}