diff --git a/CHANGELOG.md b/CHANGELOG.md index df360f07b..c2b91d119 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,12 @@ ### Added +- `ContactForceEvent::first_tick`: `true` on the step a pair's total contact force first + exceeds its `contact_force_event_threshold` (coming from below it, or from separation), + `false` while it stays above on consecutive steps — the analogue of PhysX's + "threshold force found" vs "persists" report. The status resets when the force drops + back below the threshold or the colliders separate. + - `CoefficientCombineRule::GeometricMean`: combines friction/restitution as `sqrt(c1 * c2)`, the convention used by several other engines. It has the highest rule priority, and clamps negative coefficients to zero before the square root. diff --git a/crates/rapier3d/tests/contact_force_event_first_tick.rs b/crates/rapier3d/tests/contact_force_event_first_tick.rs new file mode 100644 index 000000000..c8637a5dd --- /dev/null +++ b/crates/rapier3d/tests/contact_force_event_first_tick.rs @@ -0,0 +1,168 @@ +//! `ContactForceEvent::started` must be `true` exactly on the steps where the pair's +//! total force crosses its threshold coming from below (or from separation), and `false` +//! while the force stays above on consecutive steps — the analogue of PhysX's +//! `eNOTIFY_THRESHOLD_FORCE_FOUND` vs `_PERSISTS`. In particular it is about the *force* +//! threshold, not contact newness: a pair can rest gently (no events at all) long after +//! its `CollisionEvent::Started`. + +use std::sync::Mutex; + +use rapier3d::pipeline::{ActiveEvents, EventHandler, PhysicsWorld}; +use rapier3d::prelude::*; + +#[derive(Default)] +struct Events { + started_steps: Mutex>, + force_events: Mutex>, + step: Mutex, +} + +impl EventHandler for Events { + fn handle_collision_event( + &self, + _: &RigidBodySet, + _: &ColliderSet, + event: CollisionEvent, + _: Option<&ContactPair>, + ) { + if matches!(event, CollisionEvent::Started(..)) { + self.started_steps + .lock() + .unwrap() + .push(*self.step.lock().unwrap()); + } + } + + fn handle_contact_force_event( + &self, + dt: Real, + _: &RigidBodySet, + _: &ColliderSet, + contact_pair: &ContactPair, + total_force_magnitude: Real, + ) { + let event = ContactForceEvent::from_contact_pair(dt, contact_pair, total_force_magnitude); + self.force_events + .lock() + .unwrap() + .push((*self.step.lock().unwrap(), event.started)); + } +} + +#[test] +fn started_marks_threshold_crossings_not_contact_newness() { + let events = Events::default(); + let mut world = PhysicsWorld::new(); + + world.insert_collider( + ColliderBuilder::cuboid(10.0, 0.5, 10.0).translation(Vector::new(0.0, -0.5, 0.0)), + None, + ); + + // A 1 kg ball resting under standard gravity presses with ~9.81 N; the threshold sits + // well above that, so resting alone emits no force event. + let threshold = 30.0; + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 0.5, 0.0)) + .additional_mass(1.0) + .can_sleep(false), + ColliderBuilder::ball(0.5) + .density(0.0) + .active_events(ActiveEvents::COLLISION_EVENTS | ActiveEvents::CONTACT_FORCE_EVENTS) + .contact_force_event_threshold(threshold), + ); + + let mut press = false; + let mut step_range = |world: &mut PhysicsWorld, range: core::ops::Range, p: bool| { + press = p; + for i in range { + *events.step.lock().unwrap() = i; + if press { + world.bodies[ball].add_force(Vector::new(0.0, -100.0, 0.0), true); + } + world.step_with_events(&(), &events); + world.bodies[ball].reset_forces(true); + } + }; + + // Phase A (steps 0..100): settle and rest gently. Contact starts, no force events. + step_range(&mut world, 0..100, false); + let started = events.started_steps.lock().unwrap().clone(); + assert_eq!(started.len(), 1, "the ball should have touched down once"); + assert!( + events.force_events.lock().unwrap().is_empty(), + "resting below the threshold must not emit force events" + ); + + // Phase B (steps 100..160): press down; the crossing happens long after Started. + step_range(&mut world, 100..160, true); + { + let evts = events.force_events.lock().unwrap(); + assert!(!evts.is_empty(), "pressing must emit force events"); + assert!( + evts[0].1, + "the first event of the episode must have started" + ); + assert!( + evts[0].0 >= 100 && evts[0].0 > started[0] + 50, + "the crossing (step {}) must be decoupled from contact start (step {})", + evts[0].0, + started[0] + ); + assert!( + evts[1..].iter().all(|(_, first)| !first), + "consecutive above-threshold steps must not have started" + ); + assert!(evts.len() > 10, "the press lasts many steps"); + } + + // Phase C (steps 160..220): release. The pressed contact takes a step or two to + // relax (its impulse is still high right after the release), then the forces sit + // below the threshold and no further event fires. + step_range(&mut world, 160..165, false); + { + let evts = events.force_events.lock().unwrap(); + assert!( + evts.iter().skip(1).all(|(_, first)| !first), + "relaxation events are continuations, never started" + ); + } + let evts_after_press = events.force_events.lock().unwrap().len(); + step_range(&mut world, 165..220, false); + assert_eq!( + events.force_events.lock().unwrap().len(), + evts_after_press, + "no force events while below the threshold" + ); + + // Phase D (steps 220..280): press again: a fresh episode, started fires again. + step_range(&mut world, 220..280, true); + { + let evts = events.force_events.lock().unwrap(); + let episode2 = &evts[evts_after_press..]; + assert!(!episode2.is_empty()); + assert!( + episode2[0].1, + "a new crossing after dropping below the threshold must have started" + ); + assert!(episode2[1..].iter().all(|(_, first)| !first)); + } + + // Phase E: separate entirely, then land hard: started fires again. + let evts_before = events.force_events.lock().unwrap().len(); + world.bodies[ball].set_linvel(Vector::new(0.0, 8.0, 0.0), true); + step_range(&mut world, 280..500, false); + { + let evts = events.force_events.lock().unwrap(); + let episode3 = &evts[evts_before..]; + assert!( + !episode3.is_empty(), + "the landing impact must exceed the threshold" + ); + assert!( + episode3[0].1, + "the first event after separating must have started" + ); + } +} diff --git a/python/tests/test_examples.py b/python/tests/test_examples.py index a5fa01fed..530719a8e 100644 --- a/python/tests/test_examples.py +++ b/python/tests/test_examples.py @@ -29,7 +29,10 @@ "joints/six_dof_motor.py": "motor: lin.x=3.52 ang.z=0.49", # Moved from x=13.50 when parry 0.30.2 tightened the shape-cast TOI at GJK # stagnation (parry#429): the character controller now climbs slightly further. - "character/stairs.py": "climbed: x=13.62 y=0.28", + # y dropped 0.28 -> 0.27 when snap-to-ground started applying to purely lateral + # movement (#481): the character stays snapped onto the last step instead of ending + # a hair above it. + "character/stairs.py": "climbed: x=13.62 y=0.27", # The vehicle controller's exact speed depends on per-architecture floating # point (rapier is not bit-reproducible across arches without # enhanced-determinism), so assert the shape instead: it drove forward at a diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 13189bdf8..6879a80b6 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -30,6 +30,24 @@ impl Default for SolverFlags { } } +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + #[derive(Copy, Clone, PartialEq, Eq, Debug, Default)] + /// Event bookkeeping bits of a contact pair. + /// + /// Serialized as a single byte with the same values as the `start_event_emitted` + /// bool it replaces, so snapshots keep their exact byte layout. + pub(crate) struct PairEventStatus: u8 { + /// A `CollisionEvent::Started` was emitted for this pair. + const START_EVENT_EMITTED = 0b01; + /// The pair's total contact force exceeded its force-event threshold at the + /// previous step. [`ContactForceEvent::started`] is derived from it, and it + /// resets when the force drops back below the threshold or the pair stops + /// touching. + const INITIAL_FORCE_THRESHOLD_EVENT_EMITTED = 0b10; + } +} + #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A single contact between two collider. @@ -223,8 +241,9 @@ pub struct ContactPair { serde(default = "default_solver_color_bodies") )] pub(crate) solver_color_bodies: [u32; 2], - /// Was a `CollisionEvent::Started` emitted for this collider? - pub(crate) start_event_emitted: bool, + /// Event bookkeeping: `CollisionEvent::Started` emission and force-event + /// threshold status. + pub(crate) event_status: PairEventStatus, pub(crate) workspace: Option, /// State cached at the last full narrow-phase update, allowing the update to be /// skipped ("recycled") while the colliders' relative pose stays within @@ -319,7 +338,7 @@ impl ContactPair { solver_clusters_prev: Vec::new(), solver_color: SOLVER_COLOR_UNCOLORED, solver_color_bodies: [u32::MAX; 2], - start_event_emitted: false, + event_status: PairEventStatus::empty(), workspace: None, recycle_state: None, } @@ -336,7 +355,7 @@ impl ContactPair { self.solver_clusters_prev.clear(); self.solver_color = SOLVER_COLOR_UNCOLORED; self.solver_color_bodies = [u32::MAX; 2]; - self.start_event_emitted = false; + self.event_status = PairEventStatus::empty(); self.workspace = None; self.recycle_state = None; } @@ -468,7 +487,8 @@ impl ContactPair { colliders: &ColliderSet, events: &dyn EventHandler, ) { - self.start_event_emitted = true; + self.event_status + .insert(PairEventStatus::START_EVENT_EMITTED); events.handle_collision_event( bodies, @@ -484,7 +504,8 @@ impl ContactPair { colliders: &ColliderSet, events: &dyn EventHandler, ) { - self.start_event_emitted = false; + // Not touching anymore: the force-event threshold status resets with it. + self.event_status = PairEventStatus::empty(); events.handle_collision_event( bodies, diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index d033c0ab1..b11ec69fe 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -15,6 +15,8 @@ pub use self::collider_set::{ColliderSet, ModifiedColliders}; #[cfg(feature = "alloc")] pub(crate) use self::contact_pair::ContactRecycleState; #[cfg(feature = "alloc")] +pub(crate) use self::contact_pair::PairEventStatus; +#[cfg(feature = "alloc")] pub(crate) use self::contact_pair::SOLVER_DYNAMIC_COLOR_COUNT; #[cfg(feature = "alloc")] pub(crate) use self::contact_pair::relative_pose_drift; @@ -203,6 +205,16 @@ pub struct ContactForceEvent { pub max_force_direction: Vector, /// The magnitude of the largest force at a contact point of this contact pair. pub max_force_magnitude: Real, + /// Is this the first step the pair's total force exceeded its threshold? + /// + /// `true` on the step the force crosses the pair's + /// [`Collider::contact_force_event_threshold`] coming from below (or from not + /// touching), `false` while it stays above on consecutive steps. The status resets + /// when the force drops back below the threshold or the colliders separate, so the + /// next crossing reports `true` again. Note that this is about the *force* + /// threshold, not contact newness: a pair can touch gently for many steps (emitting + /// no force event) before its first `started` event. + pub started: bool, } #[cfg(feature = "alloc")] @@ -213,6 +225,11 @@ impl ContactForceEvent { collider1: pair.collider1, collider2: pair.collider2, total_force_magnitude, + // The pair's status is updated only after the event handlers ran, so at + // this point it still holds the previous step's value. + started: !pair + .event_status + .contains(PairEventStatus::INITIAL_FORCE_THRESHOLD_EVENT_EMITTED), ..ContactForceEvent::default() }; diff --git a/src/geometry/narrow_phase/pair_management.rs b/src/geometry/narrow_phase/pair_management.rs index 9820177e6..c8545b776 100644 --- a/src/geometry/narrow_phase/pair_management.rs +++ b/src/geometry/narrow_phase/pair_management.rs @@ -12,7 +12,7 @@ use crate::dynamics::{IslandManager, RigidBodySet}; use crate::geometry::{ BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair, ColliderSet, CollisionEvent, ContactManifoldData, ContactPair, InteractionGraph, - IntersectionPair, + IntersectionPair, PairEventStatus, }; use crate::pipeline::{ActiveEvents, EventHandler}; use crate::prelude::CollisionEventFlags; @@ -97,7 +97,10 @@ impl NarrowPhase { islands.wake_up(bodies, parent.handle, true) } - if pair.start_event_emitted { + if pair + .event_status + .contains(PairEventStatus::START_EVENT_EMITTED) + { events.handle_collision_event( bodies, colliders, @@ -109,7 +112,10 @@ impl NarrowPhase { } else { // If there is no island, don’t wake-up bodies, but do send the Stopped collision event. for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) { - if pair.start_event_emitted { + if pair + .event_status + .contains(PairEventStatus::START_EVENT_EMITTED) + { events.handle_collision_event( bodies, colliders, diff --git a/src/geometry/narrow_phase/solver_graph.rs b/src/geometry/narrow_phase/solver_graph.rs index 6c4c0dbc2..44315f62b 100644 --- a/src/geometry/narrow_phase/solver_graph.rs +++ b/src/geometry/narrow_phase/solver_graph.rs @@ -10,10 +10,12 @@ use crate::dynamics::solver::solver_contact_graph::{ }; use crate::dynamics::{IslandManager, MultibodyJointSet, RigidBodySet}; use crate::geometry::{ - ColliderHandle, ColliderSet, ContactManifold, ContactManifoldData, ContactPair, - InteractionGraph, SolverFlags, + Collider, ColliderHandle, ColliderSet, ContactManifold, ContactManifoldData, ContactPair, + InteractionGraph, PairEventStatus, SolverFlags, }; use crate::math::Real; +#[cfg(feature = "alloc")] +use crate::pipeline::EventHandler; impl NarrowPhase { /// Count-clears the solver hints of a just-asleep body's pairs so they stop @@ -450,8 +452,49 @@ impl NarrowPhase { /// The solver-active pairs with contact-force events enabled — the exact set /// the pipeline's post-solve force-event pass must inspect. - pub(crate) fn force_event_pairs(&self) -> &[u32] { - &self.force_event_pairs + /// Emits the contact force events of the solver-active pairs that request them, and + /// updates each pair's above-threshold status (from which + /// [`crate::geometry::ContactForceEvent::started`] is derived). + /// + /// The narrow-phase maintains the exact set of solver-active pairs with force events + /// enabled, so scenes without them pay nothing here. + #[cfg(feature = "alloc")] + pub(crate) fn emit_contact_force_events( + &mut self, + dt: Real, + bodies: &RigidBodySet, + colliders: &ColliderSet, + events: &dyn EventHandler, + ) { + let inv_dt = crate::utils::inv(dt); + for i in 0..self.force_event_pairs.len() { + let edge = self.force_event_pairs[i] as usize; + let pair = &mut self.contact_graph.graph.edges[edge].weight; + let threshold = |h| { + colliders + .get(h) + .map(|co: &Collider| co.effective_contact_force_event_threshold()) + .unwrap_or(Real::MAX) + }; + let threshold = threshold(pair.collider1).min(threshold(pair.collider2)); + + if threshold < Real::MAX { + let total_magnitude = pair.total_impulse_magnitude() * inv_dt; + + // NOTE: the strict inequality is important here, so we don’t + // trigger an event if the force is 0.0 and the threshold is 0.0. + if total_magnitude > threshold { + // The handler runs before the status update, so the event can read + // the previous step's status to derive `started`. + events.handle_contact_force_event(dt, bodies, colliders, pair, total_magnitude); + pair.event_status + .insert(PairEventStatus::INITIAL_FORCE_THRESHOLD_EVENT_EMITTED); + } else { + pair.event_status + .remove(PairEventStatus::INITIAL_FORCE_THRESHOLD_EVENT_EMITTED); + } + } + } } /// Raw parts of the solver-facing `ManifoldStore` view: the contact graph's edge-array diff --git a/src/pipeline/physics_pipeline/solve.rs b/src/pipeline/physics_pipeline/solve.rs index 5f7ec40c7..228627e64 100644 --- a/src/pipeline/physics_pipeline/solve.rs +++ b/src/pipeline/physics_pipeline/solve.rs @@ -6,9 +6,7 @@ use crate::alloc_prelude::*; use crate::dynamics::{ ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, RigidBodySet, }; -use crate::geometry::{ - BroadPhaseBvh, ColliderHandle, ColliderSet, NarrowPhase, TemporaryInteractionIndex, -}; +use crate::geometry::{BroadPhaseBvh, ColliderHandle, ColliderSet, NarrowPhase}; use crate::math::{Real, Vector}; use crate::pipeline::{EventHandler, PhysicsHooks}; @@ -390,34 +388,14 @@ impl PhysicsPipeline { ); } - // Generate contact force events if needed. The narrow-phase maintains the - // exact set of solver-active pairs with force events enabled, so scenes - // without them pay nothing here. - let inv_dt = crate::utils::inv(integration_parameters.dt); - for &edge_id in narrow_phase.force_event_pairs() { - let pair = narrow_phase.contact_pair_at_index(TemporaryInteractionIndex::new(edge_id)); - let co1 = &colliders[pair.collider1]; - let co2 = &colliders[pair.collider2]; - let threshold = co1 - .effective_contact_force_event_threshold() - .min(co2.effective_contact_force_event_threshold()); - - if threshold < Real::MAX { - let total_magnitude = pair.total_impulse_magnitude() * inv_dt; - - // NOTE: the strict inequality is important here, so we don’t - // trigger an event if the force is 0.0 and the threshold is 0.0. - if total_magnitude > threshold { - events.handle_contact_force_event( - integration_parameters.dt, - bodies, - colliders, - pair, - total_magnitude, - ); - } - } - } + // Generate contact force events if needed, and update each pair's + // above-threshold status (the source of `ContactForceEvent::started`). + narrow_phase.emit_contact_force_events( + integration_parameters.dt, + bodies, + colliders, + events, + ); self.counters.stages.solver_time.pause(); }