diff --git a/CHANGELOG.md b/CHANGELOG.md index c80e66aff..f046b51c9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,38 @@ +## Unreleased + +### Added + +- `Multibody::remove_dof_coupling`, `Multibody::retain_dof_couplings` and + `Multibody::clear_dof_couplings` to remove DoF couplings. +- `rapier3d-urdf`: `UrdfLink::urdf_link_index` and `UrdfJoint::urdf_joint_index` (and related) map the + loaded links and joints to their URDF counterparts, even when empty links are squeezed. + +### Fixed + +- `CollisionPipeline::step` no longer trips a debug assertion when a moving body touches another + collider. +- A zero-length step no longer corrupts the simulation: it only applies the user changes and runs + the collision detection. +- Enabling or disabling an impulse joint now updates the islands of its bodies. +- Multibody DoF couplings and disabled self-contacts are no longer lost when multibodies merge or split. +- `PhysicsPipeline::counters` now fills the contact pair, contact, and constraint counts. +- `rapier3d-urdf`: the fixed children of a squeezed empty link now stay rigidly attached together, and + the joint taking over the removed link's joint keeps its original pivot. +- 2D soft-body cluster proxies now follow counterclockwise rotations of their particles. +- The rigid colliders attached to soft-body cluster proxies no longer lag one step behind their proxy. +- The broad-phase AABBs of soft-body surfaces now match their end-of-step geometry. +- When the CCD splits a step, the end-of-step soft-body motion margin and soft-CCD AABBs now cover the + full next step instead of the last CCD pass. +- The CCD now also splits the first step of a fast body at its impact. +- The soft-body contact history driving the extra substeps now counts steps instead of CCD passes. +- The CCD fast-body check now divides the applied forces by the body's mass, so heavy bodies under + gravity are no longer flagged as fast. + +### Modified + +- The soft-body motion margin now only pads deformable colliders, not the rigid colliders of clusters. +- `RigidBodyCcd::is_moving_fast` now takes the mass-properties along with the forces. + ## v0.35.3 (28 August 2026) ### Fixed @@ -36,7 +71,7 @@ ### Added -- `ContactForceEvent::first_tick`: `true` on the step a pair's total contact force first +- `ContactForceEvent::started`: `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 diff --git a/crates/rapier2d/tests/ccd_split_step.rs b/crates/rapier2d/tests/ccd_split_step.rs new file mode 100644 index 000000000..b68d32031 --- /dev/null +++ b/crates/rapier2d/tests/ccd_split_step.rs @@ -0,0 +1,134 @@ +//! Regression tests for the steps the CCD splits into several passes (`max_ccd_substeps > 1`): +//! what predicts the next step uses the full step's `dt`, not the length of its last pass. + +use rapier2d::prelude::*; + +/// Whether the broad-phase leaf of `co` contains the point `p`. +fn leaf_reaches(world: &PhysicsWorld, co: ColliderHandle, p: Vector) -> bool { + world + .intersect_aabb_conservative(Aabb::new(p, p), QueryFilter::default()) + .any(|(handle, _)| handle == co) +} + +/// A fixed wall at the origin and a fast CCD ball that hits it `hit_time` into the steps +/// (in units of `dt`), far below the rest of the scene. +fn wall_and_ccd_ball(world: &mut PhysicsWorld, hit_time: Real) -> RigidBodyHandle { + let dt = world.integration_parameters.dt; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -20.0)), + ColliderBuilder::cuboid(0.1, 2.0), + ); + let speed = 200.0; + world + .insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(-0.2 - hit_time * speed * dt, -20.0)) + .linvel(Vector::new(speed, 0.0)) + .ccd_enabled(true), + ColliderBuilder::ball(0.1), + ) + .0 +} + +/// The end-of-step broad-phase AABB of a soft-CCD body covers its soft-CCD prediction over the +/// full coming step, even when the CCD split the step and its last pass was short. +#[test] +fn soft_ccd_aabb_spans_the_full_step_when_the_ccd_splits_it() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.max_ccd_substeps = 2; + let dt = world.integration_parameters.dt; + // Hit late in the second step: the first pass stops at the impact, the last one only covers + // a tenth of the step. + wall_and_ccd_ball(&mut world, 1.8); + let speed = 120.0; + let (_, co) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 20.0)) + .linvel(Vector::new(speed, 0.0)) + .soft_ccd_prediction(10.0), + ColliderBuilder::ball(0.1), + ); + + world.step(); + world.step(); + assert_eq!(world.physics_pipeline.counters.ccd.num_substeps, 2); + let aabb = world.colliders[co].compute_aabb(); + let reach = speed * dt; + let p = Vector::new(aabb.maxs.x + 0.9 * reach, aabb.center().y); + assert!( + leaf_reaches(&world, co, p), + "the broad-phase leaf misses the soft-CCD prediction at {p:?}" + ); +} + +/// A CCD body inserted already moving fast gets its first step split at its impact, like any +/// later step: the pre-solve CCD activation reads its current velocity, not the (zero) motion +/// solved on a previous step it didn't take part in. +#[test] +fn first_step_of_a_fast_ccd_body_is_split_at_its_impact() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.max_ccd_substeps = 2; + // Hit 80% into the first step. + let ball = wall_and_ccd_ball(&mut world, 0.8); + + world.step(); + assert_eq!(world.physics_pipeline.counters.ccd.num_substeps, 2); + let x = world.bodies[ball].translation().x; + // Touching the wall at -0.2, the contact's softness letting it sink in a little. + assert!( + (-0.21..-0.15).contains(&x), + "the ball didn't stop at the wall: x = {x}" + ); + // The pass after the impact solved the contact: the ball no longer approaches the wall. + let vx = world.bodies[ball].linvel().x; + assert!(vx < 1.0, "the ball still approaches the wall at {vx}"); +} + +/// The contact history driving the impact-adaptive substeps of a soft body covers its last two +/// steps, however many passes the CCD splits them into: a contact on the step before a split +/// step still counts. +#[test] +fn soft_contact_history_covers_steps_not_ccd_passes() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.max_ccd_substeps = 2; + let max_extra = world.integration_parameters.soft_bodies.max_extra_substeps; + // Splits the second step, far from the square. + wall_and_ccd_ball(&mut world, 1.8); + let square = SoftBodyBuilder::grid(Vector::new(0.0, 1.0), Vector::splat(0.5), 5, 5) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.05); + let h = world.insert_soft_body(square); + let root = world.soft_bodies[h].root_body(); + // Fast enough to request the most substeps while in contact (10 substeps to travel one + // particle radius per substep). + let velocity = Vector::new(30.0, 0.0); + let sb = &mut world.soft_bodies[h]; + for i in 0..sb.num_particles() { + sb.set_particle_velocity(i, velocity); + } + // A ball moving along with the square, touching its leading side. + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.8, 1.0)) + .linvel(velocity), + ColliderBuilder::ball(0.3), + ); + + world.step(); + assert_eq!(world.bodies[root].additional_solver_iterations(), max_extra); + // No contact from now on: the first step's still counts after the (split) second one. + world.remove_body(ball); + world.step(); + assert_eq!(world.physics_pipeline.counters.ccd.num_substeps, 2); + assert_eq!(world.bodies[root].additional_solver_iterations(), max_extra); + // And no longer after a third step. + world.step(); + assert!(world.bodies[root].additional_solver_iterations() < max_extra); +} diff --git a/crates/rapier2d/tests/soft_body_clusters.rs b/crates/rapier2d/tests/soft_body_clusters.rs index e52496c6e..d8dead777 100644 --- a/crates/rapier2d/tests/soft_body_clusters.rs +++ b/crates/rapier2d/tests/soft_body_clusters.rs @@ -190,3 +190,450 @@ fn cluster_joined_outside_itself_splits_only_through_its_link() { assert!(sb.particles().iter().all(|p| p.position().is_finite())); } } + +/// A soft square spun in place (no gravity, no linear motion) turns its whole-body proxy with it +/// in both directions: the proxy's dead zone must not swallow a clockwise rotation change. +#[test] +fn proxy_follows_a_soft_body_spinning_in_place() { + for spin in [-1.0, 1.0] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let square = SoftBodyBuilder::grid(Vector::ZERO, Vector::splat(0.5), 5, 5) + .material(SoftBodyMaterial { + young_modulus: 1.0e6, + ..Default::default() + }) + .particle_mass(0.1); + let h = world.insert_soft_body(square); + let sb = &mut world.soft_bodies[h]; + let corner = (0..sb.num_particles()) + .max_by(|&a, &b| { + let (pa, pb) = (sb.particle_position(a), sb.particle_position(b)); + (pa.x + pa.y).total_cmp(&(pb.x + pb.y)) + }) + .unwrap(); + let arm0 = sb.particle_position(corner) - sb.center_of_mass(); + for i in 0..sb.num_particles() { + let arm = sb.particle_position(i); + sb.set_particle_velocity(i, Vector::new(-arm.y, arm.x) * spin); + } + let proxy = world.soft_bodies[h].root_body(); + let rot0 = *world.bodies[proxy].rotation(); + + for _ in 0..10 { + world.step(); + } + + let sb = &world.soft_bodies[h]; + let arm = sb.particle_position(corner) - sb.center_of_mass(); + let particles_angle = arm0.perp_dot(arm).atan2(arm0.dot(arm)); + let proxy_angle = (rot0.inverse() * *world.bodies[proxy].rotation()).angle(); + assert!( + particles_angle * spin > 0.1, + "spin {spin}: the square did not turn: {particles_angle}" + ); + assert!( + (proxy_angle - particles_angle).abs() < 0.01, + "spin {spin}: proxy turned by {proxy_angle}, particles by {particles_angle}" + ); + } +} + +/// Asserts that a collider sits at its parent proxy's pose composed with its local pose, and that +/// the broad phase has a leaf covering it there. +fn assert_rides_its_proxy(world: &PhysicsWorld, co: ColliderHandle, step: usize) { + let collider = &world.colliders[co]; + let proxy = &world.bodies[collider.parent().unwrap()]; + let expected = proxy.position() * collider.position_wrt_parent().unwrap(); + let actual = collider.position(); + let dt = (actual.translation - expected.translation).length(); + // Compares rotated axes: an `acos`-based angle is too noisy near zero in single precision. + let dr = [Vector::X, Vector::Y] + .map(|axis| (actual.rotation * axis - expected.rotation * axis).length()) + .into_iter() + .fold(0.0, Real::max); + assert!( + dt < 1.0e-5 && dr < 1.0e-5, + "step {step}: collider {co:?} lags its proxy (translation error {dt}, rotation error {dr})" + ); + assert!( + world + .intersect_aabb_conservative(collider.compute_aabb(), QueryFilter::default()) + .any(|(handle, _)| handle == co), + "step {step}: the broad phase has no leaf covering collider {co:?}" + ); +} + +/// Rigid colliders hung with an offset on the whole-body proxy and on a sub-cluster proxy of a +/// falling, spinning soft square are at their proxy's pose at the end of every step, not one step +/// behind. +#[test] +fn rigid_colliders_on_proxies_do_not_lag_their_proxy() { + let mut world = PhysicsWorld::new(); + let square = SoftBodyBuilder::grid(Vector::new(0.0, 1.0), Vector::splat(0.5), 5, 5) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + ..Default::default() + }) + .particle_mass(0.1); + let h = world.insert_soft_body(square); + let top: Vec = world.soft_bodies[h] + .particles() + .iter() + .enumerate() + .filter(|(_, p)| p.position().y > 1.2) + .map(|(i, _)| i as u32) + .collect(); + assert!(!top.is_empty()); + let cluster = world + .soft_bodies + .add_cluster(h, &top, &mut world.bodies, &mut world.colliders) + .unwrap(); + let sb = &mut world.soft_bodies[h]; + let com = sb.center_of_mass(); + for i in 0..sb.num_particles() { + let arm = sb.particle_position(i) - com; + sb.set_particle_velocity(i, Vector::new(1.0, 0.5) + Vector::new(-arm.y, arm.x) * 2.0); + } + let root = world.soft_bodies[h].root_body(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let offset = Pose::from_parts(Vector::new(0.3, -0.2), Rotation::new(0.4)); + let colliders: Vec = [root, proxy] + .into_iter() + .map(|parent| { + world.colliders.insert_with_parent( + ColliderBuilder::cuboid(0.05, 0.05) + .density(0.1) + .position(offset), + parent, + &mut world.bodies, + ) + }) + .collect(); + + for step in 0..30 { + world.step(); + for co in &colliders { + assert_rides_its_proxy(&world, *co, step); + } + } + // The proxies did move, so a lagging collider could not have passed. + assert!(world.bodies[root].translation().y < 0.5); +} + +/// Asserts that the broad-phase leaf of a collider contains the collider's current AABB: every +/// corner of that AABB hits the leaf. +fn assert_leaf_contains_current_aabb(world: &PhysicsWorld, co: ColliderHandle, step: usize) { + let aabb = world.colliders[co].compute_aabb(); + for i in 0..4 { + let pick = |bit: usize, min: Real, max: Real| if i & bit == 0 { min } else { max }; + let corner = Vector::new( + pick(1, aabb.mins.x, aabb.maxs.x), + pick(2, aabb.mins.y, aabb.maxs.y), + ); + assert!( + world + .intersect_aabb_conservative(Aabb::new(corner, corner), QueryFilter::default()) + .any(|(handle, _)| handle == co), + "step {step}: the broad-phase leaf of collider {co:?} misses the corner {corner:?} of \ + its current AABB" + ); + } +} + +/// The surface collider of a soft square falling faster at each step (the large timestep makes +/// gravity outrun the speculative margin) has a broad-phase leaf around its end-of-step geometry, +/// so a ray cast between steps hits its current bottom edge. +#[test] +fn deformable_collider_leaves_follow_their_surface_at_the_end_of_each_step() { + let mut world = PhysicsWorld::new(); + world.integration_parameters.dt = 0.1; + let square = SoftBodyBuilder::grid(Vector::new(0.0, 1.0), Vector::splat(0.5), 5, 5) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.01); + let h = world.insert_soft_body(square); + let surfaces: Vec = world.soft_bodies[h] + .meshes() + .map(|mesh| mesh.collider()) + .collect(); + assert!(!surfaces.is_empty()); + + for step in 0..20 { + world.step(); + for co in &surfaces { + assert_leaf_contains_current_aabb(&world, *co, step); + let aabb = world.colliders[*co].compute_aabb(); + // Off the surface's vertices, which a ray may graze. + let ray = Ray::new( + Vector::new(aabb.center().x + 0.13, aabb.mins.y - 0.01), + Vector::Y, + ); + let hit = world.cast_ray(&ray, 0.02, true, QueryFilter::default()); + assert_eq!( + hit.map(|(handle, _)| handle), + Some(*co), + "step {step}: a ray cast misses the bottom edge of collider {co:?}" + ); + } + } + // The square did fall, so a stale leaf could not have passed. + let root = world.soft_bodies[h].root_body(); + assert!(world.bodies[root].translation().y < -10.0); +} + +/// A stiff soft square, with a sub-cluster on its top rows. +fn square_with_a_top_cluster(world: &mut PhysicsWorld) -> (SoftBodyHandle, u32) { + let square = SoftBodyBuilder::grid(Vector::new(0.0, 1.0), Vector::splat(0.5), 5, 5) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + ..Default::default() + }) + .particle_mass(0.1); + let h = world.insert_soft_body(square); + let top: Vec = world.soft_bodies[h] + .particles() + .iter() + .enumerate() + .filter(|(_, p)| p.position().y > 1.2) + .map(|(i, _)| i as u32) + .collect(); + assert!(!top.is_empty()); + let cluster = world + .soft_bodies + .add_cluster(h, &top, &mut world.bodies, &mut world.colliders) + .unwrap(); + (h, cluster) +} + +/// Whether the broad-phase leaf of a collider reaches the point `p`. +fn leaf_reaches(world: &PhysicsWorld, co: ColliderHandle, p: Vector) -> bool { + world + .intersect_aabb_conservative(Aabb::new(p, p), QueryFilter::default()) + .any(|(handle, _)| handle == co) +} + +/// The points `dist` past the middle of each edge of a collider's current AABB. +fn points_past_aabb(world: &PhysicsWorld, co: ColliderHandle, dist: Real) -> Vec { + let aabb = world.colliders[co].compute_aabb(); + let (center, half) = (aabb.center(), aabb.half_extents()); + (0..2) + .flat_map(|i| { + [-1.0, 1.0].map(|sign| { + let mut p = center; + p[i] += sign * (half[i] + dist); + p + }) + }) + .collect() +} + +/// The soft-body motion margin pads the deformable colliders of a fast soft square only: the +/// rigid colliders on its proxies get the broad-phase AABB of a collider on a dynamic body. +#[test] +fn only_deformable_colliders_are_padded_by_the_soft_motion_margin() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let (h, cluster) = square_with_a_top_cluster(&mut world); + // A motion margin of about 0.67 per step. + let velocity = Vector::new(40.0, 0.0); + let sb = &mut world.soft_bodies[h]; + for i in 0..sb.num_particles() { + sb.set_particle_velocity(i, velocity); + } + let surfaces: Vec = world.soft_bodies[h] + .meshes() + .map(|mesh| mesh.collider()) + .collect(); + assert!(!surfaces.is_empty()); + let shape = ColliderBuilder::cuboid(0.05, 0.05) + .density(0.1) + .translation(Vector::new(0.3, -0.2)); + let root = world.soft_bodies[h].root_body(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let mut rigid: Vec = [root, proxy] + .into_iter() + .map(|parent| { + world + .colliders + .insert_with_parent(shape.clone(), parent, &mut world.bodies) + }) + .collect(); + // The same collider on a dynamic body moving like the square, for reference. + let (_, reference) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 5.0)) + .linvel(velocity), + shape, + ); + rigid.push(reference); + + let params = world.integration_parameters; + for step in 0..10 { + world.step(); + for co in &rigid { + let collider = &world.colliders[*co]; + assert_eq!( + collider.compute_broad_phase_aabb(¶ms, &world.bodies), + collider.compute_collision_aabb(params.prediction_distance() / 2.0), + "step {step}: collider {co:?} has a padded broad-phase AABB" + ); + for p in points_past_aabb(&world, *co, 0.2) { + assert!( + !leaf_reaches(&world, *co, p), + "step {step}: the broad-phase leaf of the rigid collider {co:?} reaches {p:?}" + ); + } + } + for co in &surfaces { + for p in points_past_aabb(&world, *co, 0.2) { + assert!( + leaf_reaches(&world, *co, p), + "step {step}: the broad-phase leaf of the surface {co:?} misses {p:?}" + ); + } + } + } + // The square kept its speed, so its margin stayed large. + assert!(world.bodies[root].translation().x > 5.0); +} + +/// Shoots a ball at a rigid ball hung on the root proxy of a soft square flying toward it +/// (`on_proxy`), or on a dynamic body moving like that square. Returns whether they touched and +/// whether the shot ended past the target. +fn shoot_at_a_moving_target(on_proxy: bool, speed: Real, ccd: bool) -> (bool, bool) { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let velocity = Vector::new(10.0, 0.0); + let offset = Vector::new(1.2, 0.0); + let target = ColliderBuilder::ball(0.5).density(0.1).translation(offset); + let target = if on_proxy { + let (h, _) = square_with_a_top_cluster(&mut world); + let sb = &mut world.soft_bodies[h]; + for i in 0..sb.num_particles() { + sb.set_particle_velocity(i, velocity); + } + let root = world.soft_bodies[h].root_body(); + world + .colliders + .insert_with_parent(target, root, &mut world.bodies) + } else { + let body = RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 1.0)) + .linvel(velocity) + .additional_mass(2.5); + world.insert(body, target).1 + }; + let start = world.colliders[target].translation() + Vector::new(6.0, 0.0); + let (shot, shot_co) = world.insert( + RigidBodyBuilder::dynamic() + .translation(start) + .linvel(Vector::new(-speed, 0.0)) + .ccd_enabled(ccd), + ColliderBuilder::ball(0.1), + ); + + let mut touched = false; + for _ in 0..40 { + world.step(); + touched |= world + .narrow_phase + .contact_pair(shot_co, target) + .is_some_and(|pair| pair.has_any_active_contact()); + } + let passed = world.bodies[shot].translation().x < world.colliders[target].translation().x; + (touched, passed) +} + +/// A rigid collider on a fast soft body's proxy stops a fast ball like the same collider on a +/// dynamic body does, without the soft-body motion margin: discretely when each step's closing +/// travel is shorter than the colliders, through the ball's CCD otherwise. +#[test] +fn rigid_collider_on_a_fast_proxy_does_not_let_a_ball_through() { + for (speed, ccd) in [(20.0, false), (200.0, true)] { + for on_proxy in [false, true] { + let (touched, passed) = shoot_at_a_moving_target(on_proxy, speed, ccd); + assert!( + touched && !passed, + "speed {speed}, ccd {ccd}, on a proxy {on_proxy}: touched {touched}, passed {passed}" + ); + } + } +} + +/// The motion margin a collider's broad-phase AABB is padded with, past the prediction distance. +fn motion_margin(world: &PhysicsWorld, co: ColliderHandle) -> Real { + let params = world.integration_parameters; + let collider = &world.colliders[co]; + let padded = collider.compute_broad_phase_aabb(¶ms, &world.bodies); + let unpadded = collider.compute_collision_aabb(params.prediction_distance() / 2.0); + padded.half_extents().x - unpadded.half_extents().x +} + +/// A step split by the CCD sizes the soft-body motion margin with the full step's `dt`, not with +/// the length of its last CCD pass: the margin covers the coming step's travel. +#[test] +fn soft_motion_margin_spans_the_full_step_when_the_ccd_splits_it() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.max_ccd_substeps = 2; + let dt = world.integration_parameters.dt; + let square = SoftBodyBuilder::grid(Vector::new(0.0, 1.0), Vector::splat(0.5), 5, 5) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + ..Default::default() + }) + .particle_mass(0.1); + let h = world.insert_soft_body(square); + let velocity = Vector::new(100.0, 0.0); + let sb = &mut world.soft_bodies[h]; + for i in 0..sb.num_particles() { + sb.set_particle_velocity(i, velocity); + } + let surfaces: Vec = world.soft_bodies[h] + .meshes() + .map(|mesh| mesh.collider()) + .collect(); + assert!(!surfaces.is_empty()); + // A fast CCD ball hitting a wall late in the second step, far from the square: the first CCD + // pass stops at the impact and the last one only covers the rest of the step. + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -20.0)), + ColliderBuilder::cuboid(0.1, 2.0), + ); + let speed = 200.0; + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(-0.2 - 1.8 * speed * dt, -20.0)) + .linvel(Vector::new(speed, 0.0)) + .ccd_enabled(true), + ColliderBuilder::ball(0.1), + ); + + world.step(); + world.step(); + assert_eq!(world.physics_pipeline.counters.ccd.num_substeps, 2); + let max_speed = world.soft_bodies[h] + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + let expected = max_speed * dt; + assert!(expected > 1.5); + for co in &surfaces { + let margin = motion_margin(&world, *co); + assert!( + (margin - expected).abs() < 1.0e-3, + "surface {co:?}: margin {margin}, expected {expected}" + ); + for p in points_past_aabb(&world, *co, 0.9 * expected) { + assert!( + leaf_reaches(&world, *co, p), + "the broad-phase leaf of the surface {co:?} misses {p:?}" + ); + } + } +} diff --git a/crates/rapier2d/tests/zero_dt_step.rs b/crates/rapier2d/tests/zero_dt_step.rs new file mode 100644 index 000000000..791fd1a67 --- /dev/null +++ b/crates/rapier2d/tests/zero_dt_step.rs @@ -0,0 +1,137 @@ +//! Zero-length steps (`IntegrationParameters::dt == 0`, e.g. the first frame of a variable +//! timestep) must leave the simulation state untouched instead of corrupting it. + +use rapier2d::prelude::*; + +/// A plate welded to the top-edge cluster of a jelly: a zero-length step used to corrupt the +/// cluster's state, making the plate jump (up to NaN colliders). +#[test] +fn zero_dt_step_does_not_disturb_a_welded_cluster() { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(10.0, 0.5), + ); + let jelly = SoftBodyBuilder::grid(Vector::new(0.0, 1.0), Vector::splat(0.6), 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .particle_mass(0.08) + .particle_radius(0.05) + .can_sleep(false); + let top_y = jelly + .particle_positions() + .iter() + .map(|p| p.y) + .fold(Real::MIN, Real::max); + let top_edge: Vec = (0..jelly.particle_positions().len() as u32) + .filter(|i| jelly.particle_positions()[*i as usize].y > top_y - 1.0e-3) + .collect(); + let h = world.insert_soft_body(jelly); + let cluster = world.add_soft_body_cluster(h, &top_edge).unwrap(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let (plate, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, top_y + 0.2)), + ColliderBuilder::cuboid(0.7, 0.06), + ); + world.insert_impulse_joint( + plate, + proxy, + FixedJointBuilder::new().local_anchor1(Vector::new(0.0, -0.2)), + ); + + let particles_before: Vec = world.soft_bodies[h].particle_positions().collect(); + let plate_before = *world.bodies[plate].position(); + + // The zero-length step changes nothing and injects no velocity. + world.integration_parameters.dt = 0.0; + world.step(); + assert!( + world.soft_bodies[h] + .particle_positions() + .eq(particles_before.iter().copied()) + ); + assert_eq!(*world.bodies[plate].position(), plate_before); + assert_eq!(world.bodies[plate].linvel(), Vector::ZERO); + assert_eq!(world.bodies[plate].angvel(), 0.0); + assert!( + world.soft_bodies[h] + .particle_velocities() + .all(|v| v == Vector::ZERO) + ); + + // The regular steps that follow stay calm and finite. + world.integration_parameters.dt = 1.0 / 60.0; + world.step(); + let max_particle_speed = world.soft_bodies[h] + .particle_velocities() + .map(|v| v.length()) + .fold(0.0, Real::max); + let plate_speed = world.bodies[plate].linvel().length(); + assert!( + max_particle_speed < 0.5 && plate_speed < 0.5, + "{max_particle_speed} {plate_speed}" + ); + + for _ in 0..60 { + world.step(); + } + assert!( + world.soft_bodies[h] + .particle_positions() + .all(|p| p.is_finite()) + ); + let plate_pos = world.bodies[plate].position(); + assert!(plate_pos.translation.is_finite()); + // The plate stays (roughly) level on top of the jelly. + assert!( + plate_pos.rotation.angle().abs() < 0.3, + "{}", + plate_pos.rotation.angle() + ); + for (_, co) in world.colliders.iter() { + assert!(co.position().translation.is_finite()); + } +} + +/// A zero-length step still applies the user changes and runs collision detection, so contacts +/// and scene queries reflect the latest edits, but moves nothing. +#[test] +fn zero_dt_step_detects_collisions_without_moving_bodies() { + let mut world = PhysicsWorld::new(); + world.integration_parameters.dt = 0.0; + let (_, ground_co) = world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(10.0, 0.5), + ); + let (ball, ball_co) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 0.9)) + .linvel(Vector::new(1.0, 2.0)), + ColliderBuilder::ball(0.5), + ); + let (kinematic, _) = world.insert( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(5.0, 3.0)), + ColliderBuilder::ball(0.5), + ); + world.bodies[kinematic].set_next_kinematic_translation(Vector::new(6.0, 3.0)); + + for _ in 0..3 { + world.step(); + } + + assert_eq!(world.bodies[ball].translation(), Vector::new(0.0, 0.9)); + assert_eq!(world.bodies[ball].linvel(), Vector::new(1.0, 2.0)); + assert_eq!(world.bodies[kinematic].translation(), Vector::new(5.0, 3.0)); + + assert!( + world + .narrow_phase + .contact_pair(ground_co, ball_co) + .is_some_and(|pair| pair.has_any_active_contact()) + ); + + // The next regular step simulates normally (and moves the kinematic body). + world.integration_parameters.dt = 1.0 / 60.0; + world.step(); + assert_ne!(world.bodies[ball].translation(), Vector::new(0.0, 0.9)); + assert_eq!(world.bodies[kinematic].translation(), Vector::new(6.0, 3.0)); +} diff --git a/crates/rapier3d-urdf/src/lib.rs b/crates/rapier3d-urdf/src/lib.rs index 3bfa9051b..103ba26d8 100644 --- a/crates/rapier3d-urdf/src/lib.rs +++ b/crates/rapier3d-urdf/src/lib.rs @@ -152,15 +152,29 @@ pub struct UrdfLoaderOptions { /// chain stays equivalent. /// /// Concretely: - /// - When an empty link sits between a parent and a non-empty child connected by a - /// **fixed** joint, the empty link and the fixed joint are dropped and the surviving - /// joint inherits the type/axis/limits of the parent joint, with its origin composed - /// so the child ends up at the same world pose. + /// - When an empty link is attached to its parent by a **fixed** joint, the empty link + /// and that joint are dropped, and all its children (whatever their joint types) are + /// attached to its parent directly, with their joint origins composed so they end up at + /// the same pose. + /// - When an empty link is attached to its parent by a **moving** joint and to at least + /// one child by a **fixed** joint, the empty link and its parent joint are dropped, and + /// one of these fixed children (preferably a non-empty one) becomes its representative: + /// its joint takes over the type, axis, limits, and pivot of the dropped parent joint. + /// The other children of the empty link (whatever their joint types) are attached to the + /// representative with their joint origins composed. This way, all the fixed children + /// keep moving together as one rigid group, driven by the single original moving joint. + /// - An empty link attached to its parent and to all its children by moving joints is + /// kept, since it is needed to chain these joints. /// - When the empty link is the **root** and is connected to its child(ren) by a fixed /// joint, the link and the joint are removed, and the child is forced to be a fixed /// rigid-body (independently of [`Self::make_roots_fixed`]). /// - Empty leaves (no children) are simply dropped. /// + /// A dropped parent joint is recorded in the [`UrdfJoint::merged_urdf_joint_indices`] of + /// at most one joint: the joint of the representative (or of one of the fixed children if + /// the dropped joint is fixed). Some dropped joints have no counterpart at all, e.g., the + /// joint of an empty leaf, or the fixed joint of an empty link with only moving children. + /// /// This avoids ending up with bodyless, mass-less rigid-bodies that exist only to act /// as named frames in the URDF (a common pattern for `world` anchors and `*_tcp` /// tool-center-points). @@ -223,6 +237,11 @@ pub struct UrdfLink { /// corresponding [`UrdfLoaderOptions`] option is enabled), each paired with an /// optional visual override (see [`UrdfCollider::visual`]). pub colliders: Vec, + /// Index, in the original [`Robot::links`], of the URDF link this link was built from. + /// + /// This can differ from this link's index in [`UrdfRobot::links`] when empty links were + /// removed (see [`UrdfLoaderOptions::squeeze_empty_fixed_links`]). + pub urdf_link_index: usize, } /// An urdf joint loaded as a rapier [`GenericJoint`]. @@ -236,6 +255,38 @@ pub struct UrdfJoint { /// Index of the rigid-body (from the [`UrdfRobot`] array) at the second /// endpoint of this joint. pub link2: LinkId, + /// Index, in the original [`Robot::joints`], of the URDF joint this joint was built from. + /// + /// This is the URDF joint attached to the child link [`Self::link2`]. It can differ from + /// this joint's index in [`UrdfRobot::joints`] when empty links were removed (see + /// [`UrdfLoaderOptions::squeeze_empty_fixed_links`]). In that case, the URDF parent link + /// of the joint might also have been replaced by another one (see [`Self::link1`]). + pub urdf_joint_index: usize, + /// Indices, in the original [`Robot::joints`], of the URDF joints that were merged into + /// this joint when the empty links between them were removed (see + /// [`UrdfLoaderOptions::squeeze_empty_fixed_links`]). + /// + /// They are ordered from the child side to the parent side. The last one (if any) is the + /// joint this joint inherited its type, axis, limits, dynamics, and mimic from (see + /// [`Self::source_urdf_joint_index`]). This is empty if no joint was merged into this one. + /// + /// A URDF joint is merged into at most one joint, even if the empty link it led to had + /// several children. + pub merged_urdf_joint_indices: Vec, +} + +impl UrdfJoint { + /// Index, in the original [`Robot::joints`], of the URDF joint this joint inherited its + /// type, axis, limits, dynamics, and mimic from. + /// + /// This is [`Self::urdf_joint_index`], unless other joints were merged into this one (see + /// [`Self::merged_urdf_joint_indices`]). + pub fn source_urdf_joint_index(&self) -> usize { + self.merged_urdf_joint_indices + .last() + .copied() + .unwrap_or(self.urdf_joint_index) + } } /// A robot represented as a set of rapier rigid-bodies, colliders, and joints. @@ -243,11 +294,15 @@ pub struct UrdfJoint { pub struct UrdfRobot { /// The bodies and colliders loaded from the urdf file. /// - /// This vector matches the order of [`Robot::links`]. + /// This vector follows the order of [`Robot::links`], but some links might be missing if + /// [`UrdfLoaderOptions::squeeze_empty_fixed_links`] is enabled. Use + /// [`UrdfLink::urdf_link_index`] to find the URDF link each element was built from. pub links: Vec, /// The joints loaded from the urdf file. /// - /// This vector matches the order of [`Robot::joints`]. + /// This vector follows the order of [`Robot::joints`], but some joints might be missing if + /// [`UrdfLoaderOptions::squeeze_empty_fixed_links`] is enabled. Use + /// [`UrdfJoint::urdf_joint_index`] to find the URDF joint each element was built from. pub joints: Vec, } @@ -292,7 +347,12 @@ pub struct UrdfRobotHandles { impl UrdfRobot { /// Parses a URDF file and returns both the rapier objects (`UrdfRobot`) and the original urdf - /// structures (`Robot`). Both structures are arranged the same way, with matching indices for each part. + /// structures (`Robot`). + /// + /// Both structures are arranged in the same order, but some links and joints of the `Robot` + /// might have no counterpart in the `UrdfRobot` if [`UrdfLoaderOptions::squeeze_empty_fixed_links`] + /// is enabled. Use [`UrdfLink::urdf_link_index`] and [`UrdfJoint::urdf_joint_index`] to find the + /// `Robot` element each `UrdfRobot` element was built from. /// /// If the URDF file references external meshes, they will be loaded automatically if the format /// is supported. The format is detected from the file’s extension. All the mesh formats are @@ -322,7 +382,12 @@ impl UrdfRobot { } /// Parses a string in URDF format and returns both the rapier objects (`UrdfRobot`) and the original urdf - /// structures (`Robot`). Both structures are arranged the same way, with matching indices for each part. + /// structures (`Robot`). + /// + /// Both structures are arranged in the same order, but some links and joints of the `Robot` + /// might have no counterpart in the `UrdfRobot` if [`UrdfLoaderOptions::squeeze_empty_fixed_links`] + /// is enabled. Use [`UrdfLink::urdf_link_index`] and [`UrdfJoint::urdf_joint_index`] to find the + /// `Robot` element each `UrdfRobot` element was built from. /// /// If the URDF file references external meshes, they will be loaded automatically if the format /// is supported. The format is detected from the file’s extension. All the mesh formats are @@ -346,7 +411,12 @@ impl UrdfRobot { } /// From an already loaded urdf file as a `Robot`, this creates the matching rapier objects - /// (`UrdfRobot`). Both structures are arranged the same way, with matching indices for each part. + /// (`UrdfRobot`). + /// + /// Both structures are arranged in the same order, but some links and joints of the `Robot` + /// might have no counterpart in the `UrdfRobot` if [`UrdfLoaderOptions::squeeze_empty_fixed_links`] + /// is enabled. Use [`UrdfLink::urdf_link_index`] and [`UrdfJoint::urdf_joint_index`] to find the + /// `Robot` element each `UrdfRobot` element was built from. /// /// If the URDF file references external meshes, they will be loaded automatically if the format /// is supported. The format is detected mostly from the file’s extension. All the mesh formats are @@ -363,15 +433,25 @@ impl UrdfRobot { // Optionally rewrite the URDF graph to remove empty links connected by fixed // joints so we don't end up with bodyless rigid-bodies that exist only to // serve as named frames. - let (robot_owned, force_fixed_links): (std::borrow::Cow, HashSet) = + let (robot_owned, squeeze): (std::borrow::Cow, SqueezeResult) = if options.squeeze_empty_fixed_links { let mut clone = robot.clone(); - let force_fixed = squeeze_empty_fixed_links(&mut clone); - (std::borrow::Cow::Owned(clone), force_fixed) + let squeeze = squeeze_empty_fixed_links(&mut clone); + (std::borrow::Cow::Owned(clone), squeeze) } else { - (std::borrow::Cow::Borrowed(robot), HashSet::new()) + ( + std::borrow::Cow::Borrowed(robot), + SqueezeResult::identity(robot), + ) }; let robot = robot_owned.as_ref(); + let SqueezeResult { + force_fixed: force_fixed_links, + link_ids, + joint_ids, + mut merged_joint_ids, + child_offsets, + } = squeeze; let mut name_to_link_id = HashMap::new(); let mut link_is_root = vec![true; robot.links.len()]; @@ -395,27 +475,34 @@ impl UrdfRobot { let mut body = urdf_to_rigid_body(&options, &link.inertial); let new_pos = options.shift * body.position(); body.set_position(new_pos, false); - UrdfLink { body, colliders } - }) - .collect(); - let joints: Vec<_> = robot - .joints - .iter() - .map(|joint| { - let link1 = name_to_link_id[&joint.parent.link]; - let link2 = name_to_link_id[&joint.child.link]; - let pose1 = *links[link1].body.position(); - let rb2 = &mut links[link2].body; - let joint = urdf_to_joint(&options, joint, &pose1, rb2); - link_is_root[link2] = false; - - UrdfJoint { - joint, - link1, - link2, + UrdfLink { + body, + colliders, + urdf_link_index: link_ids[id], } }) .collect(); + // A joint places its child link relative to its parent link, so the parent must be + // placed first. + let mut joints: Vec> = vec![None; robot.joints.len()]; + for i in joints_from_roots_to_leaves(robot) { + let joint = &robot.joints[i]; + let link1 = name_to_link_id[&joint.parent.link]; + let link2 = name_to_link_id[&joint.child.link]; + let pose1 = *links[link1].body.position(); + let rb2 = &mut links[link2].body; + let generic_joint = urdf_to_joint(&options, joint, &child_offsets[i], &pose1, rb2); + link_is_root[link2] = false; + + joints[i] = Some(UrdfJoint { + joint: generic_joint, + link1, + link2, + urdf_joint_index: joint_ids[i], + merged_urdf_joint_indices: std::mem::take(&mut merged_joint_ids[i]), + }); + } + let joints = joints.into_iter().flatten().collect(); if options.make_roots_fixed { for (link, is_root) in links.iter_mut().zip(link_is_root.iter().copied()) { @@ -720,6 +807,7 @@ fn urdf_to_pose(pose: &UrdfPose) -> Pose { fn urdf_to_joint( options: &UrdfLoaderOptions, joint: &Joint, + child_offset: &Pose, pose1: &Pose, link2: &mut RigidBody, ) -> GenericJoint { @@ -742,7 +830,13 @@ fn urdf_to_joint( ) .normalize_or_zero(); - link2.set_position(pose1 * joint_to_parent, false); + // The pose of the child link in the joint frame, which isn't the identity if the joint was + // merged with the fixed joints following it. + let mut child_offset = *child_offset; + child_offset.translation *= options.scale; + let joint_to_child = child_offset.inverse(); + + link2.set_position(pose1 * joint_to_parent * child_offset, false); let mut builder = GenericJointBuilder::new(locked_axes).contacts_enabled(options.enable_joint_collisions); @@ -753,11 +847,14 @@ fn urdf_to_joint( // `local_axis2` would yield mismatched secondary axes, leaving the joint // unsatisfied at rest and snapping the bodies on the first step. let basis = GenericJoint::complete_ang_frame(joint_axis); - let frame2 = Pose::from_rotation(basis); - let frame1 = joint_to_parent * frame2; - builder = builder.local_frame1(frame1).local_frame2(frame2); + let basis = Pose::from_rotation(basis); + builder = builder + .local_frame1(joint_to_parent * basis) + .local_frame2(joint_to_child * basis); } else { - builder = builder.local_frame1(joint_to_parent); + builder = builder + .local_frame1(joint_to_parent) + .local_frame2(joint_to_child); } match joint.joint_type { @@ -860,128 +957,236 @@ fn pose_to_urdf_pose(pose: &Pose) -> UrdfPose { } } -fn compose_urdf_pose(parent: &UrdfPose, child: &UrdfPose) -> UrdfPose { - pose_to_urdf_pose(&(urdf_to_pose(parent) * urdf_to_pose(child))) +/// The indices of the joints of `robot`, ordered so that the joint attached to a link comes +/// before the joints attached to its children. +fn joints_from_roots_to_leaves(robot: &Robot) -> Vec { + let mut child_joints: HashMap<&str, Vec> = HashMap::new(); + let mut child_links = HashSet::new(); + for (i, joint) in robot.joints.iter().enumerate() { + child_joints + .entry(joint.parent.link.as_str()) + .or_default() + .push(i); + child_links.insert(joint.child.link.as_str()); + } + + let mut order = Vec::with_capacity(robot.joints.len()); + let mut visited = vec![false; robot.joints.len()]; + let mut stack: Vec<&str> = robot + .links + .iter() + .map(|link| link.name.as_str()) + .filter(|name| !child_links.contains(name)) + .collect(); + while let Some(link) = stack.pop() { + for &i in child_joints.get(link).into_iter().flatten() { + if !visited[i] { + visited[i] = true; + order.push(i); + stack.push(robot.joints[i].child.link.as_str()); + } + } + } + + // Joints unreachable from a root (in an invalid URDF with a kinematic loop) keep their order. + order.extend((0..robot.joints.len()).filter(|i| !visited[*i])); + order } -/// Re-expresses a joint axis (originally specified in the empty link's frame) in -/// the new child's frame after splicing through the empty link. -fn rotate_axis_into_child_frame(axis: &urdf_rs::Vec3, child_origin: &UrdfPose) -> urdf_rs::Vec3 { - let pose = urdf_to_pose(child_origin); - let v = Vector::new(axis[0] as Real, axis[1] as Real, axis[2] as Real); - let r = pose.rotation.inverse() * v; - urdf_rs::Vec3([r.x as f64, r.y as f64, r.z as f64]) +/// The outcome of [`squeeze_empty_fixed_links`]. +struct SqueezeResult { + /// Names of the links that must be fixed because the empty root anchoring them was removed. + force_fixed: HashSet, + /// For each remaining link, its index in the original robot. + link_ids: Vec, + /// For each remaining joint, its index in the original robot. + joint_ids: Vec, + /// For each remaining joint, the original indices of the joints merged into it. + merged_joint_ids: Vec>, + /// For each remaining joint, the pose (in URDF units) of its child link in the joint frame. + /// + /// This isn't the identity when the joint took over a moving joint leading to an empty link. + child_offsets: Vec, } -/// Rewrites `robot` to remove empty links connected by fixed joints. Returns the -/// names of links that should be marked as fixed-base bodies (because the empty -/// root that anchored them to the world was removed). -/// -/// See [`UrdfLoaderOptions::squeeze_empty_fixed_links`] for the precise rules. -fn squeeze_empty_fixed_links(robot: &mut Robot) -> HashSet { - let mut force_fixed: HashSet = HashSet::new(); +impl SqueezeResult { + /// The result of squeezing a robot without any empty link. + fn identity(robot: &Robot) -> Self { + Self { + force_fixed: HashSet::new(), + link_ids: (0..robot.links.len()).collect(), + joint_ids: (0..robot.joints.len()).collect(), + merged_joint_ids: vec![vec![]; robot.joints.len()], + child_offsets: vec![Pose::IDENTITY; robot.joints.len()], + } + } + + fn remove_link(&mut self, robot: &mut Robot, name: &str) { + if let Some(id) = robot.links.iter().position(|l| l.name == name) { + robot.links.remove(id); + self.link_ids.remove(id); + } + } + + fn remove_joint(&mut self, robot: &mut Robot, id: usize) { + robot.joints.remove(id); + self.joint_ids.remove(id); + self.merged_joint_ids.remove(id); + self.child_offsets.remove(id); + } - loop { - let empty_names: Vec = robot + /// Records that the joint `parent_joint` was merged into the joint `joint`. + fn merge_parent_joint(&mut self, joint: usize, parent_joint: usize) { + let parent_id = self.joint_ids[parent_joint]; + let parent_merged = self.merged_joint_ids[parent_joint].clone(); + let merged = &mut self.merged_joint_ids[joint]; + merged.push(parent_id); + merged.extend(parent_merged); + } +} + +/// Attaches the joint `joint` of an empty link to `new_parent`, where `new_parent_to_link` is +/// the pose of the empty link in the frame of `new_parent`. +fn reparent_joint(robot: &mut Robot, joint: usize, new_parent: &str, new_parent_to_link: &Pose) { + let joint = &mut robot.joints[joint]; + joint.parent.link = new_parent.to_string(); + joint.origin = pose_to_urdf_pose(&(new_parent_to_link * urdf_to_pose(&joint.origin))); +} + +/// Among the fixed joints attaching children to an empty link, picks the one that takes over +/// the empty link's parent joint. +/// +/// Non-empty children are preferred, then empty children that have children themselves. +fn pick_representative(robot: &Robot, fixed_child_joints: &[usize]) -> Option { + let is_empty = |name: &str| { + robot .links .iter() - .filter(|l| is_link_empty(l)) - .map(|l| l.name.clone()) - .collect(); - if empty_names.is_empty() { - break; + .find(|l| l.name == name) + .is_none_or(is_link_empty) + }; + let has_children = |name: &str| robot.joints.iter().any(|j| j.parent.link == name); + fixed_child_joints.iter().copied().min_by_key(|&i| { + let child = robot.joints[i].child.link.as_str(); + if !is_empty(child) { + 0 + } else if has_children(child) { + 1 + } else { + 2 } + }) +} - let mut progressed = false; - - for empty_name in &empty_names { - let parent_joint_idx = robot - .joints - .iter() - .position(|j| j.child.link == *empty_name); - let child_joint_indices: Vec = robot - .joints - .iter() - .enumerate() - .filter_map(|(i, j)| (j.parent.link == *empty_name).then_some(i)) - .collect(); - - match parent_joint_idx { - Some(pj_idx) => { - let parent_joint = robot.joints[pj_idx].clone(); - let mut spliced_any = false; - - for &cj_idx in &child_joint_indices { - if robot.joints[cj_idx].joint_type != urdf_rs::JointType::Fixed { - continue; - } - let child_origin = robot.joints[cj_idx].origin.clone(); - let new_origin = compose_urdf_pose(&parent_joint.origin, &child_origin); - let new_axis = - rotate_axis_into_child_frame(&parent_joint.axis.xyz, &child_origin); - - let cj = &mut robot.joints[cj_idx]; - cj.parent = parent_joint.parent.clone(); - cj.origin = new_origin; - cj.joint_type = parent_joint.joint_type.clone(); - cj.axis.xyz = new_axis; - cj.limit = parent_joint.limit.clone(); - cj.dynamics = parent_joint.dynamics.clone(); - cj.mimic = parent_joint.mimic.clone(); - cj.safety_controller = parent_joint.safety_controller.clone(); - cj.calibration = parent_joint.calibration.clone(); - spliced_any = true; - } +/// Rewrites `robot` to remove empty links connected by fixed joints, and tracks the +/// original indices of the remaining links and joints. +/// +/// See [`UrdfLoaderOptions::squeeze_empty_fixed_links`] for the precise rules. +fn squeeze_empty_fixed_links(robot: &mut Robot) -> SqueezeResult { + let mut result = SqueezeResult::identity(robot); + while squeeze_one_empty_link(robot, &mut result) {} + result +} - let still_has_child = robot - .joints - .iter() - .enumerate() - .any(|(i, j)| i != pj_idx && j.parent.link == *empty_name); +/// Removes one empty link (or the fixed joints of an empty root) from `robot`, returning `false` +/// if no empty link can be removed. +fn squeeze_one_empty_link(robot: &mut Robot, result: &mut SqueezeResult) -> bool { + let empty_names: Vec = robot + .links + .iter() + .filter(|l| is_link_empty(l)) + .map(|l| l.name.clone()) + .collect(); + + for empty_name in &empty_names { + let parent_joint = robot + .joints + .iter() + .position(|j| j.child.link == *empty_name); + let child_joints: Vec = robot + .joints + .iter() + .enumerate() + .filter_map(|(i, j)| (j.parent.link == *empty_name).then_some(i)) + .collect(); + let fixed_child_joints: Vec = child_joints + .iter() + .copied() + .filter(|i| robot.joints[*i].joint_type == urdf_rs::JointType::Fixed) + .collect(); - if !still_has_child { - robot.joints.remove(pj_idx); - if let Some(li) = robot.links.iter().position(|l| &l.name == empty_name) { - robot.links.remove(li); - } - progressed = true; - break; - } else if spliced_any { - progressed = true; - break; - } + match parent_joint { + Some(pj) if robot.joints[pj].joint_type == urdf_rs::JointType::Fixed => { + // The empty link is rigidly attached to its parent: attach its children to its + // parent directly. One fixed child records the removed joint. + let parent = robot.joints[pj].parent.link.clone(); + let parent_to_link = + urdf_to_pose(&robot.joints[pj].origin) * result.child_offsets[pj]; + for &cj in &child_joints { + reparent_joint(robot, cj, &parent, &parent_to_link); } - None => { - // Empty root. - let mut spliced_any = false; - // Remove fixed children (in reverse to keep indices valid) and - // remember their freed children so we can fix them later. - for &cj_idx in child_joint_indices.iter().rev() { - if robot.joints[cj_idx].joint_type == urdf_rs::JointType::Fixed { - force_fixed.insert(robot.joints[cj_idx].child.link.clone()); - robot.joints.remove(cj_idx); - spliced_any = true; - } - } - let still_has_child = robot.joints.iter().any(|j| j.parent.link == *empty_name); - if !still_has_child { - if let Some(li) = robot.links.iter().position(|l| &l.name == empty_name) { - robot.links.remove(li); - } - progressed = true; - break; - } else if spliced_any { - progressed = true; - break; - } + if let Some(rep) = pick_representative(robot, &fixed_child_joints) { + result.merge_parent_joint(rep, pj); } + result.remove_joint(robot, pj); + result.remove_link(robot, empty_name); + return true; } - } + Some(pj) if child_joints.is_empty() => { + // Empty leaf. + result.remove_joint(robot, pj); + result.remove_link(robot, empty_name); + return true; + } + Some(pj) => { + // The empty link is moved by its parent joint: one of its fixed children (the + // representative) takes over that joint, and its other children are attached to + // the representative so they all keep moving together. + let Some(rep) = pick_representative(robot, &fixed_child_joints) else { + // Only moving children: the empty link is needed between the moving joints. + continue; + }; + let rep_link = robot.joints[rep].child.link.clone(); + let link_to_rep = + urdf_to_pose(&robot.joints[rep].origin) * result.child_offsets[rep]; + let rep_to_link = link_to_rep.inverse(); + for &cj in &child_joints { + if cj != rep { + reparent_joint(robot, cj, &rep_link, &rep_to_link); + } + } - if !progressed { - break; + // The joint keeps its origin and axis in the frame of the empty link, which + // becomes the representative's offset from the joint frame. + let rep_joint = &robot.joints[rep]; + let mut merged_joint = robot.joints[pj].clone(); + merged_joint.name = rep_joint.name.clone(); + merged_joint.child = rep_joint.child.clone(); + robot.joints[rep] = merged_joint; + result.child_offsets[rep] = result.child_offsets[pj] * link_to_rep; + result.merge_parent_joint(rep, pj); + + result.remove_joint(robot, pj); + result.remove_link(robot, empty_name); + return true; + } + None => { + // Empty root: its fixed children are anchored to the world instead. + // Remove them in reverse to keep the indices valid. + for &cj in fixed_child_joints.iter().rev() { + let child = robot.joints[cj].child.link.clone(); + result.force_fixed.insert(child); + result.remove_joint(robot, cj); + } + if fixed_child_joints.len() == child_joints.len() { + result.remove_link(robot, empty_name); + return true; + } else if !fixed_child_joints.is_empty() { + return true; + } + } } } - force_fixed + false } diff --git a/crates/rapier3d-urdf/tests/squeezed_fixed_children.rs b/crates/rapier3d-urdf/tests/squeezed_fixed_children.rs new file mode 100644 index 000000000..7a9ad545f --- /dev/null +++ b/crates/rapier3d-urdf/tests/squeezed_fixed_children.rs @@ -0,0 +1,441 @@ +//! Checks that the children attached by fixed joints to a squeezed empty link keep moving +//! together, driven by the single moving joint of the empty link. + +use rapier3d::prelude::*; +use rapier3d_urdf::urdf_rs::Robot; +use rapier3d_urdf::{UrdfLoaderOptions, UrdfMultibodyOptions, UrdfRobot}; +use std::collections::HashMap; +use std::path::Path; + +fn link(name: &str, radius: f32) -> String { + format!( + r#" + + + + + + + + "# + ) +} + +/// `base -(hinge)-> mount -(fixed)-> a` and `mount -(fixed)-> b`, where `mount` is empty. +/// +/// If `with_moving_child` is set, `mount -(wrist)-> c` is added too. It is listed first so the +/// joints don't follow the order in which the links have to be placed after squeezing. +fn urdf(with_moving_child: bool) -> String { + let (c_link, c_joint) = if with_moving_child { + ( + link("c", 0.05), + r#" + + + + + + "#, + ) + } else { + (String::new(), "") + }; + format!( + r#" + + {base} + + {a} + {b} + {c_link} + {c_joint} + + + + + + + + + + + + + + + + + + +"#, + base = link("base", 0.05), + a = link("a", 0.05), + b = link("b", 0.05), + ) +} + +fn load(urdf: &str, squeeze: bool) -> (UrdfRobot, Robot) { + let options = UrdfLoaderOptions { + make_roots_fixed: true, + squeeze_empty_fixed_links: squeeze, + ..Default::default() + }; + UrdfRobot::from_str(urdf, options, Path::new("./")).unwrap() +} + +fn link_names(robot: &UrdfRobot, urdf: &Robot) -> Vec { + robot + .links + .iter() + .map(|l| urdf.links[l.urdf_link_index].name.clone()) + .collect() +} + +fn joint_names(urdf: &Robot, indices: &[usize]) -> Vec { + indices + .iter() + .map(|i| urdf.joints[*i].name.clone()) + .collect() +} + +/// Simulates the robot under gravity and returns the final pose of each link, by name. +fn simulate(urdf: &str, squeeze: bool, multibody: bool) -> HashMap { + let (robot, raw) = load(urdf, squeeze); + let names = link_names(&robot, &raw); + let mut world = PhysicsWorld::new(); + let links = if multibody { + robot + .insert_using_multibody_joints( + &mut world.bodies, + &mut world.colliders, + &mut world.multibody_joints, + UrdfMultibodyOptions::DISABLE_SELF_CONTACTS, + ) + .links + } else { + robot + .insert_using_impulse_joints( + &mut world.bodies, + &mut world.colliders, + &mut world.impulse_joints, + ) + .links + }; + + for _ in 0..60 { + world.step(); + } + + // Every impulse joint keeps its anchors together (up to the drift of the iterative solver). + for (_, joint) in world.impulse_joints.iter() { + let anchor1 = *world.bodies[joint.body1()].position() * joint.data.local_frame1; + let anchor2 = *world.bodies[joint.body2()].position() * joint.data.local_frame2; + assert!( + (anchor1.translation - anchor2.translation).length() < 5.0e-2, + "joint anchors diverged: {anchor1:?} vs {anchor2:?}" + ); + } + + names + .into_iter() + .zip(links) + .map(|(name, link)| (name, *world.bodies[link.body].position())) + .collect() +} + +fn assert_poses_eq(pose1: &Pose, pose2: &Pose, eps: f32) { + assert!( + (pose1.translation - pose2.translation).length() < eps, + "{pose1:?} != {pose2:?}" + ); + // Twice the sine of the half-angle, more accurate than `angle_between` for small angles. + let angle = (pose1.rotation.inverse() * pose2.rotation).xyz().length() * 2.0; + assert!(angle < eps, "{pose1:?} != {pose2:?}"); +} + +#[test] +fn fixed_children_of_squeezed_link_share_its_joint() { + let (robot, raw) = load(&urdf(false), true); + assert_eq!(link_names(&robot, &raw), ["base", "a", "b"]); + + // `b` (the first non-empty fixed child in joint order) takes over the hinge, and `a` is + // attached to it. + assert_eq!(robot.joints.len(), 2); + let b_joint = &robot.joints[0]; + assert_eq!(raw.joints[b_joint.urdf_joint_index].name, "b_joint"); + assert_eq!((b_joint.link1, b_joint.link2), (0, 2)); + assert_eq!( + joint_names(&raw, &b_joint.merged_urdf_joint_indices), + ["hinge"] + ); + assert_eq!(raw.joints[b_joint.source_urdf_joint_index()].name, "hinge"); + assert_eq!( + b_joint.joint.locked_axes, + JointAxesMask::LOCKED_REVOLUTE_AXES + ); + + let a_joint = &robot.joints[1]; + assert_eq!(raw.joints[a_joint.urdf_joint_index].name, "a_joint"); + assert_eq!((a_joint.link1, a_joint.link2), (2, 1)); + assert!(a_joint.merged_urdf_joint_indices.is_empty()); + assert_eq!(a_joint.source_urdf_joint_index(), a_joint.urdf_joint_index); + assert_eq!(a_joint.joint.locked_axes, JointAxesMask::LOCKED_FIXED_AXES); + + // The hinge still pivots around the origin of `mount`. + assert!( + (b_joint.joint.local_frame1.translation - Vector::new(0.0, 0.0, 1.0)).length() < 1.0e-5 + ); + + // The links are placed as without squeezing. + let (unsqueezed, _) = load(&urdf(false), false); + for (i, link) in robot.links.iter().enumerate() { + let expected = unsqueezed.links[link.urdf_link_index].body.position(); + assert_poses_eq(link.body.position(), expected, 1.0e-5); + assert_eq!(link.urdf_link_index, [0, 2, 3][i]); + } +} + +#[test] +fn moving_child_of_squeezed_link_is_attached_to_the_representative() { + let (robot, raw) = load(&urdf(true), true); + assert_eq!(link_names(&robot, &raw), ["base", "a", "b", "c"]); + assert_eq!(robot.joints.len(), 3); + + let wrist = &robot.joints[0]; + assert_eq!(raw.joints[wrist.urdf_joint_index].name, "wrist"); + assert_eq!((wrist.link1, wrist.link2), (2, 3)); + assert!(wrist.merged_urdf_joint_indices.is_empty()); + assert_eq!(wrist.joint.locked_axes, JointAxesMask::LOCKED_REVOLUTE_AXES); + + // Each URDF joint appears at most once in the mapping. + let mut all: Vec<_> = robot + .joints + .iter() + .flat_map(|j| { + std::iter::once(j.urdf_joint_index).chain(j.merged_urdf_joint_indices.clone()) + }) + .collect(); + all.sort(); + assert_eq!(all, [0, 1, 2, 3]); + + let (unsqueezed, _) = load(&urdf(true), false); + for link in &robot.links { + let expected = unsqueezed.links[link.urdf_link_index].body.position(); + assert_poses_eq(link.body.position(), expected, 1.0e-5); + } +} + +#[test] +fn fixed_children_of_squeezed_link_move_together() { + for with_moving_child in [false, true] { + let urdf = urdf(with_moving_child); + let (robot, raw) = load(&urdf, true); + let initial: HashMap<_, _> = link_names(&robot, &raw) + .into_iter() + .zip(robot.links.iter().map(|l| *l.body.position())) + .collect(); + let initial_rel = initial["a"].inverse() * initial["b"]; + + for multibody in [false, true] { + let poses = simulate(&urdf, true, multibody); + // Impulse joints are solved iteratively, so they drift a bit. + let eps = if multibody { 1.0e-3 } else { 5.0e-2 }; + + // The hinge actually moved `a`. + let swing = poses["a"].rotation.angle_between(initial["a"].rotation); + assert!( + swing > 0.1, + "the hinge didn't move (multibody: {multibody})" + ); + // `a` and `b` stayed rigidly attached. + let rel = poses["a"].inverse() * poses["b"]; + assert_poses_eq(&rel, &initial_rel, eps); + // `a` still rotates around the hinge's pivot, i.e., the origin of `mount`. + let pivot = Vector::new(0.0, 0.0, 1.0); + let dist = (poses["a"].translation - pivot).length(); + assert!((dist - 0.5).abs() < eps, "wrong pivot: {dist}"); + + if multibody { + // Same motion as the unsqueezed robot, where `mount` is a massless body. + let expected = simulate(&urdf, false, true); + for (name, pose) in &poses { + assert_poses_eq(pose, &expected[name], 1.0e-3); + } + } + } + } +} + +fn joint(name: &str, ty: &str, parent: &str, child: &str, origin: &str) -> String { + format!( + r#" + + + + + + "# + ) +} + +/// Checks that the squeezed robot starts at the same poses and moves like the unsqueezed one, +/// and that each URDF joint is mapped at most once. +fn check_squeeze_equivalence(urdf: &str) -> (UrdfRobot, Robot) { + let (robot, raw) = load(urdf, true); + let (unsqueezed, _) = load(urdf, false); + for link in &robot.links { + let expected = unsqueezed.links[link.urdf_link_index].body.position(); + assert_poses_eq(link.body.position(), expected, 1.0e-5); + } + + let mut all: Vec<_> = robot + .joints + .iter() + .flat_map(|j| { + std::iter::once(j.urdf_joint_index).chain(j.merged_urdf_joint_indices.clone()) + }) + .collect(); + all.sort(); + all.dedup(); + let num_mapped: usize = robot + .joints + .iter() + .map(|j| 1 + j.merged_urdf_joint_indices.len()) + .sum(); + assert_eq!(all.len(), num_mapped, "a URDF joint is mapped twice"); + + let poses = simulate(urdf, true, true); + let expected = simulate(urdf, false, true); + for (name, pose) in &poses { + assert_poses_eq(pose, &expected[name], 1.0e-3); + } + (robot, raw) +} + +#[test] +fn chain_of_squeezed_links_keeps_fixed_children_together() { + // `m1` and `m2` are empty: `base -(hinge)-> m1 -(fixed)-> m2 -(fixed)-> {a, b}` and + // `m1 -(fixed)-> d`. + let urdf = format!( + r#" + + {base} + + + {a} + {b} + {d} + {hinge} + {m1_m2} + {a_joint} + {b_joint} + {d_joint} +"#, + base = link("base", 0.05), + a = link("a", 0.05), + b = link("b", 0.05), + d = link("d", 0.05), + hinge = joint( + "hinge", + "revolute", + "base", + "m1", + r#"xyz="0 0 1" rpy="0.3 0 0""# + ), + m1_m2 = joint( + "m1_m2", + "fixed", + "m1", + "m2", + r#"xyz="0.4 0 0" rpy="0 0 0.7""# + ), + a_joint = joint("a_joint", "fixed", "m2", "a", r#"xyz="0.3 0 0""#), + b_joint = joint("b_joint", "fixed", "m2", "b", r#"xyz="0 0.3 0.1""#), + d_joint = joint("d_joint", "fixed", "m1", "d", r#"xyz="-0.3 0 0""#), + ); + let (robot, raw) = check_squeeze_equivalence(&urdf); + assert_eq!(link_names(&robot, &raw), ["base", "a", "b", "d"]); + + let joints: Vec<_> = robot + .joints + .iter() + .map(|j| { + ( + raw.joints[j.urdf_joint_index].name.as_str(), + j.link1, + j.link2, + joint_names(&raw, &j.merged_urdf_joint_indices), + ) + }) + .collect(); + // `d` (non-empty) is preferred over `m2` to take over the hinge. + assert_eq!( + joints, + [ + ("a_joint", 3, 1, vec!["m1_m2".to_string()]), + ("b_joint", 3, 2, vec![]), + ("d_joint", 0, 3, vec!["hinge".to_string()]), + ] + ); +} + +#[test] +fn moving_child_of_fixed_empty_link_is_attached_to_its_parent() { + // `base -(fixed)-> m -(fixed)-> a` and `m -(wrist)-> c`, where `m` is empty. + let urdf = format!( + r#" + + {base} + + {a} + {c} + {wrist} + {m_joint} + {a_joint} +"#, + base = link("base", 0.05), + a = link("a", 0.05), + c = link("c", 0.05), + wrist = joint( + "wrist", + "revolute", + "m", + "c", + r#"xyz="0.4 0 0" rpy="0.5 0 0""# + ), + m_joint = joint( + "m_joint", + "fixed", + "base", + "m", + r#"xyz="0 0 1" rpy="0 0.4 0""# + ), + a_joint = joint("a_joint", "fixed", "m", "a", r#"xyz="0 0.3 0""#), + ); + let (robot, raw) = check_squeeze_equivalence(&urdf); + assert_eq!(link_names(&robot, &raw), ["base", "a", "c"]); + + let joints: Vec<_> = robot + .joints + .iter() + .map(|j| { + ( + raw.joints[j.urdf_joint_index].name.as_str(), + j.link1, + j.link2, + joint_names(&raw, &j.merged_urdf_joint_indices), + ) + }) + .collect(); + assert_eq!( + joints, + [ + ("wrist", 0, 2, vec![]), + ("a_joint", 0, 1, vec!["m_joint".to_string()]), + ] + ); + assert_eq!( + robot.joints[0].joint.locked_axes, + JointAxesMask::LOCKED_REVOLUTE_AXES + ); +} diff --git a/crates/rapier3d-urdf/tests/squeezed_link_indices.rs b/crates/rapier3d-urdf/tests/squeezed_link_indices.rs new file mode 100644 index 000000000..2753f0657 --- /dev/null +++ b/crates/rapier3d-urdf/tests/squeezed_link_indices.rs @@ -0,0 +1,116 @@ +//! Checks that the links and joints of an `UrdfRobot` can be matched with the original URDF +//! links and joints after empty links were squeezed. + +use rapier3d::prelude::*; +use rapier3d_urdf::{UrdfLoaderOptions, UrdfRobot}; +use std::path::Path; + +// `world` is an empty root, `mount_a` and `mount_b` are empty links between `base` and `arm`, +// and `tcp` is an empty leaf. Only `base` and `arm` remain after squeezing. +const URDF: &str = r#" + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +"#; + +#[test] +fn squeezed_links_and_joints_map_to_urdf_indices() { + let (robot, urdf) = + UrdfRobot::from_str(URDF, UrdfLoaderOptions::default(), Path::new("./")).unwrap(); + + let link_names: Vec<_> = robot + .links + .iter() + .map(|l| urdf.links[l.urdf_link_index].name.as_str()) + .collect(); + assert_eq!(link_names, ["base", "arm"]); + // The empty root was removed, so `base` is anchored to the world. + assert!(robot.links[0].body.is_fixed()); + assert_eq!(robot.links[1].colliders.len(), 1); + + assert_eq!(robot.joints.len(), 1); + let joint = &robot.joints[0]; + assert_eq!((joint.link1, joint.link2), (0, 1)); + assert_eq!(urdf.joints[joint.urdf_joint_index].name, "mount_b_joint"); + let merged: Vec<_> = joint + .merged_urdf_joint_indices + .iter() + .map(|i| urdf.joints[*i].name.as_str()) + .collect(); + assert_eq!(merged, ["mount_a_joint", "hinge"]); + assert_eq!(urdf.joints[joint.source_urdf_joint_index()].name, "hinge"); + assert_eq!(joint.joint.locked_axes, JointAxesMask::LOCKED_REVOLUTE_AXES); + + // The arm ends up at the same place as without squeezing. + let arm_pos = robot.links[1].body.translation(); + assert!((arm_pos - Vector::new(0.5, 0.0, 1.5)).length() < 1.0e-5); + // The hinge still pivots around the origin of `mount_a`, not around the arm's origin. + let pivot = Vector::new(0.0, 0.0, 1.0); + assert!((joint.joint.local_frame1.translation - pivot).length() < 1.0e-5); + let pivot_in_arm = robot.links[1].body.position().inverse() * pivot; + assert!((joint.joint.local_frame2.translation - pivot_in_arm).length() < 1.0e-5); +} + +#[test] +fn unsqueezed_links_and_joints_map_to_urdf_indices() { + let options = UrdfLoaderOptions { + squeeze_empty_fixed_links: false, + ..Default::default() + }; + let (robot, urdf) = UrdfRobot::from_str(URDF, options, Path::new("./")).unwrap(); + + assert_eq!(robot.links.len(), urdf.links.len()); + assert_eq!(robot.joints.len(), urdf.joints.len()); + for (i, link) in robot.links.iter().enumerate() { + assert_eq!(link.urdf_link_index, i); + } + for (i, joint) in robot.joints.iter().enumerate() { + assert_eq!(joint.urdf_joint_index, i); + assert!(joint.merged_urdf_joint_indices.is_empty()); + assert_eq!(joint.source_urdf_joint_index(), i); + } +} diff --git a/crates/rapier3d/tests/ccd_split_step.rs b/crates/rapier3d/tests/ccd_split_step.rs new file mode 100644 index 000000000..38fda867d --- /dev/null +++ b/crates/rapier3d/tests/ccd_split_step.rs @@ -0,0 +1,138 @@ +//! Regression tests for the steps the CCD splits into several passes (`max_ccd_substeps > 1`): +//! what predicts the next step uses the full step's `dt`, not the length of its last pass. + +use rapier3d::prelude::*; + +/// Whether the broad-phase leaf of `co` contains the point `p`. +fn leaf_reaches(world: &PhysicsWorld, co: ColliderHandle, p: Vector) -> bool { + world + .intersect_aabb_conservative(Aabb::new(p, p), QueryFilter::default()) + .any(|(handle, _)| handle == co) +} + +/// A fixed wall at the origin and a fast CCD ball that hits it `hit_time` into the steps +/// (in units of `dt`), far below the rest of the scene. +fn wall_and_ccd_ball(world: &mut PhysicsWorld, hit_time: Real) -> RigidBodyHandle { + let dt = world.integration_parameters.dt; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -20.0, 0.0)), + ColliderBuilder::cuboid(0.1, 2.0, 2.0), + ); + let speed = 200.0; + world + .insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(-0.2 - hit_time * speed * dt, -20.0, 0.0)) + .linvel(Vector::new(speed, 0.0, 0.0)) + .ccd_enabled(true), + ColliderBuilder::ball(0.1), + ) + .0 +} + +/// The end-of-step broad-phase AABB of a soft-CCD body covers its soft-CCD prediction over the +/// full coming step, even when the CCD split the step and its last pass was short. +#[test] +fn soft_ccd_aabb_spans_the_full_step_when_the_ccd_splits_it() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.max_ccd_substeps = 2; + let dt = world.integration_parameters.dt; + // Hit late in the second step: the first pass stops at the impact, the last one only covers + // a tenth of the step. + wall_and_ccd_ball(&mut world, 1.8); + let speed = 120.0; + let (_, co) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 20.0, 0.0)) + .linvel(Vector::new(speed, 0.0, 0.0)) + .soft_ccd_prediction(10.0), + ColliderBuilder::ball(0.1), + ); + + world.step(); + world.step(); + assert_eq!(world.physics_pipeline.counters.ccd.num_substeps, 2); + let aabb = world.colliders[co].compute_aabb(); + let reach = speed * dt; + let p = Vector::new(aabb.maxs.x + 0.9 * reach, aabb.center().y, aabb.center().z); + assert!( + leaf_reaches(&world, co, p), + "the broad-phase leaf misses the soft-CCD prediction at {p:?}" + ); +} + +/// A CCD body inserted already moving fast gets its first step split at its impact, like any +/// later step: the pre-solve CCD activation reads its current velocity, not the (zero) motion +/// solved on a previous step it didn't take part in. +#[test] +fn first_step_of_a_fast_ccd_body_is_split_at_its_impact() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.max_ccd_substeps = 2; + // Hit 80% into the first step. + let ball = wall_and_ccd_ball(&mut world, 0.8); + + world.step(); + assert_eq!(world.physics_pipeline.counters.ccd.num_substeps, 2); + let x = world.bodies[ball].translation().x; + // Touching the wall at -0.2, the contact's softness letting it sink in a little. + assert!( + (-0.21..-0.15).contains(&x), + "the ball didn't stop at the wall: x = {x}" + ); + // The pass after the impact solved the contact: the ball no longer approaches the wall. + let vx = world.bodies[ball].linvel().x; + assert!(vx < 1.0, "the ball still approaches the wall at {vx}"); +} + +/// The contact history driving the impact-adaptive substeps of a soft body covers its last two +/// steps, however many passes the CCD splits them into: a contact on the step before a split +/// step still counts. +#[test] +fn soft_contact_history_covers_steps_not_ccd_passes() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.max_ccd_substeps = 2; + let max_extra = world.integration_parameters.soft_bodies.max_extra_substeps; + // Splits the second step, far from the cube. + wall_and_ccd_ball(&mut world, 1.8); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 1.0, 0.0), Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e4, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05)); + let h = world.insert_soft_body(cube); + let root = world.soft_bodies[h].root_body(); + // Fast enough to request the most substeps while in contact (10 substeps to travel one + // particle radius per substep). + let velocity = Vector::new(30.0, 0.0, 0.0); + let sb = &mut world.soft_bodies[h]; + for i in 0..sb.num_particles() { + sb.set_particle_velocity(i, velocity); + } + // A ball moving along with the cube, touching its leading side. + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.85, 1.0, 0.0)) + .linvel(velocity), + ColliderBuilder::ball(0.3), + ); + + world.step(); + assert_eq!(world.bodies[root].additional_solver_iterations(), max_extra); + // No contact from now on: the first step's still counts after the (split) second one. + world.remove_body(ball); + world.step(); + assert_eq!(world.physics_pipeline.counters.ccd.num_substeps, 2); + assert_eq!(world.bodies[root].additional_solver_iterations(), max_extra); + // And no longer after a third step. + world.step(); + assert!(world.bodies[root].additional_solver_iterations() < max_extra); +} diff --git a/crates/rapier3d/tests/collision_pipeline_moving_bodies.rs b/crates/rapier3d/tests/collision_pipeline_moving_bodies.rs new file mode 100644 index 000000000..c48fa0718 --- /dev/null +++ b/crates/rapier3d/tests/collision_pipeline_moving_bodies.rs @@ -0,0 +1,122 @@ +//! `CollisionPipeline::step` with non-fixed bodies: the pipeline doesn't register bodies in +//! the island manager, which used to trip the island-membership debug assertion as soon as a +//! kinematic or dynamic body started touching another collider. + +use rapier3d::prelude::*; + +struct CollisionWorld { + bodies: RigidBodySet, + colliders: ColliderSet, + islands: IslandManager, + broad_phase: DefaultBroadPhase, + narrow_phase: NarrowPhase, + pipeline: CollisionPipeline, +} + +impl CollisionWorld { + fn new() -> Self { + Self { + bodies: RigidBodySet::new(), + colliders: ColliderSet::new(), + islands: IslandManager::new(), + broad_phase: DefaultBroadPhase::new(), + narrow_phase: NarrowPhase::new(), + pipeline: CollisionPipeline::new(), + } + } + + fn step(&mut self) { + self.pipeline.step( + IntegrationParameters::default().prediction_distance(), + &mut self.islands, + &mut self.broad_phase, + &mut self.narrow_phase, + &mut self.bodies, + &mut self.colliders, + &(), + &(), + ); + } + + fn touching(&self, co1: ColliderHandle, co2: ColliderHandle) -> bool { + self.narrow_phase + .contact_pair(co1, co2) + .is_some_and(|pair| pair.has_any_active_contact()) + } +} + +fn check_moving_body_touches(body_type: RigidBodyType, other_type: Option) { + let mut world = CollisionWorld::new(); + // Kinematic-fixed and kinematic-kinematic contacts are disabled by default. + let all_types = ActiveCollisionTypes::all(); + + let other = match other_type { + Some(ty) => { + let body = world.bodies.insert(RigidBodyBuilder::new(ty)); + world.colliders.insert_with_parent( + ColliderBuilder::cuboid(1.0, 1.0, 1.0).active_collision_types(all_types), + body, + &mut world.bodies, + ) + } + None => world + .colliders + .insert(ColliderBuilder::cuboid(1.0, 1.0, 1.0).active_collision_types(all_types)), + }; + + let body = world + .bodies + .insert(RigidBodyBuilder::new(body_type).translation(Vector::new(0.0, 5.0, 0.0))); + let co = world.colliders.insert_with_parent( + ColliderBuilder::ball(0.5).active_collision_types(all_types), + body, + &mut world.bodies, + ); + + world.step(); + assert!(!world.touching(co, other)); + + // Move the body into the other collider: the begin-touch transition must not panic. + world.bodies[body].set_translation(Vector::new(0.0, 1.2, 0.0), true); + world.step(); + assert!(world.touching(co, other)); + world.step(); + assert!(world.touching(co, other)); + + // And move it away again (end-touch transition). + world.bodies[body].set_translation(Vector::new(0.0, 5.0, 0.0), true); + world.step(); + assert!(!world.touching(co, other)); + + // Removing the bodies with the pipeline's island manager must work too. + world.bodies.remove( + body, + &mut world.islands, + &mut world.colliders, + &mut ImpulseJointSet::new(), + &mut MultibodyJointSet::new(), + &mut SoftBodySet::new(), + true, + ); + world.step(); +} + +#[test] +fn kinematic_body_touching_fixed_collider() { + check_moving_body_touches(RigidBodyType::KinematicPositionBased, None); + check_moving_body_touches( + RigidBodyType::KinematicPositionBased, + Some(RigidBodyType::Fixed), + ); + check_moving_body_touches(RigidBodyType::KinematicVelocityBased, None); +} + +#[test] +fn dynamic_bodies_touching() { + check_moving_body_touches(RigidBodyType::Dynamic, Some(RigidBodyType::Dynamic)); + check_moving_body_touches(RigidBodyType::Dynamic, None); + check_moving_body_touches( + RigidBodyType::KinematicPositionBased, + Some(RigidBodyType::Dynamic), + ); +} diff --git a/crates/rapier3d/tests/multibody_coupling_topology.rs b/crates/rapier3d/tests/multibody_coupling_topology.rs new file mode 100644 index 000000000..26af2849a --- /dev/null +++ b/crates/rapier3d/tests/multibody_coupling_topology.rs @@ -0,0 +1,187 @@ +//! Multibody DoF couplings and the self-contacts flag across topology changes (link insertion, +//! multibody merges and splits), and the coupling removal API. + +use rapier3d::dynamics::MultibodyDofCoupling; +use rapier3d::prelude::*; + +fn insert_link(world: &mut PhysicsWorld, x: Real) -> RigidBodyHandle { + world.insert_body( + RigidBodyBuilder::dynamic() + .translation(Vector::new(x, 0.0, 0.0)) + .additional_mass_properties(MassProperties::new(Vector::ZERO, 1.0, Vector::splat(0.1))), + ) +} + +fn hinge(world: &mut PhysicsWorld, parent: RigidBodyHandle, child: RigidBodyHandle) { + world + .insert_multibody_joint(parent, child, RevoluteJointBuilder::new(Vector::Z)) + .unwrap(); +} + +fn link_id(world: &PhysicsWorld, body: RigidBodyHandle) -> usize { + world.multibody_joints.rigid_body_link(body).unwrap().id +} + +fn multibody_of(world: &PhysicsWorld, body: RigidBodyHandle) -> &Multibody { + let link = world.multibody_joints.rigid_body_link(body).unwrap(); + world + .multibody_joints + .get_multibody(link.multibody) + .unwrap() +} + +fn multibody_of_mut(world: &mut PhysicsWorld, body: RigidBodyHandle) -> &mut Multibody { + let link = *world.multibody_joints.rigid_body_link(body).unwrap(); + world + .multibody_joints + .get_multibody_mut(link.multibody) + .unwrap() +} + +/// Couples the hinges of `body1` and `body2`: `q2 = coeff·q1 + offset`. +fn couple(world: &mut PhysicsWorld, body1: RigidBodyHandle, body2: RigidBodyHandle, coeff: Real) { + let coupling = MultibodyDofCoupling { + link1: link_id(world, body1), + dof1: 0, + axis1: 3, + link2: link_id(world, body2), + dof2: 0, + axis2: 3, + coeff, + offset: 0.0, + }; + multibody_of_mut(world, body1).add_dof_coupling(coupling); +} + +/// The couplings of the multibody containing `body`, as `(body1, body2, coeff)`. +fn couplings( + world: &PhysicsWorld, + body: RigidBodyHandle, +) -> Vec<(RigidBodyHandle, RigidBodyHandle, Real)> { + let mb = multibody_of(world, body); + mb.couplings() + .iter() + .map(|c| { + assert_eq!((c.dof1, c.dof2, c.axis1, c.axis2), (0, 0, 3, 3)); + ( + mb.link(c.link1).unwrap().rigid_body_handle(), + mb.link(c.link2).unwrap().rigid_body_handle(), + c.coeff, + ) + }) + .collect() +} + +#[test] +fn couplings_and_self_contacts_survive_topology_changes() { + let mut world = PhysicsWorld::new(); + + // Multibody A: a fixed base with two coupled sibling hinges. + let base = world.insert_body(RigidBodyBuilder::fixed()); + let a1 = insert_link(&mut world, 1.0); + let a2 = insert_link(&mut world, 2.0); + hinge(&mut world, base, a1); + hinge(&mut world, base, a2); + couple(&mut world, a1, a2, 1.0); + + // Link insertion: a new link hanging from `a2`. + let a3 = insert_link(&mut world, 3.0); + hinge(&mut world, a2, a3); + assert_eq!(couplings(&world, a1), vec![(a1, a2, 1.0)]); + + // Multibody B: a dynamic root with two coupled hinges and self-contacts disabled. + let b0 = insert_link(&mut world, 10.0); + let b1 = insert_link(&mut world, 11.0); + let b2 = insert_link(&mut world, 12.0); + hinge(&mut world, b0, b1); + hinge(&mut world, b1, b2); + couple(&mut world, b1, b2, -0.5); + multibody_of_mut(&mut world, b0).set_self_contacts_enabled(false); + assert!(multibody_of(&world, a1).self_contacts_enabled()); + + // Merge: B's root becomes a child of `a3`. + world + .multibody_joints + .insert(a3, b0, RevoluteJointBuilder::new(Vector::Z), true) + .unwrap(); + assert_eq!(couplings(&world, a1), vec![(a1, a2, 1.0), (b1, b2, -0.5)]); + assert!(!multibody_of(&world, a1).self_contacts_enabled()); + + // Split: detaching B again keeps each coupling on its side. + let (b0_joint, _, _) = world.multibody_joints.joint_between(a3, b0).unwrap(); + world.multibody_joints.remove(b0_joint, true); + assert_eq!(couplings(&world, a1), vec![(a1, a2, 1.0)]); + assert_eq!(couplings(&world, b1), vec![(b1, b2, -0.5)]); + assert!(!multibody_of(&world, b1).self_contacts_enabled()); + + // Detaching `a2` removes the joint owning one of the coupled DoFs: the coupling is dropped. + let (a2_joint, _, _) = world.multibody_joints.joint_between(base, a2).unwrap(); + world.multibody_joints.remove(a2_joint, true); + assert!(couplings(&world, a1).is_empty()); + assert!(couplings(&world, a2).is_empty()); + + // The simulation runs fine with the remapped couplings. + for _ in 0..10 { + world.step(); + } +} + +#[test] +fn remove_dof_couplings() { + let mut world = PhysicsWorld::new(); + let base = world.insert_body(RigidBodyBuilder::fixed()); + let l1 = insert_link(&mut world, 1.0); + let l2 = insert_link(&mut world, 2.0); + let l3 = insert_link(&mut world, 3.0); + hinge(&mut world, base, l1); + hinge(&mut world, base, l2); + hinge(&mut world, base, l3); + couple(&mut world, l1, l2, 1.0); + couple(&mut world, l1, l3, 2.0); + couple(&mut world, l2, l3, 3.0); + + let mb = multibody_of_mut(&mut world, l1); + assert_eq!(mb.remove_dof_coupling(1).unwrap().coeff, 2.0); + assert!(mb.remove_dof_coupling(2).is_none()); + assert_eq!(couplings(&world, l1), vec![(l1, l2, 1.0), (l2, l3, 3.0)]); + + let mb = multibody_of_mut(&mut world, l1); + mb.retain_dof_couplings(|c| c.coeff > 2.0); + assert_eq!(couplings(&world, l1), vec![(l2, l3, 3.0)]); + + multibody_of_mut(&mut world, l1).clear_dof_couplings(); + assert!(couplings(&world, l1).is_empty()); +} + +/// A coupling preserved across a split (removing an unrelated link) is still enforced. +#[test] +fn preserved_coupling_is_enforced() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let base = world.insert_body(RigidBodyBuilder::fixed()); + let l1 = insert_link(&mut world, 1.0); + let l2 = insert_link(&mut world, 2.0); + hinge(&mut world, base, l1); + hinge(&mut world, base, l2); + couple(&mut world, l1, l2, 1.0); + + // Topology changes after the coupling was declared. + let l3 = insert_link(&mut world, 3.0); + hinge(&mut world, l2, l3); + let (l3_joint, _, _) = world.multibody_joints.joint_between(l2, l3).unwrap(); + world.multibody_joints.remove(l3_joint, true); + assert_eq!(couplings(&world, l1), vec![(l1, l2, 1.0)]); + + let l1_id = link_id(&world, l1); + let mb = multibody_of_mut(&mut world, l1); + let joint = &mut mb.links_mut().nth(l1_id).unwrap().joint.data; + joint.set_motor_position(JointAxis::AngX, 0.5, 100.0, 20.0); + + for _ in 0..300 { + world.step(); + } + let q1 = world.bodies[l1].rotation().to_scaled_axis().z; + let q2 = world.bodies[l2].rotation().to_scaled_axis().z; + assert!((q1 - 0.5).abs() < 0.05, "q1 = {q1}"); + assert!((q2 - q1).abs() < 0.02, "q1 = {q1}, q2 = {q2}"); +} diff --git a/crates/rapier3d/tests/persistent_islands.rs b/crates/rapier3d/tests/persistent_islands.rs index ce828f3e5..c17685549 100644 --- a/crates/rapier3d/tests/persistent_islands.rs +++ b/crates/rapier3d/tests/persistent_islands.rs @@ -107,6 +107,53 @@ fn joint_links_and_split_after_removal() { ); } +/// Disabling a joint through `ImpulseJointSet::get_mut` must unlink it from the islands (so they +/// can split), and re-enabling it must link them again. +#[test] +fn joint_enable_toggle_unlinks_and_relinks() { + let mut world = world_with_ground(); + let a = insert_box(&mut world, 0.0, 0.5); + let b = insert_box(&mut world, 20.0, 0.5); + let joint = world.impulse_joints.insert( + a, + b, + RopeJointBuilder::new(30.0) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::ZERO), + true, + ); + world.step(); + assert!(same_island(&world, a, b)); + + for wake_up in [false, true] { + world + .impulse_joints + .get_mut(joint, wake_up) + .unwrap() + .data + .set_enabled(false); + for _ in 0..SETTLE_STEPS { + world.step(); + } + assert!( + !same_island(&world, a, b), + "a disabled joint must not keep the islands together" + ); + + world + .impulse_joints + .get_mut(joint, wake_up) + .unwrap() + .data + .set_enabled(true); + world.step(); + assert!( + same_island(&world, a, b), + "a re-enabled joint must merge islands" + ); + } +} + /// Removing the middle box of a touching row must split the sides apart. #[test] fn body_removal_splits_row() { diff --git a/crates/rapier3d/tests/pipeline_counters.rs b/crates/rapier3d/tests/pipeline_counters.rs new file mode 100644 index 000000000..22aed218c --- /dev/null +++ b/crates/rapier3d/tests/pipeline_counters.rs @@ -0,0 +1,80 @@ +//! `PhysicsPipeline::counters`: the contact pair, contact and constraint counts are filled by +//! the step, and the CCD substep count is zero unless the CCD actually ran. + +use rapier3d::prelude::*; + +fn world_with_ground() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(10.0, 0.5, 10.0).translation(Vector::new(0.0, -0.5, 0.0)), + ); + world +} + +#[test] +fn contact_and_constraint_counters() { + let mut world = world_with_ground(); + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 0.5, 0.0)), + ColliderBuilder::ball(0.5), + ); + let (cube, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(5.0, 0.5, 0.0)), + ColliderBuilder::cuboid(0.5, 0.5, 0.5), + ); + // A joint between the two resting bodies. + world.insert_impulse_joint( + ball, + cube, + SphericalJointBuilder::new() + .local_anchor1(Vector::new(2.5, 0.0, 0.0)) + .local_anchor2(Vector::new(-2.5, 0.0, 0.0)), + ); + + for _ in 0..5 { + world.step(); + let counters = &world.physics_pipeline.counters; + // The ball-ground and cube-ground pairs. + assert_eq!(counters.cd.ncontact_pairs, 2); + // One manifold per pair plus the joint. + assert_eq!(counters.solver.nconstraints, 3); + // One contact point for the ball, four for the cube's face. + assert_eq!(counters.solver.ncontacts, 5); + // No CCD-enabled body. + assert_eq!(counters.ccd.num_substeps, 0); + } + + // Once the scene sleeps, nothing is solved anymore (the pairs are still tracked). + for _ in 0..300 { + world.step(); + } + assert!(world.bodies[ball].is_sleeping()); + let counters = &world.physics_pipeline.counters; + assert_eq!(counters.cd.ncontact_pairs, 2); + assert_eq!(counters.solver.nconstraints, 0); + assert_eq!(counters.solver.ncontacts, 0); +} + +#[test] +fn ccd_substeps_counter() { + let mut world = world_with_ground(); + world.integration_parameters.max_ccd_substeps = 4; + world.gravity = Vector::ZERO; + + // A slow CCD-enabled body: the CCD never needs to act. + let (slow, _) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 5.0, 0.0)) + .ccd_enabled(true), + ColliderBuilder::ball(0.5), + ); + world.step(); + assert_eq!(world.physics_pipeline.counters.ccd.num_substeps, 0); + + // A fast one heading to the ground: the CCD splits the step. + world.bodies[slow].set_linvel(Vector::new(0.0, -500.0, 0.0), true); + world.step(); + assert!(world.physics_pipeline.counters.ccd.num_substeps >= 1); + assert!(world.bodies[slow].translation().y > -0.5); +} diff --git a/crates/rapier3d/tests/soft_body_clusters.rs b/crates/rapier3d/tests/soft_body_clusters.rs index 59a6d3d18..4020f9a4a 100644 --- a/crates/rapier3d/tests/soft_body_clusters.rs +++ b/crates/rapier3d/tests/soft_body_clusters.rs @@ -829,3 +829,371 @@ fn cluster_joined_outside_itself_splits_only_through_its_link() { assert!(sb.particles().iter().all(|p| p.position().is_finite())); } } + +/// Asserts that a collider sits at its parent proxy's pose composed with its local pose, and that +/// the broad phase has a leaf covering it there. +fn assert_rides_its_proxy(world: &PhysicsWorld, co: ColliderHandle, step: usize) { + let collider = &world.colliders[co]; + let proxy = &world.bodies[collider.parent().unwrap()]; + let expected = proxy.position() * collider.position_wrt_parent().unwrap(); + let actual = collider.position(); + let dt = (actual.translation - expected.translation).length(); + // Compares rotated axes: an `acos`-based angle is too noisy near zero in single precision. + let dr = [Vector::X, Vector::Y, Vector::Z] + .map(|axis| (actual.rotation * axis - expected.rotation * axis).length()) + .into_iter() + .fold(0.0, Real::max); + assert!( + dt < 1.0e-5 && dr < 1.0e-5, + "step {step}: collider {co:?} lags its proxy (translation error {dt}, rotation error {dr})" + ); + assert!( + world + .intersect_aabb_conservative(collider.compute_aabb(), QueryFilter::default()) + .any(|(handle, _)| handle == co), + "step {step}: the broad phase has no leaf covering collider {co:?}" + ); +} + +/// Rigid colliders hung with an offset on the whole-body proxy and on a sub-cluster proxy of a +/// falling, spinning jelly are at their proxy's pose at the end of every step, not one step behind. +#[test] +fn rigid_colliders_on_proxies_do_not_lag_their_proxy() { + let mut world = PhysicsWorld::new(); + let h = jelly(&mut world); + let top = particles_above(&world, h, 0.6); + let cluster = world + .soft_bodies + .add_cluster(h, &top, &mut world.bodies, &mut world.colliders) + .unwrap(); + let sb = &mut world.soft_bodies[h]; + let com = sb.center_of_mass(); + for i in 0..sb.num_particles() { + let arm = sb.particle_position(i) - com; + sb.set_particle_velocity( + i, + Vector::new(1.0, 0.0, 0.5) + Vector::new(0.5, 2.0, 1.0).cross(arm), + ); + } + let root = world.soft_bodies[h].root_body(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let offset = Pose::from_parts( + Vector::new(0.3, 0.2, -0.1), + Rotation::from_scaled_axis(Vector::new(0.2, 0.4, 0.1)), + ); + let colliders: Vec = [root, proxy] + .into_iter() + .map(|parent| { + world.colliders.insert_with_parent( + ColliderBuilder::cuboid(0.05, 0.05, 0.05) + .density(0.1) + .position(offset), + parent, + &mut world.bodies, + ) + }) + .collect(); + + for step in 0..30 { + world.step(); + for co in &colliders { + assert_rides_its_proxy(&world, *co, step); + } + } + // The proxies did move, so a lagging collider could not have passed. + assert!(world.bodies[root].translation().y < 0.0); +} + +/// Asserts that the broad-phase leaf of a collider contains the collider's current AABB: every +/// corner of that AABB hits the leaf. +fn assert_leaf_contains_current_aabb(world: &PhysicsWorld, co: ColliderHandle, step: usize) { + let aabb = world.colliders[co].compute_aabb(); + for i in 0..8 { + let pick = |bit: usize, min: Real, max: Real| if i & bit == 0 { min } else { max }; + let corner = Vector::new( + pick(1, aabb.mins.x, aabb.maxs.x), + pick(2, aabb.mins.y, aabb.maxs.y), + pick(4, aabb.mins.z, aabb.maxs.z), + ); + assert!( + world + .intersect_aabb_conservative(Aabb::new(corner, corner), QueryFilter::default()) + .any(|(handle, _)| handle == co), + "step {step}: the broad-phase leaf of collider {co:?} misses the corner {corner:?} of \ + its current AABB" + ); + } +} + +/// The surface collider of a soft cube falling faster at each step (the large timestep makes +/// gravity outrun the speculative margin) has a broad-phase leaf around its end-of-step geometry, +/// so a ray cast between steps hits its current bottom face. +#[test] +fn deformable_collider_leaves_follow_their_surface_at_the_end_of_each_step() { + let mut world = PhysicsWorld::new(); + world.integration_parameters.dt = 0.1; + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 1.0, 0.0), Vector::splat(0.5), 3, 3, 3) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.01); + let h = world.insert_soft_body(cube); + let surfaces: Vec = world.soft_bodies[h] + .meshes() + .map(|mesh| mesh.collider()) + .collect(); + assert!(!surfaces.is_empty()); + + for step in 0..20 { + world.step(); + for co in &surfaces { + assert_leaf_contains_current_aabb(&world, *co, step); + let aabb = world.colliders[*co].compute_aabb(); + // Off the surface's vertices and edges, which a ray may graze. + let center = aabb.center(); + let ray = Ray::new( + Vector::new(center.x + 0.13, aabb.mins.y - 0.01, center.z + 0.07), + Vector::Y, + ); + let hit = world.cast_ray(&ray, 0.02, true, QueryFilter::default()); + assert_eq!( + hit.map(|(handle, _)| handle), + Some(*co), + "step {step}: a ray cast misses the bottom face of collider {co:?}" + ); + } + } + // The cube did fall, so a stale leaf could not have passed. + let root = world.soft_bodies[h].root_body(); + assert!(world.bodies[root].translation().y < -10.0); +} + +/// Whether the broad-phase leaf of a collider reaches the point `p`. +fn leaf_reaches(world: &PhysicsWorld, co: ColliderHandle, p: Vector) -> bool { + world + .intersect_aabb_conservative(Aabb::new(p, p), QueryFilter::default()) + .any(|(handle, _)| handle == co) +} + +/// The points `dist` past the middle of each face of a collider's current AABB. +fn points_past_aabb(world: &PhysicsWorld, co: ColliderHandle, dist: Real) -> Vec { + let aabb = world.colliders[co].compute_aabb(); + let (center, half) = (aabb.center(), aabb.half_extents()); + (0..3) + .flat_map(|i| { + [-1.0, 1.0].map(|sign| { + let mut p = center; + p[i] += sign * (half[i] + dist); + p + }) + }) + .collect() +} + +/// The soft-body motion margin pads the deformable colliders of a fast jelly only: the rigid +/// colliders on its proxies get the broad-phase AABB of a collider on a dynamic body. +#[test] +fn only_deformable_colliders_are_padded_by_the_soft_motion_margin() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let h = jelly(&mut world); + let top = particles_above(&world, h, 0.6); + let cluster = world + .soft_bodies + .add_cluster(h, &top, &mut world.bodies, &mut world.colliders) + .unwrap(); + // A motion margin of about 0.67 per step. + let velocity = Vector::new(40.0, 0.0, 0.0); + let sb = &mut world.soft_bodies[h]; + for i in 0..sb.num_particles() { + sb.set_particle_velocity(i, velocity); + } + let surfaces: Vec = world.soft_bodies[h] + .meshes() + .map(|mesh| mesh.collider()) + .collect(); + assert!(!surfaces.is_empty()); + let shape = ColliderBuilder::cuboid(0.05, 0.05, 0.05) + .density(0.1) + .translation(Vector::new(0.3, 0.2, -0.1)); + let root = world.soft_bodies[h].root_body(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let mut rigid: Vec = [root, proxy] + .into_iter() + .map(|parent| { + world + .colliders + .insert_with_parent(shape.clone(), parent, &mut world.bodies) + }) + .collect(); + // The same collider on a dynamic body moving like the jelly, for reference. + let (_, reference) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 5.0, 0.0)) + .linvel(velocity), + shape, + ); + rigid.push(reference); + + let params = world.integration_parameters; + for step in 0..10 { + world.step(); + for co in &rigid { + let collider = &world.colliders[*co]; + assert_eq!( + collider.compute_broad_phase_aabb(¶ms, &world.bodies), + collider.compute_collision_aabb(params.prediction_distance() / 2.0), + "step {step}: collider {co:?} has a padded broad-phase AABB" + ); + for p in points_past_aabb(&world, *co, 0.2) { + assert!( + !leaf_reaches(&world, *co, p), + "step {step}: the broad-phase leaf of the rigid collider {co:?} reaches {p:?}" + ); + } + } + for co in &surfaces { + for p in points_past_aabb(&world, *co, 0.2) { + assert!( + leaf_reaches(&world, *co, p), + "step {step}: the broad-phase leaf of the surface {co:?} misses {p:?}" + ); + } + } + } + // The jelly kept its speed, so its margin stayed large. + assert!(world.bodies[root].translation().x > 5.0); +} + +/// Shoots a ball at a rigid ball hung on the root proxy of a jelly flying toward it (`on_proxy`), +/// or on a dynamic body moving like that jelly. Returns whether they touched and whether the +/// shot ended past the target. +fn shoot_at_a_moving_target(on_proxy: bool, speed: Real, ccd: bool) -> (bool, bool) { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let velocity = Vector::new(10.0, 0.0, 0.0); + let offset = Vector::new(1.2, 0.0, 0.0); + let target = ColliderBuilder::ball(0.5).density(0.1).translation(offset); + let target = if on_proxy { + let h = jelly(&mut world); + let sb = &mut world.soft_bodies[h]; + for i in 0..sb.num_particles() { + sb.set_particle_velocity(i, velocity); + } + let root = world.soft_bodies[h].root_body(); + world + .colliders + .insert_with_parent(target, root, &mut world.bodies) + } else { + let body = RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 0.6, 0.0)) + .linvel(velocity) + .additional_mass(2.7); + world.insert(body, target).1 + }; + let start = world.colliders[target].translation() + Vector::new(6.0, 0.0, 0.0); + let (shot, shot_co) = world.insert( + RigidBodyBuilder::dynamic() + .translation(start) + .linvel(Vector::new(-speed, 0.0, 0.0)) + .ccd_enabled(ccd), + ColliderBuilder::ball(0.1), + ); + + let mut touched = false; + for _ in 0..40 { + world.step(); + touched |= world + .narrow_phase + .contact_pair(shot_co, target) + .is_some_and(|pair| pair.has_any_active_contact()); + } + let passed = world.bodies[shot].translation().x < world.colliders[target].translation().x; + (touched, passed) +} + +/// A rigid collider on a fast soft body's proxy stops a fast ball like the same collider on a +/// dynamic body does, without the soft-body motion margin: discretely when each step's closing +/// travel is shorter than the colliders, through the ball's CCD otherwise. +#[test] +fn rigid_collider_on_a_fast_proxy_does_not_let_a_ball_through() { + for (speed, ccd) in [(20.0, false), (200.0, true)] { + for on_proxy in [false, true] { + let (touched, passed) = shoot_at_a_moving_target(on_proxy, speed, ccd); + assert!( + touched && !passed, + "speed {speed}, ccd {ccd}, on a proxy {on_proxy}: touched {touched}, passed {passed}" + ); + } + } +} + +/// The motion margin a collider's broad-phase AABB is padded with, past the prediction distance. +fn motion_margin(world: &PhysicsWorld, co: ColliderHandle) -> Real { + let params = world.integration_parameters; + let collider = &world.colliders[co]; + let padded = collider.compute_broad_phase_aabb(¶ms, &world.bodies); + let unpadded = collider.compute_collision_aabb(params.prediction_distance() / 2.0); + padded.half_extents().x - unpadded.half_extents().x +} + +/// A step split by the CCD sizes the soft-body motion margin with the full step's `dt`, not with +/// the length of its last CCD pass: the margin covers the coming step's travel. +#[test] +fn soft_motion_margin_spans_the_full_step_when_the_ccd_splits_it() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.max_ccd_substeps = 2; + let dt = world.integration_parameters.dt; + let h = jelly(&mut world); + let velocity = Vector::new(100.0, 0.0, 0.0); + let sb = &mut world.soft_bodies[h]; + for i in 0..sb.num_particles() { + sb.set_particle_velocity(i, velocity); + } + let surfaces: Vec = world.soft_bodies[h] + .meshes() + .map(|mesh| mesh.collider()) + .collect(); + assert!(!surfaces.is_empty()); + // A fast CCD ball hitting a wall late in the second step, far from the jelly: the first CCD + // pass stops at the impact and the last one only covers the rest of the step. + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -20.0, 0.0)), + ColliderBuilder::cuboid(0.1, 2.0, 2.0), + ); + let speed = 200.0; + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(-0.2 - 1.8 * speed * dt, -20.0, 0.0)) + .linvel(Vector::new(speed, 0.0, 0.0)) + .ccd_enabled(true), + ColliderBuilder::ball(0.1), + ); + + world.step(); + world.step(); + assert_eq!(world.physics_pipeline.counters.ccd.num_substeps, 2); + let max_speed = world.soft_bodies[h] + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + let expected = max_speed * dt; + assert!(expected > 1.5); + for co in &surfaces { + let margin = motion_margin(&world, *co); + assert!( + (margin - expected).abs() < 1.0e-3, + "surface {co:?}: margin {margin}, expected {expected}" + ); + for p in points_past_aabb(&world, *co, 0.9 * expected) { + assert!( + leaf_reaches(&world, *co, p), + "the broad-phase leaf of the surface {co:?} misses {p:?}" + ); + } + } +} diff --git a/crates/rapier3d/tests/zero_dt_step.rs b/crates/rapier3d/tests/zero_dt_step.rs new file mode 100644 index 000000000..221dea764 --- /dev/null +++ b/crates/rapier3d/tests/zero_dt_step.rs @@ -0,0 +1,58 @@ +//! Zero-length steps (`IntegrationParameters::dt == 0`, e.g. the first frame of a variable +//! timestep) with joint motors of infinite max force (as loaded from MJCF actuators): their +//! impulse bounds (`max_force * dt`) used to be NaN, panicking in the multibody motor constraint. + +use rapier3d::prelude::*; + +fn arm(world: &mut PhysicsWorld, x: Real, multibody: bool) -> RigidBodyHandle { + let base = world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(x, 2.0, 0.0))); + let (link, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(x + 1.0, 2.0, 0.0)), + ColliderBuilder::cuboid(0.5, 0.1, 0.1), + ); + let joint = RevoluteJointBuilder::new(Vector::Z) + .local_anchor2(Vector::new(-1.0, 0.0, 0.0)) + .motor_velocity(1.0, 1000.0) + .motor_max_force(Real::INFINITY); + if multibody { + world.insert_multibody_joint(base, link, joint).unwrap(); + } else { + world.insert_impulse_joint(base, link, joint); + } + link +} + +#[test] +fn zero_dt_step_with_infinite_motor_forces() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let multibody_link = arm(&mut world, 0.0, true); + let impulse_link = arm(&mut world, 5.0, false); + let before = [ + *world.bodies[multibody_link].position(), + *world.bodies[impulse_link].position(), + ]; + + world.integration_parameters.dt = 0.0; + world.step(); + world.step(); + // Nothing moves (up to the multibody forward kinematics' rounding). + for (link, pose) in [multibody_link, impulse_link].into_iter().zip(before) { + let rb = &world.bodies[link]; + assert!((rb.position().translation - pose.translation).length() < 1.0e-5); + assert!(rb.position().rotation.abs_diff_eq(pose.rotation, 1.0e-5)); + assert_eq!(rb.angvel(), Vector::ZERO); + } + + world.integration_parameters.dt = 1.0 / 60.0; + for _ in 0..30 { + world.step(); + } + for link in [multibody_link, impulse_link] { + let rb = &world.bodies[link]; + assert!(rb.position().translation.is_finite()); + assert!(rb.angvel().is_finite()); + // The motors drive the arms at their target velocity. + assert!((rb.angvel().z - 1.0).abs() < 0.05, "{}", rb.angvel()); + } +} diff --git a/examples2d/pin_slot_joint2.rs b/examples2d/pin_slot_joint2.rs index 251e21b57..294a56fb3 100644 --- a/examples2d/pin_slot_joint2.rs +++ b/examples2d/pin_slot_joint2.rs @@ -23,8 +23,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { /* * Character we will control manually. */ + let character_pos = Vector::new(0.0, 0.3); let rigid_body_character = - RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 0.3)); + RigidBodyBuilder::kinematic_position_based().translation(character_pos); let character_collider = ColliderBuilder::cuboid(0.15, 0.3); let (character_handle, _) = world.insert(rigid_body_character, character_collider); @@ -32,9 +33,13 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { * Tethered cube. */ let rad = 0.4; + let slot_axis = Vector::new(1.0, 1.0).normalize(); + let slot_anchor = Vector::new(2.0, 2.0); + let slot_limits = [-1.0, f32::INFINITY]; + // Start the cube on the slot's lower limit so the joint doesn't have to push it back. + let cube_pos = character_pos + slot_anchor + slot_axis * slot_limits[0] - Vector::new(0.0, rad); - let rigid_body_cube = - RigidBodyBuilder::new(RigidBodyType::Dynamic).translation(Vector::new(1.0, 1.0)); + let rigid_body_cube = RigidBodyBuilder::new(RigidBodyType::Dynamic).translation(cube_pos); let cube_collider = ColliderBuilder::cuboid(rad, rad); let (cube_handle, _) = world.insert(rigid_body_cube, cube_collider); @@ -42,7 +47,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { * SimdRotation axis indicator ball. */ let rigid_body_ball = - RigidBodyBuilder::new(RigidBodyType::Dynamic).translation(Vector::new(1.0, 1.0)); + RigidBodyBuilder::new(RigidBodyType::Dynamic).translation(cube_pos + Vector::new(0.0, rad)); let ball_collider = ColliderBuilder::ball(0.1); let (ball_handle, _) = world.insert(rigid_body_ball, ball_collider); @@ -51,18 +56,17 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { */ let fixed_joint = FixedJointBuilder::new() .local_anchor1(Vector::new(0.0, 0.0)) - .local_anchor2(Vector::new(0.0, -0.4)) + .local_anchor2(Vector::new(0.0, -rad)) .build(); world.insert_impulse_joint(cube_handle, ball_handle, fixed_joint); /* * Pin slot joint between cube and ground. */ - let axis = Vector::new(1.0, 1.0).normalize(); - let pin_slot_joint = PinSlotJointBuilder::new(axis) - .local_anchor1(Vector::new(2.0, 2.0)) - .local_anchor2(Vector::new(0.0, 0.4)) - .limits([-1.0, f32::INFINITY]) // Set the limits for the pin slot joint + let pin_slot_joint = PinSlotJointBuilder::new(slot_axis) + .local_anchor1(slot_anchor) + .local_anchor2(Vector::new(0.0, rad)) + .limits(slot_limits) // Set the limits for the pin slot joint .build(); world.insert_impulse_joint(character_handle, cube_handle, pin_slot_joint); diff --git a/examples3d/soft_joints3.rs b/examples3d/soft_joints3.rs index f127ec171..75c8964df 100644 --- a/examples3d/soft_joints3.rs +++ b/examples3d/soft_joints3.rs @@ -309,14 +309,18 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { /* * Kinematic cluster (no joint): a banner whose pinned top-edge cluster is waved rigidly. */ + let (banner_width, banner_height) = (14, 10); let banner = world.insert_soft_body(SoftBodyBuilder::cloth( Vector::new(-8.0, 3.2, 4.0), Vector::X * 0.18, Vector::Y * -0.18, - 14, - 10, + banner_width, + banner_height, )); - let top_edge: Vec = (0..14).collect(); + // Cloth particles are column-major: particle `i * banner_height` is at the top of column `i`. + let top_edge: Vec = (0..banner_width) + .map(|i| (i * banner_height) as u32) + .collect(); let banner_grip = world .add_soft_body_cluster(banner, &top_edge) .expect("banner grip cluster"); @@ -349,7 +353,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { // The same-body hinge flaps. if let Some(j) = world.impulse_joints.get_mut(flap_joint, true) { j.data - .set_motor_position(JointAxis::AngZ, 0.8 * (1.4 * t).sin(), 80.0, 10.0); + .set_motor_position(JointAxis::AngX, 0.8 * (1.4 * t).sin(), 80.0, 10.0); } // The banner's grip waves. let grip_pose = Pose::from_parts( diff --git a/src/counters/ccd_counters.rs b/src/counters/ccd_counters.rs index 74ee424de..24eabc351 100644 --- a/src/counters/ccd_counters.rs +++ b/src/counters/ccd_counters.rs @@ -5,6 +5,9 @@ use core::fmt::{Display, Formatter, Result}; #[derive(Default, Clone, Copy, Debug)] pub struct CCDCounters { /// The number of substeps actually performed by the CCD resolution. + /// + /// This is zero for the steps where the CCD didn't need to act (no CCD-enabled body moving + /// fast enough). pub num_substeps: usize, /// The total time spent for TOI computation in the CCD resolution. pub toi_computation_time: Timer, diff --git a/src/counters/collision_detection_counters.rs b/src/counters/collision_detection_counters.rs index 0c4d1d7c2..25336968a 100644 --- a/src/counters/collision_detection_counters.rs +++ b/src/counters/collision_detection_counters.rs @@ -4,7 +4,7 @@ use core::fmt::{Display, Formatter, Result}; /// Performance counters related to collision detection. #[derive(Default, Clone, Copy, Debug)] pub struct CollisionDetectionCounters { - /// Number of contact pairs detected. + /// Number of contact pairs tracked by the narrow-phase (touching or not). pub ncontact_pairs: usize, /// Time spent for the broad-phase of the collision detection. pub broad_phase_time: Timer, diff --git a/src/counters/solver_counters.rs b/src/counters/solver_counters.rs index 8ab90c877..c3375ab2c 100644 --- a/src/counters/solver_counters.rs +++ b/src/counters/solver_counters.rs @@ -4,9 +4,11 @@ use core::fmt::{Display, Formatter, Result}; /// Performance counters related to constraints resolution. #[derive(Default, Clone, Copy, Debug)] pub struct SolverCounters { - /// Number of constraints generated. + /// Number of contact manifolds and impulse joints handed to the constraints solver (summed + /// over the CCD substeps). Sleeping bodies' contacts and joints aren't counted. pub nconstraints: usize, - /// Number of contacts found. + /// Number of contact points handed to the constraints solver, soft-body contacts included + /// (summed over the CCD substeps). pub ncontacts: usize, /// Time spent for the resolution of the constraints (force computation). pub velocity_resolution_time: Timer, diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index f89daf01b..68e496f85 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -67,14 +67,18 @@ impl CCDSolver { // Default tier: every fast dynamic body is a CCD origin. `ccd_enabled` // no longer gates *activation*, only the sweep *scope* (fixed-only vs all bodies), // applied later during pair selection. - if rb.is_dynamic() { + if rb.is_soft_frame() { + // A cluster proxy's pose is derived from its particles: it is never swept. + rb.ccd.ccd_active = false; + } else if rb.is_dynamic() { let moving_fast = if include_forces { - // Pre-solve (substep splitter): `next_position` isn't solved yet, use - // the velocity-based estimate including forces. + // Pre-solve (substep splitter): `next_position` isn't solved yet, use the + // current velocity with forces, like `find_first_impact`. The last solved + // motion (`ccd_vels`) is stale, or zero on a body's first step. rb.ccd.is_moving_fast( dt, - &rb.ccd_vels, - Some(&rb.forces), + &rb.vels, + Some((&rb.forces, &rb.mprops)), rb.mprops.max_extent(), ) } else { diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 8f8ba431a..2d8b7746d 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -200,6 +200,11 @@ pub struct IntegrationParameters { /// - 120 FPS: `1.0 / 120.0` ≈ 0.0083 seconds /// /// Smaller timesteps are more accurate but require more CPU time per second of simulated time. + /// + /// With a zero (or negative) `dt`, [`PhysicsPipeline::step`](crate::pipeline::PhysicsPipeline::step) + /// only applies the user changes and updates the collision detection (contacts, collision + /// events, scene queries): no time passes, so no body moves (kinematic bodies included) and + /// velocities are left untouched. pub dt: Real, /// Minimum timestep size when using CCD with multiple substeps (default: `1.0 / 60.0 / 100.0`). /// diff --git a/src/dynamics/island_manager/manager.rs b/src/dynamics/island_manager/manager.rs index afa81e262..c80e1b489 100644 --- a/src/dynamics/island_manager/manager.rs +++ b/src/dynamics/island_manager/manager.rs @@ -133,10 +133,11 @@ impl IslandManager { // Non-fixed enabled endpoints must be registered in the active set. (Two awake // touching bodies sharing an island is structural: there is at most one awake - // island.) + // island.) An island manager without any island tracks no body at all, like the + // one driven by the `CollisionPipeline` which never registers bodies. #[cfg(debug_assertions)] for handle in [handle1, handle2].into_iter().flatten() { - if let Some(rb) = bodies.get(handle) { + if let Some(rb) = bodies.get(handle).filter(|_| !self.islands.is_empty()) { debug_assert!( rb.is_fixed() || !rb.is_enabled() || rb.ids.active_island_id != u32::MAX ); diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index e814f0c24..b1c6d2658 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -244,9 +244,17 @@ impl JointMotor { // keep_lhs, target_pos: self.target_pos, target_vel: self.target_vel, - max_impulse: self.max_force * dt, + max_impulse: Self::max_impulse(self.max_force, dt), } } + + /// The largest impulse a motor with the given max force can apply during `dt`. + /// + /// This is zero if `dt` is zero, even for an infinite `max_force` (whose product with `dt` + /// would be NaN). + pub(crate) fn max_impulse(max_force: Real, dt: Real) -> Real { + if dt == 0.0 { 0.0 } else { max_force * dt } + } } #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] @@ -856,3 +864,25 @@ impl From for GenericJoint { val.0 } } + +#[cfg(all(test, feature = "alloc"))] +mod test { + use super::JointMotor; + use crate::math::Real; + + #[test] + fn infinite_motor_force_with_zero_dt_has_finite_impulse_bounds() { + let motor = JointMotor { + max_force: Real::INFINITY, + target_vel: 1.0, + damping: 1.0, + ..Default::default() + }; + assert_eq!(motor.motor_params(0.0).max_impulse, 0.0); + assert_eq!(motor.motor_params(1.0 / 60.0).max_impulse, Real::INFINITY); + + let motor = JointMotor::default(); + assert_eq!(motor.motor_params(0.0).max_impulse, 0.0); + assert_eq!(motor.motor_params(0.5).max_impulse, Real::MAX * 0.5); + } +} diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index c9974b330..f1eb00a03 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -55,6 +55,13 @@ pub struct ImpulseJointSet { /// drained at the start of the next timestep, in order. #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) island_events: Vec, + /// Joints accessed mutably since the last timestep: their enabled status may have changed, + /// so their island links are refreshed by [`Self::flush_modified_joints`]. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + modified_joints: Vec, + /// Set by [`Self::iter_mut`]: every joint counts as modified. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + all_joints_modified: bool, /// Bumped by every mutation that can affect the solver's joint constraint assembly (joint /// insertion/removal, mutable joint access, user-changes to a rigid-body with attached joints). /// The solver reuses its joint assembly while this, the joint list, and the island epoch are unchanged. @@ -77,6 +84,8 @@ impl ImpulseJointSet { to_wake_up: HashSet::default(), to_join: HashSet::default(), island_events: Vec::new(), + modified_joints: Vec::new(), + all_joints_modified: false, assembly_epoch: 0, selection_epochs: None, } @@ -239,6 +248,7 @@ impl ImpulseJointSet { ) -> Option<&mut ImpulseJoint> { self.bump_assembly_epoch(); let id = self.joint_ids.get(handle.0)?; + self.modified_joints.push(handle); let joint = self.joint_graph.graph.edge_weight_mut(*id); if wake_up_connected_bodies { if let Some(joint) = &joint { @@ -270,6 +280,7 @@ impl ImpulseJointSet { ) -> Option<(&mut ImpulseJoint, ImpulseJointHandle)> { self.bump_assembly_epoch(); let (id, handle) = self.joint_ids.get_unknown_gen(i)?; + self.modified_joints.push(ImpulseJointHandle(handle)); Some(( self.joint_graph.graph.edge_weight_mut(*id)?, ImpulseJointHandle(handle), @@ -292,6 +303,7 @@ impl ImpulseJointSet { /// Each iteration yields `(joint_handle, &mut joint)`. pub fn iter_mut(&mut self) -> impl Iterator { self.bump_assembly_epoch(); + self.all_joints_modified = true; self.joint_graph .graph .edges @@ -299,6 +311,47 @@ impl ImpulseJointSet { .map(|e| (e.weight.handle, &mut e.weight)) } + /// Queues an island link (or unlink) for each joint accessed mutably since the last call, + /// matching its current enabled status, so enabling or disabling a joint merges or splits + /// islands. Both events are no-ops for a joint already in that state. + pub(crate) fn flush_modified_joints(&mut self) { + let mut handles = core::mem::take(&mut self.modified_joints); + let all = core::mem::take(&mut self.all_joints_modified); + let graph = &self.joint_graph.graph; + let island_events = &mut self.island_events; + let mut push_event = |joint: &ImpulseJoint| { + island_events.push(if joint.data.is_enabled() { + crate::dynamics::ImpulseJointIslandEvent::Link { + handle: joint.handle, + body1: joint.body1, + body2: joint.body2, + } + } else { + crate::dynamics::ImpulseJointIslandEvent::Unlink { + handle: joint.handle, + } + }); + }; + + if all { + graph.edges.iter().for_each(|edge| push_event(&edge.weight)); + } else { + for handle in &handles { + if let Some(joint) = self + .joint_ids + .get(handle.0) + .and_then(|id| graph.edge_weight(*id)) + { + push_event(joint); + } + } + } + + // Keep the allocation. + handles.clear(); + self.modified_joints = handles; + } + pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] { &mut self.joint_graph.graph.edges[..] } diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 6fc587f9d..e71d70bd4 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -188,6 +188,8 @@ impl Multibody { let mut result = vec![]; let mut link2mb = vec![usize::MAX; self.links.len()]; let mut link_id2new_id = vec![usize::MAX; self.links.len()]; + // Links whose joint is replaced by a fixed one: their DoFs no longer exist. + let mut joint_replaced = vec![false; self.links.len()]; // Split multibody and update the set of links and ndofs. for (i, mut link) in self.links.0.into_iter().enumerate() { @@ -210,6 +212,7 @@ impl Multibody { if is_new_root { let joint = MultibodyJoint::fixed(*link.local_to_world()); link.joint = joint; + joint_replaced[i] = true; } curr_mb.ndofs += link.joint().ndofs(); @@ -255,6 +258,19 @@ impl Multibody { } } + // Keep the couplings whose DoFs still exist and ended up in the same multibody. + for coupling in &self.couplings { + let (l1, l2) = (coupling.link1, coupling.link2); + let kept = |l: usize| link_id2new_id[l] != usize::MAX && !joint_replaced[l]; + if kept(l1) && kept(l2) && link2mb[l1] == link2mb[l2] { + result[link2mb[l1]].couplings.push(MultibodyDofCoupling { + link1: link_id2new_id[l1], + link2: link_id2new_id[l2], + ..*coupling + }); + } + } + result } @@ -314,6 +330,20 @@ impl Multibody { self.links.append(&mut rhs.links); self.ndofs = self.velocities.len(); self.workspace.resize(self.links.len(), self.ndofs); + + // The rhs couplings follow its links, except the ones on its root whose joint was replaced. + self.couplings.extend( + rhs.couplings + .iter() + .filter(|c| c.link1 != 0 && c.link2 != 0) + .map(|c| MultibodyDofCoupling { + link1: c.link1 + base_internal_id, + link2: c.link2 + base_internal_id, + ..*c + }), + ); + // Self-contacts stay disabled for the links of a multibody that had them disabled. + self.self_contacts_enabled &= rhs.self_contacts_enabled; } /// Whether self-contacts are enabled on this multibody. @@ -1095,10 +1125,31 @@ impl Multibody { } /// The DoF couplings declared on this multibody. + /// + /// Couplings follow the multibody's topology changes (link insertions, merges and splits); + /// a coupling is dropped once one of its DoFs no longer exists, or when its two DoFs end up + /// in different multibodies. pub fn couplings(&self) -> &[MultibodyDofCoupling] { &self.couplings } + /// Removes the `i`-th DoF coupling (in the order of [`Self::couplings`]) and returns it. + /// + /// Returns `None` if there is no such coupling. The couplings after it shift down by one. + pub fn remove_dof_coupling(&mut self, i: usize) -> Option { + (i < self.couplings.len()).then(|| self.couplings.remove(i)) + } + + /// Keeps only the DoF couplings for which `f` returns `true`. + pub fn retain_dof_couplings(&mut self, f: impl FnMut(&MultibodyDofCoupling) -> bool) { + self.couplings.retain(f); + } + + /// Removes all the DoF couplings of this multibody. + pub fn clear_dof_couplings(&mut self) { + self.couplings.clear(); + } + /// The number of dry-friction rows `link_id`'s joint will emit: one per /// free DoF whose `frictionloss` entry is non-zero. pub(crate) fn num_friction_constraints(&self, link_id: usize) -> usize { diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index abab5bf6e..94758b97e 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -76,8 +76,9 @@ pub struct RigidBody { /// (`u32::MAX` for regular rigid bodies). #[cfg_attr(feature = "serde-serialize", serde(default = "invalid_soft_cluster"))] pub(crate) soft_cluster: u32, - /// The speculative margin of a soft-frame proxy's colliders, set by the soft-body step: the - /// farthest particle travel of its soft body over the coming step (zero for regular bodies). + /// The speculative margin of a soft-frame proxy's deformable colliders, set by the soft-body + /// step: the farthest particle travel of its soft body over the coming step (zero for + /// regular bodies). pub(crate) soft_motion_margin: Real, /// User-defined data associated to this rigid-body. pub user_data: u128, @@ -1559,7 +1560,7 @@ impl RigidBody { /// When enabled, rapidly spinning objects resist rotation axis changes (like gyroscopes). /// Examples: spinning tops, flywheels, rotating spacecraft. /// - /// **Default**: Disabled (costs performance, rarely needed in games). + /// **Default**: Enabled. Disabling it saves a slight performance overhead. #[cfg(feature = "dim3")] pub fn enable_gyroscopic_forces(&mut self, enabled: bool) { self.forces.gyroscopic_forces_enabled = enabled; @@ -1690,7 +1691,7 @@ pub struct RigidBodyBuilder { /// /// See [`RigidBody::set_additional_pgs_iterations`] for additional information. pub additional_pgs_iterations: usize, - /// Are gyroscopic forces enabled for this rigid-body? + /// Are gyroscopic forces enabled for this rigid-body? (default: `true`) pub gyroscopic_forces_enabled: bool, } @@ -2112,7 +2113,7 @@ impl RigidBodyBuilder { /// Enabling gyroscopic forces allows more realistic behaviors like gyroscopic precession, /// but result in a slight performance overhead. /// - /// Disabled by default. + /// Enabled by default. #[cfg(feature = "dim3")] pub fn gyroscopic_forces_enabled(mut self, enabled: bool) -> Self { self.gyroscopic_forces_enabled = enabled; diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index 576c81cb4..abc2247d9 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -1127,16 +1127,12 @@ impl RigidBodyCcd { &self, dt: Real, vels: &RigidBodyVelocity, - forces: Option<&RigidBodyForces>, + forces: Option<(&RigidBodyForces, &RigidBodyMassProps)>, max_extent: Real, ) -> bool { - let max_point_velocity = if let Some(forces) = forces { - let linear_part = (vels.linvel + forces.force * dt).length(); - #[cfg(feature = "dim2")] - let angular_part = (vels.angvel + forces.torque * dt).abs() * max_extent; - #[cfg(feature = "dim3")] - let angular_part = (vels.angvel + forces.torque * dt).length() * max_extent; - linear_part + angular_part + // Same velocity prediction as the CCD sweep: forces divided by the mass and inertia. + let max_point_velocity = if let Some((forces, mprops)) = forces { + self.max_point_velocity(&forces.integrate(dt, vels, mprops), max_extent) } else { self.max_point_velocity(vels, max_extent) }; @@ -1511,6 +1507,33 @@ mod tests { use super::*; use crate::math::Real; + #[test] + fn heavy_body_under_gravity_is_not_moving_fast() { + let (dt, mass, extent) = (1.0 / 60.0, 1000.0, 0.5); + #[cfg(feature = "dim2")] + let local_mprops = MassProperties::new(Vector::ZERO, mass, 1.0); + #[cfg(feature = "dim3")] + let local_mprops = MassProperties::new(Vector::ZERO, mass, Vector::splat(1.0)); + let mut mprops = RigidBodyMassProps::from(local_mprops); + mprops.update_world_mass_properties(RigidBodyType::Dynamic, &Pose::default()); + let mut forces = RigidBodyForces::default(); + forces.compute_effective_force_and_torque(Vector::Y * -9.81, Vector::splat(mass)); + let ccd = RigidBodyCcd { + ccd_thickness: extent, + ..Default::default() + }; + + // Gravity alone moves a resting body by about 0.003 in one step, whatever its mass. + let resting = RigidBodyVelocity::default(); + assert!(!ccd.is_moving_fast(dt, &resting, Some((&forces, &mprops)), extent)); + + let fast = RigidBodyVelocity { + linvel: Vector::X * 60.0, + ..Default::default() + }; + assert!(ccd.is_moving_fast(dt, &fast, Some((&forces, &mprops)), extent)); + } + #[test] fn test_interpolate_velocity() { // Interpolate and then integrate the velocity to see if diff --git a/src/dynamics/soft_body/soft_body.rs b/src/dynamics/soft_body/soft_body.rs index f1d4c3467..597a6d3f7 100644 --- a/src/dynamics/soft_body/soft_body.rs +++ b/src/dynamics/soft_body/soft_body.rs @@ -126,8 +126,13 @@ pub struct SoftBody { /// Largest normal approach speed of the rigid bodies met by the surface, for the last step /// and the one before (`None`: no contact constraint that step); with the particle speeds, it drives /// the impact-adaptive substeps (`IntegrationParameters::soft_bodies.max_extra_substeps`). + /// A step split into several CCD passes counts once, with the largest speed of its passes. #[cfg_attr(feature = "serde-serialize", serde(default))] pub(crate) contact_approach_speeds: [Option; 2], + /// Set by the first solve of a step, which shifted `contact_approach_speeds`: the step's later + /// CCD passes merge into its entry. Cleared by the end-of-step sync. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) contact_approach_step_open: bool, /// Total normal impulse of the surface's contact constraints over the last step, and the extra /// substeps currently requested from that load (see `sync_particle_positions`). #[cfg_attr(feature = "serde-serialize", serde(default))] diff --git a/src/dynamics/soft_body/soft_body_accessors.rs b/src/dynamics/soft_body/soft_body_accessors.rs index c2716a355..b73349087 100644 --- a/src/dynamics/soft_body/soft_body_accessors.rs +++ b/src/dynamics/soft_body/soft_body_accessors.rs @@ -206,9 +206,14 @@ impl SoftBody { self.particle_radius } - /// The hidden rigid body standing for this soft body in the islands and holding its colliders - /// (invalid until inserted in a set). Never move, remove or attach joints to it; it only - /// serves to recognize or exclude the soft body's colliders in queries and events. + /// The rigid body standing for this soft body in the islands (invalid until inserted in a + /// set): the proxy of its first live cluster, initially the whole-body cluster holding all of + /// its colliders. + /// + /// Impulse joints can be attached to it to act on the whole soft body. A tear splitting the + /// body moves each of them to the piece closest to its anchor in the rest shape (see + /// [`crate::dynamics::SoftBodyTearEvent::moved_joints`]), and a piece's root body may be a + /// different rigid body. Never move or remove it: its pose is driven by the particles. pub fn root_body(&self) -> RigidBodyHandle { self.root_body } diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_build.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_build.rs index 53e818f11..5ff44a58b 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_build.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_build.rs @@ -243,6 +243,7 @@ impl SoftBodyBuilder { tearing_pending: false, topology_version: 0, contact_approach_speeds: [None; 2], + contact_approach_step_open: false, contact_load: 0.0, load_extra_substeps: 0, origin: None, diff --git a/src/dynamics/soft_body/soft_body_set/soft_body_set_insert_remove.rs b/src/dynamics/soft_body/soft_body_set/soft_body_set_insert_remove.rs index 2a7c0a38e..7c459912d 100644 --- a/src/dynamics/soft_body/soft_body_set/soft_body_set_insert_remove.rs +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_insert_remove.rs @@ -63,6 +63,7 @@ impl SoftBodySet { tearing_pending: false, topology_version: 0, contact_approach_speeds: [None; 2], + contact_approach_step_open: false, contact_load: 0.0, load_extra_substeps: 0, origin: None, diff --git a/src/dynamics/soft_body/soft_body_set/soft_body_set_proxies.rs b/src/dynamics/soft_body/soft_body_set/soft_body_set_proxies.rs index 783048cde..2a0f5e07e 100644 --- a/src/dynamics/soft_body/soft_body_set/soft_body_set_proxies.rs +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_proxies.rs @@ -197,8 +197,8 @@ pub(super) fn spawn_proxy( /// What `SoftBodySet::sync_particle_positions` computed for one soft body, applied to the shared /// sets afterwards. pub(super) struct SyncOutcome { - /// The speculative margin of the body's colliders for the coming step (`None`: asleep, left - /// alone). + /// The speculative margin of the body's deformable colliders for the coming step (`None`: + /// asleep, left alone). pub(super) margin: Option, /// The substep request written on the body's root body. pub(super) additional_solver_iterations: usize, @@ -207,14 +207,19 @@ pub(super) struct SyncOutcome { } /// The per-body part of `SoftBodySet::sync_particle_positions`: updates the state derived -/// from the particle positions, then computes the colliders' speculative margin and the substep -/// request of an active body (`inactive`: asleep or disabled). +/// from the particle positions, then computes the deformable colliders' speculative margin and +/// the substep request of an active body (`inactive`: asleep or disabled). `params` are the full +/// step's; `last_pass_dt` is the length of its last CCD pass, over which the contact load was summed. pub(super) fn sync_soft_body( sb: &mut SoftBody, inactive: bool, params: &IntegrationParameters, + last_pass_dt: Real, ) -> SyncOutcome { + // The coming step's length: the margin and the impact substeps predict its travel. let dt = params.dt; + // The next step's first solve starts a new contact history entry. + sb.contact_approach_step_open = false; // Inactive: asleep, or disabled (then not asleep, just out of the simulation). let sleeping = inactive && sb.enabled; if sb.is_finite() { @@ -285,7 +290,8 @@ pub(super) fn sync_soft_body( // as 10 length units/s^2), released with hysteresis at half those loads so the island's // partition does not flicker. let mass: Real = sb.particles.iter().map(|p| p.mass).sum(); - let load = sb.contact_load / (dt * mass.max(1.0e-9) * 10.0 * params.length_unit); + // The load is the impulse of the last pass's solve: averaged over that pass. + let load = sb.contact_load / (last_pass_dt * mass.max(1.0e-9) * 10.0 * params.length_unit); let load_extra = match sb.load_extra_substeps { 0 if load > 20.0 => 1, 1 if load > 60.0 => 2, diff --git a/src/dynamics/soft_body/soft_body_set/soft_body_set_split.rs b/src/dynamics/soft_body/soft_body_set/soft_body_set_split.rs index 0d3cdd406..78d348d12 100644 --- a/src/dynamics/soft_body/soft_body_set/soft_body_set_split.rs +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_split.rs @@ -521,6 +521,7 @@ fn extract_piece( body.modified = true; body.tearing_pending = false; body.contact_approach_speeds = [None; 2]; + body.contact_approach_step_open = false; body.contact_load = 0.0; body.load_extra_substeps = 0; (body, moves, remap) diff --git a/src/dynamics/soft_body/soft_body_set/soft_body_set_step_sync.rs b/src/dynamics/soft_body/soft_body_set/soft_body_set_step_sync.rs index dede1e918..24130b246 100644 --- a/src/dynamics/soft_body/soft_body_set/soft_body_set_step_sync.rs +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_step_sync.rs @@ -7,7 +7,7 @@ use crate::dynamics::soft_body::{SoftBody, SoftBodyHandle, SoftBodyTearEvent}; use crate::dynamics::{ ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, RigidBodySet, }; -use crate::geometry::ColliderSet; +use crate::geometry::{ColliderHandle, ColliderPosition, ColliderSet}; use crate::math::{DIM, Pose, Real, Vector}; use crate::pipeline::EventHandler; #[cfg(not(feature = "std"))] @@ -76,7 +76,8 @@ impl SoftBodySet { if let Some(rb) = bodies.get_mut_internal(cluster.proxy()) { rb.soft_motion_margin = rb.soft_motion_margin.max(margin); } - // Flagged as modified so the broad phase updates their AABBs with the raised margin. + // The margin only pads the deformable colliders: flagged as modified so + // the broad phase updates their AABBs with it. for mesh in cluster.meshes() { let _ = colliders.get_mut(mesh.collider()); } @@ -128,8 +129,9 @@ impl SoftBodySet { // bit-for-bit, so the frozen contact anchors are not re-linearized by fit noise. let prev = rb.pos.position; let dt = (frame.pose.translation - prev.translation).length(); + // The 2D angle is signed: take its magnitude so either direction leaves the dead zone. #[cfg(feature = "dim2")] - let dr = frame.pose.rotation.angle_between(&prev.rotation); + let dr = frame.pose.rotation.angle_between(&prev.rotation).abs(); #[cfg(feature = "dim3")] let dr = frame.pose.rotation.angle_between(prev.rotation); if dt < 1.0e-6 && dr < 1.0e-6 { @@ -192,6 +194,29 @@ impl SoftBodySet { colliders: &mut ColliderSet, params: &IntegrationParameters, quarantined: &mut Vec, + ) { + self.sync_particle_positions_and_collect( + bodies, + colliders, + params, + params.dt, + quarantined, + &mut Vec::new(), + ); + } + + /// [`Self::sync_particle_positions`], also collecting into `synced_colliders` the deformed + /// surface colliders and the rigid colliders moved to their proxy's fresh pose, whose + /// broad-phase AABBs must follow. `params` are the full step's (sizing the coming step's + /// margin and substeps); `last_pass_dt` is the length of the step's last CCD pass. + pub(crate) fn sync_particle_positions_and_collect( + &mut self, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + params: &IntegrationParameters, + last_pass_dt: Real, + quarantined: &mut Vec, + synced_colliders: &mut Vec, ) { // Every body reads its own particles and writes only itself: updated in parallel, then // the writes to the shared sets are applied in body order. Disabled bodies are left @@ -209,8 +234,9 @@ impl SoftBodySet { }) .collect(); let mut outcomes = Vec::with_capacity(soft_bodies.len()); - let sync = - |(sb, inactive): &mut (&mut SoftBody, bool)| sync_soft_body(sb, *inactive, params); + let sync = |(sb, inactive): &mut (&mut SoftBody, bool)| { + sync_soft_body(sb, *inactive, params, last_pass_dt) + }; #[cfg(feature = "parallel")] { use rayon::prelude::*; @@ -255,18 +281,29 @@ impl SoftBodySet { co.deform_pose(frame); let pose = *co.position(); co.deform_shape(|shape| mesh.deform_shape(sb, &pose, shape)); + synced_colliders.push(mesh.collider()); } } - // The proxy's colliders (its meshes and the rigid colliders a user hung on it) ride - // the cluster's frame, which moves with the particles: they share its speculative margin. + // The speculative margin pads the proxy's deformable colliders only: the rigid + // colliders a user hung on it get the AABB of any dynamic body's collider. let Some(rb) = bodies.get_mut_internal(cluster.proxy()) else { continue; }; rb.soft_motion_margin = margin; - // Flagged as modified so the broad phase updates their AABBs with the new margin. - let proxy_colliders = rb.colliders().to_vec(); - for handle in proxy_colliders { - let _ = colliders.get_mut(handle); + let proxy_pose = rb.pos.position; + // Flagged as modified: the meshes were deformed and the rigid ones move below. + for handle in rb.colliders() { + let Some(co) = colliders.get_mut(*handle) else { + continue; + }; + // The rigid ones follow its fresh pose: the end-of-step advance leaves the + // proxies' colliders to this sync. + if !co.is_deformable_collider() { + if let Some(parent) = co.parent.as_ref() { + co.pos = ColliderPosition(proxy_pose * parent.pos_wrt_parent); + synced_colliders.push(*handle); + } + } } } } diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 142048229..924f52c8e 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -440,7 +440,12 @@ impl JointConstraintBuilderSimd { cfm_gain, target_pos: self.motor_target_pos, target_vel: self.motor_target_vel, - max_impulse: self.motor_max_force * dt, + // See `JointMotor::max_impulse`: an infinite max force times a zero dt is NaN. + max_impulse: if params.dt == 0.0 { + zero + } else { + self.motor_max_force * dt + }, } }); #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_writeback.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_writeback.rs index 49bc33eb0..c3730e3e8 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_writeback.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_writeback.rs @@ -187,8 +187,19 @@ impl SoftConstraintsSet { } sb.plastic_flowing = awake.plastic_flow.load(Ordering::Relaxed); sb.tearing_pending |= awake.torn.load(Ordering::Relaxed); - sb.contact_approach_speeds = - [awake.contact_approach_speed, sb.contact_approach_speeds[0]]; + // One history entry per step: its first pass shifts the history, its later CCD + // passes keep the largest approach speed. + let speed = awake.contact_approach_speed; + if sb.contact_approach_step_open { + let entry = &mut sb.contact_approach_speeds[0]; + *entry = match (*entry, speed) { + (Some(a), Some(b)) => Some(a.max(b)), + (a, b) => a.or(b), + }; + } else { + sb.contact_approach_speeds = [speed, sb.contact_approach_speeds[0]]; + sb.contact_approach_step_open = true; + } // Summed by the contact writeback that follows the solve. sb.contact_load = 0.0; } diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints_set.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints_set.rs index b84e09462..d3b5681e5 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints_set.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints_set.rs @@ -45,7 +45,7 @@ pub(crate) struct AwakeSoftBody { /// Set by the writeback stage when an element was strained past the material's tear /// strain (the tearing pass at the end of the step removes it). pub torn: AtomicBool, - /// Largest normal approach speed of the rigid bodies met by the surface this step (`None`: + /// Largest normal approach speed of the rigid bodies met by the surface this pass (`None`: /// no contact constraint at all), set by the contact assembly. pub contact_approach_speed: Option, /// The body's shape-matched clusters this step, with their warm-started fit rotation: diff --git a/src/dynamics/solver/staged_island_solver/init.rs b/src/dynamics/solver/staged_island_solver/init.rs index 476deff93..710c186c7 100644 --- a/src/dynamics/solver/staged_island_solver/init.rs +++ b/src/dynamics/solver/staged_island_solver/init.rs @@ -597,6 +597,18 @@ impl StagedIslandSolver { .any(|(b, c)| b.has_bouncy_seed(c.num_contacts)); self.any_bouncy.store(generic_bouncy, Ordering::Relaxed); + // Constraint statistics, summed over the CCD substeps. + if counters.enabled { + let num_rigid_contacts: usize = graph + .buckets() + .flat_map(|(_, refs)| refs) + .chain(graph.generic()) + .map(|r| store.get(*r).data.solver_contacts.len()) + .sum(); + counters.solver.nconstraints += graph.len() + joint_indices.len(); + counters.solver.ncontacts += num_rigid_contacts + self.soft_constraints.contacts.len(); + } + counters.solver.velocity_assembly_time.pause(); counters.solver.velocity_resolution_time.resume(); diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index a1c2f8e45..6a7b9650d 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -82,6 +82,16 @@ impl Collider { self.deformable_mesh_ref.is_some() } + /// The soft-body motion margin padding this collider's broad-phase AABB and contact + /// prediction: its parent proxy's for a deformable collider, zero for a rigid one. + pub(crate) fn soft_motion_margin(&self, parent: &crate::dynamics::RigidBody) -> Real { + if self.is_deformable_collider() { + parent.soft_motion_margin + } else { + 0.0 + } + } + /// Deforms the shape of a soft-body surface collider in place through `f` without /// invalidating the narrow-phase pair workspaces (its topology and identity are unchanged). pub(crate) fn deform_shape(&mut self, f: impl FnOnce(&mut dyn Shape)) { @@ -625,7 +635,7 @@ impl Collider { ) * p.pos_wrt_parent }) }); - let soft_motion_margin = parent.map_or(0.0, |(_, parent)| parent.soft_motion_margin); + let soft_motion_margin = parent.map_or(0.0, |(_, parent)| self.soft_motion_margin(parent)); let prediction_distance = params.prediction_distance(); let mut aabb = self.compute_collision_aabb(prediction_distance / 2.0 + soft_motion_margin); diff --git a/src/geometry/narrow_phase/pair_update.rs b/src/geometry/narrow_phase/pair_update.rs index 10f9b8c3c..f4cc5de41 100644 --- a/src/geometry/narrow_phase/pair_update.rs +++ b/src/geometry/narrow_phase/pair_update.rs @@ -317,10 +317,11 @@ pub(super) fn process_pair( let pos12 = co1.pos.inv_mul(&co2.pos); - // Soft bodies add the farthest motion of their particles over the step (speculative - // contacts instead of continuous collision detection for deformable geometry). - let soft_margin1 = rb1.map_or(0.0, |rb| rb.soft_motion_margin); - let soft_margin2 = rb2.map_or(0.0, |rb| rb.soft_motion_margin); + // Soft-body surfaces add the farthest motion of their particles over the step (speculative + // contacts instead of continuous collision detection for deformable geometry). Rigid + // colliders on cluster proxies get no margin, like colliders on dynamic bodies. + let soft_margin1 = rb1.map_or(0.0, |rb| co1.soft_motion_margin(rb)); + let soft_margin2 = rb2.map_or(0.0, |rb| co2.soft_motion_margin(rb)); let soft_body_prediction = soft_margin1 + soft_margin2; let contact_skin_sum = co1.contact_skin() + co2.contact_skin(); let soft_ccd_prediction1 = rb1.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0); diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs index 4a84c8ba1..530c2b03d 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs @@ -38,12 +38,12 @@ impl SoftDetectionCtx<'_> { || (sb1.origin().is_some() && sb1.origin() == sb2.origin()) } - /// The speculative motion margin of a soft body's collider, held by its parent cluster proxy - /// (zero without a parent). + /// The speculative motion margin of a soft body's deformable collider, held by its parent + /// cluster proxy (zero for a rigid collider or without a parent). pub fn motion_margin(&self, co: &Collider) -> Real { co.parent() .and_then(|h| self.bodies.get(h)) - .map_or(0.0, |rb| rb.soft_motion_margin) + .map_or(0.0, |rb| co.soft_motion_margin(rb)) } } diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 113ba5cf9..9c051bfdc 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -28,6 +28,10 @@ use crate::{dynamics::RigidBodySet, geometry::ColliderSet}; /// - Debugging collision detection separately from dynamics /// /// Like PhysicsPipeline, this only holds temporary buffers. Reuse the same instance for performance. +/// +/// Bodies are never integrated: their contacts are updated on the steps you move them (or modify +/// their colliders). There is no sleeping either, so the [`IslandManager`] passed to +/// [`Self::step`] stays empty (bodies are never registered in it). // NOTE: this contains only workspace data, so there is no point in making this serializable. pub struct CollisionPipeline { broad_phase_events: Vec, diff --git a/src/pipeline/physics_pipeline/mod.rs b/src/pipeline/physics_pipeline/mod.rs index a7632f21e..a6f3cfae4 100644 --- a/src/pipeline/physics_pipeline/mod.rs +++ b/src/pipeline/physics_pipeline/mod.rs @@ -56,6 +56,9 @@ pub struct PhysicsPipeline { /// AABBs, fed to the broad-phase update without the user-modification tracking. AABBs are /// computed inside the advance loop while body/collider are in cache. end_step_collider_aabbs: Vec<(ColliderHandle, crate::geometry::Aabb)>, + /// Colliders the end-of-step soft-body sync deformed or moved to their cluster proxy's fresh + /// pose, whose broad-phase AABBs follow them. + soft_synced_colliders: Vec, /// Non-finite state detected and neutralized during the last step. quarantine: Quarantine, /// Workspace buffer holding the active body handles (parallel body update). @@ -118,6 +121,7 @@ impl PhysicsPipeline { joint_selection_primed: false, broad_phase_events: vec![], end_step_collider_aabbs: vec![], + soft_synced_colliders: vec![], quarantine: Quarantine::default(), } } @@ -164,6 +168,10 @@ impl PhysicsPipeline { /// * `hooks` - Optional callbacks to customize collision filtering and contact modification /// * `events` - Optional handler to receive collision events (when objects start/stop touching) /// + /// A step with a zero [`IntegrationParameters::dt`] (e.g. the first frame of a variable + /// timestep) doesn't simulate anything: it only applies the user changes and updates the + /// collision detection. + /// /// # Example /// /// ``` diff --git a/src/pipeline/physics_pipeline/solve.rs b/src/pipeline/physics_pipeline/solve.rs index 838a4cedd..7ba32026d 100644 --- a/src/pipeline/physics_pipeline/solve.rs +++ b/src/pipeline/physics_pipeline/solve.rs @@ -155,6 +155,7 @@ impl PhysicsPipeline { events, ); + self.counters.cd.ncontact_pairs = narrow_phase.contact_graph().graph.edges.len(); self.counters.cd.narrow_phase_time.pause(); self.counters.stages.collision_detection_time.pause(); } diff --git a/src/pipeline/physics_pipeline/substep.rs b/src/pipeline/physics_pipeline/substep.rs index b4df95b69..913cc7ac7 100644 --- a/src/pipeline/physics_pipeline/substep.rs +++ b/src/pipeline/physics_pipeline/substep.rs @@ -81,6 +81,8 @@ impl PhysicsPipeline { self.counters.ccd.toi_computation_time.pause(); } + /// `integration_parameters.dt` is the length of the interval the soft-CCD prediction of the + /// broad-phase AABBs covers: the next CCD pass, or the next step after the last pass. fn advance_to_final_positions( &mut self, integration_parameters: &IntegrationParameters, @@ -103,7 +105,7 @@ impl PhysicsPipeline { let collider_aabb = |co: &crate::geometry::Collider, rb: &crate::dynamics::RigidBody| -> crate::geometry::Aabb { - let mut aabb = co.compute_collision_aabb(prediction / 2.0 + rb.soft_motion_margin); + let mut aabb = co.compute_collision_aabb(prediction / 2.0 + co.soft_motion_margin(rb)); if rb.soft_ccd_prediction() > 0.0 { let next_pose = rb.predict_position_using_velocity_and_forces_with_max_dist( dt, @@ -131,18 +133,22 @@ impl PhysicsPipeline { continue; } rb.pos.position = rb.pos.next_position; - for co_handle in rb.colliders.0.iter() { - let co = colliders.index_mut_internal(*co_handle); - let new_pos = rb.pos.position * co.parent.as_ref().unwrap().pos_wrt_parent; - co.pos = crate::geometry::ColliderPosition(new_pos); - if co.is_enabled() { - let aabb = collider_aabb(co, rb); - if aabb.mins.is_finite() && aabb.maxs.is_finite() { - self.end_step_collider_aabbs.push((*co_handle, aabb)); - } else { - // Finite body pose but non-finite AABB: the collider's own - // geometry is invalid. - self.quarantine.collider_workspace.push(*co_handle); + // A cluster proxy's pose isn't integrated: the end-of-step soft-body sync moves + // its colliders to its fresh pose and refreshes their AABBs instead. + if !rb.is_soft_frame() { + for co_handle in rb.colliders.0.iter() { + let co = colliders.index_mut_internal(*co_handle); + let new_pos = rb.pos.position * co.parent.as_ref().unwrap().pos_wrt_parent; + co.pos = crate::geometry::ColliderPosition(new_pos); + if co.is_enabled() { + let aabb = collider_aabb(co, rb); + if aabb.mins.is_finite() && aabb.maxs.is_finite() { + self.end_step_collider_aabbs.push((*co_handle, aabb)); + } else { + // Finite body pose but non-finite AABB: the collider's own + // geometry is invalid. + self.quarantine.collider_workspace.push(*co_handle); + } } } } @@ -189,17 +195,20 @@ impl PhysicsPipeline { } rb.pos.position = rb.pos.next_position; - for co_handle in rb.colliders.0.iter() { - let co = colliders.index_mut_internal(*co_handle); - let new_pos = - rb.pos.position * co.parent.as_ref().unwrap().pos_wrt_parent; - co.pos = crate::geometry::ColliderPosition(new_pos); - if co.is_enabled() { - let aabb = collider_aabb(co, rb); - if aabb.mins.is_finite() && aabb.maxs.is_finite() { - moved.push((*co_handle, aabb)); - } else { - quarantined_colliders.push(*co_handle); + // Cluster proxies' colliders: see the serial branch. + if !rb.is_soft_frame() { + for co_handle in rb.colliders.0.iter() { + let co = colliders.index_mut_internal(*co_handle); + let new_pos = + rb.pos.position * co.parent.as_ref().unwrap().pos_wrt_parent; + co.pos = crate::geometry::ColliderPosition(new_pos); + if co.is_enabled() { + let aabb = collider_aabb(co, rb); + if aabb.mins.is_finite() && aabb.maxs.is_finite() { + moved.push((*co_handle, aabb)); + } else { + quarantined_colliders.push(*co_handle); + } } } } @@ -241,6 +250,37 @@ impl PhysicsPipeline { } } + /// Feeds the broad-phase the AABBs of the colliders the soft-body sync deformed or moved to + /// their proxies' fresh poses, like `update_moved_collider_aabbs` does for the advanced bodies. + fn update_soft_synced_collider_aabbs( + &mut self, + integration_parameters: &IntegrationParameters, + bodies: &RigidBodySet, + colliders: &ColliderSet, + broad_phase: &mut BroadPhaseBvh, + ) { + if self.soft_synced_colliders.is_empty() { + return; + } + self.join_deferred_bvh_optimize(broad_phase); + + for handle in &self.soft_synced_colliders { + let Some(co) = colliders.get(*handle) else { + continue; + }; + if !co.is_enabled() { + continue; + } + // The AABB the next broad-phase update computes (soft-body motion margin included + // for the deformable colliders only). + let aabb = co.compute_broad_phase_aabb(integration_parameters, bodies); + // A non-finite AABB would corrupt the tree; the next steps' quarantine handles it. + if aabb.mins.is_finite() && aabb.maxs.is_finite() { + broad_phase.set_aabb(integration_parameters, *handle, aabb); + } + } + } + fn interpolate_kinematic_velocities( &mut self, integration_parameters: &IntegrationParameters, @@ -372,6 +412,7 @@ impl PhysicsPipeline { } // Persistent islands: apply the joint connectivity edits (in order). + impulse_joints.flush_modified_joints(); let joint_island_events: Vec<_> = core::mem::take(&mut impulse_joints.island_events); for event in joint_island_events { islands.apply_impulse_joint_island_event(bodies, event); @@ -426,9 +467,30 @@ impl PhysicsPipeline { removed_colliders.clear(); self.counters.stages.user_changes.pause(); + // A zero-length step only applies the user changes and updates the contacts: no time + // passes, so the dynamics (solver, integration, CCD, soft-body tears) are skipped. + if integration_parameters.dt <= 0.0 { + // The solver graph consumes this step's contact updates, as the solve would have. + narrow_phase.maintain_solver_contact_graph( + islands, + bodies, + colliders, + multibody_joints, + ); + colliders.set_modified(modified_colliders); + self.counters.step_completed(); + return; + } + let mut remaining_time = integration_parameters.dt; + // The CCD passes shrink `integration_parameters.dt`: what predicts the next step (the + // end-of-step AABBs and soft-body sync) reads the full step's parameters instead. + let step_parameters = *integration_parameters; let mut integration_parameters = *integration_parameters; + // CCD substeps are only reported when the CCD had to act during the step. + let mut num_passes = 0; + let mut any_ccd_active = false; let (ccd_is_enabled, mut remaining_substeps) = if integration_parameters.max_ccd_substeps == 0 { (false, 1) @@ -452,6 +514,7 @@ impl PhysicsPipeline { // these forces have not been integrated to the body's velocity yet. let ccd_active = ccd_solver.update_ccd_active_flags(islands, bodies, remaining_time, true); + any_ccd_active |= ccd_active; self.join_deferred_bvh_optimize(broad_phase); let first_impact = if ccd_active { ccd_solver.find_first_impact( @@ -498,7 +561,7 @@ impl PhysicsPipeline { remaining_substeps = 0; } - self.counters.ccd.num_substeps += 1; + num_passes += 1; self.counters.custom.resume(); self.interpolate_kinematic_velocities(&integration_parameters, islands, bodies); @@ -530,6 +593,7 @@ impl PhysicsPipeline { false, ), }; + any_ccd_active |= ccd_active; if ccd_active { self.join_deferred_bvh_optimize(broad_phase); self.run_ccd_motion_clamping( @@ -548,7 +612,13 @@ impl PhysicsPipeline { } self.counters.stages.update_time.resume(); - self.advance_to_final_positions(&integration_parameters, islands, bodies, colliders); + // After the last pass, the AABBs predict the whole next step, not another pass. + let prediction_parameters = if remaining_substeps > 0 { + &integration_parameters + } else { + &step_parameters + }; + self.advance_to_final_positions(prediction_parameters, islands, bodies, colliders); // Neutralize bodies whose integrated pose went non-finite before the remaining // CCD substeps can spread their velocities. self.quarantine.apply_end_step(bodies, colliders); @@ -589,12 +659,16 @@ impl PhysicsPipeline { // harvested by `advance_to_final_positions`. self.counters.stages.collision_detection_time.resume(); self.counters.cd.final_broad_phase_time.resume(); - self.update_moved_collider_aabbs(&integration_parameters, broad_phase); + self.update_moved_collider_aabbs(&step_parameters, broad_phase); self.counters.cd.final_broad_phase_time.pause(); self.counters.stages.collision_detection_time.pause(); } } + if any_ccd_active { + self.counters.ccd.num_substeps = num_passes; + } + // Finally, make sure we update the world mass-properties of the rigid-bodies // that moved. Otherwise, users may end up applying forces with respect to an // outdated center of mass. @@ -619,12 +693,22 @@ impl PhysicsPipeline { ); // Update the soft bodies' derived state (sleep state, surface orientation, substep // requests) and their colliders (picked up by the next step's user-changes handling). - soft_bodies.sync_particle_positions( + self.soft_synced_colliders.clear(); + soft_bodies.sync_particle_positions_and_collect( bodies, colliders, - &integration_parameters, + &step_parameters, + integration_parameters.dt, &mut self.quarantine.soft_bodies, + &mut self.soft_synced_colliders, ); + // The surfaces followed the particles and the rigid colliders on the proxies moved with + // them: the broad phase follows, so scene queries between steps see their final geometry. + self.counters.stages.collision_detection_time.resume(); + self.counters.cd.final_broad_phase_time.resume(); + self.update_soft_synced_collider_aabbs(&step_parameters, bodies, colliders, broad_phase); + self.counters.cd.final_broad_phase_time.pause(); + self.counters.stages.collision_detection_time.pause(); self.counters.step_completed(); } diff --git a/typescript/CHANGELOG.md b/typescript/CHANGELOG.md index 37d3236dd..6fbb00289 100644 --- a/typescript/CHANGELOG.md +++ b/typescript/CHANGELOG.md @@ -60,6 +60,10 @@ `ColliderDesc.compound` and `ColliderDesc.convexDecomposition`. - The testbeds gained soft-body demos ("soft bodies" in 2D and 3D, "soft tearing" in 3D). +### Fixed + +- The shape-cast docs now state the frame (world or local) of each witness point and normal. + ## 0.20.0 (08 August 2026) ### Breaking changes diff --git a/typescript/src.ts/control/character_controller.ts b/typescript/src.ts/control/character_controller.ts index 4c10f82c2..b065750c6 100644 --- a/typescript/src.ts/control/character_controller.ts +++ b/typescript/src.ts/control/character_controller.ts @@ -25,11 +25,11 @@ export class CharacterCollision { public toi: number; /** The world-space contact point on the collider when the collision happens. */ public witness1: Vector; - /** The local-space contact point on the character when the collision happens. */ + /** The world-space contact point on the character when the collision happens. */ public witness2: Vector; /** The world-space outward contact normal on the collider when the collision happens. */ public normal1: Vector; - /** The local-space outward contact normal on the character when the collision happens. */ + /** The world-space outward contact normal on the character when the collision happens. */ public normal2: Vector; } diff --git a/typescript/src.ts/geometry/broad_phase.ts b/typescript/src.ts/geometry/broad_phase.ts index 7101ec3f7..b80f18bd6 100644 --- a/typescript/src.ts/geometry/broad_phase.ts +++ b/typescript/src.ts/geometry/broad_phase.ts @@ -379,6 +379,10 @@ export class BroadPhase { * This is similar to ray-casting except that we are casting a whole shape instead of * just a point (the ray origin). * + * In the returned hit, `witness1` and `normal1` lie on the hit collider and are expressed in + * world-space, while `witness2` and `normal2` lie on the cast shape and are expressed in its + * local-space (relative to `shapePos` and `shapeRot`). + * * @param colliders - The set of colliders taking part in this pipeline. * @param shapePos - The initial position of the shape to cast. * @param shapeRot - The initial rotation of the shape to cast. diff --git a/typescript/src.ts/geometry/collider.ts b/typescript/src.ts/geometry/collider.ts index daa712c61..c56a8e569 100644 --- a/typescript/src.ts/geometry/collider.ts +++ b/typescript/src.ts/geometry/collider.ts @@ -1117,6 +1117,10 @@ export class Collider { /** * Computes the smallest time between this and the given shape under translational movement are separated by a distance smaller or equal to distance. * + * In the returned hit, `witness1` and `normal1` lie on this collider and are expressed in its + * local-space, while `witness2` and `normal2` lie on `shape2` and are expressed in its + * local-space (relative to `shape2Pos` and `shape2Rot`). + * * @param collider1Vel - The constant velocity of the current shape to cast (i.e. the cast direction). * @param shape2 - The shape to cast against. * @param shape2Pos - The position of the second shape. @@ -1180,6 +1184,10 @@ export class Collider { /** * Computes the smallest time between this and the given collider under translational movement are separated by a distance smaller or equal to distance. * + * In the returned hit, `collider` is `collider2`; `witness1` and `normal1` lie on this collider + * and are expressed in its local-space, while `witness2` and `normal2` lie on `collider2` and + * are expressed in its local-space. + * * @param collider1Vel - The constant velocity of the current collider to cast (i.e. the cast direction). * @param collider2 - The collider to cast against. * @param collider2Vel - The constant velocity of the second collider. diff --git a/typescript/src.ts/geometry/shape.ts b/typescript/src.ts/geometry/shape.ts index c0257796c..98ab30ef1 100644 --- a/typescript/src.ts/geometry/shape.ts +++ b/typescript/src.ts/geometry/shape.ts @@ -445,6 +445,11 @@ export abstract class Shape { /** * Computes the time of impact between two moving shapes. + * + * In the returned hit, `witness1` and `normal1` lie on this shape and are expressed in its + * local-space (relative to `shapePos1` and `shapeRot1`), while `witness2` and `normal2` lie on + * `shape2` and are expressed in its local-space (relative to `shapePos2` and `shapeRot2`). + * * @param shapePos1 - The initial position of this shape. * @param shapeRot1 - The rotation of this shape. * @param shapeVel1 - The velocity of this shape. diff --git a/typescript/src.ts/geometry/toi.ts b/typescript/src.ts/geometry/toi.ts index 013c4be38..7e43b0e9b 100644 --- a/typescript/src.ts/geometry/toi.ts +++ b/typescript/src.ts/geometry/toi.ts @@ -2,7 +2,13 @@ import {Collider} from "./collider"; import {Vector, VectorOps} from "../math"; /** - * The intersection between a ray and a collider. + * The result of a shape-cast between two shapes, returned by the pairwise casts + * `Shape.castShape` and `Collider.castShape`. + * + * The first shape is the one the cast is called on (the shape or collider `this`), and the + * second shape is the `shape2` argument. Each witness point and normal is expressed in the + * local-space of its own shape, i.e., relative to that shape's pose (it does not depend on + * the shape's translation along the cast). */ export class ShapeCastHit { /** @@ -10,23 +16,23 @@ export class ShapeCastHit { */ time_of_impact: number; /** - * The local-space contact point on the first shape, at - * the time of impact. + * The contact point on the first shape at the time of impact, expressed in the + * local-space of the first shape. */ witness1: Vector; /** - * The local-space contact point on the second shape, at - * the time of impact. + * The contact point on the second shape at the time of impact, expressed in the + * local-space of the second shape. */ witness2: Vector; /** - * The local-space normal on the first shape, at - * the time of impact. + * The outward normal on the first shape at the time of impact, expressed in the + * local-space of the first shape. */ normal1: Vector; /** - * The local-space normal on the second shape, at - * the time of impact. + * The outward normal on the second shape at the time of impact, expressed in the + * local-space of the second shape. */ normal2: Vector; @@ -92,11 +98,20 @@ export class ShapeCastHit { } /** - * The intersection between a ray and a collider. + * The result of a shape-cast that hit a collider. + * + * The frames of the witness points and normals depend on the query that returned it: + * - `World.castShape` (and `BroadPhase.castShape`): `witness1` and `normal1` lie on the hit + * `collider` and are expressed in world-space; `witness2` and `normal2` lie on the cast + * shape and are expressed in its local-space (relative to its pose, so they do not depend + * on its translation along the cast). + * - `Collider.castCollider`: `witness1` and `normal1` lie on the collider the cast is called + * on and are expressed in its local-space; `witness2` and `normal2` lie on the hit + * `collider` (the `collider2` argument) and are expressed in its local-space. */ export class ColliderShapeCastHit extends ShapeCastHit { /** - * The handle of the collider hit by the ray. + * The collider hit by the shape-cast. */ collider: Collider; diff --git a/typescript/src.ts/pipeline/world.ts b/typescript/src.ts/pipeline/world.ts index 63be24b27..a4c045477 100644 --- a/typescript/src.ts/pipeline/world.ts +++ b/typescript/src.ts/pipeline/world.ts @@ -1164,6 +1164,10 @@ export class World { * This is similar to ray-casting except that we are casting a whole shape instead of * just a point (the ray origin). * + * In the returned hit, `witness1` and `normal1` lie on the hit collider and are expressed in + * world-space, while `witness2` and `normal2` lie on the cast shape and are expressed in its + * local-space (relative to `shapePos` and `shapeRot`). + * * @param shapePos - The initial position of the shape to cast. * @param shapeRot - The initial rotation of the shape to cast. * @param shapeVel - The constant velocity of the shape to cast (i.e. the cast direction).