From 622cafbc2365fc456545ddce7e97fcfddde6e0a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 23 Aug 2026 16:12:56 +0200 Subject: [PATCH 01/32] feat(soft-body): add real-time soft bodies with deformable surface colliders, debug rendering and testbed support --- Cargo.toml | 12 +- crates/rapier2d-f64/Cargo.toml | 2 + crates/rapier2d/Cargo.toml | 2 + crates/rapier2d/tests/ccd_default_vs_fixed.rs | 3 + crates/rapier2d/tests/ccd_oriented_wall.rs | 2 + crates/rapier2d/tests/ccd_semantics.rs | 5 + .../tests/issue_629_large_ground_stability.rs | 3 + .../tests/issue_643_polyline_corner_stuck.rs | 3 + .../issue_669_polyline_ghost_collisions.rs | 3 + .../tests/issue_752_oneway_platform_normal.rs | 2 + .../tests/issue_772_pin_slot_joint.rs | 2 + .../tests/issue_818_nan_tangent_impulse.rs | 4 +- ...sue_919_prediction_distance_restitution.rs | 2 + crates/rapier2d/tests/miri_scenes.rs | 3 + crates/rapier2d/tests/snapshot_portability.rs | 2 +- crates/rapier2d/tests/snapshot_roundtrip.rs | 6 +- .../tests/soft_contact_debug_render.rs | 125 +++ crates/rapier2d/tests/speed_cap.rs | 3 + crates/rapier2d/tests/spinner_containment.rs | 2 + crates/rapier3d-mjcf/src/hooks.rs | 6 +- crates/rapier3d-mjcf/tests/actuator_gain.rs | 2 + crates/rapier3d-mjcf/tests/cassie_smoke.rs | 2 + crates/rapier3d-mjcf/tests/equality_joint.rs | 2 + crates/rapier3d-mjcf/tests/example_smoke.rs | 2 + crates/rapier3d-mjcf/tests/fixed_tendon.rs | 2 + crates/rapier3d-mjcf/tests/frictionloss.rs | 2 + .../rapier3d-mjcf/tests/keyframe_controls.rs | 2 + .../rapier3d-mjcf/tests/loader_regressions.rs | 3 + crates/rapier3d-mjcf/tests/phase1.rs | 2 + crates/rapier3d-mjcf/tests/phase2.rs | 2 + crates/rapier3d-mjcf/tests/phase3.rs | 2 + crates/rapier3d-mjcf/tests/phase4.rs | 2 + crates/rapier3d-mjcf/tests/phase5.rs | 4 + .../tests/additional_solver_iterations.rs | 3 + crates/rapier3d/tests/ccd_default_vs_fixed.rs | 3 + crates/rapier3d/tests/contact_clustering.rs | 17 +- .../tests/contact_force_event_clustering.rs | 14 +- .../tests/issue_182_heightfield_seam.rs | 3 + .../tests/issue_217_ccd_large_dt_hitch.rs | 3 + .../rapier3d/tests/issue_372_trimesh_sleep.rs | 3 + .../tests/issue_485_kcc_depenetration.rs | 3 + .../issue_524_thin_slab_trimesh_tunnel.rs | 3 + .../issue_810_cubes_thin_cylinder_tunnel.rs | 3 + .../tests/issue_862_kcc_impulse_point.rs | 4 +- .../issue_932_low_ccd_substeps_stutter.rs | 3 + .../rapier3d/tests/issue_974_restitution.rs | 2 + .../rapier3d/tests/loop_closure_com_repro.rs | 3 + crates/rapier3d/tests/miri_scenes.rs | 3 + crates/rapier3d/tests/multibody_armature.rs | 2 + crates/rapier3d/tests/multibody_coupling.rs | 2 + .../rapier3d/tests/multibody_dof_inertia.rs | 9 +- crates/rapier3d/tests/multibody_spring.rs | 2 + crates/rapier3d/tests/persistent_islands.rs | 2 +- .../tests/simd_backend_determinism.rs | 2 + crates/rapier3d/tests/sleep_wake.rs | 6 +- crates/rapier3d/tests/snapshot_portability.rs | 2 +- crates/rapier3d/tests/snapshot_roundtrip.rs | 4 +- crates/rapier3d/tests/speed_cap.rs | 3 + .../tests/thread_count_determinism.rs | 22 + crates/rapier3d/tests/unsync_callbacks.rs | 8 +- examples2d/b2d_many_pyramids.rs | 2 +- examples2d/b2d_rain.rs | 2 +- examples2d/debug_many_colliders2.rs | 6 +- examples2d/one_way_platforms2.rs | 6 +- examples2d/stress_tests/many_pyramids2.rs | 2 +- examples3d/b3d_rain.rs | 2 +- examples3d/debug_multi_collider_body3.rs | 2 +- examples3d/one_way_platforms3.rs | 6 +- src/control/character_controller.rs | 8 + src/dynamics/ccd/ccd_solver.rs | 12 +- src/dynamics/ccd/sweeps.rs | 12 +- src/dynamics/integration_parameters.rs | 29 +- src/dynamics/island_manager/global_split.rs | 86 +- src/dynamics/island_manager/local_split.rs | 116 ++- src/dynamics/island_manager/manager.rs | 52 +- src/dynamics/island_manager/persistent.rs | 81 +- src/dynamics/island_manager/substep_groups.rs | 81 +- .../joint/multibody_joint/multibody.rs | 6 +- .../multibody_joint/multibody_joint_set.rs | 4 +- src/dynamics/mod.rs | 5 + src/dynamics/rigid_body.rs | 16 + src/dynamics/rigid_body_components.rs | 4 +- src/dynamics/rigid_body_set.rs | 2 + src/dynamics/soft_body/mod.rs | 43 + src/dynamics/soft_body/soft_body.rs | 122 +++ src/dynamics/soft_body/soft_body_accessors.rs | 252 ++++++ .../soft_body/soft_body_builder/mod.rs | 17 + .../soft_body_builder/soft_body_build.rs | 188 ++++ .../soft_body_builder/soft_body_builder.rs | 114 +++ .../soft_body_builder_mesh_helpers.rs | 290 ++++++ .../soft_body_builder_settings.rs | 405 +++++++++ .../soft_body_builder_shapes.rs | 582 ++++++++++++ src/dynamics/soft_body/soft_body_coloring.rs | 80 ++ src/dynamics/soft_body/soft_body_contacts.rs | 132 +++ src/dynamics/soft_body/soft_body_elements.rs | 195 ++++ src/dynamics/soft_body/soft_body_geometry.rs | 96 ++ src/dynamics/soft_body/soft_body_handle.rs | 30 + src/dynamics/soft_body/soft_body_material.rs | 126 +++ src/dynamics/soft_body/soft_body_meshes.rs | 50 ++ src/dynamics/soft_body/soft_body_motion.rs | 140 +++ src/dynamics/soft_body/soft_body_set/mod.rs | 12 + .../soft_body/soft_body_set/soft_body_set.rs | 107 +++ .../soft_body_set_insert_remove.rs | 85 ++ .../soft_body_set/soft_body_set_proxies.rs | 186 ++++ .../soft_body_set/soft_body_set_step_sync.rs | 10 + src/dynamics/soft_body/soft_body_settings.rs | 31 + .../soft_body/soft_body_shape_matching.rs | 63 ++ .../contact_constraint_element.rs | 4 +- .../contact_with_coulomb_friction.rs | 20 +- .../contact_with_twist_friction.rs | 18 +- .../generic_contact_constraint.rs | 2 +- .../generic_joint_constraint_builder.rs | 7 +- .../joint_constraint_builder.rs | 6 +- .../joint_velocity_constraint.rs | 2 +- src/dynamics/solver/manifold_store.rs | 43 +- src/dynamics/solver/mod.rs | 1 + src/dynamics/solver/soft_constraint/mod.rs | 10 + .../solver/soft_constraint/soft_attachment.rs | 138 +++ .../soft_constraints_set/mod.rs | 21 + .../soft_constraint_prepare.rs | 209 +++++ .../soft_constraint_rigid_coupling.rs | 132 +++ .../soft_constraint_solve.rs | 150 ++++ .../soft_constraint_writeback.rs | 117 +++ .../soft_constraints_set/soft_constraints.rs | 64 ++ .../soft_constraints_set.rs | 172 ++++ .../solver/soft_constraint/soft_contact.rs | 326 +++++++ .../soft_contact_assembly/mod.rs | 17 + .../soft_contact_assembly.rs | 148 +++ .../soft_contact_assembly_body_contacts.rs | 439 +++++++++ .../soft_contact_assembly_edge_contacts.rs | 225 +++++ .../soft_contact_assembly_vertex_contacts.rs | 8 + .../soft_contact_assembly_workspace.rs | 74 ++ .../soft_contact_assembly_writeback.rs | 139 +++ .../soft_constraint/soft_contact_chunks.rs | 364 ++++++++ .../soft_element_constraint/mod.rs | 14 + .../soft_elastic_constraint.rs | 472 ++++++++++ .../soft_element_linalg.rs | 88 ++ .../soft_neo_hookean.rs | 163 ++++ .../soft_scalar_constraint.rs | 168 ++++ .../soft_element_constraint/test.rs | 358 ++++++++ .../soft_element_constraint_assembly.rs | 843 ++++++++++++++++++ .../solver/staged_island_solver/helpers.rs | 5 +- .../solver/staged_island_solver/init.rs | 111 ++- .../solver/staged_island_solver/joints.rs | 2 +- .../solver/staged_island_solver/mod.rs | 14 +- .../solver/staged_island_solver/solve.rs | 247 ++++- .../solver/staged_island_solver/worker.rs | 76 +- src/geometry/broad_phase_bvh/mod.rs | 22 +- src/geometry/broad_phase_bvh/update.rs | 37 +- src/geometry/collider.rs | 62 +- src/geometry/collider_components.rs | 7 +- src/geometry/collider_set.rs | 2 + src/geometry/contact_clustering.rs | 12 +- src/geometry/contact_pair.rs | 272 ++++-- src/geometry/mesh_converter.rs | 2 +- src/geometry/mod.rs | 16 + src/geometry/narrow_phase/contacts.rs | 35 +- src/geometry/narrow_phase/intersections.rs | 4 +- src/geometry/narrow_phase/mod.rs | 47 +- src/geometry/narrow_phase/pair_management.rs | 9 +- src/geometry/narrow_phase/pair_update.rs | 228 +++-- src/geometry/narrow_phase/queries.rs | 14 + .../narrow_phase/soft_contacts/mod.rs | 24 + .../soft_contacts/soft_contacts_driver.rs | 83 ++ .../soft_contacts/soft_contacts_edge_pass.rs | 160 ++++ .../soft_contacts/soft_contacts_types.rs | 378 ++++++++ .../soft_contacts_vertex_pass.rs | 10 + src/geometry/narrow_phase/solver_graph.rs | 12 +- src/geometry/narrow_phase/test.rs | 20 +- src/pipeline/collision_pipeline.rs | 1 + .../debug_render_backend.rs | 3 + .../debug_render_pipeline.rs | 81 +- .../debug_render_style.rs | 3 + src/pipeline/event_handler.rs | 16 +- src/pipeline/mod.rs | 5 +- src/pipeline/physics_hooks.rs | 101 ++- src/pipeline/physics_pipeline/mod.rs | 16 +- src/pipeline/physics_pipeline/quarantine.rs | 42 +- src/pipeline/physics_pipeline/solve.rs | 25 +- src/pipeline/physics_pipeline/substep.rs | 62 +- src/pipeline/physics_pipeline/test.rs | 29 +- src/pipeline/physics_world.rs | 45 +- src/pipeline/query_pipeline.rs | 3 +- src/pipeline/user_changes.rs | 11 +- src/utils/mod.rs | 2 +- src_testbed/debug_render.rs | 50 +- src_testbed/graphics.rs | 34 +- src_testbed/physics/mod.rs | 2 + src_testbed/settings.rs | 8 + src_testbed/testbed/state.rs | 7 +- src_testbed/viewer.rs | 27 +- 191 files changed, 11127 insertions(+), 572 deletions(-) create mode 100644 crates/rapier2d/tests/soft_contact_debug_render.rs create mode 100644 src/dynamics/soft_body/mod.rs create mode 100644 src/dynamics/soft_body/soft_body.rs create mode 100644 src/dynamics/soft_body/soft_body_accessors.rs create mode 100644 src/dynamics/soft_body/soft_body_builder/mod.rs create mode 100644 src/dynamics/soft_body/soft_body_builder/soft_body_build.rs create mode 100644 src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs create mode 100644 src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs create mode 100644 src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs create mode 100644 src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs create mode 100644 src/dynamics/soft_body/soft_body_coloring.rs create mode 100644 src/dynamics/soft_body/soft_body_contacts.rs create mode 100644 src/dynamics/soft_body/soft_body_elements.rs create mode 100644 src/dynamics/soft_body/soft_body_geometry.rs create mode 100644 src/dynamics/soft_body/soft_body_handle.rs create mode 100644 src/dynamics/soft_body/soft_body_material.rs create mode 100644 src/dynamics/soft_body/soft_body_meshes.rs create mode 100644 src/dynamics/soft_body/soft_body_motion.rs create mode 100644 src/dynamics/soft_body/soft_body_set/mod.rs create mode 100644 src/dynamics/soft_body/soft_body_set/soft_body_set.rs create mode 100644 src/dynamics/soft_body/soft_body_set/soft_body_set_insert_remove.rs create mode 100644 src/dynamics/soft_body/soft_body_set/soft_body_set_proxies.rs create mode 100644 src/dynamics/soft_body/soft_body_set/soft_body_set_step_sync.rs create mode 100644 src/dynamics/soft_body/soft_body_settings.rs create mode 100644 src/dynamics/soft_body/soft_body_shape_matching.rs create mode 100644 src/dynamics/solver/soft_constraint/mod.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_attachment.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_constraints_set/mod.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_writeback.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints_set.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_contact.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_contact_assembly/mod.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_contact_chunks.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_element_constraint/mod.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_element_constraint/soft_element_linalg.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs create mode 100644 src/geometry/narrow_phase/soft_contacts/mod.rs create mode 100644 src/geometry/narrow_phase/soft_contacts/soft_contacts_driver.rs create mode 100644 src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs create mode 100644 src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs create mode 100644 src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs diff --git a/Cargo.toml b/Cargo.toml index 5c32941e3..bd6679827 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -148,14 +148,16 @@ tempfile = "3" #simba = { path = "../simba" } #kiss3d = { path = "../kiss3d" } -#parry2d = { path = "../parry/crates/parry2d" } -#parry3d = { path = "../parry/crates/parry3d" } -#parry2d-f64 = { path = "../parry/crates/parry2d-f64" } -#parry3d-f64 = { path = "../parry/crates/parry3d-f64" } +# Temporary: the deformable-mesh API (parry branch `soft-bodies`), to be replaced by a +# version bump when parry is released. +parry2d = { path = "../parry/crates/parry2d" } +parry3d = { path = "../parry/crates/parry3d" } +parry2d-f64 = { path = "../parry/crates/parry2d-f64" } +parry3d-f64 = { path = "../parry/crates/parry3d-f64" } #nalgebra = { path = "../nalgebra" } #simba = { path = "../simba" } #wide = { path = "../wide" } -#glamx = { path = "../glamx" } +glamx = { path = "../glamx" } #kiss3d = { git = "https://github.com/sebcrozet/kiss3d", branch = "render-2d" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index d9792065e..ace823f3c 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -35,6 +35,8 @@ block-solver = [] alloc = [ "nalgebra/alloc", "parry2d-f64/alloc", + # Needed by `SoftBodyBuilder::volumetric`, for the Delaunay refinement. + "parry2d-f64/spade", "serde?/alloc", ] std = [ diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index 706a05ca6..930ed4551 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -35,6 +35,8 @@ block-solver = [] alloc = [ "nalgebra/alloc", "parry2d/alloc", + # Needed by `SoftBodyBuilder::volumetric`, for the Delaunay refinement. + "parry2d/spade", "serde?/alloc", ] std = [ diff --git a/crates/rapier2d/tests/ccd_default_vs_fixed.rs b/crates/rapier2d/tests/ccd_default_vs_fixed.rs index 831e2769c..4f7ba007e 100644 --- a/crates/rapier2d/tests/ccd_default_vs_fixed.rs +++ b/crates/rapier2d/tests/ccd_default_vs_fixed.rs @@ -10,6 +10,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -26,6 +27,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -48,6 +50,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier2d/tests/ccd_oriented_wall.rs b/crates/rapier2d/tests/ccd_oriented_wall.rs index 2755998ad..7fa4fa517 100644 --- a/crates/rapier2d/tests/ccd_oriented_wall.rs +++ b/crates/rapier2d/tests/ccd_oriented_wall.rs @@ -8,6 +8,7 @@ fn fires_through(wall: ColliderBuilder, speed: f32) -> f32 { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -37,6 +38,7 @@ fn fires_through(wall: ColliderBuilder, speed: f32) -> f32 { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier2d/tests/ccd_semantics.rs b/crates/rapier2d/tests/ccd_semantics.rs index a14a0a8a1..ca06f2ca1 100644 --- a/crates/rapier2d/tests/ccd_semantics.rs +++ b/crates/rapier2d/tests/ccd_semantics.rs @@ -33,6 +33,8 @@ impl EventHandler for EventCollector { _total_force_magnitude: Real, ) { } + + fn handle_soft_body_tear_event(&self, _soft_bodies: &SoftBodySet, _event: &SoftBodyTearEvent) {} } struct Harness { @@ -40,6 +42,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -57,6 +60,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -80,6 +84,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &self.events, diff --git a/crates/rapier2d/tests/issue_629_large_ground_stability.rs b/crates/rapier2d/tests/issue_629_large_ground_stability.rs index eb4ef36fa..6bf5db34d 100644 --- a/crates/rapier2d/tests/issue_629_large_ground_stability.rs +++ b/crates/rapier2d/tests/issue_629_large_ground_stability.rs @@ -11,6 +11,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -27,6 +28,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -48,6 +50,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier2d/tests/issue_643_polyline_corner_stuck.rs b/crates/rapier2d/tests/issue_643_polyline_corner_stuck.rs index 920cdd92e..c2ee02448 100644 --- a/crates/rapier2d/tests/issue_643_polyline_corner_stuck.rs +++ b/crates/rapier2d/tests/issue_643_polyline_corner_stuck.rs @@ -12,6 +12,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -28,6 +29,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -49,6 +51,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier2d/tests/issue_669_polyline_ghost_collisions.rs b/crates/rapier2d/tests/issue_669_polyline_ghost_collisions.rs index f170009b9..24030c1e4 100644 --- a/crates/rapier2d/tests/issue_669_polyline_ghost_collisions.rs +++ b/crates/rapier2d/tests/issue_669_polyline_ghost_collisions.rs @@ -11,6 +11,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -27,6 +28,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -48,6 +50,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier2d/tests/issue_752_oneway_platform_normal.rs b/crates/rapier2d/tests/issue_752_oneway_platform_normal.rs index 56e94cf3c..75130126d 100644 --- a/crates/rapier2d/tests/issue_752_oneway_platform_normal.rs +++ b/crates/rapier2d/tests/issue_752_oneway_platform_normal.rs @@ -27,6 +27,7 @@ fn oneway_platform_with_custom_normal() { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -64,6 +65,7 @@ fn oneway_platform_with_custom_normal() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &hooks, &(), diff --git a/crates/rapier2d/tests/issue_772_pin_slot_joint.rs b/crates/rapier2d/tests/issue_772_pin_slot_joint.rs index 006f9b5e9..5c44caec0 100644 --- a/crates/rapier2d/tests/issue_772_pin_slot_joint.rs +++ b/crates/rapier2d/tests/issue_772_pin_slot_joint.rs @@ -9,6 +9,7 @@ fn pin_slot_joint_allows_one_translation_and_free_rotation() { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -41,6 +42,7 @@ fn pin_slot_joint_allows_one_translation_and_free_rotation() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier2d/tests/issue_818_nan_tangent_impulse.rs b/crates/rapier2d/tests/issue_818_nan_tangent_impulse.rs index 8e5e1a1cc..2025f80d3 100644 --- a/crates/rapier2d/tests/issue_818_nan_tangent_impulse.rs +++ b/crates/rapier2d/tests/issue_818_nan_tangent_impulse.rs @@ -15,6 +15,7 @@ fn capsule_on_tilted_cuboid_reports_finite_impulses() { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut broad_phase = DefaultBroadPhase::new(); let mut narrow_phase = NarrowPhase::new(); @@ -57,13 +58,14 @@ fn capsule_on_tilted_cuboid_reports_finite_impulses() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), ); for pair in narrow_phase.contact_pairs() { - for manifold in &pair.manifolds { + for manifold in pair.manifolds() { for pt in &manifold.points { assert!( pt.data.impulse.is_finite(), diff --git a/crates/rapier2d/tests/issue_919_prediction_distance_restitution.rs b/crates/rapier2d/tests/issue_919_prediction_distance_restitution.rs index 835662c85..ce48dea00 100644 --- a/crates/rapier2d/tests/issue_919_prediction_distance_restitution.rs +++ b/crates/rapier2d/tests/issue_919_prediction_distance_restitution.rs @@ -31,6 +31,7 @@ fn rebound(e: f32, prediction_distance: Option) -> f32 { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut ccd = CCDSolver::new(); let mut params = IntegrationParameters::default(); if let Some(pred) = prediction_distance { @@ -51,6 +52,7 @@ fn rebound(e: f32, prediction_distance: Option) -> f32 { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier2d/tests/miri_scenes.rs b/crates/rapier2d/tests/miri_scenes.rs index 48a14b2ee..29b94fe4c 100644 --- a/crates/rapier2d/tests/miri_scenes.rs +++ b/crates/rapier2d/tests/miri_scenes.rs @@ -22,6 +22,7 @@ struct World { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, islands: IslandManager, broad_phase: DefaultBroadPhase, narrow_phase: NarrowPhase, @@ -38,6 +39,7 @@ impl World { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), islands: IslandManager::new(), broad_phase: DefaultBroadPhase::new(), narrow_phase: NarrowPhase::new(), @@ -59,6 +61,7 @@ impl World { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier2d/tests/snapshot_portability.rs b/crates/rapier2d/tests/snapshot_portability.rs index 153e471cf..1c4cc6d0f 100644 --- a/crates/rapier2d/tests/snapshot_portability.rs +++ b/crates/rapier2d/tests/snapshot_portability.rs @@ -31,7 +31,7 @@ use rapier2d::prelude::*; /// Snapshot size in bytes, and its FNV-1a digest. Both, because the size alone localizes a /// failure: a differing size means a container's *encoding* changed, an equal size with a /// differing digest means the values did. -const GOLDEN: (usize, u64) = (88_572, 0x9e84_4370_55e1_8c88); +const GOLDEN: (usize, u64) = (90_574, 0xf017_bf38_b19d_4eab); const STEPS: usize = 60; diff --git a/crates/rapier2d/tests/snapshot_roundtrip.rs b/crates/rapier2d/tests/snapshot_roundtrip.rs index fa9ba9bb4..d947421cb 100644 --- a/crates/rapier2d/tests/snapshot_roundtrip.rs +++ b/crates/rapier2d/tests/snapshot_roundtrip.rs @@ -6,7 +6,7 @@ //! step after the restore must yield **the same bytes** as the run it resumed. //! //! Worth its own copy rather than trusting the 3D suite: the serialized state is mostly -//! dimension-generic, but the scenes that fill it are not — 2D contact manifolds carry two +//! dimension-generic, but the scenes that fill it are not: 2D contact manifolds have two //! points instead of up to eight, `Rot`/angular velocity are scalars, and the solver's //! constraint layout differs. A field skipped only on the 2D path would go unseen here //! otherwise. @@ -307,7 +307,7 @@ fn pyramid_stress_scene_full() { check_roundtrip("large_pyramids2", world, 100, 20, false, no_edits); } -/// Fast CCD-enabled bodies. The CCD solver holds a cache that snapshots do not carry, so +/// Fast CCD-enabled bodies. The CCD solver holds a cache that snapshots do not include, so /// this checks that a restore does not depend on it. #[test] fn ccd_bodies() { @@ -369,7 +369,7 @@ fn multibody_articulation() { } /// The testbed's Save/Restore pattern: the world is replaced but the same -/// [`PhysicsPipeline`] keeps stepping, carrying its workspace across the restore. +/// [`PhysicsPipeline`] keeps stepping, keeping its workspace across the restore. #[test] fn restoring_into_a_live_pipeline() { check_roundtrip("live pipeline", pile(true), 100, 100, true, no_edits); diff --git a/crates/rapier2d/tests/soft_contact_debug_render.rs b/crates/rapier2d/tests/soft_contact_debug_render.rs new file mode 100644 index 000000000..d09326ff1 --- /dev/null +++ b/crates/rapier2d/tests/soft_contact_debug_render.rs @@ -0,0 +1,125 @@ +//! The debug-renderer draws a soft body's contacts at their witness points (not at the touched +//! element's centroid), self contacts included. +#![cfg(feature = "debug-render")] + +use rapier2d::pipeline::{ + DebugColor, DebugRenderBackend, DebugRenderMode, DebugRenderObject, DebugRenderPipeline, + DebugRenderStyle, +}; +use rapier2d::prelude::*; + +#[derive(Default)] +struct LineCollector { + lines: Vec<(Vector, Vector, DebugColor)>, +} + +impl DebugRenderBackend for LineCollector { + fn draw_line(&mut self, _: DebugRenderObject, a: Vector, b: Vector, color: DebugColor) { + self.lines.push((a, b, color)); + } +} + +/// A soft column, slender and soft enough to buckle and fold onto itself under gravity. Its +/// cells are far wider than the particles are thick, so a segment drawn to an element's centroid +/// is much longer than the contact it stands for. +const SPACING: Real = 0.6; +const RADIUS: Real = 0.05; + +fn buckling_column() -> SoftBodyBuilder { + let (nx, ny) = (3u32, 26u32); + let mut positions = vec![]; + for i in 0..nx { + for j in 0..ny { + positions.push(Vector::new( + i as Real * SPACING - SPACING, + 1.0 + j as Real * SPACING, + )); + } + } + let mut cells = vec![]; + for i in 0..nx - 1 { + for j in 0..ny - 1 { + let (a, b, c, d) = ( + i * ny + j, + i * ny + j + 1, + (i + 1) * ny + j, + (i + 1) * ny + j + 1, + ); + cells.push([a, b, c]); + cells.push([b, d, c]); + } + } + SoftBodyBuilder::new(positions) + .cells(cells) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e3, + poisson_ratio: 0.4, + ..Default::default() + }) + .particle_radius(RADIUS) + .self_contacts(true) +} + +#[test] +fn soft_contacts_are_drawn_at_their_witness_points() { + let mut world = PhysicsWorld::new(); + world.insert_collider(ColliderBuilder::cuboid(20.0, 1.0), None); + let handle = world.insert_soft_body(buckling_column()); + + // Step until the column has folded onto itself. + let mut folded = false; + for _ in 0..600 { + world.step(); + let sb = &world.soft_bodies[handle]; + if sb.vertex_contact_segments(&world.soft_bodies).count() > 0 { + folded = true; + break; + } + } + assert!(folded, "the column never self-collided; tune the scene"); + + // Every contact here is a self contact: the column is the only soft body. + let sb = &world.soft_bodies[handle]; + let contacts: Vec<_> = sb + .edge_contact_segments(&world.soft_bodies) + .chain(sb.vertex_contact_segments(&world.soft_bodies)) + .collect(); + assert!(!contacts.is_empty()); + + let mut pipeline = + DebugRenderPipeline::new(DebugRenderStyle::default(), DebugRenderMode::SOFT_BODIES); + let mut backend = LineCollector::default(); + pipeline.render_soft_bodies(&mut backend, &world.soft_bodies); + + let style = DebugRenderStyle::default(); + let depth: Vec<_> = backend + .lines + .iter() + .filter(|(.., color)| *color == style.contact_depth_color) + .collect(); + let normals: Vec<_> = backend + .lines + .iter() + .filter(|(.., color)| *color == style.contact_normal_color) + .collect(); + + // The self contacts reach the renderer, with a normal each so they are visible against the + // cage they lie on. + assert_eq!(depth.len(), contacts.len()); + assert!(!normals.is_empty()); + for (a, b, _) in &normals { + assert!(((*b - *a).length() - style.contact_normal_length).abs() < 1.0e-5); + } + + // Witness pairs are at most a contact apart (a centroid could be half an element away, which + // this scene keeps well above), and both witness points lie on the body's own surface. + for (a, b, _) in &depth { + for p in [a, b] { + let on_surface = sb.meshes().any(|mesh| { + (0..mesh.vertex_count()).any(|i| (mesh.vertex(sb, i) - *p).length() < SPACING) + }); + assert!(on_surface, "contact witness point {p:?} is off the surface"); + } + } +} diff --git a/crates/rapier2d/tests/speed_cap.rs b/crates/rapier2d/tests/speed_cap.rs index f5e40e132..6ecd7e8e6 100644 --- a/crates/rapier2d/tests/speed_cap.rs +++ b/crates/rapier2d/tests/speed_cap.rs @@ -8,6 +8,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -24,6 +25,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -45,6 +47,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier2d/tests/spinner_containment.rs b/crates/rapier2d/tests/spinner_containment.rs index 5e0fbeeee..8e7bdb331 100644 --- a/crates/rapier2d/tests/spinner_containment.rs +++ b/crates/rapier2d/tests/spinner_containment.rs @@ -16,6 +16,7 @@ fn spinner_keeps_bodies_contained() { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -98,6 +99,7 @@ fn spinner_keeps_bodies_contained() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d-mjcf/src/hooks.rs b/crates/rapier3d-mjcf/src/hooks.rs index e50295736..774faf791 100644 --- a/crates/rapier3d-mjcf/src/hooks.rs +++ b/crates/rapier3d-mjcf/src/hooks.rs @@ -71,16 +71,16 @@ impl PhysicsHooks for MjcfContactHooks { if self.exclude.contains(&(ctx.collider1, ctx.collider2)) { None } else { - Some(SolverFlags::COMPUTE_IMPULSES) + Some(SolverFlags::COMPUTE_RIGID_IMPULSES) } } fn modify_solver_contacts(&self, ctx: &mut ContactModificationContext) { let key = (ctx.collider1, ctx.collider2); if let Some(ov) = self.overrides.get(&key) { - if let Some(f) = ov.friction { + if let (Some(f), Some(rigid)) = (ov.friction, ctx.rigid_mut()) { // Contact materials are per-manifold since the solver-contact slimming. - *ctx.friction = f; + *rigid.friction = f; } } } diff --git a/crates/rapier3d-mjcf/tests/actuator_gain.rs b/crates/rapier3d-mjcf/tests/actuator_gain.rs index 8f7107a88..f933afc62 100644 --- a/crates/rapier3d-mjcf/tests/actuator_gain.rs +++ b/crates/rapier3d-mjcf/tests/actuator_gain.rs @@ -42,6 +42,7 @@ fn drive_to(steps: usize, gain_scale: Real) -> f32 { ); let link = robot.body_name_to_idx["link"]; let mut pipeline = PhysicsPipeline::new(); + let mut soft_bodies = SoftBodySet::new(); let ip = IntegrationParameters::default(); let (mut isl, mut bp, mut np, mut ccd) = ( IslandManager::new(), @@ -61,6 +62,7 @@ fn drive_to(steps: usize, gain_scale: Real) -> f32 { &mut colliders, &mut ij, &mut mbj, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d-mjcf/tests/cassie_smoke.rs b/crates/rapier3d-mjcf/tests/cassie_smoke.rs index 4f9a87fa0..28e003361 100644 --- a/crates/rapier3d-mjcf/tests/cassie_smoke.rs +++ b/crates/rapier3d-mjcf/tests/cassie_smoke.rs @@ -14,6 +14,7 @@ fn step( gravity: Vector, ) { let mut ccd = CCDSolver::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let integration_parameters = IntegrationParameters::default(); let mut islands = IslandManager::new(); @@ -32,6 +33,7 @@ fn step( colliders, impulse_joints, multibody_joints, + &mut soft_bodies, &mut ccd, &physics_hooks, &event_handler, diff --git a/crates/rapier3d-mjcf/tests/equality_joint.rs b/crates/rapier3d-mjcf/tests/equality_joint.rs index 0d8bd0645..af5358ac8 100644 --- a/crates/rapier3d-mjcf/tests/equality_joint.rs +++ b/crates/rapier3d-mjcf/tests/equality_joint.rs @@ -51,6 +51,7 @@ fn equality_joint_couples_multibody_joints() { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let handles = robot.clone().insert_using_multibody_joints( &mut bodies, &mut colliders, @@ -92,6 +93,7 @@ fn equality_joint_couples_multibody_joints() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d-mjcf/tests/example_smoke.rs b/crates/rapier3d-mjcf/tests/example_smoke.rs index f6d7d7c23..b16787fbc 100644 --- a/crates/rapier3d-mjcf/tests/example_smoke.rs +++ b/crates/rapier3d-mjcf/tests/example_smoke.rs @@ -53,6 +53,7 @@ fn step_n( n: usize, ) { let mut ccd = CCDSolver::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let integration_parameters = IntegrationParameters::default(); let mut islands = IslandManager::new(); @@ -72,6 +73,7 @@ fn step_n( colliders, impulse_joints, multibody_joints, + &mut soft_bodies, &mut ccd, &physics_hooks, &event_handler, diff --git a/crates/rapier3d-mjcf/tests/fixed_tendon.rs b/crates/rapier3d-mjcf/tests/fixed_tendon.rs index 0c63f0065..811d8817f 100644 --- a/crates/rapier3d-mjcf/tests/fixed_tendon.rs +++ b/crates/rapier3d-mjcf/tests/fixed_tendon.rs @@ -84,6 +84,7 @@ fn fixed_tendon_actuator_drives_both_joints() { // Drive the single tendon actuator toward 0.5. let ctrl = [0.5_f32]; let mut pipeline = PhysicsPipeline::new(); + let mut soft_bodies = SoftBodySet::new(); let ip = IntegrationParameters::default(); let (mut isl, mut bp, mut np, mut ccd) = ( IslandManager::new(), @@ -103,6 +104,7 @@ fn fixed_tendon_actuator_drives_both_joints() { &mut colliders, &mut ij, &mut mbj, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d-mjcf/tests/frictionloss.rs b/crates/rapier3d-mjcf/tests/frictionloss.rs index 0f3162907..7e17d22cb 100644 --- a/crates/rapier3d-mjcf/tests/frictionloss.rs +++ b/crates/rapier3d-mjcf/tests/frictionloss.rs @@ -181,6 +181,7 @@ fn damped_joint_still_swings_under_gravity() { let mut broad_phase = DefaultBroadPhase::new(); let mut narrow_phase = NarrowPhase::new(); let mut ccd = CCDSolver::new(); + let mut soft_bodies = SoftBodySet::new(); for _ in 0..60 { pipeline.step( @@ -193,6 +194,7 @@ fn damped_joint_still_swings_under_gravity() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d-mjcf/tests/keyframe_controls.rs b/crates/rapier3d-mjcf/tests/keyframe_controls.rs index cec8d9a17..05cce79bc 100644 --- a/crates/rapier3d-mjcf/tests/keyframe_controls.rs +++ b/crates/rapier3d-mjcf/tests/keyframe_controls.rs @@ -62,6 +62,7 @@ fn run( ctrl: &[Real], ) -> f32 { let mut pipeline = PhysicsPipeline::new(); + let mut soft_bodies = SoftBodySet::new(); let ip = IntegrationParameters::default(); let (mut isl, mut bp, mut np, mut ccd) = ( IslandManager::new(), @@ -81,6 +82,7 @@ fn run( colliders, ij, mbj, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d-mjcf/tests/loader_regressions.rs b/crates/rapier3d-mjcf/tests/loader_regressions.rs index acda37d63..c948ae714 100644 --- a/crates/rapier3d-mjcf/tests/loader_regressions.rs +++ b/crates/rapier3d-mjcf/tests/loader_regressions.rs @@ -15,6 +15,7 @@ struct World { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, integration_parameters: IntegrationParameters, islands: IslandManager, @@ -30,6 +31,7 @@ impl World { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), integration_parameters: IntegrationParameters::default(), islands: IslandManager::new(), @@ -50,6 +52,7 @@ impl World { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d-mjcf/tests/phase1.rs b/crates/rapier3d-mjcf/tests/phase1.rs index 0a4b6e8a8..84a9c1237 100644 --- a/crates/rapier3d-mjcf/tests/phase1.rs +++ b/crates/rapier3d-mjcf/tests/phase1.rs @@ -39,6 +39,7 @@ fn cartpole_loads_and_steps() { assert_eq!(impulse_joints.len(), 2); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut ccd = CCDSolver::new(); let mut pipeline = PhysicsPipeline::new(); let integration_parameters = IntegrationParameters::default(); @@ -60,6 +61,7 @@ fn cartpole_loads_and_steps() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &physics_hooks, &event_handler, diff --git a/crates/rapier3d-mjcf/tests/phase2.rs b/crates/rapier3d-mjcf/tests/phase2.rs index 144e423ac..8dae9f842 100644 --- a/crates/rapier3d-mjcf/tests/phase2.rs +++ b/crates/rapier3d-mjcf/tests/phase2.rs @@ -139,6 +139,7 @@ fn inertia_from_geom_when_no_inertial() { // Force a mass-properties update by simulating one step. use rapier3d::prelude::*; let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut ccd = CCDSolver::new(); let mut pipeline = PhysicsPipeline::new(); let integration_parameters = IntegrationParameters::default(); @@ -158,6 +159,7 @@ fn inertia_from_geom_when_no_inertial() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &physics_hooks, &event_handler, diff --git a/crates/rapier3d-mjcf/tests/phase3.rs b/crates/rapier3d-mjcf/tests/phase3.rs index 41948cd47..cc071312e 100644 --- a/crates/rapier3d-mjcf/tests/phase3.rs +++ b/crates/rapier3d-mjcf/tests/phase3.rs @@ -11,6 +11,7 @@ fn step_n( hooks: &dyn PhysicsHooks, ) { let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut ccd = CCDSolver::new(); let mut pipeline = PhysicsPipeline::new(); let integration_parameters = IntegrationParameters::default(); @@ -30,6 +31,7 @@ fn step_n( colliders, impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, hooks, &event_handler, diff --git a/crates/rapier3d-mjcf/tests/phase4.rs b/crates/rapier3d-mjcf/tests/phase4.rs index d57e4a114..37c33b1e8 100644 --- a/crates/rapier3d-mjcf/tests/phase4.rs +++ b/crates/rapier3d-mjcf/tests/phase4.rs @@ -12,6 +12,7 @@ fn step_n( gravity: Vector, ) { let mut ccd = CCDSolver::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let integration_parameters = IntegrationParameters::default(); let mut islands = IslandManager::new(); @@ -30,6 +31,7 @@ fn step_n( colliders, impulse_joints, multibody_joints, + &mut soft_bodies, &mut ccd, &physics_hooks, &event_handler, diff --git a/crates/rapier3d-mjcf/tests/phase5.rs b/crates/rapier3d-mjcf/tests/phase5.rs index 385b3dd40..544772708 100644 --- a/crates/rapier3d-mjcf/tests/phase5.rs +++ b/crates/rapier3d-mjcf/tests/phase5.rs @@ -11,6 +11,7 @@ fn step( n: usize, ) { let mut ccd = CCDSolver::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let integration_parameters = IntegrationParameters::default(); let mut islands = IslandManager::new(); @@ -30,6 +31,7 @@ fn step( colliders, impulse_joints, multibody_joints, + &mut soft_bodies, &mut ccd, &physics_hooks, &event_handler, @@ -165,6 +167,7 @@ fn drive_servo_to(target: Real, multibody: bool) -> Real { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); enum Ctl { Impulse(rapier3d_mjcf::MjcfRobotHandles), @@ -213,6 +216,7 @@ fn drive_servo_to(target: Real, multibody: bool) -> Real { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d/tests/additional_solver_iterations.rs b/crates/rapier3d/tests/additional_solver_iterations.rs index bf64ed4e6..aa7e492a9 100644 --- a/crates/rapier3d/tests/additional_solver_iterations.rs +++ b/crates/rapier3d/tests/additional_solver_iterations.rs @@ -9,6 +9,7 @@ struct World { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, islands: IslandManager, broad_phase: DefaultBroadPhase, @@ -23,6 +24,7 @@ impl World { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), islands: IslandManager::new(), broad_phase: DefaultBroadPhase::new(), @@ -42,6 +44,7 @@ impl World { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d/tests/ccd_default_vs_fixed.rs b/crates/rapier3d/tests/ccd_default_vs_fixed.rs index 9d37ab50c..1158fc90e 100644 --- a/crates/rapier3d/tests/ccd_default_vs_fixed.rs +++ b/crates/rapier3d/tests/ccd_default_vs_fixed.rs @@ -10,6 +10,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -26,6 +27,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -48,6 +50,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d/tests/contact_clustering.rs b/crates/rapier3d/tests/contact_clustering.rs index af7174695..59a68d5ac 100644 --- a/crates/rapier3d/tests/contact_clustering.rs +++ b/crates/rapier3d/tests/contact_clustering.rs @@ -16,6 +16,7 @@ struct World { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, ccd: CCDSolver, params: IntegrationParameters, } @@ -32,6 +33,7 @@ impl World { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), @@ -95,6 +97,7 @@ fn box_on_trimesh_floor(contact_clustering: bool) -> (World, RigidBodyHandle, Co colliders, impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), ccd: CCDSolver::new(), params: IntegrationParameters { contact_clustering, @@ -138,17 +141,17 @@ fn box_rests_stably_on_trimesh_with_clustering() { .contact_pair(box_co, world.colliders.iter().next().unwrap().0) .expect("no contact pair between the box and the floor"); assert!( - pair.manifolds.len() > 1, + pair.manifolds().len() > 1, "test setup must yield multiple per-triangle manifolds, got {}", - pair.manifolds.len() + pair.manifolds().len() ); - assert_eq!(pair.solver_clusters.len(), 1); - let cluster = &pair.solver_clusters[0]; + assert_eq!(pair.rigid().unwrap().solver_clusters.len(), 1); + let cluster = &pair.rigid().unwrap().solver_clusters[0]; assert!(!cluster.data.solver_contacts.is_empty()); assert!(cluster.data.solver_contacts.len() <= 4); // The cluster is what the solver saw: its contacts hold the impulses that support - // the box against gravity, and warm-starting must have carried them across steps. + // the box against gravity, and warm-starting must have kept them across steps. // Only the points selected as solver contacts hold the impulses of the last solve // (unselected points may keep stale values, like with parry's contact matching). let total_impulse: Real = cluster @@ -177,11 +180,11 @@ fn box_rests_stably_on_trimesh_with_clustering() { // The plain manifolds are still exposed for queries, but hold no solver contacts. assert!( - pair.manifolds + pair.manifolds() .iter() .all(|m| m.data.solver_contacts.is_empty()) ); - assert!(pair.manifolds.iter().any(|m| !m.points.is_empty())); + assert!(pair.manifolds().iter().any(|m| !m.points.is_empty())); } #[test] diff --git a/crates/rapier3d/tests/contact_force_event_clustering.rs b/crates/rapier3d/tests/contact_force_event_clustering.rs index 0d102c673..0b01e13ea 100644 --- a/crates/rapier3d/tests/contact_force_event_clustering.rs +++ b/crates/rapier3d/tests/contact_force_event_clustering.rs @@ -1,8 +1,8 @@ //! `ContactForceEvent` must report the forces the solver actually applied. //! //! With contact clustering (on by default in 3D for pairs with several manifolds) the -//! solver writes its impulses back into `pair.solver_clusters`, so an event built from -//! `pair.manifolds` reports a zero `total_force` and a degenerate `max_force_*`. +//! solver writes its impulses back into `pair.rigid().unwrap().solver_clusters`, so an event built from +//! `pair.manifolds()` reports a zero `total_force` and a degenerate `max_force_*`. use std::sync::Mutex; @@ -39,6 +39,8 @@ impl EventHandler for ForceEvents { total_force_magnitude, )); } + + fn handle_soft_body_tear_event(&self, _: &SoftBodySet, _: &SoftBodyTearEvent) {} } #[test] @@ -47,7 +49,7 @@ fn contact_force_event_is_populated_under_contact_clustering() { let mut world = PhysicsWorld::new(); // A two-triangle quad: a box resting across the diagonal touches both triangles, - // so the pair carries two manifolds and clustering applies to it. + // so the pair has two manifolds and clustering applies to it. let vertices = vec![ Vector::new(-2.0, 0.0, -2.0), Vector::new(2.0, 0.0, -2.0), @@ -79,12 +81,12 @@ fn contact_force_event_is_populated_under_contact_clustering() { .contact_pair(ground_collider, box_collider) .expect("no contact pair between the box and the ground"); assert!( - pair.manifolds.len() > 1, + pair.manifolds().len() > 1, "expected several manifolds on the trimesh pair, got {}", - pair.manifolds.len() + pair.manifolds().len() ); assert!( - !pair.solver_clusters.is_empty(), + !pair.rigid().unwrap().solver_clusters.is_empty(), "contact clustering did not apply to the pair" ); diff --git a/crates/rapier3d/tests/issue_182_heightfield_seam.rs b/crates/rapier3d/tests/issue_182_heightfield_seam.rs index cc03e25a7..662fb46c1 100644 --- a/crates/rapier3d/tests/issue_182_heightfield_seam.rs +++ b/crates/rapier3d/tests/issue_182_heightfield_seam.rs @@ -9,6 +9,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -25,6 +26,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -46,6 +48,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d/tests/issue_217_ccd_large_dt_hitch.rs b/crates/rapier3d/tests/issue_217_ccd_large_dt_hitch.rs index 20b78f78f..78f312b36 100644 --- a/crates/rapier3d/tests/issue_217_ccd_large_dt_hitch.rs +++ b/crates/rapier3d/tests/issue_217_ccd_large_dt_hitch.rs @@ -11,6 +11,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -27,6 +28,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -49,6 +51,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d/tests/issue_372_trimesh_sleep.rs b/crates/rapier3d/tests/issue_372_trimesh_sleep.rs index 71289d782..b36092557 100644 --- a/crates/rapier3d/tests/issue_372_trimesh_sleep.rs +++ b/crates/rapier3d/tests/issue_372_trimesh_sleep.rs @@ -12,6 +12,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -28,6 +29,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -49,6 +51,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d/tests/issue_485_kcc_depenetration.rs b/crates/rapier3d/tests/issue_485_kcc_depenetration.rs index 6536879e0..eef7d3ce1 100644 --- a/crates/rapier3d/tests/issue_485_kcc_depenetration.rs +++ b/crates/rapier3d/tests/issue_485_kcc_depenetration.rs @@ -10,6 +10,7 @@ struct World { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, islands: IslandManager, broad_phase: DefaultBroadPhase, @@ -23,6 +24,7 @@ impl World { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), islands: IslandManager::new(), broad_phase: DefaultBroadPhase::new(), @@ -41,6 +43,7 @@ impl World { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut CCDSolver::new(), &(), &(), diff --git a/crates/rapier3d/tests/issue_524_thin_slab_trimesh_tunnel.rs b/crates/rapier3d/tests/issue_524_thin_slab_trimesh_tunnel.rs index 1ab92cd6e..7eb8d4a84 100644 --- a/crates/rapier3d/tests/issue_524_thin_slab_trimesh_tunnel.rs +++ b/crates/rapier3d/tests/issue_524_thin_slab_trimesh_tunnel.rs @@ -12,6 +12,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -28,6 +29,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -49,6 +51,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d/tests/issue_810_cubes_thin_cylinder_tunnel.rs b/crates/rapier3d/tests/issue_810_cubes_thin_cylinder_tunnel.rs index daf535af8..7ba515a00 100644 --- a/crates/rapier3d/tests/issue_810_cubes_thin_cylinder_tunnel.rs +++ b/crates/rapier3d/tests/issue_810_cubes_thin_cylinder_tunnel.rs @@ -13,6 +13,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -29,6 +30,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -50,6 +52,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d/tests/issue_862_kcc_impulse_point.rs b/crates/rapier3d/tests/issue_862_kcc_impulse_point.rs index 176d1ea4d..77472eac2 100644 --- a/crates/rapier3d/tests/issue_862_kcc_impulse_point.rs +++ b/crates/rapier3d/tests/issue_862_kcc_impulse_point.rs @@ -17,6 +17,7 @@ fn run_push(body_translation: Vector, collider_offset: Vector) -> (Vector, Vecto let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut islands = IslandManager::new(); let mut broad_phase = DefaultBroadPhase::new(); @@ -41,6 +42,7 @@ fn run_push(body_translation: Vector, collider_offset: Vector) -> (Vector, Vecto &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -96,7 +98,7 @@ fn run_push(body_translation: Vector, collider_offset: Vector) -> (Vector, Vecto fn impulse_applied_at_collider_world_point() { // Same physical scene, two representations: // - baseline: the collider's offset is baked into the body position; - // - offset: the body sits at the origin and the collider carries the offset. + // - offset: the body sits at the origin and the collider has the offset. let (baseline_linvel, baseline_angvel) = run_push(Vector::new(0.0, 3.0, 0.0), Vector::ZERO); let (offset_linvel, offset_angvel) = run_push(Vector::ZERO, Vector::new(0.0, 3.0, 0.0)); diff --git a/crates/rapier3d/tests/issue_932_low_ccd_substeps_stutter.rs b/crates/rapier3d/tests/issue_932_low_ccd_substeps_stutter.rs index 938b50c89..45029ee93 100644 --- a/crates/rapier3d/tests/issue_932_low_ccd_substeps_stutter.rs +++ b/crates/rapier3d/tests/issue_932_low_ccd_substeps_stutter.rs @@ -11,6 +11,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -27,6 +28,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -48,6 +50,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d/tests/issue_974_restitution.rs b/crates/rapier3d/tests/issue_974_restitution.rs index 1573223cc..32877e28f 100644 --- a/crates/rapier3d/tests/issue_974_restitution.rs +++ b/crates/rapier3d/tests/issue_974_restitution.rs @@ -36,6 +36,7 @@ fn rebound(e: f32) -> f32 { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut ccd = CCDSolver::new(); let params = IntegrationParameters::default(); let gravity = Vector::new(0.0, -9.81, 0.0); @@ -53,6 +54,7 @@ fn rebound(e: f32) -> f32 { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d/tests/loop_closure_com_repro.rs b/crates/rapier3d/tests/loop_closure_com_repro.rs index 46f3ff5b0..bcddfa533 100644 --- a/crates/rapier3d/tests/loop_closure_com_repro.rs +++ b/crates/rapier3d/tests/loop_closure_com_repro.rs @@ -30,6 +30,7 @@ struct World { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, link1: RigidBodyHandle, link2: RigidBodyHandle, } @@ -90,6 +91,7 @@ fn build(use_multibody: bool, closure_anchor1: Vector) -> World { colliders, impulse_joints, multibody_joints, + soft_bodies: SoftBodySet::new(), link1, link2, } @@ -120,6 +122,7 @@ fn simulate(w: &mut World, closure_anchor1: Vector) -> (Real, Real) { &mut w.colliders, &mut w.impulse_joints, &mut w.multibody_joints, + &mut w.soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d/tests/miri_scenes.rs b/crates/rapier3d/tests/miri_scenes.rs index 8fb58da2f..e574e5dbd 100644 --- a/crates/rapier3d/tests/miri_scenes.rs +++ b/crates/rapier3d/tests/miri_scenes.rs @@ -22,6 +22,7 @@ struct World { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, islands: IslandManager, broad_phase: DefaultBroadPhase, narrow_phase: NarrowPhase, @@ -38,6 +39,7 @@ impl World { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), islands: IslandManager::new(), broad_phase: DefaultBroadPhase::new(), narrow_phase: NarrowPhase::new(), @@ -59,6 +61,7 @@ impl World { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d/tests/multibody_armature.rs b/crates/rapier3d/tests/multibody_armature.rs index 4d9f8f780..28c3ed710 100644 --- a/crates/rapier3d/tests/multibody_armature.rs +++ b/crates/rapier3d/tests/multibody_armature.rs @@ -43,6 +43,7 @@ fn spin_up(armature: Real, steps: usize) -> Real { let mut broad_phase = DefaultBroadPhase::new(); let mut narrow_phase = NarrowPhase::new(); let mut ccd = CCDSolver::new(); + let mut soft_bodies = SoftBodySet::new(); let mut step_once = |bodies: &mut RigidBodySet, mbj: &mut MultibodyJointSet| { pipeline.step( @@ -55,6 +56,7 @@ fn spin_up(armature: Real, steps: usize) -> Real { &mut colliders, &mut impulse_joints, mbj, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d/tests/multibody_coupling.rs b/crates/rapier3d/tests/multibody_coupling.rs index 2b9f227d3..ac15f94fd 100644 --- a/crates/rapier3d/tests/multibody_coupling.rs +++ b/crates/rapier3d/tests/multibody_coupling.rs @@ -13,6 +13,7 @@ fn run_coupling(coeff: Real, offset: Real, target: Real) -> (Real, Real) { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let inertia = Vector::new(0.1, 0.1, 0.1); let base = bodies.insert(RigidBodyBuilder::fixed()); @@ -79,6 +80,7 @@ fn run_coupling(coeff: Real, offset: Real, target: Real) -> (Real, Real) { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d/tests/multibody_dof_inertia.rs b/crates/rapier3d/tests/multibody_dof_inertia.rs index 72bae555a..bd787c7f0 100644 --- a/crates/rapier3d/tests/multibody_dof_inertia.rs +++ b/crates/rapier3d/tests/multibody_dof_inertia.rs @@ -62,12 +62,9 @@ fn leaf_joint_inverse_inertia_is_link_inertia() { #[test] fn parent_joint_inverse_inertia_accounts_for_children() { - // Base→link1(hinge Z)→link2(hinge Z), all rotating about the same world Z - // through the origin so the joint-space inertia is diagonal and easy to - // reason about: M[0,0] = Izz1 + Izz2 (link1's DoF carries both links), - // M[1,1] = Izz2. The off-diagonal is Izz2 (shared axis), so M is NOT - // diagonal and diag(M⁻¹) differs from 1/diag(M) — this is the articulated - // coupling we want. + // Base→link1(hinge Z)→link2(hinge Z), all about the same world Z through the origin, so + // M[0,0] = Izz1 + Izz2, M[1,1] = Izz2, off-diagonal Izz2: M is not diagonal and diag(M⁻¹) + // differs from 1/diag(M), the articulated coupling under test. let mut bodies = RigidBodySet::new(); let mut multibody_joints = MultibodyJointSet::new(); let base = bodies.insert(RigidBodyBuilder::fixed()); diff --git a/crates/rapier3d/tests/multibody_spring.rs b/crates/rapier3d/tests/multibody_spring.rs index 71d0e7f74..1822aa867 100644 --- a/crates/rapier3d/tests/multibody_spring.rs +++ b/crates/rapier3d/tests/multibody_spring.rs @@ -32,6 +32,7 @@ fn settle_angle(stiffness: Real, rest: Real, damping: Real, inertia: Real, steps let mut broad_phase = DefaultBroadPhase::new(); let mut narrow_phase = NarrowPhase::new(); let mut ccd = CCDSolver::new(); + let mut soft_bodies = SoftBodySet::new(); let mut step = |bodies: &mut RigidBodySet, mbj: &mut MultibodyJointSet| { pipeline.step( @@ -44,6 +45,7 @@ fn settle_angle(stiffness: Real, rest: Real, damping: Real, inertia: Real, steps &mut colliders, &mut impulse_joints, mbj, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d/tests/persistent_islands.rs b/crates/rapier3d/tests/persistent_islands.rs index b548e6f26..ce828f3e5 100644 --- a/crates/rapier3d/tests/persistent_islands.rs +++ b/crates/rapier3d/tests/persistent_islands.rs @@ -1,6 +1,6 @@ //! Structural tests for the persistent islands: eager merge on //! touch/joint-link, deferred split (one island per step) after constraint -//! removals, and link refreshes on body lifecycle edits. +//! removals, and link updates on body lifecycle edits. //! //! These tests observe island *equality* between bodies through the //! test-only `IslandManager::persistent_island_of` accessor; the heavy diff --git a/crates/rapier3d/tests/simd_backend_determinism.rs b/crates/rapier3d/tests/simd_backend_determinism.rs index f237969cd..faf19917d 100644 --- a/crates/rapier3d/tests/simd_backend_determinism.rs +++ b/crates/rapier3d/tests/simd_backend_determinism.rs @@ -63,6 +63,7 @@ fn run(steps: usize) -> u64 { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut broad_phase = BroadPhaseBvh::new(); let mut narrow_phase = NarrowPhase::new(); @@ -128,6 +129,7 @@ fn run(steps: usize) -> u64 { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/crates/rapier3d/tests/sleep_wake.rs b/crates/rapier3d/tests/sleep_wake.rs index 268dd1492..cb9bd5853 100644 --- a/crates/rapier3d/tests/sleep_wake.rs +++ b/crates/rapier3d/tests/sleep_wake.rs @@ -46,6 +46,7 @@ fn woken_body_is_supported_by_recycled_contacts() { colliders, impulse_joints, multibody_joints, + &mut SoftBodySet::new(), ccd, &(), &(), @@ -104,6 +105,7 @@ struct World { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -119,6 +121,7 @@ impl World { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -158,6 +161,7 @@ impl World { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), @@ -554,7 +558,7 @@ fn slow_dynamic_intruder_wakes_sleeping_body() { } /// Domino-spiral variant of the load-band wake: the wedge tip already touches -/// (carrying ~no load) when the standing dominoes fall asleep, so the sleep-time +/// (bearing ~no load) when the standing dominoes fall asleep, so the sleep-time /// monitor must stay load-armed (unloaded reference) or the ring never falls. #[test] fn grazing_wedge_wakes_sleeping_chain() { diff --git a/crates/rapier3d/tests/snapshot_portability.rs b/crates/rapier3d/tests/snapshot_portability.rs index efc1faad0..23d8b1c71 100644 --- a/crates/rapier3d/tests/snapshot_portability.rs +++ b/crates/rapier3d/tests/snapshot_portability.rs @@ -31,7 +31,7 @@ use rapier3d::prelude::*; /// Snapshot size in bytes, and its FNV-1a digest. Both, because the size alone localizes a /// failure: a differing size means a container's *encoding* changed, an equal size with a /// differing digest means the values did. -const GOLDEN: (usize, u64) = (481_608, 0xedfe_b12c_c030_e444); +const GOLDEN: (usize, u64) = (488_334, 0x50d4_0720_b93e_831b); const STEPS: usize = 60; diff --git a/crates/rapier3d/tests/snapshot_roundtrip.rs b/crates/rapier3d/tests/snapshot_roundtrip.rs index 92740db8b..f6b42d1c8 100644 --- a/crates/rapier3d/tests/snapshot_roundtrip.rs +++ b/crates/rapier3d/tests/snapshot_roundtrip.rs @@ -304,7 +304,7 @@ fn pyramid_stress_scene_full() { check_roundtrip("pyramid50", world, 100, 20, false, no_edits); } -/// Fast CCD-enabled bodies. The CCD solver holds a cache that snapshots do not carry, so +/// Fast CCD-enabled bodies. The CCD solver holds a cache that snapshots do not include, so /// this checks that a restore does not depend on it. #[test] fn ccd_bodies() { @@ -381,7 +381,7 @@ fn multibody_articulation() { } /// The testbed's Save/Restore pattern: the world is replaced but the same -/// [`PhysicsPipeline`] keeps stepping, carrying its workspace across the restore. +/// [`PhysicsPipeline`] keeps stepping, keeping its workspace across the restore. #[test] fn restoring_into_a_live_pipeline() { check_roundtrip("live pipeline", pile(true), 100, 100, true, no_edits); diff --git a/crates/rapier3d/tests/speed_cap.rs b/crates/rapier3d/tests/speed_cap.rs index 1bc6d69bc..a6ba41978 100644 --- a/crates/rapier3d/tests/speed_cap.rs +++ b/crates/rapier3d/tests/speed_cap.rs @@ -8,6 +8,7 @@ struct Harness { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + soft_bodies: SoftBodySet, pipeline: PhysicsPipeline, bf: BroadPhaseBvh, nf: NarrowPhase, @@ -24,6 +25,7 @@ impl Harness { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), pipeline: PhysicsPipeline::new(), bf: BroadPhaseBvh::new(), nf: NarrowPhase::new(), @@ -45,6 +47,7 @@ impl Harness { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd, &(), &(), diff --git a/crates/rapier3d/tests/thread_count_determinism.rs b/crates/rapier3d/tests/thread_count_determinism.rs index a91064893..c9c8eadba 100644 --- a/crates/rapier3d/tests/thread_count_determinism.rs +++ b/crates/rapier3d/tests/thread_count_determinism.rs @@ -103,6 +103,7 @@ fn run_sim(num_threads: usize, num_steps: usize) -> Vec { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -167,6 +168,26 @@ fn run_sim(num_threads: usize, num_steps: usize) -> Vec { .active_events(ActiveEvents::COLLISION_EVENTS), ); + // Soft bodies falling on the pile: a cloth, a corotational jelly cube and a pressurized + // balloon (colored soft constraints, shape/volume prepare stages, serial volume constraint). + let cloth = SoftBodyBuilder::cloth( + Vector::new(-2.0, 4.0, -2.0), + Vector::X * 0.2, + Vector::Z * 0.2, + 20, + 20, + ) + .particle_mass(0.05); + soft_bodies.insert(cloth, &mut bodies, &mut colliders); + let jelly = SoftBodyBuilder::cuboid(Vector::new(4.0, 5.0, 4.0), Vector::splat(0.6), 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .particle_mass(0.2); + soft_bodies.insert(jelly, &mut bodies, &mut colliders); + let balloon = SoftBodyBuilder::sphere(Vector::new(-4.0, 5.0, 4.0), 0.8, 2) + .shape_matching(true) + .particle_mass(0.05); + soft_bodies.insert(balloon, &mut bodies, &mut colliders); + let events = EventLog::default(); let mut snapshots = Vec::new(); for step in 0..num_steps { @@ -197,6 +218,7 @@ fn run_sim(num_threads: usize, num_steps: usize) -> Vec { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &events, diff --git a/crates/rapier3d/tests/unsync_callbacks.rs b/crates/rapier3d/tests/unsync_callbacks.rs index a4f290016..129edc5de 100644 --- a/crates/rapier3d/tests/unsync_callbacks.rs +++ b/crates/rapier3d/tests/unsync_callbacks.rs @@ -23,7 +23,7 @@ struct UnsyncHooks { impl PhysicsHooks for UnsyncHooks { fn filter_contact_pair(&self, _: &PairFilterContext) -> Option { self.filtered.set(self.filtered.get() + 1); - Some(SolverFlags::COMPUTE_IMPULSES) + Some(SolverFlags::COMPUTE_RIGID_IMPULSES) } } @@ -52,6 +52,8 @@ impl EventHandler for UnsyncEvents { _total_force_magnitude: Real, ) { } + + fn handle_soft_body_tear_event(&self, _soft_bodies: &SoftBodySet, _event: &SoftBodyTearEvent) {} } /// Enough falling boxes to push the step onto its parallel paths where `parallel` is on. @@ -102,6 +104,7 @@ fn non_sync_hooks_and_events_are_invoked() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let params = IntegrationParameters::default(); @@ -119,6 +122,7 @@ fn non_sync_hooks_and_events_are_invoked() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd_solver, &hooks, &events, @@ -162,6 +166,7 @@ fn dedicated_pool_is_usable_when_the_caller_enters_it() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let params = IntegrationParameters::default(); @@ -176,6 +181,7 @@ fn dedicated_pool_is_usable_when_the_caller_enters_it() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd_solver, &hooks, &events, diff --git a/examples2d/b2d_many_pyramids.rs b/examples2d/b2d_many_pyramids.rs index 1039dba4f..385e89878 100644 --- a/examples2d/b2d_many_pyramids.rs +++ b/examples2d/b2d_many_pyramids.rs @@ -38,7 +38,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let ground_delta_y = 2.0 * extent * (base_count as f32 + 1.0); let ground_width = 2.0 * extent * column_count as f32 * (base_count as f32 + 1.0); - // Ground: one static body carrying `row_count` horizontal segments. + // Ground: one static body with `row_count` horizontal segments. let ground = world.insert_body(RigidBodyBuilder::fixed()); let mut ground_y = 0.0f32; for _ in 0..row_count { diff --git a/examples2d/b2d_rain.rs b/examples2d/b2d_rain.rs index 17677fe55..d483f1e07 100644 --- a/examples2d/b2d_rain.rs +++ b/examples2d/b2d_rain.rs @@ -157,7 +157,7 @@ struct BoneDef { cap_a: [f32; 2], cap_b: [f32; 2], cap_r: f32, - /// Whether this bone carries the shared foot polygon (lower legs). + /// Whether this bone has the shared foot polygon (lower legs). has_foot: bool, /// Joint pivot Y (world offset from origin); joint to the parent. pivot_y: f32, diff --git a/examples2d/debug_many_colliders2.rs b/examples2d/debug_many_colliders2.rs index c05a6c9d6..8fa379ecd 100644 --- a/examples2d/debug_many_colliders2.rs +++ b/examples2d/debug_many_colliders2.rs @@ -1,9 +1,9 @@ -//! Reproduction of issue #970: a single always-awake dynamic body carrying 2,210 convex +//! Reproduction of issue #970: a single always-awake dynamic body with 2,210 convex //! colliders (130 copies of a 17-part convex decomposition, all at the same pose), in a //! zero-gravity world with nothing else. The body spins forever, so the step cost is //! entirely spent processing the moving colliders of that one body. //! -//! Set [`WITH_COMPOUND_COMPARISON`] to `true` to also spawn a second body carrying the +//! Set [`WITH_COMPOUND_COMPARISON`] to `true` to also spawn a second body with the //! very same parts as one compound-shape collider per copy: the broad phase then sees //! 130 colliders for it instead of 2,210. @@ -12,7 +12,7 @@ use rapier2d::prelude::*; /// The poster's 17 convex polygons (a convex decomposition of a complex outline), /// in the original 0.01-scaled coordinates. -// The vertex data is verbatim from the issue's JS reproduction, where two vertices carry +// The vertex data is verbatim from the issue's JS reproduction, where two vertices have // f64 precision. #[allow(clippy::excessive_precision)] fn decomposition_parts() -> Vec> { diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index da38c56ee..1e4b32266 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -47,8 +47,10 @@ impl PhysicsHooks for OneWayPlatformHook { 12.0 }; - for contact in context.solver_contacts.iter_mut() { - contact.tangent_velocity.x = tangent_velocity; + if let Some(rigid) = context.rigid_mut() { + for contact in rigid.solver_contacts.iter_mut() { + contact.tangent_velocity.x = tangent_velocity; + } } } } diff --git a/examples2d/stress_tests/many_pyramids2.rs b/examples2d/stress_tests/many_pyramids2.rs index 0373f2e01..9b59cc8e0 100644 --- a/examples2d/stress_tests/many_pyramids2.rs +++ b/examples2d/stress_tests/many_pyramids2.rs @@ -17,7 +17,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let column_count = 20; /* - * Ground: one static body carrying one segment per pyramid row. + * Ground: one static body with one segment per pyramid row. */ let ground_delta_y = 2.0 * extent * (base_count as f32 + 1.0); let ground_width = 2.0 * extent * column_count as f32 * (base_count as f32 + 1.0); diff --git a/examples3d/b3d_rain.rs b/examples3d/b3d_rain.rs index 40d872c59..778567ddb 100644 --- a/examples3d/b3d_rain.rs +++ b/examples3d/b3d_rain.rs @@ -104,7 +104,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let mut world = PhysicsWorld::new(); world.gravity = Vector::new(0.0, -10.0, 0.0); - // Static cells: a 10x10 grid, each cell one static body carrying a small + // Static cells: a 10x10 grid, each cell one static body with a small // grid-mesh floor patch and a torus obstacle. let half_mesh_grid_rows = 4; let mesh_cell_width = GRID_SIZE / (2.0 * half_mesh_grid_rows as f32); diff --git a/examples3d/debug_multi_collider_body3.rs b/examples3d/debug_multi_collider_body3.rs index 97ad22c63..a8a8db1f5 100644 --- a/examples3d/debug_multi_collider_body3.rs +++ b/examples3d/debug_multi_collider_body3.rs @@ -1,6 +1,6 @@ //! Debug scene for https://github.com/dimforge/rapier/issues/970 //! -//! One dynamic body carrying 8000 colliders (a 20x20x20 block of unit boxes) dropped tilted +//! One dynamic body with 8000 colliders (a 20x20x20 block of unit boxes) dropped tilted //! from 40m: the issue reported such a body keeping the step permanently slow, even though //! same-parent colliders never collide. Watch the step cost collapse once it sleeps (~5.7s). diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index 3a14f98ce..b236ac324 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -47,8 +47,10 @@ impl PhysicsHooks for OneWayPlatformHook { 12.0 }; - for contact in context.solver_contacts.iter_mut() { - contact.tangent_velocity.z = tangent_velocity; + if let Some(rigid) = context.rigid_mut() { + for contact in rigid.solver_contacts.iter_mut() { + contact.tangent_velocity.z = tangent_velocity; + } } } } diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs index 217ff4492..888724915 100644 --- a/src/control/character_controller.rs +++ b/src/control/character_controller.rs @@ -1003,6 +1003,7 @@ mod test { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -1087,6 +1088,7 @@ mod test { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -1172,6 +1174,7 @@ mod test { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -1235,6 +1238,7 @@ mod test { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -1317,6 +1321,7 @@ mod test { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -1350,6 +1355,7 @@ mod test { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -1387,6 +1393,7 @@ mod test { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -1442,6 +1449,7 @@ mod test { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index c9ab3c781..da33fa17d 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -19,10 +19,9 @@ use super::sweeps::{ /// and `next_position` is clamped to the earliest impact — velocities untouched, no re-solve; the /// residual approach resolves next step via speculative contacts. /// -/// Fast dynamic bodies automatically sweep against **fixed** colliders; `ccd_enabled` upgrades to -/// a *bullet* that also sweeps kinematic/dynamic bodies (never other bullets). Mesh-like colliders -/// are never swept as the *moving* shape (targets are fine), compounds sweep per -/// convex child, and [`IntegrationParameters::max_ccd_substeps`] `= 0` disables CCD entirely. +/// Fast dynamic bodies sweep against fixed colliders and soft-body meshes; `ccd_enabled` makes a +/// bullet that also sweeps kinematic/dynamic bodies (never other bullets); mesh-like colliders are +/// never the moving shape; compounds sweep per convex child; `max_ccd_substeps = 0` disables CCD. #[derive(Clone, Default)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct CCDSolver { @@ -156,8 +155,9 @@ impl CCDSolver { } /// Runs the continuous-collision pass on all fast bodies and clamps their `next_position` - /// to their earliest time of impact: non-bullets sweep fixed colliders first, then bullets - /// sweep every (possibly already clamped) body; velocities are never modified. Sensor + /// to their earliest time of impact: non-bullets sweep the automatic targets (fixed + /// colliders and soft-body collision meshes) first, then bullets sweep every (possibly + /// already clamped) body; velocities are never modified. Sensor /// crossings the narrow phase would miss entirely emit paired `Started`/`Stopped` /// intersection events. #[profiling::function] diff --git a/src/dynamics/ccd/sweeps.rs b/src/dynamics/ccd/sweeps.rs index c4d301b8d..f98eae590 100644 --- a/src/dynamics/ccd/sweeps.rs +++ b/src/dynamics/ccd/sweeps.rs @@ -32,7 +32,8 @@ pub(super) fn is_bullet(rb: &RigidBody) -> bool { } /// The target-selection rule for a fast body `rb1`: a non-bullet fast body only sweeps -/// against **fixed** targets; a bullet sweeps against every body type except other bullets. +/// against automatic targets (fixed geometry and soft-body meshes); a bullet sweeps against +/// every body type except other bullets. fn tier_allows(rb1: &RigidBody, rb2: Option<&RigidBody>) -> bool { if is_bullet(rb1) { !rb2.map(is_bullet).unwrap_or(false) @@ -125,12 +126,12 @@ fn intersect_swept_aabb<'a>( }) } -/// The candidate targets a fast body sweeps against. Non-bullets only ever hit **fixed** targets: -/// a flat AABB list skips walking the shared BVH past the dynamic neighbours (costly in dense -/// piles). Bullets and fixed-heavy worlds fall back to the full BVH. +/// The targets a fast body sweeps against. Non-bullets only hit automatic targets, so flat AABB +/// lists skip walking the shared BVH; bullets and fixed-heavy worlds use the full BVH. #[derive(Copy, Clone)] pub(super) enum CcdTargets<'a> { - /// Flat list of the fixed colliders and their (slightly fattened) AABBs. + /// Flat lists of the automatic targets and their (slightly fattened) AABBs: the cached + /// fixed colliders, and the soft-body collision meshes (rebuilt every step, as they deform). FixedList(&'a [(ColliderHandle, Aabb)]), /// The full broad-phase BVH. FullBvh(&'a Bvh), @@ -571,7 +572,6 @@ pub(super) fn sweep_fast_body( ) { return; } - if let Some(hit_fraction) = cast_collider_pair( dispatcher, &fast, diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 1be1ee01e..6cdfe8301 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -108,16 +108,27 @@ impl + Copy> SpringCoefficients { let one = N::one(); let erp = self.erp(dt); let erp_is_not_zero = erp.simd_ne(N::zero()); - let inv_erp_minus_one = one / erp - one; - // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass - // / (dt * dt * inv_erp_minus_one * inv_erp_minus_one); - // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass - // / (dt * inv_erp_minus_one); - // let cfm = 1.0 / (dt * dt * stiffness + dt * damping); - // NOTE: This simplifies to cfm = cfm_coeff / projected_mass: - let result = inv_erp_minus_one * inv_erp_minus_one - / ((one + inv_erp_minus_one) * N::splat(4.0) * self.damping_ratio * self.damping_ratio); + let damped = { + let inv_erp_minus_one = one / erp - one; + + // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass + // / (dt * dt * inv_erp_minus_one * inv_erp_minus_one); + // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass + // / (dt * inv_erp_minus_one); + // let cfm = 1.0 / (dt * dt * stiffness + dt * damping); + // NOTE: This simplifies to cfm = cfm_coeff / projected_mass: + inv_erp_minus_one * inv_erp_minus_one + / ((one + inv_erp_minus_one) * N::splat(4.0) * self.damping_ratio * self.damping_ratio) + }; + let undamped = { + // Undamped version if the damping ratio is zero. + let dt_omega = dt * self.angular_frequency(); + one / (dt_omega * (dt_omega + N::splat(2.0) * self.damping_ratio)) + }; + + let damping_is_zero = self.damping_ratio.simd_eq(N::zero()); + let result = undamped.select(damping_is_zero, damped); result.select(erp_is_not_zero, N::zero()) } diff --git a/src/dynamics/island_manager/global_split.rs b/src/dynamics/island_manager/global_split.rs index 7661c5ce9..8ffad0781 100644 --- a/src/dynamics/island_manager/global_split.rs +++ b/src/dynamics/island_manager/global_split.rs @@ -10,7 +10,7 @@ use super::persistent::{INVALID_ISLAND, INVALID_LOC, PersistentIslands, SPLIT_RE #[derive(Clone, Default)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub(super) struct SplitScratch { +pub(super) struct SplitWorkspace { uf: UnionFind, contact_counts: Vec, joint_counts: Vec, @@ -73,29 +73,29 @@ impl PersistentIslands { return; } - let mut scratch = core::mem::take(&mut self.split_scratch); - scratch.uf.reset(body_count); - scratch.contact_counts.clear(); - scratch.contact_counts.resize(body_count, 0); - scratch.joint_counts.clear(); - scratch.joint_counts.resize(body_count, 0); + let mut workspace = core::mem::take(&mut self.split_workspace); + workspace.uf.reset(body_count); + workspace.contact_counts.clear(); + workspace.contact_counts.resize(body_count, 0); + workspace.joint_counts.clear(); + workspace.joint_counts.resize(body_count, 0); // A body's union-find node id is its `island_index`. Rather than read it back from the // body arena (random, cache-missing access per link endpoint), index the island's members - // by arena index up front — handles carry everything needed, `island.bodies` is all live. - scratch.node_map_stamp = scratch.node_map_stamp.wrapping_add(1); - if scratch.node_map_stamp == 0 { + // by arena index up front; handles hold everything needed, `island.bodies` is all live. + workspace.node_map_stamp = workspace.node_map_stamp.wrapping_add(1); + if workspace.node_map_stamp == 0 { // Wrapped: a zeroed (never-written) entry would alias stamp 0. - scratch.node_map.clear(); - scratch.node_map_stamp = 1; + workspace.node_map.clear(); + workspace.node_map_stamp = 1; } - let stamp = scratch.node_map_stamp; + let stamp = workspace.node_map_stamp; for (node, handle) in self.islands[island_id as usize].bodies.iter().enumerate() { let (index, generation) = handle.into_raw_parts(); - if scratch.node_map.len() <= index as usize { - scratch.node_map.resize(index as usize + 1, (0, 0, 0)); + if workspace.node_map.len() <= index as usize { + workspace.node_map.resize(index as usize + 1, (0, 0, 0)); } - scratch.node_map[index as usize] = (stamp, generation, node as u32); + workspace.node_map[index as usize] = (stamp, generation, node as u32); } // An endpoint that is not a member of this island (fixed body, removed @@ -110,7 +110,7 @@ impl PersistentIslands { // a link goes from island-owned memory alone; `INVALID_ISLAND` marks a link whose two // endpoints both left the island (dropped below). let island = &self.islands[island_id as usize]; - let SplitScratch { + let SplitWorkspace { uf, contact_counts, joint_counts, @@ -118,7 +118,7 @@ impl PersistentIslands { joint_nodes, node_map, .. - } = &mut scratch; + } = &mut workspace; contact_nodes.clear(); contact_nodes.reserve(island.contact_links.len()); joint_nodes.clear(); @@ -156,15 +156,15 @@ impl PersistentIslands { // Flatten so the move passes below can resolve roots with immutable // single reads, then pick the biggest component (by the union-find's // set sizes, i.e. body counts): it keeps the base island. - scratch.uf.flatten(); + workspace.uf.flatten(); let mut component_count = 0usize; let mut keep_root = 0u32; let mut keep_size = 0u32; for i in 0..body_count as u32 { - if scratch.uf.root(i) == i { + if workspace.uf.root(i) == i { component_count += 1; - if scratch.uf.size(i) > keep_size { - keep_size = scratch.uf.size(i); + if workspace.uf.size(i) > keep_size { + keep_size = workspace.uf.size(i); keep_root = i; } } @@ -173,7 +173,7 @@ impl PersistentIslands { let island = &mut self.islands[island_id as usize]; island.constraint_remove_count = 0; island.split_denied_until = self.sleep_scan_stamp + SPLIT_RETRY_COOLDOWN; - self.split_scratch = scratch; + self.split_workspace = workspace; return; } @@ -182,36 +182,36 @@ impl PersistentIslands { // the final root. Fold them up onto final roots instead of trusting // them as-is. for i in 0..body_count as u32 { - let root = scratch.uf.root(i); + let root = workspace.uf.root(i); if root != i { - scratch.contact_counts[root as usize] += scratch.contact_counts[i as usize]; - scratch.joint_counts[root as usize] += scratch.joint_counts[i as usize]; - scratch.contact_counts[i as usize] = 0; - scratch.joint_counts[i as usize] = 0; + workspace.contact_counts[root as usize] += workspace.contact_counts[i as usize]; + workspace.joint_counts[root as usize] += workspace.joint_counts[i as usize]; + workspace.contact_counts[i as usize] = 0; + workspace.joint_counts[i as usize] = 0; } } // One new island per component, except `keep_root`'s, which stays in // the base island (and keeps its sleeping/cooldown state). let base_sleeping = self.islands[island_id as usize].sleeping; - scratch.root_island.clear(); - scratch.root_island.resize(body_count, INVALID_ISLAND); - scratch.root_island[keep_root as usize] = island_id; + workspace.root_island.clear(); + workspace.root_island.resize(body_count, INVALID_ISLAND); + workspace.root_island[keep_root as usize] = island_id; for i in 0..body_count { - let root = scratch.uf.root(i as u32) as usize; - if scratch.root_island[root] == INVALID_ISLAND { + let root = workspace.uf.root(i as u32) as usize; + if workspace.root_island[root] == INVALID_ISLAND { let new_id = self.alloc_island(); let island = &mut self.islands[new_id as usize]; island.sleeping = base_sleeping; - island.bodies.reserve(scratch.uf.size(root as u32) as usize); + island.bodies.reserve(workspace.uf.size(root as u32) as usize); island .contact_links - .reserve(scratch.contact_counts[root] as usize); + .reserve(workspace.contact_counts[root] as usize); island .joint_links - .reserve(scratch.joint_counts[root] as usize); - scratch.root_island[root] = new_id; + .reserve(workspace.joint_counts[root] as usize); + workspace.root_island[root] = new_id; } } @@ -220,12 +220,12 @@ impl PersistentIslands { // pulls in an element that stays: everything past the current index was already visited. let mut links = core::mem::take(&mut self.islands[island_id as usize].contact_links); for i in (0..links.len()).rev() { - let node = scratch.contact_nodes[i]; + let node = workspace.contact_nodes[i]; let target = if node == INVALID_ISLAND { // Both endpoints left (dead/fixed/disabled): drop the link. INVALID_ISLAND } else { - scratch.root_island[scratch.uf.root(node) as usize] + workspace.root_island[workspace.uf.root(node) as usize] }; if target == island_id { continue; @@ -249,11 +249,11 @@ impl PersistentIslands { let mut links = core::mem::take(&mut self.islands[island_id as usize].joint_links); for i in (0..links.len()).rev() { - let node = scratch.joint_nodes[i]; + let node = workspace.joint_nodes[i]; let target = if node == INVALID_ISLAND { INVALID_ISLAND } else { - scratch.root_island[scratch.uf.root(node) as usize] + workspace.root_island[workspace.uf.root(node) as usize] }; if target == island_id { continue; @@ -281,7 +281,7 @@ impl PersistentIslands { // untouched, so the union-find roots stay valid for the rest of the loop. let mut island_bodies = core::mem::take(&mut self.islands[island_id as usize].bodies); for i in (0..island_bodies.len()).rev() { - let target = scratch.root_island[scratch.uf.root(i as u32) as usize]; + let target = workspace.root_island[workspace.uf.root(i as u32) as usize]; if target == island_id { continue; } @@ -303,6 +303,6 @@ impl PersistentIslands { let island = &mut self.islands[island_id as usize]; island.constraint_remove_count = 0; island.split_denied_until = self.sleep_scan_stamp + SPLIT_RETRY_COOLDOWN; - self.split_scratch = scratch; + self.split_workspace = workspace; } } diff --git a/src/dynamics/island_manager/local_split.rs b/src/dynamics/island_manager/local_split.rs index 2ba6be9cb..10c1209e1 100644 --- a/src/dynamics/island_manager/local_split.rs +++ b/src/dynamics/island_manager/local_split.rs @@ -1,14 +1,18 @@ //! Bounded local island splits (decremental connectivity). Answering every removal *globally* is an O(island) union-find — a 43k-body pyramid shedding one bouncing box would pay a 163k-link scan every other step; here a **lockstep** dual search from the unlinked edge's endpoints answers it at O(smaller piece): meeting the other side ⇒ still connected, island NOT dirtied and still sleep-eligible; one frontier exhausting ⇒ that side is the detached — necessarily smaller — component, moved out in O(its size); [`SEARCH_BUDGET`] exceeded ⇒ dirty the island and defer to the global split ([`PersistentIslands::split_island_now`]), which bounds the worst case at the old behavior. -//! Adjacency reuses existing structures (no new bookkeeping): [`NarrowPhase::touching_edges_with`] (its edge ids and touching predicate are exactly what keys the island's contact links), [`ImpulseJointSet::attached_joints`], and multibody chain links (a multibody is one atomic-for-sleep neighborhood; its chain links travel with it). +//! Adjacency reuses existing structures (no new bookkeeping): [`NarrowPhase::touching_edges_with`] (its edge ids and touching predicate are exactly what keys the island's contact links), [`ImpulseJointSet::attached_joints`], multibody chain links (a multibody is one atomic-for-sleep neighborhood; its chain links travel with it), and the soft bodies' particle attachments (root body to attached rigid body, both ways through the soft-body set's attachment index). //! Ordering: every unlink of a step happens in the narrow phase, *before* islands update, so all searches run against the final post-removal graph — verdicts are batch-order-independent. #[cfg(not(feature = "std"))] use simba::scalar::ComplexField as _; use super::INVALID_ISLAND; -use super::persistent::{JointLinkKey, PersistentIslands, Removal, multibody_index_key}; +use super::persistent::{ + JointLinkKey, PersistentIslands, Removal, multibody_index_key, soft_body_key, +}; use crate::alloc_prelude::*; -use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, RigidBodyHandle, RigidBodySet}; +use crate::dynamics::{ + ImpulseJointSet, MultibodyJointSet, RigidBodyHandle, RigidBodySet, SoftBodySet, +}; use crate::geometry::{ColliderSet, NarrowPhase}; use crate::math::Real; use crate::utils::DotProduct; @@ -29,9 +33,9 @@ enum IncidentLink { /// Reusable buffers: the search runs every step and must not allocate once warm. #[derive(Clone, Default)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub(super) struct LocalSplitScratch { +pub(super) struct LocalSplitWorkspace { /// `(stamp, side)` per body-arena index. Stamped, so a search never clears the - /// map — only the bodies it actually touched carry the live stamp. + /// map: only the bodies it actually touched have the live stamp. visited: Vec<(u32, u8)>, stamp: u32, /// The two frontiers, and every body each side has reached. @@ -49,6 +53,7 @@ struct IslandGraph<'a> { narrow_phase: &'a NarrowPhase, impulse_joints: &'a ImpulseJointSet, multibody_joints: &'a MultibodyJointSet, + soft_bodies: &'a SoftBodySet, } impl IslandGraph<'_> { @@ -105,10 +110,24 @@ impl IslandGraph<'_> { } } } + + // Soft-body attachments: a soft body's root neighbors the bodies its particles are + // attached to, and conversely; a cluster proxy neighbors its sibling proxies (the + // proxy chain). + if let Some(sb) = rb.soft_body().and_then(|h| self.soft_bodies.get(h)) { + for attachment in sb.particle_attachments() { + visit(attachment.body); + } + } + for (sb_handle, _) in self.soft_bodies.attached_to(body) { + if let Some(sb) = self.soft_bodies.get(*sb_handle) { + visit(sb.root_body()); + } + } } /// Calls `f` for every island link incident to `body` — including links whose far side is - /// *not* an island member (contact against fixed geometry): those carry no connectivity, but + /// *not* an island member (contact against fixed geometry): those provide no connectivity, but /// they belong to the island and must follow the body when it moves out. fn for_each_incident_link(&self, body: RigidBodyHandle, mut f: impl FnMut(IncidentLink)) { let Some(rb) = self.bodies.get(body) else { @@ -128,7 +147,7 @@ impl IslandGraph<'_> { } // The chain links are keyed by ordinal, not by body: one per *consecutive - // pair of members*, exactly as `refresh_multibody_chain` numbers them. A + // pair of members*, exactly as `update_multibody_chain` numbers them. A // multibody moves as a whole, so emit all of them. if let Some(link) = self.multibody_joints.rigid_body_link(body) { let mb_id = link.multibody; @@ -143,6 +162,27 @@ impl IslandGraph<'_> { } } } + + // Soft-body attachments (numbered as `update_soft_body_attachments` does): the root's + // and, for a rigid body, those targeting it. A proxy also brings its soft body's whole + // proxy chain (numbered as `update_soft_body_proxy_chain` does); proxies move together. + if let Some(sb_handle) = rb.soft_body() { + if let Some(sb) = self.soft_bodies.get(sb_handle) { + let soft_body = soft_body_key(sb_handle); + for ordinal in 0..sb.particle_attachments().len() as u32 { + f(IncidentLink::Joint(JointLinkKey::SoftAttachment { + soft_body, + ordinal, + })); + } + } + } + for (sb_handle, ordinal) in self.soft_bodies.attached_to(body) { + f(IncidentLink::Joint(JointLinkKey::SoftAttachment { + soft_body: soft_body_key(*sb_handle), + ordinal: *ordinal, + })); + } } } @@ -151,7 +191,7 @@ enum Verdict { /// The endpoints are still connected: nothing changed. Connected, /// They are in different components now; `side` is the smaller one, and its - /// bodies are in `scratch.reached[side]`. + /// bodies are in `workspace.reached[side]`. Detached(usize), /// Not settled within the budget: hand it to the global split. OverBudget, @@ -168,6 +208,7 @@ impl PersistentIslands { narrow_phase: &NarrowPhase, impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, + soft_bodies: &SoftBodySet, length_unit: Real, ) { if self.removal_journal.is_empty() { @@ -175,7 +216,7 @@ impl PersistentIslands { } let journal = core::mem::take(&mut self.removal_journal); - let mut scratch = core::mem::take(&mut self.local_split); + let mut workspace = core::mem::take(&mut self.local_split); for removal in &journal { let graph = IslandGraph { @@ -184,6 +225,7 @@ impl PersistentIslands { narrow_phase, impulse_joints, multibody_joints, + soft_bodies, }; // Re-derive the endpoints' islands: an earlier removal of this batch @@ -192,7 +234,7 @@ impl PersistentIslands { graph.island_of(removal.body1), graph.island_of(removal.body2), ) else { - // An endpoint is fixed, disabled or gone: the edge carried no connectivity, so + // An endpoint is fixed, disabled or gone: the edge provided no connectivity, so // losing it can't disconnect anything. (A body *removal* is different — a body can // be a cut vertex — and `remove_body_raw` still dirties its island eagerly.) continue; @@ -235,7 +277,7 @@ impl PersistentIslands { continue; } - match search(&graph, &mut scratch, island1, removal) { + match search(&graph, &mut workspace, island1, removal) { Verdict::Connected => {} Verdict::OverBudget => { self.islands[island1 as usize].constraint_remove_count += 1; @@ -243,18 +285,18 @@ impl PersistentIslands { Verdict::Detached(side) => { // Collect the component's links while `bodies` is still only // immutably borrowed, then relocate bodies and links together. - let component = core::mem::take(&mut scratch.reached[side]); - scratch.links.clear(); + let component = core::mem::take(&mut workspace.reached[side]); + workspace.links.clear(); for body in &component { - graph.for_each_incident_link(*body, |link| scratch.links.push(link)); + graph.for_each_incident_link(*body, |link| workspace.links.push(link)); } - self.move_component_out(bodies, island1, &component, &scratch.links); - scratch.reached[side] = component; + self.move_component_out(bodies, island1, &component, &workspace.links); + workspace.reached[side] = component; } } } - self.local_split = scratch; + self.local_split = workspace; self.removal_journal = journal; self.removal_journal.clear(); } @@ -343,32 +385,32 @@ impl PersistentIslands { /// The lockstep dual search. See the module docs. fn search( graph: &IslandGraph, - scratch: &mut LocalSplitScratch, + workspace: &mut LocalSplitWorkspace, island_id: u32, removal: &Removal, ) -> Verdict { - scratch.stamp = scratch.stamp.wrapping_add(1); - if scratch.stamp == 0 { + workspace.stamp = workspace.stamp.wrapping_add(1); + if workspace.stamp == 0 { // Wrapped: a zeroed (never-visited) entry would alias stamp 0. - scratch.visited.clear(); - scratch.stamp = 1; + workspace.visited.clear(); + workspace.stamp = 1; } - let stamp = scratch.stamp; + let stamp = workspace.stamp; for side in 0..2 { - scratch.frontier[side].clear(); - scratch.reached[side].clear(); + workspace.frontier[side].clear(); + workspace.reached[side].clear(); } for (side, seed) in [removal.body1, removal.body2].into_iter().enumerate() { - mark(scratch, seed, stamp, side as u8); - scratch.frontier[side].push(seed); - scratch.reached[side].push(seed); + mark(workspace, seed, stamp, side as u8); + workspace.frontier[side].push(seed); + workspace.reached[side].push(seed); } let mut expansions = 0; loop { for side in 0..2 { - let Some(body) = scratch.frontier[side].pop() else { + let Some(body) = workspace.frontier[side].pop() else { // This side ran out of frontier without ever reaching the other // seed: it is a detached component — and, having advanced in // lockstep, the smaller of the two pieces. @@ -378,16 +420,16 @@ fn search( let mut met = false; graph.for_each_neighbor(body, island_id, |neighbor| { let (index, _) = neighbor.into_raw_parts(); - match scratch.visited.get(index as usize) { + match workspace.visited.get(index as usize) { Some(&(s, seen_side)) if s == stamp => { // Reaching a body the *other* search already owns means the // two endpoints are still connected through it. met |= seen_side != side as u8; } _ => { - mark(scratch, neighbor, stamp, side as u8); - scratch.frontier[side].push(neighbor); - scratch.reached[side].push(neighbor); + mark(workspace, neighbor, stamp, side as u8); + workspace.frontier[side].push(neighbor); + workspace.reached[side].push(neighbor); } } }); @@ -404,10 +446,10 @@ fn search( } #[inline] -fn mark(scratch: &mut LocalSplitScratch, handle: RigidBodyHandle, stamp: u32, side: u8) { +fn mark(workspace: &mut LocalSplitWorkspace, handle: RigidBodyHandle, stamp: u32, side: u8) { let (index, _) = handle.into_raw_parts(); - if scratch.visited.len() <= index as usize { - scratch.visited.resize(index as usize + 1, (0, 0)); + if workspace.visited.len() <= index as usize { + workspace.visited.resize(index as usize + 1, (0, 0)); } - scratch.visited[index as usize] = (stamp, side); + workspace.visited[index as usize] = (stamp, side); } diff --git a/src/dynamics/island_manager/manager.rs b/src/dynamics/island_manager/manager.rs index ad440b668..7abcbbbc1 100644 --- a/src/dynamics/island_manager/manager.rs +++ b/src/dynamics/island_manager/manager.rs @@ -2,7 +2,7 @@ use super::Island; use crate::alloc_prelude::*; use crate::dynamics::{ ImpulseJointSet, MultibodyJointSet, RigidBody, RigidBodyChanges, RigidBodyHandle, RigidBodyIds, - RigidBodySet, + RigidBodySet, RigidBodyType, SoftBodyHandle, SoftBodySet, }; use crate::geometry::{ColliderSet, NarrowPhase}; use crate::math::Real; @@ -42,7 +42,7 @@ pub struct IslandManager { /// awake set (no body has `additional_solver_iterations > 0`). #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) solve_groups: Vec, - /// Scratch buffers for [`Self::update_substep_groups`]. + /// Workspace buffers for [`Self::update_substep_groups`]. #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(super) substep_groups_workspace: super::substep_groups::SubstepGroupsWorkspace, /// The persistent islands: connected components of the touching-contact/joint @@ -162,10 +162,10 @@ impl IslandManager { self.persistent.apply_impulse_joint_event(bodies, event); } - /// Refreshes a multibody's island-connectivity chain, first waking its + /// Updates a multibody's island-connectivity chain, first waking its /// sleeping members if any member is awake (a multibody is atomic: its /// bodies must share one sleep state). - pub(crate) fn refresh_multibody_chain( + pub(crate) fn update_multibody_chain( &mut self, bodies: &mut RigidBodySet, multibody_joints: &MultibodyJointSet, @@ -192,7 +192,26 @@ impl IslandManager { } } self.persistent - .refresh_multibody_chain(bodies, multibody_joints, mb_id); + .update_multibody_chain(bodies, multibody_joints, mb_id); + } + + /// Updates a soft body's attachment links and its proxy chain, first waking the sleeping + /// side of every attachment whose other side is awake, and the sleeping proxies if any + /// sibling is awake (a soft body is atomic for sleep, like a multibody). + pub(crate) fn update_soft_body_attachments( + &mut self, + bodies: &mut RigidBodySet, + soft_bodies: &SoftBodySet, + handle: SoftBodyHandle, + ) { + if let Some(sb) = soft_bodies.get(handle) { + let root = sb.root_body(); + for attachment in sb.particle_attachments() { + self.wake_for_link(bodies, root, attachment.body); + } + } + self.persistent + .update_soft_body_attachments(bodies, soft_bodies, handle); } /// Wakes the sleeping side of a new link when the other side is awake. @@ -317,8 +336,23 @@ impl IslandManager { /// Updates a body's sleep-eligibility timer from its current velocities /// and last-step displacement. - pub(crate) fn update_body_energy(rb: &mut RigidBody, dt: Real, length_unit: Real) { - let sq_linvel = rb.vels.linvel.length_squared(); + pub(crate) fn update_body_energy( + rb: &mut RigidBody, + soft_bodies: &SoftBodySet, + dt: Real, + length_unit: Real, + ) { + // A soft-frame proxy's velocities are its cluster's average, so the sleep rule reads the + // soft body's fastest particle instead. + // TODO: have the soft-body code write a frame-velocity estimate into the rigid body? + let sq_linvel = if rb.body_type == RigidBodyType::SoftFrame { + let speed = soft_bodies + .get(rb.soft_body) + .map_or(0.0, |sb| sb.sleep_speed); + speed * speed + } else { + rb.vels.linvel.length_squared() + }; let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); let pose = rb.pos.position; rb.activation.update_energy( @@ -339,6 +373,7 @@ impl IslandManager { narrow_phase: &mut NarrowPhase, impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, + soft_bodies: &SoftBodySet, sleep_observations: &[(u32, bool)], ) { // First update after construction or deserialization: rebuild the persistent islands @@ -350,12 +385,13 @@ impl IslandManager { narrow_phase.touching_pairs_with_ids(colliders), impulse_joints, multibody_joints, + soft_bodies, ); for handle in to_wake { self.wake_up(bodies, handle, false); } - // `max_extent` (sleep metric) is only refreshed when colliders + // `max_extent` (sleep metric) is only updated when colliders // change: seed it for deserialized snapshots that predate it. let handles: Vec = bodies.iter().map(|(h, _)| h).collect(); for handle in handles { diff --git a/src/dynamics/island_manager/persistent.rs b/src/dynamics/island_manager/persistent.rs index 6a8df2fd4..f392faa9d 100644 --- a/src/dynamics/island_manager/persistent.rs +++ b/src/dynamics/island_manager/persistent.rs @@ -1,10 +1,12 @@ //! Persistent islands: eager union-by-size merges, deferred splits over flat per-island link arrays that cache body handles (split never dereferences contact/joint records); fixed bodies are never members, enabled non-fixed bodies are in exactly one island. //! Splitting is two-tiered: [`super::local_split`] settles ~all removals (99.98% on a 43k pyramid) at O(smaller piece) and *proves* harmless ones, so contact churn doesn't dirty the island or block sleep; leftovers reach the global O(island) union-find ([`PersistentIslands::split_island_now`]), cooldown-throttled ([`PersistentIsland::split_denied_until`]) and capped at one island/step — running that full scan inline every other step spiked alternating multi-ms. -//! Location back-references live here only: bodies carry `RigidBodyIds::island_id/island_index`; contact links via [`PersistentIslands::contact_link_locs`] (dense per contact-graph edge id, mirrors the edges vec's swap-removes like `pair_solver_hints`); joint links via a [`JointLinkKey`] map. +//! Location back-references live here only: bodies store `RigidBodyIds::island_id/island_index`; contact links via [`PersistentIslands::contact_link_locs`] (dense per contact-graph edge id, mirrors the edges vec's swap-removes like `pair_solver_hints`); joint links via a [`JointLinkKey`] map. -use super::global_split::SplitScratch; +use super::global_split::SplitWorkspace; use crate::alloc_prelude::*; -use crate::dynamics::{MultibodyIndex, MultibodyJointSet, RigidBodyHandle, RigidBodySet}; +use crate::dynamics::{ + MultibodyIndex, MultibodyJointSet, RigidBodyHandle, RigidBodySet, SoftBodyHandle, SoftBodySet, +}; use parry::utils::VecMap; use parry::utils::hashmap::HashMap; @@ -41,6 +43,10 @@ pub(crate) enum JointLinkKey { /// (packed arena index + generation). Multibodies are atomic for sleep: their non-fixed /// bodies are chained in link order, rebuilt whenever the multibody's structure changes. MultibodyChain { multibody: u64, ordinal: u32 }, + /// The `ordinal`-th particle attachment of the soft body at `soft_body` (packed arena index + + /// generation): a link between the soft body's root body and the rigid body the particle is + /// attached to, rebuilt whenever the soft body's attachments change. + SoftAttachment { soft_body: u64, ordinal: u32 }, } /// Packs a multibody arena index (index + generation) into a stable key. @@ -49,6 +55,12 @@ pub(super) fn multibody_index_key(id: MultibodyIndex) -> u64 { ((idx as u64) << 32) | generation as u64 } +/// Packs a soft-body handle (index + generation) into a stable key. +pub(super) fn soft_body_key(handle: SoftBodyHandle) -> u64 { + let (idx, generation) = handle.into_raw_parts(); + ((idx as u64) << 32) | generation as u64 +} + /// A touching contact edge cached inside an island. The body handles are denormalized here /// so the split's union-find pass iterates island-owned memory only. #[derive(Copy, Clone, Debug)] @@ -119,6 +131,9 @@ fn serialize_joint_link_locs( JointLinkKey::MultibodyChain { multibody, ordinal } => { (1u8, *multibody, *ordinal as u64) } + JointLinkKey::SoftAttachment { soft_body, ordinal } => { + (2u8, *soft_body, *ordinal as u64) + } }, s, ) @@ -418,7 +433,7 @@ impl PersistentIslands { /// blocking sleep and buying a global split — once the local search fails to settle it cheaply. fn journal_removal(&mut self, body1: RigidBodyHandle, body2: RigidBodyHandle) { if body1 == body2 { - // Self-loop (two colliders of the *same* body): never carried connectivity, so losing + // Self-loop (two colliders of the *same* body): never provided connectivity, so losing // it can't disconnect anything — and it would fool the local search (both endpoints // seed the same body). return; @@ -540,10 +555,10 @@ impl PersistentIslands { } } - /// Refreshes the multibody's internal connectivity chain: unlinks the old chain, then (if it + /// Updates the multibody's internal connectivity chain: unlinks the old chain, then (if it /// still exists under `mb_id`) re-links one over its non-fixed bodies in link order. /// Multibodies are atomic for sleep — branches under a fixed root must share one island, which per-joint edges wouldn't guarantee (an edge to a fixed root doesn't connect). - pub fn refresh_multibody_chain( + pub fn update_multibody_chain( &mut self, bodies: &mut RigidBodySet, multibody_joints: &MultibodyJointSet, @@ -594,6 +609,52 @@ impl PersistentIslands { } } + /// Updates a soft body's attachment links: unlinks the old ones, then (if the soft body + /// still exists) links its root body to every rigid body one of its particles is attached to, + /// in attachment order. + pub fn update_soft_body_attachments( + &mut self, + bodies: &mut RigidBodySet, + soft_bodies: &SoftBodySet, + handle: SoftBodyHandle, + ) { + self.unlink_soft_body_attachments(handle); + + let Some(sb) = soft_bodies.get(handle) else { + return; + }; + let raw = soft_body_key(handle); + let root = sb.root_body(); + for (ordinal, attachment) in sb.particle_attachments().iter().enumerate() { + self.link_joint( + bodies, + JointLinkKey::SoftAttachment { + soft_body: raw, + ordinal: ordinal as u32, + }, + root, + attachment.body, + ); + } + } + + /// Unlinks a soft body's attachment links (ordinals are dense from 0). + pub fn unlink_soft_body_attachments(&mut self, handle: SoftBodyHandle) { + let raw = soft_body_key(handle); + let mut ordinal = 0; + loop { + let key = JointLinkKey::SoftAttachment { + soft_body: raw, + ordinal, + }; + if !self.joint_link_locs.contains_key(&key) { + break; + } + self.unlink_joint(key); + ordinal += 1; + } + } + /// Rebuilds everything from the current world state (first step after construction or /// deserialization): singleton islands, then every touching contact and enabled joint linked. /// Returns sleeping bodies stranded in a *non*-sleeping island (partial-island-era serialized state); the caller must wake them to restore the whole-island invariant. @@ -603,6 +664,7 @@ impl PersistentIslands { touching_pairs: impl Iterator, Option)>, impulse_joints: &crate::dynamics::ImpulseJointSet, multibody_joints: &MultibodyJointSet, + soft_bodies: &SoftBodySet, ) -> Vec { self.islands = VecMap::default(); self.free_islands.clear(); @@ -642,7 +704,12 @@ impl PersistentIslands { .map(|(id, _)| MultibodyIndex(id)) .collect(); for mb_id in mb_ids { - self.refresh_multibody_chain(bodies, multibody_joints, mb_id); + self.update_multibody_chain(bodies, multibody_joints, mb_id); + } + + let sb_handles: Vec = soft_bodies.iter().map(|(h, _)| h).collect(); + for handle in sb_handles { + self.update_soft_body_attachments(bodies, soft_bodies, handle); } // Merging propagated `sleeping &=`, so an island is `sleeping` iff diff --git a/src/dynamics/island_manager/substep_groups.rs b/src/dynamics/island_manager/substep_groups.rs index 9ae23691d..15bd76e00 100644 --- a/src/dynamics/island_manager/substep_groups.rs +++ b/src/dynamics/island_manager/substep_groups.rs @@ -1,10 +1,13 @@ -//! Substep solve-groups: partition of the awake set by effective `RigidBody::additional_solver_iterations`. Coupled bodies must share a substep cadence, so the unit of elevation is a connected component of the awake constraint graph (count = max over members); components are then grouped **by count, not by component**, so the awake set splits into `#distinct counts` contiguous ranges (typically 2) instead of re-fragmenting into per-island solves. -//! Runs only when some awake body is elevated (`any_extra`); recomputed from live state every step but *applied* (reorder + `active_set_id` re-stamp + epoch bump) only when the list isn't already grouped — the epoch bump forces a full solver-contact-graph rebuild, so steady-state elevated scenes must re-partition zero times per step. +//! Substep solve groups: the awake set partitioned by effective `additional_solver_iterations` +//! and `additional_pgs_iterations` (max over each awake connected component), grouped by count +//! into contiguous ranges. Applied (reorder, re-stamp, epoch bump) only when not already grouped. use super::IslandManager; use crate::data::union_find::UnionFind; -use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, RigidBodyHandle, RigidBodySet}; -use crate::geometry::NarrowPhase; +use crate::dynamics::{ + ImpulseJointSet, MultibodyJointSet, RigidBodyHandle, RigidBodySet, SoftBodySet, +}; +use crate::geometry::{ColliderSet, ContactPair, NarrowPhase}; use alloc::vec::Vec; use core::ops::Range; @@ -21,20 +24,20 @@ pub(crate) struct SolveGroup { pub extra_iters: u32, } -/// Scratch state for [`IslandManager::update_substep_groups`], kept to reuse +/// Workspace state for [`IslandManager::update_substep_groups`], kept to reuse /// allocations across steps. #[derive(Clone, Default)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub(crate) struct SubstepGroupsWorkspace { uf: UnionFind, - /// Effective extra count per awake body slot (component max). + /// Effective key per awake body slot (component max of each count, see `key`). keys: Vec, - /// Distinct extra counts in use, sorted descending. + /// Distinct keys in use, sorted descending. distinct: Vec, /// Scatter cursors/counts per distinct key. offsets: Vec, - /// Reorder scratch for the awake `bodies` vec. - scratch: Vec, + /// Reorder workspace for the awake `bodies` vec. + workspace: Vec, } impl IslandManager { @@ -46,8 +49,10 @@ impl IslandManager { any_extra: bool, bodies: &mut RigidBodySet, narrow_phase: &NarrowPhase, + colliders: &ColliderSet, impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, + soft_bodies: &SoftBodySet, ) { self.solve_groups.clear(); if !any_extra { @@ -70,17 +75,14 @@ impl IslandManager { .then_some(rb.ids.active_set_id) }; - // Contact edges, from the narrow-phase's current pairs (post `detect_collisions`: no stale - // refs, no one-step lag). Solver-side filters (COMPUTE_IMPULSES) are deliberately ignored: - // over-merging only substeps slightly more; under-merging would split coupled bodies' cadences. + // Contact edges from the narrow-phase's current pairs; solver-side filters are ignored as + // under-merging would split coupled bodies' cadences. A soft body's colliders hang on its + // cluster proxies, so a pair touching a soft surface couples it like any other pair. for pair in narrow_phase.contact_pairs() { if !pair.has_any_active_contact() { continue; } - let Some(manifold) = pair.manifolds.first() else { - continue; - }; - if let (Some(h1), Some(h2)) = (manifold.data.rigid_body1, manifold.data.rigid_body2) + if let (Some(h1), Some(h2)) = pair_bodies(pair, colliders) && let (Some(s1), Some(s2)) = (slot(h1), slot(h2)) { ws.uf.union(s1, s2); @@ -111,9 +113,22 @@ impl IslandManager { let _ = link_id; } - // Effective extra count per body = max over its component. Non-dynamic - // awake bodies (kinematic) stay singletons here and get key 0, then are - // lifted to the highest-cadence group they touch below. + // Soft-body attachments: a soft body (its root) is coupled with the bodies its particles + // are attached to, and its cluster proxies with each other (one substep cadence per + // soft body). + for (_, sb) in soft_bodies.iter() { + let Some(root) = slot(sb.root_body()) else { + continue; + }; + for attachment in sb.particle_attachments() { + if let Some(s) = slot(attachment.body) { + ws.uf.union(root, s); + } + } + } + + // Effective counts per body = max of each over its component; non-dynamic awake bodies + // stay singletons with key 0, then are lifted to the highest-cadence group they touch. ws.keys.clear(); ws.keys.resize(num_bodies, 0); for i in 0..num_bodies { @@ -148,10 +163,7 @@ impl IslandManager { if !pair.has_any_active_contact() { continue; } - let Some(manifold) = pair.manifolds.first() else { - continue; - }; - if let (Some(h1), Some(h2)) = (manifold.data.rigid_body1, manifold.data.rigid_body2) { + if let (Some(h1), Some(h2)) = pair_bodies(pair, colliders) { lift(ws, h1, h2); lift(ws, h2, h1); } @@ -194,14 +206,14 @@ impl IslandManager { } let island_bodies = &mut self.islands[awake_id].bodies; - ws.scratch.clear(); - ws.scratch.resize(num_bodies, RigidBodyHandle::invalid()); + ws.workspace.clear(); + ws.workspace.resize(num_bodies, RigidBodyHandle::invalid()); for (i, &handle) in island_bodies.iter().enumerate() { let ord = ws.distinct.iter().position(|&d| d == ws.keys[i]).unwrap(); - ws.scratch[ws.offsets[ord]] = handle; + ws.workspace[ws.offsets[ord]] = handle; ws.offsets[ord] += 1; } - core::mem::swap(island_bodies, &mut ws.scratch); + core::mem::swap(island_bodies, &mut ws.workspace); // Re-stamp the invariant `active_set_id == index in bodies`, re-derive // the (now sorted) keys, and invalidate every ordering-derived cache. @@ -215,6 +227,21 @@ impl IslandManager { } /// Derives contiguous group ranges from a descending-sorted key sequence. +/// The two bodies a touching pair couples: its first manifold's, or the colliders' parents +/// for a pair of two soft surfaces. +fn pair_bodies( + pair: &ContactPair, + colliders: &ColliderSet, +) -> (Option, Option) { + match pair.manifolds().first() { + Some(manifold) => (manifold.data.rigid_body1, manifold.data.rigid_body2), + None => ( + colliders.get(pair.collider1).and_then(|c| c.parent()), + colliders.get(pair.collider2).and_then(|c| c.parent()), + ), + } +} + fn push_group_ranges(groups: &mut Vec, keys: &[u32]) { let mut start = 0; for i in 1..=keys.len() { diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 8126611b7..5ce4ddad0 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -70,7 +70,7 @@ fn concat_rb_mass_matrix( #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug)] pub struct MultibodyDofCoupling { - /// Internal id of the link carrying the first joint. + /// Internal id of the link owning the first joint. pub link1: usize, /// Local free-DoF index of the coupled DoF within `link1` (its position in /// that link's slice of the generalized vectors). @@ -78,7 +78,7 @@ pub struct MultibodyDofCoupling { /// Spatial-coordinate axis (`0..6`) of `link1`'s coupled DoF, used to read /// its generalized position from the joint coords. pub axis1: usize, - /// Internal id of the link carrying the second joint. + /// Internal id of the link owning the second joint. pub link2: usize, /// Local free-DoF index of the coupled DoF within `link2`. pub dof2: usize, @@ -1638,7 +1638,7 @@ impl Multibody { let jb1 = &self.body_jacobians[link1.internal_id]; let jb2 = &self.body_jacobians[link2.internal_id]; - // Use the (overwritten below) W·J slot as scratch for J1ᵀ·f1. + // Use the (overwritten below) W·J slot as workspace for J1ᵀ·f1. let (mut out_j, mut scratch) = jacobians.rows_range_pair_mut(*j_id..*j_id + self.ndofs, wj_id..wj_id + self.ndofs); jb2.tr_mul_to(force2.as_vector(), &mut out_j); diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 7f18b0d9b..540966aa3 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -63,9 +63,9 @@ pub struct MultibodyJointSet { /// A set of rigid-body pairs to join in the island manager during the next timestep. pub(crate) to_join: HashSet<(RigidBodyHandle, RigidBodyHandle)>, /// Multibodies whose structure changed (created, merged, split, removed): - /// the persistent islands refresh each one's internal connectivity chain + /// the persistent islands update each one's internal connectivity chain /// at the start of the next timestep, in order. Ids of *removed* - /// multibodies are pushed too (the refresh then only unlinks). + /// multibodies are pushed too (the update then only unlinks). #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) island_chain_events: Vec, /// Epoch bumped whenever a rigid-body's multibody membership can change diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 51888cbd7..47396fadf 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -29,6 +29,9 @@ pub use parry::mass_properties::MassProperties; pub use self::rigid_body::{RigidBody, RigidBodyBuilder}; #[cfg(feature = "alloc")] pub use self::rigid_body_set::{BodyPair, RigidBodySet}; +#[cfg(feature = "alloc")] +pub use self::soft_body::*; +pub(crate) use self::soft_body::soft_body_crossing_tests; #[cfg(feature = "alloc")] mod ccd; @@ -47,3 +50,5 @@ pub(crate) mod solver; mod rigid_body; #[cfg(feature = "alloc")] mod rigid_body_set; +#[cfg(feature = "alloc")] +mod soft_body; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 868167a0f..2482c8638 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -65,6 +65,12 @@ pub struct RigidBody { pub(crate) dominance: RigidBodyDominance, pub(crate) enabled: bool, pub(crate) additional_solver_iterations: usize, + /// The soft body this rigid body is the root of (invalid for regular rigid bodies). + #[cfg_attr( + feature = "serde-serialize", + serde(default = "crate::dynamics::SoftBodyHandle::invalid") + )] + pub(crate) soft_body: crate::dynamics::SoftBodyHandle, /// User-defined data associated to this rigid-body. pub user_data: u128, } @@ -94,12 +100,14 @@ impl RigidBody { enabled: true, user_data: 0, additional_solver_iterations: 0, + soft_body: crate::dynamics::SoftBodyHandle::invalid(), } } pub(crate) fn reset_internal_references(&mut self) { self.colliders.0 = Vec::new(); self.ids = Default::default(); + self.soft_body = crate::dynamics::SoftBodyHandle::invalid(); } /// Copy all the characteristics from `other` to `self`. @@ -137,6 +145,7 @@ impl RigidBody { dominance, enabled, additional_solver_iterations, + soft_body: _soft_body, // The soft-body marker belongs to the root body, not to its state. user_data, } = other; @@ -165,6 +174,13 @@ impl RigidBody { self.additional_solver_iterations } + /// The soft body this rigid body is a cluster proxy of, if any. A proxy stands for its cluster + /// in islands and joints, holds its colliders, and its removal removes the cluster; see + /// [`crate::dynamics::SoftBodySet::insert`] and [`crate::dynamics::SoftBodySet::add_cluster`]. + pub fn soft_body(&self) -> Option { + (!self.soft_body.is_invalid()).then_some(self.soft_body) + } + /// Set the additional number of solver substeps run for the simulation island containing this /// rigid-body (default: 0). Each extra substep re-derives the soft constraint bias at a smaller /// timestep, improving accuracy for stiff couplings (joint chains, high mass-ratio stacks). The diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index e23842521..882ea6709 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -325,7 +325,7 @@ pub struct RigidBodyMassProps { pub additional_local_mprops: Option>, /// Conservative bound on the distance of any shape point from the local center of mass; /// the sleep metric and the CCD fast-body criterion use it to turn angular velocity into - /// a farthest-point speed. Refreshed with the mass properties; `0` for collider-less bodies. + /// a farthest-point speed. Updated with the mass properties; `0` for collider-less bodies. #[cfg_attr(feature = "serde-serialize", serde(default))] pub(crate) max_extent: Real, } @@ -488,7 +488,7 @@ impl RigidBodyMassProps { self.update_world_mass_properties(body_type, position); } - /// Refreshes [`Self::max_extent`] from the attached colliders' bounding + /// Updates [`Self::max_extent`] from the attached colliders' bounding /// spheres, measured about the local center of mass. pub(crate) fn recompute_max_extent( &mut self, diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 19d4c2543..c36652d2a 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -185,6 +185,7 @@ impl RigidBodySet { /// # let mut impulse_joints = ImpulseJointSet::new(); /// # let mut multibody_joints = MultibodyJointSet::new(); /// # let handle = bodies.insert(RigidBodyBuilder::dynamic()); + /// # let mut soft_bodies = SoftBodySet::new(); /// // Remove a body and everything attached to it /// if let Some(body) = bodies.remove( /// handle, @@ -192,6 +193,7 @@ impl RigidBodySet { /// &mut colliders, /// &mut impulse_joints, /// &mut multibody_joints, + /// &mut soft_bodies, /// true // Remove colliders too /// ) { /// println!("Removed body at {:?}", body.translation()); diff --git a/src/dynamics/soft_body/mod.rs b/src/dynamics/soft_body/mod.rs new file mode 100644 index 000000000..01b069026 --- /dev/null +++ b/src/dynamics/soft_body/mod.rs @@ -0,0 +1,43 @@ +//! Soft bodies: deformable bodies made of particles linked by elastic constraints, solved +//! together with rigid bodies, contacts and joints. + +pub use self::collision_mesh::{ + SoftBindingError, SoftCollisionMesh, SoftMeshBinding, SoftMeshBindingMode, SoftMeshCellBinding, + SoftMeshId, SoftMeshMapping, SoftMeshRef, +}; +#[cfg(feature = "dim3")] +pub use self::soft_body_elements::SoftBodyDihedral; +pub use self::soft_body::{SOFT_BODY_MAX_CONSTRAINT_PARTICLES, SoftBody}; +pub use self::soft_body_elements::{ + SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyEdgeKind, SoftBodyParticle, +}; +pub use self::soft_body_material::SoftBodyMaterial; +pub use self::soft_body_motion::SoftParticleAttachment; +pub use self::soft_body_builder::{SoftBodyBuilder, SoftBodyParticleSettings}; +pub use self::soft_body_handle::SoftBodyHandle; +pub use self::soft_body_set::SoftBodySet; +pub use self::soft_body_plasticity::SoftEdgePlasticFlow; + +pub(crate) use self::soft_body::SOFT_BODY_OVERFLOW_COLOR; +pub(crate) use self::soft_body_contacts::{SoftEdgeContact, SoftVertexContact}; + +#[allow(unused_imports)] +use self::soft_body_elements::{CELL_IMPULSES}; +#[allow(unused_imports)] +use self::soft_body_material::{default_true}; +pub(crate) mod collision_mesh; +mod soft_body; +mod soft_body_accessors; +mod soft_body_builder; +mod soft_body_coloring; +mod soft_body_contacts; +mod soft_body_elements; +mod soft_body_geometry; +mod soft_body_handle; +mod soft_body_material; +mod soft_body_meshes; +mod soft_body_motion; +mod soft_body_plasticity; +mod soft_body_set; +pub(crate) mod soft_body_shape_matching; +mod tearing; diff --git a/src/dynamics/soft_body/soft_body.rs b/src/dynamics/soft_body/soft_body.rs new file mode 100644 index 000000000..728855c7c --- /dev/null +++ b/src/dynamics/soft_body/soft_body.rs @@ -0,0 +1,122 @@ +//! The `SoftBody` struct and the shared constants of the soft-body constraints. +use crate::alloc_prelude::*; +use crate::dynamics::RigidBodyHandle; +use crate::math::{DIM, Real, Vector}; +use super::{ + SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial, SoftBodyParticle, + SoftParticleAttachment, +}; +#[cfg(feature = "dim3")] +use super::SoftBodyDihedral; +#[cfg(feature = "fem")] +use super::SoftBodySolver; +#[cfg(feature = "serde-serialize")] +use super::soft_body_material::default_true; + +/// Number of particles a soft-body constraint can touch (a simplex cell). +pub const SOFT_BODY_MAX_CONSTRAINT_PARTICLES: usize = DIM + 1; + +/// Color id marking a soft-body element that could not be given a parallel color +/// (solved serially by the solver). +pub(crate) const SOFT_BODY_OVERFLOW_COLOR: u8 = 128; + +/// A deformable body: [`SoftBodyParticle`]s linked by elastic elements ([`SoftBodyEdge`]s, bending, +/// [`SoftBodyCell`]s, volume preservation, shape matching). Its hidden root rigid body (tagged by +/// [`crate::dynamics::RigidBody::soft_body`]) must never be moved, removed or attached to joints. +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftBody { + pub(crate) particles: Vec, + /// The hidden rigid body standing for this soft body in the islands and holding its + /// colliders (invalid until the soft body is inserted in a set): the whole-body cluster's + /// proxy, re-pointed to another live cluster's proxy if that cluster is removed. + pub(crate) root_body: RigidBodyHandle, + /// The particles attached to rigid bodies. + pub(crate) attachments: Vec, + /// Whether the soft body is currently asleep (mirrors the root body's state at the end of + /// each step). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) sleeping: bool, + /// Whether the soft body takes part in the simulation (applied to the root body at the start + /// of the next step; cleared by the NaN quarantine). + #[cfg_attr(feature = "serde-serialize", serde(default = "default_true"))] + pub(crate) enabled: bool, + /// Set by the setters that move particles between steps (positions, pinning): the colliders + /// are updated at the start of the next step so the narrow phase sees the new geometry. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) positions_modified: bool, + /// Set when the attachments changed since the last step: the island links are updated at + /// the start of the next step. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) attachments_modified: bool, + pub(crate) edges: Vec, + #[cfg(feature = "dim3")] + pub(crate) dihedrals: Vec, + pub(crate) cells: Vec, + /// The computational mesh's boundary: segments (2D) or triangles (3D), oriented outward. + /// Used by the volume constraints, and as the geometry of the default collision mesh. + pub(crate) boundary: Vec<[u32; DIM]>, + /// Whether the boundary is closed (every segment vertex / triangle edge shared by exactly + /// two elements). + pub(crate) boundary_closed: bool, + /// The cell owning each boundary element (`u32::MAX`: none): the tearing pass follows it, + /// and the winding of an element whose cell is inverted no longer tells where the outside + /// is. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) boundary_element_cells: Vec, + pub(crate) material: SoftBodyMaterial, + pub(crate) cell_model: SoftBodyCellModel, + /// Whether the volume pieces are constrained (only set while there is at least one piece). + pub(crate) volume_preservation: bool, + /// The pieces of material enclosed by a closed boundary, each with its own volume constraint + /// (rebuilt with the boundary tables). + /// Multiplier of the target volume (`> 1` inflates the body). + pub(crate) volume_factor: Real, + /// The rest shape's center of mass at insertion (world space). + pub(crate) rest_com: Vector, + /// The particle spacing: the impact-adaptive substeps bound the travel per substep by it, + /// and the collision meshes' colliders take their default contact skin from it. + pub(crate) particle_radius: Real, + /// Rigid-body settings applied to the particles at insertion. + pub(crate) particle_settings: super::SoftBodyParticleSettings, + /// Number of parallel colors used by the elements (`0..num_colors`, plus possibly the + /// overflow color). + pub(crate) num_colors: u8, + /// Whether some element has the overflow color. + pub(crate) has_overflow_color: bool, + /// Set by the setters that change the dynamics (material, volume factor, shape matching...): + /// the pipeline wakes the soft body up at the start of the next step so the change takes + /// effect even if it was sleeping. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) modified: bool, + /// Set by the solver when a cell flowed plastically by a significant amount during the last + /// step: the body is kept awake so its creep goes on. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) plastic_flowing: bool, + /// The speed the sleep rule reads for this body's cluster proxies: the fastest particle at the + /// end of the last step, or `Real::MAX` while the body flows plastically (kept awake). + /// `RigidBodyActivation::update_energy` compares it against the proxies' activation threshold. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) sleep_speed: Real, + /// Set when some edge or cell has a `torn` mark: the tearing pass at the end of the step + /// removes them (see `SoftBodySet::tear`). + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) tearing_pending: bool, + /// Bumped whenever the topology changes (a tear removed elements or duplicated particles): + /// renderers keying their meshes on the particles and elements rebuild them on change. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) topology_version: u32, + /// 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`). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) contact_approach_speeds: [Option; 2], + /// 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))] + pub(crate) contact_load: Real, + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) load_extra_substeps: u8, + /// User-defined data associated to this soft body. + pub user_data: u128, +} diff --git a/src/dynamics/soft_body/soft_body_accessors.rs b/src/dynamics/soft_body/soft_body_accessors.rs new file mode 100644 index 000000000..f39ae73cb --- /dev/null +++ b/src/dynamics/soft_body/soft_body_accessors.rs @@ -0,0 +1,252 @@ +//! Accessors and simple setters of a soft body: particles, elements, material, volume preservation, sleeping and enabling. +use super::{SoftBody, SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial, SoftBodyParticle}; +#[cfg(feature = "dim3")] +use super::SoftBodyDihedral; +#[cfg(feature = "fem")] +use super::SoftBodySolver; +use crate::dynamics::RigidBodyHandle; +use crate::math::{DIM, Real, Vector}; + +impl SoftBody { + /// A counter bumped whenever the topology changes (a tear removed elements or duplicated + /// particles): a renderer keying its mesh on the particles and elements rebuilds it when + /// this value changes. + pub fn topology_version(&self) -> u32 { + self.topology_version + } + + /// The particles of this soft body. + pub fn particles(&self) -> &[SoftBodyParticle] { + &self.particles + } + + /// The number of particles of this soft body. + pub fn num_particles(&self) -> usize { + self.particles.len() + } + + /// The world-space position of the `i`-th particle. + pub fn particle_position(&self, i: usize) -> Vector { + self.particles[i].position + } + + /// The world-space positions of every particle. + pub fn particle_positions(&self) -> impl ExactSizeIterator + '_ { + self.particles.iter().map(|p| p.position) + } + + /// The world-space velocity of the `i`-th particle. + pub fn particle_velocity(&self, i: usize) -> Vector { + self.particles[i].velocity + } + + /// The world-space velocities of every particle. + pub fn particle_velocities(&self) -> impl ExactSizeIterator + '_ { + self.particles.iter().map(|p| p.velocity) + } + + /// Teleports the `i`-th particle to `position` (no velocity change). For a pinned particle + /// moving along a path, prefer [`Self::set_particle_kinematic_target`], which gives it the + /// matching velocity so friction and contacts see the motion. + pub fn set_particle_position(&mut self, i: usize, position: Vector) { + self.particles[i].position = position; + self.particles[i].next_position = None; + self.positions_modified = true; + self.modified = true; + } + + /// Sets the velocity of the `i`-th particle. A pinned particle keeps moving at that velocity + /// (a velocity-driven kinematic particle) until it is set again or given a kinematic target. + pub fn set_particle_velocity(&mut self, i: usize, velocity: Vector) { + self.particles[i].velocity = velocity; + self.particles[i].next_position = None; + self.modified = true; + } + + /// Moves the pinned `i`-th particle to `position` over the next step (with the matching + /// velocity, so dragged cloth and friction see the motion), then holds it there until the next + /// target or velocity is set. Ignored for a free particle. + pub fn set_particle_kinematic_target(&mut self, i: usize, position: Vector) { + let particle = &mut self.particles[i]; + if particle.inv_mass == 0.0 { + particle.next_position = Some(position); + // A target away from the particle is a motion: the body must be awake to follow it. + if position != particle.position { + self.modified = true; + } + } + } + + /// The distance constraints of this soft body. + pub fn edges(&self) -> &[SoftBodyEdge] { + &self.edges + } + + /// The dihedral bending constraints of this soft body. + #[cfg(feature = "dim3")] + pub fn dihedrals(&self) -> &[SoftBodyDihedral] { + &self.dihedrals + } + + /// The simplex cells (triangles in 2D, tetrahedra in 3D) of this soft body. + pub fn cells(&self) -> &[SoftBodyCell] { + &self.cells + } + + /// The boundary of this soft body's computational mesh: segments in 2D, triangles in 3D + /// (particle indices), oriented outward. This is what the volume constraint integrates over; the + /// geometry the body collides through is its [`crate::dynamics::SoftCollisionMesh`]. + pub fn boundary(&self) -> &[[u32; DIM]] { + &self.boundary + } + + /// The material (per-family stiffness and damping) of this soft body. + pub fn material(&self) -> &SoftBodyMaterial { + &self.material + } + + /// Sets the material of this soft body. + pub fn set_material(&mut self, material: SoftBodyMaterial) { + self.material = material; + self.modified = true; + } + + /// Mutable access to the material of this soft body. + pub fn material_mut(&mut self) -> &mut SoftBodyMaterial { + self.modified = true; + &mut self.material + } + + /// The constitutive model of this soft body's cells. + pub fn cell_model(&self) -> SoftBodyCellModel { + self.cell_model + } + + /// Whether the global area/volume preservation row is enabled. + pub fn volume_preservation_enabled(&self) -> bool { + self.volume_preservation + } + + /// Enables or disables the global area/volume preservation row (requires a closed + /// surface). + pub fn enable_volume_preservation(&mut self, enabled: bool) { + self.volume_preservation = enabled && !self.boundary.is_empty(); + self.modified = true; + } + + /// The signed area (2D) or volume (3D) enclosed by the surface at rest. + pub fn rest_volume(&self) -> Real { + self.rest_volume + } + + /// The signed area (2D) or volume (3D) currently enclosed by the surface. + pub fn volume(&self) -> Real { + Self::boundary_volume(&self.boundary, |i| self.particles[i as usize].position) + } + + /// The multiplier applied to the rest volume to obtain the volume-preservation target + /// (`> 1` inflates the body). + pub fn volume_factor(&self) -> Real { + self.volume_factor + } + + /// Sets the multiplier applied to the rest volume to obtain the volume-preservation + /// target (`> 1` inflates the body). + pub fn set_volume_factor(&mut self, factor: Real) { + self.volume_factor = factor; + self.modified = true; + } + + /// The particle spacing of this soft body: what the impact-adaptive substeps bound the + /// travel per substep by, and the default contact skin of its collision meshes' colliders. + pub fn particle_radius(&self) -> Real { + self.particle_radius + } + + /// The hidden rigid body standing for this soft body in the islands and carrying its + /// colliders (see the type-level documentation; invalid until the soft body is inserted in a + /// set). Never move, remove or attach joints to it: it is only useful to recognize the soft + /// body's colliders (their parent) in query results and events, or to exclude them from a + /// query. + pub fn root_body(&self) -> RigidBodyHandle { + self.root_body + } + + /// The dynamics settings of the particles (damping, gravity scale, sleeping...). + pub fn particle_settings(&self) -> &super::SoftBodyParticleSettings { + &self.particle_settings + } + + /// The number of parallel solver colors used by this soft body's elements (informational: + /// elements of a same color share no particle and are solved concurrently). + pub fn num_solver_colors(&self) -> usize { + self.num_colors as usize + self.has_overflow_color as usize + } + + /// The mass-weighted center of the particles (from the cached positions). + pub fn center_of_mass(&self) -> Vector { + let mut com = Vector::ZERO; + let mut mass = 0.0; + for p in &self.particles { + com += p.position * p.mass; + mass += p.mass; + } + if mass > 0.0 { com / mass } else { com } + } + + /// The total nominal mass of the particles. + pub fn mass(&self) -> Real { + self.particles.iter().map(|p| p.mass).sum() + } + + /// Whether this soft body is currently sleeping (a soft body sleeps and wakes as a unit, + /// with the island of the rigid bodies it touches). Updated at the end of every step, and + /// by [`Self::wake_up`]. + pub fn is_sleeping(&self) -> bool { + self.sleeping + } + + /// Wakes this soft body up (effective at the start of the next step, together with the + /// island of the bodies it touches). + pub fn wake_up(&mut self) { + self.sleeping = false; + self.modified = true; + } + + /// Whether this soft body takes part in the simulation. A disabled soft body (by + /// [`Self::set_enabled`], or by the NaN quarantine: `PhysicsPipeline::quarantine`) is + /// left where it is, without colliders or constraints, until it is enabled again. + pub fn is_enabled(&self) -> bool { + self.enabled + } + + /// Enables or disables this soft body (effective at the start of the next step). Re-enabling + /// a quarantined body: set its non-finite particles right first + /// ([`Self::set_particle_position`]), they are not repaired. + pub fn set_enabled(&mut self, enabled: bool) { + if self.enabled != enabled { + self.enabled = enabled; + self.modified = true; + } + } + + /// Whether every particle's state is finite. + pub(crate) fn is_finite(&self) -> bool { + self.particles.iter().all(|p| { + p.position.is_finite() + && p.velocity.is_finite() + && p.force.is_finite() + && p.next_position.is_none_or(|t| t.is_finite()) + }) + } + + /// Neutralizes the dynamics of a non-finite body (velocities, forces, targets); the + /// positions are left as they are. + pub(crate) fn sanitize_dynamics(&mut self) { + for p in &mut self.particles { + p.velocity = Vector::ZERO; + p.force = Vector::ZERO; + p.next_position = None; + } + } +} diff --git a/src/dynamics/soft_body/soft_body_builder/mod.rs b/src/dynamics/soft_body/soft_body_builder/mod.rs new file mode 100644 index 000000000..3d5dab3ef --- /dev/null +++ b/src/dynamics/soft_body/soft_body_builder/mod.rs @@ -0,0 +1,17 @@ +//! The soft-body builder: its settings struct, the builder struct itself and the module wiring. + +mod soft_body_build; +mod soft_body_builder; +mod soft_body_builder_mesh_helpers; +mod soft_body_builder_settings; +mod soft_body_builder_shapes; + +pub use self::soft_body_builder::{SoftBodyBuilder, SoftBodyParticleSettings}; +pub(crate) use self::soft_body_builder_mesh_helpers::{ + cell_faces, element_vertices, surface_element_cells, surface_is_closed, surface_rings, + surface_vertex_elements, +}; +#[cfg(feature = "dim3")] +pub(crate) use self::soft_body_builder_mesh_helpers::surface_edge_table; +#[cfg(feature = "fem")] +pub(super) use super::SoftBodySolver; 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 new file mode 100644 index 000000000..fe0804184 --- /dev/null +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_build.rs @@ -0,0 +1,188 @@ +//! The build path of the soft-body builder: the soft body itself and its collision meshes. + +use crate::alloc_prelude::*; +use crate::dynamics::RigidBodyHandle; +use crate::math::{DIM, Matrix, Real, Rotation, Vector}; +use parry::utils::hashmap::HashMap; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +#[cfg(feature = "dim3")] +use super::super::SoftBodyDihedral; +use super::super::{ + SoftBody, SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyEdgeKind, SoftBodyParticle, + SoftCollisionMesh, soft_body_coloring, +}; +#[cfg(feature = "dim3")] +use super::soft_body_builder_mesh_helpers::dihedral_angle; +use super::soft_body_builder_mesh_helpers::{cell_boundary, surface_element_cells, surface_is_closed}; +use super::SoftBodyBuilder; + +impl SoftBodyBuilder { + /* + * Build. + */ + + /// Builds the soft body (not yet inserted in a set: its root rigid body and colliders are + /// created by [`super::SoftBodySet::insert`]). + pub(crate) fn build(&self) -> SoftBody { + let num_particles = self.positions.len(); + let masses: Vec = if self.masses.len() == num_particles { + self.masses.clone() + } else { + vec![self.particle_mass; num_particles] + }; + let mut pinned = vec![false; num_particles]; + for &i in &self.pinned { + if let Some(p) = pinned.get_mut(i as usize) { + *p = true; + } + } + + // Rest center of mass and rest positions. + let mut rest_com = Vector::ZERO; + let mut total_mass = 0.0; + for (p, m) in self.positions.iter().zip(masses.iter()) { + rest_com += *p * *m; + total_mass += *m; + } + if total_mass > 0.0 { + rest_com /= total_mass; + } + + let particles: Vec = self + .positions + .iter() + .zip(masses.iter()) + .zip(pinned.iter()) + .map(|((p, m), pinned)| SoftBodyParticle { + position: *p, + velocity: Vector::ZERO, + rest_position: *p - rest_com, + mass: *m, + inv_mass: if *pinned { 0.0 } else { crate::utils::inv(*m) }, + force: Vector::ZERO, + next_position: None, + }) + .collect(); + let pos = |i: u32| self.positions[i as usize]; + + // Cells: positive orientation, rest matrices. + let mut cells = Vec::with_capacity(self.cells.len()); + for c in &self.cells { + let mut vertices = *c; + let mut x: [Vector; DIM + 1] = core::array::from_fn(|k| pos(vertices[k])); + if SoftBody::cell_volume(x) < 0.0 { + vertices.swap(1, 2); + x = core::array::from_fn(|k| pos(vertices[k])); + } + let rest_volume = SoftBody::cell_volume(x); + let edge_matrix = SoftBody::cell_edge_matrix(x); + let inv_rest_matrix = if rest_volume.abs() > Real::EPSILON { + edge_matrix.inverse() + } else { + Matrix::ZERO + }; + cells.push(SoftBodyCell { + vertices, + rest_volume, + inv_rest_matrix, + plastic_stretch: Matrix::IDENTITY, + impulses: [0.0; super::super::CELL_IMPULSES], + rotation: Rotation::IDENTITY, + color: 0, + torn: false, + // Surface: given, or the boundary of the cells. + let surface = if !self.surface.is_empty() { + self.surface.clone() + } else { + cell_boundary(&cells) + }; + + // Edges: given, or the cell edges for the volume model (the corotational cells provide the + // whole material response: springs on top would add a stiffness floor of their own, + // independent of the Young modulus, and hold the original shape against plastic flow). + let mut edge_list: Vec<([u32; 2], SoftBodyEdgeKind)> = Vec::new(); + if self.edges.is_empty() + && !cells.is_empty() + && self.cell_model == SoftBodyCellModel::Volume + { + let mut seen: HashMap<[u32; 2], ()> = HashMap::default(); + for c in &cells { + for a in 0..DIM + 1 { + for b in a + 1..DIM + 1 { + let key = [ + c.vertices[a].min(c.vertices[b]), + c.vertices[a].max(c.vertices[b]), + ]; + if seen.insert(key, ()).is_none() { + edge_list.push((key, SoftBodyEdgeKind::Structural)); + } + } + } + } + } else { + edge_list.extend( + self.edges + .iter() + .map(|e| (*e, SoftBodyEdgeKind::Structural)), + ); + } + edge_list.extend(self.bend_edges.iter().map(|e| (*e, SoftBodyEdgeKind::Bend))); + let mut edges: Vec = edge_list + .iter() + .map(|(vertices, kind)| SoftBodyEdge { + vertices: *vertices, + rest_length: (pos(vertices[0]) - pos(vertices[1])).length(), + kind: *kind, + tension_only: false, + softness: None, + impulse: 0.0, + color: 0, + torn: false, + .collect(); + for &i in &self.tension_only_edges { + if let Some(e) = edges.get_mut(i as usize) { + e.tension_only = true; + } + } + for &(i, softness) in &self.edge_softness { + if let Some(e) = edges.get_mut(i as usize) { + e.softness = Some(softness); + } + } + + #[cfg(feature = "dim3")] + let mut dihedrals: Vec = self + .dihedrals + .iter() + .map(|v| SoftBodyDihedral { + vertices: *v, + rest_angle: dihedral_angle(pos(v[0]), pos(v[1]), pos(v[2]), pos(v[3])), + impulse: 0.0, + color: 0, + }) + .collect(); + + let (num_colors, has_overflow_color) = soft_body_coloring::assign_colors( + num_particles, + &mut edges, + &mut cells, + #[cfg(feature = "dim3")] + &mut dihedrals, + ); + + let rest_volume = SoftBody::boundary_volume(&surface, pos); + let boundary_closed = surface_is_closed(&surface); + let boundary_element_cells = surface_element_cells(&surface, &cells); + SoftBody { + volume_preservation: self.volume_preservation && !surface.is_empty(), + boundary_inverted: false, + rest_volume, + volume_impulse: 0.0, + } + } +} + +} diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs new file mode 100644 index 000000000..1d2498216 --- /dev/null +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs @@ -0,0 +1,114 @@ +//! The particle settings, the builder struct and its `Default` and `From` impls. + +use crate::alloc_prelude::*; +use crate::dynamics::SpringCoefficients; +use crate::geometry::ColliderBuilder; +use crate::math::{DIM, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::super::{SoftBody, SoftBodyCellModel, SoftBodyMaterial}; + +/// The dynamics settings shared by every particle of a soft body. +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftBodyParticleSettings { + /// Linear damping of the particles (air friction). + pub linear_damping: Real, + /// Gravity scale of the particles. + pub gravity_scale: Real, + /// Extra solver substeps requested for the particles (and everything they touch). + pub additional_solver_iterations: usize, + /// Whether the soft body may fall asleep. + pub can_sleep: bool, + /// Dominance group of the soft body (see [`crate::dynamics::RigidBody::dominance_group`]). + pub dominance_group: i8, +} + +impl Default for SoftBodyParticleSettings { + fn default() -> Self { + Self { + linear_damping: 0.0, + gravity_scale: 1.0, + additional_solver_iterations: 0, + can_sleep: true, + dominance_group: 0, + } + } +} + +/// A builder for [`crate::dynamics::SoftBody`]s: particles are given in world space and elements +/// reference particles by index. The generators ([`Self::rope`], plus `cloth`/`cuboid`/`sphere` +/// in 3D and `polygon`/`disk`/`grid` in 2D) make complete bodies; the setters build any body. +#[derive(Clone, Debug)] +pub struct SoftBodyBuilder { + /// World-space particle positions. + pub positions: Vec, + /// Per-particle masses (empty = uniform `particle_mass`). + pub masses: Vec, + /// Uniform particle mass, used when `masses` is empty. + pub particle_mass: Real, + /// Indices of the pinned particles. + pub pinned: Vec, + /// Structural edges (particle index pairs). + pub edges: Vec<[u32; 2]>, + /// Bending edges (particle index pairs). + pub bend_edges: Vec<[u32; 2]>, + /// Edges that only resist stretching, never compression (indices into the concatenation of + /// `edges` then `bend_edges`). + pub tension_only_edges: Vec, + /// Per-edge softness overrides (indices into the concatenation of `edges` then + /// `bend_edges`); the material's softness applies to the others. + pub edge_softness: Vec<(u32, SpringCoefficients)>, + /// Dihedral bending constraints: shared edge then the two opposite vertices. + #[cfg(feature = "dim3")] + pub dihedrals: Vec<[u32; 4]>, + /// Simplex cells (triangles in 2D, tetrahedra in 3D). + pub cells: Vec<[u32; DIM + 1]>, + /// Boundary elements (segments in 2D, triangles in 3D), oriented outward. Derived from the + /// cells when empty. + pub surface: Vec<[u32; DIM]>, + /// The material. + pub material: SoftBodyMaterial, + /// The constitutive model of the cells. + pub cell_model: SoftBodyCellModel, + /// Whether area/volume preservation is enabled (one constraint per piece of material enclosed + /// by a closed surface). + pub volume_preservation: bool, + /// Target volume multiplier (`> 1` inflates the body). + pub volume_factor: Real, + /// Whether shape matching is enabled. + pub shape_matching: bool, + /// Whether the body's surface collides with itself. + pub self_contacts: bool, + /// Thickness of the particles: the radius of their ball colliders (bodies colliding through + /// their particles), the surface collider's contact skin otherwise. + pub particle_radius: Real, + /// Template of the body's colliders: the deformable surface collider, or the particles' + /// balls (their shape is replaced by a ball of `particle_radius`) for a body colliding through + /// its particles; `None` disables collisions. + pub collider_template: Option, + /// Mesh held by the cells, if any: `(vertices, elements)` in world space, bound to the + /// cells at build time (see [`super::SoftBodySkin`]). + pub skin: Option<(Vec, Vec<[u32; DIM]>)>, + /// Whether the body meets the world through its skin rather than through its cells' boundary + /// (off by default, and no effect without a skin). + pub skin_collision: bool, + /// Dynamics settings of the particles. + pub particle_settings: SoftBodyParticleSettings, + /// User data of the soft body. + pub user_data: u128, +} + +impl Default for SoftBodyBuilder { + fn default() -> Self { + Self::new(Vec::new()) + } +} + +impl From for SoftBody { + fn from(builder: SoftBodyBuilder) -> Self { + builder.build() + } +} diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs new file mode 100644 index 000000000..b52f7c44b --- /dev/null +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs @@ -0,0 +1,290 @@ +//! Free mesh helpers of the soft-body builder: stray-piece dropping, surface tables, cell boundaries and the icosphere. + +use crate::alloc_prelude::*; +use crate::math::{DIM, Real, Vector}; +use parry::utils::hashmap::HashMap; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::super::SoftBodyCell; + +/// Drops the pieces of a filled mesh that are negligible next to its largest one; pieces of a +/// comparable size are all kept, since dropping them would lose limbs rather than specks. +pub(super) fn drop_stray_pieces( + mesh: &mut parry::transformation::VolumeMesh, + components: &[u32], + count: usize, +) { + let mut sizes = vec![0; count]; + for id in components { + sizes[*id as usize] += 1; + } + let largest = sizes.iter().max().copied().unwrap_or(0); + let mut kept = components + .iter() + .map(|id| sizes[*id as usize] * 20 >= largest); + mesh.cells.retain(|_| kept.next().unwrap_or(true)); + mesh.compact(); +} + +/// Mean length of `edges` over `positions` (`0.0` without edges). +pub(super) fn mean_edge_length(positions: &[Vector], edges: &[[u32; 2]]) -> Real { + if edges.is_empty() { + return 0.0; + } + edges + .iter() + .map(|e| (positions[e[0] as usize] - positions[e[1] as usize]).length()) + .sum::() + / edges.len() as Real +} + + counts.values().all(|&c| c == 2) + +/// The cell owning each surface element (`u32::MAX` when no cell contains its vertices). +pub(crate) fn surface_element_cells(surface: &[[u32; DIM]], cells: &[SoftBodyCell]) -> Vec { + if cells.is_empty() { + return Vec::new(); + } + let mut owners: HashMap<[u32; DIM], u32> = HashMap::default(); + for (ci, cell) in cells.iter().enumerate() { + for k in 0..=DIM { + let mut facet = [0u32; DIM]; + let mut n = 0; + for (j, &v) in cell.vertices.iter().enumerate() { + if j != k { + facet[n] = v; + n += 1; + } + } + facet.sort_unstable(); + owners.entry(facet).or_insert(ci as u32); + } + } + surface + .iter() + .map(|element| { + let mut key = *element; + key.sort_unstable(); + owners.get(&key).copied().unwrap_or(u32::MAX) + }) + .collect() +} + +/// The unique edges of a triangle surface (`[a, b]`, `a < b`, in first-seen order), the edge ids +/// of every triangle (edge `k` opposite to vertex `k`), and the triangle owning each edge (the +/// first one containing it). +#[cfg(feature = "dim3")] +pub(crate) fn surface_edge_table( + surface: &[[u32; DIM]], +) -> (Vec<[u32; 2]>, Vec<[u32; DIM]>, Vec) { + let mut ids: HashMap<[u32; 2], u32> = HashMap::default(); + let mut edges = Vec::new(); + let mut owners = Vec::new(); + let mut element_edges = Vec::with_capacity(surface.len()); + for (ei, element) in surface.iter().enumerate() { + let mut e = [u32::MAX; DIM]; + for k in 0..DIM { + let a = element[(k + 1) % DIM]; + let b = element[(k + 2) % DIM]; + let key = [a.min(b), a.max(b)]; + let id = *ids.entry(key).or_insert_with(|| { + edges.push(key); + owners.push(ei as u32); + edges.len() as u32 - 1 + }); + e[k] = id; + } + element_edges.push(e); + } + (edges, element_edges, owners) +} + +/// The surface elements incident to every particle as a CSR (offsets, element ids). +pub(crate) fn surface_vertex_elements( + num_particles: usize, + surface: &[[u32; DIM]], +) -> (Vec, Vec) { + let mut lists: Vec> = vec![Vec::new(); num_particles]; + for (i, element) in surface.iter().enumerate() { + for &v in element_vertices(element) { + if let Some(list) = lists.get_mut(v as usize) { + list.push(i as u32); + } + } + } + let mut offsets = Vec::with_capacity(num_particles + 1); + let mut flat = Vec::new(); + offsets.push(0); + for list in &lists { + flat.extend_from_slice(list); + offsets.push(flat.len() as u32); + } + (offsets, flat) +} + +/// The surface 1-ring of every particle as a CSR (offsets, sorted neighbor ids). +pub(crate) fn surface_rings(num_particles: usize, surface: &[[u32; DIM]]) -> (Vec, Vec) { + let mut rings: Vec> = vec![Vec::new(); num_particles]; + for element in surface { + for &a in element { + for &b in element { + if a != b { + if let Some(ring) = rings.get_mut(a as usize) { + ring.push(b); + } + } + } + } + } + let mut offsets = Vec::with_capacity(num_particles + 1); + let mut flat = Vec::new(); + offsets.push(0); + for ring in &mut rings { + ring.sort_unstable(); + ring.dedup(); + flat.extend_from_slice(ring); + offsets.push(flat.len() as u32); + } + (offsets, flat) +} + +/// The boundary of a set of positively-oriented cells: the faces (segments in 2D, triangles in +/// 3D) belonging to exactly one cell, oriented outward. +pub(super) fn cell_boundary(cells: &[SoftBodyCell]) -> Vec<[u32; DIM]> { + let mut count: HashMap<[u32; DIM], u32> = HashMap::default(); + let mut faces: Vec<[u32; DIM]> = Vec::new(); + for c in cells { + for face in cell_faces(c.vertices) { + let mut key = face; + key.sort_unstable(); + *count.entry(key).or_insert(0) += 1; + faces.push(face); + } + } + faces + .into_iter() + .filter(|face| { + let mut key = *face; + key.sort_unstable(); + count[&key] == 1 + }) + .collect() +} + +/// The outward-oriented faces of a positively-oriented simplex cell. +pub(crate) fn cell_faces(v: [u32; DIM + 1]) -> [[u32; DIM]; DIM + 1] { + #[cfg(feature = "dim2")] + { + [[v[0], v[1]], [v[1], v[2]], [v[2], v[0]]] + } + #[cfg(feature = "dim3")] + { + [ + [v[0], v[2], v[1]], + [v[0], v[1], v[3]], + [v[0], v[3], v[2]], + [v[1], v[2], v[3]], + ] + } +} + +/// The dihedral angle between the triangles `(p0, p1, p2)` and `(p0, p1, p3)`. +#[cfg(feature = "dim3")] +pub(crate) fn dihedral_angle(p0: Vector, p1: Vector, p2: Vector, p3: Vector) -> Real { + let e = p1 - p0; + let n1 = e.cross(p2 - p0); + let n2 = e.cross(p3 - p0); + let (l1, l2) = (n1.length(), n2.length()); + if l1 == 0.0 || l2 == 0.0 { + return 0.0; + } + (n1.dot(n2) / (l1 * l2)).clamp(-1.0, 1.0).acos() +} + +/// An icosphere: vertices on the sphere of the given center and radius, outward-oriented +/// triangles. +#[cfg(feature = "dim3")] +pub(super) fn icosphere(center: Vector, radius: Real, subdivisions: usize) -> (Vec, Vec<[u32; 3]>) { + let t = (1.0 + Real::sqrt(5.0)) * 0.5; + let mut vertices: Vec = [ + (-1.0, t, 0.0), + (1.0, t, 0.0), + (-1.0, -t, 0.0), + (1.0, -t, 0.0), + (0.0, -1.0, t), + (0.0, 1.0, t), + (0.0, -1.0, -t), + (0.0, 1.0, -t), + (t, 0.0, -1.0), + (t, 0.0, 1.0), + (-t, 0.0, -1.0), + (-t, 0.0, 1.0), + ] + .iter() + .map(|(x, y, z)| Vector::new(*x, *y, *z).normalize()) + .collect(); + let mut triangles: Vec<[u32; 3]> = vec![ + [0, 11, 5], + [0, 5, 1], + [0, 1, 7], + [0, 7, 10], + [0, 10, 11], + [1, 5, 9], + [5, 11, 4], + [11, 10, 2], + [10, 7, 6], + [7, 1, 8], + [3, 9, 4], + [3, 4, 2], + [3, 2, 6], + [3, 6, 8], + [3, 8, 9], + [4, 9, 5], + [2, 4, 11], + [6, 2, 10], + [8, 6, 7], + [9, 8, 1], + ]; + + for _ in 0..subdivisions { + let mut midpoints: HashMap<[u32; 2], u32> = HashMap::default(); + let mut midpoint = |a: u32, b: u32, vertices: &mut Vec| -> u32 { + let key = [a.min(b), a.max(b)]; + *midpoints.entry(key).or_insert_with(|| { + let m = ((vertices[a as usize] + vertices[b as usize]) * 0.5).normalize(); + vertices.push(m); + vertices.len() as u32 - 1 + }) + }; + let mut next = Vec::with_capacity(triangles.len() * 4); + for [a, b, c] in triangles { + let ab = midpoint(a, b, &mut vertices); + let bc = midpoint(b, c, &mut vertices); + let ca = midpoint(c, a, &mut vertices); + next.push([a, ab, ca]); + next.push([b, bc, ab]); + next.push([c, ca, bc]); + next.push([ab, bc, ca]); + } + triangles = next; + } + + // Outward orientation. + for tri in &mut triangles { + let (a, b, c) = ( + vertices[tri[0] as usize], + vertices[tri[1] as usize], + vertices[tri[2] as usize], + ); + if (b - a).cross(c - a).dot(a + b + c) < 0.0 { + tri.swap(1, 2); + } + } + + for v in &mut vertices { + *v = center + *v * radius; + } + (vertices, triangles) +} diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs new file mode 100644 index 000000000..385817deb --- /dev/null +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs @@ -0,0 +1,405 @@ +//! Setters and topology helpers of the soft-body builder: elements, materials, tearing, colliders and per-body settings. + +use crate::alloc_prelude::*; +use crate::dynamics::SpringCoefficients; +use crate::geometry::ColliderBuilder; +use crate::math::{DIM, Real, Vector}; +use parry::utils::hashmap::HashMap; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::super::{SoftBodyCellModel, SoftBodyMaterial}; +use super::{SoftBodyBuilder, SoftBodyParticleSettings}; + +impl SoftBodyBuilder { + /* + * Setters. + */ + + /// The particle positions set so far. + pub fn particle_positions(&self) -> &[Vector] { + &self.positions + } + + /// Sets the world-space positions of the particles. + pub fn positions(mut self, positions: Vec) -> Self { + self.positions = positions; + self + } + + /// Sets the mass of every particle. + pub fn particle_mass(mut self, mass: Real) -> Self { + self.particle_mass = mass; + self.masses.clear(); + self + } + + /// Sets the total mass of the soft body (split uniformly across the particles). + pub fn mass(mut self, mass: Real) -> Self { + self.particle_mass = mass / self.positions.len().max(1) as Real; + self.masses.clear(); + self + } + + /// Sets per-particle masses. + pub fn masses(mut self, masses: Vec) -> Self { + self.masses = masses; + self + } + + /// Pins the given particles (infinite mass, moved kinematically: see + /// [`crate::dynamics::SoftBody::set_particle_kinematic_target`]). + pub fn pinned_particles(mut self, pinned: impl IntoIterator) -> Self { + self.pinned.extend(pinned); + self + } + + /// Sets the structural edges. + pub fn edges(mut self, edges: Vec<[u32; 2]>) -> Self { + self.edges = edges; + self + } + + /// Adds structural edges (seams between two appended pieces, extra springs...); their + /// rest length is the current distance of their particles. + pub fn add_edges(mut self, edges: impl IntoIterator) -> Self { + let inserted: Vec<[u32; 2]> = edges.into_iter().collect(); + // The per-edge overrides index the concatenation of `edges` and `bend_edges`: the + // bending indices shift. + let shift = inserted.len() as u32; + let first_bend = self.edges.len() as u32; + for i in &mut self.tension_only_edges { + if *i >= first_bend { + *i += shift; + } + } + for (i, _) in &mut self.edge_softness { + if *i >= first_bend { + *i += shift; + } + } + self.edges.extend(inserted); + self + } + + /// Appends the particles and elements of `other` (its indices shifted past this builder's + /// particles): the two pieces become one soft body, to be sewn together with + /// [`Self::add_edges`]. This builder's material, model and settings are kept. + pub fn append(mut self, other: SoftBodyBuilder) -> Self { + let offset = self.positions.len() as u32; + // Per-particle masses become explicit as soon as one side has some. + if !self.masses.is_empty() || !other.masses.is_empty() { + if self.masses.is_empty() { + self.masses = vec![self.particle_mass; self.positions.len()]; + } + if other.masses.is_empty() { + self.masses.extend(core::iter::repeat_n( + other.particle_mass, + other.positions.len(), + )); + } else { + self.masses.extend_from_slice(&other.masses); + } + } + // The per-edge overrides of both sides, re-indexed into the new concatenation. + let (self_edges, self_bends) = (self.edges.len() as u32, self.bend_edges.len() as u32); + let (other_edges, other_bends) = (other.edges.len() as u32, other.bend_edges.len() as u32); + let remap_self = |i: u32| if i < self_edges { i } else { i + other_edges }; + let remap_other = |i: u32| { + if i < other_edges { + i + self_edges + } else { + i - other_edges + self_edges + other_edges + self_bends + } + }; + for i in &mut self.tension_only_edges { + *i = remap_self(*i); + } + for (i, _) in &mut self.edge_softness { + *i = remap_self(*i); + } + self.tension_only_edges + .extend(other.tension_only_edges.iter().map(|&i| remap_other(i))); + self.edge_softness.extend( + other + .edge_softness + .iter() + .map(|&(i, c)| (remap_other(i), c)), + ); + let _ = other_bends; + let shift = |v: &[u32]| -> Vec { v.iter().map(|&i| i + offset).collect() }; + self.positions.extend_from_slice(&other.positions); + self.pinned.extend(shift(&other.pinned)); + self.edges + .extend(other.edges.iter().map(|e| [e[0] + offset, e[1] + offset])); + self.bend_edges.extend( + other + .bend_edges + .iter() + .map(|e| [e[0] + offset, e[1] + offset]), + ); + #[cfg(feature = "dim3")] + self.dihedrals.extend( + other + .dihedrals + .iter() + .map(|d| [d[0] + offset, d[1] + offset, d[2] + offset, d[3] + offset]), + ); + self.cells + .extend(other.cells.iter().map(|c| c.map(|i| i + offset))); + self.surface + .extend(other.surface.iter().map(|s| s.map(|i| i + offset))); + self + } + + /// Sets the bending edges. + pub fn bend_edges(mut self, edges: Vec<[u32; 2]>) -> Self { + self.bend_edges = edges; + self + } + + /// Makes every edge (structural and bending) only resist stretching, never compression. + pub fn tension_only(mut self) -> Self { + self.tension_only_edges = (0..(self.edges.len() + self.bend_edges.len()) as u32).collect(); + self + } + + /// Sets the dihedral bending constraints (shared edge then the two opposite vertices). + #[cfg(feature = "dim3")] + pub fn dihedrals(mut self, dihedrals: Vec<[u32; 4]>) -> Self { + self.dihedrals = dihedrals; + self + } + + /// Sets the simplex cells (triangles in 2D, tetrahedra in 3D). Structural edges along the + /// cell edges are added by [`Self::build`] if none were given. + pub fn cells(mut self, cells: Vec<[u32; DIM + 1]>) -> Self { + self.cells = cells; + self + } + + /// Moves everything the builder holds in world space: the particles and the skin. + /// + /// Useful to drop copies of a meshed body at several places without meshing it again; moving + /// the particles alone would leave the skin behind. + pub fn translated(mut self, translation: Vector) -> Self { + for position in &mut self.positions { + *position += translation; + } + if let Some((vertices, _)) = &mut self.skin { + for vertex in vertices { + *vertex += translation; + } + } + self + } + + /// Sets the mesh the cells carry: the body's default collision mesh is then that mesh, + /// bound to the cells ([`crate::dynamics::SoftMeshMapping::Skinned`]), instead of the cells' boundary. + /// + /// The vertices are in world space, in the pose the cells are built in; each is bound to the + /// cell closest to it. The mesh adds no particle and no constraint row, so it can be as + /// detailed as wanted, and it can hold features the cells are far too coarse to resolve. + pub fn skin(mut self, vertices: Vec, indices: Vec<[u32; DIM]>) -> Self { + self.skin = Some((vertices, indices)); + self + } + + /// Whether the body's skin is its collision mesh rather than its cells' boundary (off by + /// default, and no effect without a skin). Further meshes, skinned or not, are added after + /// insertion with [`crate::geometry::ColliderSet::insert_deformable`]. + /// + /// A skin is what the body looks like, so it is also what the body could collide as: the + /// contact is then resolved through the particles of the cell carrying the touched part of + /// the skin, and the skin needs to be at least as fine as the cells. Off, collisions stay on + /// the cells, which is coarser but is what the expulsion and internal-feature rules were + /// written for. + pub fn skin_collision(mut self, enabled: bool) -> Self { + self.skin_collision = enabled; + self + } + + /// Sets the computational mesh's boundary elements (segments in 2D, triangles in 3D), + /// oriented outward: what the volume constraint integrates over, and the geometry of the default + /// collision mesh. + pub fn surface(mut self, surface: Vec<[u32; DIM]>) -> Self { + self.surface = surface; + self + } + + /// Sets the material. + pub fn material(mut self, material: SoftBodyMaterial) -> Self { + self.material = material; + self + } + + /// Sets the same softness for every constraint family. + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.material = SoftBodyMaterial::uniform(softness); + self + } + + /// Sets the material's tear strain (see [`SoftBodyMaterial::tear_strain`]). + pub fn tear_strain(mut self, strain: Real) -> Self { + self.material.tear_strain = Some(strain); + self + } + + /// Sets the constitutive model of the cells. + pub fn cell_model(mut self, model: SoftBodyCellModel) -> Self { + self.cell_model = model; + self + } + + /// Enables area/volume preservation: one constraint per piece of material enclosed by a + /// closed surface (see [`crate::dynamics::SoftBody::volume_pieces`]). + pub fn volume_preservation(mut self, enabled: bool) -> Self { + self.volume_preservation = enabled; + self + } + + /// Sets the target volume multiplier (`> 1` inflates the body). + pub fn volume_factor(mut self, factor: Real) -> Self { + self.volume_factor = factor; + self + } + + /// Enables shape matching. + pub fn shape_matching(mut self, enabled: bool) -> Self { + self.shape_matching = enabled; + self + } + + /// Enables self contacts between the body's surface vertices/edges and its own surface (a + /// body colliding through its surface only: the particle balls of a body colliding through + /// its particles never collide with each other). + pub fn self_contacts(mut self, enabled: bool) -> Self { + self.self_contacts = enabled; + self + } + + /// Sets the thickness of the particles (the radius of their ball colliders for a body + /// colliding through its particles, the surface collider's contact skin otherwise). + pub fn particle_radius(mut self, radius: Real) -> Self { + self.particle_radius = radius; + self + } + + } + + /// Gives the body no default collision mesh collider: it collides through the meshes added + /// after insertion, if any. + pub fn no_surface_collider(mut self) -> Self { + self.collider_template = None; + self + } + + /// Sets the dynamics settings of the particles (damping, gravity scale, sleeping...). + pub fn particle_settings(mut self, settings: SoftBodyParticleSettings) -> Self { + self.particle_settings = settings; + self + } + + /// Sets the linear damping of the particles. + pub fn linear_damping(mut self, damping: Real) -> Self { + self.particle_settings.linear_damping = damping; + self + } + + /// Sets the gravity scale of the particles. + pub fn gravity_scale(mut self, scale: Real) -> Self { + self.particle_settings.gravity_scale = scale; + self + } + + /// Requests extra solver substeps for the particles (and everything they touch). + pub fn additional_solver_iterations(mut self, iterations: usize) -> Self { + self.particle_settings.additional_solver_iterations = iterations; + self + } + + /// Whether the particles may fall asleep. + pub fn can_sleep(mut self, can_sleep: bool) -> Self { + self.particle_settings.can_sleep = can_sleep; + self + } + + /// Sets the user data of the soft body. + pub fn user_data(mut self, data: u128) -> Self { + self.user_data = data; + self + } + + /* + * Topology helpers. + */ + + /// The unique edges of the surface elements. + pub fn surface_edges(&self) -> Vec<[u32; 2]> { + let mut seen: HashMap<[u32; 2], ()> = HashMap::default(); + let mut edges = Vec::new(); + for element in &self.surface { + for k in 0..DIM { + let a = element[k]; + let b = element[(k + 1) % DIM]; + let key = [a.min(b), a.max(b)]; + if seen.insert(key, ()).is_none() { + edges.push(key); + } + } + } + edges + } + + /// The unique edges of the cells. + pub fn cell_edges(&self) -> Vec<[u32; 2]> { + let mut seen: HashMap<[u32; 2], ()> = HashMap::default(); + let mut edges = Vec::new(); + for cell in &self.cells { + for i in 0..DIM + 1 { + for j in i + 1..DIM + 1 { + let key = [cell[i].min(cell[j]), cell[i].max(cell[j])]; + if seen.insert(key, ()).is_none() { + edges.push(key); + } + } + } + } + edges + } + + /// The dihedral constraints across every interior edge of the triangle surface (3D only). + #[cfg(feature = "dim3")] + pub fn surface_dihedrals(&self) -> Vec<[u32; 4]> { + // edge -> (first opposite vertex, second opposite vertex) + let mut opposite: HashMap<[u32; 2], (u32, Option)> = HashMap::default(); + let mut order = Vec::new(); + for tri in &self.surface { + for k in 0..3 { + let a = tri[k]; + let b = tri[(k + 1) % 3]; + let c = tri[(k + 2) % 3]; + let key = [a.min(b), a.max(b)]; + match opposite.get_mut(&key) { + None => { + opposite.insert(key, (c, None)); + order.push(key); + } + Some((_, second)) => { + if second.is_none() { + *second = Some(c); + } + } + } + } + } + order + .into_iter() + .filter_map(|key| { + let (c, d) = opposite[&key]; + d.map(|d| [key[0], key[1], c, d]) + }) + .collect() + } +} diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs new file mode 100644 index 000000000..5e2bc0897 --- /dev/null +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs @@ -0,0 +1,582 @@ +//! Shape constructors of the soft-body builder: meshes, ropes, cloths, cuboids, spheres, polygons, disks and grids. + +use crate::alloc_prelude::*; +#[cfg(feature = "dim3")] +use crate::dynamics::SpringCoefficients; +use crate::geometry::ColliderBuilder; +use crate::math::{DIM, Real, Vector}; +use na::SimdRealField; +use parry::utils::hashmap::HashMap; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::super::{SoftBodyCellModel, SoftBodyMaterial}; +use super::soft_body_builder_mesh_helpers::{cell_faces, drop_stray_pieces, mean_edge_length}; +#[cfg(feature = "dim3")] +use super::soft_body_builder_mesh_helpers::icosphere; +use super::{SoftBodyBuilder, SoftBodyParticleSettings}; + +impl SoftBodyBuilder { + /// A builder over the given world-space particle positions, with no element. + pub fn new(positions: Vec) -> Self { + Self { + positions, + masses: Vec::new(), + particle_mass: 1.0, + pinned: Vec::new(), + edges: Vec::new(), + bend_edges: Vec::new(), + tension_only_edges: Vec::new(), + edge_softness: Vec::new(), + #[cfg(feature = "dim3")] + dihedrals: Vec::new(), + cells: Vec::new(), + surface: Vec::new(), + material: SoftBodyMaterial::default(), + cell_model: SoftBodyCellModel::default(), + volume_preservation: false, + volume_factor: 1.0, + shape_matching: false, + self_contacts: false, + particle_radius: 0.01, + skin: None, + skin_collision: false, + collider_template: Some(ColliderBuilder::ball(0.05).density(0.0)), + particle_settings: SoftBodyParticleSettings::default(), + user_data: 0, + } + } + + /* + * Generators. + */ + + /// A triangle-mesh soft body without volumetric cells (`None` if empty): vertices are + /// particles, edges are structural and shape matching holds the shape. 3D: the triangles are + /// the surface (dihedral bending across interior edges); 2D: the boundary is the surface. + pub fn trimesh(vertices: Vec, indices: Vec<[u32; 3]>) -> Option { + if vertices.is_empty() || indices.is_empty() { + return None; + } + #[cfg(feature = "dim3")] + { + let mut builder = Self::new(vertices).surface(indices); + let edges = builder.surface_edges(); + let dihedrals = builder.surface_dihedrals(); + let mean_edge = mean_edge_length(&builder.positions, &edges); + builder = builder + .edges(edges) + .dihedrals(dihedrals) + .shape_matching(true) + .particle_radius(mean_edge * 0.5); + Some(builder) + } + #[cfg(feature = "dim2")] + { + // Counter-clockwise triangles, so their unshared (boundary) edges wind + // counter-clockwise around the shape (and clockwise around its holes). + let triangles: Vec<[u32; 3]> = indices + .iter() + .map(|&[a, b, c]| { + let (pa, pb, pc) = ( + vertices[a as usize], + vertices[b as usize], + vertices[c as usize], + ); + if (pb - pa).perp_dot(pc - pa) >= 0.0 { + [a, b, c] + } else { + [a, c, b] + } + }) + .collect(); + let mut directed: HashMap<[u32; 2], ()> = HashMap::default(); + let mut structural: HashMap<[u32; 2], ()> = HashMap::default(); + let mut edges = Vec::new(); + for tri in &triangles { + for k in 0..3 { + let (u, v) = (tri[k], tri[(k + 1) % 3]); + directed.insert([u, v], ()); + let key = [u.min(v), u.max(v)]; + if structural.insert(key, ()).is_none() { + edges.push(key); + } + } + } + // A directed edge whose reverse is used by no triangle bounds the shape. + let mut boundary: Vec<[u32; 2]> = Vec::new(); + for tri in &triangles { + for k in 0..3 { + let (u, v) = (tri[k], tri[(k + 1) % 3]); + if !directed.contains_key(&[v, u]) { + boundary.push([u, v]); + } + } + } + let mean_edge = mean_edge_length(&vertices, &edges); + Some( + Self::new(vertices) + .edges(edges) + .surface(boundary) + .shape_matching(true) + .particle_radius(mean_edge * 0.5), + ) + } + } + + /// A soft body from a polyline (2D only): particles at the vertices, structural edges and the + /// surface on the segments, bending edges at two-segment vertices, shape matching, an inside + /// ([`Self::volume_preservation`]) if closed counter-clockwise; `None` for an empty polyline. + #[cfg(feature = "dim2")] + pub fn polyline(vertices: Vec, indices: Option>) -> Option { + if vertices.len() < 2 { + return None; + } + let segments = + indices.unwrap_or_else(|| (0..vertices.len() as u32 - 1).map(|i| [i, i + 1]).collect()); + if segments.is_empty() { + return None; + } + // The two neighbors of every vertex along the polyline (`u32::MAX`: none). + let mut neighbors: Vec<[u32; 2]> = vec![[u32::MAX; 2]; vertices.len()]; + let mut valence: Vec = vec![0; vertices.len()]; + for &[a, b] in &segments { + for (v, other) in [(a, b), (b, a)] { + let n = &mut neighbors[v as usize]; + let count = &mut valence[v as usize]; + if *count < 2 { + n[*count as usize] = other; + } + *count += 1; + } + } + let bend_edges: Vec<[u32; 2]> = neighbors + .iter() + .zip(&valence) + .filter(|(n, count)| **count == 2 && n[0] != n[1]) + .map(|(n, _)| [n[0].min(n[1]), n[0].max(n[1])]) + .collect(); + let mean_edge = mean_edge_length(&vertices, &segments); + Some( + Self::new(vertices) + .edges(segments.clone()) + .bend_edges(bend_edges) + .surface(segments) + .shape_matching(true) + .particle_radius(mean_edge * 0.5), + ) + } + + /// A rope of `num_particles` particles from `start` to `end`: structural edges between + /// neighbors, bending edges between second neighbors; in 2D the segments are also the surface. + /// The particle radius (the rope's thickness) defaults to half the segment length. + pub fn rope(start: Vector, end: Vector, num_particles: usize) -> Self { + let num_particles = num_particles.max(2); + let positions: Vec = (0..num_particles) + .map(|i| start.lerp(end, i as Real / (num_particles - 1) as Real)) + .collect(); + let edges: Vec<[u32; 2]> = (0..num_particles as u32 - 1).map(|i| [i, i + 1]).collect(); + let bend_edges = (0..num_particles as u32) + .filter_map(|i| (i + 2 < num_particles as u32).then_some([i, i + 2])) + .collect(); + let segment = (end - start).length() / (num_particles - 1) as Real; + let builder = Self::new(positions).particle_radius(segment * 0.5); + #[cfg(feature = "dim2")] + let builder = builder.surface(edges.clone()); + builder.edges(edges).bend_edges(bend_edges) + } + + /// A rectangular cloth of `nu × nv` particles (3D only): `origin + i * du + j * dv`, with + /// structural, shear and bending edges, and a triangle surface for rendering. The particle + /// radius defaults to half the smallest edge length so the particles tile the surface. + #[cfg(feature = "dim3")] + pub fn cloth(origin: Vector, du: Vector, dv: Vector, nu: usize, nv: usize) -> Self { + let (nu, nv) = (nu.max(2), nv.max(2)); + let idx = |i: usize, j: usize| (i * nv + j) as u32; + let mut positions = Vec::with_capacity(nu * nv); + for i in 0..nu { + for j in 0..nv { + positions.push(origin + du * i as Real + dv * j as Real); + } + } + let mut edges = Vec::new(); + let mut bend_edges = Vec::new(); + let mut surface = Vec::new(); + for i in 0..nu { + for j in 0..nv { + if i + 1 < nu { + edges.push([idx(i, j), idx(i + 1, j)]); + } + if j + 1 < nv { + edges.push([idx(i, j), idx(i, j + 1)]); + } + if i + 1 < nu && j + 1 < nv { + // Shear edges (both diagonals). + edges.push([idx(i, j), idx(i + 1, j + 1)]); + edges.push([idx(i + 1, j), idx(i, j + 1)]); + // Two triangles per quad, alternating diagonals. + if (i + j) % 2 == 0 { + surface.push([idx(i, j), idx(i + 1, j), idx(i + 1, j + 1)]); + surface.push([idx(i, j), idx(i + 1, j + 1), idx(i, j + 1)]); + } else { + surface.push([idx(i, j), idx(i + 1, j), idx(i, j + 1)]); + surface.push([idx(i + 1, j), idx(i + 1, j + 1), idx(i, j + 1)]); + } + } + if i + 2 < nu { + bend_edges.push([idx(i, j), idx(i + 2, j)]); + } + if j + 2 < nv { + bend_edges.push([idx(i, j), idx(i, j + 2)]); + } + } + } + let radius = du.length().min(dv.length()) * 0.5; + Self::new(positions) + .edges(edges) + .bend_edges(bend_edges) + .surface(surface) + .particle_radius(radius) + } + + /// A cloth tube: `num_along` rings of `num_around` particles around the segment `origin` to + /// `origin + axis`, radius linear from `radius_start` to `radius_end`; edges and surface of + /// [`Self::cloth`] closed around the axis. Ring-major particles: ring `0` is `0..num_around`. + #[cfg(feature = "dim3")] + pub fn cloth_tube( + origin: Vector, + axis: Vector, + radius_start: Real, + radius_end: Real, + num_around: usize, + num_along: usize, + ) -> Self { + use crate::utils::OrthonormalBasis; + let (na, nl) = (num_around.max(3), num_along.max(2)); + let dir = axis.try_normalize().unwrap_or(Vector::Y); + let [u, v] = dir.orthonormal_basis(); + let idx = |ring: usize, k: usize| (ring * na + (k % na)) as u32; + let mut positions = Vec::with_capacity(na * nl); + for ring in 0..nl { + let t = ring as Real / (nl - 1) as Real; + let radius = radius_start + (radius_end - radius_start) * t; + let center = origin + axis * t; + for k in 0..na { + let a = k as Real / na as Real * Real::simd_two_pi(); + positions.push(center + (u * a.cos() + v * a.sin()) * radius); + } + } + let mut edges = Vec::new(); + let mut bend_edges = Vec::new(); + let mut surface = Vec::new(); + for ring in 0..nl { + for k in 0..na { + edges.push([idx(ring, k), idx(ring, k + 1)]); + bend_edges.push([idx(ring, k), idx(ring, k + 2)]); + if ring + 1 < nl { + edges.push([idx(ring, k), idx(ring + 1, k)]); + edges.push([idx(ring, k), idx(ring + 1, k + 1)]); + edges.push([idx(ring, k + 1), idx(ring + 1, k)]); + if (ring + k) % 2 == 0 { + surface.push([idx(ring, k), idx(ring + 1, k), idx(ring + 1, k + 1)]); + surface.push([idx(ring, k), idx(ring + 1, k + 1), idx(ring, k + 1)]); + } else { + surface.push([idx(ring, k), idx(ring + 1, k), idx(ring, k + 1)]); + surface.push([idx(ring + 1, k), idx(ring + 1, k + 1), idx(ring, k + 1)]); + } + } + if ring + 2 < nl { + bend_edges.push([idx(ring, k), idx(ring + 2, k)]); + } + } + } + let ring_step = radius_start.min(radius_end) * Real::simd_two_pi() / na as Real; + let radius = ring_step.min(axis.length() / (nl - 1) as Real) * 0.5; + Self::new(positions) + .edges(edges) + .bend_edges(bend_edges) + .surface(surface) + .particle_radius(radius) + } + + /// [`Self::cloth`] with anisotropic stiffness: the edges along `du` (warp), along `dv` + /// (weft) and the diagonals (shear) get their own softness (the material's `bend_softness` + /// still applies to the bending edges). + #[cfg(feature = "dim3")] + pub fn cloth_anisotropic( + origin: Vector, + du: Vector, + dv: Vector, + nu: usize, + nv: usize, + warp: SpringCoefficients, + weft: SpringCoefficients, + shear: SpringCoefficients, + ) -> Self { + let mut builder = Self::cloth(origin, du, dv, nu, nv); + let (nu, nv) = (nu.max(2), nv.max(2)); + let idx = |i: usize, j: usize| (i * nv + j) as u32; + // Same edge order as `cloth`. + let mut k = 0u32; + for i in 0..nu { + for j in 0..nv { + if i + 1 < nu { + debug_assert_eq!(builder.edges[k as usize], [idx(i, j), idx(i + 1, j)]); + builder.edge_softness.push((k, warp)); + k += 1; + } + if j + 1 < nv { + builder.edge_softness.push((k, weft)); + k += 1; + } + if i + 1 < nu && j + 1 < nv { + builder.edge_softness.push((k, shear)); + builder.edge_softness.push((k + 1, shear)); + k += 2; + } + } + } + builder + } + + /// A solid box tetrahedralized on a `nx × ny × nz` particle grid (3D only), with structural + /// edges along the tetrahedra edges and the boundary triangles as surface. + /// + /// The particle radius defaults to half the smallest cell size. + #[cfg(feature = "dim3")] + pub fn cuboid(center: Vector, half_extents: Vector, nx: usize, ny: usize, nz: usize) -> Self { + let (nx, ny, nz) = (nx.max(2), ny.max(2), nz.max(2)); + let step = Vector::new( + 2.0 * half_extents.x / (nx - 1) as Real, + 2.0 * half_extents.y / (ny - 1) as Real, + 2.0 * half_extents.z / (nz - 1) as Real, + ); + let idx = |i: usize, j: usize, k: usize| ((i * ny + j) * nz + k) as u32; + let mut positions = Vec::with_capacity(nx * ny * nz); + for i in 0..nx { + for j in 0..ny { + for k in 0..nz { + positions.push( + center - half_extents + + Vector::new( + step.x * i as Real, + step.y * j as Real, + step.z * k as Real, + ), + ); + } + } + } + // Five tetrahedra per cube, split parity alternating so neighboring cubes share their + // face diagonals. + let mut cells = Vec::new(); + for i in 0..nx - 1 { + for j in 0..ny - 1 { + for k in 0..nz - 1 { + // Corner `c` has bits (x, y, z) = (c & 1, c >> 1 & 1, c >> 2 & 1). + let v: [u32; 8] = core::array::from_fn(|c| { + idx(i + (c & 1), j + ((c >> 1) & 1), k + ((c >> 2) & 1)) + }); + let tets: [[usize; 4]; 5] = if (i + j + k) % 2 == 0 { + [ + [0, 3, 5, 6], + [1, 0, 3, 5], + [2, 0, 3, 6], + [4, 0, 5, 6], + [7, 3, 5, 6], + ] + } else { + [ + [1, 2, 4, 7], + [0, 1, 2, 4], + [3, 1, 2, 7], + [5, 1, 4, 7], + [6, 2, 4, 7], + ] + }; + for t in tets { + cells.push([v[t[0]], v[t[1]], v[t[2]], v[t[3]]]); + } + } + } + } + let radius = step.x.min(step.y).min(step.z) * 0.5; + Self::new(positions).cells(cells).particle_radius(radius) + } + + /// A volumetric body filling a closed, outward-oriented mesh (triangles in 3D, segments in 2D) + /// with cells of size `cell_size`; the surface is their boundary. Particle radius defaults to + /// half the mean cell edge; `None` if the mesh is empty, open or encloses nothing at that size. + pub fn volumetric( + vertices: &[Vector], + indices: &[[u32; DIM]], + cell_size: Real, + ) -> Option { + Self::volumetric_with( + vertices, + indices, + &parry::transformation::VolumeMeshParameters::new(cell_size), + ) + } + + /// The same as [`Self::volumetric`], with the meshing parameters spelled out: element size, + /// the cover's smoothing, guard and subdivision, and whether the surface alone is covered + /// (the crust). + pub fn volumetric_with( + vertices: &[Vector], + indices: &[[u32; DIM]], + params: &parry::transformation::VolumeMeshParameters, + ) -> Option { + let mut mesh = parry::transformation::volume_mesh(vertices, indices, params)?; + let components = mesh.connected_components(); + let count = components.iter().max().map(|id| *id as usize + 1)?; + if count > 1 { + drop_stray_pieces(&mut mesh, &components, count); + } + Self::from_cells(mesh) + } + + /// A body of the cells of a filled mesh, with the particle radius the mesh asks for. + fn from_cells(mesh: parry::transformation::VolumeMesh) -> Option { + let mut builder = Self::new(mesh.vertices).cells(mesh.cells); + // Half the mean edge of the *boundary*: with a subdivided cover the interior edges are + // longer than the boundary's, and it is the boundary the radius is a thickness for. + let mut counts: HashMap<[u32; DIM], u32> = HashMap::default(); + for cell in &builder.cells { + for face in cell_faces(*cell) { + let mut key = face; + key.sort_unstable(); + *counts.entry(key).or_insert(0) += 1; + } + } + let mut seen: HashMap<[u32; 2], ()> = HashMap::default(); + let mut edges = Vec::new(); + for (face, count) in &counts { + if *count != 1 { + continue; + } + for k in 0..DIM { + let (a, b) = (face[k], face[(k + 1) % DIM]); + let key = [a.min(b), a.max(b)]; + if seen.insert(key, ()).is_none() { + edges.push(key); + } + } + } + let radius = mean_edge_length(&builder.positions, &edges) * 0.5; + builder = builder.particle_radius(radius); + Some(builder) + } + + /// The same as [`Self::volumetric`], keeping the boundary mesh as the body's skin: the cells + /// hold it and the body is drawn as it, so thin features survive a cell size that cannot + /// resolve them; it still collides through its cells, so contacts are as coarse as they are. + pub fn volumetric_skinned( + vertices: &[Vector], + indices: &[[u32; DIM]], + cell_size: Real, + ) -> Option { + let builder = Self::volumetric(vertices, indices, cell_size)?; + Some(builder.skin(vertices.to_vec(), indices.to_vec())) + } + + /// A hollow sphere (3D only): an icosphere surface with `subdivisions` refinement levels, + /// structural edges along the triangle edges, dihedral bending constraints across them, and + /// volume preservation. The particle radius defaults to half the mean edge length. + #[cfg(feature = "dim3")] + pub fn sphere(center: Vector, radius: Real, subdivisions: usize) -> Self { + let (positions, triangles) = icosphere(center, radius, subdivisions); + let mut builder = Self::new(positions).surface(triangles); + let dihedrals = builder.surface_dihedrals(); + let edges = builder.surface_edges(); + let mean_edge = edges + .iter() + .map(|e| (builder.positions[e[0] as usize] - builder.positions[e[1] as usize]).length()) + .sum::() + / edges.len().max(1) as Real; + builder = builder + .edges(edges) + .dihedrals(dihedrals) + .volume_preservation(true) + .particle_radius(mean_edge * 0.5); + builder + } + + /// A closed polygon (counter-clockwise `points`): structural edges along the boundary, + /// bending edges between second neighbors, and area preservation (2D only). + /// + /// The particle radius defaults to half the mean edge length. + #[cfg(feature = "dim2")] + pub fn polygon(points: Vec) -> Self { + let n = points.len().max(3) as u32; + let edges: Vec<[u32; 2]> = (0..n).map(|i| [i, (i + 1) % n]).collect(); + let bend_edges: Vec<[u32; 2]> = if n > 4 { + (0..n).map(|i| [i, (i + 2) % n]).collect() + } else { + Vec::new() + }; + let surface: Vec<[u32; DIM]> = edges.clone(); + let mean_edge = edges + .iter() + .map(|e| (points[e[0] as usize] - points[e[1] as usize]).length()) + .sum::() + / n as Real; + Self::new(points) + .edges(edges) + .bend_edges(bend_edges) + .surface(surface) + .volume_preservation(true) + .particle_radius(mean_edge * 0.5) + } + + /// A regular polygon with `num_particles` boundary particles (see [`Self::polygon`], 2D only). + #[cfg(feature = "dim2")] + pub fn disk(center: Vector, radius: Real, num_particles: usize) -> Self { + let n = num_particles.max(3); + let points = (0..n) + .map(|i| { + let angle = i as Real / n as Real * Real::simd_two_pi(); + center + Vector::new(angle.cos(), angle.sin()) * radius + }) + .collect(); + Self::polygon(points) + } + + /// A solid rectangle triangulated on a `nx × ny` particle grid (2D only), with structural + /// edges along the triangle edges and the boundary segments as surface. + /// + /// The particle radius defaults to half the smallest cell size. + #[cfg(feature = "dim2")] + pub fn grid(center: Vector, half_extents: Vector, nx: usize, ny: usize) -> Self { + let (nx, ny) = (nx.max(2), ny.max(2)); + let step = Vector::new( + 2.0 * half_extents.x / (nx - 1) as Real, + 2.0 * half_extents.y / (ny - 1) as Real, + ); + let idx = |i: usize, j: usize| (i * ny + j) as u32; + let mut positions = Vec::with_capacity(nx * ny); + for i in 0..nx { + for j in 0..ny { + positions.push( + center - half_extents + Vector::new(step.x * i as Real, step.y * j as Real), + ); + } + } + let mut cells = Vec::new(); + for i in 0..nx - 1 { + for j in 0..ny - 1 { + let (a, b, c, d) = (idx(i, j), idx(i + 1, j), idx(i + 1, j + 1), idx(i, j + 1)); + if (i + j) % 2 == 0 { + cells.push([a, b, c]); + cells.push([a, c, d]); + } else { + cells.push([a, b, d]); + cells.push([b, c, d]); + } + } + } + let radius = step.x.min(step.y) * 0.5; + Self::new(positions).cells(cells).particle_radius(radius) + } +} diff --git a/src/dynamics/soft_body/soft_body_coloring.rs b/src/dynamics/soft_body/soft_body_coloring.rs new file mode 100644 index 000000000..e191c64be --- /dev/null +++ b/src/dynamics/soft_body/soft_body_coloring.rs @@ -0,0 +1,80 @@ +//! Greedy coloring of a soft body's elements: two elements sharing a particle never get the +//! same color, so the solver can sweep a whole color in parallel. Runs at build time and again +//! whenever the topology changes (tearing). + +use crate::alloc_prelude::*; + +#[cfg(feature = "dim3")] +use super::SoftBodyDihedral; +use super::soft_body::SOFT_BODY_OVERFLOW_COLOR; +use super::{SoftBodyCell, SoftBodyEdge}; + +/// Colors every element of a soft body (edges, cells, dihedrals in one shared color space); +/// returns `(num_colors, has_overflow)` as [`color_elements`] does. +pub(crate) fn assign_colors( + num_particles: usize, + edges: &mut [SoftBodyEdge], + cells: &mut [SoftBodyCell], + #[cfg(feature = "dim3")] dihedrals: &mut [SoftBodyDihedral], +) -> (u8, bool) { + let elements = edges + .iter() + .map(|e| &e.vertices[..]) + .chain(cells.iter().map(|c| &c.vertices[..])); + #[cfg(feature = "dim3")] + let elements = elements.chain(dihedrals.iter().map(|d| &d.vertices[..])); + let (colors, num_colors, has_overflow) = color_elements(num_particles, elements); + let mut colors = colors.into_iter(); + + for e in edges.iter_mut() { + e.color = colors.next().unwrap(); + } + + for c in cells.iter_mut() { + c.color = colors.next().unwrap(); + } + + #[cfg(feature = "dim3")] + for d in dihedrals.iter_mut() { + d.color = colors.next().unwrap(); + } + + (num_colors, has_overflow) +} + +/// Number of parallel colors available to soft-body elements (the `u128` per-particle masks). +const NUM_PARALLEL_COLORS: u32 = 128; + +/// Assigns colors to elements (small sets of particle indices); returns `(colors, num_colors, +/// has_overflow)`: each element's color, the number of parallel colors (`0..num_colors`), and +/// whether some element got [`SOFT_BODY_OVERFLOW_COLOR`] (all colors taken at one particle). +pub(crate) fn color_elements<'a>( + num_particles: usize, + elements: impl Iterator, +) -> (Vec, u8, bool) { + let mut particle_masks = vec![0u128; num_particles]; + let mut colors = Vec::new(); + let mut num_colors = 0u32; + let mut has_overflow = false; + + for element in elements { + let mut mask = 0u128; + for &v in element { + mask |= particle_masks[v as usize]; + } + let free = !mask; + let color = free.trailing_zeros(); + if color >= NUM_PARALLEL_COLORS { + colors.push(SOFT_BODY_OVERFLOW_COLOR); + has_overflow = true; + continue; + } + num_colors = num_colors.max(color + 1); + colors.push(color as u8); + for &v in element { + particle_masks[v as usize] |= 1u128 << color; + } + } + + (colors, num_colors as u8, has_overflow) +} diff --git a/src/dynamics/soft_body/soft_body_contacts.rs b/src/dynamics/soft_body/soft_body_contacts.rs new file mode 100644 index 000000000..f3a137366 --- /dev/null +++ b/src/dynamics/soft_body/soft_body_contacts.rs @@ -0,0 +1,132 @@ +//! The contact records a soft body keeps between steps (edge, vertex, overlap and volume contacts) and their debug-render accessors. +use crate::alloc_prelude::*; +use super::{SoftBody, SoftCollisionMesh, SoftMeshRef}; +use crate::math::{AngVector, DIM, Pose, Real, Vector}; + +/// An edge-vs-edge contact between two surface edges (segments in 2D, triangle edges in 3D): +/// the impulses of the last step, matched by edge ids for warm starting. +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) struct SoftEdgeContact { + /// The other edge's collision mesh (this mesh for a self contact). + pub other: SoftMeshRef, + /// Edge id in this body (element id in 2D), and in the other body. + pub edge: u32, + pub other_edge: u32, + /// Normal impulse and world-space friction impulse. + pub impulse: Real, + pub tangent_impulse: Vector, +} + +/// A vertex-vs-surface contact between a surface vertex of another soft body (or of this body: +/// self contacts) and one of this body's surface elements: the impulses of the last step, matched +/// by ids for warm starting. +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) struct SoftVertexContact { + /// The vertex's collision mesh (this mesh for a self contact). + pub other: SoftMeshRef, + /// The vertex (particle index in `other`) and the element (in this body). `flipped`: the + /// vertex is this body's and the element the other's (the other body is fully pinned and + /// not simulated, this body assembles both vertex passes). + pub vertex: u32, + pub element: u32, + pub flipped: bool, + /// Normal impulse and world-space friction impulse. + pub impulse: Real, + pub tangent_impulse: Vector, +} + + +#[cfg(all(test, feature = "dim2"))] +mod test { + use crate::alloc_prelude::*; + use crate::dynamics::soft_body::SoftVertexContact; + use crate::prelude::*; + + /// The closest point of segment `ab` to `p`, derived independently of the code under test. + fn clamped_projection(p: Vector, a: Vector, b: Vector) -> Vector { + let ab = b - a; + let t = ((p - a).dot(ab) / ab.length_squared()).clamp(0.0, 1.0); + a + ab * t + } + + /// A debug-render contact segment must end at the contact's witness point on the element (its + /// closest point to the vertex), not at the element's centroid: on a coarse mesh the two are + /// far apart, and the contact is drawn where nothing touches. + #[test] + fn vertex_contact_segment_ends_at_the_witness_point() { + let mut world = PhysicsWorld::new(); + let mut positions = Vec::new(); + for i in 0..4u32 { + for j in 0..4u32 { + positions.push(Vector::new(i as Real * 1.7, 10.0 + j as Real * 1.3)); + } + } + let mut cells = Vec::new(); + for i in 0..3u32 { + for j in 0..3u32 { + let (a, b, c, d) = (i * 4 + j, i * 4 + j + 1, (i + 1) * 4 + j, (i + 1) * 4 + j + 1); + cells.push([a, b, c]); + cells.push([b, d, c]); + } + } + let handle = world.insert_soft_body(SoftBodyBuilder::new(positions).cells(cells)); + world.step(); + + // The (vertex, element) pair whose witness point is furthest from the element's centroid: + // the configuration the two candidate renderings disagree on the most. + let sb = &world.soft_bodies[handle]; + let mesh = sb.meshes().next().expect("the body has a collision mesh"); + let id = mesh.id(); + let mut best = (0u32, 0u32, -1.0 as Real); + for v in 0..mesh.vertex_count() as u32 { + for (e, element) in mesh.indices().iter().enumerate() { + if element.contains(&v) { + continue; + } + let p = mesh.vertex(sb, v as usize); + let (a, b) = ( + mesh.vertex(sb, element[0] as usize), + mesh.vertex(sb, element[1] as usize), + ); + let spread = (clamped_projection(p, a, b) - (a + b) * 0.5).length(); + if spread > best.2 { + best = (v, e as u32, spread); + } + } + } + let (vertex, element, spread) = best; + assert!(spread > 0.5, "no off-center pair in this mesh"); + + // Inject that contact, then read back what the debug-renderer would draw for it. + let sb = world.soft_bodies.get_mut(handle).unwrap(); + let mesh = sb.mesh_mut(id).unwrap(); + mesh.vertex_contacts.clear(); + mesh.vertex_contacts.push(SoftVertexContact { + other: super::SoftMeshRef { body: handle, id }, + vertex, + element, + flipped: false, + impulse: 0.0, + tangent_impulse: Vector::ZERO, + }); + + let sb = &world.soft_bodies[handle]; + let mesh = sb.meshes().next().unwrap(); + let segments: Vec<_> = sb.vertex_contact_segments(&world.soft_bodies).collect(); + assert_eq!(segments.len(), 1); + + let idx = mesh.indices()[element as usize]; + let p = mesh.vertex(sb, vertex as usize); + let (a, b) = ( + mesh.vertex(sb, idx[0] as usize), + mesh.vertex(sb, idx[1] as usize), + ); + let (drawn_from, drawn_to) = segments[0]; + assert!((drawn_from - p).length() < 1.0e-5); + assert!((drawn_to - clamped_projection(p, a, b)).length() < 1.0e-5); + // What the centroid rendering would have drawn instead. + assert!((drawn_to - (a + b) * 0.5).length() > 0.5); + } +} diff --git a/src/dynamics/soft_body/soft_body_elements.rs b/src/dynamics/soft_body/soft_body_elements.rs new file mode 100644 index 000000000..2ac97dbfc --- /dev/null +++ b/src/dynamics/soft_body/soft_body_elements.rs @@ -0,0 +1,195 @@ +//! The element types of a soft body: particles, edges, dihedrals, cells, and the per-body solver and cell-model choices. +#[cfg(doc)] +use super::SoftBodyMaterial; +#[cfg(feature = "serde-serialize")] +use super::soft_body_material::one; +use crate::dynamics::SpringCoefficients; +use crate::math::{DIM, Matrix, Real, Rotation, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// One particle (point mass) of a soft body: plain state owned by its +/// [`crate::dynamics::SoftBody`], given its own solver body for the step when the body is awake. +/// A pinned particle (`inv_mass == 0`) is kinematic: it moves at its velocity or toward its target. +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftBodyParticle { + /// World-space position. + pub(crate) position: Vector, + /// World-space velocity. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) velocity: Vector, + /// Position in the body's rest shape, relative to the rest center of mass (the + /// shape-matching goal shape). Follows the plastic flow of the elements (see + /// [`Self::initial_rest_position`]). + pub(crate) rest_position: Vector, + /// Nominal mass (used by shape matching even for pinned particles). + pub(crate) mass: Real, + /// `0.0` for pinned particles. + pub(crate) inv_mass: Real, + /// User force applied to the particle every step until reset (`SoftBody::reset_forces`). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) force: Vector, + /// Where a pinned particle must be at the end of the next step (`None`: it keeps moving at + /// its velocity). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) next_position: Option, +} +impl SoftBodyParticle { + /// The world-space position of this particle. + pub fn position(&self) -> Vector { + self.position + } + + /// The world-space velocity of this particle. + pub fn velocity(&self) -> Vector { + self.velocity + } + + /// The user force currently applied to this particle (see [`crate::dynamics::SoftBody::add_particle_force`]). + pub fn force(&self) -> Vector { + self.force + } + + /// The position a pinned particle is moving to over the next step, if one was set with + /// [`crate::dynamics::SoftBody::set_particle_kinematic_target`]. + pub fn kinematic_target(&self) -> Option { + self.next_position + } + + /// The position of this particle in the rest shape (relative to the rest center of mass). + /// With a plastic material the rest shape follows the elements' flow (see + /// [`SoftBodyMaterial::plastic_yield`]); [`Self::initial_rest_position`] predates any flow. + pub fn rest_position(&self) -> Vector { + self.rest_position + } + + /// The nominal mass of this particle. + pub fn mass(&self) -> Real { + self.mass + } + + /// The inverse mass of this particle (`0.0` for pinned particles). + pub fn inv_mass(&self) -> Real { + self.inv_mass + } + + /// Whether this particle is pinned (infinite mass, moved kinematically). + pub fn is_pinned(&self) -> bool { + self.inv_mass == 0.0 + } + + /// Whether an element touching this particle has torn (or [`crate::dynamics::SoftBody::set_particle_damaged`] +} +/// The role of a soft-body edge (distance constraint). +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum SoftBodyEdgeKind { + /// A structural edge, keeping two neighboring particles at their rest distance + /// (softness: [`SoftBodyMaterial::edge_softness`]). + Structural, + /// A bending edge, keeping two second-neighbor particles at their rest distance + /// (softness: [`SoftBodyMaterial::bend_softness`]). + Bend, +} + +/// A distance constraint between two particles of a soft body. +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftBodyEdge { + /// The two particles. + pub vertices: [u32; 2], + /// The distance the edge tries to maintain. + pub rest_length: Real, + /// Structural or bending edge. + pub kind: SoftBodyEdgeKind, + /// If `true`, the edge only resists stretching (a rope segment), never compression. + pub tension_only: bool, + /// Softness of this edge, overriding the material's (`edge_softness` / `bend_softness`) + /// when set: anisotropic cloth (warp, weft and shear stiffness), stiffer seams... + pub softness: Option>, + /// Accumulated impulse of the last step (warm-start state). + pub(crate) impulse: Real, + /// Parallel solve color. + pub(crate) color: u8, + /// (or by [`crate::dynamics::SoftBody::tear_edge`]): removed by the tearing pass at the end of the step. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) torn: bool, +} +impl SoftBodyEdge { + /// The impulse this edge applied during the last substep, along the edge direction + /// (positive when the edge was resisting stretching). + pub fn impulse(&self) -> Real { + self.impulse + } + +} +/// A dihedral bending constraint between two triangles sharing an edge (3D only). +/// +/// `vertices[0..2]` is the shared edge, `vertices[2]` and `vertices[3]` the two opposite +/// vertices. +#[cfg(feature = "dim3")] +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftBodyDihedral { + /// The four particles. + pub vertices: [u32; 4], + /// The angle between the two triangle normals at rest. + pub rest_angle: Real, + /// Accumulated impulse of the last step (warm-start state). + pub(crate) impulse: Real, + /// Parallel solve color. + pub(crate) color: u8, +} + +/// The constitutive model of a soft body's cells (triangles in 2D, tetrahedra in 3D). +#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum SoftBodyCellModel { + /// One constraint per cell keeping its signed area/volume at its rest value + /// (softness: [`SoftBodyMaterial::volume_softness`]). Cheap; combined with the edges this + /// gives a convincing jelly. + #[default] + Volume, + /// Corotational elasticity (linear elasticity in the cell's rotation-free frame) plus an exact + /// volume constraint, parameterized by [`SoftBodyMaterial::young_modulus`], `poisson_ratio` and + /// `elastic_damping_ratio`. Stable at any stiffness, recovers from inverted cells. + Corotational, + /// Stable Neo-Hookean hyperelasticity (Smith et al. 2018): the parameters (plasticity too) and + /// small-strain behavior of [`SoftBodyCellModel::Corotational`], but Neo-Hookean at large + /// strains (stiffens when stretched, inverted cells pushed back through the collapse); 2x cost. + NeoHookean, +} + +/// Number of warm-start impulses stored per cell (the corotational model's strain rows plus its +/// volumetric row). +pub(crate) const CELL_IMPULSES: usize = DIM * (DIM + 1) / 2 + 1; + +/// A simplex cell of a soft body: a triangle in 2D, a tetrahedron in 3D. +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftBodyCell { + /// The cell's particles, positively oriented (positive signed area/volume at rest). + pub vertices: [u32; DIM + 1], + /// The signed area (2D) or volume (3D) of the cell at rest. + pub rest_volume: Real, + /// Inverse of the rest edge matrix `[x1 - x0, x2 - x0, (x3 - x0)]`. + pub(crate) inv_rest_matrix: Matrix, + /// Accumulated plastic stretch of the rest shape (material frame, unit determinant): the + /// current rest edge matrix is `plastic_stretch * Dm₀`. Bounded by the material's + /// `plastic_max`, which keeps flowing cells from degenerating into slivers. + pub(crate) plastic_stretch: Matrix, + /// Accumulated impulses of the last step (warm-start state): the volume row's in `[0]`, or + /// the corotational rows' (strain rows in the cell frame, then volumetric). + pub(crate) impulses: [Real; CELL_IMPULSES], + /// Rotation of the polar decomposition of the cell's deformation gradient at the last step + /// (warm start of the corotational rows' rotation extraction). + pub(crate) rotation: Rotation, + /// Parallel solve color. + pub(crate) color: u8, + /// Set when the cell was strained past the material's `tear_strain` during the last step + /// (or by [`crate::dynamics::SoftBody::tear_cell`]): removed by the tearing pass at the end of the step. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) torn: bool, +} diff --git a/src/dynamics/soft_body/soft_body_geometry.rs b/src/dynamics/soft_body/soft_body_geometry.rs new file mode 100644 index 000000000..63abc2e3a --- /dev/null +++ b/src/dynamics/soft_body/soft_body_geometry.rs @@ -0,0 +1,96 @@ +//! Geometric helpers of a soft body: the end-of-step orientation update and the boundary and cell volumes with their gradients. +use super::SoftBody; +use crate::math::{DIM, Matrix, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +impl SoftBody { + /// Updates the state derived from the particle positions at the end of a step: the volume + /// pieces' inside-out flags (their pressure targets) and the collision mesh's orientation. + pub(crate) fn update_orientation(&mut self) { + // With hysteresis: a crumpled body hovering around a zero volume must not flip its + // pressure target every step. + let volume = + Self::boundary_volume(&self.boundary, |i| self.particles[i as usize].position); + let ratio = volume / self.rest_volume; + if ratio < -0.25 { + } else if ratio > 0.25 { + } + self.for_each_mesh_mut(|body, mesh| mesh.update_orientation(body)); + } + + /// The signed area (2D) or volume (3D) enclosed by the elements of `boundary`. + pub(crate) fn boundary_volume( + boundary: &[[u32; DIM]], + position: impl Fn(u32) -> Vector, + ) -> Real { + let mut vol = 0.0; + for element in boundary { + #[cfg(feature = "dim2")] + { + let a = position(element[0]); + let b = position(element[1]); + vol += a.perp_dot(b) * 0.5; + } + #[cfg(feature = "dim3")] + { + let a = position(element[0]); + let b = position(element[1]); + let c = position(element[2]); + vol += a.dot(b.cross(c)) / 6.0; + } + } + vol + } + + /// Accumulates the gradient of the enclosed area/volume with respect to every particle. + pub(crate) fn boundary_volume_gradients( + boundary: &[[u32; DIM]], + position: impl Fn(u32) -> Vector, + gradients: &mut [Vector], + ) { + for element in boundary { + #[cfg(feature = "dim2")] + { + let a = position(element[0]); + let b = position(element[1]); + gradients[element[0] as usize] += Vector::new(b.y, -b.x) * 0.5; + gradients[element[1] as usize] += Vector::new(-a.y, a.x) * 0.5; + } + #[cfg(feature = "dim3")] + { + let a = position(element[0]); + let b = position(element[1]); + let c = position(element[2]); + gradients[element[0] as usize] += b.cross(c) / 6.0; + gradients[element[1] as usize] += c.cross(a) / 6.0; + gradients[element[2] as usize] += a.cross(b) / 6.0; + } + } + } + + /// Signed area (2D) or volume (3D) of a simplex cell. + pub(crate) fn cell_volume(x: [Vector; DIM + 1]) -> Real { + #[cfg(feature = "dim2")] + { + (x[1] - x[0]).perp_dot(x[2] - x[0]) * 0.5 + } + #[cfg(feature = "dim3")] + { + (x[1] - x[0]).dot((x[2] - x[0]).cross(x[3] - x[0])) / 6.0 + } + } + + /// The edge matrix `[x1 - x0, x2 - x0, (x3 - x0)]` of a simplex cell. + pub(crate) fn cell_edge_matrix(x: [Vector; DIM + 1]) -> Matrix { + #[cfg(feature = "dim2")] + { + Matrix::from_cols(x[1] - x[0], x[2] - x[0]) + } + #[cfg(feature = "dim3")] + { + Matrix::from_cols(x[1] - x[0], x[2] - x[0], x[3] - x[0]) + } + } +} diff --git a/src/dynamics/soft_body/soft_body_handle.rs b/src/dynamics/soft_body/soft_body_handle.rs new file mode 100644 index 000000000..59d77d5f6 --- /dev/null +++ b/src/dynamics/soft_body/soft_body_handle.rs @@ -0,0 +1,30 @@ +/// The unique handle of a soft body added to a [`super::SoftBodySet`]. +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[repr(transparent)] +pub struct SoftBodyHandle(pub crate::data::arena::Index); + +impl SoftBodyHandle { + /// Converts this handle into its (index, generation) components. + pub fn into_raw_parts(self) -> (u32, u32) { + self.0.into_raw_parts() + } + + /// Reconstructs an handle from its (index, generation) components. + pub fn from_raw_parts(id: u32, generation: u32) -> Self { + Self(crate::data::arena::Index::from_raw_parts(id, generation)) + } + + /// An always-invalid soft-body handle. + pub fn invalid() -> Self { + Self(crate::data::arena::Index::from_raw_parts( + crate::INVALID_U32, + crate::INVALID_U32, + )) + } + + /// Whether this is the always-invalid handle returned by [`Self::invalid`]. + pub fn is_invalid(self) -> bool { + self == Self::invalid() + } +} diff --git a/src/dynamics/soft_body/soft_body_material.rs b/src/dynamics/soft_body/soft_body_material.rs new file mode 100644 index 000000000..9432d3f9f --- /dev/null +++ b/src/dynamics/soft_body/soft_body_material.rs @@ -0,0 +1,126 @@ +//! The `SoftBodyMaterial`: per-family stiffness, damping, plasticity and tearing parameters. +use crate::dynamics::soft_body::SoftEdgePlasticFlow; +use crate::dynamics::SpringCoefficients; +use crate::math::Real; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// The stiffness and damping of every soft-body constraint family: a `natural_frequency` (Hz) and +/// `damping_ratio` per family, normalized by the constraint's effective mass (independent of mass, +/// substeps and timestep); a frequency far above the substep rate saturates instead of exploding. +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftBodyMaterial { + /// Softness of the structural edges. + pub edge_softness: SpringCoefficients, + /// Softness of the bending edges and dihedral constraints. + pub bend_softness: SpringCoefficients, + /// Softness of the per-cell volume rows ([`crate::dynamics::SoftBodyCellModel::Volume`]) and of the global + /// volume-preservation row. + pub volume_softness: SpringCoefficients, + /// Softness of the shape-matching rows. + pub shape_matching_softness: SpringCoefficients, + /// Young's modulus of the elastic cells ([`crate::dynamics::SoftBodyCellModel::Corotational`] and + /// [`crate::dynamics::SoftBodyCellModel::NeoHookean`]), in force per unit area (3D) or per unit length (2D). + /// + /// Unlike the other families, the elastic cells are parameterized physically: their per-cell + /// natural frequency is derived from the modulus, the cell's rest volume and the particle + /// masses, so a finer mesh of the same material behaves like the coarser one (up to the + /// discretization) instead of getting stiffer. + pub young_modulus: Real, + /// Poisson's ratio of the elastic cells, in `[0, 0.5)`. + pub poisson_ratio: Real, + /// Damping ratio of the elastic cells. + pub elastic_damping_ratio: Real, + /// Plastic yield of the elastic cells (corotational and Neo-Hookean models): the strain + /// magnitude beyond which a cell's rest shape starts flowing toward its current shape + /// (default: `0.0`, meaning no plasticity). Strain here is the Frobenius norm of the + pub plastic_yield: Real, + /// Rate (per second) at which the strain in excess of the yield is absorbed into the rest + /// shape (default: `1.0`); the flow is deviatoric (volume preserving). + pub plastic_creep: Real, + /// Largest accumulated plastic deformation of a cell (default: `1.0`): the Frobenius norm of + /// `P - I`, `P` the plastic stretch of its rest shape (a cell flattened to 40% of its rest + /// thickness is at about `1.0`). Flow past it is discarded, which keeps a repeatedly crushed + /// cell from flowing toward a sliver (and from there to inversions and instability). + pub plastic_max: Real, + /// Rate (per second) at which the particles' velocities are pulled toward the body's + /// best-fit rigid motion, damping every deformation mode without slowing the body down as a + /// whole (default: `0.0`, off). + /// + /// The material rows only damp their own strain rates: the residual bending of a stiff + /// slender body (a standing letter, a cantilever) is left by the solver's finite convergence + /// and is not damped by them, so such bodies sway for a long time. A rate of a few units per + /// second settles them within a second or two; soft jelly-like bodies keep it at zero (it + /// would damp their wobble too). + pub deformation_damping: Real, + /// Strain beyond which the elements break (default: `None`, unbreakable): a structural or + /// cell whose largest tensile principal strain exceeds it, is torn at the end of the step + /// (see [`crate::dynamics::SoftBody::tear_edge`] for what a tear removes). Cells of the volume model do not + /// tear on their own, their edges do. The cell strain is the one seen by the solver, capped + /// at `1.0`, so a cell threshold of one or more never fires. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub tear_strain: Option, + /// [`crate::dynamics::SoftBody::tear_edge`]). Unlike the strain, the force reports the load of an edge that + /// projectile being caught like in a net while its rim tears one edge at a time; the + /// default lets a puncture open at once. + #[cfg_attr( + feature = "serde-serialize", + serde(default = "default_max_tears_per_step") + )] + pub max_tears_per_step: u32, +} + +#[cfg(feature = "serde-serialize")] +fn default_max_tears_per_step() -> u32 { + u32::MAX +} + +#[cfg(feature = "serde-serialize")] +pub(super) fn default_true() -> bool { + true +} + +impl Default for SoftBodyMaterial { + fn default() -> Self { + Self { + edge_softness: SpringCoefficients::new(30.0, 1.0), + bend_softness: SpringCoefficients::new(10.0, 1.0), + volume_softness: SpringCoefficients::new(30.0, 1.0), + shape_matching_softness: SpringCoefficients::new(10.0, 1.0), + young_modulus: 1.0e4, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + plastic_yield: 0.0, + plastic_creep: 1.0, + plastic_max: 1.0, + deformation_damping: 0.0, + tear_strain: None, + max_tears_per_step: u32::MAX, + } + } +} + +impl SoftBodyMaterial { + /// A material with the same softness for every spring family (edges, bending, volume, shape + /// matching); the elastic-cell parameters keep their defaults. + pub fn uniform(softness: SpringCoefficients) -> Self { + Self { + edge_softness: softness, + bend_softness: softness, + volume_softness: softness, + shape_matching_softness: softness, + ..Default::default() + } + } + + /// The Lamé parameters `(μ, λ)` of the elastic cells. + pub fn lame_parameters(&self) -> (Real, Real) { + let nu = self.poisson_ratio.clamp(0.0, 0.499); + let e = self.young_modulus.max(0.0); + let mu = e / (2.0 * (1.0 + nu)); + let lambda = e * nu / ((1.0 + nu) * (1.0 - 2.0 * nu)); + (mu, lambda) + } +} diff --git a/src/dynamics/soft_body/soft_body_meshes.rs b/src/dynamics/soft_body/soft_body_meshes.rs new file mode 100644 index 000000000..58bf7da5e --- /dev/null +++ b/src/dynamics/soft_body/soft_body_meshes.rs @@ -0,0 +1,50 @@ +//! The collision meshes of a soft body: lookup, binding to clusters, insertion and removal, and update after a step or a tear. +use crate::alloc_prelude::*; +use super::{SoftBody, SoftBodyCell, SoftCollisionMesh, SoftMeshId}; +use crate::math::{DIM, Vector}; + +impl SoftBody { + /// The meshes this soft body meets the world through, over all its clusters (and the ones + /// it is only drawn as: a mesh whose collision is disabled has no collider). + pub fn meshes(&self) -> impl Iterator { + self.clusters + .iter() + .filter(|cluster| cluster.is_live()) + .flat_map(|cluster| cluster.meshes()) + } + + /// The mesh with the given id, if it is live. + pub fn mesh(&self, id: SoftMeshId) -> Option<&SoftCollisionMesh> { + self.clusters + .get(id.cluster as usize) + .filter(|cluster| cluster.is_live()) + .and_then(|cluster| cluster.mesh(id.mesh)) + } + + /// The mesh held by `collider`, if this body owns it. + pub fn mesh_of(&self, collider: crate::geometry::ColliderHandle) -> Option<&SoftCollisionMesh> { + self.meshes().find(|mesh| mesh.collider() == collider) + } + + /// The first mesh this soft body collides through, if any. + pub fn collision_mesh(&self) -> Option<&SoftCollisionMesh> { + self.meshes().find(|mesh| mesh.collision_enabled()) + } + + pub(crate) fn for_each_mesh_mut(&mut self, mut f: impl FnMut(&SoftBody, &mut SoftCollisionMesh)) { + let mut clusters = core::mem::take(&mut self.clusters); + for cluster in &mut clusters { + for mesh in cluster.meshes.iter_mut().flatten() { + f(self, mesh); + } + } + /// Updates the meshes' cached vertices from the current particle positions. + pub(crate) fn update_meshes(&mut self) { + self.for_each_mesh_mut(|body, mesh| mesh.update(&body.cells, &body.particles)); + } + + /// Rebuilds the meshes' derived tables after a topology change (a tear). + pub(crate) fn rebuild_mesh_tables(&mut self) { + self.for_each_mesh_mut(|body, mesh| mesh.rebuild_tables(body)); + } +} diff --git a/src/dynamics/soft_body/soft_body_motion.rs b/src/dynamics/soft_body/soft_body_motion.rs new file mode 100644 index 000000000..110ed61d1 --- /dev/null +++ b/src/dynamics/soft_body/soft_body_motion.rs @@ -0,0 +1,140 @@ +//! Moving a soft body's particles from outside: pinning, attachments to rigid bodies, user forces and impulses. +use super::SoftBody; +use crate::dynamics::{RigidBodyHandle, RigidBodySet}; +use crate::math::{Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// A particle of a soft body attached to a rigid body (see [`SoftBody::attach_particle`]): a +/// point-to-point constraint between the particle and a point of the body. +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftParticleAttachment { + /// The attached particle. + pub particle: u32, + /// The rigid body it is attached to. + pub body: RigidBodyHandle, + /// The attachment point in the rigid body's local frame. + pub local_anchor: Vector, + /// Accumulated impulse of the last step (warm-start state), one per world axis. + pub(crate) impulse: Vector, +} + +impl SoftParticleAttachment { + /// The impulse this attachment applied during the last substep (world axes, positive when + /// pulling the particle back toward the anchor). + pub fn impulse(&self) -> Vector { + self.impulse + } +} + +impl SoftBody { + /// Pins (`pinned = true`) or releases (`pinned = false`) the `i`-th particle: a pinned one + /// is kinematic, holding its position or following [`Self::set_particle_kinematic_target`] or + /// [`Self::set_particle_velocity`]; release restores its nominal mass and keeps its velocity. + pub fn set_particle_pinned(&mut self, i: usize, pinned: bool) { + let particle = &mut self.particles[i]; + if pinned { + // Held where it is until given a target or a velocity. + particle.inv_mass = 0.0; + particle.velocity = Vector::ZERO; + particle.next_position = Some(particle.position); + } else { + particle.inv_mass = crate::utils::inv(particle.mass); + particle.next_position = None; + } + self.modified = true; + } + + /// Attaches the `i`-th particle to a rigid body by a two-way point-to-point constraint (unlike + /// pinning), undone by [`Self::detach_particle`]; the anchor is its current position in + /// `body`'s local frame, and a twice-attached particle keeps both attachments. + pub fn attach_particle(&mut self, i: usize, body: RigidBodyHandle, bodies: &RigidBodySet) { + let particle = &self.particles[i]; + let local_anchor = bodies + .get(body) + .map(|rb| rb.position().inverse_transform_point(particle.position)) + .unwrap_or(particle.position); + self.attachments.push(SoftParticleAttachment { + particle: i as u32, + body, + local_anchor, + impulse: Vector::ZERO, + }); + self.attachments_modified = true; + self.modified = true; + } + + /// Detaches the `i`-th particle from every rigid body it was attached to with + /// [`Self::attach_particle`]. Returns whether it was attached at all. + pub fn detach_particle(&mut self, i: usize) -> bool { + let before = self.attachments.len(); + self.attachments.retain(|a| a.particle != i as u32); + let detached = self.attachments.len() != before; + if detached { + self.attachments_modified = true; + self.modified = true; + } + detached + } + + /// The particles of this soft body attached to rigid bodies. + pub fn particle_attachments(&self) -> &[SoftParticleAttachment] { + &self.attachments + } + + /// Adds `force` to the user force of every particle (persistent until + /// [`Self::reset_forces`], like a rigid body's forces). Pinned particles ignore it. + pub fn add_force(&mut self, force: Vector, wake_up: bool) { + for p in &mut self.particles { + p.force += force; + } + if wake_up { + self.wake_up(); + } + } + + /// Adds `force` to the user force of the `i`-th particle (persistent until + /// [`Self::reset_forces`]). + pub fn add_particle_force(&mut self, i: usize, force: Vector, wake_up: bool) { + self.particles[i].force += force; + if wake_up { + self.wake_up(); + } + } + + /// Clears the user forces of every particle. + pub fn reset_forces(&mut self, wake_up: bool) { + for p in &mut self.particles { + p.force = Vector::ZERO; + } + if wake_up { + self.wake_up(); + } + } + + /// Applies a linear velocity change (`impulse` per unit of mass) to every free particle of + /// this soft body: the whole body is kicked at the same velocity. + pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { + for p in &mut self.particles { + if p.inv_mass > 0.0 { + p.velocity += impulse; + } + } + if wake_up { + self.wake_up(); + } + } + + /// Applies an impulse to the `i`-th particle (its velocity changes by `impulse / mass`; + /// pinned particles ignore it). + pub fn apply_particle_impulse(&mut self, i: usize, impulse: Vector, wake_up: bool) { + let p = &mut self.particles[i]; + p.velocity += impulse * p.inv_mass; + if wake_up { + self.wake_up(); + } + } + +} diff --git a/src/dynamics/soft_body/soft_body_set/mod.rs b/src/dynamics/soft_body/soft_body_set/mod.rs new file mode 100644 index 000000000..489671d3e --- /dev/null +++ b/src/dynamics/soft_body/soft_body_set/mod.rs @@ -0,0 +1,12 @@ +//! The `SoftBodySet`: storage of the soft bodies, their insertion and removal, and their clusters. +mod soft_body_set; +mod soft_body_set_clusters; +mod soft_body_set_insert_remove; +mod soft_body_set_proxies; +mod soft_body_set_step_sync; + +pub use self::soft_body_set::SoftBodySet; +pub(crate) use self::soft_body_set::SoftBodyIslandEvent; +pub(super) use super::{SoftBodyParticleSettings, SoftCollisionMesh}; +#[cfg(feature = "dim3")] +pub(super) use super::soft_body_builder; diff --git a/src/dynamics/soft_body/soft_body_set/soft_body_set.rs b/src/dynamics/soft_body/soft_body_set/soft_body_set.rs new file mode 100644 index 000000000..11c95167a --- /dev/null +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set.rs @@ -0,0 +1,107 @@ +//! The `SoftBodySet` struct, its island event, its accessors and its `Index` impls. +use crate::alloc_prelude::*; +use crate::data::arena::Arena; +use crate::dynamics::RigidBodyHandle; +#[allow(unused_imports)] +use crate::dynamics::RigidBodySet; +use crate::dynamics::soft_body::{SoftBody, SoftBodyHandle}; +#[allow(unused_imports)] +use crate::geometry::ColliderSet; +use parry::utils::hashmap::HashMap; + +/// Deferred island-connectivity event emitted by [`SoftBodySet`] on insertion, removal and +/// attachment changes, drained at the start of the next step. +#[derive(Copy, Clone, Debug)] +pub(crate) struct SoftBodyIslandEvent { + /// The soft body whose attachment links must be updated (relinked if it still exists, + /// unlinked otherwise). + pub handle: SoftBodyHandle, +} + +/// A set of soft bodies that can be handled by a physics pipeline. Inserting a soft body creates +/// its hidden root rigid body in the [`RigidBodySet`] (see [`SoftBody`]) and its colliders in the +/// [`ColliderSet`]; removing it removes them, so never remove those yourself. +#[derive(Clone, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftBodySet { + pub(crate) bodies: Arena, + /// Island-connectivity events (soft body inserted/removed, attachments changed), drained at + /// the start of the next timestep. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) island_events: Vec, + /// The soft bodies attached to each rigid body, with the attachment's index (rebuilt when + /// attachments change; the island manager's adjacency). + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) attached_to: HashMap>, + /// Whether `attached_to` must be rebuilt (a body with attachments was removed, or the set + /// was deserialized). + #[cfg_attr(feature = "serde-serialize", serde(skip, default = "default_true"))] + pub(super) attached_to_stale: bool, +} + +#[cfg(feature = "serde-serialize")] +fn default_true() -> bool { + true +} + +impl SoftBodySet { + /// Creates an empty set. + pub fn new() -> Self { + Self::default() + } + + /// The number of soft bodies in this set. + pub fn len(&self) -> usize { + self.bodies.len() + } + + /// Whether this set is empty. + pub fn is_empty(&self) -> bool { + self.bodies.is_empty() + } + + /// Whether the given handle identifies a soft body of this set. + pub fn contains(&self, handle: SoftBodyHandle) -> bool { + self.bodies.contains(handle.0) + } + + /// The soft body with the given handle. + pub fn get(&self, handle: SoftBodyHandle) -> Option<&SoftBody> { + self.bodies.get(handle.0) + } + + /// The soft body with the given handle. + pub fn get_mut(&mut self, handle: SoftBodyHandle) -> Option<&mut SoftBody> { + self.bodies.get_mut(handle.0) + } + + /// Iterates over the soft bodies of this set. + pub fn iter(&self) -> impl ExactSizeIterator { + self.bodies.iter().map(|(h, b)| (SoftBodyHandle(h), b)) + } + + /// Iterates mutably over the soft bodies of this set. + pub fn iter_mut(&mut self) -> impl ExactSizeIterator { + self.bodies.iter_mut().map(|(h, b)| (SoftBodyHandle(h), b)) + } + + /// The soft bodies with a particle attached to the given rigid body, with the attachment's + /// index in their attachment list (as of the start of the current step). + pub(crate) fn attached_to(&self, body: RigidBodyHandle) -> &[(SoftBodyHandle, u32)] { + self.attached_to.get(&body).map_or(&[], |v| v.as_slice()) + } +} + +impl core::ops::Index for SoftBodySet { + type Output = SoftBody; + + fn index(&self, index: SoftBodyHandle) -> &SoftBody { + &self.bodies[index.0] + } +} + +impl core::ops::IndexMut for SoftBodySet { + fn index_mut(&mut self, index: SoftBodyHandle) -> &mut SoftBody { + &mut self.bodies[index.0] + } +} 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 new file mode 100644 index 000000000..339f60a3d --- /dev/null +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_insert_remove.rs @@ -0,0 +1,85 @@ +//! Insertion and removal of the soft bodies of a `SoftBodySet`. +use crate::alloc_prelude::*; +use crate::dynamics::{ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyHandle, RigidBodySet, SoftBodyBuilder}; +use crate::geometry::ColliderSet; +use crate::dynamics::soft_body::{SoftBody, SoftBodyCluster, SoftBodyHandle, SoftMeshId}; +use crate::math::{Pose, Rotation}; +use super::soft_body_set_proxies::{spawn_mesh_collider, spawn_proxy}; +use super::{SoftBodyIslandEvent, SoftBodySet}; + +impl SoftBodySet { + /// Inserts a soft body, creating its root rigid body and its colliders from the collider + /// template (the deformable surface collider, or one ball per surface particle for a body + /// colliding through its particles); its element topology must index valid particles. + pub fn insert( + &mut self, + soft_body_builder: SoftBodyBuilder, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) -> SoftBodyHandle { + let mut soft_body = soft_body_builder.build(); + soft_body.sleeping = false; + soft_body.enabled = true; + soft_body.positions_modified = false; + // Attachments made before the insertion get their island links with the insertion event. + soft_body.attachments_modified = !soft_body.attachments.is_empty(); + // Reserve the handle first: the root body stores it. + let handle = SoftBodyHandle(self.bodies.insert_with(|_| SoftBody { + particles: Vec::new(), + root_body: RigidBodyHandle::invalid(), + attachments: Vec::new(), + sleeping: false, + enabled: true, + positions_modified: false, + attachments_modified: false, + edges: Vec::new(), + #[cfg(feature = "dim3")] + dihedrals: Vec::new(), + cells: Vec::new(), + boundary: Vec::new(), + boundary_closed: false, + boundary_element_cells: Vec::new(), + material: soft_body.material, + cell_model: soft_body.cell_model, + volume_preservation: false, + volume_factor: 1.0, + rest_com: soft_body.rest_com, + particle_radius: soft_body.particle_radius, + particle_settings: soft_body.particle_settings, + num_colors: 0, + has_overflow_color: false, + modified: false, + plastic_flowing: false, + sleep_speed: 0.0, + tearing_pending: false, + topology_version: 0, + contact_approach_speeds: [None; 2], + contact_load: 0.0, + load_extra_substeps: 0, + user_data: 0, + })); + + // The deformable meshes' colliders attach to the root body (so they link islands and wake + // the soft body), with vertices in the whole-body cluster's frame (the proxy's pose holds + // the rigid motion). They collide with everything, fixed and kinematic colliders included. + if let Some(template) = soft_body_builder.collider_template.as_ref() { + *self.bodies.get_mut(handle.0).unwrap() = soft_body; + self.island_events.push(SoftBodyIslandEvent { handle }); + handle + } + + /// Removes a soft body, its cluster proxies (root rigid body included) and its colliders. + pub fn remove( + &mut self, + handle: SoftBodyHandle, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + ) -> Option { + let soft_body = self.bodies.remove(handle.0)?; + islands.persistent.unlink_soft_body_attachments(handle); + self.attached_to_stale |= !soft_body.attachments.is_empty(); + } +} 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 new file mode 100644 index 000000000..befc912bb --- /dev/null +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_proxies.rs @@ -0,0 +1,186 @@ +//! Spawning the cluster proxies and mesh colliders of a soft body, and the per-body end-of-step particle sync. +use crate::alloc_prelude::*; +use crate::dynamics::{IntegrationParameters, RigidBodyBuilder, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderBuilder, ColliderHandle, ColliderSet, SharedShape}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; +use crate::dynamics::soft_body::{SoftBody, SoftBodyHandle, SoftMeshRef}; +use crate::math::{DIM, Pose, Real, Vector}; + +/// The deformable surface shape (a 2D polyline or 3D triangle mesh over `vertices`), flagged +/// deformable so the narrow phase never trusts contact points cached across a vertex update. +/// A closed 2D polyline is one-sided (pushing outward); an open one (a rope) is two-sided. +pub(crate) fn surface_shape( + surface: &[[u32; DIM]], +) -> Option { + if surface.is_empty() || vertices.is_empty() { + return None; + } + #[cfg(feature = "dim2")] + { + use parry::shape::{Polyline, PolylineFlags}; + Some(SharedShape::new(Polyline::with_flags( + vertices, + Some(surface.to_vec()), + flags, + ))) + } + #[cfg(feature = "dim3")] + { + use parry::shape::{Polyline, PolylineFlags, TriMeshFlags}; + if super::soft_body_builder::element_vertices(&surface[0]).len() < DIM { + let segments: Vec<[u32; 2]> = surface.iter().map(|e| [e[0], e[1]]).collect(); + return Some(SharedShape::new(Polyline::with_flags( + Some(segments), + SharedShape::trimesh_with_flags(vertices, surface.to_vec(), TriMeshFlags::DEFORMABLE).ok() + } +} + +/// Rebuilds a surface shape whose topology changed (a tear), keeping the replaced shape's flags +/// except those that would edit the vertex or index buffers; a 2D polyline's orientation +/// follows `closed` (a torn open loop is two-sided). +pub(super) fn rebuilt_surface_shape( + prev: &dyn parry::shape::Shape, + vertices: Vec, + surface: &[[u32; DIM]], +) -> Option { + let mut shape = surface_shape(vertices, surface, closed)?; + let rebuilt = shape.make_mut(); + if let (Some(prev), Some(rebuilt)) = (prev.as_polyline(), rebuilt.as_polyline_mut()) { + #[allow(unused_mut)] + let mut flags = prev.flags(); + #[cfg(feature = "dim2")] + flags.set(parry::shape::PolylineFlags::ORIENTED, closed); + rebuilt.set_flags(flags); + } + #[cfg(feature = "dim3")] + if let (Some(prev), Some(rebuilt)) = (prev.as_trimesh(), rebuilt.as_trimesh_mut()) { + use parry::shape::TriMeshFlags; + let flags = prev.flags() + & !(TriMeshFlags::MERGE_DUPLICATE_VERTICES + | TriMeshFlags::DELETE_DEGENERATE_TRIANGLES + | TriMeshFlags::DELETE_DUPLICATE_TRIANGLES + | TriMeshFlags::DELETE_BAD_TOPOLOGY_TRIANGLES); + // A torn mesh may no longer satisfy a topology flag: it falls back to the bare + // deformable shape. + if rebuilt.set_flags(flags).is_err() { + let _ = rebuilt.set_flags(TriMeshFlags::DEFORMABLE); + } + } + Some(shape) +} + +) -> RigidBodyHandle { + let rb = RigidBodyBuilder::dynamic() + .gravity_scale(0.0) + .additional_solver_iterations(settings.additional_solver_iterations) + .can_sleep(settings.can_sleep) + .dominance_group(settings.dominance_group) + .build(); + let rb_handle = bodies.insert(rb); + rb_handle +} + +/// 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). + pub(super) margin: Option, + /// The substep request written on the body's root body. + pub(super) additional_solver_iterations: usize, + /// Whether every particle is finite (`false`: the body is quarantined). + pub(super) finite: bool, +} + +/// 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). +pub(super) fn sync_soft_body( + sb: &mut SoftBody, + inactive: bool, + params: &IntegrationParameters, +) -> SyncOutcome { + let dt = params.dt; + // Inactive: asleep, or disabled (then not asleep, just out of the simulation). + let sleeping = inactive && sb.enabled; + if sb.is_finite() { + sb.update_orientation(); + } + if sleeping && !sb.sleeping { + // Like a rigid body falling asleep: at rest means at rest. + for p in &mut sb.particles { + p.velocity = Vector::ZERO; + } + } + sb.sleeping = sleeping; + // Creep keeps the body awake: the sleep rule sees an infinite speed until the flow stops. + let plastic_flowing = core::mem::take(&mut sb.plastic_flowing); + if plastic_flowing { + sb.sleep_speed = Real::MAX; + } + let mut outcome = SyncOutcome { + margin: None, + additional_solver_iterations: 0, + finite: true, + }; + + for cluster in &mut sb.clusters { + cluster.prev_shape_matching_target = cluster.shape_matching_target; + } + + if inactive { + return outcome; + } + if !sb.is_finite() { + outcome.finite = false; + outcome.margin = Some(0.0); + return outcome; + } + // The skin only reads this body's particles, so it rides along with the per-body pass. + sb.update_meshes(); + let mut max_speed: Real = 0.0; + for p in &sb.particles { + max_speed = max_speed.max(p.velocity.length()); + } + // The soft body's own sleep rule runs on its proxies at the start of the next step + // (`RigidBodyActivation::update_energy`), on the fastest particle. + if !plastic_flowing { + sb.sleep_speed = max_speed; + } + outcome.margin = Some(max_speed * dt); + + // Impact-adaptive substeps: while something moves faster than one particle radius per + // substep at this body's contacts, its island component runs enough substeps to bound that + // travel. Only bodies with contact constraints this step or the last count. + let mut speed: Real = 0.0; + if sb.contact_approach_speeds.iter().any(|s| s.is_some()) { + speed = max_speed; + for s in sb.contact_approach_speeds.iter().flatten() { + speed = speed.max(*s); + } + } + let radius = sb.particle_radius.max(1.0e-6); + let needed = (speed * dt / radius).ceil().max(0.0) as usize; + let impact_extra = needed.saturating_sub(params.num_solver_iterations); + // Load-adaptive substeps: +1 past 20 g of contact load per unit mass, +2 past 60 g (g taken + // 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); + let load_extra = match sb.load_extra_substeps { + 0 if load > 20.0 => 1, + 1 if load > 60.0 => 2, + 1 if load < 10.0 => 0, + 2 if load < 30.0 => 1, + current => current, + }; + sb.load_extra_substeps = load_extra; + let extra = impact_extra + .max(load_extra as usize) + .min(params.soft_bodies.max_extra_substeps); + outcome.additional_solver_iterations = + sb.particle_settings.additional_solver_iterations.max(extra); + outcome +} 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 new file mode 100644 index 000000000..aba0e24f8 --- /dev/null +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_step_sync.rs @@ -0,0 +1,10 @@ + /// refreshes the attachment index and island links of the bodies whose attachments changed. + // so a one-refresh-stale proxy pose is fine. + /// Refreshes every soft body's derived state at the end of a step: the surface orientation, + // Every body reads its own particles and writes only itself: updated in parallel, then + /// Tears a soft body right away: removes the given edges and cells and everything spanning + /// them, splits the particles the tear passes through (the velocity of the original kept, its + + if let Some(rb) = bodies.get_mut(sb.root_body) { + rb.wake_up(true); +} diff --git a/src/dynamics/soft_body/soft_body_settings.rs b/src/dynamics/soft_body/soft_body_settings.rs new file mode 100644 index 000000000..4b9434cdb --- /dev/null +++ b/src/dynamics/soft_body/soft_body_settings.rs @@ -0,0 +1,31 @@ +//! Simulation settings shared by every soft body of a world, grouped on +//! [`IntegrationParameters::soft_bodies`](crate::dynamics::IntegrationParameters::soft_bodies). + +use super::SoftRecoverySettings; +use crate::math::Real; + +/// Simulation settings shared by every soft body of a world. +/// +/// Lives on [`IntegrationParameters::soft_bodies`], so it can be changed between steps. +/// +/// [`IntegrationParameters::soft_bodies`]: crate::dynamics::IntegrationParameters::soft_bodies +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "serde-serialize", serde(default))] +pub struct SoftBodiesSettings { + /// Runtime toggles and tuning for the soft-body tangle detection and recovery stack + /// (see [`SoftRecoverySettings`]). Every mechanism can be switched off individually. + pub recovery: SoftRecoverySettings, + /// Strain beyond which a soft-body constraint (an elastic cell's strain component, a distance + /// constraint's relative error) is re-solved after the contacts inside every substep, so a + /// light body buried under heavier ones is not torn (default: `0.75`; `Real::MAX` to disable). + pub resweep_strain: Real, + /// Maximum number of extra substeps a soft body requests for its island component while it + /// is hit fast (default: `4`; `0` disables the impact-adaptive substeps): enough substeps to + /// bound the per-substep travel of its particles and approaching rigid bodies by one radius. + pub max_extra_substeps: usize, + /// Factor applied to the [`IntegrationParameters`](crate::dynamics::IntegrationParameters) + /// contact softness natural frequencies for the soft-body contacts (default: `4.0`); they run + /// stiffer because a contact's effective mass is a few particles' worth, not the whole body's. + pub contact_stiffening: Real, +} diff --git a/src/dynamics/soft_body/soft_body_shape_matching.rs b/src/dynamics/soft_body/soft_body_shape_matching.rs new file mode 100644 index 000000000..1de69f6d9 --- /dev/null +++ b/src/dynamics/soft_body/soft_body_shape_matching.rs @@ -0,0 +1,63 @@ +//! Shape matching helpers: the rotation of the best-fit rigid transform of a particle cloud onto +//! its rest shape (polar decomposition of the mass-weighted covariance matrix). + +#[cfg(feature = "dim3")] +use crate::math::Real; +use crate::math::{Matrix, Rotation}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// The rotation `R` of the polar decomposition `A = R S` of a covariance matrix, warm-started +/// from `warm`: closed form in 2D; in 3D, Müller et al.'s iterative rotation extraction, which +/// never leaves the rotation group and converges in a couple of iterations from a warm start. +pub(crate) fn extract_rotation(a: Matrix, warm: Rotation) -> Rotation { + #[cfg(feature = "dim2")] + { + // The rotation maximizing tr(Rᵀ A): θ = atan2(a10 - a01, a00 + a11). + let sin = a.x_axis.y - a.y_axis.x; + let cos = a.x_axis.x + a.y_axis.y; + if sin == 0.0 && cos == 0.0 { + warm + } else { + Rotation::new(sin.atan2(cos)) + } + } + #[cfg(feature = "dim3")] + { + use crate::math::Vector; + const MAX_ITERS: usize = 8; + let mut q = warm; + for _ in 0..MAX_ITERS { + let r = Matrix::from_quat(q); + let cols = [ + (r.x_axis, a.x_axis), + (r.y_axis, a.y_axis), + (r.z_axis, a.z_axis), + ]; + let mut omega = Vector::ZERO; + let mut denom: Real = 0.0; + for (rc, ac) in cols { + omega += rc.cross(ac); + denom += rc.dot(ac); + } + let omega = omega * (1.0 / (denom.abs() + 1.0e-9)); + let w = omega.length(); + // Converged once the correction is at the rounding noise of the cross products (a + // few ulps): a tolerance below that never triggers and runs every iteration. + if w < 16.0 * Real::EPSILON { + break; + } + // The exact update `exp(ω/2) q` for a large step; a small one takes the + // first-order quaternion `(1, ω/2)` (renormalized) instead of sin/cos: same fixed + // point, and the next iteration absorbs the O(w³) difference. + let step = if w > 0.1 { + Rotation::from_axis_angle(omega / w, w) + } else { + Rotation::from_xyzw(0.5 * omega.x, 0.5 * omega.y, 0.5 * omega.z, 1.0) + }; + q = (step * q).normalize(); + } + q + } +} diff --git a/src/dynamics/solver/contact_constraint/contact_constraint_element.rs b/src/dynamics/solver/contact_constraint/contact_constraint_element.rs index 1a4fa4658..011d81dcf 100644 --- a/src/dynamics/solver/contact_constraint/contact_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/contact_constraint_element.rs @@ -268,7 +268,7 @@ impl ContactConstraintNormalPart { /// `-seed` (`restitution * approach_velocity`, captured at prepare), rigidly and AFTER /// all substeps — inside the substep rhs the speculative slack truncates the bounce. /// - /// Gated per lane on the point having carried a normal impulse this step. + /// Gated per lane on the point having applied a normal impulse this step. #[inline] pub fn solve_restitution( &mut self, @@ -354,7 +354,7 @@ impl ContactConstraintNormalPart { // Degraded lanes (`block_flag` = 0: manifold lacks both points, or the pair matrix // wasn't invertible at build time): solve the two points one after the other, exactly - // as the feature-off build does, with `k12` carrying the first solve's velocity change. + // as the feature-off build does, with `k12` propagating the first solve's velocity change. // Use `degraded_dvel_a` rather than `dvel.x` so that this case matches the non-mlcp // solve exactly. // TODO: measure if using `degraded_dvel_a` instead of `dvel.x` has any performance diff --git a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs index 21f41afec..322d1b728 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs @@ -41,9 +41,9 @@ impl Default for CoulombContactPointInfos { pub(crate) struct ContactWithCoulombFrictionBuilder { infos: [CoulombContactPointInfos; MAX_MANIFOLD_POINTS], /// The contact normal in the first body's (com-centered) local frame, so - /// `refresh` can re-derive the world normal without touching the manifold. + /// `update` can re-derive the world normal without touching the manifold. local_n1: ::Vector, - /// The pair's restitution coefficient (needed by `refresh` to recompute the + /// The pair's restitution coefficient (needed by `update` to recompute the /// restitution rhs seed). restitution: SimdReal, } @@ -165,7 +165,7 @@ impl ContactWithCoulombFrictionBuilder { // Warm-start impulses and contact newness read straight off the manifold points // (not duplicated on the solver contacts): a zero `impulse` means the contact never - // carried a load — exactly what the emission-time is-new bit encoded. + // bore a load, exactly what the emission-time is-new bit encoded. let cids = solver_contact.contact_indices(); let pt_data = |ii: usize| &manifolds[ii].points[cids[ii] as usize].data; let warmstart_impulse = SimdReal::from(gather![|ii| pt_data(ii).warmstart_impulse]); @@ -195,7 +195,7 @@ impl ContactWithCoulombFrictionBuilder { // Reconstruct the world contact points and separation from the body-local anchors // and solver poses (a world-attached side gathers the identity pose, so its anchor - // passes through). This replaces the narrow-phase's per-frame refresh of recycled contacts. + // passes through). Recycled contacts are updated here, not per frame by the narrow phase. let p1 = poses1.transform_point(solver_contact.anchor1); let p2 = poses2.transform_point(solver_contact.anchor2); let dist = (p1 - p2).gdot(force_dir1); @@ -347,8 +347,8 @@ impl ContactWithCoulombFrictionBuilder { // Degenerate (redundant contacts) or partially-active lanes clear the block // flag: `solve_pair` then solves the two points sequentially instead. let block = is_invertible & pair_active; - // `k12` stays on every lane: the degraded path carries the first point's - // impulse change over to the second. + // `k12` stays on every lane: the degraded path propagates the first point's + // impulse change to the second. out_constraint.normal_part[k0].r_mat_elts = [k12, SimdReal::splat(1.0).select(block, SimdReal::zero())]; out_constraint.normal_part[k1].r_mat_elts = [SimdReal::zero(); 2]; @@ -454,10 +454,10 @@ impl ContactWithCoulombFrictionBuilder { constraint.cfm_factor = cfm_factor; } - /// Relax-pass refresh: recompute the unbiased rhs (speculative term included) + /// Relax-pass update: recompute the unbiased rhs (speculative term included) /// from the CURRENT solver poses and strip softness and penetration bias. See - /// `ContactWithTwistFrictionBuilder::refresh_rhs_wo_bias` for why stale separations are unusable. - pub fn refresh_rhs_wo_bias( + /// `ContactWithTwistFrictionBuilder::update_rhs_wo_bias` for why stale separations are unusable. + pub fn update_rhs_wo_bias( &self, params: &IntegrationParameters, solved_dt: Real, @@ -500,7 +500,7 @@ impl ContactWithCoulombFrictionBuilder { /// End-of-step restitution pass (box2d-style): after all substeps, drive each bouncy /// point's normal velocity to its prepare-time `restitution * approach_velocity`, gated - /// on the point having carried an impulse. See `ContactConstraintNormalPart::solve_restitution`. + /// on the point having applied an impulse. See `ContactConstraintNormalPart::solve_restitution`. pub fn apply_restitution( &self, constraint: &mut ContactWithCoulombFriction, diff --git a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs index 35196ecf4..baad26989 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs @@ -47,9 +47,9 @@ pub(crate) struct ContactWithTwistFrictionBuilder { local_friction_center2: N::Vector, tangent_vel: N::Vector, /// The contact normal in the first body's (com-centered) local frame, so - /// `refresh` can re-derive the world normal without touching the manifold. + /// `update` can re-derive the world normal without touching the manifold. local_n1: N::Vector, - /// The pair's restitution coefficient (needed by `refresh` to recompute the + /// The pair's restitution coefficient (needed by `update` to recompute the /// restitution rhs seed). restitution: N, } @@ -191,7 +191,7 @@ impl ContactWithTwistFrictionBuilder { // Warm-start impulses and contact newness read straight off the manifold points // (not duplicated on the solver contacts): a zero `impulse` means the contact never - // carried a load — exactly what the emission-time is-new bit encoded. + // bore a load, exactly what the emission-time is-new bit encoded. let cids = solver_contact.contact_indices(); let pt_data = |ii: usize| &manifolds[ii].points[cids[ii] as usize].data; let warmstart_impulse = SimdReal::from(gather![|ii| pt_data(ii).warmstart_impulse]); @@ -220,7 +220,7 @@ impl ContactWithTwistFrictionBuilder { // Reconstruct the world contact points and separation from the body-local anchors // and solver poses (a world-attached side gathers the identity pose, so its anchor - // passes through). This replaces the narrow-phase's per-frame refresh of recycled contacts. + // passes through). Recycled contacts are updated here, not per frame by the narrow phase. let p1 = poses1.transform_point(solver_contact.anchor1); let p2 = poses2.transform_point(solver_contact.anchor2); let dist = (p1 - p2).gdot(force_dir1); @@ -414,8 +414,8 @@ impl ContactWithTwistFrictionBuilder { // Degenerate or partially-active lanes store `[0, 0]`: // `solve_pair` degrades to the scalar soft solve of point k0. let block = is_invertible & pair_active; - // `k12` stays on every lane: the degraded path carries the first point's - // impulse change over to the second. + // `k12` stays on every lane: the degraded path propagates the first point's + // impulse change to the second. out_constraint.normal_part[k0].r_mat_elts = [k12, SimdReal::splat(1.0).select(block, SimdReal::zero())]; out_constraint.normal_part[k1].r_mat_elts = [SimdReal::zero(); 2]; @@ -521,12 +521,12 @@ impl ContactWithTwistFrictionBuilder { constraint.cfm_factor = cfm_factor; } - /// Relax-pass refresh: recompute the unbiased rhs (speculative term included) + /// Relax-pass update: recompute the unbiased rhs (speculative term included) /// from the CURRENT solver poses, stripping softness and penetration bias. Positions /// integrate between the biased and unbiased passes, so the separations `update` baked are /// stale by one substep; enforcing them makes a lifted edge read as still touching (its /// returning velocity cancelled), driving the rocking mode of tall stacks instead of damping it. - pub fn refresh_rhs_wo_bias( + pub fn update_rhs_wo_bias( &self, params: &IntegrationParameters, solved_dt: Real, @@ -564,7 +564,7 @@ impl ContactWithTwistFrictionBuilder { /// End-of-step restitution pass (box2d-style): after all substeps, drive each bouncy /// point's normal velocity to its prepare-time `restitution * approach_velocity`, gated - /// on the point having carried an impulse. See `ContactConstraintNormalPart::solve_restitution`. + /// on the point having applied an impulse. See `ContactConstraintNormalPart::solve_restitution`. pub fn apply_restitution( &self, constraint: &mut ContactWithTwistFriction, diff --git a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs index cb2e19335..e5990621b 100644 --- a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs @@ -509,7 +509,7 @@ impl GenericContactConstraintBuilder { /// End-of-step restitution pass (box2d-style): after all substeps, drive each bouncy /// point's normal velocity to its prepare-time `restitution * approach_velocity`, gated - /// on the point having carried an impulse. Implemented by re-running the normal solve + /// on the point having applied an impulse. Implemented by re-running the normal solve /// with `rhs = seed` on those points and a zeroed effective mass on the others. pub fn apply_restitution( &self, diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs index 0f6fe6a16..a7aae8d97 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -116,10 +116,9 @@ impl JointGenericExternalConstraintBuilder { return; } - // Each constraint row appends the jacobian and the weighted jacobian for - // both sides, i.e. `2 * multibodies_ndof` entries. Reserve exactly the - // rows this joint emits: an axis carrying both a motor and a limit - // produces two rows, so a joint can exceed `SPATIAL_DIM` of them. + // Each row appends `2 * multibodies_ndof` entries (jacobian and weighted jacobian, both + // sides); reserve exactly the rows this joint emits (motor plus limit on an axis gives two, + // so more than `SPATIAL_DIM`), counted after `strip_soft_frame_angular_axes` runs. let num_rows = joint_num_constraints(joint); let required_jacobian_len = *j_id + multibodies_ndof * 2 * num_rows; *j_id += multibodies_ndof * 2 * num_rows; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 0adb35f44..3ccdbd31d 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -108,7 +108,7 @@ impl JointConstraintBuilder { let out_rows = &mut out[self.constraint_id..]; - // When warm-starting, carry the impulses accumulated by the previous substep + // When warm-starting, keep the impulses accumulated by the previous substep // across the row rebuild (the row layout only depends on the static joint // configuration, so it is stable across the substeps of a step). const MAX_ROWS: usize = 4 * SPATIAL_DIM; @@ -200,7 +200,7 @@ pub struct JointConstraintBuilderSimd { /// Like `prev_dof_impulses`, for the limit rows. prev_limit_impulses: [SimdReal; SPATIAL_DIM], /// The bodies' effective inverse masses/angular inertias, cached by the substep-0 update. - /// Step-constant (solver-body mass properties refresh once per step), so later substeps + /// Step-constant (solver-body mass properties are updated once per step), so later substeps /// only gather the transform part of the solver poses — about half the transposition work. im1: ::Vector, ii1: ::AngInertia, @@ -442,7 +442,7 @@ impl JointConstraintBuilderSimd { #[cfg(feature = "dim3")] let ang_motor_params: Option> = None; - // See the scalar builder: carry impulses across the row rebuild when warm-starting. + // See the scalar builder: keep impulses across the row rebuild when warm-starting. // The SIMD builder emits at most one motor row, one row per locked axis and one per // (uncoupled) limited axis; the masks are disjoint. const MAX_WIDE_ROWS: usize = SPATIAL_DIM + 1; diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index 494b3f9ee..1c47839bb 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -126,7 +126,7 @@ impl JointConstraint { /// Applies the currently-accumulated impulse to the body velocities. Only used when /// `IntegrationParameters::warmstart_joints` is enabled: the constraint's `impulse` was - /// carried from the previous substep (or seeded from last step's writeback) by the update. + /// kept from the previous substep (or seeded from last step's writeback) by the update. pub fn warmstart_generic( &mut self, solver_vel1: &mut SolverVel, diff --git a/src/dynamics/solver/manifold_store.rs b/src/dynamics/solver/manifold_store.rs index 031f41aa1..ea179ff14 100644 --- a/src/dynamics/solver/manifold_store.rs +++ b/src/dynamics/solver/manifold_store.rs @@ -2,7 +2,7 @@ use crate::data::graph::Edge; use crate::dynamics::solver::solver_contact_graph::ContactRef; -use crate::geometry::{ContactManifold, ContactPair}; +use crate::geometry::{ContactManifold, ContactPair, PairContacts, RigidPairContacts}; /// The contact graph's edge-array pointer and length, type-erased (the pointer is held as a /// `usize` so the value stays `Send` and holdable across a later exclusive narrow-phase borrow) @@ -13,6 +13,37 @@ pub(crate) struct ManifoldStoreParts { num_edges: usize, } +/// The offset of the rigid contacts (the `Rigid` payload) inside [`PairContacts`]: its +/// representation is fixed, so a raw pointer can be projected to it without materializing a +/// reference to the whole pair (see the aliasing contract of [`ManifoldStore`]). +fn rigid_contacts_offset() -> usize { + use core::sync::atomic::{AtomicUsize, Ordering}; + static OFFSET: AtomicUsize = AtomicUsize::new(usize::MAX); + let cached = OFFSET.load(Ordering::Relaxed); + if cached != usize::MAX { + return cached; + } + let probe = ContactPair::default(); + let base = &raw const probe.contacts as usize; + let PairContacts::Rigid(rigid) = &probe.contacts else { + unreachable!("a fresh pair holds rigid contacts"); + }; + let offset = &raw const *rigid as usize - base; + OFFSET.store(offset, Ordering::Relaxed); + offset +} + +/// A raw projection to the rigid contacts of `pair`; the solver graph only ever holds rigid pairs. +/// # Safety +/// `pair` must point to a live [`ContactPair`] whose contacts are the `Rigid` variant. +#[inline] +unsafe fn rigid_contacts_ptr(pair: *mut ContactPair) -> *mut RigidPairContacts { + unsafe { + let contacts = &raw mut (*pair).contacts; + (contacts as *mut u8).add(rigid_contacts_offset()) as *mut RigidPairContacts + } +} + impl ManifoldStoreParts { /// Erases the contact graph's live edge array into a pointer + length. pub(crate) fn new(edges: *mut Edge, num_edges: usize) -> Self { @@ -73,9 +104,10 @@ impl<'a> ManifoldStore<'a> { // reference to the whole pair (distinct ordinals of one pair may be // resolved concurrently; see the aliasing contract). unsafe { - let clusters: *mut _ = &raw mut (*pair).solver_clusters; + let rigid = rigid_contacts_ptr(pair); + let clusters: *mut _ = &raw mut (*rigid).solver_clusters; let vec = if (*clusters).is_empty() { - &raw mut (*pair).manifolds + &raw mut (*rigid).manifolds } else { clusters }; @@ -117,9 +149,10 @@ impl<'a> ManifoldStore<'a> { } let pair = self.pair_ptr(r.edge); unsafe { - let clusters = &raw mut (*pair).solver_clusters; + let rigid = rigid_contacts_ptr(pair); + let clusters = &raw mut (*rigid).solver_clusters; let vec = if (*clusters).is_empty() { - &raw mut (*pair).manifolds + &raw mut (*rigid).manifolds } else { clusters }; diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs index e26a1af5a..dde9bfe8f 100644 --- a/src/dynamics/solver/mod.rs +++ b/src/dynamics/solver/mod.rs @@ -11,6 +11,7 @@ mod contact_constraint; mod interaction_groups; mod joint_constraint; pub(crate) mod manifold_store; +mod soft_constraint; mod solver_body; pub(crate) mod solver_contact_graph; mod staged_island_solver; diff --git a/src/dynamics/solver/soft_constraint/mod.rs b/src/dynamics/solver/soft_constraint/mod.rs new file mode 100644 index 000000000..dd61bbbcd --- /dev/null +++ b/src/dynamics/solver/soft_constraint/mod.rs @@ -0,0 +1,10 @@ +//! Soft-body constraints solved by the staged island solver alongside joints and contacts. + +pub(crate) use self::soft_constraints_set::SoftConstraintsSet; + +mod soft_attachment; +mod soft_constraints_set; +mod soft_contact; +mod soft_contact_assembly; +mod soft_contact_chunks; +mod soft_element_constraint_assembly; diff --git a/src/dynamics/solver/soft_constraint/soft_attachment.rs b/src/dynamics/solver/soft_constraint/soft_attachment.rs new file mode 100644 index 000000000..45ab299d3 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_attachment.rs @@ -0,0 +1,138 @@ +//! Point-to-point constraints attaching a soft-body particle to a rigid body +//! (`SoftBody::attach_particle`): `DIM` coupled bilateral rows keeping the particle on a point of +//! the body, solved like a locked joint (both passes, no bias in relax, softness, warm start). + +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::math::{AngVector, AngularInertia, Matrix, Real, Vector}; +use crate::utils::{AngularInertiaOps, ComponentMul, CrossProduct}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// One particle attachment, as solved by the staged solver. +#[derive(Copy, Clone, Debug)] +pub(crate) struct SoftAttachmentConstraint { + /// Index of the soft body in the solver's awake list, and of the attachment in it. + pub soft_body: u32, + pub attachment: u32, + /// The particle's solver slot and inverse mass. + pub particle: u32, + pub im_particle: Real, + /// The rigid body's solver slot (`u32::MAX`: fixed or not simulated, the anchor stays at + /// `anchor0`) and its attachment point in its CoM-local frame. + pub body: u32, + pub body_local_point: Vector, + pub anchor0: Vector, + /// Joint softness. + pub erp_inv_dt: Real, + pub cfm_coeff: Real, + // Solve state. + pub body_im: Vector, + pub body_ii: AngularInertia, + /// World lever arm of the anchor about the body's center of mass. + pub arm: Vector, + /// The rows' effective-mass matrix and its (softened) inverse. + pub lhs: Matrix, + pub inv_lhs: Matrix, + pub rhs: Vector, + pub impulse: Vector, +} + +/// The matrix `-[r]× ii [r]×` (3D) or `ii perp(r) perp(r)ᵀ` (2D): the velocity change of the +/// point at lever arm `r` per unit impulse applied there, through the body's rotation. +fn rotational_effective_mass(arm: Vector, ii: AngularInertia) -> Matrix { + #[cfg(feature = "dim2")] + { + let p = Vector::new(-arm.y, arm.x); + Matrix::from_cols(p * (p.x * ii), p * (p.y * ii)) + } + #[cfg(feature = "dim3")] + { + let rx = Matrix::from_cols( + Vector::new(0.0, arm.z, -arm.y), + Vector::new(-arm.z, 0.0, arm.x), + Vector::new(arm.y, -arm.x, 0.0), + ); + -(rx * ii.into_matrix() * rx) + } +} + +impl SoftAttachmentConstraint { + /// Current world position of the body's attachment point. + #[inline] + fn anchor(&self, bodies: &SolverBodies) -> Vector { + if self.body == u32::MAX { + self.anchor0 + } else { + bodies.get_pose(self.body).pose() * self.body_local_point + } + } + + /// Updates the position error, effective mass and bias from the current solver state. + /// `wo_bias`: relax pass (no position correction). + pub fn update(&mut self, bodies: &SolverBodies, wo_bias: bool) { + let particle = bodies.get_pose(self.particle).translation; + let anchor = self.anchor(bodies); + let mut lhs = Matrix::IDENTITY * self.im_particle; + if self.body != u32::MAX { + let pose = bodies.get_pose(self.body); + self.body_im = pose.im; + self.body_ii = pose.ii; + self.arm = anchor - pose.translation; + lhs += Matrix::from_diagonal(pose.im) + rotational_effective_mass(self.arm, pose.ii); + } + self.lhs = lhs; + let softened = lhs * (1.0 + self.cfm_coeff); + // No degree of freedom on either side (a pinned particle on a fixed body): inert constraint. + self.inv_lhs = if softened.determinant().abs() > Real::EPSILON * Real::EPSILON { + softened.inverse() + } else { + Matrix::ZERO + }; + // Uncapped, like a joint's locked axes: the attachment must win over the contact constraints + // solved after it (a cloth corner attached to a bar its surface also touches). + self.rhs = if wo_bias { + Vector::ZERO + } else { + (particle - anchor) * self.erp_inv_dt + }; + } + + /// Applies `impulse` (positive along the constraint: the particle is pulled back, the body pulled + /// toward it). + #[inline] + fn apply(&self, bodies: &mut SolverBodies, impulse: Vector) { + if (self.particle as usize) < bodies.len() { + bodies.vels[self.particle as usize].linear -= impulse * self.im_particle; + } + if self.body != u32::MAX && (self.body as usize) < bodies.len() { + let v = &mut bodies.vels[self.body as usize]; + v.linear += self.body_im.component_mul(&impulse); + let torque: AngVector = self.arm.gcross(impulse); + v.angular += self.body_ii.transform_vector(torque); + } + } + + #[inline] + pub fn warmstart(&mut self, bodies: &mut SolverBodies, coeff: Real) { + self.impulse *= coeff; + self.apply(bodies, self.impulse); + } + + #[inline] + pub fn solve(&mut self, bodies: &mut SolverBodies) { + let vp = bodies.get_vel(self.particle).linear; + let va = if self.body == u32::MAX { + Vector::ZERO + } else { + let v = bodies.get_vel(self.body); + v.linear + v.angular.gcross(self.arm) + }; + let dv = vp - va; + let total = self.impulse + + self.inv_lhs * (dv + self.rhs - (self.lhs * self.impulse) * self.cfm_coeff); + let delta = total - self.impulse; + self.impulse = total; + self.apply(bodies, delta); + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/mod.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/mod.rs new file mode 100644 index 000000000..93730b51f --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/mod.rs @@ -0,0 +1,21 @@ +//! The soft-body constraints of one step over the awake soft bodies (element, shape-matching, +//! volume-piece and contact constraints) with their per-pass prepare, solve and writeback; the +//! assembly lives in `soft_element_constraint_assembly` and `soft_contact_assembly`. + +mod soft_constraint_plasticity; +mod soft_constraint_prepare; +mod soft_constraint_rigid_coupling; +mod soft_constraint_solve; +mod soft_constraint_writeback; +mod soft_constraints; +mod soft_constraints_set; + +use super::{soft_attachment, soft_contact_assembly, soft_contact_chunks, soft_element_constraint_assembly}; + +#[cfg(feature = "fem")] +pub(crate) use self::soft_constraint_prepare::soft_fem_amplification_cap; +pub(crate) use self::soft_constraint_plasticity::*; +pub(crate) use self::soft_constraint_rigid_coupling::*; +pub(crate) use self::soft_constraints::*; +pub(crate) use self::soft_constraints_set::*; +use self::soft_constraint_writeback::*; diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs new file mode 100644 index 000000000..25cd45b99 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs @@ -0,0 +1,209 @@ +//! Per-step and per-pass preparation: solver bodies from the particles, deformation damping, shape-matching goals and volume-piece gradients. + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::dynamics::solver::solver_body::{SolverBodies, SolverPose, SolverVel}; +use crate::dynamics::SoftBody; +use crate::math::{AngVector, Matrix, Real, Vector}; +use crate::utils::{AngularInertiaOps, CrossProduct}; + +use super::*; + +impl SoftConstraintsSet { + /// Per-pass prepare of one awake soft body: the substep's first pass (`update`) applies the + /// deformation damping and refits what only moves with the poses; every pass refits the + /// shape-matching goal velocities. Writes only this body's entries and particle velocities. + pub fn prepare_awake_body(&mut self, ai: usize, bodies: &mut SolverBodies, update: bool) { + let Self { + awake, + slots, + shape_constraints, + volume_constraints, + volume_grads, + } = self; + let awake = &mut awake[ai]; + // SAFETY: read-only access to the soft body's particles and surface. + let sb = unsafe { &*awake.ptr }; + let slots = &slots[awake.slot_start..awake.slot_start + awake.num_particles]; + if update && awake.damping_factor > 0.0 { + damp_deformation(sb, slots, bodies, awake.damping_factor); + } + let bodies = &*bodies; + let pos = |i: usize| -> Vector { + let s = slots[i]; + if s == u32::MAX { + sb.particles[i].position + } else { + bodies.get_pose(s).translation + } + }; + + if let Some(vi) = awake.volume_constraint { + let vc = &mut volume_constraints[vi]; + let grads = &mut volume_grads[vc.grads.clone()]; + grads.fill(Vector::ZERO); + let position = |i: u32| pos(i as usize); + let volume = SoftBody::boundary_volume(&sb.boundary, position); + SoftBody::boundary_volume_gradients(&sb.boundary, position, grads); + let mut w = 0.0; + for (i, p) in sb.particles.iter().enumerate() { + w += p.inv_mass * grads[i].length_squared(); + } + vc.cfm_gain = w * vc.cfm_coeff; + vc.inv_lhs = crate::utils::inv(w + vc.cfm_gain); + // The position error, biased and capped at solve time. A piece turned inside out is + // accepted as mirrored: the target follows the current orientation instead of + // collapsing the body through zero volume to restore the rest winding. + let target = if piece.inverted { + -vc.target + } else { + vc.target + }; + vc.rhs = volume - target; + } + } + + /// Fills the solver bodies of the awake soft bodies' particles: a pinned one is kinematic + /// (zero inverse mass, velocity to its target), a free one gets scaled gravity and user force. + /// `group_dt(group)` is the group's substep length, `step_dt` the whole step's. + pub fn init_solver_bodies( + &self, + bodies: &mut SolverBodies, + increments: &mut [SolverVel], + gravity: Vector, + step_dt: Real, + group_dt: impl Fn(usize) -> Real, + ) { + let inv_step_dt = crate::utils::inv(step_dt); + for awake in &self.awake { + // SAFETY: read-only access to the soft body's particles. + let sb = unsafe { &*awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let dt = group_dt(awake.group as usize); + let settings = &sb.particle_settings; + for (p, &slot) in sb.particles.iter().zip(slots) { + let slot = slot as usize; + let (vel, incr) = if p.inv_mass == 0.0 { + let vel = match p.next_position { + Some(target) => (target - p.position) * inv_step_dt, + None => p.velocity, + }; + (vel, Vector::ZERO) + } else { + ( + p.velocity, + (gravity * settings.gravity_scale + p.force * p.inv_mass) * dt, + ) + }; + let pose = &mut bodies.poses[slot]; + *pose = SolverPose::default(); + pose.translation = p.position; + pose.im = Vector::splat(p.inv_mass); + let v = &mut bodies.vels[slot]; + *v = SolverVel::zero(); + v.linear = vel; + bodies.flags[slot] = 0; + let inc = &mut increments[slot]; + *inc = SolverVel::zero(); + inc.linear = incr; + } + } + } +} + +/// Pulls the particles' velocities toward the body's best-fit rigid motion, removing the fraction +/// `factor` of the non-rigid part (see `SoftBodyMaterial::deformation_damping`). Pinned particles +/// weigh far more than free ones in the fit, so a clamped body's whole swing is damped. +fn damp_deformation(sb: &SoftBody, slots: &[u32], bodies: &mut SolverBodies, factor: Real) { + let (com, vcom, omega) = rigid_fit_velocity(sb, slots, bodies); + for (i, p) in sb.particles.iter().enumerate() { + let s = slots[i]; + if s == u32::MAX || p.inv_mass == 0.0 || (s as usize) >= bodies.len() { + continue; + } + let r = bodies.get_pose(s).translation - com; + let v_rigid = vcom + omega.gcross(r); + let v = &mut bodies.vels[s as usize].linear; + *v += (v_rigid - *v) * factor; + } +} + +/// The best-fit rigid motion of a soft body's particles: mass-weighted center of mass, its +/// velocity and the angular velocity matching the angular momentum about it (pinned particles +/// weigh `1e4x` the total mass, so a pinned body's rigid motion is the pins'). +fn rigid_fit_velocity( + sb: &SoftBody, + slots: &[u32], + bodies: &SolverBodies, +) -> (Vector, Vector, AngVector) { + let total_mass: Real = sb.particles.iter().map(|p| p.mass).sum(); + let pin_weight = total_mass * 1.0e4; + let state = |i: usize| -> (Vector, Vector, Real) { + let s = slots[i]; + let p = &sb.particles[i]; + let weight = if p.inv_mass == 0.0 { + pin_weight + } else { + p.mass + }; + if s == u32::MAX { + (p.position, Vector::ZERO, weight) + } else { + ( + bodies.get_pose(s).translation, + bodies.get_vel(s).linear, + weight, + ) + } + }; + let mut mass = 0.0; + let mut com = Vector::ZERO; + let mut vcom = Vector::ZERO; + for i in 0..sb.particles.len() { + let (x, v, w) = state(i); + mass += w; + com += x * w; + vcom += v * w; + } + if mass <= 0.0 { + return (Vector::ZERO, Vector::ZERO, AngVector::default()); + } + com /= mass; + vcom /= mass; + // Angular momentum and inertia about the center of mass. + #[cfg(feature = "dim2")] + let omega = { + let mut l = 0.0; + let mut inertia = 0.0; + for i in 0..sb.particles.len() { + let (x, v, w) = state(i); + let r = x - com; + l += w * r.gcross(v - vcom); + inertia += w * r.length_squared(); + } + if inertia > 0.0 { l / inertia } else { 0.0 } + }; + #[cfg(feature = "dim3")] + let omega = { + let mut l = Vector::ZERO; + let mut inertia = Matrix::ZERO; + for i in 0..sb.particles.len() { + let (x, v, w) = state(i); + let r = x - com; + l += r.cross(v - vcom) * w; + inertia += (Matrix::IDENTITY * r.length_squared() - outer(r, r)) * w; + } + // Regularized: particles on a line (a rope) leave the rotation about that line + // undetermined, and its momentum is zero anyway. + let trace = inertia.x_axis.x + inertia.y_axis.y + inertia.z_axis.z; + if trace > 0.0 { + (inertia + Matrix::IDENTITY * (trace * 1.0e-6)).inverse() * l + } else { + Vector::ZERO + } + }; + (com, vcom, omega) +} + diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs new file mode 100644 index 000000000..f0ae32b41 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs @@ -0,0 +1,132 @@ +//! Coupling with the rigid bodies: cluster proxy assembly with its gather/scatter stages, and the particle attachment constraints. + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::dynamics::solver::solver_body::{SolverBodies, SolverVel}; +use crate::math::{DIM, Matrix, Real, Vector}; +use crate::utils::CrossProduct; + +use super::super::soft_element_constraint::MAX_CONSTRAINT_PARTICLES; +use super::*; + +impl SoftConstraintsSet { + /// Builds the particle attachment rows of the awake soft bodies (group-major, so it runs + /// after [`Self::assemble`] laid the groups out): the particle's slot against the rigid + /// body's (a fixed or non-simulated body anchors the particle to a world point). + /// `group_dt(group)` is the group's substep length (the rows' softness). + pub fn assemble_attachments( + &mut self, + island_id: usize, + bodies: &crate::dynamics::RigidBodySet, + group_dt: impl Fn(usize) -> Real, + ) { + self.attachments.clear(); + for gi in 0..self.groups.len() { + let start = self.attachments.len(); + let dt = group_dt(gi); + let softness = crate::dynamics::SpringCoefficients::::joint_defaults(); + let (erp_inv_dt, cfm_coeff) = (softness.erp_inv_dt(dt), softness.cfm_coeff(dt)); + for ai in self.groups[gi].awake.clone() { + let awake = &self.awake[ai]; + if awake.frozen { + continue; + } + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + for (k, attachment) in sb.attachments.iter().enumerate() { + let Some(&particle) = slots.get(attachment.particle as usize) else { + continue; + }; + let im_particle = sb.particles[attachment.particle as usize].inv_mass; + let Some(rb) = bodies.get(attachment.body) else { + continue; + }; + let com_pose = rb + .pos + .position + .prepend_translation(rb.mprops.local_mprops.local_com); + let anchor0 = rb.pos.position * attachment.local_anchor; + let simulated = rb.ids.active_island_id == island_id as u32 + && !rb.is_sleeping() + && rb.is_enabled() + && rb.is_dynamic_or_kinematic(); + let (body, body_local_point) = if simulated { + ( + rb.ids.active_set_id, + com_pose.inverse_transform_point(anchor0), + ) + } else { + (u32::MAX, anchor0) + }; + self.attachments + .push(super::soft_attachment::SoftAttachmentConstraint { + soft_body: ai as u32, + attachment: k as u32, + particle, + im_particle, + body_local_point, + anchor0, + erp_inv_dt, + cfm_coeff, + body_im: Vector::ZERO, + body_ii: Default::default(), + arm: Vector::ZERO, + lhs: Matrix::ZERO, + inv_lhs: Matrix::ZERO, + rhs: Vector::ZERO, + impulse: attachment.impulse, + }); + } + } + self.groups[gi].attachments = start..self.attachments.len(); + } + } +} + +#[allow(dead_code)] +const _: () = assert!(DIM + 1 == MAX_CONSTRAINT_PARTICLES); + +/// Barycentric weights of `p` on the surface element (segment in 2D, triangle in 3D), +/// clamped to the element. +pub(crate) fn barycentric_weights(x: &[Vector], p: Vector) -> [Real; DIM] { + #[cfg(feature = "dim2")] + { + let e = x[1] - x[0]; + let l2 = e.length_squared(); + let t = if l2 > 0.0 { + ((p - x[0]).dot(e) / l2).clamp(0.0, 1.0) + } else { + 0.5 + }; + [1.0 - t, t] + } + #[cfg(feature = "dim3")] + { + // Least squares on the triangle plane, then clamped into the triangle. + let e1 = x[1] - x[0]; + let e2 = x[2] - x[0]; + let d = p - x[0]; + let a11 = e1.dot(e1); + let a12 = e1.dot(e2); + let a22 = e2.dot(e2); + let b1 = d.dot(e1); + let b2 = d.dot(e2); + let det = a11 * a22 - a12 * a12; + let (mut u, mut v) = if det.abs() > 1.0e-12 { + ((a22 * b1 - a12 * b2) / det, (a11 * b2 - a12 * b1) / det) + } else { + (1.0 / 3.0, 1.0 / 3.0) + }; + u = u.max(0.0); + v = v.max(0.0); + let sum = u + v; + if sum > 1.0 { + u /= sum; + v /= sum; + } + [1.0 - u - v, u, v] + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs new file mode 100644 index 000000000..db75d6547 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs @@ -0,0 +1,150 @@ +//! The solve passes: volume-piece and intersection-volume constraints, the colored element constraints and their re-sweep, shape-matching and attachment constraints. + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::math::{Real, Vector}; +use crate::utils::{AngularInertiaOps, ComponentMul, DotProduct}; + +use super::*; + +impl SoftConstraintsSet { + /// Solves (or warm-starts) one volume-piece constraint on the given solver bodies. + pub fn solve_volume_constraint( + &mut self, + vi: usize, + bodies: &mut SolverBodies, + warmstart: Option, + ) { + let vc = &mut self.volume_constraints[vi]; + let awake = &self.awake[vc.soft_body as usize]; + // SAFETY: read-only access to the soft body's particles. + let sb = unsafe { &*awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let grads = &self.volume_grads[vc.grads.clone()]; + // The bias is capped so that it pushes no particle faster than the max corrective + // velocity: a crushed closed surface has a large volume error but near-cancelling + // gradients, and the uncapped correction would fling its particles. + let mut g_max: Real = 0.0; + let max_bias = if g_max * vc.inv_lhs > 0.0 { + vc.max_bias_velocity / (g_max * vc.inv_lhs) + } else { + Real::MAX + }; + let rhs_bias = (vc.rhs * vc.erp_inv_dt).clamp(-max_bias, max_bias); + + if let Some(coeff) = warmstart { + vc.impulse *= coeff; + apply(bodies, vc.impulse); + } + + let mut dc = 0.0; + for i in 0..sb.particles.len() { + let s = slots[i]; + if s != u32::MAX { + dc += grads[i].gdot(bodies.get_vel(s).linear); + } + } + let rhs = dc + rhs_bias; + let total = vc.impulse + vc.inv_lhs * (rhs - vc.cfm_gain * vc.impulse); + let delta = total - vc.impulse; + vc.impulse = total; + apply(bodies, delta); + } + + /// Total number of element constraints (scalar + elastic-cell blocks), the writeback stage's domain. + #[inline] + pub fn num_element_constraints(&self) -> usize { + self.scalar_constraints.len() + self.elastic_constraints.len() + } + + /// Updates, optionally warm-starts, and solves the `idx`-th constraint of `range` (virtual + /// index over the color's scalar then block constraints); an update also records whether it + /// is strained past `min_strain` (see [`Self::resweep_strained_constraints`]). + #[inline] + pub fn solve_color_constraint( + &mut self, + range: &SoftColorRange, + idx: usize, + bodies: &mut SolverBodies, + update: bool, + warmstart: Option, + min_strain: Real, + ) { + if idx < range.scalar_constraints.len() { + let i = range.scalar_constraints.start + idx; + let constraint = &mut self.scalar_constraints[i]; + if update { + constraint.update(bodies); + self.strained[i] = constraint.is_strained(min_strain); + } + if let Some(coeff) = warmstart { + constraint.impulse *= coeff; + constraint.warmstart(bodies); + } + constraint.solve(bodies); + } else { + let i = range.elastic_constraints.start + idx - range.scalar_constraints.len(); + let constraint = &mut self.elastic_constraints[i]; + if update { + constraint.update(bodies); + self.strained[self.scalar_constraints.len() + i] = + constraint.is_strained(min_strain); + } + if let Some(coeff) = warmstart { + constraint.strain_impulse *= coeff; + constraint.vol_impulse *= coeff; + constraint.warmstart(bodies); + } + constraint.solve(bodies); + } + } + + /// Re-solves, serially, every constraint of the group flagged strained at its last update + /// (distance error or largest cell strain past the re-sweep threshold): the re-sweep after + /// the contacts touches only the few constraints being torn. + pub fn resweep_strained_constraints(&mut self, group: usize, bodies: &mut SolverBodies) { + let colors = self.groups[group].colors.clone(); + let serial = self.groups[group].serial.clone(); + let num_scalar = self.scalar_constraints.len(); + for ci in colors.chain(core::iter::once(usize::MAX)) { + let (constraints, elastic_constraints) = if ci == usize::MAX { + (serial.scalar_constraints.clone(), serial.elastic_constraints.clone()) + } else { + let r = &self.color_ranges[ci]; + (r.scalar_constraints.clone(), r.elastic_constraints.clone()) + }; + for i in constraints { + if self.strained[i] { + self.scalar_constraints[i].solve(bodies); + } + } + for i in elastic_constraints { + if self.strained[num_scalar + i] { + self.elastic_constraints[i].solve(bodies); + } + } + } + } + + /// Solves the attachment constraints of `group` (serial): updated from the current poses when + /// `update`, warm-started with `Some(coefficient)`, without bias in the relax pass. + pub fn solve_attachments( + &mut self, + group: usize, + bodies: &mut SolverBodies, + wo_bias: bool, + update: bool, + warmstart: Option, + ) { + for constraint in &mut self.attachments[self.groups[group].attachments.clone()] { + if update { + constraint.update(bodies, wo_bias); + } + if let Some(coeff) = warmstart { + } + } + } +} 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 new file mode 100644 index 000000000..713da0f73 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_writeback.rs @@ -0,0 +1,117 @@ +//! Writeback of the solved state to the soft bodies: element impulses and plastic flow, particle poses and velocities, shape-matching, volume and attachment state. + +use core::ops::Range; +use core::sync::atomic::Ordering; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::math::{Real, Vector}; + +use super::super::soft_element_constraint::SoftScalarConstraintWriteback; +use super::*; + +impl SoftConstraintsSet { + /// Writes the accumulated impulses (and shape-matching rotations) back to the soft bodies, + /// applies the elastic cells' plastic flow (`dt`: the step length) and updates the elements' + /// smoothed tear load. SAFETY: `range` constraints write disjoint elements, one worker each. + pub unsafe fn writeback_constraints(&self, range: Range, dt: Real) { + // Virtual index space: the scalar constraints first, then the elastic-cell block constraints. + let num_scalar_constraints = self.scalar_constraints.len(); + for idx in range { + if idx < num_scalar_constraints { + let constraint = &self.scalar_constraints[idx]; + let awake = &self.awake[constraint.soft_body as usize]; + let sb = unsafe { &mut *awake.ptr }; + let impulse = crate::utils::canonicalize_zero(constraint.impulse); + match constraint.writeback { + SoftScalarConstraintWriteback::Edge => { + let edge = &mut sb.edges[constraint.element as usize]; + edge.impulse = impulse; + } + } + #[cfg(feature = "dim3")] + SoftScalarConstraintWriteback::Dihedral => { + sb.dihedrals[constraint.element as usize].impulse = impulse + } + SoftScalarConstraintWriteback::CellVolume => { + sb.cells[constraint.element as usize].impulses[0] = impulse + } + } + } else { + let constraint = &self.elastic_constraints[idx - num_scalar_constraints]; + let awake = &self.awake[constraint.soft_body as usize]; + let sb = unsafe { &mut *awake.ptr }; + let material = sb.material; + let cell = &mut sb.cells[constraint.element as usize]; + let impulses = + (constraint.strain_impulse.iter()).chain(core::iter::once(&constraint.vol_impulse)); + for (dst, src) in cell.impulses.iter_mut().zip(impulses) { + *dst = crate::utils::canonicalize_zero(*src); + } + cell.rotation = constraint.rotation; + if material.plastic_yield > 0.0 + && material.plastic_creep > 0.0 + && plastic_flow(cell, &constraint.strain, &material, dt) + { + awake.plastic_flow.store(true, Ordering::Relaxed); + } + } + } + } + } + } + + /// Writes the solved poses and velocities of awake body `ai`'s particles back to the soft + /// body (linear damping applied, a pinned particle snapped to its target). SAFETY: writes only + /// that body's particles; the caller guarantees each awake body is written back by one worker. + pub unsafe fn writeback_particles(&self, ai: usize, bodies: &SolverBodies, step_dt: Real) { + let awake = &self.awake[ai]; + let sb = unsafe { &mut *awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let damping = crate::utils::inv(1.0 + step_dt * sb.particle_settings.linear_damping); + for (p, &slot) in sb.particles.iter_mut().zip(slots) { + let vel = bodies.get_vel(slot).linear; + if p.inv_mass == 0.0 { + // A target-driven pin lands exactly on its target (and stays in that mode: with + // the target reached, the next step's velocity is zero unless a new one is set). + p.velocity = vel; + p.position = match p.next_position { + Some(target) => target, + None => bodies.get_pose(slot).translation, + }; + } else { + p.velocity = vel * damping; + p.position = bodies.get_pose(slot).translation; + } + } + } + + /// Writes back the shape-matching, volume-piece and attachment state (serial). + pub fn writeback_bodies(&self) { + for constraint in &self.attachments { + // SAFETY: serial stage, exclusive access. + let ptr = self.awake[constraint.soft_body as usize].ptr; + let sb = unsafe { &mut *ptr }; + if let Some(a) = sb.attachments.get_mut(constraint.attachment as usize) { + a.impulse = constraint.impulse; + } + } + for awake in &self.awake { + // SAFETY: serial stage, exclusive access. + let sb = unsafe { &mut *awake.ptr }; + if let Some(vi) = awake.volume_constraint { + sb.volume_impulse = crate::utils::canonicalize_zero(self.volume_constraints[vi].impulse); + } + 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]]; + // Summed by the contact writeback that follows the solve. + sb.contact_load = 0.0; + } + } +} + +pub(super) use super::super::soft_element_constraint::outer_product as outer; diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs new file mode 100644 index 000000000..25e7c36ee --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs @@ -0,0 +1,64 @@ +//! The constraint types of the set: the shape-matching, volume-piece and intersection-volume constraints with their FEM sides. + +use core::ops::Range; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::math::{AngVector, DIM, Matrix, Real, Rotation, Vector}; +use crate::utils::ComponentMul; + +use super::super::soft_attachment::FemAttachment; + +/// The `DIM` axis constraints pulling one particle toward its shape-matching goal. +#[derive(Copy, Clone, Debug)] +pub(crate) struct SoftShapeConstraint { + pub particle: u32, + pub solver_id: u32, + pub im: Vector, + pub goal: Vector, + /// Velocity of the goal (the body's best-fit rigid motion, or the target's): the constraint damps + /// the particle's velocity relative to it, so the rigid modes (a free fall, a spin) are left + /// alone; damping the absolute velocity summed to a brake on the whole body. + pub goal_vel: Vector, + pub erp_inv_dt: Real, + pub cfm_coeff: Real, + pub inv_lhs: Vector, + pub cfm_gain: Vector, + pub rhs: Vector, + pub impulse: Vector, +} +impl SoftShapeConstraint { + #[inline] + pub fn update(&mut self, bodies: &SolverBodies) { + let pos = bodies.get_pose(self.solver_id).translation; + // Uncapped: the bias is the spring force (see `SoftScalarConstraint::update`). + self.rhs = (pos - self.goal) * self.erp_inv_dt; + } + + #[inline] + #[inline] + if (self.solver_id as usize) >= bodies.len() { + return; + } + let delta = total - self.impulse; + self.impulse = total; +} +/// The global area/volume preservation row of a soft body (touches every particle: solved +/// serially). +#[derive(Clone, Debug)] +pub(crate) struct SoftVolumeConstraint { + pub soft_body: u32, + pub target: Real, + /// Range of this row's particle gradients in `volume_grads`. + pub grads: Range, + pub erp_inv_dt: Real, + pub cfm_coeff: Real, + /// Cap on the velocity the bias may give any particle in one solve. + pub max_bias_velocity: Real, + pub inv_lhs: Real, + pub cfm_gain: Real, + pub rhs: Real, + pub impulse: Real, +} 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 new file mode 100644 index 000000000..283b1c528 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints_set.rs @@ -0,0 +1,172 @@ +//! The awake bodies and clusters, the per-color and per-group layout, the cluster records and the constraints set itself. + +use crate::alloc_prelude::*; +use core::ops::Range; +use core::sync::atomic::AtomicBool; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::dynamics::solver::solver_body::SolverVel; +use crate::dynamics::{SoftBody, SoftBodyHandle}; +use crate::math::{AngVector, AngularInertia, Real, Rotation, Vector}; + +use super::super::soft_contact::SoftContact; +use super::super::soft_element_constraint::{SoftElasticConstraint, SoftScalarConstraint}; +use super::*; + +/// One awake soft body, as seen by the solver for the current step. +pub(crate) struct AwakeSoftBody { + /// Raw access to the soft body for the solve scope (the set is not mutated meanwhile); + /// constraints write their impulses back through it, to disjoint elements. + pub ptr: *mut SoftBody, + pub handle: SoftBodyHandle, + /// Substep solve-group of the body's particles. + pub group: u16, + /// No particle is a solver DOF this step (every one pinned, or the free ones asleep): the + /// body only lends its surface to the contact constraints holding the awake bodies touching it. + pub frozen: bool, + /// Solver-body slot of each particle: `slots[slot_start..slot_start + num_particles]` (the + /// particles' slots are contiguous, laid out group-major after the rigid bodies'). + pub slot_start: usize, + pub num_particles: usize, + pub shape_com: Vector, + /// Range of this body's constraints in `shape_constraints`. + pub shape_constraints: Range, + /// Range of this body's volume constraints in `volume_constraints` (one per volume piece). + pub volume_constraint: Option, + /// Fraction of the non-rigid velocity removed per substep (`deformation_damping`). + pub damping_factor: Real, + /// Set by the writeback stage when a cell flowed plastically by a significant amount + /// (the body is kept awake so its creep goes on). + pub plastic_flow: AtomicBool, + /// 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`: + /// no contact row at all), set by the contact assembly. + pub contact_approach_speed: Option, +} +// SAFETY: the raw pointer is only dereferenced under the staged solver's stage discipline. +unsafe impl Send for AwakeSoftBody {} +unsafe impl Sync for AwakeSoftBody {} + +pub(crate) struct AwakeCluster { + /// Index of the cluster. + pub cluster: u32, + /// Warm-start rotation of the fit, or the kinematic target's rotation. + pub rotation: Rotation, + /// Linear velocity of the kinematic target if any. Zero otherwise. + pub target_linvel: Vector, + /// Angular velocity of the kinematic target if any. Zero otherwise. + pub target_angvel: AngVector, + /// The fit's centroids and inverse inertia of the last substep update: the current and rest + /// centers of mass of all the cluster's particles, the current center of mass of its free ones, + /// and the pseudo-inverse of their inertia about it. + pub com: Vector, + pub rest_com: Vector, + pub dyn_com: Vector, + pub inv_inertia: AngularInertia, +} + +/// The constraints of one parallel color: a range of scalar constraints and a range of +/// elastic-cell block constraints, addressed by one virtual index range +/// `0..scalar_constraints.len() + elastic_constraints.len()`. +#[derive(Clone, Debug, Default)] +pub(crate) struct SoftColorRange { + pub scalar_constraints: Range, + pub elastic_constraints: Range, +} + +impl SoftColorRange { + #[inline] + pub fn len(&self) -> usize { + self.scalar_constraints.len() + self.elastic_constraints.len() + } +} + +/// Per-group slices of the soft layout. +#[derive(Clone, Debug, Default)] +pub(crate) struct SoftGroupLayout { + /// Range of awake soft bodies (into `awake`) belonging to the group. + pub awake: Range, + /// Solver-body slot range of the group's particles. + pub slots: Range, + /// Range into `attachments`. + pub attachments: Range, + /// Index range into `color_ranges`. + pub colors: Range, + /// Constraints solved serially by worker 0 (overflow color). + pub serial: SoftColorRange, + /// Range into `shape_constraints`. + pub shape_constraints: Range, + /// Range into `volume_constraints`. + pub volume_constraints: Range, + /// Range into `contacts`. + pub contacts: Range, + /// Range into `contact_colors`: the parallel contact stages. + pub contact_colors: Range, + /// Range into `contact_chunks`: the contact chunks solved serially by worker 0 after the + /// colors. + pub contact_serial: Range, + /// Whether an awake body of the group damps its deformation. + pub damping: bool, +} +/// The soft-body constraints of one step. +#[derive(Default)] +pub(crate) struct SoftConstraintsSet { + pub awake: Vec, + pub slots: Vec, + pub scalar_constraints: Vec, + pub elastic_constraints: Vec, + /// Whether each element constraint (the scalar ones, then the elastic ones) was strained + /// past the re-sweep threshold at its last update: the re-sweep reads these flags instead + /// of every constraint. + pub strained: Vec, + /// Row ranges of the parallel colors, group-major then color ascending. + pub color_ranges: Vec, + pub shape_constraints: Vec, + pub volume_constraints: Vec, + pub volume_grads: Vec, + /// Contacts against the awake soft bodies' surface colliders, group-major. + pub contacts: Vec, + /// The particle attachments of the awake soft bodies, group-major. + pub attachments: Vec, + /// The contact rows chunked by body pair (see `soft_contact_chunks`): row indices into + /// `contacts` chunk by chunk, the chunks' ranges into them (group-major, parallel colors + /// then the serial tail), and the chunk ranges of the parallel colors. + pub contact_chunk_constraints: Vec, + pub contact_chunks: Vec>, + pub contact_colors: Vec>, + pub groups: Vec, + /// Scratch of the element-constraint assembly (see `soft_element_constraint_assembly`). + pub(crate) element_constraint_workspace: + super::soft_element_constraint_assembly::ElementConstraintAssemblyWorkspace, + /// Workspace of the edge-vs-edge contact assembly (see `soft_contact_assembly`). + pub(crate) edge_workspace: super::soft_contact_assembly::EdgeContactWorkspace, + /// Workspace of the contact chunking (see `soft_contact_chunks`). + pub(crate) contact_workspace: super::soft_contact_chunks::ContactChunkWorkspace, +} + +impl SoftConstraintsSet { + pub fn new() -> Self { + Self::default() + } + + /// Whether the step has any soft-body work at all. + #[inline] + pub fn is_empty(&self) -> bool { + self.awake.is_empty() + } + + /// Whether the given group has soft-body work needing the per-pass prepare stage + /// (shape matching, volume pieces, deformation damping). + #[inline] + pub fn group_needs_prepare(&self, group: usize, update: bool) -> bool { + let g = &self.groups[group]; + // The volume gradients and the damping only move with the poses (once per substep); + // the shape constraints re-fit the velocities every pass. + !g.shape_constraints.is_empty() + || (update && (!g.volume_constraints.is_empty() || g.damping)) + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_contact.rs b/src/dynamics/solver/soft_constraint/soft_contact.rs new file mode 100644 index 000000000..e572a80c3 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_contact.rs @@ -0,0 +1,326 @@ +//! Contact constraints between a soft-body surface element (`DIM` particles, barycentric weights) +//! and a solver body or another surface element (surface-vs-surface and self contacts). Same model +//! as the rigid kernel: speculation, softness bias, Coulomb friction, warm start, total impulses. + +use crate::dynamics::IntegrationParameters; +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::geometry::ColliderHandle; +use crate::math::{AngVector, AngularInertia, DIM, Real, Vector}; +use crate::utils::{AngularInertiaOps, ComponentMul, CrossProduct, DotProduct, OrthonormalBasis}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// Where a soft contact writes its impulses back: a narrow-phase manifold point, or (`manifold` +/// set to an assembly sentinel) an entry of the owner body's edge-vs-edge or vertex-vs-surface +/// list, plus the pair's contact `slot` reporting the impulse (`u32::MAX` for a self contact). +#[derive(Copy, Clone, Debug)] +pub(crate) struct SoftContactSource { + pub collider1: ColliderHandle, + pub collider2: ColliderHandle, + pub manifold: u32, + pub point: u32, + pub slot: u32, +} + +/// The other side of a soft contact when it is a surface element too: its particles' solver +/// slots (`u32::MAX`: frozen at `frozen_pos`), barycentric weights, inverse masses. +#[derive(Copy, Clone, Debug)] +pub(crate) struct SoftContactElement { + pub particles: [u32; CONTACT_ANCHORS], + pub weights: [Real; CONTACT_ANCHORS], + pub im_particles: [Real; CONTACT_ANCHORS], + pub frozen_pos: [Vector; CONTACT_ANCHORS], +} + +/// Number of particles the soft side of a contact is spread over: a surface element's, or a +/// cell's when the body collides through a skin the cells carry. +pub(crate) const CONTACT_ANCHORS: usize = DIM + 1; + +/// One contact point between a soft surface element and a solver body. +#[derive(Copy, Clone, Debug)] +pub(crate) struct SoftContact { + pub source: SoftContactSource, + /// Index of the surface's soft body in the solver's awake list, and the element's particle + /// indices in it (the vertex support rows write their warm start back to the particle). + pub support_body: u32, + pub support_particle: [u32; CONTACT_ANCHORS], + /// Solver slots of the particles carrying the contact point (`u32::MAX`: frozen at + /// `frozen_pos`, or an unused anchor when its weight is zero). + pub particles: [u32; CONTACT_ANCHORS], + pub weights: [Real; CONTACT_ANCHORS], + pub im_particles: [Real; CONTACT_ANCHORS], + pub frozen_pos: [Vector; CONTACT_ANCHORS], + /// The other side: solver slot (`u32::MAX`: world-attached at `body_point0`, or a surface + /// element when `element` is set). + pub body: u32, + pub element: Option, + /// Index of the other side's soft body in the solver's awake list when it is a particle + /// or an element of one (`u32::MAX`: a rigid body or the world), for the chunking of the + /// constraints by soft-body pair. + pub other_body: u32, + pub body_im: Vector, + pub body_ii: AngularInertia, + /// The other side's contact point in its CoM-local frame, and frozen world lever arm. + pub body_local_point: Vector, + pub body_arm: Vector, + /// World contact points at build time (surface side and other side). + pub surface_point0: Vector, + pub body_point0: Vector, + /// Force direction on the surface (minus the contact normal from surface to body). + pub dir: Vector, + pub tangents: [Vector; DIM - 1], + /// Separation at build time (skins baked in), and its live value. + pub dist0: Real, + pub friction: Real, + /// The other side is a soft-body particle: the row is solved in the biased pass only, like + /// the springs driving both sides. + pub soft_other: bool, + /// Static or dynamic contact softness. + pub erp_inv_dt: Real, + pub cfm_factor: Real, + /// Cap of the penetration-recovery velocity (`Real::MAX`: the world's + /// `max_corrective_velocity`); a demoted expulsion holds its intruder quietly. + pub max_bias: Real, + // Solve state. + pub torque_dir: AngVector, + pub ii_torque_dir: AngVector, + pub r_normal: Real, + pub rhs_normal: Real, + pub cfm_normal: Real, + pub impulse_normal: Real, + pub impulse_normal_acc: Real, + pub torque_tangent: [AngVector; DIM - 1], + pub ii_torque_tangent: [AngVector; DIM - 1], + pub r_tangent: [Real; DIM - 1], + pub rhs_tangent: [Real; DIM - 1], + pub impulse_tangent: [Real; DIM - 1], + pub impulse_tangent_acc: [Real; DIM - 1], +} + +impl SoftContact { + /// Current world position of the surface point (barycentric blend of the particles). + #[inline] + fn surface_point(&self, bodies: &SolverBodies) -> Vector { + let mut p = Vector::ZERO; + for k in 0..CONTACT_ANCHORS { + let id = self.particles[k]; + let x = if id == u32::MAX { + self.frozen_pos[k] + } else { + bodies.get_pose(id).translation + }; + p += x * self.weights[k]; + } + p + } + + /// Current world position of the other side's contact point. + #[inline] + fn body_point(&self, bodies: &SolverBodies) -> Vector { + if let Some(e) = &self.element { + let mut p = Vector::ZERO; + for k in 0..CONTACT_ANCHORS { + let id = e.particles[k]; + let x = if id == u32::MAX { + e.frozen_pos[k] + } else { + bodies.get_pose(id).translation + }; + p += x * e.weights[k]; + } + p + } else if self.body == u32::MAX { + self.body_point0 + } else { + bodies.get_pose(self.body).pose() * self.body_local_point + } + } + + /// Updates the separation, rhs and effective masses from the current solver state. + /// `wo_bias`: relax pass (speculative term only, no softness, no tangent drift bias). + pub fn update(&mut self, bodies: &SolverBodies, params: &IntegrationParameters, wo_bias: bool) { + let inv_dt = params.inv_dt(); + let ps = self.surface_point(bodies); + let po = self.body_point(bodies); + let delta = (ps - self.surface_point0) - (po - self.body_point0); + let dist = self.dist0 + delta.gdot(self.dir); + + // Effective masses (the other side's mass properties are step-constant, read once the + // solver bodies are filled: this runs after the body-init stage). + if self.body != u32::MAX { + let pose = bodies.get_pose(self.body); + self.body_im = pose.im; + self.body_ii = pose.ii; + } + // A side acting through pinned particles but for a sliver of weight on a free one is a + // lever (the free particle would need a huge impulse): it is frozen for the constraint. + fn freeze_levers(weights: &[Real; N], im: &mut [Real; N]) { + let mut w2 = 0.0; + let mut im_max: Real = 0.0; + for k in 0..N { + w2 += weights[k] * weights[k] * im[k]; + im_max = im_max.max(im[k]); + } + if im_max > 0.0 && w2 < 0.04 * im_max { + *im = [0.0; N]; + } + } + freeze_levers(&self.weights, &mut self.im_particles); + if let Some(e) = &mut self.element { + freeze_levers(&e.weights, &mut e.im_particles); + } + let mut w_particles = 0.0; + for k in 0..CONTACT_ANCHORS { + w_particles += self.weights[k] * self.weights[k] * self.im_particles[k]; + } + if let Some(e) = &self.element { + for k in 0..CONTACT_ANCHORS { + w_particles += e.weights[k] * e.weights[k] * e.im_particles[k]; + } + } + self.torque_dir = self.body_arm.gcross(-self.dir); + self.ii_torque_dir = self.body_ii.transform_vector(self.torque_dir); + self.r_normal = crate::utils::inv( + w_particles + + self.dir.gdot(self.body_im.component_mul(&self.dir)) + + self.ii_torque_dir.gdot(self.torque_dir), + ); + for j in 0..DIM - 1 { + let t = self.tangents[j]; + self.torque_tangent[j] = self.body_arm.gcross(-t); + self.ii_torque_tangent[j] = self.body_ii.transform_vector(self.torque_tangent[j]); + self.r_tangent[j] = crate::utils::inv( + w_particles + + t.gdot(self.body_im.component_mul(&t)) + + self.ii_torque_tangent[j].gdot(self.torque_tangent[j]), + ); + } + + let rhs_wo_bias = dist.max(0.0) * inv_dt; + if wo_bias { + self.rhs_normal = rhs_wo_bias; + self.cfm_normal = 1.0; + for j in 0..DIM - 1 { + self.rhs_tangent[j] = 0.0; + } + } else { + let max_corrective_velocity = params.max_corrective_velocity().min(self.max_bias); + let bias = (dist * self.erp_inv_dt).clamp(-max_corrective_velocity, 0.0); + self.rhs_normal = rhs_wo_bias + bias; + self.cfm_normal = if dist <= 0.0 { self.cfm_factor } else { 1.0 }; + for j in 0..DIM - 1 { + self.rhs_tangent[j] = delta.gdot(self.tangents[j]) * inv_dt; + } + // Bank the previous substep's impulses (they were applied in full) before the + // warm-start scaling of the new substep. + self.impulse_normal_acc += self.impulse_normal; + self.impulse_normal *= params.warmstart_coefficient; + for j in 0..DIM - 1 { + self.impulse_tangent_acc[j] += self.impulse_tangent[j]; + self.impulse_tangent[j] *= params.warmstart_coefficient; + } + } + } + + /// Which sides belong to a FEM body: `(surface side, other side)`. + #[inline] + fn apply(&self, bodies: &mut SolverBodies, dir: Vector, ii_torque: AngVector, lambda: Real) { + for k in 0..CONTACT_ANCHORS { + let id = self.particles[k]; + if id != u32::MAX && (id as usize) < bodies.len() { + bodies.vels[id as usize].linear += + dir * (self.weights[k] * self.im_particles[k] * lambda); + for k in 0..CONTACT_ANCHORS { + let id = e.particles[k]; + if id != u32::MAX && (id as usize) < bodies.len() { + bodies.vels[id as usize].linear -= + dir * (e.weights[k] * e.im_particles[k] * lambda); + } + } + } else if self.body != u32::MAX && (self.body as usize) < bodies.len() { + } + + /// Relative velocity of the surface point w.r.t. the other side's contact point, dotted + /// with `dir` (its torque term given). + #[inline] + fn relative_velocity(&self, bodies: &SolverBodies, dir: Vector, torque: AngVector) -> Real { + let mut vs = Vector::ZERO; + for k in 0..CONTACT_ANCHORS { + let id = self.particles[k]; + if id != u32::MAX { + vs += bodies.get_vel(id).linear * self.weights[k]; + } + } + let mut dvel = dir.gdot(vs); + if let Some(e) = &self.element { + let mut vo = Vector::ZERO; + for k in 0..CONTACT_ANCHORS { + let id = e.particles[k]; + if id != u32::MAX { + vo += bodies.get_vel(id).linear * e.weights[k]; + } + } + dvel -= dir.gdot(vo); + } else if self.body != u32::MAX { + let vo = bodies.get_vel(self.body); + dvel += -dir.gdot(vo.linear) + torque.gdot(vo.angular); + } + dvel + } + + /// Applies the accumulated (warm-start) impulses. + pub fn warmstart(&self, bodies: &mut SolverBodies) { + self.apply(bodies, self.dir, self.ii_torque_dir, self.impulse_normal); + for j in 0..DIM - 1 { + self.apply( + bodies, + self.tangents[j], + self.ii_torque_tangent[j], + self.impulse_tangent[j], + ); + } + } + + /// One Gauss-Seidel iteration: normal part then Coulomb friction. + pub fn solve(&mut self, bodies: &mut SolverBodies) { + // Normal. + let dvel = self.relative_velocity(bodies, self.dir, self.torque_dir) + self.rhs_normal; + let new_impulse = (self.cfm_normal * (self.impulse_normal - self.r_normal * dvel)).max(0.0); + let delta = new_impulse - self.impulse_normal; + self.impulse_normal = new_impulse; + self.apply(bodies, self.dir, self.ii_torque_dir, delta); + + // Friction: per-tangent update, then a clamp to the disc of radius μλ. + let limit = self.friction * self.impulse_normal; + let mut new_tangent = self.impulse_tangent; + for j in 0..DIM - 1 { + let dvel = self.relative_velocity(bodies, self.tangents[j], self.torque_tangent[j]) + + self.rhs_tangent[j]; + new_tangent[j] -= self.r_tangent[j] * dvel; + } + #[cfg(feature = "dim2")] + { + new_tangent[0] = new_tangent[0].clamp(-limit, limit); + } + #[cfg(feature = "dim3")] + { + let norm = (new_tangent[0] * new_tangent[0] + new_tangent[1] * new_tangent[1]).sqrt(); + if norm > limit { + let scale = if norm > 0.0 { limit / norm } else { 0.0 }; + new_tangent[0] *= scale; + new_tangent[1] *= scale; + } + } + for j in 0..DIM - 1 { + let delta = new_tangent[j] - self.impulse_tangent[j]; + self.impulse_tangent[j] = new_tangent[j]; + self.apply(bodies, self.tangents[j], self.ii_torque_tangent[j], delta); + } + } + + /// The tangent basis of a force direction. + pub fn tangent_basis(dir: Vector) -> [Vector; DIM - 1] { + dir.orthonormal_basis() + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/mod.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/mod.rs new file mode 100644 index 000000000..8e4ee59a5 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/mod.rs @@ -0,0 +1,17 @@ +//! Assembly of the soft contact constraints: the narrow-phase pairs of the awake soft bodies' +//! surface colliders and particle balls, the vertex-vs-surface and edge-vs-edge candidates between +//! surfaces (see `geometry::soft_contacts`), and the write-back of their impulses. + +mod soft_contact_assembly; +mod soft_contact_assembly_body_contacts; +mod soft_contact_assembly_edge_contacts; +mod soft_contact_assembly_vertex_contacts; +mod soft_contact_assembly_volume_constraints; +mod soft_contact_assembly_workspace; +mod soft_contact_assembly_writeback; + +pub(crate) use soft_contact_assembly_workspace::EdgeContactWorkspace; +use soft_contact_assembly_workspace::{ + AssemblyCtx, BodyContacts, MeshContacts, OverlapConstraintWorkspace, SOURCE_EDGE_CONTACT, + SOURCE_VERTEX_CONTACT, material_pace, mesh_of, mesh_ref, +}; diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs new file mode 100644 index 000000000..d97553675 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs @@ -0,0 +1,148 @@ +//! The `assemble_contacts` entry point: runs the per-body assembly and appends the constraints to the substep groups. + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::{AssemblyCtx, BodyContacts}; +use super::super::soft_constraints_set::{SoftConstraintsSet, SoftOverlapConstraint}; +use super::super::soft_contact::SoftContact; +use crate::dynamics::{IntegrationParameters, RigidBodySet, SoftBodySet, SpringCoefficients}; +use crate::geometry::{ColliderSet, NarrowPhase}; +use crate::math::{Real, Vector}; + +impl SoftConstraintsSet { + /// Builds the contact constraints of the awake soft bodies' surface colliders from the + /// narrow-phase pairs and the vertex-vs-surface and edge-vs-edge candidates. Group-major: runs + /// after [`Self::assemble`], appending to each group's `contacts` range (`group_dt`: softness). + #[allow(clippy::too_many_arguments)] + pub fn assemble_contacts( + &mut self, + island_id: usize, + params: &IntegrationParameters, + narrow_phase: &NarrowPhase, + colliders: &ColliderSet, + bodies: &RigidBodySet, + soft_bodies: &SoftBodySet, + group_of_slot: impl Fn(u32) -> usize, + group_dt: impl Fn(usize) -> Real, + ) { + if self.awake.is_empty() { + return; + } + // The contact softness is normalized by the constraint's effective mass, i.e. by a few particles' + // mass here, while the load comes from the whole soft body through them: at the rigid + // defaults the constraints would sink under a body's weight, so they run stiffer. + let stiffen = |c: &SpringCoefficients| SpringCoefficients { + natural_frequency: c.natural_frequency * params.soft_bodies.contact_stiffening, + damping_ratio: c.damping_ratio, + }; + let dyn_soft = stiffen(¶ms.contact_softness); + let static_soft = stiffen(¶ms.static_contact_softness); + let ctx = AssemblyCtx { + island_id, + params, + narrow_phase, + colliders, + bodies, + soft_bodies, + dyn_soft: ( + dyn_soft.erp_inv_dt(params.dt), + dyn_soft.cfm_factor(params.dt), + ), + static_soft: ( + static_soft.erp_inv_dt(params.dt), + static_soft.cfm_factor(params.dt), + ), + }; + let mut per_body = core::mem::take(&mut self.edge_workspace.per_body); + per_body.resize_with(self.awake.len(), Default::default); + { + let workspace = &mut self.edge_workspace; + workspace.awake_of.clear(); + for (ai, awake) in self.awake.iter().enumerate() { + workspace.awake_of.insert(awake.handle, ai); + } + } + + // Every awake body's constraints only read the shared sets: assembled in parallel, appended in + // awake order (deterministic). + let this = &*self; + let assemble_body = |(ai, out): (usize, &mut BodyContacts)| { + out.clear(); + this.assemble_body_contacts(ai, &ctx, out); + }; + #[cfg(feature = "parallel")] + { + use rayon::prelude::*; + per_body.par_iter_mut().enumerate().for_each(assemble_body); + } + #[cfg(not(feature = "parallel"))] + per_body.iter_mut().enumerate().for_each(assemble_body); + + for (ai, body) in per_body.iter().enumerate() { + // A frozen body has no particle to substep for; its request would only land on a + // pinned particle's kinematic body. + self.awake[ai].contact_approach_speed = (!body.contacts.is_empty() + && !self.awake[ai].frozen) + .then_some(body.max_approach_speed); + } + // A frozen body's constraint is solved with the group of the body it holds (its own particles + // are not solver DOFs): the other side's slot, or the other element's first live particle. + let constraint_group = |c: &SoftContact, own: u16| -> usize { + if c.body != u32::MAX { + return group_of_slot(c.body); + } + let live = c + .element + .as_ref() + .and_then(|e| e.particles.iter().copied().find(|&s| s != u32::MAX)); + live.map(&group_of_slot).unwrap_or(own as usize) + }; + for gi in 0..self.groups.len() { + let start = self.contacts.len(); + for ai in self.groups[gi].awake.clone() { + if !self.awake[ai].frozen { + self.contacts.append(&mut per_body[ai].contacts); + } + } + for (ai, awake) in self.awake.iter().enumerate() { + if awake.frozen { + self.contacts.extend( + per_body[ai] + .contacts + .iter() + .filter(|c| constraint_group(c, awake.group) == gi), + ); + } + } + // The softness of a constraint follows its group's substep length (the constraints were built + // with the first group's). + let dyn_soft = ( + dyn_soft.erp_inv_dt(group_dt(gi)), + dyn_soft.cfm_factor(group_dt(gi)), + ); + let static_soft = ( + static_soft.erp_inv_dt(group_dt(gi)), + static_soft.cfm_factor(group_dt(gi)), + ); + for c in &mut self.contacts[start..] { + let world_fixed = c.element.is_none() && c.body == u32::MAX; + (c.erp_inv_dt, c.cfm_factor) = if world_fixed { static_soft } else { dyn_soft }; + } + self.groups[gi].contacts = start..self.contacts.len(); + } + // Hand the new edge and vertex contacts to their owner bodies. + // SAFETY: the assembly holds no other reference into the soft bodies any more. + for (ai, awake) in self.awake.iter().enumerate() { + let sb = unsafe { &mut *awake.ptr }; + for assembled in &mut per_body[ai].meshes { + let Some(mesh) = sb.mesh_mut(assembled.id) else { + continue; + }; + core::mem::swap(&mut mesh.edge_contacts, &mut assembled.edge_contacts); + core::mem::swap(&mut mesh.vertex_contacts, &mut assembled.vertex_contacts); + self.edge_workspace.per_body = per_body; + self.chunk_contacts(); + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs new file mode 100644 index 000000000..5f814bfb0 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs @@ -0,0 +1,439 @@ +//! Per-body contact constraints: the surface's narrow-phase manifold constraints (rigid bodies and particle balls), then the dispatch to the vertex and edge passes. + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::alloc_prelude::*; + +use super::{AssemblyCtx, BodyContacts, mesh_ref}; +use super::super::soft_constraints_set::{SoftConstraintsSet, barycentric_weights}; +use super::super::soft_contact::CONTACT_ANCHORS; +use super::super::soft_contact::{SoftContact, SoftContactSource}; +use crate::dynamics::soft_body::SoftPatchConstraints; +use crate::dynamics::SoftBodyHandle; +use crate::geometry::ColliderHandle; +use crate::geometry::soft_contacts::SoftEdgePass; +use crate::math::{DIM, Real, Vector}; +use crate::utils::{CrossProduct, DotProduct}; + +impl SoftConstraintsSet { + /// The contact constraints of awake body `ai`: its surface's narrow-phase pairs, then the + /// vertex-vs-surface and edge-vs-edge constraints against the other soft surfaces met and against + /// itself (self contacts). + pub(super) fn assemble_body_contacts(&self, ai: usize, ctx: &AssemblyCtx, out: &mut BodyContacts) { + use crate::geometry::contact_pair::NEW_CONTACT_BIT; + let AssemblyCtx { + island_id, + params, + narrow_phase, + colliders, + bodies, + soft_bodies, + dyn_soft: (dyn_erp, dyn_cfm), + static_soft: (static_erp, static_cfm), + .. + } = *ctx; + { + let awake = &self.awake[ai]; + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + for mesh in sb.meshes().filter(|mesh| mesh.collision_enabled()) { + let surface_handle = mesh.collider(); + let self_frozen = awake.frozen; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + // The other soft surfaces whose edge-vs-edge constraints this body owns: (awake + // index, handle, surface collider, the pair's edge candidates), assembled + // once the pairs are done. + let mut edge_tasks: Vec<( + Option, + SoftBodyHandle, + ColliderHandle, + Option<&SoftEdgePass>, + )> = Vec::new(); + + for pair in narrow_phase.contact_pairs_with(surface_handle) { + let surface_first = pair.collider1 == surface_handle; + let other_handle = if surface_first { + pair.collider2 + } else { + pair.collider1 + }; + let (Some(surface_co), Some(other_co)) = + (colliders.get(surface_handle), colliders.get(other_handle)) + else { + continue; + }; + // Another soft body: vertex-vs-surface constraints (each owner, in pair order) + // and edge-vs-edge ones (lower awake index, after its pairs) from the detected + // candidates; a pinned other is frozen, a non-awake one gets both passes here. + if other_co.deformable_mesh_ref.is_some() { + // The pair reports a contact while the two surfaces' bounds overlap + // (see the narrow phase): nothing to do otherwise. + if !pair.has_any_active_contact() { + continue; + } + let Some(detected) = pair.soft() else { + continue; + }; + let other_ref = mesh_ref(other_co); + let other_sb_handle = other_ref.body; + let other_ai = self.edge_workspace.awake_of.get(&other_sb_handle).copied(); + let Some(other_sb) = soft_bodies.get(other_sb_handle) else { + continue; + }; + let frozen = match other_ai { + Some(bi) => self.awake[bi].frozen, + None => other_sb.particles.iter().all(|p| p.inv_mass == 0.0), + }; + // Two frozen surfaces have no DOF between them. + if self_frozen && frozen { + continue; + } + let both = match other_ai { + Some(_) => false, + None if frozen => true, + None => continue, + }; + let flips: &[bool] = if both { &[false, true] } else { &[false] }; + for &flipped in flips { + // The vertex pass whose surface side this pass assembles. + let surface = if flipped { other_handle } else { surface_handle }; + let Some(vertex_pass) = detected.vertex_pass_on(surface) else { + continue; + }; + self.assemble_vertex_contacts( + ctx, + ai, + other_ai, + other_sb, + (surface_handle, surface_co), + (other_handle, other_co), + flipped, + vertex_pass, + detected.volume.as_ref(), + &[], + out, + ); + } + continue; + } + if !pair.has_any_active_contact() { + continue; + } + let surface_rb = surface_co.parent().and_then(|h| bodies.get(h)); + let other_rb = other_co.parent().and_then(|h| bodies.get(h)); + // The other side's soft body and particle when it is a particle ball, and + // that body's awake index (for the chunking). + let other_body = u32::MAX; + // Self contact: the other side is one of this soft body's own particles. + let self_particle = None; + let com_pose = |rb: &crate::dynamics::RigidBody| { + rb.pos + .position + .prepend_translation(rb.mprops.local_mprops.local_com) + }; + // The other side's solver body (its contact point is tracked in that body's + // CoM frame), if it is simulated in this island: a rigid body, or the + // particle's solver body (its frame is at the particle). + let (other_slot, other_com_pose) = match other_rb + { + Some(rb) + if rb.ids.active_island_id == island_id as u32 + && !rb.is_sleeping() + && rb.is_enabled() + && rb.is_dynamic_or_kinematic() => + { + (rb.ids.active_set_id, Some(com_pose(rb))) + } + _ => (u32::MAX, None), + }; + // A frozen surface against a fixed or sleeping body: no DOF on either side. + if self_frozen && other_slot == u32::MAX { + continue; + } + let (erp_inv_dt, cfm_factor) = if other_slot == u32::MAX { + (static_erp, static_cfm) + } else { + (dyn_erp, dyn_cfm) + }; + // Adjacent elements of a flat patch all report the shared vertex as a + // contact point: keep one row per (particle, pair) for those (the deepest), + // interior points are kept as they are. + let mut vertex_constraints: parry::utils::hashmap::HashMap = + Default::default(); + // A particle at a rigid feature (a box edge under a cloth) collects one + // row per adjacent element per feature, up to ~16, all speculative by a + // few mm: in the relax pass each releases a little slack toward the + // feature and the sum is a phantom velocity of the resting particle + // (undone by the next biased pass). Rows of the same rigid feature whose + // point is dominated by the same particle are merged into the deepest one. + let mut feature_constraints: parry::utils::hashmap::HashMap<(u32, u32), usize> = + Default::default(); + // The narrow phase localized the anchors in the parent bodies' CoM frames, + // except on the dominant side of a pair (world anchor, like a fixed body): + // the same rule (`relative_dominance`) recovers the world anchors. + let (rb1, rb2) = if surface_first { + (surface_rb, other_rb) + } else { + (other_rb, surface_rb) + }; + let anchor_poses = |manifold: &crate::geometry::ContactManifold| { + let rel_dom = manifold.data.relative_dominance; + ( + rb1.filter(|_| rel_dom <= 0).map(com_pose), + rb2.filter(|_| rel_dom >= 0).map(com_pose), + ) + }; + let localized = |anchor: Vector, pose: &Option| match pose { + Some(pose) => pose * anchor, + None => anchor, + }; + let manifold_element = |manifold: &crate::geometry::ContactManifold| { + let element_id = if surface_first { + manifold.subshape1 + } else { + manifold.subshape2 + } as usize; + (element_id < mesh.indices().len()) + .then(|| (element_id, mesh.element(element_id))) + }; + // An element's unused slots (a wire's third one) stay at the neutral + // values the constraints already use: no vertex, no weight. + let contact_geometry = + |element: &[u32], + manifold: &crate::geometry::ContactManifold, + sc: &crate::geometry::SolverContact| + -> ([Vector; DIM], [Real; DIM], Vector) { + let (pose1, pose2) = anchor_poses(manifold); + let (surface_anchor, other_anchor) = if surface_first { + (localized(sc.anchor1, &pose1), localized(sc.anchor2, &pose2)) + } else { + (localized(sc.anchor2, &pose2), localized(sc.anchor1, &pose1)) + }; + (positions, weights, other_anchor) + }; + // Contacts whose point lies inside the element (vertex/edge contacts have + // normals of their own). + let is_interior = |element: &[u32], weights: &[Real; DIM]| { + weights[..element.len()].iter().all(|w| *w > 0.02) + }; + // Normal approach speed of the other body (rigid ones only: soft particles + // count through their own body's speeds) toward the element's point. + let rigid_other = other_rb.filter(|rb| rb.soft_body().is_none()); + let approach_speed = |element: &[u32], + weights: &[Real; DIM], + anchor: Vector, + dir: Vector| { + let Some(rb) = rigid_other else { + return 0.0; + }; + let v_other = + rb.linvel() + rb.angvel().gcross(anchor - rb.center_of_mass()); + let mut v_surface = Vector::ZERO; + for (k, v) in element.iter().enumerate() { + v_surface += mesh.vertex_velocity(sb, *v as usize) * weights[k]; + } + (v_other - v_surface).gdot(dir).max(0.0) + }; + + // A closed surface's reversed interior contacts (a body that crossed it + // The rule reads the cells' boundary and its winding, which a skin is + // not: a skinned body collides plainly. + let skinned = mesh.is_skinned(); + let closed = mesh.is_closed() && self_particle.is_none() && !skinned; + + // The row tracks the barycentric point of the element: its reference + // The point the constraint tracks, expressed in the particles it acts through. + // ghost of that internal vertex/edge (the neighbor carries the + // once, their vertex contacts are real support. + } + + // The pair's predictive vertex contacts (see `SoftRigidVertexContact`): the + // support of the vertices a manifold leaves free when the rigid shape wraps + // the elements, unless a manifold constraint already holds the vertex. + let vertex_contacts = pair + .rigid() + .and_then(|r| r.soft.as_deref()) + .map_or(&[][..], |s| &s.vertices[..]); + let other_pose = other_co.position(); + held_vertices.sort_unstable(); + for (index, vc) in vertex_contacts.iter().enumerate() { + if held_vertices.binary_search(&vc.vertex).is_ok() { + continue; + } + // A vertex of the volume patch is left to the volume constraint. + if rigid_patch.as_ref().is_some_and(|(depths, _)| { + depths + .get(vc.vertex as usize) + .is_some_and(|d| *d > Real::NEG_INFINITY) + }) { + continue; + } + let Some(manifold) = pair.manifolds().get(vc.manifold as usize) else { + continue; + }; + let dir = other_pose.rotation * vc.local_dir; + // A closed surface's reversed contact (a body inside it) is dropped, like + // the manifold constraints'. + if closed + && manifold_element(manifold) + .and_then(|(id, _)| mesh.element_outward_normal(sb, id)) + .and_then(|n| n.try_normalize()) + .is_some_and(|n| dir.gdot(n) > 0.5) + { + continue; + } + let other_anchor = other_pose * vc.local_point; + let vertex_pos = mesh.vertex(sb, vc.vertex as usize); + let (anchors, anchor_weights, surface_point0) = + mesh.contact_anchors(sb, &[vc.vertex], &[1.0], vertex_pos); + let particles: [u32; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if anchors[k] == u32::MAX { + u32::MAX + } else { + slots[anchors[k] as usize] + } + }); + let im_particles: [Real; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if particles[k] == u32::MAX { + 0.0 + } else { + sb.particles[anchors[k] as usize].inv_mass + } + }); + let anchor_positions: [Vector; CONTACT_ANCHORS] = + core::array::from_fn(|k| { + if anchors[k] == u32::MAX { + Vector::ZERO + } else { + sb.particles[anchors[k] as usize].position + } + }); + let (body_local_point, body_arm) = match &other_com_pose { + Some(pose) => ( + pose.inverse_transform_point(other_anchor), + other_anchor - pose.translation, + ), + None => (other_anchor, Vector::ZERO), + }; + let tangents = SoftContact::tangent_basis(dir); + let warm_tangent: [Real; DIM - 1] = + core::array::from_fn(|k| vc.tangent_impulse.gdot(tangents[k])); + let mut weights = [0.0; DIM]; + weights[0] = 1.0; + out.max_approach_speed = out.max_approach_speed.max(approach_speed( + &[vc.vertex], + &weights, + other_anchor, + dir, + )); + out.contacts.push(SoftContact { + source: SoftContactSource { + collider1: pair.collider1, + collider2: pair.collider2, + manifold: SOURCE_RIGID_VERTEX, + point: index as u32, + slot: u32::MAX, + }, + support_body: ai as u32, + support_particle: anchors, + particles, + weights: anchor_weights, + im_particles, + frozen_pos: anchor_positions, + body: other_slot, + element: None, + other_body, + body_im: Vector::ZERO, + body_ii: Default::default(), + body_local_point, + body_arm, + surface_point0, + body_point0: other_anchor, + dir, + tangents, + dist0: vc.dist, + friction: manifold.data.friction, + soft_other: false, + erp_inv_dt, + cfm_factor, + max_bias: Real::MAX, + torque_dir: Default::default(), + ii_torque_dir: Default::default(), + r_normal: 0.0, + rhs_normal: 0.0, + cfm_normal: 1.0, + impulse_normal: vc.impulse, + impulse_normal_acc: -vc.impulse, + torque_tangent: [Default::default(); DIM - 1], + ii_torque_tangent: [Default::default(); DIM - 1], + r_tangent: [0.0; DIM - 1], + rhs_tangent: [0.0; DIM - 1], + impulse_tangent: warm_tangent, + impulse_tangent_acc: core::array::from_fn(|k| -warm_tangent[k]), + }); + } + } + // Self contacts (the body's own vertices and edges against its surface), then + // the edge-vs-edge constraints against the other soft surfaces met above. + let Some(surface_co) = colliders.get(surface_handle) else { + continue; + }; + if mesh.self_contacts && !self_frozen { + if let Some(detected) = narrow_phase.soft_self_contacts(surface_handle) { + // The tangle signal the narrow phase read off the surface (see + // `soft_contacts`), and the crossing sweep's travel bookkeeping. + out.mesh().crossing_sweep_travel = detected.crossing_sweep_travel_next; + out.tangled_elements.clone_from(&detected.tangled_elements); + out.tangled_vertices.clone_from(&detected.tangled_vertices); + out.crossings.clone_from(&detected.crossings); + self.assemble_vertex_contacts( + ctx, + ai, + Some(ai), + sb, + (surface_handle, surface_co), + (surface_handle, surface_co), + false, + &detected.vertex_pass, + None, + &detected.region_bins, + out, + ); + self.assemble_edge_contacts( + params, + ai, + Some(ai), + sb, + (surface_handle, surface_co), + (surface_handle, surface_co), + (dyn_erp, dyn_cfm), + detected.edges.as_ref(), + out, + ); + } + } + for (other_ai, other_sb_handle, other_surface, detected) in edge_tasks { + let (Some(other_sb), Some(other_co)) = ( + soft_bodies.get(other_sb_handle), + colliders.get(other_surface), + ) else { + continue; + }; + self.assemble_edge_contacts( + params, + ai, + other_ai, + other_sb, + (surface_handle, surface_co), + (other_surface, other_co), + (dyn_erp, dyn_cfm), + detected, + out, + ); + } + } + } + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs new file mode 100644 index 000000000..de0cf2b9f --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs @@ -0,0 +1,225 @@ +//! Edge-vs-edge contact constraints between two soft surfaces (or a surface and itself). + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + + +use super::{BodyContacts, SOURCE_EDGE_CONTACT, mesh_of, mesh_ref}; +use super::super::soft_constraints_set::SoftConstraintsSet; +use super::super::soft_contact::CONTACT_ANCHORS; +use super::super::soft_contact::{SoftContact, SoftContactElement, SoftContactSource}; +use crate::dynamics::soft_body::{SoftCollisionMesh, SoftEdgeContact}; +use crate::dynamics::{IntegrationParameters, SoftBody}; +use crate::geometry::soft_contacts::SoftEdgePass; +use crate::geometry::{Collider, ColliderHandle}; +use crate::math::{DIM, Real, Vector}; +use crate::utils::DotProduct; + +impl SoftConstraintsSet { + /// Edge-vs-edge constraints between the surfaces of awake body `ai` and `other` (itself for + /// self contacts; `other_ai` is its awake index, `None` when fully pinned and frozen), from the + /// candidates `detected` (`None` without an edge pass); complements the vertex-vs-surface pass. + #[allow(clippy::too_many_arguments)] + #[cfg_attr(feature = "dim2", allow(unused_variables))] + pub(super) fn assemble_edge_contacts( + &self, + params: &IntegrationParameters, + ai: usize, + other_ai: Option, + other: &SoftBody, + (surface_handle, surface_co): (ColliderHandle, &Collider), + (other_surface_handle, other_co): (ColliderHandle, &Collider), + (erp_inv_dt, cfm_factor): (Real, Real), + detected: Option<&SoftEdgePass>, + out: &mut BodyContacts, + ) { + let awake = &self.awake[ai]; + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + let (Some(mesh), Some(other_mesh)) = ( + mesh_of(sb, surface_handle), + mesh_of(other, other_surface_handle), + ) else { + return; + }; + let is_self = other_ai == Some(ai); + if mesh.is_closed() && other_mesh.is_closed() { + return; + } + let other_handle = mesh_ref(other_co); + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let other_slots = other_ai.map(|bi| { + let a = &self.awake[bi]; + &self.slots[a.slot_start..a.slot_start + a.num_particles] + }); + let friction = crate::dynamics::CoefficientCombineRule::combine( + surface_co.friction(), + other_co.friction(), + surface_co.friction_combine_rule(), + other_co.friction_combine_rule(), + ); + + let workspace = out; + workspace.previous.clear(); + for c in &mesh.edge_contacts { + if c.other == other_handle { + workspace.previous.insert( + (c.other, c.edge, c.other_edge), + (c.impulse, c.tangent_impulse), + ); + } + } + if detected.crossed { + // The pair is recovery-owned: the crossing guard leaves it alone. + let inv_mass_of = |body: &SoftBody, slots: Option<&[u32]>, v: u32| -> (u32, Real) { + match slots { + Some(slots) if slots[v as usize] != u32::MAX => { + (slots[v as usize], body.particles[v as usize].inv_mass) + } + _ => (u32::MAX, 0.0), + } + }; + + // The pass is oriented from its owner (see `soft_contacts`); consumed from the other + // side (an owner that is not simulated), its candidates are mirrored. + let mirrored = detected.own != surface_handle; + for (ci, c) in detected.candidates.iter().enumerate() { + // A candidate the contact-modification hook disabled gets no constraint. + if !c.enabled { + continue; + } + // The pair's contact slot of this candidate (none for a self contact). + let slot = if detected.slot_offset == u32::MAX { + u32::MAX + } else { + detected.slot_offset + ci as u32 + }; + let (ea, eb, ba, bb, point_a, point_b, dir) = if mirrored { + ( + c.other_edge, + c.edge, + c.other_bcoords, + c.bcoords, + c.other_point, + c.point, + -c.dir, + ) + } else { + (c.edge, c.other_edge, c.bcoords, c.other_bcoords, c.point, c.other_point, c.dir) + }; + let dist = c.dist; + let (va, vb) = (mesh.edge_vertices(ea), other_mesh.edge_vertices(eb)); + { + { + let tangents = SoftContact::tangent_basis(dir); + let (warm_normal, warm_tangent_world) = workspace + .previous + .get(&(other_handle, ea, eb)) + .copied() + .unwrap_or((0.0, Vector::ZERO)); + let warm_tangent: [Real; DIM - 1] = + core::array::from_fn(|k| warm_tangent_world.gdot(tangents[k])); + + // Each side's contact point acts through its particles: the + // edge's own two vertices, or the cell holding them under a skin. + let side = |body: &SoftBody, + body_mesh: &SoftCollisionMesh, + body_slots: Option<&[u32]>, + vertices: [u32; 2], + bcoords: [Real; 2], + point: Vector| { + let (anchors, weights, _) = + body_mesh.contact_anchors(body, &vertices, &bcoords, point); + let particles: [u32; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if anchors[k] == u32::MAX { + u32::MAX + } else { + inv_mass_of(body, body_slots, anchors[k]).0 + } + }); + let im_particles: [Real; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if anchors[k] == u32::MAX { + 0.0 + } else { + inv_mass_of(body, body_slots, anchors[k]).1 + } + }); + let frozen_pos: [Vector; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if anchors[k] == u32::MAX { + Vector::ZERO + } else { + body.particles[anchors[k] as usize].position + } + }); + (anchors, particles, weights, im_particles, frozen_pos) + }; + let (support_particle, particles, weights, im_particles, frozen_pos) = + side(sb, mesh, Some(slots), va, ba, point_a); + let (_, particles2, weights2, im_particles2, frozen_pos2) = + side(other, other_mesh, other_slots, vb, bb, point_b); + let assembled = &mut workspace.meshes[current_mesh]; + let point = assembled.edge_contacts.len() as u32; + assembled.edge_contacts.push(SoftEdgeContact { + other: other_handle, + edge: ea, + other_edge: eb, + impulse: warm_normal, + tangent_impulse: warm_tangent_world, + }); + let contact = SoftContact { + source: SoftContactSource { + collider1: surface_handle, + collider2: other_surface_handle, + manifold: SOURCE_EDGE_CONTACT, + point, + slot, + }, + support_body: ai as u32, + support_particle, + particles, + weights, + im_particles, + frozen_pos, + body: u32::MAX, + element: Some(SoftContactElement { + particles: particles2, + weights: weights2, + im_particles: im_particles2, + frozen_pos: frozen_pos2, + }), + other_body: other_ai.map_or(u32::MAX, |bi| bi as u32), + body_im: Vector::ZERO, + body_ii: Default::default(), + body_local_point: point_b, + body_arm: Vector::ZERO, + surface_point0: point_a, + body_point0: point_b, + dir, + tangents, + dist0: dist, + friction, + soft_other: true, + erp_inv_dt, + cfm_factor, + max_bias: Real::MAX, + torque_dir: Default::default(), + ii_torque_dir: Default::default(), + r_normal: 0.0, + rhs_normal: 0.0, + cfm_normal: 1.0, + impulse_normal: warm_normal, + impulse_normal_acc: -warm_normal, + torque_tangent: [Default::default(); DIM - 1], + ii_torque_tangent: [Default::default(); DIM - 1], + r_tangent: [0.0; DIM - 1], + rhs_tangent: [0.0; DIM - 1], + impulse_tangent: warm_tangent, + impulse_tangent_acc: core::array::from_fn(|k| -warm_tangent[k]), + }; + workspace.contacts.push(contact); + } + } + } + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs new file mode 100644 index 000000000..b83abb550 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs @@ -0,0 +1,8 @@ + /// Vertex-vs-surface contact rows: the surface vertices of `other` (itself for self + /// simulated) against the surface of awake body `ai`, one row per (vertex, element) the + /// the other's surface, for an other body that is not in the awake list). A row holds the + // already pass through each other, the pair's keep-apart rows are wrong-sided by + // the self stand-down opened), so they stand down around it, like the self rows do + // boundary, the particles of the cell carrying it when it collides through a skin. + // A candidate the contact-modification hook disabled gets no constraint. + // would pin the intruder inside; the volume rows and the elasticity diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs new file mode 100644 index 000000000..35d97d8d8 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs @@ -0,0 +1,74 @@ +//! Workspace and context of the contact assembly: the per-body and per-mesh contact state kept across steps, the step-constant inputs and the small helpers the passes share. + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::alloc_prelude::*; +use parry::utils::hashmap::HashMap; + +use super::super::soft_contact::SoftContact; +use crate::dynamics::soft_body::{ + SoftCollisionMesh, SoftEdgeContact, SoftMeshId, SoftMeshRef, SoftVertexContact, +}; +use crate::dynamics::{IntegrationParameters, RigidBodySet, SoftBody, SoftBodyHandle, SoftBodySet}; +use crate::geometry::{Collider, ColliderHandle, ColliderSet, NarrowPhase}; +use crate::dynamics::soft_body::{SoftOverlapState, SoftVolumeContact}; +use crate::math::{AngVector, Real, Rotation, Vector}; + +/// The collision mesh a soft body's collider carries. +pub(super) fn mesh_of(sb: &SoftBody, collider: ColliderHandle) -> Option<&SoftCollisionMesh> { + sb.mesh_of(collider) +} + +pub(super) fn mesh_ref(co: &Collider) -> SoftMeshRef { + co.deformable_mesh_ref.expect("not a deformable collider") +} + +/// Scratch of the contact assembly kept across steps. +#[derive(Default)] +pub(crate) struct EdgeContactWorkspace { + /// Awake index of every awake soft body, by handle. + pub(super) awake_of: HashMap, + /// The rows and new edge contacts of every awake body (assembled in parallel, appended in + /// awake order). + pub(super) per_body: Vec, +} + +/// `SoftContactSource::manifold` of the rows without a manifold point: an edge-vs-edge row (its +/// warm start lives in the owner's `edge_contacts`) or a vertex-vs-surface row (`vertex_contacts`). +pub(super) const SOURCE_EDGE_CONTACT: u32 = u32::MAX; +pub(super) const SOURCE_VERTEX_CONTACT: u32 = u32::MAX - 1; +/// The contact rows of one awake soft body, and its new edge-vs-edge and vertex-vs-surface +/// contacts (the rows' `point` indices are positions in those lists). +#[derive(Default)] +pub(super) struct BodyContacts { + pub(super) contacts: Vec, + /// The contact state of every mesh assembled this step, in the order the body's meshes were + /// visited (a row's `point` indexes its own mesh's lists). + pub(super) meshes: Vec, + /// The mesh being assembled. + pub(super) current_mesh: usize, + /// Largest normal approach speed of the rigid bodies met (the impact-adaptive substeps). + pub(super) max_approach_speed: Real, + /// The owner's edge contacts of the last step, by (other mesh, edge, other edge). + pub(super) previous: HashMap<(SoftMeshRef, u32, u32), (Real, Vector)>, + /// The owner's vertex contacts of the last step with the current other body (and + /// vertex pass), by (vertex, element). + pub(super) previous_vertex: HashMap<(u32, u32), (Real, Vector)>, +} +impl BodyContacts { +} +/// The step-constant inputs of the per-body contact assembly. +pub(super) struct AssemblyCtx<'a> { + pub(super) island_id: usize, + /// The first group's substep parameters (the constraints' softness). + pub(super) params: &'a IntegrationParameters, + pub(super) narrow_phase: &'a NarrowPhase, + pub(super) colliders: &'a ColliderSet, + pub(super) bodies: &'a RigidBodySet, + pub(super) soft_bodies: &'a SoftBodySet, + /// Dynamic and static contact softness `(erp_inv_dt, cfm_factor)`. + pub(super) dyn_soft: (Real, Real), + pub(super) static_soft: (Real, Real), +} diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs new file mode 100644 index 000000000..87de53205 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs @@ -0,0 +1,139 @@ +//! Write-back of the soft contact constraints' impulses to their narrow-phase manifold points and volume warm starts. + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::alloc_prelude::*; + +use super::{SOURCE_EDGE_CONTACT, SOURCE_VERTEX_CONTACT}; +use super::super::soft_constraints_set::SoftConstraintsSet; +use super::super::soft_contact::SoftContact; +use crate::dynamics::soft_body::SoftOverlapWarm; +use crate::geometry::NarrowPhase; +use crate::math::{AngVector, DIM, Vector}; + +/// Adds a constraint's impulses to its manifold point (the point's impulses are cleared +/// before its pair's constraints report; a point split into endpoint constraints sums them): +/// the step's total for the events, the last substep's for the warm start. +fn report_impulses(data: &mut crate::geometry::ContactData, c: &SoftContact) { + #[cfg(feature = "dim2")] + { + data.tangent_impulse[0] = crate::utils::canonicalize_zero( + data.tangent_impulse[0] + c.impulse_tangent_acc[0] + c.impulse_tangent[0], + ); + data.warmstart_tangent_impulse[0] = crate::utils::canonicalize_zero( + data.warmstart_tangent_impulse[0] + c.impulse_tangent[0], + ); + } + #[cfg(feature = "dim3")] + { + for j in 0..2 { + data.tangent_impulse[j] = crate::utils::canonicalize_zero( + data.tangent_impulse[j] + c.impulse_tangent_acc[j] + c.impulse_tangent[j], + ); + data.warmstart_tangent_impulse[j] = crate::utils::canonicalize_zero( + data.warmstart_tangent_impulse[j] + c.impulse_tangent[j], + ); + } + data.warmstart_tangent_world = crate::utils::canonicalize_zero( + + c.tangents[0] * c.impulse_tangent[0] + + c.tangents[1] * c.impulse_tangent[1], + ); + } +} + +impl SoftConstraintsSet { + /// Writes the soft contacts' impulses back to their narrow-phase manifold points (total + /// impulse for the contact events, warm-start impulses for the next step). + pub fn writeback_contacts(&self, narrow_phase: &mut NarrowPhase) { + // The constraints of a pair are contiguous: on entering a new pair, clear the reported impulses + // of all its points first (points without a constraint this step, e.g. deduplicated vertex + // contacts, must not keep reporting a stale force). + let mut prev_pair = None; + for c in &self.contacts { + { + // The load borne by the surface (its extra substeps under a heavy pile). + // SAFETY: the writeback stage is serial and owns the soft bodies. + let ptr = self.awake[c.support_body as usize].ptr; + let sb = unsafe { &mut *ptr }; + sb.contact_load += (c.impulse_normal_acc + c.impulse_normal).abs(); + } + if c.source.manifold == SOURCE_EDGE_CONTACT + || c.source.manifold == SOURCE_VERTEX_CONTACT + { + // An edge-vs-edge or vertex-vs-surface constraint: its warm start lives on the owner + // soft body. + // SAFETY: the writeback stage is serial and owns the soft bodies. + let ptr = self.awake[c.support_body as usize].ptr; + let sb = unsafe { &mut *ptr }; + // The constraint's first collider is the mesh that owns its warm start. + let Some(mesh) = sb.mesh_of_mut(c.source.collider1) else { + continue; + }; + let normal = crate::utils::canonicalize_zero(c.impulse_normal); + let mut tangent = Vector::ZERO; + for k in 0..DIM - 1 { + tangent += c.tangents[k] * c.impulse_tangent[k]; + } + let tangent = crate::utils::canonicalize_zero(tangent); + if c.source.manifold == SOURCE_EDGE_CONTACT { + if let Some(edge) = mesh.edge_contacts.get_mut(c.source.point as usize) { + edge.impulse = normal; + edge.tangent_impulse = tangent; + } + } else if let Some(vertex) = mesh.vertex_contacts.get_mut(c.source.point as usize) { + vertex.impulse = normal; + vertex.tangent_impulse = tangent; + } + // The pair's contact slot of the candidate (rebuilt from scratch by every + // narrow-phase update, so nothing stale to clear) reports the impulse. + if c.source.slot != u32::MAX { + if let Some(soft) = narrow_phase + .contact_pair_mut(c.source.collider1, c.source.collider2) + .and_then(|pair| pair.soft_mut()) + { + soft.report_impulse( + c.source.slot, + crate::utils::canonicalize_zero( + c.impulse_normal_acc + c.impulse_normal, + ), + crate::utils::canonicalize_zero( + c.impulse_tangent_acc + .iter() + .zip(&c.impulse_tangent) + .enumerate() + .map(|(k, (a, i))| c.tangents[k] * (a + i)) + .sum::(), + ), + ); + } + } + continue; + } + let Some(rigid) = narrow_phase + .contact_pair_mut(c.source.collider1, c.source.collider2) + .and_then(|pair| pair.rigid_mut()) + else { + continue; + }; + let key = (c.source.collider1, c.source.collider2); + if prev_pair != Some(key) { + prev_pair = Some(key); + for manifold in &mut rigid.manifolds { + for point in &mut manifold.points { + point.data.impulse = 0.0; + point.data.tangent_impulse = Default::default(); + } + } + } + let Some(manifold) = rigid.manifolds.get_mut(c.source.manifold as usize) else { + continue; + }; + let Some(point) = manifold.points.get_mut(c.source.point as usize) else { + continue; + }; + report_impulses(&mut point.data, c); + } + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs b/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs new file mode 100644 index 000000000..a454a5d94 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs @@ -0,0 +1,364 @@ +//! Chunking of the soft contact constraints for the parallel contact stages: constraints are +//! grouped by the pair of bodies they hold and the chunks colored so that one color shares no +//! solver body (one stage per color, uncolored chunks in the serial tail on worker 0). + +use crate::alloc_prelude::*; +use parry::utils::hashmap::HashMap; + +use super::soft_constraints_set::SoftConstraintsSet; +use super::soft_contact::SoftContact; +use crate::dynamics::IntegrationParameters; +use crate::dynamics::solver::solver_body::SolverBodies; + +/// Colors tried before a chunk goes to the serial tail. +const MAX_CONTACT_COLORS: usize = 16; +/// A chunk against the world (one soft body, no rigid solver body) holding at least this many +/// constraints is split by particle colors into pieces (see [`PIECE_LEN`]), so a big cloth on the +/// ground does not sit entirely in the serial tail. +const SPLIT_MIN: usize = 128; +/// Constraints per piece of a split chunk (the claim granularity of its parallel stages). +const PIECE_LEN: usize = 32; +/// Particle colors tried before a constraint of a split chunk stays in the serial chunk. +const MAX_PARTICLE_COLORS: usize = 16; + +/// The bodies a chunk holds: two soft bodies (`u32::MAX`: none) and a rigid solver slot +/// (`u32::MAX`: none or the world). +#[derive(Copy, Clone, PartialEq, Eq, Hash)] +struct ChunkKey { + soft: [u32; 2], + rigid: u32, +} + +impl ChunkKey { + fn of(c: &SoftContact) -> Self { + let (soft, rigid) = if c.other_body != u32::MAX { + ( + [ + c.support_body.min(c.other_body), + c.support_body.max(c.other_body), + ], + u32::MAX, + ) + } else if c.element.is_none() && !c.soft_other { + ([c.support_body, u32::MAX], c.body) + } else { + // A soft side without a solver DOF (asleep, or frozen and not in the awake list). + ([c.support_body, u32::MAX], u32::MAX) + }; + Self { soft, rigid } + } +} + +/// The solver slots written by the chunks of one color so far. +#[derive(Default)] +struct ColorSlots { + written: Vec, +} + +impl ColorSlots { + fn clear(&mut self, num_slots: usize) { + self.written.clear(); + self.written.resize(num_slots, false); + } + + /// Whether a chunk writing `slots` shares none of them with the color's chunks. + fn accepts(&self, mut slots: impl Iterator) -> bool { + slots.all(|slot| !self.written[slot as usize]) + } + + fn insert(&mut self, slots: impl Iterator) { + for slot in slots { + self.written[slot as usize] = true; + } + } +} + +/// The solver slots a contact constraint writes: its anchors, the other side's slot (a rigid +/// body, or a particle of another soft body) or element anchors, and every particle of a FEM +/// side's body (the response spans the body). +fn written_slots(c: &SoftContact) -> impl Iterator + '_ { + let anchors = c + .particles + .iter() + .chain(core::iter::once(&c.body)) + .chain(c.element.iter().flat_map(|e| e.particles.iter())) + .copied() + .filter(|&slot| slot != u32::MAX); + let fem = c + .fem + .iter() + .flatten() + .flat_map(|side| side.first_slot..side.first_slot + side.num_particles); + anchors.chain(fem) +} + +/// Workspace of the contact chunking kept across steps. +#[derive(Default)] +pub(crate) struct ContactChunkWorkspace { + chunk_of_key: HashMap, + /// Chunk of every constraint of the group. + constraint_chunk: Vec, + /// Per chunk: its key, its row count, its first row (cursor of the counting sort). + keys: Vec, + counts: Vec, + cursors: Vec, + color_slots: Vec, + /// Per color: its chunks, in chunk order (the last entry: the uncolored chunks). + color_chunks: Vec>, + /// The chunks in layout order. + ordered: Vec, + /// The constraints of every chunk (CSR over `chunk_rows`), for the coloring. + chunk_row_offsets: Vec, + chunk_rows: Vec, + /// Per solver slot: the particle colors of the split chunk at hand touching it. + slot_masks: Vec, +} + +impl SoftConstraintsSet { + /// Builds the chunk layout of every group's contact constraints (see the module doc): the chunks of + /// the parallel colors first, in color order, then the serial tail. Colors with a single + /// chunk go to the tail as well (a stage of one chunk is as serial, without the barrier). + pub(super) fn chunk_contacts(&mut self) { + self.contact_chunk_constraints.clear(); + self.contact_chunks.clear(); + self.contact_colors.clear(); + let mut workspace = core::mem::take(&mut self.contact_workspace); + for gi in 0..self.groups.len() { + let constraints = self.groups[gi].contacts.clone(); + let colors_start = self.contact_colors.len(); + let chunks_start = self.contact_chunks.len(); + if constraints.is_empty() { + let g = &mut self.groups[gi]; + g.contact_colors = colors_start..colors_start; + g.contact_serial = chunks_start..chunks_start; + continue; + } + // Chunks by first appearance (deterministic: the constraints are in assembly order); the + // constraints of a pair are contiguous, so the map is rarely consulted. + workspace.chunk_of_key.clear(); + workspace.keys.clear(); + workspace.counts.clear(); + workspace.constraint_chunk.clear(); + let mut last: Option<(ChunkKey, u32)> = None; + for c in &self.contacts[constraints.clone()] { + let key = ChunkKey::of(c); + let chunk = match last { + Some((k, chunk)) if k == key => chunk, + _ => { + let next = workspace.keys.len() as u32; + let chunk = *workspace.chunk_of_key.entry(key).or_insert(next); + if chunk == next { + workspace.keys.push(key); + workspace.counts.push(0); + } + last = Some((key, chunk)); + chunk + } + }; + workspace.counts[chunk as usize] += 1; + workspace.constraint_chunk.push(chunk); + } + // Particle-colored pieces of the large chunks against the world (see `SPLIT_MIN`): a + // constraint goes to the first particle color none of its particles has; one with a + // FEM side or beyond the colors stays in the original chunk. Pieces are chunks too. + let mut num_slots = 0usize; + for c in &self.contacts[constraints.clone()] { + for slot in written_slots(c) { + num_slots = num_slots.max(slot as usize + 1); + } + } + let plain_chunks = workspace.keys.len(); + for chunk in 0..plain_chunks { + let key = workspace.keys[chunk]; + if key.rigid != u32::MAX + || key.soft[1] != u32::MAX + || workspace.counts[chunk] < SPLIT_MIN + { + continue; + } + workspace.slot_masks.clear(); + workspace.slot_masks.resize(num_slots, 0); + // The current piece of every particle color, and its fill. + let mut pieces = [(usize::MAX, 0usize); MAX_PARTICLE_COLORS]; + for (i, c) in self.contacts[constraints.clone()].iter().enumerate() { + if workspace.constraint_chunk[i] as usize != chunk { + continue; + } + if c.fem.iter().any(|side| side.is_some()) { + continue; + } + let mut mask = 0u16; + for slot in written_slots(c) { + mask |= workspace.slot_masks[slot as usize]; + } + let color = mask.trailing_ones() as usize; + if color >= MAX_PARTICLE_COLORS { + continue; + } + for slot in written_slots(c) { + workspace.slot_masks[slot as usize] |= 1 << color; + } + let (piece, fill) = &mut pieces[color]; + if *fill == PIECE_LEN || *piece == usize::MAX { + *piece = workspace.keys.len(); + *fill = 0; + workspace.keys.push(key); + workspace.counts.push(0); + } + *fill += 1; + workspace.constraint_chunk[i] = *piece as u32; + workspace.counts[*piece] += 1; + workspace.counts[chunk] -= 1; + } + } + let num_chunks = workspace.keys.len(); + + // The constraints of every chunk (counting sort, stable). + workspace.chunk_row_offsets.clear(); + workspace.chunk_row_offsets.resize(num_chunks + 1, 0); + for &chunk in &workspace.constraint_chunk { + workspace.chunk_row_offsets[chunk as usize + 1] += 1; + } + for chunk in 0..num_chunks { + workspace.chunk_row_offsets[chunk + 1] += workspace.chunk_row_offsets[chunk]; + } + workspace.cursors.clear(); + workspace + .cursors + .extend_from_slice(&workspace.chunk_row_offsets[..num_chunks]); + workspace.chunk_rows.clear(); + workspace.chunk_rows.resize(constraints.len(), 0); + for (i, &chunk) in workspace.constraint_chunk.iter().enumerate() { + let slot = &mut workspace.cursors[chunk as usize]; + workspace.chunk_rows[*slot as usize] = i as u32; + *slot += 1; + } + + // Greedy coloring: the first color whose chunks write none of this chunk's solver + // slots (its rigid slot, its constraints' particles). Chunks of one color are then + // solved concurrently. + workspace + .color_slots + .resize_with(MAX_CONTACT_COLORS, Default::default); + for slots in &mut workspace.color_slots { + slots.clear(num_slots); + } + for color_chunks in &mut workspace.color_chunks { + color_chunks.clear(); + } + workspace + .color_chunks + .resize_with(MAX_CONTACT_COLORS + 1, Default::default); + for chunk in 0..num_chunks { + let rows = &workspace.chunk_rows[workspace.chunk_row_offsets[chunk] as usize + ..workspace.chunk_row_offsets[chunk + 1] as usize]; + let contacts = &self.contacts[constraints.clone()]; + let slots = || rows.iter().flat_map(|&i| written_slots(&contacts[i as usize])); + let color = workspace + .color_slots + .iter() + .position(|color| color.accepts(slots())) + .unwrap_or(MAX_CONTACT_COLORS); + if color < MAX_CONTACT_COLORS { + workspace.color_slots[color].insert(slots()); + } + workspace.color_chunks[color].push(chunk); + } + + // Layout: chunk id -> position, parallel colors (two chunks at least) then the tail + // (single-chunk colors in color order, then the overflow). + let ordered = &mut workspace.ordered; + ordered.clear(); + for color_chunks in &workspace.color_chunks[..MAX_CONTACT_COLORS] { + if color_chunks.len() < 2 { + continue; + } + let start = chunks_start + ordered.len(); + ordered.extend_from_slice(color_chunks); + self.contact_colors + .push(start..chunks_start + ordered.len()); + } + let serial_start = chunks_start + ordered.len(); + for color_chunks in &workspace.color_chunks[..MAX_CONTACT_COLORS] { + if color_chunks.len() == 1 { + ordered.push(color_chunks[0]); + } + } + ordered.extend_from_slice(&workspace.color_chunks[MAX_CONTACT_COLORS]); + debug_assert_eq!(ordered.len(), num_chunks); + + // Constraint ranges of the chunks in layout order, then the constraints (counting sort, stable). + let mut cursor = self.contact_chunk_constraints.len(); + workspace.cursors.clear(); + workspace.cursors.resize(num_chunks, 0); + for &chunk in ordered.iter() { + let count = workspace.counts[chunk]; + workspace.cursors[chunk] = cursor as u32; + self.contact_chunks.push(cursor..cursor + count); + cursor += count; + } + self.contact_chunk_constraints.resize(cursor, 0); + for (i, &chunk) in workspace.constraint_chunk.iter().enumerate() { + let slot = &mut workspace.cursors[chunk as usize]; + self.contact_chunk_constraints[*slot as usize] = (constraints.start + i) as u32; + *slot += 1; + } + + let g = &mut self.groups[gi]; + g.contact_colors = colors_start..self.contact_colors.len(); + g.contact_serial = serial_start..self.contact_chunks.len(); + // Every parallel color must be slot-disjoint: two chunks of a color writing one + // solver body would race on its velocity. + #[cfg(debug_assertions)] + for color in &self.contact_colors[g.contact_colors.clone()] { + let mut seen = vec![usize::MAX; num_slots]; + for chunk in color.clone() { + for &row in &self.contact_chunk_constraints[self.contact_chunks[chunk].clone()] { + let c = &self.contacts[row as usize]; + for slot in written_slots(c) { + let prev = core::mem::replace(&mut seen[slot as usize], chunk); + if prev != usize::MAX && prev != chunk { + let key = |ch: usize| workspace.keys[workspace.ordered[ch - chunks_start]]; + let (k1, k2) = (key(prev), key(chunk)); + panic!( + "slot {slot} shared: chunk {prev} (soft {:?} rigid {}) and chunk {chunk} (soft {:?} rigid {}); row body {} element {} soft_other {} fem {:?} particles {:?}", + k1.soft, k1.rigid, k2.soft, k2.rigid, c.body, c.element.is_some(), c.soft_other, + c.fem.iter().map(|f| f.is_some()).collect::>(), c.particles + ); + } + } + } + } + } + } + self.contact_workspace = workspace; + } + + /// Solves the constraints of one contact chunk: update, optional warm start, one Gauss-Seidel + /// sweep. Chunks of one color hold pairwise-disjoint solver bodies, so they are solved + /// concurrently. + #[inline] + pub fn solve_contact_chunk( + &mut self, + chunk: usize, + bodies: &mut SolverBodies, + params: &IntegrationParameters, + wo_bias: bool, + update: bool, + warmstart: bool, + ) { + for &row in &self.contact_chunk_rows[self.contact_chunks[chunk].clone()] { + let c = &mut self.contacts[row as usize]; + if wo_bias && c.soft_other && c.dist0 > 0.0 { + continue; + } + if update { + c.update(bodies, params, wo_bias); + } + if warmstart { + c.warmstart(bodies); + } + c.solve(bodies); + } + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/mod.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/mod.rs new file mode 100644 index 000000000..a4f3597c3 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/mod.rs @@ -0,0 +1,14 @@ +//! The scalar N-particle constraint shared by the soft-body element kinds (distance, +//! dihedral bending, cell volume), and the block constraint of the corotational elastic cells. + +mod soft_elastic_constraint; +mod soft_element_linalg; +mod soft_neo_hookean; +mod soft_scalar_constraint; +#[cfg(test)] +mod test; + +pub(crate) use self::soft_elastic_constraint::*; +pub(crate) use self::soft_element_linalg::*; +pub(crate) use self::soft_neo_hookean::*; +pub(crate) use self::soft_scalar_constraint::*; diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs new file mode 100644 index 000000000..276e5aea6 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs @@ -0,0 +1,472 @@ +//! The elastic-cell block constraint: strain blocks, their inverse, the per-substep update, warm start and the corotational and Neo-Hookean solves. + +use crate::dynamics::SoftBody; +use crate::dynamics::soft_body::soft_body_shape_matching::extract_rotation; +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::math::{DIM, Matrix, Real, Rotation, Vector}; +use crate::utils::{DotProduct, RotationOps}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; +use super::*; + +/// Number of strain rows of an elastic cell: the independent components of the symmetric +/// strain tensor. +pub(crate) const STRAIN_ROWS: usize = DIM * (DIM + 1) / 2; + +/// One value per strain row (a strain, its rate, or the strain impulses). +pub(crate) type StrainVector = na::SVector; +/// A `STRAIN_ROWS × STRAIN_ROWS` matrix over the strain rows. +pub(crate) type StrainMatrix = na::SMatrix; +/// The strain-row gradients of one particle, as the columns of a `DIM × STRAIN_ROWS` matrix. +#[cfg(feature = "fem")] +pub(crate) type StrainJacobian = na::SMatrix; + +/// The `(a, b)` tensor indices of the strain rows: the diagonal first, then `a < b`. +#[inline] +pub(crate) const fn strain_pairs() -> [(usize, usize); STRAIN_ROWS] { + #[cfg(feature = "dim2")] + { + [(0, 0), (1, 1), (0, 1)] + } + #[cfg(feature = "dim3")] + { + [(0, 0), (1, 1), (2, 2), (0, 1), (0, 2), (1, 2)] + } +} + +/// The strain-constraint coordinates of the symmetric part of `m`: `½ (m_ab + m_ba)` for the constraint +/// `(a, b)` (in [`strain_pairs`] order). +#[inline] +pub(crate) fn strain_rows_of(m: &Matrix) -> StrainVector { + let (x, y) = (m.x_axis, m.y_axis); + #[cfg(feature = "dim2")] + { + StrainVector::new(x.x, y.y, 0.5 * (y.x + x.y)) + } + #[cfg(feature = "dim3")] + { + let z = m.z_axis; + StrainVector::new( + x.x, + y.y, + z.z, + 0.5 * (y.x + x.y), + 0.5 * (z.x + x.z), + 0.5 * (z.y + y.z), + ) + } +} + +/// The symmetric tensor `T` with `T coeffs[p] = Σᵣ gᵣ[p] λᵣ`: `λᵣ` on the diagonal rows, `½ λᵣ` +/// on both entries of the off-diagonal ones. +#[inline] +fn strain_tensor(lambda: &StrainVector) -> Matrix { + #[cfg(feature = "dim2")] + { + let h = 0.5 * lambda.z; + Matrix::from_cols(Vector::new(lambda.x, h), Vector::new(h, lambda.y)) + } + #[cfg(feature = "dim3")] + { + let (h01, h02, h12) = (0.5 * lambda.w, 0.5 * lambda.a, 0.5 * lambda.b); + Matrix::from_cols( + Vector::new(lambda.x, h01, h02), + Vector::new(h01, lambda.y, h12), + Vector::new(h02, h12, lambda.z), + ) + } +} + +/// The symmetric matrix of a strain in strain-constraint coordinates: `ε_ab = ε_ba = εᵣ` for +/// the constraint `(a, b)`. +#[inline] +pub(crate) fn strain_matrix(strain: &StrainVector) -> Matrix { + let e = strain; + #[cfg(feature = "dim2")] + { + Matrix::from_cols(Vector::new(e.x, e.z), Vector::new(e.z, e.y)) + } + #[cfg(feature = "dim3")] + { + Matrix::from_cols( + Vector::new(e.x, e.w, e.a), + Vector::new(e.w, e.y, e.b), + Vector::new(e.a, e.b, e.z), + ) + } +} + +/// The strain-row coordinates of `ε ↦ m : ε` for a symmetric `m`: `m_aa` on the diagonal rows, +/// `m_ab + m_ba` on the off-diagonal ones (a shear constraint is two entries of `ε`). +#[inline] +pub(crate) fn strain_rows_dual(m: &Matrix) -> StrainVector { + let (x, y) = (m.x_axis, m.y_axis); + #[cfg(feature = "dim2")] + { + StrainVector::new(x.x, y.y, y.x + x.y) + } + #[cfg(feature = "dim3")] + { + let z = m.z_axis; + StrainVector::new(x.x, y.y, z.z, y.x + x.y, z.x + x.z, z.y + y.z) + } +} + +/// `Σₚ aₚ bₚᵀ`: with `a` the particle velocities and `b` the cell coefficients, the velocity +/// gradient of the cell, whose strain-constraint coordinates are the strain rates. +#[inline] +pub(crate) fn outer_sum( + a: &[Vector; MAX_CONSTRAINT_PARTICLES], + b: &[Vector; MAX_CONSTRAINT_PARTICLES], +) -> Matrix { + let mut m = Matrix::ZERO; + for j in 0..DIM { + let mut col = Vector::ZERO; + for p in 0..MAX_CONSTRAINT_PARTICLES { + col += a[p] * b[p][j]; + } + *m.col_mut(j) = col; + } + m +} + +/// The elastic constraints of a cell, solved together as one block: the independent components of +/// the corotated strain `ε = sym(RᵀF) - I` (`R` from the polar decomposition of `F`, extracted +/// before every solve), with the corotational or Neo-Hookean model of [`SoftElasticModel`]. +#[derive(Copy, Clone, Debug)] +pub(crate) struct SoftElasticConstraint { + pub soft_body: u32, + pub element: u32, + pub solver_ids: [u32; MAX_CONSTRAINT_PARTICLES], + pub pos: [Vector; MAX_CONSTRAINT_PARTICLES], + pub im: [Real; MAX_CONSTRAINT_PARTICLES], + /// `∂F_cj/∂(x_a)_c = coeffs[a][j]` (constraints of `Dm⁻¹`, particle 0 minus their sum). + pub coeffs: [Vector; MAX_CONSTRAINT_PARTICLES], + pub inv_rest_matrix: Matrix, + /// Rotation of the polar decomposition of `F` (warm-started from the previous pass). + pub rotation: Rotation, + /// `rotation` as a matrix, refreshed with it at every update: the solves read it. + pub rot_mat: Matrix, + /// Corotational: inverse of the softened strain effective-mass block `(A + CFM)⁻¹`. + /// Neo-Hookean: `(M A + I)⁻¹` with `M = dt (dt K + D)` (see [`NeoHookeanConstraint`]). + pub inv_a: StrainMatrix, + /// Per-constraint cap on the strain error fed to the bias while the cell is inverted + /// (`det F < 0`); non-inverted cells are clamped at 100% strain or this cap when it is larger. + pub strain_cap: StrainVector, + /// Strain of the last update (cell frame, clamped). + pub strain: StrainVector, + /// Strain impulses (cell frame). + pub strain_impulse: StrainVector, + /// Volumetric impulse (corotational only). + pub vol_impulse: Real, + pub model: SoftElasticModel, +} + +/// The constitutive model of a [`SoftElasticRow`] and its model-specific state. +#[derive(Copy, Clone, Debug)] +pub(crate) enum SoftElasticModel { + Corotational(CorotationalConstraint), + NeoHookean(NeoHookeanConstraint), +} + +/// Corotational state of a [`SoftElasticRow`]: constant per-row spring coefficients and the +/// volumetric row solved through a Schur complement of the strain block. +#[derive(Copy, Clone, Debug)] +pub(crate) struct CorotationalConstraint { + pub erp_strain: StrainVector, + pub cfm_strain: StrainVector, + pub erp_vol: Real, + pub cfm_coeff_vol: Real, + /// Whether the volumetric row is active (a zero Poisson ratio disables it). + pub volumetric: bool, + /// Snap-back cap of the volumetric row (see `SoftElasticConstraint::strain_cap`). + pub vol_cap: Real, + // Per-pass state. + pub grad_vol: [Vector; MAX_CONSTRAINT_PARTICLES], + pub cfm_gain_vol: Real, + pub rhs_vol: Real, + /// Coupling of the volumetric row with the strain rows, and its image through `inv_a`. + pub b: StrainVector, + pub z: StrainVector, + pub inv_s: Real, +} + +impl SoftElasticConstraint { + /// Whether a strain component (as last updated) exceeds `min_strain`: the criterion of the + /// re-sweep after the contacts. + #[inline] + pub fn is_strained(&self, min_strain: Real) -> bool { + self.strain.iter().any(|s| s.abs() > min_strain) + } + + pub fn max_tensile_strain(&self) -> Real { + /// Deviatoric coefficients of a cell from its inverse rest matrix. + pub fn coefficients(inv_rest_matrix: &Matrix) -> [Vector; MAX_CONSTRAINT_PARTICLES] { + let constraints = inv_rest_matrix.transpose(); + let mut coeffs = [Vector::ZERO; MAX_CONSTRAINT_PARTICLES]; + for a in 0..DIM { + coeffs[a + 1] = constraints.col(a); + coeffs[0] -= coeffs[a + 1]; + } + coeffs + } + + /// Gradient of strain row `r` w.r.t. particle `p`'s velocity, in the cell frame: + /// `½ (coeffs[p][b] e_a + coeffs[p][a] e_b)`. + #[inline] + } + + /// The strain-constraint gradients of particle `p` rotated by `rotation`: the columns `R gᵣ[p]`. + #[cfg(feature = "fem")] + #[inline] + pub(crate) fn strain_jacobian( + coeffs: &[Vector; MAX_CONSTRAINT_PARTICLES], + p: usize, + rotation: &Matrix, + ) -> StrainJacobian { + let mut jacobian = StrainJacobian::zeros(); + for r in 0..STRAIN_ROWS { + let column = *rotation * Self::strain_gradient(coeffs, r, p); + jacobian.set_column(r, &na::SVector::::from(column)); + } + jacobian + } + + /// The strain effective-mass block `A_rs = Σₚ wₚ gᵣ[p]·gₛ[p]`, in closed form from + /// `G = Σₚ wₚ cₚ cₚᵀ` (see [`NeoHookeanConstraint::strain_block_from_g`]). + pub fn strain_block(coeffs: &[Vector; MAX_CONSTRAINT_PARTICLES], im: &[Real]) -> StrainMatrix { + let weighted: [Vector; MAX_CONSTRAINT_PARTICLES] = + core::array::from_fn(|p| coeffs[p] * im[p]); + NeoHookeanConstraint::strain_block_from_g(&outer_sum(&weighted, coeffs)) + } + + /// Inverse of a small symmetric positive-definite matrix (Cholesky); constraints flagged inert are + /// zeroed in the result, and a block that is not positive definite gives the zero matrix. + pub fn invert_block(mut a: StrainMatrix, inert: [bool; STRAIN_ROWS]) -> StrainMatrix { + for (r, inert) in inert.iter().enumerate() { + if *inert { + a.row_mut(r).fill(0.0); + a.column_mut(r).fill(0.0); + a[(r, r)] = 1.0; + } + } + let mut inv = a.cholesky().map_or_else(StrainMatrix::zeros, |chol| chol.inverse()); + for (r, inert) in inert.iter().enumerate() { + if *inert { + inv.row_mut(r).fill(0.0); + inv.column_mut(r).fill(0.0); + } + } + inv + } + + /// Updates positions, the rotation, the strain, and the model state: the volumetric + /// gradient, rhs and Schur complement (corotational), or the tangent stiffness and gradient + /// (Neo-Hookean). + #[inline] + pub fn update(&mut self, bodies: &SolverBodies) { + for k in 0..MAX_CONSTRAINT_PARTICLES { + let id = self.solver_ids[k]; + if id != u32::MAX { + self.pos[k] = bodies.get_pose(id).translation; + } + } + let f = self.deformation_gradient(); + self.rotation = extract_rotation(f, self.rotation); + self.rot_mat = self.rotation.to_mat(); + let inv_rot = self.rot_mat.transpose(); + let s = inv_rot * f; + // Snap-back caps: an inverted cell recovers under the max corrective velocity over a few + // substeps; a cell torn far past rest has its strain error clamped at 100% (or the same cap + // when larger) to keep the bias continuous on un-inversion; ordinary strains are unclamped. + let inverted = f.determinant() < 0.0; + let caps = if inverted { + self.strain_cap + } else { + self.strain_cap.map(|cap| cap.max(1.0)) + }; + self.strain = + strain_rows_of(&(s - Matrix::IDENTITY)).zip_map(&caps, |e, cap| e.clamp(-cap, cap)); + + match &mut self.model { + SoftElasticModel::Corotational(c) => { + let mut c_vol = + Self::volumetric_gradient(&f, &self.inv_rest_matrix, &mut c.grad_vol); + let cap = if inverted { + c.vol_cap + } else { + c.vol_cap.max(1.0) + }; + c_vol = c_vol.clamp(-cap, cap); + + // Volumetric effective mass and its coupling `bᵣ = Σₚ wₚ gᵣ[p]·(Rᵀ∇C_V)ₚ` with + // the strain rows (cell frame). + let mut a_vv = 0.0; + let mut weighted = [Vector::ZERO; MAX_CONSTRAINT_PARTICLES]; + for p in 0..MAX_CONSTRAINT_PARTICLES { + let g = c.grad_vol[p]; + a_vv += self.im[p] * g.length_squared(); + weighted[p] = (inv_rot * g) * self.im[p]; + } + let b = strain_rows_of(&outer_sum(&weighted, &self.coeffs)); + c.cfm_gain_vol = a_vv * c.cfm_coeff_vol; + c.z = self.inv_a * b; + let schur = a_vv + c.cfm_gain_vol - b.dot(&c.z); + c.b = b; + c.inv_s = if c.volumetric { + crate::utils::inv(schur) + } else { + 0.0 + }; + c.rhs_vol = c_vol * c.erp_vol; + } + SoftElasticModel::NeoHookean(nh) => { + nh.update(&self.strain, &self.coeffs, &self.im, &mut self.inv_a); + } + } + } + + /// The deformation gradient `F = Ds · Dm⁻¹` from `self.pos`. + #[inline] + fn deformation_gradient(&self) -> Matrix { + let x: [Vector; DIM + 1] = core::array::from_fn(|k| self.pos[k]); + let ds = SoftBody::cell_edge_matrix(x); + ds * self.inv_rest_matrix + } + + /// The volumetric gradient of `C_V = det(F) - 1` per particle; returns `C_V`. + #[inline] + fn volumetric_gradient( + f: &Matrix, + inv_rest_matrix: &Matrix, + grad_vol: &mut [Vector; MAX_CONSTRAINT_PARTICLES], + ) -> Real { + // ∂C_V/∂F = cofactor(F); through F = Ds · Dm⁻¹, the columns of (∂C/∂F) · Dm⁻ᵀ are the + // gradients of particles 1..=DIM, particle 0 gets minus their sum. + let c_vol = f.determinant() - 1.0; + let dc_dds = cofactor(f) * inv_rest_matrix.transpose(); + let mut g0 = Vector::ZERO; + for j in 0..DIM { + grad_vol[j + 1] = dc_dds.col(j); + g0 -= grad_vol[j + 1]; + } + grad_vol[0] = g0; + c_vol + } + + /// Recomputes `F` and the volumetric gradient from `self.pos`; returns `(F, C_V)` + /// (corotational constraints only). + #[cfg(test)] + pub(super) fn compute_gradients(&mut self) -> (Matrix, Real) { + let f = self.deformation_gradient(); + let SoftElasticModel::Corotational(c) = &mut self.model else { + unreachable!() + }; + let c_vol = Self::volumetric_gradient(&f, &self.inv_rest_matrix, &mut c.grad_vol); + (f, c_vol) + } + + #[inline] + pub fn warmstart(&self, bodies: &mut SolverBodies) { + match &self.model { + SoftElasticModel::Corotational(c) => self.apply_impulses( + bodies, + &self.strain_impulse, + Some((&c.grad_vol, self.vol_impulse)), + ), + SoftElasticModel::NeoHookean(_) => { + self.apply_impulses(bodies, &self.strain_impulse, None) + } + } + } + + /// One block Gauss-Seidel iteration. + #[inline] + pub fn solve(&mut self, bodies: &mut SolverBodies) { + match &self.model { + SoftElasticModel::Corotational(_) => self.solve_corotational(bodies), + SoftElasticModel::NeoHookean(_) => self.solve_neo_hookean(bodies), + } + } + + /// The particle velocities in the cell frame, and the corotational volumetric rate + /// `Σₚ ∇C_V[p]·vₚ` (zero when `grad_vol` is `None`). + #[inline] + fn local_velocities( + &self, + bodies: &SolverBodies, + grad_vol: Option<&[Vector; MAX_CONSTRAINT_PARTICLES]>, + ) -> ([Vector; MAX_CONSTRAINT_PARTICLES], Real) { + let inv_rot = self.rot_mat.transpose(); + let mut vel = [Vector::ZERO; MAX_CONSTRAINT_PARTICLES]; + let mut vol_rate = 0.0; + for k in 0..MAX_CONSTRAINT_PARTICLES { + let id = self.solver_ids[k]; + if id != u32::MAX { + let v = bodies.get_vel(id).linear; + if let Some(grad_vol) = grad_vol { + vol_rate += grad_vol[k].gdot(v); + } + vel[k] = inv_rot * v; + } + } + (vel, vol_rate) + } + + #[inline] + fn solve_corotational(&mut self, bodies: &mut SolverBodies) { + let SoftElasticModel::Corotational(c) = &self.model else { + unreachable!() + }; + let (vel, vol_rate) = self.local_velocities(bodies, Some(&c.grad_vol)); + let r_vol = c.rhs_vol - c.cfm_gain_vol * self.vol_impulse + vol_rate; + // Strain residuals `ε̇ᵣ + erpᵣ εᵣ - cfmᵣ λᵣ` (cell frame). + let rate = strain_rows_of(&outer_sum(&vel, &self.coeffs)); + let res = rate + c.erp_strain.component_mul(&self.strain) + - c.cfm_strain.component_mul(&self.strain_impulse); + let y = self.inv_a * res; + // Schur complement: volumetric first, strain corrected by the coupling. + let d_vol = c.inv_s * (r_vol - c.b.dot(&y)); + let d_strain = y - c.z * d_vol; + self.strain_impulse += d_strain; + self.vol_impulse += d_vol; + self.apply_impulses(bodies, &d_strain, Some((&c.grad_vol, d_vol))); + } + + /// `Δλ = (M A + I)⁻¹ (M ε̇ + dt g - λ)` (see [`NeoHookeanConstraint`]). + #[inline] + fn solve_neo_hookean(&mut self, bodies: &mut SolverBodies) { + let SoftElasticModel::NeoHookean(nh) = &self.model else { + unreachable!() + }; + let (vel, _) = self.local_velocities(bodies, None); + let rate = strain_rows_of(&outer_sum(&vel, &self.coeffs)); + let d_strain = self.inv_a * (nh.m * rate + nh.dt_grad - self.strain_impulse); + self.strain_impulse += d_strain; + self.apply_impulses(bodies, &d_strain, None); + } + + /// Applies strain impulses (cell frame) and, if given, a volumetric impulse along its + /// gradients. + #[inline] + fn apply_impulses( + &self, + bodies: &mut SolverBodies, + strain: &StrainVector, + vol: Option<(&[Vector; MAX_CONSTRAINT_PARTICLES], Real)>, + ) { + let t = strain_tensor(strain); + for p in 0..MAX_CONSTRAINT_PARTICLES { + let id = self.solver_ids[p]; + if id != u32::MAX && (id as usize) < bodies.len() { + let local = t * self.coeffs[p]; + let dv = match vol { + Some((grad_vol, vol)) => self.rot_mat * local + grad_vol[p] * vol, + None => self.rot_mat * local, + }; + bodies.vels[id as usize].linear -= dv * self.im[p]; + } + } + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_element_linalg.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_element_linalg.rs new file mode 100644 index 000000000..66794b5bb --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_element_linalg.rs @@ -0,0 +1,88 @@ +//! Small dense linear-algebra helpers of the soft rows: the cofactor matrix and the outer product (which glam and glamx lack), and the symmetric eigen-decompositions. + +use crate::glamx::MatExt; +use crate::math::{DIM, Matrix, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// Eigen-decomposition of a symmetric matrix by cyclic Jacobi, stopped once the off-diagonal part +/// is below 1e-4 of the diagonal: the eigenvalues, and the eigenvectors as columns. +pub(crate) fn symmetric_eigen(m: &Matrix) -> (Vector, Matrix) { + let mut a = m.transpose().to_cols_array_2d(); + let mut v = Matrix::IDENTITY.to_cols_array_2d(); + const MAX_SWEEPS: usize = 8; + for _ in 0..MAX_SWEEPS { + let mut off = 0.0; + let mut diag = 0.0; + for p in 0..DIM { + diag += a[p][p] * a[p][p]; + for q in p + 1..DIM { + off += a[p][q] * a[p][q]; + } + } + if off <= 1.0e-8 * diag { + break; + } + for p in 0..DIM { + for q in p + 1..DIM { + let apq = a[p][q]; + if apq == 0.0 { + continue; + } + // tan φ of the rotation zeroing a_pq (the smaller root), c = cos φ, s = sin φ. + let d = a[q][q] - a[p][p]; + let r = (d * d + 4.0 * apq * apq).sqrt(); + let t = 2.0 * apq / (d + d.signum() * r); + let c = 1.0 / (1.0 + t * t).sqrt(); + let s = t * c; + // A ← Jᵀ A J on rows/columns p and q, V ← V J. + for k in 0..DIM { + let akp = a[k][p]; + let akq = a[k][q]; + a[k][p] = c * akp - s * akq; + a[k][q] = s * akp + c * akq; + } + for k in 0..DIM { + let apk = a[p][k]; + let aqk = a[q][k]; + a[p][k] = c * apk - s * aqk; + a[q][k] = s * apk + c * aqk; + } + for row in &mut v { + let vp = row[p]; + let vq = row[q]; + row[p] = c * vp - s * vq; + row[q] = s * vp + c * vq; + } + } + } + } + let values = Vector::from_array(core::array::from_fn(|k| a[k][k])); + // `v[i][k]` is component `i` of eigenvector `k`. + (values, Matrix::from_cols_array_2d(&v).transpose()) +} + +/// The largest eigenvalue of a symmetric matrix (glamx's closed form). +#[inline] +pub(crate) fn max_symmetric_eigenvalue(m: &Matrix) -> Real { + m.symmetric_eigenvalues().max_element() +} + + +/// The cofactor matrix `∂det(F)/∂F`. +#[inline] +pub(crate) fn cofactor(f: &Matrix) -> Matrix { + #[cfg(feature = "dim2")] + { + // F = [[a, b], [c, d]] (rows), det = ad - bc, ∂det/∂F = [[d, -c], [-b, a]]. + let (a, c) = (f.x_axis.x, f.x_axis.y); + let (b, d) = (f.y_axis.x, f.y_axis.y); + Matrix::from_cols(Vector::new(d, -b), Vector::new(-c, a)) + } + #[cfg(feature = "dim3")] + { + let (f0, f1, f2) = (f.x_axis, f.y_axis, f.z_axis); + Matrix::from_cols(f1.cross(f2), f2.cross(f0), f0.cross(f1)) + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs new file mode 100644 index 000000000..99e131069 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs @@ -0,0 +1,163 @@ +//! The stable Neo-Hookean state of an elastic cell: stiffness update, energy gradient and Hessian. + +use crate::math::{DIM, Matrix, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; +use super::*; + +/// Stable Neo-Hookean state of a [`SoftElasticConstraint`]: the energy of Smith et al. 2018 in +/// Kim & Eberle 2020's form, bounded below when inverted. `update` refreshes the gradient every +/// substep and the Hessian after a [`STIFFNESS_UPDATE_STRAIN`] move, modes floored at rest. +#[derive(Copy, Clone, Debug)] +pub(crate) struct NeoHookeanConstraint { + /// Substep length. + pub dt: Real, + /// `μ V₀`. + pub mu: Real, + /// `λ' V₀ = (λ + μ) V₀`. + pub lambda: Real, + /// Damping coefficient of every strain row (cell frame). + pub damping: StrainVector, + // Per-pass state. + /// Strain at which `m` and the block inverse were last computed (`Real::MAX`: never). + pub stiffness_strain: StrainVector, + /// `M = dt (dt K + D)`. + pub m: StrainMatrix, + /// `dt g`, the gradient of the energy scaled by the substep length. + pub dt_grad: StrainVector, +} + +/// Strain change (any component) past which a Neo-Hookean row recomputes its tangent stiffness. +impl NeoHookeanConstraint { + /// Rest stiffness of strain row `r` per unit rest volume: `2μ + λ` on the diagonal rows, + /// `4μ` on the shear rows (the linear-elastic block, which the Neo-Hookean Hessian + /// reproduces at rest). + #[inline] + pub fn rest_stiffness(mu: Real, lambda: Real, r: usize) -> Real { + if r < DIM { 2.0 * mu + lambda } else { 4.0 * mu } + } + + /// Refreshes `M = dt (dt K + D)` and the gradient `dt g` at `strain`, and the block inverse + /// `(M A + I)⁻¹` into `inv_a`. + pub(super) fn update( + &mut self, + strain: &StrainVector, + coeffs: &[Vector; MAX_CONSTRAINT_PARTICLES], + im: &[Real; MAX_CONSTRAINT_PARTICLES], + inv_a: &mut StrainMatrix, + ) { + let (s, kappa, grad) = Self::gradient(self.mu, self.lambda, strain); + let dt = self.dt; + self.dt_grad = grad * dt; + // The stiffness only linearizes: it is kept while the strain stays within + // `STIFFNESS_UPDATE_STRAIN` of the one it was computed at (a resting body then pays + // one update per step, at its first substep). + let fresh = (strain - self.stiffness_strain) + .iter() + .all(|d| d.abs() <= STIFFNESS_UPDATE_STRAIN); + if fresh { + return; + } + self.stiffness_strain = *strain; + let hess = Self::hessian(self.mu, self.lambda, &s, kappa); + self.m = (hess * dt + StrainMatrix::from_diagonal(&self.damping)) * dt; + // `(M A + I)⁻¹`, with the effective-mass block `A_rs = Σₚ wₚ gᵣ[p]·gₛ[p]` built from + // `G = Σₚ wₚ cₚ cₚᵀ` (the gradients are axis-aligned). + let weighted: [Vector; MAX_CONSTRAINT_PARTICLES] = core::array::from_fn(|p| coeffs[p] * im[p]); + let a = Self::strain_block_from_g(&outer_sum(&weighted, coeffs)); + *inv_a = (self.m * a + StrainMatrix::identity()) + .try_inverse() + .unwrap_or_else(StrainMatrix::zeros); + } + + /// The strain effective-mass block from `G = Σₚ wₚ cₚ cₚᵀ`: `A_rs = ¼ (δ_ik G_jl + δ_il G_jk + + /// δ_jk G_il + δ_jl G_ik)`, with `gᵣ[p] = ½ (c_pj e_i + c_pi e_j)` for the row `r = (i, j)`. + pub(super) fn strain_block_from_g(g: &Matrix) -> StrainMatrix { + let pairs = strain_pairs(); + let delta = |i: usize, j: usize| if i == j { 1.0 } else { 0.0 }; + let at = |i: usize, j: usize| g.col(j)[i]; + StrainMatrix::from_fn(|r, s| { + let ((i, j), (k, l)) = (pairs[r], pairs[s]); + 0.25 * (delta(i, k) * at(j, l) + + delta(i, l) * at(j, k) + + delta(j, k) * at(i, l) + + delta(j, l) * at(i, k)) + }) + } + + /// The gradient `V₀ ∂Ψ/∂ε` and tangent stiffness (Hessian floored mode-wise at the rest + /// curvature, see the type docs) at the strain `ε` (cell frame), `mu`/`lambda` scaled by `V₀`. + #[cfg(test)] + pub fn gradient_and_hessian( + mu: Real, + lambda: Real, + strain: &StrainVector, + ) -> (StrainVector, StrainMatrix) { + let (s, kappa, grad) = Self::gradient(mu, lambda, strain); + (grad, Self::hessian(mu, lambda, &s, kappa)) + } + + /// `S = I + ε`, `κ = λ'(J - 1) - μ` and the gradient `V₀ ∂Ψ/∂ε` at the strain `ε`. + mu: Real, + lambda: Real, + strain: &StrainVector, + ) -> (Matrix, Real, StrainVector) { + let s = Matrix::IDENTITY + strain_matrix(strain); + let kappa = lambda * (s.determinant() - 1.0) - mu; + // ∂Ψ/∂S = μ S + κ cof(S). + let grad = strain_rows_dual(&(s * mu + cofactor(&s) * kappa)); + (s, kappa, grad) + } + + /// The tangent stiffness at `S` (see [`Self::gradient_and_hessian`]). + pub(crate) fn hessian(mu: Real, lambda: Real, s: &Matrix, kappa: Real) -> StrainMatrix { + // Hessian of Ψ in the principal frame of `S = Q diag(σ) Qᵀ`: μ I from the ‖F‖² term, the + // ∂²J/∂σ² blocks from J, and λ' g_s g_sᵀ (g_s = ∂J/∂σ) from the volume term. + let (sigma, q) = symmetric_eigen(s); + // The product of the principal stretches other than `a` and `b`. + let others = |a: usize, b: usize| { + (0..DIM) + .filter(|c| *c != a && *c != b) + .fold(1.0, |prod, c| prod * sigma[c]) + }; + let mut g_s = Vector::ZERO; + let mut h_s = Matrix::IDENTITY * mu; + for a in 0..DIM { + g_s[a] = others(a, a); + for b in a + 1..DIM { + h_s.col_mut(b)[a] += kappa * others(a, b); + h_s.col_mut(a)[b] += kappa * others(a, b); + } + } + h_s += outer_product(g_s, g_s) * lambda; + // Floor every mode at its rest curvature (`2μ I + λ 11ᵀ` on the scaling block, `2μ` on the + // flips); see the type docs. + let (mut nu, v) = symmetric_eigen(&h_s); + for k in 0..DIM { + let sum = v.col(k).element_sum(); + nu[k] = nu[k].max(2.0 * mu + (lambda - mu) * sum * sum); + } + + // Back to strain coordinates: `d_a = (q_a q_aᵀ) : ε`, `u_ab = √2 (q_a q_bᵀ) : ε`, so + // `K = Σ_k ν_k w_k w_kᵀ + Σ_f h_f t_f t_fᵀ` with `w_k = Σ_a v_ak t_a`. + let scaling: [StrainVector; DIM] = + core::array::from_fn(|a| strain_rows_dual(&outer_product(q.col(a), q.col(a)))); + let mut k = StrainMatrix::zeros(); + for m in 0..DIM { + let mut w = StrainVector::zeros(); + for a in 0..DIM { + w += scaling[a] * v.col(m)[a]; + } + k.ger(nu[m], &w, &w, 1.0); + } + // Flip modes: pairs (a, b), scaled by the remaining axis c in 3D (no such axis in 2D). + let sqrt2 = Real::sqrt(2.0); + for (a, b) in &strain_pairs()[DIM..] { + let h_f = (mu - kappa * others(*a, *b)).max(2.0 * mu); + let t = strain_rows_dual(&outer_product(q.col(*a), q.col(*b))) * sqrt2; + k.ger(h_f, &t, &t, 1.0); + } + k + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs new file mode 100644 index 000000000..542f377df --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs @@ -0,0 +1,168 @@ +//! The scalar N-particle constraint: gradients of each element kind, its update, warm start and solve. + +use crate::dynamics::SoftBody; +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::math::{DIM, Real, Vector}; +use crate::utils::DotProduct; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// Number of particles a soft constraint can touch. +pub(crate) const MAX_CONSTRAINT_PARTICLES: usize = DIM + 1; + +/// What a soft constraint constrains. +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub(crate) enum SoftScalarConstraintKind { + /// `|x1 - x0| - rest`, two particles. + Distance, + /// The angle between two triangles sharing an edge, four particles (3D only). + #[cfg(feature = "dim3")] + Dihedral, + /// The signed area/volume of a simplex cell minus its rest value. + CellVolume, +} + +/// Where a soft constraint writes its accumulated impulse back (warm-start state on the soft body). +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub(crate) enum SoftScalarConstraintWriteback { + Edge, + #[cfg(feature = "dim3")] + Dihedral, + /// `impulses[0]` of a cell (the volume model). + CellVolume, +} + +/// One scalar constraint over up to `MAX_CONSTRAINT_PARTICLES` particles. +/// +/// Solved like a joint constraint: `Δλ = inv_lhs · (Ċ + rhs - cfm_gain·λ)`, clamped to the impulse +/// bounds, then `v_i -= im_i ∘ ∇_i C · Δλ`; gradients follow the positions before every pass. +#[derive(Copy, Clone, Debug)] +pub(crate) struct SoftScalarConstraint { + pub kind: SoftScalarConstraintKind, + pub num_particles: u8, + pub writeback: SoftScalarConstraintWriteback, + /// Index of the owning soft body in the solver's awake list. + pub soft_body: u32, + /// Index of the element in the soft body's element array. + pub element: u32, + /// Solver-body slots of the particles (`u32::MAX`: no slot, position frozen). + pub solver_ids: [u32; MAX_CONSTRAINT_PARTICLES], + /// Current particle positions (updated by `update`). + pub pos: [Vector; MAX_CONSTRAINT_PARTICLES], + /// Particle inverse masses (step-constant). + pub im: [Real; MAX_CONSTRAINT_PARTICLES], + /// Current constraint gradients. + pub grad: [Vector; MAX_CONSTRAINT_PARTICLES], + /// Rest length / rest volume / rest angle, depending on the kind. + pub rest: Real, + pub erp_inv_dt: Real, + pub cfm_coeff: Real, + pub inv_lhs: Real, + pub cfm_gain: Real, + pub rhs: Real, + pub impulse: Real, + pub impulse_bounds: [Real; 2], +} + +impl SoftScalarConstraint { + /// Whether this constraint is strained beyond `min_strain` (relative error of a distance constraint; the + /// other kinds never are): the criterion of the re-sweep after the contacts. + #[inline] + pub fn is_strained(&self, min_strain: Real) -> bool { + self.kind == SoftScalarConstraintKind::Distance + && self.erp_inv_dt > 0.0 + && self.rest > 0.0 + && (self.rhs / self.erp_inv_dt).abs() > min_strain * self.rest + } + + /// Updates the positions, gradients, rhs and effective mass from the current solver-body + /// poses. Soft constraints are springs whose bias is the spring force, not an error-correction + /// velocity, so they are solved once per substep (biased pass only) with the bias always on. + #[inline] + pub fn update(&mut self, bodies: &SolverBodies) { + let n = self.num_particles as usize; + for k in 0..n { + let id = self.solver_ids[k]; + if id != u32::MAX { + self.pos[k] = bodies.get_pose(id).translation; + } + } + + let c = self.compute_gradients(); + + // Effective mass and softness. + let mut w = 0.0; + for k in 0..n { + w += self.grad[k].gdot(self.grad[k] * self.im[k]); + } + let cfm_gain = w * self.cfm_coeff; + self.cfm_gain = cfm_gain; + self.inv_lhs = crate::utils::inv(w + cfm_gain); + + // No corrective-velocity cap: the bias is the spring force itself, and capping it would + // make the material's response depend on the substep count. + self.rhs = c * self.erp_inv_dt; + } + + /// Recomputes the gradients from `self.pos`; returns the constraint value. + #[inline] + pub(super) fn compute_gradients(&mut self) -> Real { + match self.kind { + SoftScalarConstraintKind::Distance => { + let d = self.pos[1] - self.pos[0]; + let len = d.length(); + let n = if len > 1.0e-9 { d / len } else { Vector::X }; + self.grad[0] = -n; + self.grad[1] = n; + len - self.rest + } + #[cfg(feature = "dim3")] + SoftScalarConstraintKind::Dihedral => self.dihedral_gradients(), + SoftScalarConstraintKind::CellVolume => { + let x: [Vector; DIM + 1] = core::array::from_fn(|k| self.pos[k]); + let vol = SoftBody::cell_volume(x); + vol - self.rest + } + } + } + + #[cfg(feature = "dim3")] + fn dihedral_gradients(&mut self) -> Real { + /// Applies the accumulated impulse to the particle velocities. + #[inline] + pub fn warmstart(&self, bodies: &mut SolverBodies) { + self.apply_impulse(bodies, self.impulse); + } + + /// One Gauss-Seidel iteration on this constraint. + #[inline] + pub fn solve(&mut self, bodies: &mut SolverBodies) { + let n = self.num_particles as usize; + let mut dc = 0.0; + for k in 0..n { + let id = self.solver_ids[k]; + if id != u32::MAX { + dc += self.grad[k].gdot(bodies.get_vel(id).linear); + } + } + let rhs = dc + self.rhs; + let total = (self.impulse + self.inv_lhs * (rhs - self.cfm_gain * self.impulse)) + .clamp(self.impulse_bounds[0], self.impulse_bounds[1]); + let delta = total - self.impulse; + self.impulse = total; + self.apply_impulse(bodies, delta); + } + + #[inline] + fn apply_impulse(&self, bodies: &mut SolverBodies, impulse: Real) { + let n = self.num_particles as usize; + for k in 0..n { + let id = self.solver_ids[k]; + if id != u32::MAX && (id as usize) < bodies.len() { + let dv = self.grad[k] * self.im[k] * impulse; + bodies.vels[id as usize].linear -= dv; + } + } + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs new file mode 100644 index 000000000..8e1ec44c9 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs @@ -0,0 +1,358 @@ +//! Finite-difference and linear-algebra checks of the soft constraints. + +use crate::dynamics::SoftBody; +use crate::dynamics::soft_body::soft_body_shape_matching::extract_rotation; +use crate::math::{DIM, Matrix, Real, Rotation, Vector}; +use crate::utils::{DotProduct, RotationOps}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; +use super::*; + +#[cfg(feature = "dim2")] +fn base_positions() -> [Vector; 3] { + [ + Vector::new(0.1, 0.2), + Vector::new(1.15, -0.2), + Vector::new(0.3, 0.95), + ] +} +#[cfg(feature = "dim3")] +fn base_positions() -> [Vector; 4] { + [ + Vector::new(0.1, 0.2, -0.1), + Vector::new(1.15, -0.2, 0.0), + Vector::new(0.3, 0.95, 0.4), + Vector::new(-0.2, 0.3, 1.2), + ] +} + +/// Finite-difference check of every scalar constraint kind's gradients. +#[test] +fn gradients_match_finite_differences() { + let mut constraint = SoftScalarConstraint { + kind: SoftScalarConstraintKind::Distance, + num_particles: 2, + writeback: SoftScalarConstraintWriteback::Edge, + soft_body: 0, + element: 0, + solver_ids: [u32::MAX; MAX_CONSTRAINT_PARTICLES], + pos: [Vector::ZERO; MAX_CONSTRAINT_PARTICLES], + im: [1.0; MAX_CONSTRAINT_PARTICLES], + grad: [Vector::ZERO; MAX_CONSTRAINT_PARTICLES], + rest: 0.3, + erp_inv_dt: 0.0, + cfm_coeff: 0.0, + inv_lhs: 0.0, + cfm_gain: 0.0, + rhs: 0.0, + impulse: 0.0, + impulse_bounds: [-Real::MAX, Real::MAX], + }; + #[cfg(feature = "dim2")] + let kinds = [(SoftScalarConstraintKind::Distance, 2), (SoftScalarConstraintKind::CellVolume, 3)]; + #[cfg(feature = "dim3")] + let kinds = [ + (SoftScalarConstraintKind::Distance, 2), + (SoftScalarConstraintKind::Dihedral, 4), + (SoftScalarConstraintKind::CellVolume, 4), + ]; + for (kind, n) in kinds { + constraint.kind = kind; + constraint.num_particles = n as u8; + constraint.pos = base_positions(); + constraint.rest = 0.3; + let c0 = constraint.compute_gradients(); + let grad = constraint.grad; + let eps = 1.0e-3; + for k in 0..n { + for axis in 0..DIM { + let mut plus = constraint; + plus.pos[k][axis] += eps; + let mut minus = constraint; + minus.pos[k][axis] -= eps; + let fd = (plus.compute_gradients() - minus.compute_gradients()) / (2.0 * eps); + let an = grad[k][axis]; + assert!( + (fd - an).abs() < 2.0e-3 * (1.0 + an.abs()), + "{kind:?}: particle {k} axis {axis}: fd {fd} vs analytic {an} (C = {c0})" + ); + } + } + let sum: Vector = grad[..n].iter().copied().sum(); + assert!( + sum.length() < 1.0e-5, + "{kind:?}: gradients don't sum to zero: {sum:?}" + ); + } +} + +/// Finite-difference check of the elastic-cell gradients: the volumetric one through the +/// cofactor path, the strain ones (with the rotation frozen) through the coefficients. +#[test] +fn elastic_cell_gradients_match_finite_differences() { + let rest = base_positions(); + let inv_rest_matrix = SoftBody::cell_edge_matrix(rest).inverse(); + let mut constraint = SoftElasticConstraint { + soft_body: 0, + element: 0, + solver_ids: [u32::MAX; MAX_CONSTRAINT_PARTICLES], + pos: rest, + im: [1.0; MAX_CONSTRAINT_PARTICLES], + coeffs: SoftElasticConstraint::coefficients(&inv_rest_matrix), + inv_rest_matrix, + rotation: Rotation::IDENTITY, + rot_mat: Matrix::IDENTITY, + inv_a: StrainMatrix::zeros(), + strain_cap: StrainVector::repeat(Real::MAX), + strain: StrainVector::zeros(), + strain_impulse: StrainVector::zeros(), + vol_impulse: 0.0, + model: SoftElasticModel::Corotational(CorotationalConstraint { + erp_strain: StrainVector::zeros(), + cfm_strain: StrainVector::zeros(), + erp_vol: 0.0, + cfm_coeff_vol: 0.0, + volumetric: true, + vol_cap: Real::MAX, + grad_vol: [Vector::ZERO; MAX_CONSTRAINT_PARTICLES], + cfm_gain_vol: 0.0, + rhs_vol: 0.0, + b: StrainVector::zeros(), + z: StrainVector::zeros(), + inv_s: 0.0, + }), + }; + let grad_vol_of = |r: &SoftElasticConstraint| match &r.model { + SoftElasticModel::Corotational(c) => c.grad_vol, + SoftElasticModel::NeoHookean(_) => unreachable!(), + }; + // Deform. + constraint.pos[1] += Vector::splat(0.07); + constraint.pos[2] -= Vector::splat(0.11); + let (f0, c_vol) = constraint.compute_gradients(); + let rot = extract_rotation(f0, Rotation::IDENTITY); + let gv = grad_vol_of(&constraint); + let strain_of = |r: &mut SoftElasticConstraint| -> [Real; STRAIN_ROWS] { + let (f, _) = r.compute_gradients(); + let s = rot.inverse().to_mat() * f; + let mut out = [0.0; STRAIN_ROWS]; + for (i, (a, b)) in strain_pairs().iter().enumerate() { + let sym = 0.5 * (s.col(*b)[*a] + s.col(*a)[*b]); + out[i] = if a == b { sym - 1.0 } else { sym }; + } + out + }; + let eps = 1.0e-3; + for k in 0..MAX_CONSTRAINT_PARTICLES { + for axis in 0..DIM { + let mut plus = constraint; + plus.pos[k][axis] += eps; + let mut minus = constraint; + minus.pos[k][axis] -= eps; + let (_, pv) = plus.compute_gradients(); + let (_, mv) = minus.compute_gradients(); + let fd_vol = (pv - mv) / (2.0 * eps); + assert!( + (fd_vol - gv[k][axis]).abs() < 2.0e-3 * (1.0 + gv[k][axis].abs()), + "volumetric: particle {k} axis {axis}: fd {fd_vol} vs {} (C = {c_vol})", + gv[k][axis] + ); + // Strain rows: the analytic gradient is in the cell frame, so world `axis` maps to + // `Rᵀ e_axis`. + let sp = strain_of(&mut plus); + let sm = strain_of(&mut minus); + let mut e = Vector::ZERO; + e[axis] = 1.0; + let local = rot.inverse() * e; + for r in 0..STRAIN_ROWS { + let fd = (sp[r] - sm[r]) / (2.0 * eps); + let expected = SoftElasticConstraint::strain_gradient(&constraint.coeffs, r, k).gdot(local); + assert!( + (fd - expected).abs() < 2.0e-3 * (1.0 + expected.abs()), + "strain row {r}: particle {k} axis {axis}: fd {fd} vs {expected}" + ); + } + } + } +} + +/// The strain block inverse inverts the block (and zeroes inert constraints), and the closed form +/// from `G` used by the Neo-Hookean constraints matches the direct sum. +#[test] +fn strain_block_inverse() { + let rest = base_positions(); + let inv_rest_matrix = SoftBody::cell_edge_matrix(rest).inverse(); + let coeffs = SoftElasticConstraint::coefficients(&inv_rest_matrix); + let im = [1.0, 0.5, 2.0, 0.25][..MAX_CONSTRAINT_PARTICLES].to_vec(); + // The block from its definition, `A_rs = Σₚ wₚ gᵣ[p]·gₛ[p]`. + let mut a = StrainMatrix::zeros(); + for r in 0..STRAIN_ROWS { + for s in r..STRAIN_ROWS { + let mut sum = 0.0; + for p in 0..MAX_CONSTRAINT_PARTICLES { + sum += im[p] + * SoftElasticConstraint::strain_gradient(&coeffs, r, p) + .gdot(SoftElasticConstraint::strain_gradient(&coeffs, s, p)); + } + a[(r, s)] = sum; + a[(s, r)] = sum; + } + } + let closed_form = SoftElasticConstraint::strain_block(&coeffs, &im); + assert!((a - closed_form).abs().max() < 1.0e-5 * (1.0 + a.abs().max())); + let mut g = Matrix::ZERO; + for p in 0..MAX_CONSTRAINT_PARTICLES { + for a in 0..DIM { + for b in 0..DIM { + g.col_mut(b)[a] += im[p] * coeffs[p][a] * coeffs[p][b]; + } + } + } + let from_g = NeoHookeanConstraint::strain_block_from_g(&g); + for r in 0..STRAIN_ROWS { + for s in 0..STRAIN_ROWS { + assert!( + (a[(r, s)] - from_g[(r, s)]).abs() < 1.0e-5 * (1.0 + a[(r, s)].abs()), + "({r},{s}): {} vs {}", + a[(r, s)], + from_g[(r, s)] + ); + } + } + let inv = SoftElasticConstraint::invert_block(a, [false; STRAIN_ROWS]); + let err = (a * inv - StrainMatrix::identity()).abs().max(); + assert!(err < 1.0e-4, "inverse error {err}"); +} + +/// The Neo-Hookean gradient matches finite differences of the energy, and the Hessian +/// (unprojected where the energy is convex) matches finite differences of the gradient. +#[test] +fn neo_hookean_gradient_and_hessian_match_finite_differences() { + let (mu, lambda) = (1.3, 2.7); + let energy = |strain: &StrainVector| -> Real { + let m = Matrix::IDENTITY + strain_matrix(strain); + let j = m.determinant(); + let norm2: Real = (0..DIM).map(|k| m.col(k).length_squared()).sum(); + 0.5 * mu * (norm2 - DIM as Real) - mu * (j - 1.0) + 0.5 * lambda * (j - 1.0) * (j - 1.0) + }; + // Moderate strains around rest (convex region) and a strongly compressed cell. + let cases = [ + StrainVector::from_fn(|r, _| { + 0.05 * (r as Real + 1.0) * if r % 2 == 0 { 1.0 } else { -1.0 } + }), + StrainVector::from_fn(|r, _| if r < DIM { -0.4 } else { 0.1 }), + ]; + let eps = 1.0e-3; + let perturbed = |strain: &StrainVector, r: usize, delta: Real| { + let mut out = *strain; + out[r] += delta; + out + }; + let fd_hessian = |strain: &StrainVector| -> StrainMatrix { + let mut h = StrainMatrix::zeros(); + for r in 0..STRAIN_ROWS { + let plus = perturbed(strain, r, eps); + let minus = perturbed(strain, r, -eps); + let (gp, _) = NeoHookeanConstraint::gradient_and_hessian(mu, lambda, &plus); + let (gm, _) = NeoHookeanConstraint::gradient_and_hessian(mu, lambda, &minus); + h.row_mut(r).copy_from(&((gp - gm) / (2.0 * eps)).transpose()); + } + h + }; + for strain in &cases { + let (grad, hess) = NeoHookeanConstraint::gradient_and_hessian(mu, lambda, strain); + for r in 0..STRAIN_ROWS { + let fd = (energy(&perturbed(strain, r, eps)) - energy(&perturbed(strain, r, -eps))) + / (2.0 * eps); + assert!( + (fd - grad[r]).abs() < 2.0e-3 * (1.0 + grad[r].abs()), + "gradient row {r}: fd {fd} vs {}", + grad[r] + ); + } + // The stiffness dominates the tangent (floored modes): diagonal entries and 2x2 + // minors of the difference are non-negative. + let diff = hess - fd_hessian(strain); + for r in 0..STRAIN_ROWS { + let d_rr = diff[(r, r)]; + assert!( + d_rr >= -5.0e-3 * (1.0 + hess[(r, r)].abs()), + "({r},{r}): {d_rr}" + ); + for s in 0..STRAIN_ROWS { + let minor = d_rr * diff[(s, s)] - diff[(r, s)] * diff[(r, s)]; + assert!( + minor >= -1.0e-2 * (1.0 + hess[(r, r)] * hess[(s, s)]).abs(), + "minor ({r},{s}): {minor}" + ); + } + } + } + // Under uniform stretch the volumetric mode is above its floor: the stiffness is the + // tangent along it. + let stretched = StrainVector::from_fn(|r, _| if r < DIM { 0.5 } else { 0.0 }); + let (_, hess) = NeoHookeanConstraint::gradient_and_hessian(mu, lambda, &stretched); + let fd = fd_hessian(&stretched); + let quad_h = hess.fixed_view::(0, 0).sum(); + let quad_fd = fd.fixed_view::(0, 0).sum(); + assert!( + (quad_h - quad_fd).abs() < 1.0e-2 * quad_fd.abs(), + "volumetric tangent under stretch: {quad_h} vs fd {quad_fd}" + ); + // At rest the Hessian is the linear-elastic block: 2μ + λ on the diagonal rows, λ between + // them, 4μ on the shears. + let (grad, hess) = NeoHookeanConstraint::gradient_and_hessian(mu, lambda, &StrainVector::zeros()); + assert!(grad.amax() < 1.0e-6, "nonzero rest gradient {grad:?}"); + for r in 0..STRAIN_ROWS { + for s in 0..STRAIN_ROWS { + let expected = if r == s { + NeoHookeanConstraint::rest_stiffness(mu, lambda - mu, r) + } else if r < DIM && s < DIM { + lambda - mu + } else { + 0.0 + }; + assert!( + (hess[(r, s)] - expected).abs() < 1.0e-4, + "rest hessian ({r},{s}) = {} vs {expected}", + hess[(r, s)] + ); + } + } +} + +/// The cofactor matrix is the gradient of the determinant: `det(F) F⁻ᵀ` for an invertible `F`. +#[test] +fn cofactor_is_the_determinant_gradient() { + let f = SoftBody::cell_edge_matrix(base_positions()); + let expected = f.inverse().transpose() * f.determinant(); + let cof = cofactor(&f); + for k in 0..DIM { + let err = (cof.col(k) - expected.col(k)).abs().max_element(); + assert!(err < 1.0e-5, "column {k}: {:?} vs {:?}", cof.col(k), expected.col(k)); + } +} + +/// The symmetric eigen-decomposition reconstructs the degenerate matrices a resting or uniformly +/// stretched cell produces: identity, diagonal, and repeated eigenvalues with a small shear. +#[test] +fn symmetric_eigen_handles_degenerate_matrices() { + let mut shear = Matrix::IDENTITY * 1.5; + shear.col_mut(1)[0] = 1.0e-3; + shear.col_mut(0)[1] = 1.0e-3; + let diagonal = Matrix::from_diagonal(Vector::ONE + Vector::X * 0.25); + for m in [Matrix::IDENTITY, diagonal, shear] { + let (values, vectors) = symmetric_eigen(&m); + let rebuilt = vectors * Matrix::from_diagonal(values) * vectors.transpose(); + let orthonormality = vectors.transpose() * vectors; + for k in 0..DIM { + let err = (rebuilt.col(k) - m.col(k)).abs().max_element(); + let ortho = (orthonormality.col(k) - Matrix::IDENTITY.col(k)).abs().max_element(); + assert!(err < 1.0e-4 && ortho < 1.0e-4, "{m:?}: column {k}: {err} {ortho}"); + } + let max = max_symmetric_eigenvalue(&m); + assert!((max - values.max_element()).abs() < 1.0e-4, "{m:?}: {max} vs {values:?}"); + } +} + diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs new file mode 100644 index 000000000..7888e6c28 --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs @@ -0,0 +1,843 @@ +//! Per-step assembly of the awake soft bodies' element constraints into the group-major, +//! color-by-color layout the solver expects: per-color counts and prefix sums (serial), then each +//! body fills its own slots of every color's range (per body, in parallel under `parallel`). + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::alloc_prelude::*; +use core::ops::Range; +use core::sync::atomic::AtomicBool; + +use super::soft_constraints_set::{ + AwakeSoftBody, SoftColorRange, SoftConstraintsSet, SoftGroupLayout, SoftShapeConstraint, + SoftVolumeConstraint, +}; +use super::soft_element_constraint::{ + CorotationalConstraint, MAX_CONSTRAINT_PARTICLES, NeoHookeanConstraint, STRAIN_ROWS, + SoftElasticModel, SoftElasticConstraint, SoftScalarConstraint, SoftScalarConstraintKind, + SoftScalarConstraintWriteback, StrainMatrix, StrainVector, +}; +use crate::dynamics::soft_body::SOFT_BODY_OVERFLOW_COLOR; +use crate::dynamics::solver::reset_buffer_reusing; +use crate::dynamics::{ + RigidBodyHandle, RigidBodySet, SoftBody, SoftBodyCellModel, SoftBodyEdgeKind, SoftBodyHandle, + SoftBodySet, SpringCoefficients, +}; +use crate::math::{AngVector, DIM, Real, Vector}; +use na::SimdRealField; +use super::soft_constraints_set::AwakeCluster; + +/// Number of element colors: the parallel ones, then the overflow color. +const NUM_COLORS: usize = SOFT_BODY_OVERFLOW_COLOR as usize + 1; + +/// Workspace of the element-constraint assembly kept across steps. +#[derive(Default)] +pub(crate) struct ElementConstraintAssemblyWorkspace { + /// Substep length of every group. + group_dts: Vec, + /// Per (awake body, color): the body's scalar / elastic constraint count, turned into its first + /// slot of the color's range by the layout pass. + offsets: Vec, + elastic_offsets: Vec, + /// Per awake body: its shape constraint count, then its first shape constraint. + shape_offsets: Vec, +} + +/// The constraint buffers being filled, as raw pointers shared by the per-body fill jobs. +/// +/// Safety: the layout pass gives every body its own slots (the offsets partition the buffers), +/// so a job only writes slots no other job touches. +#[derive(Copy, Clone)] +struct ConstraintSlots { + scalar_constraints: *mut SoftScalarConstraint, + elastic_constraints: *mut SoftElasticConstraint, + shape_constraints: *mut SoftShapeConstraint, +} + +// SAFETY: see the struct-level contract. +unsafe impl Send for ConstraintSlots {} +unsafe impl Sync for ConstraintSlots {} + +/// The per-body cursors of one fill job. +struct BodyCursors { + slots: ConstraintSlots, + scalar_constraints: [usize; NUM_COLORS], + elastic_constraints: [usize; NUM_COLORS], + shape_constraints: usize, +} + +impl BodyCursors { + #[inline] + fn push_constraint(&mut self, color: u8, constraint: SoftScalarConstraint) { + let slot = &mut self.scalar_constraints[color as usize]; + // SAFETY: the body's own slot (see `ConstraintSlots`). + unsafe { self.slots.scalar_constraints.add(*slot).write(constraint) }; + *slot += 1; + } + + #[inline] + fn push_elastic_constraint(&mut self, color: u8, constraint: SoftElasticConstraint) { + let slot = &mut self.elastic_constraints[color as usize]; + // SAFETY: the body's own slot (see `ConstraintSlots`). + unsafe { self.slots.elastic_constraints.add(*slot).write(constraint) }; + *slot += 1; + } + + #[inline] + fn push_shape_constraint(&mut self, constraint: SoftShapeConstraint) { + // SAFETY: the body's own slot (see `ConstraintSlots`). + unsafe { self.slots.shape_constraints.add(self.shape_constraints).write(constraint) }; + self.shape_constraints += 1; + } +} + +impl SoftConstraintsSet { + /// Assembles every awake soft body's constraints and lays out their particles' solver slots + /// (`slot_base` and up, group-major, in awake order); `group_of_slot(slot)` maps a rigid slot + /// to its substep group, `group_dt(group)` its substep length and `step_dt` the whole step's. + #[allow(clippy::too_many_arguments)] + pub fn assemble( + &mut self, + island_id: usize, + slot_base: usize, + num_groups: usize, + group_of_slot: impl Fn(u32) -> usize, + group_dt: impl Fn(usize) -> Real, + step_dt: Real, + max_corrective_velocity: Real, + bodies: &RigidBodySet, + soft_bodies: &mut SoftBodySet, + ) { + self.awake.clear(); + self.slots.clear(); + self.scalar_constraints.clear(); + self.elastic_constraints.clear(); + self.color_ranges.clear(); + self.shape_constraints.clear(); + self.volume_constraints.clear(); + self.volume_grads.clear(); + self.contacts.clear(); + self.attachments.clear(); + self.groups.clear(); + + if soft_bodies.is_empty() { + return; + } + self.collect_awake_bodies(island_id, group_of_slot, step_dt, bodies, soft_bodies); + if self.awake.is_empty() { + return; + } + // Group-major order (stable, so the handle order is kept within a group), then the + // particles' solver slots: contiguous per body, bodies in awake order. + self.awake.sort_by_key(|a| a.group); + for awake in &mut self.awake { + awake.slot_start = self.slots.len(); + let first = slot_base + self.slots.len(); + self.slots + .extend((0..awake.num_particles).map(|i| (first + i) as u32)); + } + self.groups.resize(num_groups, SoftGroupLayout::default()); + + let mut workspace = core::mem::take(&mut self.element_constraint_workspace); + workspace.group_dts.clear(); + workspace.group_dts.extend((0..num_groups).map(group_dt)); + let num_awake = self.awake.len(); + workspace.offsets.clear(); + workspace.offsets.resize(num_awake * NUM_COLORS, 0); + workspace.elastic_offsets.clear(); + workspace.elastic_offsets.resize(num_awake * NUM_COLORS, 0); + workspace.shape_offsets.clear(); + workspace.shape_offsets.resize(num_awake, 0); + + // Pass 1: every body's constraint counts per color (serial: a few percent of the fill, less + // than a fork-join). + for ai in 0..num_awake { + let (counts, elastic_counts) = ( + &mut workspace.offsets[ai * NUM_COLORS..(ai + 1) * NUM_COLORS], + &mut workspace.elastic_offsets[ai * NUM_COLORS..(ai + 1) * NUM_COLORS], + ); + // SAFETY: read-only access during assembly. + let range = 0..element_count(unsafe { &*self.awake[ai].ptr }); + self.count_body_constraints(ai, counts, elastic_counts, range); + workspace.shape_offsets[ai] = self.count_body_shape_constraints(ai); + } + + // Pass 2 (serial): the layout. Per group: the awake range, the shape and volume constraints + // (awake order), then per color the constraint range and each body's first slot in it + // (awake order, so a group's constraints of a color are contiguous and deterministic). + let mut awake_cursor = 0; + let mut cursor = 0; + let mut elastic_cursor = 0; + let mut shape_cursor = 0; + for gi in 0..num_groups { + let awake_start = awake_cursor; + while awake_cursor < self.awake.len() && self.awake[awake_cursor].group as usize == gi { + awake_cursor += 1; + } + let dt = workspace.group_dts[gi]; + let shape_start = shape_cursor; + let volume_start = self.volume_constraints.len(); + let mut damping = false; + for ai in awake_start..awake_cursor { + let awake = &mut self.awake[ai]; + let shape_count = workspace.shape_offsets[ai]; + workspace.shape_offsets[ai] = shape_cursor; + awake.shape_constraints = shape_cursor..shape_cursor + shape_count; + shape_cursor += shape_count; + if awake.frozen { + continue; + } + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + let material = &sb.material; + if material.deformation_damping > 0.0 { + awake.damping_factor = 1.0 - (-material.deformation_damping * dt).exp(); + damping = true; + } + if sb.volume_preservation && !sb.boundary.is_empty() { + let grads_start = self.volume_grads.len(); + self.volume_grads + .resize(grads_start + sb.particles.len(), Vector::ZERO); + awake.volume_constraint = Some(self.volume_constraints.len()); + self.volume_constraints.push(SoftVolumeConstraint { + soft_body: ai as u32, + target: sb.rest_volume * sb.volume_factor, + grads: grads_start..grads_start + sb.particles.len(), + erp_inv_dt: material.volume_softness.erp_inv_dt(dt), + cfm_coeff: material.volume_softness.cfm_coeff(dt), + max_bias_velocity: max_corrective_velocity, + inv_lhs: 0.0, + cfm_gain: 0.0, + rhs: 0.0, + }); + } + } + let colors_start = self.color_ranges.len(); + for color in 0..NUM_COLORS { + let start = cursor; + let elastic_start = elastic_cursor; + for ai in awake_start..awake_cursor { + let count = &mut workspace.offsets[ai * NUM_COLORS + color]; + let elastic_count = &mut workspace.elastic_offsets[ai * NUM_COLORS + color]; + (cursor, *count) = (cursor + *count, cursor); + (elastic_cursor, *elastic_count) = + (elastic_cursor + *elastic_count, elastic_cursor); + } + let range = SoftColorRange { + scalar_constraints: start..cursor, + elastic_constraints: elastic_start..elastic_cursor, + }; + if color == SOFT_BODY_OVERFLOW_COLOR as usize { + self.groups[gi].serial = range; + } else if range.len() > 0 { + self.color_ranges.push(range); + } + } + let g = &mut self.groups[gi]; + g.awake = awake_start..awake_cursor; + g.slots = match (self.awake.get(awake_start), awake_cursor.checked_sub(1)) { + (Some(first), Some(last)) if awake_start < awake_cursor => { + let last = &self.awake[last]; + slot_base + first.slot_start..slot_base + last.slot_start + last.num_particles + } + _ => slot_base..slot_base, + }; + g.colors = colors_start..self.color_ranges.len(); + g.shape_constraints = shape_start..shape_cursor; + g.volume_constraints = volume_start..self.volume_constraints.len(); + g.damping = damping; + } + + // Pass 3: every body builds its constraints straight into its slots. + // SAFETY: plain-old-data constraints; every slot below the new lengths is written by the fill + // before being read (the counts of pass 1 are exact). + unsafe { + reset_buffer_reusing(&mut self.scalar_constraints, cursor); + reset_buffer_reusing(&mut self.elastic_constraints, elastic_cursor); + self.strained.clear(); + self.strained.resize(cursor + elastic_cursor, false); + reset_buffer_reusing(&mut self.shape_constraints, shape_cursor); + } + { + let slots = ConstraintSlots { + scalar_constraints: self.scalar_constraints.as_mut_ptr(), + elastic_constraints: self.elastic_constraints.as_mut_ptr(), + shape_constraints: self.shape_constraints.as_mut_ptr(), + }; + let this = &*self; + let workspace = &workspace; + let fill = |ai: usize| { + let mut cursors = BodyCursors { + slots, + scalar_constraints: [0; NUM_COLORS], + elastic_constraints: [0; NUM_COLORS], + shape_constraints: workspace.shape_offsets[ai], + }; + cursors + .scalar_constraints + .copy_from_slice(&workspace.offsets[ai * NUM_COLORS..(ai + 1) * NUM_COLORS]); + cursors.elastic_constraints.copy_from_slice( + &workspace.elastic_offsets[ai * NUM_COLORS..(ai + 1) * NUM_COLORS], + ); + let dt = workspace.group_dts[this.awake[ai].group as usize]; + // SAFETY: read-only access during assembly. + let num_elements = element_count(unsafe { &*this.awake[ai].ptr }); + #[cfg(feature = "parallel")] + if num_elements >= 2 * FILL_CHUNK && rayon::current_num_threads() > 1 { + // A large body fills its elements in chunks across threads: each chunk counts + // its constraints per color, takes cursors from the prefix sums (slot order + // stays the element order, as serially), then fills them. + use rayon::prelude::*; + let num_chunks = num_elements.div_ceil(FILL_CHUNK); + let chunk_range = + |c: usize| c * FILL_CHUNK..((c + 1) * FILL_CHUNK).min(num_elements); + let mut chunk_counts: Vec<([usize; NUM_COLORS], [usize; NUM_COLORS])> = + (0..num_chunks) + .into_par_iter() + .map(|c| { + let mut counts = ([0; NUM_COLORS], [0; NUM_COLORS]); + this.count_body_constraints( + ai, + &mut counts.0, + &mut counts.1, + chunk_range(c), + ); + counts + }) + .collect(); + for (scalar, elastic) in chunk_counts.iter_mut() { + for color in 0..NUM_COLORS { + let (count, elastic_count) = (scalar[color], elastic[color]); + scalar[color] = cursors.scalar_constraints[color]; + elastic[color] = cursors.elastic_constraints[color]; + cursors.scalar_constraints[color] += count; + cursors.elastic_constraints[color] += elastic_count; + } + } + chunk_counts + .into_par_iter() + .enumerate() + .for_each(|(c, (scalar, elastic))| { + let mut cursors = BodyCursors { + slots, + scalar_constraints: scalar, + elastic_constraints: elastic, + shape_constraints: usize::MAX, + }; + this.fill_body_constraints( + ai, + dt, + max_corrective_velocity, + &mut cursors, + chunk_range(c), + ); + }); + this.fill_body_shape_constraints(ai, dt, &mut cursors); + debug_assert_eq!( + cursors.shape_constraints, + this.awake[ai].shape_constraints.end + ); + return; + } + this.fill_body_constraints( + ai, + dt, + max_corrective_velocity, + &mut cursors, + 0..num_elements, + ); + this.fill_body_shape_constraints(ai, dt, &mut cursors); + debug_assert_eq!(cursors.shape_constraints, this.awake[ai].shape_constraints.end); + }; + #[cfg(feature = "parallel")] + { + use rayon::prelude::*; + (0..num_awake).into_par_iter().for_each(fill); + } + #[cfg(not(feature = "parallel"))] + (0..num_awake).for_each(fill); + } + self.element_constraint_workspace = workspace; + + // Every parallel color must be particle-disjoint: two constraints of a color sharing a solver + // body would race on its velocity. + #[cfg(debug_assertions)] + for range in &self.color_ranges { + let mut seen: parry::utils::hashmap::HashMap = Default::default(); + let ids = self.scalar_constraints[range.scalar_constraints.clone()] + .iter() + .flat_map(|c| c.solver_ids[..c.num_particles as usize].iter().copied()) + .chain( + self.elastic_constraints[range.elastic_constraints.clone()] + .iter() + .flat_map(|constraint| constraint.solver_ids.iter().copied()), + ); + for id in ids { + if id != u32::MAX { + debug_assert!( + seen.insert(id, ()).is_none(), + "soft-body solver slot {id} shared by two constraints of one color" + ); + } + } + } + } + + /// Fills `awake` (in handle order) with the soft bodies whose root body is awake in island + /// `island_id`. A fully pinned body is frozen: no constraint of its own, but its particles + /// still move kinematically and its surface holds awake bodies; the caller lays out slots. + fn collect_awake_bodies( + &mut self, + island_id: usize, + group_of_slot: impl Fn(u32) -> usize, + step_dt: Real, + bodies: &RigidBodySet, + soft_bodies: &mut SoftBodySet, + ) { + let inv_dt = crate::utils::inv(step_dt); + + // The root body's slot in the island, if awake in it. + let root_slot = |h: RigidBodyHandle| -> Option { + let rb = bodies.get(h)?; + (rb.ids.active_island_id == island_id as u32 && !rb.is_sleeping() && rb.is_enabled()) + .then_some(rb.ids.active_set_id) + }; + let mut selected: Vec<(SoftBodyHandle, u16, bool)> = Vec::new(); + for (handle, sb) in soft_bodies.iter() { + let Some(slot) = root_slot(sb.root_body()) else { + // Asleep or not simulated. + continue; + }; + let frozen = !sb.particles.iter().any(|p| p.inv_mass > 0.0); + selected.push((handle, group_of_slot(slot) as u16, frozen)); + } + for (handle, group, frozen) in selected { + let Some(sb) = soft_bodies.get_mut(handle) else { + continue; + }; + + let awake_clusters = + sb + .clusters + .iter() + .enumerate() + .filter(|(_, c)| c.is_live() && c.shape_matching) + .map(|(ci, c)| { + let mut target_linvel = Default::default(); + let mut target_angvel = Default::default(); + + if let (Some(prev_target_pose), Some(target_pose)) = (&c.prev_shape_matching_target, &c.shape_matching_target) { + let dpos = target_pose.translation - prev_target_pose.translation; + let drot = target_pose.rotation * prev_target_pose.rotation.inverse(); + target_linvel = dpos * inv_dt; + #[cfg(feature = "dim2")] + { target_angvel = drot.angle() * inv_dt; } + #[cfg(feature = "dim3")] + { target_angvel = drot.to_scaled_axis() * inv_dt; } + } + + AwakeCluster { + cluster: ci as u32, + rotation: c.rotation, + target_linvel, + target_angvel, + com: Vector::ZERO, + rest_com: Vector::ZERO, + dyn_com: Vector::ZERO, + inv_inertia: zero_inertia(), + } + }) + .collect(); + + self.awake.push(AwakeSoftBody { + ptr: sb as *mut SoftBody, + handle, + group, + frozen, + slot_start: 0, + num_particles: sb.particles.len(), + shape_com: Vector::ZERO, + shape_constraints: 0..0, + volume_constraint: None, + damping_factor: 0.0, + plastic_flow: AtomicBool::new(false), + torn: AtomicBool::new(false), + contact_approach_speed: None, + }); + } + } + + /// Counts the constraints of awake body `ai` per color (scalar constraints, elastic block + /// constraints) and its shape-matching constraints: the exact numbers + /// `fill_body_constraints` emits. + fn count_body_constraints( + &self, + ai: usize, + counts: &mut [usize], + elastic_counts: &mut [usize], + range: Range, + ) { + let awake = &self.awake[ai]; + if awake.frozen { + // No solver DOF: no element, shape or volume constraint, nothing to prepare. + return; + } + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let (edges, dihedrals, cells) = element_ranges(sb, range); + #[cfg(feature = "dim2")] + let _ = dihedrals; + for e in &sb.edges { + counts[e.color as usize] += 1; + } + #[cfg(feature = "dim3")] + for d in &sb.dihedrals { + counts[d.color as usize] += 1; + } + let (mu, _) = sb.material.lame_parameters(); + for c in &sb.cells[cells] { + match sb.cell_model { + SoftBodyCellModel::Volume => counts[c.color as usize] += 1, + SoftBodyCellModel::Corotational | SoftBodyCellModel::NeoHookean => { + if mu > 0.0 && elastic_cell_terms(sb, slots, c).2 > 0.0 { + } + } + } + } + } + + /// The number of shape-matching constraints of awake body `ai`: one per free, slotted + /// particle of each shape-matched cluster (a particle in several such clusters gets one + /// constraint per cluster). + fn count_body_shape_constraints(&self, ai: usize) -> usize { + let awake = &self.awake[ai]; + if awake.frozen { + return 0; + } + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let mut count = 0; + count + } + + /// Builds awake body `ai`'s element constraints (substep length `dt`) into its slots: distance, + /// dihedral and cell constraints in element order, restricted to `range` of its element index + /// space (see [`element_ranges`]). Reads only the body's own soft body and the shared `slots`. + fn fill_body_constraints( + &self, + ai: usize, + dt: Real, + max_corrective_velocity: Real, + out: &mut BodyCursors, + range: Range, + ) { + let awake = &self.awake[ai]; + if awake.frozen { + return; + } + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let (edges, dihedrals, cells) = element_ranges(sb, range); + #[cfg(feature = "dim2")] + let _ = dihedrals; + let material = &sb.material; + let coeffs_of = |c: &SpringCoefficients| (c.erp_inv_dt(dt), c.cfm_coeff(dt)); + let (edge_erp, edge_cfm) = coeffs_of(&material.edge_softness); + let (bend_erp, bend_cfm) = coeffs_of(&material.bend_softness); + let (vol_erp, vol_cfm) = coeffs_of(&material.volume_softness); + let (mu, lambda) = material.lame_parameters(); + let elastic_zeta = material.elastic_damping_ratio; + let im_of = |i: u32| -> Real { + let s = slots[i as usize]; + if s == u32::MAX { + 0.0 + } else { + sb.particles[i as usize].inv_mass + } + }; + let pos_of = |i: u32| sb.particles[i as usize].position; + let base_constraint = |kind: SoftScalarConstraintKind, + writeback: SoftScalarConstraintWriteback, + element: usize, + vertices: &[u32], + rest: Real, + erp: Real, + cfm: Real, + impulse: Real| + -> SoftScalarConstraint { + let mut solver_ids = [u32::MAX; MAX_CONSTRAINT_PARTICLES]; + let mut pos = [Vector::ZERO; MAX_CONSTRAINT_PARTICLES]; + let mut im = [0.0; MAX_CONSTRAINT_PARTICLES]; + for (k, &v) in vertices.iter().enumerate() { + solver_ids[k] = slots[v as usize]; + pos[k] = pos_of(v); + im[k] = im_of(v); + } + SoftScalarConstraint { + kind, + num_particles: vertices.len() as u8, + writeback, + soft_body: ai as u32, + element: element as u32, + solver_ids, + pos, + im, + grad: [Vector::ZERO; MAX_CONSTRAINT_PARTICLES], + rest, + erp_inv_dt: erp, + cfm_coeff: cfm, + inv_lhs: 0.0, + cfm_gain: 0.0, + rhs: 0.0, + impulse, + impulse_bounds: [-Real::MAX, Real::MAX], + } + }; + + for (ei, e) in sb.edges.iter().enumerate() { + } + + #[cfg(feature = "dim3")] + for (di, d) in sb + .dihedrals + .iter() + .enumerate() + .take(dihedrals.end) + .skip(dihedrals.start) + .filter(|_| element_constraints) + { + let constraint = base_constraint( + SoftScalarConstraintKind::Dihedral, + SoftScalarConstraintWriteback::Dihedral, + di, + &d.vertices, + d.rest_angle, + bend_erp, + bend_cfm, + d.impulse, + ); + out.push_constraint(d.color, constraint); + } + + for (ci, c) in sb.cells.iter().enumerate().take(cells.end).skip(cells.start) { + match sb.cell_model { + SoftBodyCellModel::Volume => { + let constraint = base_constraint( + SoftScalarConstraintKind::CellVolume, + SoftScalarConstraintWriteback::CellVolume, + ci, + &c.vertices, + c.rest_volume, + vol_erp, + vol_cfm, + c.impulses[0], + ); + out.push_constraint(c.color, constraint); + } + SoftBodyCellModel::Corotational | SoftBodyCellModel::NeoHookean => { + let neo_hookean = sb.cell_model == SoftBodyCellModel::NeoHookean; + // Constraint-space stiffnesses 2μV₀ / 4μV₀ (diagonal / shear rows) and λV₀ + // (volumetric, at rest) map to natural frequencies via the effective masses, + // ω² = k · Σ w |∇C|²; Neo-Hookean damping/snap-back caps use (2μ + λ)V₀ / 4μV₀. + let vol = c.rest_volume.abs(); + let (coeffs, im, w_vol) = elastic_cell_terms(sb, slots, c); + if w_vol <= 0.0 || mu <= 0.0 { + // Every particle pinned, a degenerate cell, or no material. + continue; + } + let mut solver_ids = [u32::MAX; MAX_CONSTRAINT_PARTICLES]; + let mut pos = [Vector::ZERO; MAX_CONSTRAINT_PARTICLES]; + for (k, &v) in c.vertices.iter().enumerate() { + solver_ids[k] = slots[v as usize]; + pos[k] = pos_of(v); + } + let block = SoftElasticConstraint::strain_block(&coeffs, &im); + let two_pi = Real::simd_two_pi(); + // Snap-back caps: a strain error moves the particles at about + // `erp * strain * cell size` per second. + let cell_size = vol.powf(1.0 / DIM as Real).max(1.0e-6); + let strain_cap = |erp: Real| { + if erp > 0.0 { + max_corrective_velocity / (erp * cell_size) + } else { + Real::MAX + } + }; + let mut erp_strain = StrainVector::zeros(); + let mut cfm_strain = StrainVector::zeros(); + let mut softened = block; + let mut inert = [false; STRAIN_ROWS]; + let mut damping = StrainVector::zeros(); + for r in 0..STRAIN_ROWS { + let w = block[(r, r)]; + let k = if neo_hookean { + NeoHookeanConstraint::rest_stiffness(mu, lambda, r) * vol + } else if r < DIM { + 2.0 * mu * vol + } else { + 4.0 * mu * vol + }; + let omega = (k * w).sqrt(); + if omega > 0.0 { + let (erp, cfm) = + coeffs_of(&SpringCoefficients::new(omega / two_pi, elastic_zeta)); + erp_strain[r] = erp; + cfm_strain[r] = w * cfm; + softened[(r, r)] += cfm_strain[r]; + // Damping c = 2ζω/w (the constraint as a spring of mass 1/w). + damping[r] = 2.0 * elastic_zeta * omega / w; + } else { + inert[r] = true; + } + } + let strain_caps = StrainVector::from_fn(|r, _| strain_cap(erp_strain[r])); + let strain_impulse = StrainVector::from_fn(|r, _| c.impulses[r]); + let (inv_a, model, vol_impulse) = if neo_hookean { + let model = SoftElasticModel::NeoHookean(NeoHookeanConstraint { + dt, + mu: mu * vol, + lambda: (lambda + mu) * vol, + damping, + stiffness_strain: StrainVector::repeat(Real::MAX), + m: StrainMatrix::zeros(), + dt_grad: StrainVector::zeros(), + }); + (StrainMatrix::zeros(), model, 0.0) + } else { + let omega_vol = (lambda * vol * w_vol).sqrt(); + // A zero Poisson ratio disables the volumetric row (zero gain + // and rhs: its impulse stays zero). + let (erp_vol, cfm_coeff_vol) = if omega_vol > 0.0 { + coeffs_of(&SpringCoefficients::new(omega_vol / two_pi, elastic_zeta)) + } else { + (0.0, 0.0) + }; + let inv_a = SoftElasticConstraint::invert_block(softened, inert); + let model = SoftElasticModel::Corotational(CorotationalConstraint { + erp_strain, + cfm_strain, + erp_vol, + cfm_coeff_vol, + volumetric: omega_vol > 0.0, + vol_cap: strain_cap(erp_vol), + grad_vol: [Vector::ZERO; MAX_CONSTRAINT_PARTICLES], + cfm_gain_vol: 0.0, + rhs_vol: 0.0, + b: StrainVector::zeros(), + z: StrainVector::zeros(), + inv_s: 0.0, + }); + (inv_a, model, c.impulses[STRAIN_ROWS]) + }; + let constraint = SoftElasticConstraint { + soft_body: ai as u32, + element: ci as u32, + solver_ids, + pos, + im, + coeffs, + inv_rest_matrix: c.inv_rest_matrix, + rotation: c.rotation, + rot_mat: c.rotation.to_mat(), + inv_a, + strain_cap: strain_caps, + strain: StrainVector::zeros(), + strain_impulse, + vol_impulse, + model, + }; + out.push_elastic_constraint(c.color, constraint); + } + } + } + + } + + /// Builds the shape-matching constraints of awake body `ai` into its slots (see + /// [`Self::count_body_shape_constraints`]). + fn fill_body_shape_constraints(&self, ai: usize, dt: Real, out: &mut BodyCursors) { + let awake = &self.awake[ai]; + if awake.frozen { + return; + } + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let coeffs_of = |c: &SpringCoefficients| (c.erp_inv_dt(dt), c.cfm_coeff(dt)); + let (shape_erp, shape_cfm) = coeffs_of(&sb.material.shape_matching_softness); + } + } +} + +/// Elements per chunk of a large body's parallel fill. +#[cfg(feature = "parallel")] +const FILL_CHUNK: usize = 2048; + +/// The size of a body's element index space: its edges, then its dihedrals (3D), then its cells. +fn element_count(sb: &SoftBody) -> usize { + #[cfg(feature = "dim3")] + let dihedrals = sb.dihedrals.len(); + #[cfg(feature = "dim2")] + let dihedrals = 0; + sb.edges.len() + dihedrals + sb.cells.len() +} + +/// The edge, dihedral and cell index ranges covered by `range` of a body's element index space. +fn element_ranges( + sb: &SoftBody, + range: Range, +) -> (Range, Range, Range) { + let sub = |start: usize, len: usize| -> Range { + range.start.saturating_sub(start).min(len)..range.end.saturating_sub(start).min(len) + }; + let edges = sb.edges.len(); + #[cfg(feature = "dim3")] + let dihedrals = sb.dihedrals.len(); + #[cfg(feature = "dim2")] + let dihedrals = 0; + ( + sub(0, edges), + sub(edges, dihedrals), + sub(edges + dihedrals, sb.cells.len()), + ) +} + +/// An elastic cell's strain coefficients, its particles' inverse masses (zero without a solver +/// slot) and its rest volumetric weight `Σ_p w_p |∇_p C_V|²` (at rest `∇C_V` of particle `p` is +/// `Σ_j coeffs[p][j] e_j`); a zero weight (all pinned, degenerate cell) means no constraint. +#[inline] +fn elastic_cell_terms( + sb: &SoftBody, + slots: &[u32], + c: &crate::dynamics::SoftBodyCell, +) -> ([Vector; MAX_CONSTRAINT_PARTICLES], [Real; MAX_CONSTRAINT_PARTICLES], Real) { + let coeffs = SoftElasticConstraint::coefficients(&c.inv_rest_matrix); + let mut im = [0.0; MAX_CONSTRAINT_PARTICLES]; + for (k, &v) in c.vertices.iter().enumerate() { + if slots[v as usize] != u32::MAX { + im[k] = sb.particles[v as usize].inv_mass; + } + } + let mut w_vol = 0.0; + for (w, c) in im.iter().zip(coeffs.iter()) { + w_vol += w * c.length_squared(); + } + (coeffs, im, w_vol) +} + +/// A zero angular inertia (the cluster fit's initial value). +fn zero_inertia() -> crate::math::AngularInertia { + #[cfg(feature = "dim2")] + { + 0.0 + } + #[cfg(feature = "dim3")] + { + parry::utils::SdpMatrix3::zero() + } +} diff --git a/src/dynamics/solver/staged_island_solver/helpers.rs b/src/dynamics/solver/staged_island_solver/helpers.rs index 1db1fa7bf..f168b5df8 100644 --- a/src/dynamics/solver/staged_island_solver/helpers.rs +++ b/src/dynamics/solver/staged_island_solver/helpers.rs @@ -57,13 +57,16 @@ impl VelocitySolver { &mut self, params: &IntegrationParameters, island_bodies: &[RigidBodyHandle], + num_awake_soft_mass_points: usize, bodies: &mut RigidBodySet, multibodies: &mut MultibodyJointSet, ) { self.multibody_roots.clear(); self.solver_bodies.clear(); - let aligned_solver_bodies_len = island_bodies.len().div_ceil(SIMD_WIDTH) * SIMD_WIDTH; + // The island's rigid bodies, then the awake soft bodies' particles. + let aligned_solver_bodies_len = + (island_bodies.len() + num_awake_soft_mass_points).div_ceil(SIMD_WIDTH) * SIMD_WIDTH; self.solver_bodies.resize(aligned_solver_bodies_len); self.solver_vels_increment.clear(); diff --git a/src/dynamics/solver/staged_island_solver/init.rs b/src/dynamics/solver/staged_island_solver/init.rs index 20234e755..07c0637e7 100644 --- a/src/dynamics/solver/staged_island_solver/init.rs +++ b/src/dynamics/solver/staged_island_solver/init.rs @@ -10,7 +10,7 @@ use crate::dynamics::solver::reset_buffer_reusing; use crate::dynamics::solver::solver_contact_graph::{ContactRef, SolverContactGraph}; use crate::dynamics::{ IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, - RigidBodySet, + RigidBodySet, SoftBodySet, }; use crate::math::Real; use parry::math::SIMD_WIDTH; @@ -32,6 +32,7 @@ impl StagedIslandSolver { num_workers: usize, island_id: usize, counters: &mut Counters, + gravity: crate::math::Vector, base_params: &IntegrationParameters, islands: &IslandManager, bodies: &mut RigidBodySet, @@ -44,6 +45,10 @@ impl StagedIslandSolver { joint_indices: &[JointIndex], joint_assembly_epoch: u32, multibodies: &mut MultibodyJointSet, + soft_bodies: &mut SoftBodySet, + // The narrow phase and colliders: read-only, for the soft-body surface contacts. + narrow_phase: &crate::geometry::NarrowPhase, + colliders: &crate::geometry::ColliderSet, // The narrow-phase's per-body masks of persistent contact solver colors, // used to color the joints in the same color space as the contacts. contact_color_masks: &[u128], @@ -66,6 +71,7 @@ impl StagedIslandSolver { let num_substeps = base_params.num_solver_iterations + group.extra_iters as usize; self.groups.push(GroupLayout { bodies: group.body_range.clone(), + soft_slots: 0..0, chunks: 0..0, colors: 0..0, overflow: 0..0, @@ -81,6 +87,7 @@ impl StagedIslandSolver { let num_substeps = base_params.num_solver_iterations + max_extra; self.groups.push(GroupLayout { bodies: 0..island_bodies.len(), + soft_slots: 0..0, chunks: 0..0, colors: 0..0, overflow: 0..0, @@ -101,16 +108,60 @@ impl StagedIslandSolver { params.dt /= num_solver_iterations as Real; /* - * Serial pre-phase: solver buffers & multibodies, coloring, joint & generic - * constraints. (Plain solver bodies are initialized by the workers in the - * first parallel stage.) + * Serial pre-phase: soft-body rows and particle slots, solver buffers & multibodies, + * coloring, joint & generic constraints. (Plain solver bodies are initialized by the + * workers in the first parallel stage; the soft particles' solver bodies here.) */ + // Soft-body constraints: assembled every step from the awake soft bodies' elements (the warm + // start impulses persist on the elements themselves). Their particles take the solver + // slots after the island's rigid bodies, group-major. + { + let groups = &self.groups; + let group_of_slot = |slot: u32| -> usize { + groups + .iter() + .position(|g| g.bodies.contains(&(slot as usize))) + .unwrap_or(0) + }; + let group_dt = |gi: usize| groups[gi].dt; + self.soft_constraints.assemble( + island_id, + island_bodies.len(), + groups.len(), + group_of_slot, + group_dt, + base_params.dt, + params.max_corrective_velocity(), + bodies, + soft_bodies, + ); + } + for (g, sg) in self + .groups + .iter_mut() + .zip(self.soft_constraints.groups.iter()) + { + g.soft_slots = sg.slots.clone(); + } + let num_soft_slots = self.soft_constraints.slots.len(); self.velocity_solver.init_solver_buffers_and_multibodies( ¶ms, island_bodies, + num_soft_slots, bodies, multibodies, ); + if num_soft_slots > 0 { + let groups = &self.groups; + let vs = &mut self.velocity_solver; + self.soft_constraints.init_solver_bodies( + &mut vs.solver_bodies, + &mut vs.solver_vels_increment, + gravity, + base_params.dt, + |gi| groups[gi].dt, + ); + } // The persistent solver contact graph already holds two-body manifolds grouped by // (color, contact count) — colors touch pairwise-disjoint bodies, buckets slice straight @@ -127,9 +178,38 @@ impl StagedIslandSolver { set.simd_velocity_coulomb_constraints.clear(); set.simd_velocity_coulomb_constraints_builder.clear(); } + // Soft-body contact and attachment constraints (group-major, after the constraint layout above). + if num_soft_slots > 0 { + let groups = &self.groups; + let group_of_slot = |slot: u32| -> usize { + groups + .iter() + .position(|g| { + g.bodies.contains(&(slot as usize)) + || g.soft_slots.contains(&(slot as usize)) + }) + .unwrap_or(0) + }; + let group_dt = |gi: usize| groups[gi].dt; + self.soft_constraints.assemble_contacts( + island_id, + ¶ms, + narrow_phase, + colliders, + bodies, + soft_bodies, + group_of_slot, + group_dt, + ); + self.soft_constraints + .assemble_attachments(island_id, bodies, group_dt); + } + // Avoid spawning more workers than there is work to distribute. let num_two_body = graph.len() - graph.generic().len(); - let approx_chunks = num_two_body / SIMD_WIDTH + joint_indices.len() / 4; + let approx_chunks = num_two_body / SIMD_WIDTH + + joint_indices.len() / 4 + + (self.soft_constraints.rows.len() + self.soft_constraints.shape_rows.len()) / 8; let num_workers = num_workers.clamp(1, (approx_chunks / 16).max(1)); // Joints: colored like contacts and laid out color by color (scalar @@ -214,13 +294,13 @@ impl StagedIslandSolver { // bodies; linear chunking would alias a dynamic body across SIMD lanes and the // last-writer-wins scatter would drop an impulse. Body-mask grouping keeps each // chunk's lanes disjoint (ungroupable => 1-lane chunks); worker 0 solves serially. - self.overflow_scratch.clear(); - self.overflow_scratch.extend_from_slice(refs); + self.overflow_workspace.clear(); + self.overflow_workspace.extend_from_slice(refs); set.interaction_groups.clear_groups(); set.interaction_groups.group_manifold_refs( island_bodies.len(), store, - &self.overflow_scratch, + &self.overflow_workspace, ); self.overflow_chunk_refs .extend_from_slice(&set.interaction_groups.simd_ref_interactions); @@ -295,22 +375,22 @@ impl StagedIslandSolver { let mut split: Vec> = alloc::vec![Vec::new(); num_groups]; for (color, refs) in graph.buckets() { let is_overflow_color = (color as usize) == NUM_COLORS - 1; - for scratch in &mut split { - scratch.clear(); + for workspace in &mut split { + workspace.clear(); } for r in refs { split[group_of(r)].push(*r); } - for (gi, scratch) in split.iter().enumerate() { - if scratch.is_empty() { + for (gi, workspace) in split.iter().enumerate() { + if workspace.is_empty() { continue; } if is_overflow_color { - overflow_by_group[gi].extend_from_slice(scratch); + overflow_by_group[gi].extend_from_slice(workspace); } else { let start = self.grouped_chunk_refs.len() as u32; - self.grouped_chunk_refs.extend_from_slice(scratch); - runs[gi].push((color, start, scratch.len() as u32)); + self.grouped_chunk_refs.extend_from_slice(workspace); + runs[gi].push((color, start, workspace.len() as u32)); } } } @@ -483,6 +563,7 @@ impl StagedIslandSolver { velocity_solver: &mut self.velocity_solver as *mut _, joint_constraints: &mut self.joint_constraints as *mut _, contact_constraints: set as *mut _, + soft_constraints: &mut self.soft_constraints as *mut _, coulomb_builders: set.simd_velocity_coulomb_constraints_builder.as_mut_ptr(), coulomb_constraints: set.simd_velocity_coulomb_constraints.as_mut_ptr(), #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/staged_island_solver/joints.rs b/src/dynamics/solver/staged_island_solver/joints.rs index 0ce11e67f..2c5772d0c 100644 --- a/src/dynamics/solver/staged_island_solver/joints.rs +++ b/src/dynamics/solver/staged_island_solver/joints.rs @@ -55,7 +55,7 @@ impl StagedIslandSolver { // written-back impulses) go stale, and only matter with joint warm-starting on. if warmstart_joints { let joints = &mut self.joint_constraints; - // Joint-heavy scenes refresh thousands of independent builders: + // Joint-heavy scenes update thousands of independent builders: // run in parallel (this is the dominant serial-assembly cost on // the rain benchmark, ~0.23ms/step of ragdoll joints). #[cfg(feature = "parallel")] diff --git a/src/dynamics/solver/staged_island_solver/mod.rs b/src/dynamics/solver/staged_island_solver/mod.rs index e0c4fc419..0d31d7879 100644 --- a/src/dynamics/solver/staged_island_solver/mod.rs +++ b/src/dynamics/solver/staged_island_solver/mod.rs @@ -17,6 +17,7 @@ use core::sync::atomic::AtomicBool; use crate::dynamics::solver::contact_constraint::ContactConstraintsSet; use crate::dynamics::solver::interaction_groups::ParallelInteractionGroups; use crate::dynamics::solver::manifold_store::ManifoldStore; +use crate::dynamics::solver::soft_constraint::SoftConstraintsSet; use crate::dynamics::solver::solver_contact_graph::ContactRef; use crate::dynamics::solver::{JointConstraintsSet, VelocitySolver}; use crate::dynamics::{ @@ -116,6 +117,8 @@ fn chunk_at(segments: &[ChunkSegment], chunk_id: usize) -> [ContactRef; SIMD_WID struct GroupLayout { /// Solver-body slot range (== awake-island body index range). bodies: Range, + /// Solver-body slot range of the group's soft-body particles (after every rigid body's). + soft_slots: Range, /// Global chunk-id range (this group's parallel colors + overflow tail). chunks: Range, /// Index range into `color_ranges`. @@ -187,6 +190,9 @@ struct SharedCtx<'a> { velocity_solver: *mut VelocitySolver, joint_constraints: *mut JointConstraintsSet, contact_constraints: *mut ContactConstraintsSet, + /// The soft-body constraints of the awake soft bodies (empty when the step has none: every soft + /// stage is then skipped, identically on every worker). + soft_constraints: *mut SoftConstraintsSet, coulomb_builders: *mut ContactWithCoulombFrictionBuilder, coulomb_constraints: *mut ContactWithCoulombFriction, @@ -218,6 +224,7 @@ unsafe impl Sync for SharedCtx<'_> {} pub(crate) struct StagedIslandSolver { pub contact_constraints: ContactConstraintsSet, pub joint_constraints: JointConstraintsSet, + pub soft_constraints: SoftConstraintsSet, pub velocity_solver: VelocitySolver, /// The SIMD chunk layout: segments over the persistent bucket slices (and /// over `overflow_chunk_refs`), in global chunk-id order. @@ -248,7 +255,7 @@ pub(crate) struct StagedIslandSolver { joint_chunk_lanes: Vec<[JointIndex; SIMD_WIDTH]>, /// Constraint-row range of each SIMD joint chunk. joint_chunk_rows: Vec>, - /// Scratch: (row signature, joint index) of a color's SIMD-eligible joints. + /// Workspace: (row signature, joint index) of a color's SIMD-eligible joints. joint_sig_scratch: Vec<(u32, JointIndex)>, /// Scalar joints solved by worker 0: joints without a wide row formulation /// (motors, coupled limits), extra-solver-iterations joints, and the joints @@ -256,9 +263,9 @@ pub(crate) struct StagedIslandSolver { joint_overflow_scratch: Vec, /// Joint builders from colors too small to parallelize: solved by worker 0. joint_overflow_range: Range, - /// Scratch: the overflow-color manifolds handed to the greedy body-mask + /// Workspace: the overflow-color manifolds handed to the greedy body-mask /// grouper (snapshotted to sidestep an aliasing borrow of `contact_constraints`). - overflow_scratch: Vec, + overflow_workspace: Vec, /// Constraint-row range of each scalar joint builder in /// `joint_constraints.velocity_constraints` (a joint yields several rows which /// must be solved by a same worker since they touch the same bodies). @@ -297,6 +304,7 @@ impl StagedIslandSolver { Self { contact_constraints: ContactConstraintsSet::new(), joint_constraints: JointConstraintsSet::new(), + soft_constraints: SoftConstraintsSet::new(), velocity_solver: VelocitySolver::new(), chunk_segments: Vec::new(), overflow_chunk_refs: Vec::new(), diff --git a/src/dynamics/solver/staged_island_solver/solve.rs b/src/dynamics/solver/staged_island_solver/solve.rs index 7b005b134..b11cce6df 100644 --- a/src/dynamics/solver/staged_island_solver/solve.rs +++ b/src/dynamics/solver/staged_island_solver/solve.rs @@ -1,25 +1,64 @@ -//! One solve pass of a substep: joint colors, worker-0 joint overflow, contact -//! colors, then the worker-0 contact overflow (all completion-gated stages). +//! One solve pass of a substep: soft-body prepare, joint colors, worker-0 joint overflow, +//! soft-body colors/shape constraints/serial constraints, contact colors, then the worker-0 contact +//! overflow (all completion-gated stages). use crate::dynamics::IntegrationParameters; use crate::dynamics::solver::JointConstraintsSet; use crate::math::Real; -use super::{GroupLayout, SharedCtx, stage_batch}; +use super::{SharedCtx, stage_batch}; -/// One solve pass: joints (colored, then overflow + generic on worker 0) then -/// contact colors (parallel). Returns the caller's updated stage ordinal. +/// One solve pass: joints (colored, then overflow + generic on worker 0), soft-body rows, then +/// contact colors (parallel); returns the updated stage ordinal. `soft_update` updates the soft +/// constraints from the poses first; `soft_warmstart` is `Some(coefficient)` to warm-start them. pub(super) unsafe fn solve_pass( ctx: &SharedCtx, - group: &GroupLayout, + group_index: usize, worker_id: usize, mut stage: usize, wo_bias: bool, warmstart_joints: bool, + soft_update: bool, + soft_warmstart: Option, params: &IntegrationParameters, solved_dt: Real, ) -> usize { + let group = &ctx.groups[group_index]; let sync = ctx.sync; + // Soft-body work of this group. The set is shared and immutable in shape during the solve, so + // every worker takes the same branches (identical stage sequences). + // Soft rows are springs solved once per substep, in the biased pass only: solving them + // again in the relax pass (with or without their bias) leaves the two passes fighting over + // the same impulse, and the velocity read out after the relax pass carries the difference. + let soft_ref = unsafe { &*ctx.soft_constraints }; + let has_soft = !soft_ref.is_empty() && !wo_bias; + // Particle attachments are joints: solved in both passes, right after the joints. + let has_attachments = + !soft_ref.is_empty() && !soft_ref.groups[group_index].attachments.is_empty(); + /* + * Stage: soft-body prepare (parallel over the group's awake soft bodies): shape-matching + * goals and global-volume gradients from the current poses. + */ + if has_soft && soft_ref.group_needs_prepare(group_index, soft_update) { + let awake = soft_ref.groups[group_index].awake.clone(); + let stage_work = awake.len(); + let mut done = 0; + while let Some(claimed) = sync.claim(stage, &awake, 1, worker_id) { + // SAFETY: each awake body is claimed by exactly one worker per stage, and the + // prepare only writes that body's own records and its own particles' velocities + // (soft bodies share no particle). + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + let soft = unsafe { &mut *ctx.soft_constraints }; + let claimed_len = claimed.len(); + for ai in claimed { + soft.prepare_awake_body(ai, solver_bodies, soft_update); + } + done += claimed_len; + } + sync.complete(stage, done, stage_work); + stage = sync.sync(stage, stage_work); + } + // Optionally, friction can be solved only in the unbiased pass // ("no friction when applying bias"), unless there is no unbiased pass. let solve_friction = wo_bias @@ -68,11 +107,11 @@ pub(super) unsafe fn solve_pass( if ctx.use_twist { let c = unsafe { &mut *ctx.twist_constraints.add(chunk_id) }; if wo_bias { - // Positions were integrated after the biased pass: refresh the unbiased rhs + // Positions were integrated after the biased pass: update the unbiased rhs // from the current poses (reusing pre-integration separations // destabilizes stack rocking). let builder = unsafe { &*ctx.twist_builders.add(chunk_id) }; - builder.refresh_rhs_wo_bias(params, solved_dt, solver_bodies, c); + builder.update_rhs_wo_bias(params, solved_dt, solver_bodies, c); } c.solve(solver_bodies, true, solve_friction); } @@ -80,7 +119,7 @@ pub(super) unsafe fn solve_pass( let c = unsafe { &mut *ctx.coulomb_constraints.add(chunk_id) }; if wo_bias { let builder = unsafe { &*ctx.coulomb_builders.add(chunk_id) }; - builder.refresh_rhs_wo_bias(params, solved_dt, solver_bodies, c); + builder.update_rhs_wo_bias(params, solved_dt, solver_bodies, c); } c.solve(solver_bodies, true, solve_friction); } @@ -124,15 +163,27 @@ pub(super) unsafe fn solve_pass( stage = sync.sync(stage, virt.end); } - // Overflow + generic joints, solved exclusively by worker 0 (they write body - // velocities, so no other worker may access solver bodies concurrently: they - // wait at the barrier). Skipped entirely when empty (the jointless case). - if !group.joint_overflow.is_empty() || has_generic_joints { + // Overflow + generic joints and soft-body particle attachments, solved exclusively by + // worker 0 (they write body velocities, so no other worker may access solver bodies + // concurrently: they wait at the barrier). Skipped entirely when empty (the jointless case). + if !group.joint_overflow.is_empty() || has_generic_joints || has_attachments { if worker_id == 0 { for joint_id in group.joint_overflow.clone() { solve_joint(joint_id); } + if has_attachments { + let soft = unsafe { &mut *ctx.soft_constraints }; + let vs = unsafe { &mut *ctx.velocity_solver }; + soft.solve_attachments( + group_index, + &mut vs.solver_bodies, + wo_bias, + soft_update, + soft_warmstart, + ); + } + if has_generic_joints { let joints = unsafe { &mut *ctx.joint_constraints }; let vs = unsafe { &mut *ctx.velocity_solver }; @@ -157,6 +208,24 @@ pub(super) unsafe fn solve_pass( stage = sync.sync(stage, 1); } + /* + * Soft-body stages, after the joints and before the contacts: contacts must win a body's + * velocity residual over the soft body's own structure (constraints strained past + * `soft_bodies.resweep_strain` are swept again after the contacts, at the end of the pass). + */ + if has_soft { + stage = unsafe { + solve_soft_constraints( + ctx, + group_index, + worker_id, + stage, + soft_update, + soft_warmstart, + ) + }; + } + // Contact color stages (ascending color id). for (_, chunk_range) in &ctx.color_ranges[group.colors.clone()] { let virt = 0..chunk_range.end - chunk_range.start; @@ -178,6 +247,37 @@ pub(super) unsafe fn solve_pass( stage = sync.sync(stage, virt.end); } + // Soft-body surface contact stages (constraints over the surface's particles and the other body), + // solved in both passes like every contact: one stage per color of chunks holding + // pairwise-disjoint bodies (see `soft_contact_chunks`), then the serial tail below. + let has_soft_contacts = + !soft_ref.is_empty() && !soft_ref.groups[group_index].contacts.is_empty(); + if has_soft_contacts { + let sg = &soft_ref.groups[group_index]; + for chunk_range in &soft_ref.contact_colors[sg.contact_colors.clone()] { + let mut done = 0; + while let Some(claimed) = sync.claim(stage, chunk_range, 1, worker_id) { + // SAFETY: chunks of a same color touch pairwise-disjoint solver bodies. + let soft = unsafe { &mut *ctx.soft_constraints }; + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + let claimed_len = claimed.len(); + for chunk in claimed { + soft.solve_contact_chunk( + chunk, + solver_bodies, + params, + wo_bias, + soft_update, + soft_warmstart.is_some(), + ); + } + done += claimed_len; + } + sync.complete(stage, done, chunk_range.len()); + stage = sync.sync(stage, chunk_range.len()); + } + } + // Overflow + generic contacts, solved exclusively by worker 0. Unlike the // joint overflow stage this one is unconditional: it doubles as the pass' // trailing barrier so every worker leaves solve_pass in lockstep. @@ -186,6 +286,22 @@ pub(super) unsafe fn solve_pass( solve_chunk(chunk_id); } + // The serial tail of the soft contacts. + if has_soft_contacts { + let soft = unsafe { &mut *ctx.soft_constraints }; + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + for chunk in soft.groups[group_index].contact_serial.clone() { + soft.solve_contact_chunk( + chunk, + solver_bodies, + params, + wo_bias, + soft_update, + soft_warmstart.is_some(), + ); + } + } + if has_generic_contacts { let contacts = unsafe { &mut *ctx.contact_constraints }; let vs = unsafe { &mut *ctx.velocity_solver }; @@ -205,5 +321,108 @@ pub(super) unsafe fn solve_pass( } sync.complete(stage, 1, 1); } - sync.sync(stage, 1) + stage = sync.sync(stage, 1); + + if has_soft && ctx.base_params.soft_bodies.resweep_strain < Real::MAX { + // Second sweep of the strained soft constraints after the contacts (already updated and + // warm-started by the first sweep): a torn body's structure gets the substep's last word; + // the rest stay with the contacts (re-solving them re-violates contacts). Worker 0, serial. + if worker_id == 0 { + let soft = unsafe { &mut *ctx.soft_constraints }; + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + soft.resweep_strained_constraints(group_index, solver_bodies); + sync.complete(stage, 1, 1); + } + stage = sync.sync(stage, 1); + } + stage +} + +/// The soft-body constraint stages of one pass: colored element constraints (parallel within a color), +/// shape-matching constraints (parallel: one particle each), then the serial tail on worker 0 +/// (overflow-color constraints and the global-volume constraints). Returns the updated stage ordinal. +unsafe fn solve_soft_constraints( + ctx: &SharedCtx, + group_index: usize, + worker_id: usize, + mut stage: usize, + soft_update: bool, + soft_warmstart: Option, +) -> usize { + let sync = ctx.sync; + let soft_ref = unsafe { &*ctx.soft_constraints }; + let sg = soft_ref.groups[group_index].clone(); + let min_strain = ctx.base_params.soft_bodies.resweep_strain; + for color_range in &soft_ref.color_ranges[sg.colors.clone()] { + let virt = 0..color_range.len(); + let mut done = 0; + while let Some(claimed) = sync.claim( + stage, + &virt, + stage_batch(virt.end, sync.num_workers), + worker_id, + ) { + // SAFETY: rows of a same color touch pairwise-disjoint particles. + let soft = unsafe { &mut *ctx.soft_constraints }; + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + let claimed_len = claimed.len(); + for idx in claimed { + soft.solve_color_constraint( + color_range, + idx, + solver_bodies, + soft_update, + soft_warmstart, + min_strain, + ); + } + done += claimed_len; + } + sync.complete(stage, done, virt.end); + stage = sync.sync(stage, virt.end); + } + + if !sg.shape_rows.is_empty() { + let mut done = 0; + while let Some(claimed) = sync.claim( + stage, + &sg.shape_constraints, + stage_batch(sg.shape_constraints.len(), sync.num_workers), + worker_id, + ) { + // SAFETY: without per-cluster shape matching, shape constraints touch one particle each, + // all distinct. + let soft = unsafe { &mut *ctx.soft_constraints }; + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + let claimed_len = claimed.len(); + for constraint_id in claimed { + } + done += claimed_len; + } + sync.complete(stage, done, sg.shape_constraints.len()); + stage = sync.sync(stage, sg.shape_constraints.len()); + } + + if sg.serial.len() > 0 || !sg.volume_rows.is_empty() { + if worker_id == 0 { + let soft = unsafe { &mut *ctx.soft_constraints }; + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + for idx in 0..sg.serial.len() { + soft.solve_color_constraint( + &sg.serial, + idx, + solver_bodies, + soft_update, + soft_warmstart, + min_strain, + ); + } + for vi in sg.volume_constraints.clone() { + soft.solve_volume_constraint(vi, solver_bodies, soft_warmstart); + } + sync.complete(stage, 1, 1); + } + stage = sync.sync(stage, 1); + } + stage } diff --git a/src/dynamics/solver/staged_island_solver/worker.rs b/src/dynamics/solver/staged_island_solver/worker.rs index df54ff1df..6146acab2 100644 --- a/src/dynamics/solver/staged_island_solver/worker.rs +++ b/src/dynamics/solver/staged_island_solver/worker.rs @@ -204,7 +204,8 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { // constraint-generation barrier so every worker agrees. Bounce-free steps (the common // case) skip the end-of-step restitution stages entirely. let has_bouncy = unsafe { &*ctx.any_bouncy }.load(Ordering::Relaxed); - for group in ctx.groups { + let has_soft = !unsafe { &*ctx.soft_constraints }.is_empty(); + for (group_index, group) in ctx.groups.iter().enumerate() { // This group's substep parameters: identical to `base_params` except // for the substep dt. Everything dt-derived downstream (soft erp/cfm, // rhs, speculative terms, integration) recomputes from this copy. @@ -216,7 +217,18 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { // (island slots only — the omitted padding tail slots hold zero velocities and // increments, so skipping them is behavior-preserving). let group_bodies = group.bodies.clone(); - let num_group_bodies = group_bodies.len(); + // The per-slot stages (velocity increments, integration) cover the group's rigid slots + // then its soft-body particle slots, through one virtual index range. + let group_soft_slots = group.soft_slots.clone(); + let num_group_bodies = group_bodies.len() + group_soft_slots.len(); + let group_slots = 0..num_group_bodies; + let slot_of = |i: usize| -> usize { + if i < group_bodies.len() { + group_bodies.start + i + } else { + group_soft_slots.start + (i - group_bodies.len()) + } + }; let num_joint_chunks = group.joint_chunks.len(); let joint_builders = group.joint_builders.clone(); // Update-stage virtual claim domain: this group's contact chunks, @@ -236,10 +248,11 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { // +1: worker 0's generic increments. let stage_work = num_group_bodies + 1; let mut done = 0; - while let Some(claimed) = sync.claim(stage, &group_bodies, BODY_BATCH, worker_id) { + while let Some(claimed) = sync.claim(stage, &group_slots, BODY_BATCH, worker_id) { let vs = unsafe { &mut *ctx.velocity_solver }; let claimed_len = claimed.len(); for i in claimed { + let i = slot_of(i); let incr = vs.solver_vels_increment[i]; { let vels = &mut vs.solver_bodies.vels[i]; @@ -546,14 +559,18 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { // joint applies its carried impulse right before being solved (sound // within a color; Gauss-Seidel-ordered across colors). let warmstart_joints = params.warmstart_joints && pgs_iter == 0; + let soft_warmstart = (pgs_iter == 0 && params.warmstart_coefficient != 0.0) + .then_some(params.warmstart_coefficient); stage = unsafe { solve_pass( ctx, - group, + group_index, worker_id, stage, false, warmstart_joints, + pgs_iter == 0, + soft_warmstart, params, solved_dt, ) @@ -575,10 +592,11 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { let max_ang = MAX_ROTATION * base_params.inv_dt(); let mut done = 0; - while let Some(claimed) = sync.claim(stage, &group_bodies, BODY_BATCH, worker_id) { + while let Some(claimed) = sync.claim(stage, &group_slots, BODY_BATCH, worker_id) { let vs = unsafe { &mut *ctx.velocity_solver }; let claimed_len = claimed.len(); for i in claimed { + let i = slot_of(i); { let vel = &mut vs.solver_bodies.vels[i]; if max_lin != Real::MAX { @@ -633,15 +651,17 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { /* * Stages: solve without bias. */ - for _ in 0..params.num_internal_stabilization_iterations { + for stab_iter in 0..params.num_internal_stabilization_iterations { stage = unsafe { solve_pass( ctx, - group, + group_index, worker_id, stage, true, false, + stab_iter == 0, + None, params, solved_dt + params.dt, ) @@ -651,7 +671,7 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { /* * Stages: end-of-step restitution, color by color (box2d-style). Applied ONCE after - * all substeps, gated on the point having carried an impulse: inside the substep rhs + * all substeps, gated on the point having applied an impulse: inside the substep rhs * the speculative `dist * inv_dt` slack would truncate the bounce. */ if has_bouncy { @@ -801,6 +821,46 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { stage = sync.sync(stage, num_chunks + 1); } + /* + * Stage: write the soft-body impulses back to the soft bodies (parallel over constraints) and + * the solved particles back to their bodies (parallel over awake bodies; the virtual claim + * range covers the constraints then the bodies); worker 0 also writes back the per-body state. + */ + if has_soft { + let soft = unsafe { &*ctx.soft_constraints }; + let num_constraints = soft.num_element_constraints(); + let all_units = 0..num_constraints + soft.awake.len(); + let stage_work = all_units.len() + 1; + let mut done = 0; + while let Some(claimed) = sync.claim( + stage, + &all_units, + stage_batch(all_units.end, sync.num_workers), + worker_id, + ) { + let claimed_len = claimed.len(); + let constraints = claimed.start.min(num_constraints)..claimed.end.min(num_constraints); + let awake = claimed.start.max(num_constraints) - num_constraints + ..claimed.end.max(num_constraints) - num_constraints; + // SAFETY: each constraint is claimed by exactly one worker and writes its own element; each + // awake body likewise writes its own particles. + unsafe { + soft.writeback_constraints(constraints, ctx.base_params.dt); + let vs = &*ctx.velocity_solver; + for ai in awake { + soft.writeback_particles(ai, &vs.solver_bodies, ctx.base_params.dt); + } + } + done += claimed_len; + } + sync.complete(stage, done, stage_work); + if worker_id == 0 { + soft.writeback_bodies(); + sync.complete(stage, 1, stage_work); + } + stage = sync.sync(stage, stage_work); + } + /* * Stage: write velocities and poses back to the rigid-bodies (parallel over * claimed island bodies; each writes its own rigid-body). The multibody diff --git a/src/geometry/broad_phase_bvh/mod.rs b/src/geometry/broad_phase_bvh/mod.rs index 26c8be3f3..85c2f096b 100644 --- a/src/geometry/broad_phase_bvh/mod.rs +++ b/src/geometry/broad_phase_bvh/mod.rs @@ -36,21 +36,21 @@ pub struct BroadPhaseBvh { /// stale-pair detection examine only pairs adjacent to changed colliders instead of /// re-scanning the whole `pairs` map (a pair can only stop overlapping if one side changed). pair_adjacency: Coarena>, - /// Scratch buffer holding the colliders whose AABB was updated in the tree + /// Workspace buffer holding the colliders whose AABB was updated in the tree /// during the last `update` call. #[cfg_attr(feature = "serde-serialize", serde(skip))] updated_colliders: Vec, - /// Scratch buffer holding the leaf pairs reported by the tree traversal. Only the + /// Workspace buffer holding the leaf pairs reported by the tree traversal. Only the /// sequential traversal needs it (it reports through a closure); the parallel one /// returns its own vector. #[cfg(not(feature = "parallel"))] #[cfg_attr(feature = "serde-serialize", serde(skip))] - candidates_scratch: Vec<(u32, u32)>, - /// Scratch: per-collider "was updated this step" bit (collider arena index), so the + candidates_workspace: Vec<(u32, u32)>, + /// Workspace: per-collider "was updated this step" bit (collider arena index), so the /// stale-pair scan can visit a pair from one side only when both sides moved. #[cfg_attr(feature = "serde-serialize", serde(skip))] updated_mask: Vec, - /// Scratch buffer holding the stale pairs detected during `update`. + /// Workspace buffer holding the stale pairs detected during `update`. /// /// The boolean indicates if a `DeletePair` event must be emitted for the pair. #[cfg_attr(feature = "serde-serialize", serde(skip))] @@ -75,14 +75,14 @@ pub struct BroadPhaseBvh { /// SAH re-insertion (tree quality without an O(tree) optimizer/refit pass); bulk regimes keep cheaper /// in-place updates + the periodic optimizer. One step of hysteresis: `set_aabb` runs between updates. reinsert_leaf_updates: bool, - /// Scratch buffer for the precomputed leaf updates of [`Self::update`]. + /// Workspace buffer for the precomputed leaf updates of [`Self::update`]. #[cfg_attr(feature = "serde-serialize", serde(skip))] - update_scratch: Vec<(ColliderHandle, Aabb, Real)>, + update_workspace: Vec<(ColliderHandle, Aabb, Real)>, /// Workspace of the parallel leaf-update batches (the tree API takes raw /// leaf indices). #[cfg(feature = "parallel")] #[cfg_attr(feature = "serde-serialize", serde(skip))] - update_batch_scratch: Vec<(Aabb, u32, Real)>, + update_batch_workspace: Vec<(Aabb, u32, Real)>, #[cfg(feature = "parallel")] #[cfg_attr(feature = "serde-serialize", serde(skip))] update_batch_statuses: Vec, @@ -272,12 +272,12 @@ mod test { use crate::prelude::{ CCDSolver, ColliderBuilder, ColliderSet, DefaultBroadPhase, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, NarrowPhase, PhysicsPipeline, - RigidBodyBuilder, RigidBodySet, + RigidBodyBuilder, RigidBodySet, SoftBodySet, }; /// With the adaptive change-detection margin enabled, collisions must still be /// detected and resolved like with the fixed margin (the margin only affects how - /// often tree leaves are refreshed, not which pairs eventually collide). + /// often tree leaves are updated, not which pairs eventually collide). #[test] fn adaptive_change_detection_margin_smoke() { let mut final_ys = [0.0; 2]; @@ -287,6 +287,7 @@ mod test { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut islands = IslandManager::new(); let mut broad_phase = DefaultBroadPhase::new(); @@ -311,6 +312,7 @@ mod test { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &(), &(), diff --git a/src/geometry/broad_phase_bvh/update.rs b/src/geometry/broad_phase_bvh/update.rs index 42b040604..46e69a77b 100644 --- a/src/geometry/broad_phase_bvh/update.rs +++ b/src/geometry/broad_phase_bvh/update.rs @@ -95,8 +95,8 @@ impl BroadPhaseBvh { // The AABB (and margin) computation is the expensive part of the leaf-update // loop; precompute it in parallel and keep only the tree writes sequential. - let mut update_scratch = core::mem::take(&mut self.update_scratch); - update_scratch.clear(); + let mut update_workspace = core::mem::take(&mut self.update_workspace); + update_workspace.clear(); let compute_update = |modified: &ColliderHandle| -> Option<(ColliderHandle, Aabb, Real)> { let collider = colliders.get(*modified)?; @@ -131,17 +131,17 @@ impl BroadPhaseBvh { .par_chunks(1024) .map(|chunk| chunk.iter().filter_map(compute_update).collect()) .collect(); - update_scratch.extend(precomputed.into_iter().flatten()); + update_workspace.extend(precomputed.into_iter().flatten()); } #[cfg(not(feature = "parallel"))] - update_scratch.extend(modified_colliders.iter().filter_map(compute_update)); + update_workspace.extend(modified_colliders.iter().filter_map(compute_update)); // Small change volumes relocate moved leaves via SAH re-insertion (O(log n) per leaf): // tree quality maintains itself and the periodic O(tree) optimizer never runs — what keeps // huge mostly-static scenes free of multi-ms spikes. Bulk volumes keep O(1) in-place updates. let leaf_count = self.tree.leaf_count() as usize; let use_reinsert = - self.reinsert_leaf_updates && update_scratch.len() * 16 < leaf_count && !first_pass; + self.reinsert_leaf_updates && update_workspace.len() * 16 < leaf_count && !first_pass; // In-place leaf updates apply in parallel; the change-flag bookkeeping // below stays sequential (it's a cheap push per *changed* leaf). @@ -149,19 +149,19 @@ impl BroadPhaseBvh { let parallel_leaf_updates = !use_reinsert; #[cfg(feature = "parallel")] if parallel_leaf_updates { - self.update_batch_scratch.clear(); - self.update_batch_scratch.extend( - update_scratch + self.update_batch_workspace.clear(); + self.update_batch_workspace.extend( + update_workspace .iter() .map(|(handle, aabb, skin)| (*aabb, handle.into_raw_parts().0, *skin)), ); self.tree.insert_or_update_batch_partially_parallel( - &self.update_batch_scratch, + &self.update_batch_workspace, &mut self.update_batch_statuses, ); for ((modified, _, _), status) in - update_scratch.iter().zip(self.update_batch_statuses.iter()) + update_workspace.iter().zip(self.update_batch_statuses.iter()) { let leaf_index = modified.into_raw_parts().0; match status { @@ -193,7 +193,7 @@ impl BroadPhaseBvh { // mixes moved colliders with newly added ones would otherwise build a // different tree here than it does there. let mut deferred_inserts: Vec = Vec::new(); - for (i, (modified, aabb, change_detection_skin)) in update_scratch.iter().enumerate() { + for (i, (modified, aabb, change_detection_skin)) in update_workspace.iter().enumerate() { let leaf_index = modified.into_raw_parts().0; // `..._if_present` reports a missing leaf through the lookup it already // performs, so deferring insertions costs no extra probe. @@ -230,11 +230,11 @@ impl BroadPhaseBvh { } } - // Pass 2: the structural insertions, in `update_scratch` order. Fresh + // Pass 2: the structural insertions, in `update_workspace` order. Fresh // insertions pick their spot by SAH descent, so they never count toward the // optimizer's debt, and their status is always `Inserted`. for i in deferred_inserts { - let (modified, aabb, change_detection_skin) = &update_scratch[i]; + let (modified, aabb, change_detection_skin) = &update_workspace[i]; let leaf_index = modified.into_raw_parts().0; let status = self.tree @@ -245,7 +245,7 @@ impl BroadPhaseBvh { } } - self.update_scratch = update_scratch; + self.update_workspace = update_workspace; // The incremental optimizer (and its O(tree) full refit) only runs when enough quality-degrading // changes accumulated: every frame under bulk volumes, every 8th for moderate, never for small @@ -306,7 +306,7 @@ impl BroadPhaseBvh { // sequential tail only pays for genuinely new pairs. // // The probe is read-only in every build. The alternative (a sequential collector - // refreshing each visited pair's timestamp, so stale-pair detection could skip it + // updating each visited pair's timestamp, so stale-pair detection could skip it // with an integer compare) cannot run concurrently, and its map writes are part of // the serialized broad-phase state: keeping it would make the two builds' snapshots // differ even on an identical simulation. @@ -320,7 +320,7 @@ impl BroadPhaseBvh { // Reused across steps: the sequential walk reports through a closure, so // collecting it into the same shape the parallel walk returns costs nothing // beyond the (amortized) buffer. - let mut candidates = core::mem::take(&mut self.candidates_scratch); + let mut candidates = core::mem::take(&mut self.candidates_workspace); candidates.clear(); self.tree .traverse_bvtt_single_tree::<{ Self::CHANGE_DETECTION_ENABLED }>( @@ -372,6 +372,7 @@ impl BroadPhaseBvh { }; let rb_type1 = rb_type(collider1); let rb_type2 = rb_type(collider2); + if !collider1 .flags .active_collision_types @@ -428,7 +429,7 @@ impl BroadPhaseBvh { #[cfg(not(feature = "parallel"))] { - self.candidates_scratch = candidates; + self.candidates_workspace = candidates; } /* @@ -442,7 +443,7 @@ impl BroadPhaseBvh { // changed in the tree, so only pairs adjacent to updated/removed colliders are // checked. (A linear scan of the whole pair map used to be the fallback when most // colliders moved, but it was only worth it thanks to a per-pair timestamp - // refreshed by the sequential pair collector — a map write the parallel collector + // updated by the sequential pair collector: a map write the parallel collector // cannot do, and part of the serialized broad-phase state. One scan for every // build is both simpler and what the parallel build already did.) self.stale_pairs.clear(); diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 8f884fe4a..0940e5745 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -59,6 +59,9 @@ pub struct Collider { pub(crate) flags: ColliderFlags, contact_skin: Real, contact_force_event_threshold: Real, + /// Reference to the soft body associated to this collider's shape if it is deformable; the soft + /// body keeps its mesh (automatic skinning, tearing) and syncs it into the shape each frame. + pub(crate) deformable_mesh_ref: Option, /// User-defined data associated to this collider. pub user_data: u128, } @@ -66,6 +69,41 @@ pub struct Collider { impl Collider { pub(crate) fn reset_internal_references(&mut self) { self.changes = ColliderChanges::all(); + self.deformable_mesh_ref = None; + } + + /// The soft-body collision mesh this collider is associated to, if any. + pub fn deformable_mesh_ref(&self) -> Option { + self.deformable_mesh_ref + } + + /// Whether this collider is associated to a soft body's deformable collision mesh. + pub(crate) fn is_deformable_collider(&self) -> bool { + self.deformable_mesh_ref.is_some() + } + + /// 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)) { + f(self.shape.make_mut()); + self.changes.insert(ColliderChanges::DEFORMED); + } + + /// Replaces the shape of a soft-body surface collider whose topology changed (a tear), as a + /// deformation: no narrow-phase pair workspace invalidation, no wake-up. + pub(crate) fn replace_deformed_shape(&mut self, shape: SharedShape) { + self.shape = shape; + self.changes.insert(ColliderChanges::DEFORMED); + } + + /// Moves a soft-body particle ball to its particle's world position (a simulation-driven + /// motion, like a deformation: no wake-up, no narrow-phase workspace invalidation). `frame` + /// is the parent proxy's pose: the ball's local pose is kept consistent with it. + pub(crate) fn deform_position(&mut self, position: Vector) { + if let Some(parent) = self.parent.as_mut() { + parent.pos_wrt_parent = Pose::from_translation(position); + } + self.pos = ColliderPosition(Pose::from_translation(position)); } pub(crate) fn effective_contact_force_event_threshold(&self) -> Real { @@ -130,6 +168,7 @@ impl Collider { contact_force_event_threshold, user_data, contact_skin, + deformable_mesh_ref: _soft_mesh, // The soft-body markers belong to the collider's identity. } = other; if self.parent.is_none() { @@ -574,9 +613,11 @@ impl Collider { params: &IntegrationParameters, bodies: &RigidBodySet, ) -> Aabb { + let parent = self + .parent + .and_then(|p| Some((p, bodies.get(p.handle)?))); // Take soft-ccd into account by growing the aabb. - let next_pose = self.parent.and_then(|p| { - let parent = bodies.get(p.handle)?; + let next_pose = parent.and_then(|(p, parent)| { (parent.soft_ccd_prediction() > 0.0).then(|| { parent.predict_position_using_velocity_and_forces_with_max_dist( params.dt, @@ -584,9 +625,11 @@ impl Collider { ) * p.pos_wrt_parent }) }); + let soft_motion_margin = parent.map_or(0.0, |(_, parent)| parent.soft_motion_margin); let prediction_distance = params.prediction_distance(); - let mut aabb = self.compute_collision_aabb(prediction_distance / 2.0); + let mut aabb = + self.compute_collision_aabb(prediction_distance / 2.0 + soft_motion_margin); if let Some(next_pose) = next_pose { let next_aabb = self .shape @@ -896,6 +939,18 @@ impl ColliderBuilder { Self::new(SharedShape::polyline(vertices, indices)) } + /// Initializes a collider builder with a polyline shape defined by its vertex and index + /// buffers and the flags controlling its optional data (orientation, deformability). + pub fn polyline_with_flags( + vertices: Vec, + indices: Option>, + flags: parry::shape::PolylineFlags, + ) -> Self { + Self::new(SharedShape::new(parry::shape::Polyline::with_flags( + vertices, indices, flags, + ))) + } + /// Initializes a collider builder with an **oriented** (one-sided) polyline shape. /// /// Unlike [`Self::polyline`], the segments only collide from their outward side, determined by @@ -1450,6 +1505,7 @@ impl ColliderBuilder { coll_type, contact_force_event_threshold: self.contact_force_event_threshold, contact_skin: self.contact_skin, + deformable_mesh_ref: None, user_data: self.user_data, } } diff --git a/src/geometry/collider_components.rs b/src/geometry/collider_components.rs index 91e04a945..b2e9b862e 100644 --- a/src/geometry/collider_components.rs +++ b/src/geometry/collider_components.rs @@ -31,6 +31,10 @@ bitflags::bitflags! { const PARENT_EFFECTIVE_DOMINANCE = 1 << 7; // NF update. /// Flag indicating that whether or not the collider is enabled was changed. const ENABLED_OR_DISABLED = 1 << 8; // BF & NF updates. + /// Flag indicating that the collider's shape was deformed in place (a soft-body surface + /// following its particles): BF & NF updates, no NF pair workspace invalidation, never + /// recycled. + const DEFORMED = 1 << 9; } } @@ -47,7 +51,8 @@ impl ColliderChanges { ColliderChanges::PARENT | ColliderChanges::POSITION | ColliderChanges::SHAPE - | ColliderChanges::ENABLED_OR_DISABLED, + | ColliderChanges::ENABLED_OR_DISABLED + | ColliderChanges::DEFORMED, ) } diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 2de280391..1e2ea3bf6 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -322,11 +322,13 @@ impl ColliderSet { /// # let mut bodies = RigidBodySet::new(); /// # let mut islands = IslandManager::new(); /// # let body_handle = bodies.insert(RigidBodyBuilder::dynamic().build()); + /// # let mut soft_bodies = SoftBodySet::new(); /// # let handle = colliders.insert_with_parent(ColliderBuilder::ball(0.5).build(), body_handle, &mut bodies); /// if let Some(collider) = colliders.remove( /// handle, /// &mut islands, /// &mut bodies, + /// &mut soft_bodies, /// true // Wake up the parent body /// ) { /// println!("Removed collider with shape: {:?}", collider.shared_shape()); diff --git a/src/geometry/contact_clustering.rs b/src/geometry/contact_clustering.rs index 04e0e2fe2..5761494e1 100644 --- a/src/geometry/contact_clustering.rs +++ b/src/geometry/contact_clustering.rs @@ -4,7 +4,7 @@ //! giving the solver redundant constraints for one contact plane. This merges them into //! per-normal "cluster" manifolds rebuilt every frame — only clusters reach the solver; the //! per-subshape manifolds stay untouched for user-facing queries and events. Warm-start impulses -//! live in the cluster points, carried by nearest-position matching (cluster identity is unstable). +//! live in the cluster points, kept by nearest-position matching (cluster identity is unstable). use crate::alloc_prelude::*; use crate::geometry::{ContactManifold, ContactManifoldData}; @@ -14,7 +14,7 @@ use crate::math::{Real, Vector}; const COS_MERGE_ANGLE: Real = 0.996; /// Hard cap on points per cluster (the solver stores point indices as `u8`). Reduction /// selects at most 4 for the constraints; keeping all deduplicated points until then makes -/// selection and warm-start carry independent of the manifold iteration order. +/// selection and warm-start transfer independent of the manifold iteration order. const MAX_CLUSTER_POINTS: usize = 255; fn manifold_normal1(manifold: &ContactManifold) -> Vector { @@ -28,7 +28,7 @@ fn has_warmstart_data(data: &crate::geometry::ContactData) -> bool { data.impulse != 0.0 || data.warmstart_impulse != 0.0 } -/// Rebuilds `out` as the cluster manifolds for `manifolds`, carrying warm-start data from +/// Rebuilds `out` as the cluster manifolds for `manifolds`, transferring warm-start data from /// `prev` (the clusters solved at the previous step). Buffers in `out` are reused. pub(crate) fn cluster_manifolds_for_solver( manifolds: &[ContactManifold], @@ -90,7 +90,7 @@ pub(crate) fn cluster_manifolds_for_solver( if let Some(pos2) = manifold.subshape_pos2() { pt.local_p2 = *pos2 * pt.local_p2; } - // The warm-start data is carried from `prev` below, not from the + // The warm-start data is transferred from `prev` below, not from the // per-subshape manifolds (the solver never writes those back). pt.data = Default::default(); @@ -120,13 +120,13 @@ pub(crate) fn cluster_manifolds_for_solver( out.truncate(num_out); - carry_warmstart_data(prev, out, prediction_distance); + transfer_warmstart_data(prev, out, prediction_distance); } /// Copies warm-start data from `prev` manifold points into the best-matching points of /// `targets` (nearest-position in the first shape's local space). Each previous point is /// consumed at most once, so impulses are never duplicated. -pub(crate) fn carry_warmstart_data( +pub(crate) fn transfer_warmstart_data( prev: &[ContactManifold], targets: &mut [ContactManifold], prediction_distance: Real, diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 6879a80b6..3636d10df 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -1,6 +1,7 @@ use super::CollisionEvent; use crate::alloc_prelude::*; use crate::dynamics::{RigidBodyHandle, RigidBodySet}; +use crate::geometry::soft_contacts::{SoftPairContacts, SoftRigidContacts}; use crate::geometry::{ColliderHandle, ColliderSet, Contact, ContactManifold}; use crate::math::{Pose, Real, TangentImpulse, Vector}; use crate::pipeline::EventHandler; @@ -19,14 +20,15 @@ bitflags::bitflags! { /// Flags affecting the behavior of the constraints solver for a given contact manifold. pub struct SolverFlags: u32 { /// The constraint solver will take this contact manifold into - /// account for force computation. - const COMPUTE_IMPULSES = 0b001; + /// account for force computation between two rigid colliders. + const COMPUTE_RIGID_IMPULSES = 0b001; + // TODO: add a COMPUTE_SOFT_IMPULSES flag too? } } impl Default for SolverFlags { fn default() -> Self { - SolverFlags::COMPUTE_IMPULSES + SolverFlags::COMPUTE_RIGID_IMPULSES } } @@ -210,6 +212,39 @@ pub struct ContactPair { pub collider1: ColliderHandle, /// The second collider involved in the contact pair. pub collider2: ColliderHandle, + /// Event bookkeeping: `CollisionEvent::Started` emission and force-event + /// threshold status. + pub(crate) event_status: PairEventStatus, + /// The pair's contacts, in the form the two colliders call for: contact manifolds, or + /// the contact candidates of two soft surfaces. + pub contacts: PairContacts, +} + +/// The contacts of a [`ContactPair`], with a fixed layout: the solver addresses a pair's manifolds +/// through raw pointers (see `ManifoldStore`), so the offset of the rigid contacts must not depend +/// on the value. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +#[repr(C, u8)] +pub enum PairContacts { + /// Contact manifolds: every pair but the pairs of two soft surfaces. + Rigid(RigidPairContacts), + /// The contact candidates of two soft surfaces (see [`SoftPairContacts`]), rebuilt by + /// every update of the pair and turned into constraints by the soft-body solver. + Soft { + /// Whether some feature of one surface is within contact reach of the other, or the + /// two surfaces cross: the pair's touching state. + touching: bool, + /// The candidates (not serialized: the next update rebuilds them). + #[cfg_attr(feature = "serde-serialize", serde(skip))] + candidates: Box, + }, +} + +/// The contact manifolds of a [`ContactPair`], with the solver's bookkeeping about them. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub struct RigidPairContacts { /// The set of contact manifolds between the two colliders. /// /// All contact manifold contain themselves contact points between the colliders. @@ -227,7 +262,7 @@ pub struct ContactPair { /// [`IntegrationParameters::contact_clustering`]: crate::dynamics::IntegrationParameters::contact_clustering pub solver_clusters: Vec, /// The clusters solved at the previous step, kept as the warm-start source (and - /// reused as scratch buffers) when rebuilding `solver_clusters` each frame. + /// reused as workspace buffers) when rebuilding `solver_clusters` each frame. #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) solver_clusters_prev: Vec, /// The persistent solver graph color of this pair: same-color active pairs never share @@ -241,9 +276,6 @@ pub struct ContactPair { serde(default = "default_solver_color_bodies") )] pub(crate) solver_color_bodies: [u32; 2], - /// Event bookkeeping: `CollisionEvent::Started` emission and force-event - /// threshold status. - pub(crate) event_status: PairEventStatus, pub(crate) workspace: Option, /// State cached at the last full narrow-phase update, allowing the update to be /// skipped ("recycled") while the colliders' relative pose stays within @@ -253,6 +285,62 @@ pub struct ContactPair { /// reference pose, or its first update recomputes manifolds (and re-derives the /// world-frozen solver anchors) where the uninterrupted run would have recycled. pub(crate) recycle_state: Option, + /// The soft contacts of a soft-rigid pair beside its manifolds (the predictive vertex + /// contacts and the volume patch, see `SoftRigidContacts`), rebuilt by every update; `None` + /// for every other pair. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) soft: Option>, +} + +impl RigidPairContacts { + fn new() -> Self { + Self { + manifolds: Vec::new(), + solver_clusters: Vec::new(), + solver_clusters_prev: Vec::new(), + solver_color: SOLVER_COLOR_UNCOLORED, + solver_color_bodies: [u32::MAX; 2], + workspace: None, + recycle_state: None, + soft: None, + } + } + + /// The manifolds actually seen by the constraint solver: the contact clusters if + /// clustering applied to this pair, the plain manifolds otherwise. + pub fn solver_manifolds(&self) -> &[ContactManifold] { + if self.solver_clusters.is_empty() { + &self.manifolds + } else { + &self.solver_clusters + } + } + + pub(crate) fn solver_manifolds_mut(&mut self) -> &mut [ContactManifold] { + if self.solver_clusters.is_empty() { + &mut self.manifolds + } else { + &mut self.solver_clusters + } + } + + /// Clears the manifolds and the update's caches; the solver colour is left to the pair + /// transitions. + fn clear(&mut self) { + self.manifolds.clear(); + self.solver_clusters.clear(); + self.solver_clusters_prev.clear(); + self.workspace = None; + self.recycle_state = None; + self.soft = None; + } + + pub fn has_any_active_contact(&self) -> bool { + self + .solver_manifolds() + .iter() + .any(|m| !m.data.solver_contacts.is_empty()) + } } /// The relative configuration of a contact pair at its last full narrow-phase @@ -333,14 +421,8 @@ impl ContactPair { Self { collider1, collider2, - manifolds: Vec::new(), - solver_clusters: Vec::new(), - solver_clusters_prev: Vec::new(), - solver_color: SOLVER_COLOR_UNCOLORED, - solver_color_bodies: [u32::MAX; 2], event_status: PairEventStatus::empty(), - workspace: None, - recycle_state: None, + contacts: PairContacts::Rigid(RigidPairContacts::new()), } } @@ -350,75 +432,143 @@ impl ContactPair { pub(crate) fn reset_for_reuse(&mut self, collider1: ColliderHandle, collider2: ColliderHandle) { self.collider1 = collider1; self.collider2 = collider2; - self.manifolds.clear(); - self.solver_clusters.clear(); - self.solver_clusters_prev.clear(); - self.solver_color = SOLVER_COLOR_UNCOLORED; - self.solver_color_bodies = [u32::MAX; 2]; self.event_status = PairEventStatus::empty(); - self.workspace = None; - self.recycle_state = None; + match &mut self.contacts { + PairContacts::Rigid(rigid) => { + rigid.clear(); + rigid.solver_color = SOLVER_COLOR_UNCOLORED; + rigid.solver_color_bodies = [u32::MAX; 2]; + } + PairContacts::Soft { .. } => { + self.contacts = PairContacts::Rigid(RigidPairContacts::new()); + } + } + } + + /// Turns the pair into a pair of two soft surfaces (see [`PairContacts::Soft`]), keeping + /// it if it already is one. + pub(crate) fn make_soft(&mut self) { + if !matches!(self.contacts, PairContacts::Soft { .. }) { + self.contacts = PairContacts::Soft { + touching: false, + candidates: Box::default(), + }; + } + } + + /// The pair's contact manifolds (`None` for a pair of two soft surfaces). + pub fn rigid(&self) -> Option<&RigidPairContacts> { + match &self.contacts { + PairContacts::Rigid(rigid) => Some(rigid), + PairContacts::Soft { .. } => None, + } + } + + pub(crate) fn rigid_mut(&mut self) -> Option<&mut RigidPairContacts> { + match &mut self.contacts { + PairContacts::Rigid(rigid) => Some(rigid), + PairContacts::Soft { .. } => None, + } + } + + /// The pair's contact candidates (`None` unless the pair is two soft surfaces). + pub fn soft(&self) -> Option<&SoftPairContacts> { + match &self.contacts { + PairContacts::Soft { candidates, .. } => Some(candidates), + PairContacts::Rigid(_) => None, + } + } + + pub(crate) fn soft_mut(&mut self) -> Option<&mut SoftPairContacts> { + match &mut self.contacts { + PairContacts::Soft { candidates, .. } => Some(candidates), + PairContacts::Rigid(_) => None, + } + } + + /// The pair's contact manifolds (empty for a pair of two soft surfaces, whose contacts + /// are its candidates: see [`Self::soft`]). + pub fn manifolds(&self) -> &[ContactManifold] { + self.rigid().map_or(&[], |r| &r.manifolds) } /// The manifolds actually seen by the constraint solver: the contact clusters if - /// clustering applied to this pair, the plain manifolds otherwise. + /// clustering applied to this pair, the plain manifolds otherwise (empty for a pair of + /// two soft surfaces). pub fn solver_manifolds(&self) -> &[ContactManifold] { - if self.solver_clusters.is_empty() { - &self.manifolds - } else { - &self.solver_clusters - } + self.rigid().map_or(&[], |r| r.solver_manifolds()) } /// Mutable twin of [`Self::solver_manifolds`]: the manifolds the constraint /// solver actually sees (the solver clusters if any, else the plain manifolds). #[cfg_attr(feature = "parallel", allow(dead_code))] // Single-threaded solver path. pub(crate) fn solver_manifolds_mut(&mut self) -> &mut [ContactManifold] { - if self.solver_clusters.is_empty() { - &mut self.manifolds - } else { - &mut self.solver_clusters - } + self.rigid_mut().map_or(&mut [], |r| r.solver_manifolds_mut()) + } + + /// The pair's solver graph colour (`SOLVER_COLOR_UNCOLORED` for a pair of two soft + /// surfaces, which the rigid solver never sees). + pub(crate) fn solver_color(&self) -> u8 { + self.rigid().map_or(SOLVER_COLOR_UNCOLORED, |r| r.solver_color) } /// Is there any active contact in this contact pair? pub fn has_any_active_contact(&self) -> bool { - self.solver_manifolds() - .iter() - .any(|m| !m.data.solver_contacts.is_empty()) + match &self.contacts { + PairContacts::Rigid(rigid) => rigid.has_any_active_contact(), + PairContacts::Soft { touching, .. } => *touching, + } } /// Clears all the contacts of this contact pair. pub fn clear(&mut self) { - self.manifolds.clear(); - self.solver_clusters.clear(); - self.solver_clusters_prev.clear(); - self.workspace = None; - self.recycle_state = None; + match &mut self.contacts { + PairContacts::Rigid(rigid) => rigid.clear(), + PairContacts::Soft { + touching, + candidates, + } => { + *touching = false; + candidates.clear(); + } + } } // NOTE: while recycled, a pair's world-space solver data (normal, frozen lever arms — see // `ContactData::solver_dp1`) keeps its last-full-update values (anchor freezing): the solver - // rebuilds world points/separations from body-local anchors + current poses, so no per-step refresh; user data stays stale within the recycle drift bound. + // rebuilds world points/separations from body-local anchors + current poses, so no per-step update; user data stays stale within the recycle drift bound. /// The total impulse (force × time) applied by all contacts. /// /// This is the accumulated force that pushed the colliders apart. /// Useful for determining impact strength. pub fn total_impulse(&self) -> Vector { - self.solver_manifolds() - .iter() - .map(|m| m.total_impulse() * m.data.normal) - .sum() + match &self.contacts { + PairContacts::Rigid(rigid) => rigid + .solver_manifolds() + .iter() + .map(|m| m.total_impulse() * m.data.normal) + .sum(), + PairContacts::Soft { candidates, .. } => candidates + .impulses(self.collider1) + .map(|i| i.normal * i.impulse) + .sum(), + } } /// The total magnitude of all contact impulses (sum of lengths, not length of sum). /// /// This is what's compared against `contact_force_event_threshold`. pub fn total_impulse_magnitude(&self) -> Real { - self.solver_manifolds() - .iter() - .fold(0.0, |a, m| a + m.total_impulse()) + match &self.contacts { + PairContacts::Rigid(rigid) => rigid + .solver_manifolds() + .iter() + .fold(0.0, |a, m| a + m.total_impulse()), + PairContacts::Soft { candidates, .. } => candidates + .impulses(self.collider1) + .fold(0.0, |a, i| a + i.impulse), + } } /// Finds the strongest contact impulse and its direction. @@ -427,11 +577,22 @@ impl ContactPair { pub fn max_impulse(&self) -> (Real, Vector) { let mut result = (0.0, Vector::ZERO); - for m in self.solver_manifolds() { - let impulse = m.total_impulse(); + match &self.contacts { + PairContacts::Rigid(rigid) => { + for m in rigid.solver_manifolds() { + let impulse = m.total_impulse(); - if impulse > result.0 { - result = (impulse, m.data.normal); + if impulse > result.0 { + result = (impulse, m.data.normal); + } + } + } + PairContacts::Soft { candidates, .. } => { + for i in candidates.impulses(self.collider1) { + if i.impulse > result.0 { + result = (i.impulse, i.normal); + } + } } } @@ -446,7 +607,8 @@ impl ContactPair { /// - Determining primary contact direction /// - Custom penetration resolution /// - /// Returns both the contact point and its parent manifold. + /// Returns both the contact point and its parent manifold (`None` for a pair of two soft + /// surfaces, whose contacts are candidates: see [`Self::soft`]). /// /// # Example /// ``` @@ -462,7 +624,7 @@ impl ContactPair { pub fn find_deepest_contact(&self) -> Option<(&ContactManifold, &Contact)> { let mut deepest = None; - for m2 in &self.manifolds { + for m2 in self.manifolds() { let deepest_candidate = m2.find_deepest_contact(); deepest = match (deepest, deepest_candidate) { @@ -620,7 +782,7 @@ pub struct SolverContactGeneric { // on `ContactManifoldData`, is-new in bit 31 of `contact_id`, warm-starts on the manifold points. /// The contact point on the first body's surface (contact skin baked in), in that /// body's CoM-centered local frame so it rides rigidly with the body — what lets - /// contact recycling skip the per-frame world refresh. World-space instead for a side + /// contact recycling skip the per-frame world update. World-space instead for a side /// without a solver body (none, or world-attached by dominance — fixed bodies included). /// Inside [`PhysicsHooks::modify_solver_contacts`] this always holds the fresh /// **world-space** point (hooks run before localization). diff --git a/src/geometry/mesh_converter.rs b/src/geometry/mesh_converter.rs index d6379e201..cf3f16703 100644 --- a/src/geometry/mesh_converter.rs +++ b/src/geometry/mesh_converter.rs @@ -15,7 +15,7 @@ use parry::transformation::vhacd::VHACDParameters; /// Error that can be generated by the [`MeshConverter`]. #[derive(thiserror::Error, Debug)] pub enum MeshConverterError { - /// The convex hull calculation carried out by the [`MeshConverter::ConvexHull`] failed. + /// The convex hull calculation performed by the [`MeshConverter::ConvexHull`] failed. #[error("convex-hull computation failed")] ConvexHullFailed, /// The TriMesh building failed. diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index b11ec69fe..deb31f299 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -23,6 +23,7 @@ pub(crate) use self::contact_pair::relative_pose_drift; #[cfg(feature = "alloc")] pub use self::contact_pair::{ ContactData, ContactId, ContactManifoldData, ContactPair, IntersectionPair, NEW_CONTACT_BIT, + PairContacts, RigidPairContacts, SimdSolverContact, SolverContact, SolverContactGeneric, SolverContacts, SolverFlags, is_bouncy, is_bouncy_simd, }; @@ -35,6 +36,11 @@ pub use self::interaction_groups::{Group, InteractionGroups, InteractionTestMode pub use self::mesh_converter::{MeshConverter, MeshConverterError}; #[cfg(feature = "alloc")] pub use self::narrow_phase::NarrowPhase; +pub use self::narrow_phase::soft_contacts::{ + SoftContactImpulse, SoftEdgeCandidate, SoftEdgePass, SoftPairContacts, SoftVertexCandidate, + SoftVertexHits, SoftVertexPass, SoftVolumePatch, VolumeBin, +}; +pub(crate) use self::narrow_phase::soft_contacts; #[cfg(feature = "alloc")] pub use parry::utils::Array2; @@ -246,6 +252,16 @@ impl ContactForceEvent { result.total_force += m.data.normal * total_manifold_impulse; } + // Two soft surfaces: their contacts are candidates, each reporting its impulse. + if let Some(soft) = pair.soft() { + for i in soft.impulses(pair.collider1) { + if i.impulse > result.max_force_magnitude { + result.max_force_magnitude = i.impulse; + result.max_force_direction = i.normal; + } + result.total_force += i.normal * i.impulse; + } + } let inv_dt = crate::utils::inv(dt); // NOTE: convert impulses to forces. Note that we diff --git a/src/geometry/narrow_phase/contacts.rs b/src/geometry/narrow_phase/contacts.rs index bb870c612..ea4400673 100644 --- a/src/geometry/narrow_phase/contacts.rs +++ b/src/geometry/narrow_phase/contacts.rs @@ -11,7 +11,11 @@ use super::{ pack_color_body_info, strong_wake_sleeping_side, unpack_color_body_info, }; use crate::alloc_prelude::*; -use crate::dynamics::{ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodySet}; +use super::soft_contacts::SoftDetectionCtx; +use crate::dynamics::{ + ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, RigidBodySet, + SoftBodySet, +}; use crate::geometry::{ColliderHandle, ColliderSet, ContactPair}; use crate::math::Real; #[cfg(all(feature = "parallel", feature = "unsync-callbacks"))] @@ -34,9 +38,21 @@ impl NarrowPhase { modified_colliders: &[ColliderHandle], hooks: &dyn PhysicsHooks, events: &dyn EventHandler, + // The soft bodies and the step's parameters: the soft contact detection runs on + // the soft-surface pairs and, after the transitions, on the awake self-colliding + // meshes (`None`: no soft contact detection, as in the collision pipeline). + soft_bodies: Option<(&SoftBodySet, &IntegrationParameters)>, ) { - self.refresh_awake_body_mask(islands); + self.update_awake_body_mask(islands); let awake_body_mask = core::mem::take(&mut self.awake_body_mask); + let soft_ctx = soft_bodies.map(|(soft_bodies, params)| SoftDetectionCtx { + soft_bodies, + bodies: &*bodies, + colliders, + params, + dt, + }); + let soft_ctx = soft_ctx.as_ref(); // The solver hints follow the contact-graph edge indexing; removals are // mirrored eagerly, so this resize only matters for appended pairs (their @@ -99,6 +115,7 @@ impl NarrowPhase { query_dispatcher, &awake_body_mask, hints_ptr, + soft_ctx, &mut transitions, ) }; @@ -125,6 +142,7 @@ impl NarrowPhase { query_dispatcher, &awake_body_mask, hints_ptr, + soft_ctx, &snd, ) }; @@ -278,6 +296,19 @@ impl NarrowPhase { self.apply_pair_transitions(&mut transitions, islands, bodies, colliders, events); } + // The self contacts, once the transitions woke the bodies a new pair touches (the context + // is rebuilt: the transitions borrowed the bodies mutably). + if let Some((soft_bodies, params)) = soft_bodies { + let ctx = SoftDetectionCtx { + soft_bodies, + bodies: &*bodies, + colliders, + params, + dt, + }; + self.update_soft_self_contacts(&ctx, bodies); + } + self.update_candidates = update_candidates; self.awake_body_mask = awake_body_mask; #[cfg(not(feature = "parallel"))] diff --git a/src/geometry/narrow_phase/intersections.rs b/src/geometry/narrow_phase/intersections.rs index 5a8df1144..94c7d04f5 100644 --- a/src/geometry/narrow_phase/intersections.rs +++ b/src/geometry/narrow_phase/intersections.rs @@ -1,4 +1,4 @@ -//! The intersection (sensor) pair update: filters and refreshes every +//! The intersection (sensor) pair update: filters and updates every //! intersection pair involving a moved or user-modified collider. #[cfg(feature = "parallel")] @@ -23,7 +23,7 @@ impl NarrowPhase { hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { - self.refresh_awake_body_mask(islands); + self.update_awake_body_mask(islands); let awake_body_mask = core::mem::take(&mut self.awake_body_mask); // Only iterate on pairs involving at least one changed collider instead of diff --git a/src/geometry/narrow_phase/mod.rs b/src/geometry/narrow_phase/mod.rs index 65ae91da2..f2fcc5654 100644 --- a/src/geometry/narrow_phase/mod.rs +++ b/src/geometry/narrow_phase/mod.rs @@ -7,6 +7,7 @@ mod intersections; mod pair_management; mod pair_update; mod queries; +pub(crate) mod soft_contacts; mod solver_graph; #[cfg(test)] #[cfg(feature = "f32")] @@ -67,7 +68,7 @@ fn strong_wake_sleeping_side( } /// Packs a coloring body descriptor `(arena index, is_fixed)` into a `u32` for the -/// deferred-coloring scratch list: `u32::MAX` = no body, else `(id << 1) | is_fixed`. +/// deferred-coloring workspace list: `u32::MAX` = no body, else `(id << 1) | is_fixed`. fn pack_color_body_info(info: Option<(u32, bool)>) -> u32 { match info { None => u32::MAX, @@ -97,6 +98,10 @@ fn assign_pair_solver_color( SOLVER_COLOR_OVERFLOW, SOLVER_COLOR_UNCOLORED, SOLVER_DYNAMIC_COLOR_COUNT, }; + // A pair of two soft surfaces never reaches the rigid solver: no color. + let Some(pair) = pair.rigid_mut() else { + return; + }; if pair.solver_color != SOLVER_COLOR_UNCOLORED { return; } @@ -157,6 +162,9 @@ fn assign_pair_solver_color( fn clear_pair_solver_color(masks: &mut [u128], pair: &mut ContactPair) { use crate::geometry::contact_pair::{SOLVER_COLOR_OVERFLOW, SOLVER_COLOR_UNCOLORED}; + let Some(pair) = pair.rigid_mut() else { + return; + }; if pair.solver_color < SOLVER_COLOR_OVERFLOW { for id in pair.solver_color_bodies { if id != u32::MAX { @@ -201,7 +209,7 @@ fn single_manifold_bucket_drift(pair: &ContactPair, selectable: bool) -> bool { && manifold .data .solver_flags - .contains(SolverFlags::COMPUTE_IMPULSES) + .contains(SolverFlags::COMPUTE_RIGID_IMPULSES) && manifold.data.num_active_contacts() != 0; let pos = manifold.data.graph_pos; if !qualifies { @@ -215,7 +223,7 @@ fn single_manifold_bucket_drift(pair: &ContactPair, selectable: bool) -> bool { if pos.bucket() == GENERIC_BUCKET { return false; } - let mut color = pair.solver_color; + let mut color = pair.solver_color(); if color == SOLVER_COLOR_UNCOLORED { color = SOLVER_COLOR_OVERFLOW; } @@ -225,18 +233,12 @@ fn single_manifold_bucket_drift(pair: &ContactPair, selectable: bool) -> bool { /// The number of this pair's solver manifolds that the constraint solver must see /// (impulses to compute and at least one active contact). fn pair_qualified_manifold_count(pair: &ContactPair) -> u16 { - let solver_manifolds = if pair.solver_clusters.is_empty() { - &pair.manifolds - } else { - &pair.solver_clusters - }; - let mut count: u16 = 0; - for manifold in solver_manifolds { + for manifold in pair.solver_manifolds() { if manifold .data .solver_flags - .contains(SolverFlags::COMPUTE_IMPULSES) + .contains(SolverFlags::COMPUTE_RIGID_IMPULSES) && manifold.data.num_active_contacts() != 0 { count = count.saturating_add(1); @@ -292,7 +294,7 @@ fn collect_pairs_to_update( push_edges_of(*handle, true); } - // Active bodies' colliders may have moved this step without carrying any + // Active bodies' colliders may have moved this step without having any // change flag (internal motion doesn't go through the user-modification // tracking), so their pairs are always candidates. for body_handle in islands.active_bodies() { @@ -332,7 +334,7 @@ pub struct NarrowPhase { contact_graph: InteractionGraph, intersection_graph: InteractionGraph, graph_indices: Coarena, - /// Scratch buffer holding the edge indices of pairs to process during a step, so + /// Workspace buffer holding the edge indices of pairs to process during a step, so /// the per-step loops don’t have to iterate on the whole interaction graphs. #[cfg_attr(feature = "serde-serialize", serde(skip))] update_candidates: Vec, @@ -341,13 +343,13 @@ pub struct NarrowPhase { /// so the solver never recolors its constraint graph from scratch. #[cfg_attr(feature = "serde-serialize", serde(default))] body_solver_color_masks: Vec, - /// Scratch: per-body packed qualification info (rigid-body arena index), rebuilt during + /// Workspace: per-body packed qualification info (rigid-body arena index), rebuilt during /// solver-graph maintenance. `u64::MAX` = missing/fixed/kinematic-or-sleeping; /// else `(active_set_id << 32) | is_dynamic`. #[cfg_attr(feature = "serde-serialize", serde(skip))] body_qualify_info: Vec, - /// Scratch: per-body awake bit (arena index), rebuilt each narrow-phase update. - /// Internal motion carries no change flags, so "the parent body is awake" is the + /// Workspace: per-body awake bit (arena index), rebuilt each narrow-phase update. + /// Internal motion sets no change flags, so "the parent body is awake" is the /// narrow-phase's it-may-have-moved signal for pair updates. #[cfg_attr(feature = "serde-serialize", serde(skip))] awake_body_mask: Vec, @@ -371,7 +373,7 @@ pub struct NarrowPhase { /// A mismatch means bodies may have joined/left a multibody (manifolds can switch /// between color buckets and the generic list), forcing a full rebuild. solver_graph_mb_epoch: u32, - /// Scratch: edge indices fully updated this step (`OUTCOME_FULL`) — possible bucket + /// Workspace: edge indices fully updated this step (`OUTCOME_FULL`): possible bucket /// membership change. Consumed by the incremental maintenance in /// [`Self::maintain_solver_contact_graph`] and the force-event list reconciliation. #[cfg_attr(feature = "serde-serialize", serde(skip))] @@ -384,7 +386,7 @@ pub struct NarrowPhase { /// O(1) membership reconciliation. Edge-index shifts (pair/collider removal) are covered /// by the full rebuild those removals already force via `solver_graph_valid`. force_event_pos: Vec, - /// Scratch: edges flagged for force-event membership reconciliation because a collider + /// Workspace: edges flagged for force-event membership reconciliation because a collider /// was user-modified this step (an `ActiveEvents`/threshold flip has no change flag /// and need not trigger a contact update, so it would otherwise go unnoticed mid-epoch). force_event_flagged: Vec, @@ -392,11 +394,15 @@ pub struct NarrowPhase { /// through every transition, so it does NOT need epoch full rebuilds; `false` /// (a degenerate state) triggers the from-scratch scan. force_list_valid: bool, - /// Scratch: begin-touch pairs deferred for greedy coloring in canonical + /// Workspace: begin-touch pairs deferred for greedy coloring in canonical /// `(min, max body id)` order (discovery-order independent: ≈ Δ colors instead of ≈ 2Δ). /// Entries are `(edge id, packed body infos)`, see [`pack_color_body_info`]. #[cfg_attr(feature = "serde-serialize", serde(skip))] solver_color_todo: Vec<(u32, u32, u32)>, + /// The self contact detection of the soft collision meshes (by collider), rebuilt by + /// every update for the awake bodies (see `soft_contacts`). + #[cfg_attr(feature = "serde-serialize", serde(skip))] + soft_self: parry::utils::hashmap::HashMap, /// Pool of retired [`ContactPair`]s, reused by [`Self::add_pair`] so /// pair-churn-heavy scenes (hundreds of broad-phase add/delete events per /// step) skip the buffer reallocation of freshly constructed pairs. @@ -444,10 +450,11 @@ impl NarrowPhase { force_event_flagged: Vec::new(), force_list_valid: false, solver_color_todo: Vec::new(), + soft_self: Default::default(), } } - fn refresh_awake_body_mask(&mut self, islands: &IslandManager) { + fn update_awake_body_mask(&mut self, islands: &IslandManager) { self.awake_body_mask.clear(); let len = islands .active_bodies() diff --git a/src/geometry/narrow_phase/pair_management.rs b/src/geometry/narrow_phase/pair_management.rs index c8545b776..2eb99b280 100644 --- a/src/geometry/narrow_phase/pair_management.rs +++ b/src/geometry/narrow_phase/pair_management.rs @@ -237,7 +237,12 @@ impl NarrowPhase { // so that the narrow-phase properly takes into account the change in, e.g., // collision groups. Waking up the modified collider's parent isn't enough because // it could be a fixed or kinematic body which don't propagate the wake-up state. - if let Some(islands) = islands.as_deref_mut() { + // A pure in-place deformation (soft-body surface) is simulation-driven, not a + // user change: it must not keep its island awake. + let deformed_only = (co.changes + & !(ColliderChanges::IN_MODIFIED_SET | ColliderChanges::DEFORMED)) + .is_empty(); + if let Some(islands) = islands.as_deref_mut().filter(|_| !deformed_only) { if let Some(co_parent) = &co.parent { islands.wake_up(bodies, co_parent.handle, true); } @@ -295,7 +300,7 @@ impl NarrowPhase { // Persistent islands: after re-parenting or a body type change, // a link recorded with the old endpoints may no longer describe - // connectivity (a link to a fixed body doesn't connect). Refresh it. + // connectivity (a link to a fixed body doesn't connect). Update it. if let Some(islands) = islands.as_deref_mut() { let parent = |co: ColliderHandle| { colliders.get(co).and_then(|c| c.parent.map(|p| p.handle)) diff --git a/src/geometry/narrow_phase/pair_update.rs b/src/geometry/narrow_phase/pair_update.rs index b1f626c32..d8fe3b996 100644 --- a/src/geometry/narrow_phase/pair_update.rs +++ b/src/geometry/narrow_phase/pair_update.rs @@ -13,11 +13,14 @@ use crate::dynamics::{ RigidBodyType, }; use crate::geometry::{ - BoundingVolume, ColliderChanges, ColliderSet, ContactData, ContactManifoldData, ContactPair, - SolverContact, SolverFlags, + BoundingVolume, ColliderChanges, ColliderSet, ContactData, + ContactManifoldData, ContactPair, SolverContact, SolverFlags, }; use crate::math::{MAX_MANIFOLD_POINTS, Real}; -use crate::pipeline::{ActiveHooks, ContactModificationContext, PairFilterContext, PhysicsHooks}; +use crate::pipeline::{ + ActiveHooks, ContactModificationContext, ModifiableContacts, ModifiableManifold, + PairFilterContext, PhysicsHooks, +}; use parry::query::PersistentQueryDispatcher; use parry::utils::PoseOpt; @@ -80,6 +83,7 @@ pub(super) fn process_pair( query_dispatcher: &dyn PersistentQueryDispatcher, awake_body_mask: &[bool], hints_ptr: &HintsPtr, + soft: Option<&super::soft_contacts::SoftDetectionCtx>, #[cfg(not(feature = "parallel"))] transitions: &mut Vec, #[cfg(feature = "parallel")] snd: &std::sync::mpsc::Sender, ) -> u8 { @@ -109,7 +113,7 @@ pub(super) fn process_pair( // last full update, skip contact determination entirely. Must run before the // `has_any_active_contact` walk below (whose result recycling cannot change). if contact_recycle_distance > 0.0 { - if let Some(state) = &pair.recycle_state { + if let Some(state) = pair.rigid().and_then(|r| r.recycle_state.as_ref()) { // Anything beyond a position change (shape, groups, type, // enabled flag, ...) requires a full update, as do pairs // relying on per-step user hooks. @@ -190,6 +194,10 @@ pub(super) fn process_pair( let rb_type1 = rb1.map(|rb| rb.body_type).unwrap_or(RigidBodyType::Fixed); let rb_type2 = rb2.map(|rb| rb.body_type).unwrap_or(RigidBodyType::Fixed); + #[cfg(feature = "dim3")] + let soft_surface_pair = co1.deformable_mesh_ref.is_some() || co2.deformable_mesh_ref.is_some(); + let two_soft_surfaces = co1.deformable_mesh_ref.is_some() && co2.deformable_mesh_ref.is_some(); + // Deal with contacts disabled between bodies attached by joints. if let (Some(co_parent1), Some(co_parent2)) = (&co1.parent, &co2.parent) { for (_, joint) in impulse_joints.joints_between(co_parent1.handle, co_parent2.handle) { @@ -277,19 +285,25 @@ pub(super) fn process_pair( }; if !co1.flags.solver_groups.test(co2.flags.solver_groups) { - solver_flags.remove(SolverFlags::COMPUTE_IMPULSES); + solver_flags.remove(SolverFlags::COMPUTE_RIGID_IMPULSES); } - if co1.changes.contains(ColliderChanges::SHAPE) - || co2.changes.contains(ColliderChanges::SHAPE) - { - // The shape changed so the workspace is no longer valid. - pair.workspace = None; + // Contacts with a soft body (its surface) are solved by the + // soft-body solver (as constraints over the touched particles), never by the rigid contact + // solver. Events still fire. + if co1.is_deformable_collider() || co2.is_deformable_collider() { + solver_flags.remove(SolverFlags::COMPUTE_RIGID_IMPULSES); } let pos12 = co1.pos.inv_mul(&co2.pos); - let contact_skin_sum = co1.contact_skin() + co2.contact_skin(); + // 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); + 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); let soft_ccd_prediction2 = rb2.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0); let effective_prediction_distance = if soft_ccd_prediction1 > 0.0 @@ -314,21 +328,12 @@ pub(super) fn process_pair( break 'emit_events; } - prediction_distance.max(dt * (linvel1 - linvel2).length()) + contact_skin_sum + prediction_distance.max(dt * (linvel1 - linvel2).length()) + contact_skin_sum + soft_body_prediction } else { - prediction_distance + contact_skin_sum + prediction_distance + contact_skin_sum + soft_body_prediction }; outcome = OUTCOME_FULL; - let _ = query_dispatcher.contact_manifolds( - &pos12, - &*co1.shape, - &*co2.shape, - effective_prediction_distance, - &mut pair.manifolds, - &mut pair.workspace, - ); - let friction = CoefficientCombineRule::combine( co1.material.friction, co2.material.friction, @@ -342,12 +347,103 @@ pub(super) fn process_pair( co2.material.restitution_combine_rule, ); + /* + * + * Handle two soft colliders. + * + */ + if two_soft_surfaces { + pair.make_soft(); + let aabb1 = co1.compute_collision_aabb(prediction_distance / 2.0 + soft_margin1); + let aabb2 = co2.compute_collision_aabb(prediction_distance / 2.0 + soft_margin2); + // TODO(perf): would it be worth it to check if the aabb overlaps the leaves of composite shapes? + let touching = aabb1.intersects(&aabb2); + + if !touching { + if clear_filtered_pair(pair) { + outcome = OUTCOME_CLEARED_IN_GRAPH; + } + break 'emit_events; + } + + match soft { + Some(soft) => { + super::soft_contacts::update_pair_soft_soft(pair, co1, co2, soft); + // The contact-modification hook, on the candidates. + if active_hooks.contains(ActiveHooks::MODIFY_SOLVER_CONTACTS) { + let (collider1, collider2) = (pair.collider1, pair.collider2); + + if let Some(candidates) = pair.soft_mut() { + let mut context = ContactModificationContext { + bodies, + colliders, + rigid_body1: rb_handle1, + rigid_body2: rb_handle2, + collider1, + collider2, + contacts: ModifiableContacts::Soft(candidates), + }; + hooks.modify_solver_contacts(&mut context); + } + } + } + // No soft bodies at hand (the collision pipeline): no soft contacts either. + None => pair.clear(), + } + + // Exit. Everything below is about contact pairs involving at least one rigid-body. + break 'emit_events; + } + + /* + * + * Handle soft-vs-rigid and rigid-vs-rigid. + * + */ + let (collider1, collider2) = (pair.collider1, pair.collider2); + + let Some(rigid) = pair.rigid_mut() else { + break 'emit_events; + }; + + if co1.changes.contains(ColliderChanges::SHAPE) + || co2.changes.contains(ColliderChanges::SHAPE) + { + // The shape changed so the workspace is no longer valid. + rigid.workspace = None; + } + + // Rigid-vs-rigid and soft-vs-rigid collisions. + let _ = query_dispatcher.contact_manifolds( + &pos12, + &*co1.shape, + &*co2.shape, + effective_prediction_distance, + &mut rigid.manifolds, + &mut rigid.workspace, + ); + + if let Some(soft) = soft.filter(|_| co1.deformable_mesh_ref.is_some() || co2.deformable_mesh_ref.is_some()) { + // Soft-vs-rigid: the predictive vertex contacts of the elements the manifolds reached + // (within the solver contacts' separation bound), and the intersection volume. + super::soft_contacts::update_pair_soft_rigid( + rigid, + (collider1, co1), + (collider2, co2), + bodies, + prediction_distance + soft_body_prediction, + soft, + ); + } + let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups. let dominance1 = rb1.map(|rb| rb.dominance).unwrap_or(zero); let dominance2 = rb2.map(|rb| rb.dominance).unwrap_or(zero); + // Soft-surface pairs keep one manifold per triangle: the soft-body solver needs each + // point's triangle (the manifold's subshape id), which a cluster loses. #[cfg(feature = "dim3")] - let use_clusters = contact_clustering && pair.manifolds.len() > 1; + let use_clusters = contact_clustering && rigid.manifolds.len() > 1 && !soft_surface_pair; #[cfg(not(feature = "dim3"))] let use_clusters = { // Contact clustering isn’t implemented in 2D (manifolds hold at @@ -360,17 +456,17 @@ pub(super) fn process_pair( if use_clusters { // Rebuild the solver clusters, using the clusters solved at the // previous step as the warm-start source. - core::mem::swap(&mut pair.solver_clusters, &mut pair.solver_clusters_prev); + core::mem::swap(&mut rigid.solver_clusters, &mut rigid.solver_clusters_prev); crate::geometry::contact_clustering::cluster_manifolds_for_solver( - &pair.manifolds, - &pair.solver_clusters_prev, - &mut pair.solver_clusters, + &rigid.manifolds, + &rigid.solver_clusters_prev, + &mut rigid.solver_clusters, prediction_distance, ); // The plain manifolds won't be seen by the solver, but keep their // user-facing data coherent. - for manifold in &mut pair.manifolds { + for manifold in &mut rigid.manifolds { let world_pos1 = manifold.subshape_pos1().prepend_to(&co1.pos); manifold.data.solver_contacts.clear(); manifold.data.rigid_body1 = rb_handle1; @@ -382,22 +478,22 @@ pub(super) fn process_pair( dominance1.effective_group(&rb_type1) - dominance2.effective_group(&rb_type2); manifold.data.normal = world_pos1.rotation * manifold.local_n1; } - } else if !pair.solver_clusters.is_empty() { - // Clustering stopped applying to this pair: carry the warm-start + } else if !rigid.solver_clusters.is_empty() { + // Clustering stopped applying to this pair: transfer the warm-start // data back into the plain manifolds once, then drop the clusters. - crate::geometry::contact_clustering::carry_warmstart_data( - &pair.solver_clusters, - &mut pair.manifolds, + crate::geometry::contact_clustering::transfer_warmstart_data( + &rigid.solver_clusters, + &mut rigid.manifolds, prediction_distance, ); - pair.solver_clusters.clear(); - pair.solver_clusters_prev.clear(); + rigid.solver_clusters.clear(); + rigid.solver_clusters_prev.clear(); } let solver_manifolds = if use_clusters { - &mut pair.solver_clusters + &mut rigid.solver_clusters } else { - &mut pair.manifolds + &mut rigid.manifolds }; for manifold in solver_manifolds { @@ -461,18 +557,19 @@ pub(super) fn process_pair( let contact = &manifold.points[*contact_id]; let effective_contact_dist = contact.dist - co1.contact_skin() - co2.contact_skin(); - let keep_solver_contact = effective_contact_dist < prediction_distance || { - let world_pt1 = world_pos1 * contact.local_p1; - let world_pt2 = world_pos2 * contact.local_p2; - let vel1 = rb1 - .map(|rb| rb.velocity_at_point(world_pt1)) - .unwrap_or_default(); - let vel2 = rb2 - .map(|rb| rb.velocity_at_point(world_pt2)) - .unwrap_or_default(); - effective_contact_dist + (vel2 - vel1).dot(manifold.data.normal) * dt - < prediction_distance - }; + let keep_solver_contact = + effective_contact_dist < prediction_distance + soft_body_prediction || { + let world_pt1 = world_pos1 * contact.local_p1; + let world_pt2 = world_pos2 * contact.local_p2; + let vel1 = rb1 + .map(|rb| rb.velocity_at_point(world_pt1)) + .unwrap_or_default(); + let vel2 = rb2 + .map(|rb| rb.velocity_at_point(world_pt2)) + .unwrap_or_default(); + effective_contact_dist + (vel2 - vel1).dot(manifold.data.normal) * dt + < prediction_distance + }; if keep_solver_contact { // The anchors hold world-space points until the localization pass @@ -511,14 +608,16 @@ pub(super) fn process_pair( colliders, rigid_body1: rb_handle1, rigid_body2: rb_handle2, - collider1: pair.collider1, - collider2: pair.collider2, - manifold, - solver_contacts: &mut modifiable_solver_contacts, - normal: &mut modifiable_normal, - friction: &mut modifiable_friction, - restitution: &mut modifiable_restitution, - user_data: &mut modifiable_user_data, + collider1, + collider2, + contacts: ModifiableContacts::Rigid(ModifiableManifold { + manifold, + solver_contacts: &mut modifiable_solver_contacts, + normal: &mut modifiable_normal, + friction: &mut modifiable_friction, + restitution: &mut modifiable_restitution, + user_data: &mut modifiable_user_data, + }), }; hooks.modify_solver_contacts(&mut context); @@ -532,7 +631,7 @@ pub(super) fn process_pair( // Localize solver contacts: bake skins (and hook-written `dist`) into the anchors, then // express each in its body's CoM frame (world-attached/dominance-superior sides keep world - // anchors, matching the solver's identity pose). Riding rigidly lets recycled steps skip refresh. + // anchors, matching the solver's identity pose). Riding rigidly lets recycled steps skip updates. { let normal = manifold.data.normal; let rel_dom = manifold.data.relative_dominance; @@ -570,6 +669,7 @@ pub(super) fn process_pair( Some(pose) => pose.inverse_transform_point(p1), None => p1, }; + if let Some(pose) = &com_pose2 { sc.anchor2 = pose.inverse_transform_point(sc.anchor2); } @@ -585,7 +685,7 @@ pub(super) fn process_pair( // shape does, so reuse the previous full update's value. let shapes_changed = (co1.changes | co2.changes).contains(crate::geometry::ColliderChanges::SHAPE); - let max_extent = match &pair.recycle_state { + let max_extent = match &rigid.recycle_state { Some(state) if !shapes_changed => state.max_extent, _ => { let origin_radius = |co: &crate::geometry::Collider| { @@ -598,12 +698,13 @@ pub(super) fn process_pair( // A pair without contacts has an (unknown) separation larger than // the prediction distance. Cap its recycle window by the // prediction distance so an incoming contact can't be missed. - let max_drift = if pair.has_any_active_contact() { + let max_drift = if rigid.has_any_active_contact() + { contact_recycle_distance } else { contact_recycle_distance.min(prediction_distance) }; - pair.recycle_state = Some(crate::geometry::ContactRecycleState { + rigid.recycle_state = Some(crate::geometry::ContactRecycleState { pos12, rot1: co1.pos.rotation, rot2: co2.pos.rotation, @@ -620,6 +721,7 @@ pub(super) fn process_pair( * the result independent of the update schedule). */ let has_any_active_contact = pair.has_any_active_contact(); + if has_any_active_contact != had_any_active_contact { let transition = (edge_id, rb_handle1, rb_handle2, has_any_active_contact); #[cfg(not(feature = "parallel"))] @@ -628,7 +730,7 @@ pub(super) fn process_pair( let _ = snd.send(transition); } - // Refresh the pair's solver-qualification hint from its final state + // Update the pair's solver-qualification hint from its final state // (this point is reached by every path that may have changed the // manifolds: full updates and the various pair-clearing branches). let mut membership_changed = true; @@ -670,7 +772,7 @@ pub(super) fn process_pair( // Composite pairs have unstable manifold ordinals (see `OUTCOME_FULL_COMPOSITE`), // so signal a full rebuild. Must be checked before the `FULL_CLEAN` shortcut: a // surviving manifold can look "clean" while a dropped sibling leaked its graph slot. - if outcome == OUTCOME_FULL && pair.workspace.is_some() { + if outcome == OUTCOME_FULL && pair.rigid().is_some_and(|r| r.workspace.is_some()) { return OUTCOME_FULL_COMPOSITE; } if outcome == OUTCOME_FULL && !membership_changed { diff --git a/src/geometry/narrow_phase/queries.rs b/src/geometry/narrow_phase/queries.rs index db6de26ae..0b6682f0b 100644 --- a/src/geometry/narrow_phase/queries.rs +++ b/src/geometry/narrow_phase/queries.rs @@ -145,6 +145,20 @@ impl NarrowPhase { .map(|c| c.2) } + /// Mutable access to the contact pair involving two specific colliders (the soft-body + /// solver writes its contact impulses back through it). + pub(crate) fn contact_pair_mut( + &mut self, + collider1: ColliderHandle, + collider2: ColliderHandle, + ) -> Option<&mut ContactPair> { + let id1 = self.graph_indices.get(collider1.0)?; + let id2 = self.graph_indices.get(collider2.0)?; + self.contact_graph + .interaction_pair_mut(id1.contact_graph_index, id2.contact_graph_index) + .map(|c| c.2) + } + /// The intersection pair involving two specific colliders. /// /// It is strongly recommended to use the [`NarrowPhase::intersection_pair`] method instead. This diff --git a/src/geometry/narrow_phase/soft_contacts/mod.rs b/src/geometry/narrow_phase/soft_contacts/mod.rs new file mode 100644 index 000000000..f14b0d4e7 --- /dev/null +++ b/src/geometry/narrow_phase/soft_contacts/mod.rs @@ -0,0 +1,24 @@ +//! Soft-body contact detection: the geometric passes the narrow phase runs on every updated pair +//! with a deformable collider (vertex-vs-surface and edge-vs-edge candidates, boundary crossings) +//! and per soft mesh for self contacts; results: [`SoftPairContacts`], [`SoftSelfContacts`]. + +mod soft_contacts_classify; +mod soft_contacts_driver; +mod soft_contacts_edge_pass; +mod soft_contacts_pairs; +mod soft_contacts_types; +mod soft_contacts_vertex_pass; +mod soft_contacts_volume; +mod soft_self_contacts; + +pub(crate) use soft_contacts_edge_pass::detect_edges; +pub(crate) use soft_contacts_pairs::{update_pair_soft_rigid, update_pair_soft_soft}; +pub(crate) use soft_self_contacts::SoftSelfContacts; +pub use soft_contacts_types::{ + SoftContactImpulse, SoftEdgeCandidate, SoftEdgePass, SoftPairContacts, SoftRigidPatch, + SoftVertexCandidate, SoftVertexHits, SoftVertexPass, SoftVolumePatch, +}; +pub(crate) use soft_contacts_types::{SelfTangles, SoftDetectionCtx, SoftRigidContacts, body_frozen}; +use soft_contacts_types::Side; +pub(crate) use soft_contacts_vertex_pass::{detect_vertex_pass, element_plane, elements_cross}; +pub use soft_contacts_volume::VolumeBin; diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_driver.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_driver.rs new file mode 100644 index 000000000..c95b904cf --- /dev/null +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_driver.rs @@ -0,0 +1,83 @@ +//! The narrow-phase driver of the self contact detection: runs the self pass of every self-colliding mesh of the awake soft bodies and keeps its results per collider. + +use crate::alloc_prelude::*; + +use crate::dynamics::{RigidBodySet, SoftMeshRef}; +use crate::geometry::ColliderHandle; +use crate::geometry::narrow_phase::NarrowPhase; + +use super::soft_self_contacts::update_self; +use super::{SoftDetectionCtx, SoftSelfContacts, body_frozen}; + +impl NarrowPhase { + /// The self contact detection of the soft collision mesh held by `collider`, as of the + /// last narrow-phase update that ran it (`None` for a sleeping body, or a mesh without + /// self contacts). + pub(crate) fn soft_self_contacts(&self, collider: ColliderHandle) -> Option<&SoftSelfContacts> { + self.soft_self.get(&collider) + } + + /// Runs the self contact detection of every self-colliding mesh of the awake soft bodies + /// (after the pair transitions: a body woken by a new contact is included), in parallel + /// under the `parallel` feature. + pub(crate) fn update_soft_self_contacts( + &mut self, + ctx: &SoftDetectionCtx, + bodies: &RigidBodySet, + ) { + let mut jobs: Vec<(ColliderHandle, SoftMeshRef, SoftSelfContacts)> = Vec::new(); + for (handle, sb) in ctx.soft_bodies.iter() { + let awake = bodies + .get(sb.root_body()) + .is_some_and(|rb| !rb.is_sleeping() && rb.is_enabled()); + if !awake || body_frozen(sb) { + continue; + } + for mesh in sb.meshes() { + if !mesh.collision_enabled() || !mesh.self_contacts_enabled() { + continue; + } + let collider = mesh.collider(); + let payload = self.soft_self.remove(&collider).unwrap_or_default(); + jobs.push(( + collider, + SoftMeshRef { + body: handle, + id: mesh.id(), + }, + payload, + )); + } + } + // The entries left are those of the sleeping bodies (kept for their wake-up) and of + // the removed meshes (dropped). + let colliders = ctx.colliders; + self.soft_self + .retain(|h, _| colliders.get(*h).is_some_and(|c| c.deformable_mesh_ref.is_some())); + let run = |(collider, mesh_ref, payload): &mut ( + ColliderHandle, + SoftMeshRef, + SoftSelfContacts, + )| { + let (Some(sb), Some(co)) = + (ctx.soft_bodies.get(mesh_ref.body), colliders.get(*collider)) + else { + return; + }; + let Some(mesh) = sb.mesh(mesh_ref.id) else { + return; + }; + update_self(payload, sb, mesh, *collider, co, ctx); + }; + #[cfg(feature = "parallel")] + { + use rayon::prelude::*; + jobs.par_iter_mut().for_each(run); + } + #[cfg(not(feature = "parallel"))] + jobs.iter_mut().for_each(run); + for (collider, _, payload) in jobs { + self.soft_self.insert(collider, payload); + } + } +} diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs new file mode 100644 index 000000000..9bf282654 --- /dev/null +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs @@ -0,0 +1,160 @@ +//! The edge-vs-edge pass of a pair of soft surfaces (or of a mesh against itself). + +use crate::alloc_prelude::*; + +use crate::dynamics::{SoftBody, SoftCollisionMesh}; +use crate::math::{Real, Vector}; +use crate::utils::DotProduct; +use parry::bounding_volume::BoundingVolume; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +#[cfg(feature = "parallel")] +use super::soft_contacts_vertex_pass::PARALLEL_CHUNK; +use super::soft_contacts_vertex_pass::element_points; +use super::{SelfTangles, Side, SoftDetectionCtx, SoftEdgeCandidate, SoftEdgePass, elements_cross}; + +/// The edge-vs-edge candidates between the surfaces of `own` and `other` (itself for a self pass, +/// with `tangles` set); complements the vertex-vs-surface constraints, keeping only contacts +/// interior to both edges. Returns whether the pass runs at all. +pub(crate) fn detect_edges( + out: &mut SoftEdgePass, + (sb, mesh, surface_handle, surface_co): Side<'_>, + (other, other_mesh, other_surface_handle, other_co): Side<'_>, + tangles: Option>, + ctx: &SoftDetectionCtx, +) -> bool { + let params = ctx.params; + let step_dt = ctx.dt; + let is_self = tangles.is_some(); + out.candidates.clear(); + out.crossed = false; + out.slot_offset = u32::MAX; + out.own = surface_handle; + out.other = other_surface_handle; + // In 2D two closed surfaces meet through their vertex constraints alone (point-vs-segment is + // complete for first contact); in 3D two edge-leading closed bodies (slim bars crossing + // corner-first) have no vertex near the other's surface and need the edge constraints. + let Some(other_bvh) = other_co.shape().as_composite_shape().map(|c| c.bvh()) else { + return false; + }; + let skins = surface_co.contact_skin() + other_co.contact_skin(); + let motion = ctx.motion_margin(surface_co) + ctx.motion_margin(other_co); + let scan = EdgeScan { + }; + let n_e = mesh.indices().len(); + #[cfg(feature = "parallel")] + if n_e >= 2 * PARALLEL_CHUNK && rayon::current_num_threads() > 1 { + let chunks: Vec> = (0..n_e.div_ceil(PARALLEL_CHUNK)) + .into_par_iter() + .map(|c| { + let mut candidates = Vec::new(); + for i in c * PARALLEL_CHUNK..((c + 1) * PARALLEL_CHUNK).min(n_e) { + scan.scan_element(i, &mut candidates); + } + }) + .collect(); + for chunk in chunks { + out.candidates.extend(chunk); + } + } + for i in 0..n_e { + scan.scan_element(i, &mut out.candidates); + } +} +struct EdgeScan<'a> { + tangled_elements: &'a [bool], + crossed_own: &'a [bool], + crossed_other: &'a [bool], +} +impl EdgeScan<'_> { + fn scan_element(&self, i: usize, out: &mut Vec) { + let (mesh, other_mesh) = (self.mesh, self.other_mesh); + let element = mesh.element(i); + let mut aabb = crate::geometry::Aabb::new_invalid(); + for &v in element { + aabb.take_point(mesh.cached_vertex(v as usize)); + } + for j in self.other_bvh.intersect_aabb(&aabb) { + let i = i as u32; + if self.is_self { + if j <= i { + continue; + } + let (ei, ej) = (mesh.element(i as usize), other_mesh.element(j as usize)); + if ei.iter().any(|v| ej.contains(v)) { + continue; + } + for ea in self.mesh.element_edge_ids(i as usize) { + if self.mesh.edge_owner(ea) != i { + continue; + } + let va = self.mesh.edge_vertices(ea); + let pa = [ + self.mesh.cached_vertex(va[0] as usize), + self.mesh.cached_vertex(va[1] as usize), + ]; + // The edge's reach box: an edge pair whose boxes miss is farther apart than + // the reach (the box distance bounds the segment distance from below). + let box_a = crate::geometry::Aabb::new(pa[0].min(pa[1]), pa[0].max(pa[1])) + .loosened(self.reach); + for eb in self.other_mesh.element_edge_ids(j as usize) { + if self.other_mesh.edge_owner(eb) != j { + continue; + } + let vb = self.other_mesh.edge_vertices(eb); + if self.is_self && va.iter().any(|v| vb.contains(v)) { + continue; + } + let pb = [ + self.other_mesh.cached_vertex(vb[0] as usize), + self.other_mesh.cached_vertex(vb[1] as usize), + ]; + if !box_a.intersects(&crate::geometry::Aabb::new(pb[0].min(pb[1]), pb[0].max(pb[1]))) + { + continue; + } + // Near-parallel edges never cross: their proximity is a face contact, + // handled by the vertex constraints (and their closest points are ill-defined). + let (da, db) = (pa[1] - pa[0], pb[1] - pb[0]); + #[cfg(feature = "dim2")] + let sin_sq = { + let c = da.perp_dot(db); + c * c + }; + #[cfg(feature = "dim3")] + let sin_sq = da.cross(db).length_squared(); + if sin_sq < 0.01 * da.length_squared() * db.length_squared() { + continue; + } + let (loc_a, loc_b) = + // Endpoint contacts are vertex contacts, carried by the vertex rows. + ) = (loc_a, loc_b) + else { + continue; + let point_a = pa[0] * ba[0] + pa[1] * ba[1]; + let point_b = pb[0] * bb[0] + pb[1] * bb[1]; + let sep = point_b - point_a; + let len = sep.length(); + if len >= self.reach || len < 1.0e-6 { + // Force direction on this body's edge (away from the self.other edge). + let dir = -sep / len; + out.push(SoftEdgeCandidate { + edge: ea, + other_edge: eb, + bcoords: ba, + other_bcoords: bb, + point: point_a, + other_point: point_b, + dir, + dist, + enabled: true, + impulse: 0.0, + tangent_impulse: Vector::ZERO, + }); + } + } + } + } +} diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs new file mode 100644 index 000000000..7693ec50e --- /dev/null +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs @@ -0,0 +1,378 @@ +//! The data types of the soft contact detection: the step context, the pair candidates (vertex, edge and volume), the pair payload and its impulse reports, the self-tangle signal. + +use crate::alloc_prelude::*; +use core::ops::Range; + +use crate::dynamics::{ + IntegrationParameters, RigidBodySet, SoftBody, SoftBodySet, SoftCollisionMesh, +}; +use crate::geometry::{Collider, ColliderHandle, ColliderSet}; +use crate::math::{DIM, Real, Rotation, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::VolumeBin; + +/// The step-constant inputs of the soft contact detection. +pub(crate) struct SoftDetectionCtx<'a> { + pub soft_bodies: &'a SoftBodySet, + pub bodies: &'a RigidBodySet, + pub colliders: &'a ColliderSet, + pub params: &'a IntegrationParameters, + /// The whole step's length (the closing-speed rule of the speculative reach). + pub dt: Real, +} + +impl SoftDetectionCtx<'_> { + /// The speculative motion margin of a soft body's collider, held by its parent cluster proxy + /// (zero 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) + } +} + +/// A surface element within reach of a vertex of the other surface (see [`SoftVertexPass`]): +/// the closest point's barycentric weights on the element and the separation, skins deducted. +/// The contact hook may edit `dir` and `dist` or disable it; the solver reports its impulse. +#[derive(Copy, Clone, Debug)] +pub struct SoftVertexCandidate { + /// The element of the surface side. + pub element: u32, + /// The closest point's barycentric weights on the element. + pub weights: [Real; DIM], + /// The force direction on the surface (from the vertex toward its closest point). + pub dir: Vector, + /// The separation, skins deducted (negative: penetrating). + pub dist: Real, + /// The element's outward normal (closed surfaces only). + pub outward: Option, + /// The closest point lies inside the element (not on its boundary). + pub interior: bool, + /// Whether the solver builds a constraint for it (the hook clears it to drop the contact). + pub enabled: bool, + /// The normal impulse the solver applied through it at the last step. + pub impulse: Real, + /// The world-space friction impulse the solver applied through it at the last step. + pub tangent_impulse: Vector, +} + +/// The candidates of one surface vertex against the other side's surface. +#[derive(Clone, Debug)] +pub struct SoftVertexHits { + /// The vertex of the vertex side. + pub vertex: u32, + /// Range into [`SoftVertexPass::candidates`]. + pub candidates: Range, + /// The vertex crossed the closed surface (seen from behind only, and inside by parity): + /// the pair is recovery-owned. + pub crossed: bool, +} + +/// An edge-vs-edge contact candidate (see [`SoftEdgePass`]): the closest points of two +/// edges, interior to both. The contact-modification hook may edit `dir` and `dist`, or +/// disable the candidate; the soft-body solver reports the impulse it applied. +#[derive(Copy, Clone, Debug)] +pub struct SoftEdgeCandidate { + /// The edge of the owner mesh. + pub edge: u32, + /// The edge of the other mesh. + pub other_edge: u32, + /// The closest point's barycentric weights on the owner's edge. + pub bcoords: [Real; 2], + /// The closest point's barycentric weights on the other edge. + pub other_bcoords: [Real; 2], + /// The closest point on the owner's edge (world space). + pub point: Vector, + /// The closest point on the other edge (world space). + pub other_point: Vector, + /// The force direction on the owner's edge (away from the other edge). + pub dir: Vector, + /// The separation, skins deducted (negative: penetrating). + pub dist: Real, + /// Whether the solver builds a constraint for it (the hook clears it to drop the contact). + pub enabled: bool, + /// The normal impulse the solver applied through it at the last step. + pub impulse: Real, + /// The world-space friction impulse the solver applied through it at the last step. + pub tangent_impulse: Vector, +} + +/// The vertex-vs-surface pass of one side of a pair of soft surfaces: the vertices of +/// `vertices_of` against the surface of `surface` (for a self pass, both are the same mesh). +#[derive(Clone, Default, Debug)] +pub struct SoftVertexPass { + /// The collider whose surface the vertices are tested against. + pub surface: ColliderHandle, + /// The collider whose surface vertices are tested. + pub vertices_of: ColliderHandle, + /// Elements of the surface side, and vertices of the vertex side, taking part in a + /// crossing between the two surfaces (empty while they do not cross): constraints touching them + /// may only expel, never hold. + pub cross_tangled_elements: Vec, + pub cross_tangled_vertices: Vec, + /// Element-granularity crossing flags on the vertex-side mesh, and the recorded + /// (surface-side, vertex-side) crossing element pairs. + pub cross_tangled_vb_elements: Vec, + pub cross_pairs: Vec<(u32, u32)>, + pub hits: Vec, + pub candidates: Vec, + /// The volume normals guiding the crossing repulsion (see `crossing_repulsion_guide`): per + /// grid cell, its center and its normal from the surface side into the vertex side (for a + /// self pass, the push direction of the fold's vertices, the facing surface's the opposite). + pub repel_guides: Vec<(Vector, Vector)>, + /// Per vertex of the vertex side, whether it lies inside the surface side's volume patch + /// (see `overlap_patch_constraints`; empty under the `Keep` policy). + pub patch_inside_vb: Vec, + /// For a self pass, whether each vertex lies in its own body's self-intersection region + /// (see `classify_inside_self`). + pub repel_inside: Vec, + /// The pair's contact slot of the first candidate (candidate `i` is slot `offset + i`, + /// see [`SoftPairContacts::report_impulse`]); `u32::MAX` for a self pass. + pub(crate) slot_offset: u32, +} + +impl SoftVertexPass { + pub(super) fn clear(&mut self) { + self.cross_tangled_elements.clear(); + self.cross_tangled_vertices.clear(); + self.cross_tangled_vb_elements.clear(); + self.cross_pairs.clear(); + self.hits.clear(); + self.candidates.clear(); + self.repel_guides.clear(); + self.patch_inside_vb.clear(); + self.repel_inside.clear(); + self.slot_offset = u32::MAX; + } + + /// The candidates of a vertex hit. + pub fn candidates_of(&self, hit: &SoftVertexHits) -> &[SoftVertexCandidate] { + &self.candidates[hit.candidates.start as usize..hit.candidates.end as usize] + } +} + +/// The edge-vs-edge candidates of a pair, owned by (oriented from) `own`. +#[derive(Clone, Default, Debug)] +pub struct SoftEdgePass { + /// The collider whose edges own the pass (the candidates' `edge` and `dir`). + pub own: ColliderHandle, + /// The other collider. + pub other: ColliderHandle, + /// The candidates. + pub candidates: Vec, + /// The two surfaces cross (found by the edge pass's own crossing test): the pair is + /// recovery-owned. + pub crossed: bool, + /// The pair's contact slot of the first candidate (see + /// [`SoftPairContacts::report_impulse`]); `u32::MAX` for a self pass. + pub(crate) slot_offset: u32, +} + +/// The volume patches of a crossed pair of closed soft surfaces (see `SoftOverlapConstraint`): the +/// bins' own side is the surface of `own` (the pair's lower collider). +#[derive(Clone, Debug)] +pub struct SoftVolumePatch { + /// The collider of the bins' own side. + pub own: ColliderHandle, + /// The bins (the volume constraints). + pub bins: Vec, + /// The pair's contact slot of the first bin (see [`SoftPairContacts::report_impulse`]). + pub(crate) slot_offset: u32, +} + +/// The volume patch of a closed soft surface intruded by a rigid collider (see +/// `SoftOverlapConstraint`): the bins (own side only), the surface vertices' depths in the patch +/// (`NEG_INFINITY` outside, patch-constraint policies only), and the rigid side's CoM and rotation. +#[derive(Clone, Debug)] +pub struct SoftRigidPatch { + pub bins: Vec, + pub depth: Vec, + pub com: Vector, + pub rot: Rotation, +} + +/// The contacts of a pair of two soft surfaces: the candidates the narrow phase detected between +/// them, which the soft-body solver turns into constraints, rebuilt by every update of the pair. +/// Each slot (vertex-pass, edge, then volume-bin candidates) reports the solver's impulse. +#[derive(Clone, Default, Debug)] +pub struct SoftPairContacts { + /// Both vertex passes: the vertices of the pair's second collider against the first's + /// surface, then the reverse. + pub vertex_passes: Vec, + /// The edge-vs-edge pass (`None`: no edge contacts for this pair). + pub edges: Option, + /// The volume patches of a crossed pair of closed surfaces. + pub volume: Option, +} + +/// The impulse a contact slot reported (see [`SoftPairContacts::impulses`]). +#[derive(Copy, Clone, Debug)] +pub struct SoftContactImpulse { + /// The world-space normal, from the pair's first collider to the second. + pub normal: Vector, + /// The normal impulse applied along it. + pub impulse: Real, + /// The world-space friction impulse. + pub tangent_impulse: Vector, +} + +impl SoftPairContacts { + pub(crate) fn clear(&mut self) { + for pass in &mut self.vertex_passes { + pass.clear(); + } + self.vertex_passes.clear(); + if let Some(e) = &mut self.edges { + e.candidates.clear(); + e.crossed = false; + e.slot_offset = u32::MAX; + } + self.edges = None; + self.volume = None; + } + + /// The vertex pass whose surface side is `surface`. + pub fn vertex_pass_on(&self, surface: ColliderHandle) -> Option<&SoftVertexPass> { + self.vertex_passes.iter().find(|pass| pass.surface == surface) + } + + /// Disables every candidate (the contact-modification hook's way of dropping the pair's + /// contacts: the pair still reports as touching). + pub fn disable_all(&mut self) { + for pass in &mut self.vertex_passes { + for c in &mut pass.candidates { + c.enabled = false; + } + } + if let Some(e) = &mut self.edges { + for c in &mut e.candidates { + c.enabled = false; + } + } + if let Some(v) = &mut self.volume { + for b in &mut v.bins { + b.enabled = false; + } + } + } + + /// Whether some candidate exists: a feature of one surface within contact reach of the + /// other, or a volume patch of the crossed pair. + pub fn is_touching(&self) -> bool { + self.vertex_passes.iter().any(|pass| !pass.hits.is_empty()) + || self + .edges + .as_ref() + .is_some_and(|e| !e.candidates.is_empty()) + || self.volume.as_ref().is_some_and(|v| !v.bins.is_empty()) + } + + /// Numbers the contact slots: the vertex passes' candidates, the edge candidates, the bins. + pub(super) fn number_slots(&mut self) { + let mut n = 0u32; + for pass in &mut self.vertex_passes { + pass.slot_offset = n; + n += pass.candidates.len() as u32; + } + if let Some(e) = &mut self.edges { + e.slot_offset = n; + n += e.candidates.len() as u32; + } + if let Some(v) = &mut self.volume { + v.slot_offset = n; + } + } + + /// Records the impulse the solver applied through contact slot `slot` (a bin takes the + /// normal impulse only). + pub(crate) fn report_impulse(&mut self, slot: u32, impulse: Real, tangent_impulse: Vector) { + for pass in &mut self.vertex_passes { + if let Some(c) = slot + .checked_sub(pass.slot_offset) + .and_then(|i| pass.candidates.get_mut(i as usize)) + { + c.impulse = impulse; + c.tangent_impulse = tangent_impulse; + return; + } + } + if let Some(e) = &mut self.edges { + if let Some(c) = slot + .checked_sub(e.slot_offset) + .and_then(|i| e.candidates.get_mut(i as usize)) + { + c.impulse = impulse; + c.tangent_impulse = tangent_impulse; + return; + } + } + if let Some(v) = &mut self.volume { + if let Some(b) = slot + .checked_sub(v.slot_offset) + .and_then(|i| v.bins.get_mut(i as usize)) + { + b.impulse = impulse; + } + } + } + + /// The impulses the solver applied through the contacts at the last step, with their + /// normals oriented from the pair's first collider (`collider1`) to the second. + pub fn impulses( + &self, + collider1: ColliderHandle, + ) -> impl Iterator + '_ { + let vertices = self.vertex_passes.iter().flat_map(move |pass| { + // `dir` is the force on the surface, from the vertex side into the surface side. + let sign = if pass.surface == collider1 { -1.0 } else { 1.0 }; + pass.candidates.iter().map(move |c| SoftContactImpulse { + normal: c.dir * sign, + impulse: c.impulse, + tangent_impulse: c.tangent_impulse, + }) + }); + let edges = self.edges.iter().flat_map(move |e| { + // `dir` is the force on the owner's edge, from the other side into the owner. + let sign = if e.own == collider1 { -1.0 } else { 1.0 }; + e.candidates.iter().map(move |c| SoftContactImpulse { + normal: c.dir * sign, + impulse: c.impulse, + tangent_impulse: c.tangent_impulse, + }) + }); + let bins = self.volume.iter().flat_map(move |v| { + // A bin's normal goes from its own side into the other. + let sign = if v.own == collider1 { 1.0 } else { -1.0 }; + v.bins.iter().map(move |b| SoftContactImpulse { + normal: b.normal * sign, + impulse: b.impulse, + tangent_impulse: Vector::ZERO, + }) + }); + vertices.chain(edges).chain(bins) + } +} + +/// Whether a soft body has no free particle at all (fully pinned): it has no constraint of its own, +/// but its surface still holds the awake bodies meeting it. +pub(crate) fn body_frozen(sb: &SoftBody) -> bool { + !sb.particles.iter().any(|p| p.inv_mass > 0.0) +} + +/// The self-tangle signal of a mesh, for a self pass: the features whose self contacts stand +/// down, and the recorded self-crossings. +pub(crate) struct SelfTangles<'a> { + pub tangled_elements: &'a [bool], + pub tangled_vertices: &'a [bool], + pub crossings: &'a [(u32, u32)], +} + +pub(super) type Side<'a> = ( + &'a SoftBody, + &'a SoftCollisionMesh, + ColliderHandle, + &'a Collider, +); diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs new file mode 100644 index 000000000..3fda74352 --- /dev/null +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs @@ -0,0 +1,10 @@ + // Crossing repulsion (see `repel_row`): the detected crossing pairs also get rows + if len >= reach || len < 1.0e-6 { + } + let dist = len - skins; + // Beyond the prediction distance, only a contact closing fast enough to happen + // `params.dt` is the substep): the rest of the reach is the bodies' motion + // margin, and a resting vertex must not carry rows (warm-started) against every + // a ghost of that internal feature (the neighbor carries the actual contact); + // Deterministic order, and one constraint per surface vertex touched: the elements sharing + diff --git a/src/geometry/narrow_phase/solver_graph.rs b/src/geometry/narrow_phase/solver_graph.rs index 44315f62b..a97a86a87 100644 --- a/src/geometry/narrow_phase/solver_graph.rs +++ b/src/geometry/narrow_phase/solver_graph.rs @@ -527,7 +527,7 @@ impl NarrowPhase { let selectable = hint & PAIR_HINT_DYN_BIT != 0 && hint & PAIR_HINT_COUNT_MASK != 0; let pair: *const ContactPair = unsafe { &(*edges_ptr.add(edge as usize)).weight }; - let pair_color = unsafe { (*pair).solver_color }; + let pair_color = unsafe { (*pair).solver_color() }; let sm = unsafe { (*pair).solver_manifolds() }; let mut first_of_pair = true; @@ -536,7 +536,7 @@ impl NarrowPhase { && manifold .data .solver_flags - .contains(SolverFlags::COMPUTE_IMPULSES) + .contains(SolverFlags::COMPUTE_RIGID_IMPULSES) && manifold.data.num_active_contacts() != 0; let desired = if qualifies { Self::qualify_manifold_bqi(bqi, manifold).map(|solver_body_ids| { @@ -593,7 +593,7 @@ impl NarrowPhase { let selectable = hint & PAIR_HINT_DYN_BIT != 0 && hint & PAIR_HINT_COUNT_MASK != 0; let pair: *mut ContactPair = unsafe { &mut (*edges_ptr.add(edge as usize)).weight }; - let pair_color = unsafe { (*pair).solver_color }; + let pair_color = unsafe { (*pair).solver_color() }; let (sm_ptr, num): (*mut ContactManifold, usize) = unsafe { let sm = (*pair).solver_manifolds_mut(); (sm.as_mut_ptr(), sm.len()) @@ -608,7 +608,7 @@ impl NarrowPhase { && unsafe { (*mdata) .solver_flags - .contains(SolverFlags::COMPUTE_IMPULSES) + .contains(SolverFlags::COMPUTE_RIGID_IMPULSES) && (*mdata).num_active_contacts() != 0 }; let desired = if qualifies { @@ -740,13 +740,13 @@ impl NarrowPhase { continue; } let pair = &edges[pair_id].weight; - let pair_color = pair.solver_color; + let pair_color = pair.solver_color(); let mut first_of_pair = true; for (ordinal, manifold) in pair.solver_manifolds().iter().enumerate() { if !manifold .data .solver_flags - .contains(SolverFlags::COMPUTE_IMPULSES) + .contains(SolverFlags::COMPUTE_RIGID_IMPULSES) || manifold.data.num_active_contacts() == 0 { continue; diff --git a/src/geometry/narrow_phase/test.rs b/src/geometry/narrow_phase/test.rs index f1bef5297..9353b1b35 100644 --- a/src/geometry/narrow_phase/test.rs +++ b/src/geometry/narrow_phase/test.rs @@ -11,7 +11,7 @@ use std::println; use super::*; -use crate::dynamics::{ImpulseJointSet, MultibodyJointSet}; +use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, SoftBodySet}; /// Test for https://github.com/dimforge/rapier/issues/734. #[test] @@ -55,6 +55,7 @@ pub fn collider_set_parent_depenetration() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); let mut multibody_joint_set = MultibodyJointSet::new(); + let mut soft_body_set = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let physics_hooks = (); let event_handler = (); @@ -69,6 +70,7 @@ pub fn collider_set_parent_depenetration() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -83,7 +85,7 @@ pub fn collider_set_parent_depenetration() { assert!( narrow_phase .contact_pair(collider_1_handle, collider_2_handle) - .is_none_or(|pair| pair.manifolds.is_empty()), + .is_none_or(|pair| pair.manifolds().is_empty()), "No contact should be simulated between same-parent colliders." ); assert!( @@ -105,6 +107,7 @@ pub fn collider_set_parent_depenetration() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -113,7 +116,7 @@ pub fn collider_set_parent_depenetration() { let contact_pair = narrow_phase .contact_pair(collider_1_handle, collider_2_handle) .expect("The contact pair should exist."); - assert_eq!(contact_pair.manifolds.len(), 1); + assert_eq!(contact_pair.manifolds().len(), 1); assert!( narrow_phase .intersection_pair(collider_1_handle, collider_2_handle) @@ -133,6 +136,7 @@ pub fn collider_set_parent_depenetration() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -198,6 +202,7 @@ pub fn collider_set_parent_no_self_intersection() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); let mut multibody_joint_set = MultibodyJointSet::new(); + let mut soft_body_set = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let physics_hooks = (); let event_handler = (); @@ -212,6 +217,7 @@ pub fn collider_set_parent_no_self_intersection() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -221,7 +227,7 @@ pub fn collider_set_parent_no_self_intersection() { .contact_pair(collider_1_handle, collider_2_handle) .expect("The contact pair should exist."); assert_eq!( - contact_pair.manifolds.len(), + contact_pair.manifolds().len(), 1, "There should be a contact manifold." ); @@ -242,6 +248,7 @@ pub fn collider_set_parent_no_self_intersection() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -251,7 +258,7 @@ pub fn collider_set_parent_no_self_intersection() { .contact_pair(collider_1_handle, collider_2_handle) .expect("The contact pair should no longer exist."); assert_eq!( - contact_pair.manifolds.len(), + contact_pair.manifolds().len(), 0, "Colliders with same parent should not be in contact together." ); @@ -268,6 +275,7 @@ pub fn collider_set_parent_no_self_intersection() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -277,7 +285,7 @@ pub fn collider_set_parent_no_self_intersection() { .contact_pair(collider_1_handle, collider_2_handle) .expect("The contact pair should exist."); assert_eq!( - contact_pair.manifolds.len(), + contact_pair.manifolds().len(), 1, "There should be a contact manifold." ); diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 795128af1..113ba5cf9 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -111,6 +111,7 @@ impl CollisionPipeline { modified_colliders, hooks, events, + None, ); narrow_phase.compute_intersections( islands, diff --git a/src/pipeline/debug_render_pipeline/debug_render_backend.rs b/src/pipeline/debug_render_pipeline/debug_render_backend.rs index 86e5c8074..22d56f8af 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_backend.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_backend.rs @@ -1,6 +1,7 @@ use super::DebugColor; use crate::dynamics::{ ImpulseJoint, ImpulseJointHandle, Multibody, MultibodyLink, RigidBody, RigidBodyHandle, + SoftBody, SoftBodyHandle, }; use crate::geometry::{Aabb, Collider, ContactPair}; use crate::math::{Pose, Vector}; @@ -21,6 +22,8 @@ pub enum DebugRenderObject<'a> { MultibodyJoint(MultibodyJointHandle, &'a Multibody, &'a MultibodyLink), /// The contacts of a contact-pair are being rendered. ContactPair(&'a ContactPair, &'a Collider, &'a Collider), + /// A soft body's elements and edge-vs-edge contacts are being rendered. + SoftBody(SoftBodyHandle, &'a SoftBody), } /// Trait implemented by graphics backends responsible for rendering the physics scene. diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs index 73df0df43..07b9d81ed 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs @@ -2,6 +2,7 @@ use super::{DebugColor, DebugRenderBackend, outlines}; use crate::alloc_prelude::*; use crate::dynamics::{ GenericJoint, ImpulseJointSet, MultibodyJointSet, RigidBody, RigidBodySet, RigidBodyType, + SoftBodyEdgeKind, SoftBodySet, }; use crate::geometry::{Ball, ColliderSet, Cuboid, NarrowPhase, Shape, TypedShape}; #[cfg(feature = "dim3")] @@ -37,6 +38,9 @@ bitflags::bitflags! { const CONTACTS = 1 << 5; /// If this flag is set, the Aabbs of colliders will be rendered. const COLLIDER_AABBS = 1 << 6; + /// If this flag is set, the soft bodies' elements (structural edges and cell edges) and + /// their soft-vs-soft contacts (vertex-vs-surface and edge-vs-edge) will be rendered. + const SOFT_BODIES = 1 << 7; } } @@ -63,6 +67,9 @@ pub struct DebugRenderPipeline { /// Flags controlling what part of the physics engine need to /// be rendered. pub mode: DebugRenderMode, + /// The soft-body edges to draw for the body being rendered, with their load (a cage edge is + /// shared by every cell around it: the largest). Kept here to be reused from frame to frame. + drawn_edges: HashMap<[u32; 2], ()>, } impl Default for DebugRenderPipeline { @@ -78,11 +85,12 @@ impl DebugRenderPipeline { instances: outlines::instances(style.subdivisions), style, mode, + drawn_edges: HashMap::default(), } } /// The color multiplier for one body's attached entities: disabled, asleep, sleep-eligible, - /// or plain awake. `eligible_tint` opts out for entities whose hue already carries meaning. + /// or plain awake. `eligible_tint` opts out for entities whose hue already conveys meaning. fn body_color_multiplier( &self, rb: &RigidBody, @@ -116,11 +124,80 @@ impl DebugRenderPipeline { impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, narrow_phase: &NarrowPhase, + soft_bodies: &SoftBodySet, ) { self.render_rigid_bodies(backend, bodies); self.render_colliders(backend, bodies, colliders); self.render_joints(backend, bodies, impulse_joints, multibody_joints); self.render_contacts(backend, colliders, narrow_phase); + self.render_soft_bodies(backend, soft_bodies); + } + + /// Render the soft bodies' elements (structural edges and cell edges as a wireframe, from + /// the particles' positions at the end of the last step) and their soft-vs-soft contacts + /// (vertex-vs-surface and edge-vs-edge). + #[profiling::function] + pub fn render_soft_bodies( + &mut self, + backend: &mut B, + soft_bodies: &SoftBodySet, + ) { + if !self.mode.contains(DebugRenderMode::SOFT_BODIES) { + return; + } + for (handle, sb) in soft_bodies.iter() { + let object = DebugRenderObject::SoftBody(handle, sb); + if !backend.filter_object(object) { + continue; + } + let color = self.style.soft_body_element_color; + // The cage is drawn edge by edge rather than cell by cell (an edge is shared by every + // cell around it, and duplicates only thicken the picture); each edge keeps the largest + // load of the elements sharing it. + self.drawn_edges.clear(); + let draw_once = |backend: &mut B, drawn: &mut HashMap<[u32; 2], ()>, e: [u32; 2]| { + let key = [e[0].min(e[1]), e[0].max(e[1])]; + if drawn.insert(key, ()).is_none() { + backend.draw_line( + object, + sb.particle_position(key[0] as usize), + sb.particle_position(key[1] as usize), + color, + ); + } + }; + for e in sb.edges() { + if e.kind == SoftBodyEdgeKind::Structural { + draw_once(backend, &mut self.drawn_edges, e.vertices); + } + } + for c in sb.cells() { + for a in 0..DIM + 1 { + for b in a + 1..DIM + 1 { + draw_once( + backend, + &mut self.drawn_edges, + [c.vertices[a], c.vertices[b]], + ); + } + } + } + // Same reading as the rigid contacts: the depth segment joins the two witness + // points, and the normal starts on the surface side. A contact sitting exactly on + // the surface has no direction to show, so it gets the segment alone. + let depth_color = self.style.contact_depth_color; + let normal_color = self.style.contact_normal_color; + let normal_length = self.style.contact_normal_length; + for (a, b) in sb + .edge_contact_segments(soft_bodies) + .chain(sb.vertex_contact_segments(soft_bodies)) + { + backend.draw_line(object, a, b, depth_color); + if let Some(n) = (a - b).try_normalize() { + backend.draw_line(object, b, b + n * normal_length, normal_color); + } + } + } } /// Render contact. @@ -173,7 +250,7 @@ impl DebugRenderPipeline { let object = DebugRenderObject::ContactPair(pair, co1, co2); if backend.filter_object(object) { - for manifold in &pair.manifolds { + for manifold in pair.manifolds() { let world_pos1 = manifold.subshape_pos1().prepend_to(co1.position()); let world_pos2 = manifold.subshape_pos2().prepend_to(co2.position()); for contact in &manifold.data.solver_contacts { diff --git a/src/pipeline/debug_render_pipeline/debug_render_style.rs b/src/pipeline/debug_render_pipeline/debug_render_style.rs index 9b2d64fd3..0c5cd9d77 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_style.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_style.rs @@ -57,6 +57,8 @@ pub struct DebugRenderStyle { pub contact_normal_color: DebugColor, /// The length of the contact normals. pub contact_normal_length: Real, + /// The color of the soft bodies' elements (structural edges, cell edges). + pub soft_body_element_color: DebugColor, /// The color of the colliders' [`Aabb`](crate::geometry::Aabb)s. pub collider_aabb_color: DebugColor, } @@ -83,6 +85,7 @@ impl Default for DebugRenderStyle { contact_depth_color: [120.0, 1.0, 0.4, 1.0], contact_normal_color: [0.0, 1.0, 1.0, 1.0], contact_normal_length: 0.3, + soft_body_element_color: [200.0, 0.8, 0.5, 1.0], collider_aabb_color: [124.0, 1.0, 0.4, 1.0], } } diff --git a/src/pipeline/event_handler.rs b/src/pipeline/event_handler.rs index 4905a4ee8..602b9403d 100644 --- a/src/pipeline/event_handler.rs +++ b/src/pipeline/event_handler.rs @@ -49,11 +49,13 @@ impl Default for ActiveEvents { } } -/// A callback interface for receiving physics events (collisions starting/stopping, contact forces). +/// A callback interface for receiving physics events (collisions starting/stopping, contact forces, +/// soft-body tears). /// /// Implement this trait to get notified when: /// - Two colliders start or stop touching ([`handle_collision_event`](Self::handle_collision_event)) /// - Contact forces exceed a threshold ([`handle_contact_force_event`](Self::handle_contact_force_event)) +/// - A soft body tears ([`handle_soft_body_tear_event`](Self::handle_soft_body_tear_event)) /// /// # Common use cases /// - Playing sound effects when objects collide @@ -88,6 +90,7 @@ impl Default for ActiveEvents { /// } /// } /// # fn handle_contact_force_event(&self, _dt: Real, _bodies: &RigidBodySet, _colliders: &ColliderSet, _contact_pair: &ContactPair, _total_force_magnitude: Real) {} +/// # fn handle_soft_body_tear_event(&self, _soft_bodies: &SoftBodySet, _event: &SoftBodyTearEvent) {} /// } /// ``` #[cfg(feature = "alloc")] @@ -174,7 +177,9 @@ impl EventHandler for () { /// /// let (collision_send, collision_recv) = channel(); /// let (contact_force_send, contact_force_recv) = channel(); -/// let event_handler = ChannelEventCollector::new(collision_send, contact_force_send); +/// let (soft_body_tear_send, soft_body_tear_recv) = channel(); +/// let event_handler = +/// ChannelEventCollector::new(collision_send, contact_force_send, soft_body_tear_send); /// /// // After physics step: /// while let Ok(collision_event) = collision_recv.try_recv() { @@ -183,6 +188,9 @@ impl EventHandler for () { /// CollisionEvent::Stopped(h1, h2, _) => println!("Separated"), /// } /// } +/// while let Ok(tear_event) = soft_body_tear_recv.try_recv() { +/// println!("Soft body {:?} tore", tear_event.soft_body); +/// } /// ``` #[cfg(feature = "std")] pub struct ChannelEventCollector { @@ -192,7 +200,9 @@ pub struct ChannelEventCollector { #[cfg(feature = "std")] impl ChannelEventCollector { - /// Initialize a new collision event handler from channel senders. + /// Initialize a new event collector from channel senders, one per event kind. + /// + /// A sender whose receiver was dropped just discards the events of its kind. pub fn new( collision_event_sender: std::sync::mpsc::Sender, contact_force_event_sender: std::sync::mpsc::Sender, diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs index 95730d909..b662bac9b 100644 --- a/src/pipeline/mod.rs +++ b/src/pipeline/mod.rs @@ -10,7 +10,10 @@ pub use event_handler::EventHandler; #[cfg(feature = "alloc")] pub use physics_hooks::ActiveHooks; #[cfg(feature = "alloc")] -pub use physics_hooks::{ContactModificationContext, PairFilterContext, PhysicsHooks}; +pub use physics_hooks::{ + ContactModificationContext, ModifiableContacts, ModifiableManifold, PairFilterContext, + PhysicsHooks, +}; #[cfg(feature = "alloc")] pub use physics_pipeline::{PhysicsPipeline, Quarantine}; #[cfg(feature = "alloc")] diff --git a/src/pipeline/physics_hooks.rs b/src/pipeline/physics_hooks.rs index 4fdb40bdd..552ff6825 100644 --- a/src/pipeline/physics_hooks.rs +++ b/src/pipeline/physics_hooks.rs @@ -1,6 +1,7 @@ #[cfg(feature = "alloc")] use crate::dynamics::{RigidBodyHandle, RigidBodySet}; #[cfg(feature = "alloc")] +use crate::geometry::SoftPairContacts; use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, SolverContacts, SolverFlags}; #[cfg(feature = "alloc")] use crate::math::{Real, Vector}; @@ -39,6 +40,25 @@ pub struct ContactModificationContext<'a> { pub rigid_body1: Option, /// The handle of the first body involved in the potential collision. pub rigid_body2: Option, + /// The contacts to modify, in the form the pair holds them: a manifold's solver contacts, + /// or the candidates of two soft surfaces. + pub contacts: ModifiableContacts<'a>, +} + +/// The contacts a [`ContactModificationContext`] lets the hook modify (see +/// [`PhysicsHooks::modify_solver_contacts`]). +#[cfg(feature = "alloc")] +pub enum ModifiableContacts<'a> { + /// One contact manifold and its solver contacts: every pair but two soft surfaces. + Rigid(ModifiableManifold<'a>), + /// The contact candidates of two soft surfaces (see [`SoftPairContacts`]), editable in + /// place: a candidate's `dir` and `dist`, or `enabled` to drop it. + Soft(&'a mut SoftPairContacts), +} + +/// A contact manifold as the hook may modify it (see [`ModifiableContacts::Rigid`]). +#[cfg(feature = "alloc")] +pub struct ModifiableManifold<'a> { /// The contact manifold. pub manifold: &'a ContactManifold, /// The solver contacts that can be modified. @@ -65,6 +85,41 @@ pub struct ContactModificationContext<'a> { pub user_data: &'a mut u32, } +#[cfg(feature = "alloc")] +impl<'a> ContactModificationContext<'a> { + /// The manifold being modified (`None` for a pair of two soft surfaces). + pub fn rigid(&self) -> Option<&ModifiableManifold<'a>> { + match &self.contacts { + ModifiableContacts::Rigid(rigid) => Some(rigid), + ModifiableContacts::Soft(_) => None, + } + } + + /// The manifold being modified (`None` for a pair of two soft surfaces). + pub fn rigid_mut(&mut self) -> Option<&mut ModifiableManifold<'a>> { + match &mut self.contacts { + ModifiableContacts::Rigid(rigid) => Some(rigid), + ModifiableContacts::Soft(_) => None, + } + } + + /// The soft contact candidates being modified (`None` unless the pair is two soft surfaces). + pub fn soft(&self) -> Option<&SoftPairContacts> { + match &self.contacts { + ModifiableContacts::Rigid(_) => None, + ModifiableContacts::Soft(soft) => Some(&*soft), + } + } + + /// The soft contact candidates being modified (`None` unless the pair is two soft surfaces). + pub fn soft_mut(&mut self) -> Option<&mut SoftPairContacts> { + match &mut self.contacts { + ModifiableContacts::Soft(soft) => Some(soft), + ModifiableContacts::Rigid(_) => None, + } + } +} + #[cfg(feature = "alloc")] impl ContactModificationContext<'_> { /// Helper function to update `self` to emulate a oneway-platform. @@ -75,37 +130,40 @@ impl ContactModificationContext<'_> { /// /// To make this method work properly it must be called as part of the /// `PhysicsHooks::modify_solver_contacts` method at each timestep, for each - /// contact manifold involving a one-way platform. The `self.user_data` field - /// must not be modified from the outside of this method. + /// contact manifold involving a one-way platform. The manifold's `user_data` field must not be + /// modified outside this method; a pair of two soft surfaces (no manifold) is left alone. pub fn update_as_oneway_platform(&mut self, allowed_local_n1: Vector, allowed_angle: Real) { const CONTACT_CONFIGURATION_UNKNOWN: u32 = 0; const CONTACT_CURRENTLY_ALLOWED: u32 = 1; const CONTACT_CURRENTLY_FORBIDDEN: u32 = 2; + let Some(rigid) = self.rigid_mut() else { + return; + }; let cang = ComplexField::cos(allowed_angle); // Test the allowed normal with the local-space contact normal that // points towards the exterior of context.collider1. - let contact_is_ok = self.manifold.local_n1.dot(allowed_local_n1) >= cang; + let contact_is_ok = rigid.manifold.local_n1.dot(allowed_local_n1) >= cang; - match *self.user_data { + match *rigid.user_data { CONTACT_CONFIGURATION_UNKNOWN => { if contact_is_ok { // The contact is close enough to the allowed normal. - *self.user_data = CONTACT_CURRENTLY_ALLOWED; + *rigid.user_data = CONTACT_CURRENTLY_ALLOWED; } else { // The contact normal isn't close enough to the allowed // normal, so remove all the contacts and mark further contacts // as forbidden. - self.solver_contacts.clear(); + rigid.solver_contacts.clear(); // NOTE: in some very rare cases `local_n1` will be // zero if the objects are exactly touching at one point. // So in this case we can't really conclude. // If the norm is non-zero, then we can tell we need to forbid // further contacts. Otherwise we have to wait for the next frame. - if self.manifold.local_n1.length_squared() > 0.1 { - *self.user_data = CONTACT_CURRENTLY_FORBIDDEN; + if rigid.manifold.local_n1.length_squared() > 0.1 { + *rigid.user_data = CONTACT_CURRENTLY_FORBIDDEN; } } } @@ -114,18 +172,18 @@ impl ContactModificationContext<'_> { // until all the contacts are non-penetrating again. In that case, if // the contacts are OK with respect to the contact normal, then we can // mark them as allowed. - if contact_is_ok && self.solver_contacts.iter().all(|c| c.dist > 0.0) { - *self.user_data = CONTACT_CURRENTLY_ALLOWED; + if contact_is_ok && rigid.solver_contacts.iter().all(|c| c.dist > 0.0) { + *rigid.user_data = CONTACT_CURRENTLY_ALLOWED; } else { // Discard all the contacts. - self.solver_contacts.clear(); + rigid.solver_contacts.clear(); } } CONTACT_CURRENTLY_ALLOWED => { // We allow all the contacts right now. The configuration becomes // uncertain again when the contact manifold no longer contains any contact. - if self.solver_contacts.is_empty() { - *self.user_data = CONTACT_CONFIGURATION_UNKNOWN; + if rigid.solver_contacts.is_empty() { + *rigid.user_data = CONTACT_CONFIGURATION_UNKNOWN; } } _ => unreachable!(), @@ -196,12 +254,12 @@ pub trait PhysicsHooks: crate::utils::MaybeSync { /// not compute any contact manifolds for it. /// If this returns `Some`, then the narrow-phase will compute contact manifolds for /// this pair of colliders, and configure them with the returned solver flags. For - /// example, if this returns `Some(SolverFlags::COMPUTE_IMPULSES)` then the contacts + /// example, if this returns `Some(SolverFlags::COMPUTE_RIGID_IMPULSES)` then the contacts /// will be taken into account by the constraints solver. If this returns /// `Some(SolverFlags::empty())` then the constraints solver will ignore these /// contacts. fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option { - Some(SolverFlags::COMPUTE_IMPULSES) + Some(SolverFlags::COMPUTE_RIGID_IMPULSES) } /// Applies the intersection pair filter. @@ -237,10 +295,13 @@ pub trait PhysicsHooks: crate::utils::MaybeSync { /// By default, the content of `solver_contacts` is computed from `manifold.points`. /// This method will be called on each contact manifold which have the flag `SolverFlags::modify_solver_contacts` set. /// This method can be used to modify the set of solver contacts seen by the constraints solver: contacts - /// can be removed and modified. + /// can be removed and modified. They sit in `context.contacts` (see + /// [`ModifiableContacts`]): the manifold's solver contacts for every pair but two soft + /// surfaces, whose candidates come instead. /// /// Note that if all the contacts have to be ignored by the constraint solver, you may simply - /// do `context.solver_contacts.clear()`. + /// do `context.rigid_mut().unwrap().solver_contacts.clear()` (or `disable_all()` on the + /// soft candidates). /// /// Modifying the solver contacts allow you to achieve various effects, including: /// - Simulating conveyor belts by setting the `surface_velocity` of a solver contact. @@ -252,7 +313,11 @@ pub trait PhysicsHooks: crate::utils::MaybeSync { /// timesteps (as long as the contact manifold exists). This user-defined data is initialized /// as 0 and can be modified in `context.user_data`. /// - /// The world-space contact normal can be modified in `context.normal`. + /// The world-space contact normal can be modified in the manifold's `normal`. + /// + /// Soft bodies: a soft surface's contacts with a rigid collider go through this hook like any + /// manifold; two soft surfaces' contacts arrive as candidates ([`ModifiableContacts::Soft`]) + /// whose direction and distance may be edited, or disabled; self contacts never reach the hook. fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {} } diff --git a/src/pipeline/physics_pipeline/mod.rs b/src/pipeline/physics_pipeline/mod.rs index 02f236455..a7632f21e 100644 --- a/src/pipeline/physics_pipeline/mod.rs +++ b/src/pipeline/physics_pipeline/mod.rs @@ -5,7 +5,7 @@ use crate::alloc_prelude::*; use crate::counters::Counters; use crate::dynamics::{ CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, - RigidBodySet, + RigidBodySet, SoftBodySet, }; use crate::geometry::{ BroadPhaseBvh, BroadPhasePairEvent, ColliderHandle, ColliderSet, ContactManifoldIndex, @@ -31,7 +31,7 @@ mod test_staged; /// resolving contacts so objects don't overlap, and updating positions and velocities. /// /// ## Performance note -/// This structure only contains temporary working memory (scratch buffers). You can create +/// This structure only contains temporary working memory (workspace buffers). You can create /// a new one anytime, but it's more efficient to reuse the same instance across frames /// since Rapier can reuse allocated memory. /// @@ -53,15 +53,15 @@ pub struct PhysicsPipeline { joint_selection_primed: bool, broad_phase_events: Vec, /// Colliders moved by the last `advance_to_final_positions` with their fresh broad-phase - /// AABBs, fed to the broad-phase refresh without the user-modification tracking. AABBs are + /// 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)>, /// Non-finite state detected and neutralized during the last step. quarantine: Quarantine, - /// Scratch buffer holding the active body handles (parallel body update). + /// Workspace buffer holding the active body handles (parallel body update). #[cfg(feature = "parallel")] active_body_handles: Vec, - /// Scratch: per-active-body sleep observations `(persistent island id, eligible)`, + /// Workspace: per-active-body sleep observations `(persistent island id, eligible)`, /// run-length compressed by the fused traversal, consumed by `IslandManager:: /// update_islands`'s whole-island sleep decision — which never re-touches the body arena. sleep_observations: Vec<(u32, bool)>, @@ -159,6 +159,7 @@ impl PhysicsPipeline { /// * `colliders` - The collision shapes attached to your bodies (boxes, spheres, meshes, etc.) /// * `impulse_joints` - Regular joints connecting bodies (hinges, sliders, etc.) /// * `multibody_joints` - Articulated joints for robot-like structures (optional, can be empty) + /// * `soft_bodies` - Deformable particle bodies (optional, can be empty) /// * `ccd_solver` - Continuous collision detection to prevent fast objects from tunneling through thin walls /// * `hooks` - Optional callbacks to customize collision filtering and contact modification /// * `events` - Optional handler to receive collision events (when objects start/stop touching) @@ -171,6 +172,7 @@ impl PhysicsPipeline { /// # let mut colliders = ColliderSet::new(); /// # let mut impulse_joints = ImpulseJointSet::new(); /// # let mut multibody_joints = MultibodyJointSet::new(); + /// # let mut soft_bodies = SoftBodySet::new(); /// # let mut islands = IslandManager::new(); /// # let mut broad_phase = BroadPhaseBvh::new(); /// # let mut narrow_phase = NarrowPhase::new(); @@ -188,6 +190,7 @@ impl PhysicsPipeline { /// &mut colliders, /// &mut impulse_joints, /// &mut multibody_joints, + /// &mut soft_bodies, /// &mut ccd_solver, /// &(), // No custom hooks /// &(), // No event handler @@ -204,6 +207,7 @@ impl PhysicsPipeline { colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, + soft_bodies: &mut SoftBodySet, ccd_solver: &mut CCDSolver, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, @@ -222,6 +226,7 @@ impl PhysicsPipeline { colliders, impulse_joints, multibody_joints, + soft_bodies, ccd_solver, hooks, events, @@ -239,6 +244,7 @@ impl PhysicsPipeline { colliders, impulse_joints, multibody_joints, + soft_bodies, ccd_solver, hooks, events, diff --git a/src/pipeline/physics_pipeline/quarantine.rs b/src/pipeline/physics_pipeline/quarantine.rs index 70edd5bf7..da85cb53e 100644 --- a/src/pipeline/physics_pipeline/quarantine.rs +++ b/src/pipeline/physics_pipeline/quarantine.rs @@ -6,28 +6,30 @@ //! one is known, and reported through [`Quarantine`]. use crate::alloc_prelude::*; -use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet, RigidBodyVelocity}; +use crate::dynamics::{ + RigidBody, RigidBodyHandle, RigidBodySet, RigidBodyVelocity, SoftBodyHandle, +}; use crate::geometry::{ColliderHandle, ColliderSet}; use crate::math::{Pose, Vector}; use crate::pipeline::PhysicsPipeline; -/// Non-finite (NaN or infinite) state detected and neutralized during the last step. -/// -/// Reported bodies got rolled back to their last valid pose (when known), their velocities and -/// user forces zeroed, and disabled; they can be re-enabled with `set_enabled(true)`. Reports -/// are cleared each step. +/// Non-finite (NaN or infinite) state detected and neutralized during the last step; reports are +/// cleared each step. Reported bodies are reset to their last valid pose (when known), zeroed +/// and disabled (`set_enabled(true)` re-enables them); soft bodies are zeroed and disabled. #[derive(Default)] pub struct Quarantine { /// Bodies disabled because their pose or velocity went non-finite. bodies: Vec, /// Colliders disabled because their own geometry went non-finite. colliders: Vec, + /// Soft bodies disabled because a particle's position or velocity went non-finite. + pub(super) soft_bodies: Vec, /// Bodies flagged by the end-of-step advance with their last valid pose; /// consumed by `apply_end_step`. - pub(super) body_scratch: Vec<(RigidBodyHandle, Pose)>, + pub(super) body_workspace: Vec<(RigidBodyHandle, Pose)>, /// Colliders flagged by the end-of-step advance with a non-finite AABB despite a finite /// body pose; consumed by `apply_end_step`. - pub(super) collider_scratch: Vec, + pub(super) collider_workspace: Vec, } impl Quarantine { @@ -41,15 +43,23 @@ impl Quarantine { &self.colliders } + /// The soft bodies quarantined during the last step: disabled, velocities zeroed, non-finite + /// particle positions left as is (fix them with `SoftBody::set_particle_position` before + /// `SoftBody::set_enabled`). + pub fn soft_bodies(&self) -> &[SoftBodyHandle] { + &self.soft_bodies + } + /// Was nothing quarantined during the last step? pub fn is_empty(&self) -> bool { - self.bodies.is_empty() && self.colliders.is_empty() + self.bodies.is_empty() && self.colliders.is_empty() && self.soft_bodies.is_empty() } /// Clears the reports at the beginning of a step. pub(super) fn clear(&mut self) { self.bodies.clear(); self.colliders.clear(); + self.soft_bodies.clear(); } /// Neutralizes every non-finite value found in `rb`'s velocities and user forces. @@ -133,12 +143,12 @@ impl Quarantine { bodies: &mut RigidBodySet, colliders: &mut ColliderSet, ) { - if self.body_scratch.is_empty() && self.collider_scratch.is_empty() { + if self.body_workspace.is_empty() && self.collider_workspace.is_empty() { return; } - let mut body_scratch = core::mem::take(&mut self.body_scratch); - for (handle, prev_pose) in body_scratch.drain(..) { + let mut body_workspace = core::mem::take(&mut self.body_workspace); + for (handle, prev_pose) in body_workspace.drain(..) { let Some(rb) = bodies.get_mut_internal_with_modification_tracking(handle) else { continue; }; @@ -157,10 +167,10 @@ impl Quarantine { self.bodies.push(handle); } } - self.body_scratch = body_scratch; + self.body_workspace = body_workspace; - let mut collider_scratch = core::mem::take(&mut self.collider_scratch); - for handle in collider_scratch.drain(..) { + let mut collider_workspace = core::mem::take(&mut self.collider_workspace); + for handle in collider_workspace.drain(..) { let Some(co) = colliders.get_mut_internal_with_modification_tracking(handle) else { continue; }; @@ -169,7 +179,7 @@ impl Quarantine { self.colliders.push(handle); } } - self.collider_scratch = collider_scratch; + self.collider_workspace = collider_workspace; } } diff --git a/src/pipeline/physics_pipeline/solve.rs b/src/pipeline/physics_pipeline/solve.rs index 228627e64..b90a18296 100644 --- a/src/pipeline/physics_pipeline/solve.rs +++ b/src/pipeline/physics_pipeline/solve.rs @@ -5,6 +5,7 @@ use crate::alloc_prelude::*; use crate::dynamics::{ ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, RigidBodySet, + SoftBodySet, }; use crate::geometry::{BroadPhaseBvh, ColliderHandle, ColliderSet, NarrowPhase}; use crate::math::{Real, Vector}; @@ -57,6 +58,7 @@ impl PhysicsPipeline { hooks: &dyn PhysicsHooks, events: &dyn EventHandler, handle_user_changes: bool, + soft_bodies: &SoftBodySet, ) { self.counters.stages.collision_detection_time.resume(); self.counters.cd.broad_phase_time.resume(); @@ -142,6 +144,7 @@ impl PhysicsPipeline { modified_colliders, hooks, events, + Some((soft_bodies, integration_parameters)), ); narrow_phase.compute_intersections( islands, @@ -166,6 +169,7 @@ impl PhysicsPipeline { colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, + soft_bodies: &mut SoftBodySet, events: &dyn EventHandler, ) { // Persistent islands, two tiers: a bounded local dual search settles each removal (proves @@ -178,6 +182,7 @@ impl PhysicsPipeline { narrow_phase, impulse_joints, multibody_joints, + soft_bodies, integration_parameters.length_unit, ); islands.persistent.run_pending_split(bodies); @@ -236,10 +241,11 @@ impl PhysicsPipeline { { let dt = integration_parameters.dt; let length_unit = integration_parameters.length_unit; + let soft_bodies: &SoftBodySet = soft_bodies; let observations = &mut self.sleep_observations; for handle in islands.active_bodies() { let rb = bodies.index_mut_internal(handle); - IslandManager::update_body_energy(rb, dt, length_unit); + IslandManager::update_body_energy(rb, soft_bodies, dt, length_unit); let effective_mass = rb.mprops.effective_mass(); rb.forces .compute_effective_force_and_torque(gravity, effective_mass); @@ -253,6 +259,7 @@ impl PhysicsPipeline { use rayon::prelude::*; let dt = integration_parameters.dt; let length_unit = integration_parameters.length_unit; + let soft_bodies: &SoftBodySet = soft_bodies; self.active_body_handles.clear(); self.active_body_handles.extend(islands.active_bodies()); let bodies_ptr = core::sync::atomic::AtomicPtr::new(bodies as *mut RigidBodySet); @@ -269,7 +276,7 @@ impl PhysicsPipeline { let mut observations = Vec::new(); for handle in chunk { let rb = bodies.index_mut_internal(*handle); - IslandManager::update_body_energy(rb, dt, length_unit); + IslandManager::update_body_energy(rb, soft_bodies, dt, length_unit); let effective_mass = rb.mprops.effective_mass(); rb.forces .compute_effective_force_and_torque(gravity, effective_mass); @@ -303,6 +310,7 @@ impl PhysicsPipeline { narrow_phase, impulse_joints, multibody_joints, + soft_bodies, &self.sleep_observations, ); @@ -313,8 +321,10 @@ impl PhysicsPipeline { any_extra_iterations, bodies, narrow_phase, + colliders, impulse_joints, multibody_joints, + soft_bodies, ); self.counters.stages.island_construction_time.pause(); @@ -350,7 +360,7 @@ impl PhysicsPipeline { .pause(); // NOTE: world-space mass-properties are NOT recomputed before the solver: they were - // refreshed by `advance_to_final_positions`, the user-changes handler, or multibody forward + // updated by `advance_to_final_positions`, the user-changes handler, or multibody forward // kinematics; effective forces by the fused traversal above. self.counters.stages.solver_time.resume(); @@ -375,6 +385,7 @@ impl PhysicsPipeline { num_threads, island_id, &mut self.counters, + gravity, integration_parameters, islands, bodies, @@ -384,8 +395,16 @@ impl PhysicsPipeline { &self.joint_constraint_indices, joint_assembly_epoch, multibody_joints, + soft_bodies, + narrow_phase, + colliders, unsafe { contact_color_masks.as_slice() }, ); + // The soft-body surface contacts live outside the solver contact graph: write their + // impulses back to the manifolds (contact-force events, warm start) here. + self.staged_solver + .soft_constraints + .writeback_contacts(narrow_phase); } // Generate contact force events if needed, and update each pair's diff --git a/src/pipeline/physics_pipeline/substep.rs b/src/pipeline/physics_pipeline/substep.rs index 4697be899..d1501c49c 100644 --- a/src/pipeline/physics_pipeline/substep.rs +++ b/src/pipeline/physics_pipeline/substep.rs @@ -5,7 +5,7 @@ use crate::alloc_prelude::*; use crate::dynamics::{ CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, - RigidBodyChanges, RigidBodySet, RigidBodyType, + RigidBodyChanges, RigidBodySet, RigidBodyType, SoftBodySet, }; #[cfg(feature = "parallel")] use crate::geometry::ColliderHandle; @@ -86,9 +86,9 @@ impl PhysicsPipeline { bodies: &mut RigidBodySet, colliders: &mut ColliderSet, ) { - // Set bodies to their final position, propagate to colliders, refresh world mass-properties + // Set bodies to their final position, propagate to colliders, update world mass-properties // (user code between steps applies forces w.r.t. the fresh CoM). NOTE: internal motion skips the - // user-modification tracking — moved colliders are harvested here for the broad-phase refresh and narrow-phase walk (no flags). + // user-modification tracking: moved colliders are harvested here for the broad-phase update and narrow-phase walk (no flags). use parry::bounding_volume::BoundingVolume; self.end_step_collider_aabbs.clear(); @@ -101,7 +101,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); + let mut aabb = co.compute_collision_aabb(prediction / 2.0 + rb.soft_motion_margin); if rb.soft_ccd_prediction() > 0.0 { let next_pose = rb.predict_position_using_velocity_and_forces_with_max_dist( dt, @@ -123,7 +123,7 @@ impl PhysicsPipeline { // Non-finite pose: leave the body at its last valid state for // `Quarantine::apply_end_step` to neutralize. if !rb.pos.next_position.is_finite() { - self.quarantine.body_scratch.push((handle, rb.pos.position)); + self.quarantine.body_workspace.push((handle, rb.pos.position)); continue; } rb.pos.position = rb.pos.next_position; @@ -138,7 +138,7 @@ impl PhysicsPipeline { } else { // Finite body pose but non-finite AABB: the collider's own // geometry is invalid. - self.quarantine.collider_scratch.push(*co_handle); + self.quarantine.collider_workspace.push(*co_handle); } } } @@ -212,10 +212,10 @@ impl PhysicsPipeline { for (chunk, quarantined_bodies, quarantined_colliders) in &moved { self.end_step_collider_aabbs.extend_from_slice(chunk); self.quarantine - .body_scratch + .body_workspace .extend_from_slice(quarantined_bodies); self.quarantine - .collider_scratch + .collider_workspace .extend_from_slice(quarantined_colliders); } } @@ -224,7 +224,7 @@ impl PhysicsPipeline { /// Feeds the broad-phase the AABBs computed by the last `advance_to_final_positions` /// call, through `set_aabb` (whose `pending_set_aabb` protocol makes the next /// broad-phase update account for them in change-flag resolution and stale-pair detection). - fn refresh_moved_collider_aabbs( + fn update_moved_collider_aabbs( &mut self, integration_parameters: &IntegrationParameters, broad_phase: &mut BroadPhaseBvh, @@ -273,6 +273,7 @@ impl PhysicsPipeline { colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, + soft_bodies: &mut SoftBodySet, ccd_solver: &mut CCDSolver, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, @@ -300,6 +301,11 @@ impl PhysicsPipeline { // Quarantine user-introduced non-finite state before it reaches the broad-phase. self.quarantine.detect_user_changes(bodies, colliders); + // Soft bodies whose settings changed must wake up for the change to take effect; the + // colliders of the ones whose particles were moved follow them (picked up as modified + // colliders below). + soft_bodies.apply_user_changes(bodies, colliders, &mut self.quarantine.soft_bodies); + // Apply modifications. let mut modified_colliders = colliders.take_modified(); let mut removed_colliders = colliders.take_removed(); @@ -363,10 +369,18 @@ impl PhysicsPipeline { } let mut mb_chain_events = core::mem::take(&mut multibody_joints.island_chain_events); for mb_id in &mb_chain_events { - islands.refresh_multibody_chain(bodies, multibody_joints, *mb_id); + islands.update_multibody_chain(bodies, multibody_joints, *mb_id); } mb_chain_events.clear(); multibody_joints.island_chain_events = mb_chain_events; + // Soft bodies inserted/removed or reattached since the last step: update their + // attachment links. + let mut sb_events = core::mem::take(&mut soft_bodies.island_events); + for event in &sb_events { + islands.update_soft_body_attachments(bodies, soft_bodies, event.handle); + } + sb_events.clear(); + soft_bodies.island_events = sb_events; self.counters.stages.user_changes.pause(); // TODO: do this only on user-change. @@ -378,6 +392,8 @@ impl PhysicsPipeline { .update_rigid_bodies_internal(bodies, true, false, false); } + // The soft meshes' vertex caches, read by the soft contact passes. + soft_bodies.refresh_vertex_caches(); self.detect_collisions( integration_parameters, islands, @@ -392,6 +408,7 @@ impl PhysicsPipeline { hooks, events, true, + soft_bodies, ); self.counters.stages.user_changes.resume(); @@ -486,6 +503,7 @@ impl PhysicsPipeline { colliders, impulse_joints, multibody_joints, + soft_bodies, events, ); @@ -549,6 +567,7 @@ impl PhysicsPipeline { hooks, events, false, + soft_bodies, ); self.clear_modified_colliders(colliders, &mut modified_colliders); @@ -559,7 +578,7 @@ impl PhysicsPipeline { // harvested by `advance_to_final_positions`. self.counters.stages.collision_detection_time.resume(); self.counters.cd.final_broad_phase_time.resume(); - self.refresh_moved_collider_aabbs(&integration_parameters, broad_phase); + self.update_moved_collider_aabbs(&integration_parameters, broad_phase); self.counters.cd.final_broad_phase_time.pause(); self.counters.stages.collision_detection_time.pause(); } @@ -571,12 +590,31 @@ impl PhysicsPipeline { // TODO: avoid updating the world mass properties twice (here, and // at the beginning of the next timestep) for bodies that were // not modified by the user in the mean time. - // NOTE: the world mass-properties of the bodies that moved were refreshed by + // NOTE: the world mass-properties of the bodies that moved were updated by // `advance_to_final_positions`. // Re-insert the modified vector we extracted for the borrow-checker. colliders.set_modified(modified_colliders); + // The tears the solver marked come first: the pieces they leave get their derived state + // synced below like any other body. + soft_bodies.apply_pending_tears( + islands, + bodies, + colliders, + impulse_joints, + multibody_joints, + events, + ); + // 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( + bodies, + colliders, + &integration_parameters, + &mut self.quarantine.soft_bodies, + ); + self.counters.step_completed(); } } diff --git a/src/pipeline/physics_pipeline/test.rs b/src/pipeline/physics_pipeline/test.rs index 9bddb00ea..622a606e6 100644 --- a/src/pipeline/physics_pipeline/test.rs +++ b/src/pipeline/physics_pipeline/test.rs @@ -9,13 +9,14 @@ use crate::geometry::{BroadPhaseBvh, ColliderBuilder, ColliderSet, NarrowPhase}; use crate::math::Rotation; use crate::math::Vector; use crate::pipeline::PhysicsPipeline; -use crate::prelude::{MultibodyJointSet, RevoluteJointBuilder, RigidBodyType}; +use crate::prelude::{MultibodyJointSet, RevoluteJointBuilder, RigidBodyType, SoftBodySet}; #[test] fn kinematic_and_fixed_contact_crash() { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -42,6 +43,7 @@ fn kinematic_and_fixed_contact_crash() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -53,6 +55,7 @@ fn rigid_body_removal_before_step() { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -97,6 +100,7 @@ fn rigid_body_removal_before_step() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -187,6 +191,7 @@ fn ccd_respects_filter_contact_pair_hook() { let mut ccd = CCDSolver::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut islands = IslandManager::new(); let hooks = RejectAllHooks { calls: AtomicUsize::new(0), @@ -232,6 +237,7 @@ fn ccd_respects_filter_contact_pair_hook() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &hooks, &event_handler, @@ -268,6 +274,7 @@ fn collider_removal_before_step() { let mut ccd = CCDSolver::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut islands = IslandManager::new(); let physics_hooks = (); let event_handler = (); @@ -297,6 +304,7 @@ fn collider_removal_before_step() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut ccd, &physics_hooks, &event_handler, @@ -309,6 +317,7 @@ fn rigid_body_type_changed_dynamic_is_in_active_set() { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -334,6 +343,7 @@ fn rigid_body_type_changed_dynamic_is_in_active_set() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -356,6 +366,7 @@ fn rigid_body_type_changed_dynamic_is_in_active_set() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -376,6 +387,7 @@ fn joint_step_delta_time_0() { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -416,6 +428,7 @@ fn joint_step_delta_time_0() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -464,6 +477,7 @@ fn test_multi_sap_disable_body() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); let mut multibody_joint_set = MultibodyJointSet::new(); + let mut soft_body_set = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let physics_hooks = (); let event_handler = (); @@ -478,6 +492,7 @@ fn test_multi_sap_disable_body() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -503,6 +518,7 @@ fn test_multi_sap_disable_body() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -528,6 +544,7 @@ fn test_multi_sap_disable_body() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -541,6 +558,7 @@ fn user_force_persists_across_steps() { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut pipeline = PhysicsPipeline::new(); let mut bf = BroadPhaseBvh::new(); let mut nf = NarrowPhase::new(); @@ -562,6 +580,7 @@ fn user_force_persists_across_steps() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -579,6 +598,7 @@ fn user_force_persists_across_steps() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -608,6 +628,7 @@ fn user_force_persists_across_steps() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, &mut CCDSolver::new(), &(), &(), @@ -646,6 +667,12 @@ fn contact_force_events_follow_runtime_active_events_flips() { ) { self.0.fetch_add(1, Ordering::Relaxed); } + fn handle_soft_body_tear_event( + &self, + _: &crate::dynamics::SoftBodySet, + _: &crate::dynamics::SoftBodyTearEvent, + ) { + } } #[cfg(feature = "dim2")] diff --git a/src/pipeline/physics_world.rs b/src/pipeline/physics_world.rs index 0164283ce..a9ce0778e 100644 --- a/src/pipeline/physics_world.rs +++ b/src/pipeline/physics_world.rs @@ -1,9 +1,6 @@ use crate::alloc_prelude::*; -use crate::dynamics::{ - CCDSolver, GenericJoint, ImpulseJoint, ImpulseJointHandle, ImpulseJointSet, - IntegrationParameters, IslandManager, Multibody, MultibodyJointHandle, MultibodyJointSet, - MultibodyLink, MultibodyLinkId, RigidBody, RigidBodyHandle, RigidBodySet, -}; +use crate::dynamics::{CCDSolver, GenericJoint, ImpulseJoint, ImpulseJointHandle, ImpulseJointSet, IntegrationParameters, IslandManager, Multibody, MultibodyJointHandle, MultibodyJointSet, MultibodyLink, MultibodyLinkId, RigidBody, RigidBodyHandle, RigidBodySet, SoftBindingError, SoftBody, SoftBodyBuilder, SoftBodyHandle, SoftBodySet, + SoftClusterRemoval, SoftMeshBinding}; use crate::geometry::{ BroadPhaseBvh, Collider, ColliderHandle, ColliderSet, ContactPair, DefaultBroadPhase, NarrowPhase, @@ -80,6 +77,8 @@ pub struct PhysicsWorld { pub impulse_joints: ImpulseJointSet, /// All multibody joints (kinematic chains, articulations). pub multibody_joints: MultibodyJointSet, + /// All soft bodies (deformable particle bodies). + pub soft_bodies: SoftBodySet, /// The continuous collision detection solver. /// /// Workspace only: not part of a snapshot (see the type docs). @@ -100,6 +99,7 @@ impl Default for PhysicsWorld { colliders: ColliderSet::new(), impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), + soft_bodies: SoftBodySet::new(), ccd_solver: CCDSolver::new(), } } @@ -130,7 +130,9 @@ impl PhysicsWorld { /// # use std::sync::mpsc::channel; /// let (collision_send, collision_recv) = channel(); /// let (contact_force_send, contact_force_recv) = channel(); - /// let event_handler = ChannelEventCollector::new(collision_send, contact_force_send); + /// let (soft_body_tear_send, soft_body_tear_recv) = channel(); + /// let event_handler = + /// ChannelEventCollector::new(collision_send, contact_force_send, soft_body_tear_send); /// /// world.step_with_events(&(), &event_handler); /// @@ -149,6 +151,7 @@ impl PhysicsWorld { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, &mut self.ccd_solver, hooks, events, @@ -383,6 +386,35 @@ impl PhysicsWorld { self.multibody_joints.attached_joints(body) } + // ── Soft bodies ───────────────────────────────────────────────────── + + /// Insert a soft body and return its handle. + /// + /// This creates the soft body's hidden root rigid body and its colliders (a deformable + /// surface, or one ball per surface particle); they are removed by + /// [`Self::remove_soft_body`]. + pub fn insert_soft_body(&mut self, soft_body: SoftBodyBuilder) -> SoftBodyHandle { + self.soft_bodies + .insert(soft_body, &mut self.bodies, &mut self.colliders) + } + + /// Remove a soft body, its root rigid body and its colliders. + pub fn remove_soft_body(&mut self, handle: SoftBodyHandle) -> Option { + self.soft_bodies.remove( + handle, + &mut self.islands, + &mut self.bodies, + &mut self.colliders, + &mut self.impulse_joints, + &mut self.multibody_joints, + ) + } + + /// Iterate over every soft body in the world. + pub fn soft_bodies(&self) -> impl Iterator { + self.soft_bodies.iter() + } + // ── Scene queries ─────────────────────────────────────────────────── /// Get a [`QueryPipeline`] for performing spatial queries (raycasts, shape casts, etc.). @@ -793,6 +825,7 @@ impl PhysicsWorld { &self.impulse_joints, &self.multibody_joints, &self.narrow_phase, + &self.soft_bodies, ); } } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 124c0d7cb..d752617bb 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -792,7 +792,7 @@ mod test { use super::QueryFilter; use crate::dynamics::{ CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, - RigidBodyBuilder, RigidBodySet, + RigidBodyBuilder, RigidBodySet, SoftBodySet, }; use crate::geometry::{BroadPhaseBvh, ColliderBuilder, ColliderSet, NarrowPhase}; use crate::math::{Real, Vector}; @@ -815,6 +815,7 @@ mod test { colliders, &mut ImpulseJointSet::new(), &mut MultibodyJointSet::new(), + &mut SoftBodySet::new(), &mut CCDSolver::new(), &(), &(), diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index ccc05ad36..6a7f7b723 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -95,10 +95,9 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( rb.colliders .update_positions(colliders, modified_colliders, &rb.pos.position); - // Refresh the world-space mass-properties. This is the only pre-solver - // refresh for user-moved (or newly inserted) bodies: the regular - // per-step refresh happens at the end of the step, right after pose - // integration. + // Update the world-space mass-properties: the only pre-solver update for + // user-moved (or newly inserted) bodies; the regular per-step update happens at + // the end of the step, right after pose integration. rb.mprops .update_world_mass_properties(rb.body_type, &rb.pos.position); } @@ -203,8 +202,8 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( if type_changed { // Persistent islands: a link recorded while an endpoint was // fixed doesn't connect (and vice versa), so a type change - // must refresh every joint link of this body. (Contact links - // are refreshed by the narrow-phase's modified-colliders pass; + // must update every joint link of this body. (Contact links + // are updated by the narrow-phase's modified-colliders pass; // the body's own island membership by `rigid_body_updated`.) let mut joint_island_events = Vec::new(); impulse_joints.map_attached_joints_mut(*handle, |rb1, rb2, joint_handle, joint| { diff --git a/src/utils/mod.rs b/src/utils/mod.rs index 54322a340..7e57e636b 100644 --- a/src/utils/mod.rs +++ b/src/utils/mod.rs @@ -79,7 +79,7 @@ pub fn try_normalize_and_get_length(v: Vector, threshold: Real) -> Option<(Vecto /// Forces a negative-zero float (or any negative-zero component) to positive zero via /// `x + 0.0`, leaving every other value untouched. /// -/// Serialized solver state must not carry `-0.0`: whether a min/max/clamp returns `+0.0` +/// Serialized solver state must not contain `-0.0`: whether a min/max/clamp returns `+0.0` /// or `-0.0` for a signed-zero tie is platform-specific (SSE returns one of the operands /// picked by argument order, NEON's `fminnm`/`fmaxnm` order the zeros), so a stored /// signed zero breaks cross-platform snapshot determinism even though `-0.0 == +0.0` diff --git a/src_testbed/debug_render.rs b/src_testbed/debug_render.rs index ae5aac18a..d3c79ec3b 100644 --- a/src_testbed/debug_render.rs +++ b/src_testbed/debug_render.rs @@ -1,5 +1,7 @@ #![allow(clippy::unnecessary_cast)] // Casts are needed for switching between f32/f64. +#[cfg(feature = "dim3")] +use kiss3d::renderer::Polyline3d; use kiss3d::window::Window; use rapier::math::Vector; use rapier::pipeline::{ @@ -17,16 +19,36 @@ impl Default for DebugRenderPipelineResource { Self { pipeline: DebugRenderPipeline::new( Default::default(), - !DebugRenderMode::RIGID_BODY_AXES & !DebugRenderMode::COLLIDER_AABBS, + !(DebugRenderMode::COLLIDER_AABBS | DebugRenderMode::PSEUDO_NORMALS), ), enabled: false, } } } +/// Slight depth bias to avoid z-fighting with other lines (like mesh geometry). +#[cfg(feature = "dim3")] +const SOFT_BODY_DEPTH_BIAS: f32 = 1.0e-6; + /// Kiss3d-based debug render backend pub struct Kiss3dLinesRenderBackend<'a> { pub window: &'a mut Window, + /// Reused from line to line: a depth-biased line goes through a polyline, and rebuilding one + /// per line would allocate thousands of times per frame. + #[cfg(feature = "dim3")] + biased: Polyline3d, +} + +impl<'a> Kiss3dLinesRenderBackend<'a> { + pub fn new(window: &'a mut Window) -> Self { + Self { + window, + #[cfg(feature = "dim3")] + biased: Polyline3d::new(vec![glamx::Vec3::ZERO; 2]) + .with_width(4.0) + .with_depth_bias(SOFT_BODY_DEPTH_BIAS), + } + } } impl<'a> DebugRenderBackend for Kiss3dLinesRenderBackend<'a> { @@ -43,16 +65,23 @@ impl<'a> DebugRenderBackend for Kiss3dLinesRenderBackend<'a> { } #[cfg(feature = "dim3")] - fn draw_line(&mut self, _: DebugRenderObject, a: Vector, b: Vector, color: DebugColor) { + fn draw_line(&mut self, object: DebugRenderObject, a: Vector, b: Vector, color: DebugColor) { // Convert HSLA to RGB let rgb = hsla_to_rgb(color[0], color[1], color[2], color[3]); - self.window.draw_line( - glamx::Vec3::new(a.x as f32, a.y as f32, a.z as f32), - glamx::Vec3::new(b.x as f32, b.y as f32, b.z as f32), - rgb.into(), - 4.0, - false, - ); + let a = glamx::Vec3::new(a.x as f32, a.y as f32, a.z as f32); + let b = glamx::Vec3::new(b.x as f32, b.y as f32, b.z as f32); + + // A soft body's cage is inside the surface the body is drawn as, so it is drawn in front + // of it instead of behind. Nothing else is biased: it would only hide what it overlaps. + if matches!(object, DebugRenderObject::SoftBody(..)) { + let Self { window, biased } = self; + biased.vertices[0] = a; + biased.vertices[1] = b; + biased.color = rgb.into(); + window.draw_polyline(biased); + } else { + self.window.draw_line(a, b, rgb.into(), 4.0, false); + } } } @@ -63,7 +92,7 @@ pub fn debug_render_scene( world: &PhysicsWorld, ) { if debug_render.enabled { - let mut backend = Kiss3dLinesRenderBackend { window }; + let mut backend = Kiss3dLinesRenderBackend::new(window); debug_render.pipeline.render( &mut backend, &world.bodies, @@ -71,6 +100,7 @@ pub fn debug_render_scene( &world.impulse_joints, &world.multibody_joints, &world.narrow_phase, + &world.soft_bodies, ); } } diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs index 3715a183f..b14ed7deb 100644 --- a/src_testbed/graphics.rs +++ b/src_testbed/graphics.rs @@ -2,11 +2,23 @@ use crate::testbed::TestbedStateFlags; use kiss3d::prelude::*; -use rapier::dynamics::{RigidBodyHandle, RigidBodySet}; +use rapier::dynamics::{RigidBodyHandle, RigidBodySet, SoftBodySet}; use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType, SharedShape}; use std::collections::HashMap; use std::path::Path; +use graphics_polyline::polyline_geometry; +#[cfg(feature = "dim2")] +use graphics_polyline::stroked_polyline; +#[cfg(feature = "dim3")] +use graphics_polyline::tube_polyline; +use graphics_soft_bodies::SoftBodyGraphics; +#[cfg(feature = "dim3")] +use graphics_soft_bodies::is_hidden_by_skin; + +mod graphics_polyline; +mod graphics_soft_bodies; + #[cfg(feature = "dim2")] pub use kiss3d::prelude::SceneNode2d as SceneNode; #[cfg(feature = "dim3")] @@ -167,6 +179,8 @@ pub struct GraphicsManager { individual_nodes: Vec, /// Body-attached render-only meshes (no physics counterpart). body_attached_nodes: Vec, + /// Soft-body colors and the render nodes of the meshes soft bodies only draw. + soft_graphics: SoftBodyGraphics, /// Per-channel visibility — `false` hides every collider-derived node /// (instanced + individual). Composed with the global `DRAW_SURFACES` /// flag, which still wins when off. @@ -174,11 +188,14 @@ pub struct GraphicsManager { /// Per-channel visibility for body-attached render-only meshes /// (e.g. MJCF visual meshes added via [`Self::add_body_render_mesh`]). body_render_meshes_visible: bool, - /// Last-known state of the global `DRAW_SURFACES` flag, refreshed on every + /// Last-known state of the global `DRAW_SURFACES` flag, updated on every /// [`Self::draw`]. Nodes created between two `draw` calls (e.g. during a /// scene reset) are spawned with this visibility so they don't flash for /// one frame when surface rendering is disabled (see #843). draw_surfaces: bool, + /// Mesh colliders drawn with shared vertices (smooth normals) instead of flat shading + /// (3D only: a 2D mesh collider is a polyline). + smooth_mesh_colliders: bool, /// Map from collider to its render nodes c2nodes: HashMap, /// Colliders attached to a particular body, used to identify the @@ -251,6 +268,7 @@ impl GraphicsManager { colliders_visible: true, body_render_meshes_visible: true, draw_surfaces: true, + smooth_mesh_colliders: false, c2nodes: HashMap::new(), b2colliders: HashMap::new(), b2color: HashMap::new(), @@ -264,9 +282,12 @@ impl GraphicsManager { pub fn clear(&mut self) { self.scene = SceneNode::empty(); + self.curr_color_index = 0; #[cfg(feature = "dim3")] { - // Setup lights. + // A key light off the (1, 1, 1) diagonal, so the three faces of a box seen from the + // usual eye get three different shades, and a weak fill from the other side so the + // faces it misses are not flat ambient. let mut light = self .scene .add_light(Light::directional(Vec3::new(-1.0, -1.0, -1.0))); @@ -336,7 +357,7 @@ impl GraphicsManager { if matches!(shape.shape_type(), ShapeType::TriMesh) { // Build the kiss3d mesh ourselves so we can plumb the // authored UVs *and* normals straight through. The standard - // `create_individual_node` path carries neither and always + // `create_individual_node` path handles neither and always // recomputes flat per-face normals — for a visual mesh // that replaces the default collider render we instead want // to match the source asset exactly. @@ -381,7 +402,7 @@ impl GraphicsManager { ); if !have_normals { // No usable authored normals (e.g. an STL/OBJ that - // carried none): fall back to flat shading so the mesh + // had none): fall back to flat shading so the mesh // still lights. This splits shared vertices, so it must // run *only* when we didn't supply normals above — // never on the smooth path. @@ -450,6 +471,8 @@ impl GraphicsManager { }); } + } + pub fn scene(&self) -> &SceneNode { &self.scene } @@ -1139,6 +1162,7 @@ impl GraphicsManager { colliders: &ColliderSet, ) { self.draw_surfaces = flags.contains(TestbedStateFlags::DRAW_SURFACES); + self.smooth_mesh_colliders = flags.contains(TestbedStateFlags::SMOOTH_MESH_COLLIDERS); let show_colliders = self.draw_surfaces && self.colliders_visible; let show_body_meshes = self.draw_surfaces && self.body_render_meshes_visible; diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs index a47622f53..3db7ee6f5 100644 --- a/src_testbed/physics/mod.rs +++ b/src_testbed/physics/mod.rs @@ -45,6 +45,7 @@ pub fn restore_world(world: &mut PhysicsWorld, snapshot: &PhysicsSnapshot) -> us colliders, impulse_joints, multibody_joints, + soft_bodies, ccd_solver: _, } = restored; world.gravity = gravity; @@ -56,6 +57,7 @@ pub fn restore_world(world: &mut PhysicsWorld, snapshot: &PhysicsSnapshot) -> us world.colliders = colliders; world.impulse_joints = impulse_joints; world.multibody_joints = multibody_joints; + world.soft_bodies = soft_bodies; timestep_id } diff --git a/src_testbed/settings.rs b/src_testbed/settings.rs index 7801a2782..b7154bc1d 100644 --- a/src_testbed/settings.rs +++ b/src_testbed/settings.rs @@ -77,6 +77,14 @@ impl ExampleSettings { &self.non_restart_keys } + /// Removes a setting, so an example can drop the entries that stopped being relevant + /// (a parameter of a mode that is no longer selected). The remaining entries keep + /// their order. + pub fn remove(&mut self, key: &str) { + let _ = self.values.shift_remove(key); + self.non_restart_keys.remove(key); + } + pub fn len(&self) -> usize { self.values.len() } diff --git a/src_testbed/testbed/state.rs b/src_testbed/testbed/state.rs index 0230f43cb..7d3e6da74 100644 --- a/src_testbed/testbed/state.rs +++ b/src_testbed/testbed/state.rs @@ -16,7 +16,7 @@ pub enum RunMode { } /// A loop transition requested from the UI: stop entirely, or switch to another -/// example (or re-run the current one). The target is carried in +/// example (or re-run the current one). The target is stored in /// [`TestbedState::selected_display_index`]; this only signals the /// example-owned `while viewer.render_frame()` loop to exit so the outer demo /// runner can dispatch the next example. @@ -41,6 +41,9 @@ bitflags! { const WIREFRAME = 1 << 8; const STATISTICS = 1 << 9; const DRAW_SURFACES = 1 << 10; + /// Soft-body surfaces rendered with shared vertices (smooth per-vertex normals) + /// instead of flat-shaded triangles (3D). + const SMOOTH_MESH_COLLIDERS = 1 << 11; } } @@ -113,7 +116,7 @@ pub struct TestbedState { pub broad_phase_type: RapierBroadPhaseType, pub snapshot: Option, /// Number of physics steps run since the example was (re)started. Bumped by - /// [`crate::TestbedViewer::simulating`], and carried through snapshot + /// [`crate::TestbedViewer::simulating`], and kept through snapshot /// save/restore so a restored world reports the step it was saved at. pub timestep_id: usize, pub camera_locked: bool, diff --git a/src_testbed/viewer.rs b/src_testbed/viewer.rs index 7b71c6672..f1db6c5df 100644 --- a/src_testbed/viewer.rs +++ b/src_testbed/viewer.rs @@ -27,7 +27,7 @@ use kiss3d::color::Color; use kiss3d::event::{Action, Key, WindowEvent}; use kiss3d::window::Window; -use rapier::dynamics::{RigidBodyActivation, RigidBodyHandle}; +use rapier::dynamics::{RigidBodyActivation, RigidBodyHandle, SoftBodyHandle}; use rapier::geometry::{ColliderHandle, SharedShape}; use rapier::pipeline::PhysicsWorld; @@ -158,8 +158,8 @@ impl TestbedViewer { self.autosave(); highlight_hovered_body(&mut self.graphics, &self.scene_mouse, world); - self.graphics - .draw(self.state.flags, &world.bodies, &world.colliders); + self.graphics.draw( + ); debug_render_scene(&mut self.window, &mut self.debug_render, world); // Snapshot flags before the UI runs (`draw_ui` mutates `state.flags`) so next frame's @@ -290,7 +290,8 @@ impl TestbedViewer { } } - /// Camera orientation as a unit quaternion (3D only). + /// Camera orientation as a unit quaternion (3D only). OpenGL convention: the camera looks + /// down its local -z, so `rot * z` points from the scene back toward the eye. #[cfg(feature = "dim3")] pub fn camera_rotation(&self) -> na::UnitQuaternion { let rot_x = na::UnitQuaternion::from_axis_angle(&na::Vector3::y_axis(), self.camera.at().x); @@ -299,7 +300,7 @@ impl TestbedViewer { rot_x * rot_y } - /// Camera forward direction (3D only). + /// Camera forward direction, from the eye toward the look-at point (3D only). #[cfg(feature = "dim3")] pub fn camera_fwd_dir(&self) -> na::Vector3 { self.camera_rotation() * na::Vector3::z() @@ -323,6 +324,10 @@ impl TestbedViewer { self.graphics.set_initial_collider_color(collider, color); } + pub fn set_initial_soft_body_color(&mut self, soft_body: SoftBodyHandle, color: Color) { + self.graphics.set_initial_soft_body_color(soft_body, color); + } + pub fn set_body_wireframe(&mut self, body: RigidBodyHandle, wireframe_enabled: bool) { self.graphics.set_body_wireframe(body, wireframe_enabled); } @@ -394,7 +399,7 @@ impl TestbedViewer { self.graphics.remove_collider_nodes(handle); } - /// Refreshes a collider's render nodes after its shape was modified in place. + /// Updates a collider's render nodes after its shape was modified in place. pub fn update_collider(&mut self, handle: ColliderHandle, world: &PhysicsWorld) { self.graphics.remove_collider_nodes(handle); self.graphics @@ -444,7 +449,7 @@ impl TestbedViewer { // ───────────────────────────── registry / loop ────────────────────────── /// Display index of the example the UI currently has selected, clamped to a - /// valid range (a stale autosave may carry an index past the current list). + /// valid range (a stale autosave may hold an index past the current list). pub fn selected(&self) -> usize { self.state .selected_display_index @@ -557,6 +562,14 @@ impl TestbedViewer { self.state .action_flags .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, false); + // Soft bodies: one color per soft body, worn by every one of its cluster proxies so + // its collision meshes (colliders like any other) come out in that color. + for (sb_handle, sb) in world.soft_bodies.iter() { + let color = self.graphics.soft_body_color(sb_handle); + for (_, cluster) in sb.live_clusters() { + self.graphics.set_initial_body_color(cluster.proxy(), color); + } + } for (handle, _) in world.bodies.iter() { self.graphics.add_body_colliders( &mut self.window, From 67389b37342d71c5a76550e19cc5e1b9f1cf77cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 28 Aug 2026 17:41:29 +0200 Subject: [PATCH 02/32] feat(soft-body): add the 2D and 3D demos and integration tests --- crates/rapier2d/tests/soft_bodies.rs | 1374 ++++++++++++ crates/rapier3d/tests/soft_bodies.rs | 2870 ++++++++++++++++++++++++++ examples2d/all_examples2.rs | 23 + examples2d/soft_blobs2.rs | 66 + examples2d/soft_bodies2.rs | 107 + examples2d/soft_jelly2.rs | 88 + examples2d/soft_letters2.rs | 72 + examples2d/soft_pile2.rs | 115 ++ examples2d/soft_plasticity2.rs | 144 ++ examples2d/soft_surface2.rs | 129 ++ examples2d/soft_tearing2.rs | 116 ++ examples2d/soft_thin_features2.rs | 130 ++ examples2d/utils/svg.rs | 78 + examples3d/all_examples3.rs | 26 + examples3d/soft_bodies3.rs | 103 + examples3d/soft_cloth3.rs | 75 + examples3d/soft_cloth_stress3.rs | 128 ++ examples3d/soft_dress3.rs | 238 +++ examples3d/soft_jelly3.rs | 143 ++ examples3d/soft_pile3.rs | 106 + examples3d/soft_plasticity3.rs | 154 ++ examples3d/soft_surface3.rs | 158 ++ examples3d/soft_tearing3.rs | 134 ++ examples3d/soft_thin_features3.rs | 170 ++ examples3d/soft_trimesh3.rs | 475 +++++ 25 files changed, 7222 insertions(+) create mode 100644 crates/rapier2d/tests/soft_bodies.rs create mode 100644 crates/rapier3d/tests/soft_bodies.rs create mode 100644 examples2d/soft_blobs2.rs create mode 100644 examples2d/soft_bodies2.rs create mode 100644 examples2d/soft_jelly2.rs create mode 100644 examples2d/soft_letters2.rs create mode 100644 examples2d/soft_pile2.rs create mode 100644 examples2d/soft_plasticity2.rs create mode 100644 examples2d/soft_surface2.rs create mode 100644 examples2d/soft_tearing2.rs create mode 100644 examples2d/soft_thin_features2.rs create mode 100644 examples3d/soft_bodies3.rs create mode 100644 examples3d/soft_cloth3.rs create mode 100644 examples3d/soft_cloth_stress3.rs create mode 100644 examples3d/soft_dress3.rs create mode 100644 examples3d/soft_jelly3.rs create mode 100644 examples3d/soft_pile3.rs create mode 100644 examples3d/soft_plasticity3.rs create mode 100644 examples3d/soft_surface3.rs create mode 100644 examples3d/soft_tearing3.rs create mode 100644 examples3d/soft_thin_features3.rs create mode 100644 examples3d/soft_trimesh3.rs diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs new file mode 100644 index 000000000..eb0fbeb6d --- /dev/null +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -0,0 +1,1374 @@ +//! Soft-body regression tests (2D): substep invariance, ropes, blobs (area preservation), +//! triangulated bodies, shape matching, two-way coupling, sleeping and removal. + +use rapier2d::prelude::*; + +fn world_with_ground() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(50.0, 0.5).translation(Vector::new(0.0, -0.5)), + ); + world +} + +fn assert_finite(world: &PhysicsWorld, handle: SoftBodyHandle) { + for p in world.soft_bodies[handle].particles() { + assert!(p.position().is_finite(), "non-finite particle position"); + } +} + +/// The period of a mass-spring pair set by its natural frequency does not depend on the +/// substep count. +#[test] +fn edge_period_is_substep_invariant() { + let natural_frequency = 2.0; + let mut periods = Vec::new(); + for substeps in [1usize, 4, 16] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.num_solver_iterations = substeps; + world.integration_parameters.dt = 1.0 / 240.0; + let builder = SoftBodyBuilder::new(vec![Vector::ZERO, Vector::new(1.0, 0.0)]) + .edges(vec![[0, 1]]) + .pinned_particles([0]) + .softness(SpringCoefficients::new(natural_frequency, 0.0)) + .no_surface_collider() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + world.soft_bodies[handle].set_particle_position(1, Vector::new(1.2, 0.0)); + let mut crossings = Vec::new(); + let mut prev_len = 1.2; + for step in 0..2000 { + world.step(); + let len = world.soft_bodies[handle].particle_position(1).x; + if prev_len > 1.0 && len <= 1.0 { + crossings.push(step as Real * world.integration_parameters.dt); + if crossings.len() == 2 { + break; + } + } + prev_len = len; + } + assert_eq!( + crossings.len(), + 2, + "no oscillation with {substeps} substeps" + ); + periods.push(crossings[1] - crossings[0]); + } + let expected = 1.0 / natural_frequency; + for period in &periods { + assert!( + (period - expected).abs() < 0.08 * expected, + "period {period} vs {expected}" + ); + } +} + +/// A rope pinned at one end hangs under gravity. +#[test] +fn rope_hangs_from_anchor() { + let mut world = PhysicsWorld::new(); + let rope = SoftBodyBuilder::rope(Vector::ZERO, Vector::new(2.0, 0.0), 21) + .pinned_particles([0]) + .softness(SpringCoefficients::new(60.0, 1.0)) + .linear_damping(1.0); + let handle = world.insert_soft_body(rope); + for _ in 0..600 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let end = sb.particle_position(20); + assert!(end.y < -1.9, "rope end did not fall: {end:?}"); + assert!( + end.x.abs() < 0.2, + "rope end did not settle under the anchor: {end:?}" + ); +} + +/// A disk with area preservation dropped on the ground keeps its area; inflating it grows it. +#[test] +fn disk_keeps_its_area() { + let mut world = world_with_ground(); + let disk = SoftBodyBuilder::disk(Vector::new(0.0, 1.5), 1.0, 32) + .softness(SpringCoefficients::new(20.0, 1.0)); + let handle = world.insert_soft_body(disk); + let rest = world.soft_bodies[handle].rest_volume(); + assert!(rest > 3.0, "disk area too small: {rest}"); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let area = world.soft_bodies[handle].volume(); + assert!( + (area - rest).abs() < 0.15 * rest, + "area drifted: {area} vs {rest}" + ); + let min_y = world.soft_bodies[handle] + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + assert!(min_y > -0.05 && min_y < 0.3, "disk sank or floats: {min_y}"); + + world.soft_bodies[handle].set_volume_factor(1.5); + for _ in 0..300 { + world.step(); + } + let inflated = world.soft_bodies[handle].volume(); + assert!( + inflated > 1.25 * rest, + "disk did not inflate: {inflated} vs {rest}" + ); +} + +/// A triangulated square (both cell models) dropped on the ground settles without inverting +/// any cell. +#[test] +fn grid_settles() { + for model in [SoftBodyCellModel::Volume, SoftBodyCellModel::Corotational] { + let mut world = world_with_ground(); + let grid = SoftBodyBuilder::grid(Vector::new(0.0, 1.0), Vector::splat(0.5), 5, 5) + .cell_model(model) + .softness(SpringCoefficients::new(30.0, 1.0)); + let handle = world.insert_soft_body(grid); + let rest = world.soft_bodies[handle].rest_volume(); + assert!((rest - 1.0).abs() < 1.0e-3, "square rest area {rest}"); + for _ in 0..400 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let area = sb.volume(); + assert!( + (area - rest).abs() < 0.2 * rest, + "{model:?}: area drifted: {area} vs {rest}" + ); + for c in sb.cells() { + let x: [Vector; 3] = + core::array::from_fn(|k| sb.particle_position(c.vertices[k] as usize)); + let a = (x[1] - x[0]).perp_dot(x[2] - x[0]) * 0.5; + assert!(a > 0.0, "{model:?}: inverted cell (area {a})"); + } + } +} + +/// A very stiff corotational square (E = 1e8, 0.1 kg particles), squeezed by 20% and dropped +/// spinning, settles and sleeps; a free-floating one keeps its angular momentum. +#[test] +fn stiff_elastic_square_is_stable() { + let square = |young: Real| { + SoftBodyBuilder::grid(Vector::new(0.0, 3.0), Vector::splat(0.75), 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + }; + let spin = |world: &mut PhysicsWorld, handle: SoftBodyHandle, squeeze: Real| { + let sb = &mut world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + let mut t = sb.particle_position(i); + t.y = 3.0 + (t.y - 3.0) * squeeze; + sb.set_particle_position(i, t); + let arm = t - Vector::new(0.0, 3.0); + sb.set_particle_velocity(i, Vector::new(-arm.y, arm.x) * 5.0); + } + }; + for young in [3.0e4, 3.0e6, 1.0e8] { + let mut world = world_with_ground(); + let handle = world.insert_soft_body(square(young)); + spin(&mut world, handle, 0.8); + for _ in 0..500 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let max_vel = sb + .particle_velocities() + .map(|v| v.length()) + .fold(0.0, Real::max); + assert!( + max_vel < 0.05, + "E = {young}: square still moving at {max_vel} m/s" + ); + assert!(sb.is_sleeping(), "E = {young}: square did not fall asleep"); + let area = sb.volume(); + assert!( + (area - sb.rest_volume()).abs() < 0.05 * sb.rest_volume(), + "E = {young}: area {area} vs rest {}", + sb.rest_volume() + ); + } + + let angular_momentum = |world: &PhysicsWorld, handle: SoftBodyHandle| { + let sb = &world.soft_bodies[handle]; + let com = sb.center_of_mass(); + sb.particles() + .iter() + .map(|p| (p.position() - com).perp_dot(p.velocity() * p.mass())) + .sum::() + }; + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let handle = world.insert_soft_body(square(3.0e6)); + spin(&mut world, handle, 1.0); + world.step(); + let l0 = angular_momentum(&world, handle); + for _ in 0..600 { + world.step(); + } + let l1 = angular_momentum(&world, handle); + assert!( + (l1 - l0).abs() < 0.01 * l0.abs(), + "angular momentum drifted: {l0} -> {l1}" + ); +} + +/// The same squeezed, spinning Neo-Hookean square dropped at E = 1e4, 1e6 and 1e8 settles, +/// sleeps and keeps its area. +#[test] +fn stiff_neo_hookean_square_is_stable() { + for young in [1.0e4, 1.0e6, 1.0e8] { + let mut world = world_with_ground(); + let square = SoftBodyBuilder::grid(Vector::new(0.0, 3.0), Vector::splat(0.75), 5, 5) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1); + let handle = world.insert_soft_body(square); + { + let sb = &mut world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + let mut t = sb.particle_position(i); + t.y = 3.0 + (t.y - 3.0) * 0.8; + sb.set_particle_position(i, t); + let arm = t - Vector::new(0.0, 3.0); + sb.set_particle_velocity(i, Vector::new(-arm.y, arm.x) * 5.0); + } + } + for _ in 0..500 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let max_vel = sb + .particle_velocities() + .map(|v| v.length()) + .fold(0.0, Real::max); + assert!( + max_vel < 0.05, + "E = {young}: square still moving at {max_vel} m/s" + ); + assert!(sb.is_sleeping(), "E = {young}: square did not fall asleep"); + let area = sb.volume(); + assert!( + (area - sb.rest_volume()).abs() < 0.05 * sb.rest_volume(), + "E = {young}: area {area} vs rest {}", + sb.rest_volume() + ); + } +} + +/// A soft Neo-Hookean square squashed to 30% of its height by a kinematic plate keeps its cells +/// positively oriented (up to a couple of transient flips under the plate while held) and +/// recovers its area within 5% once the plate lifts, with no inverted cell left. +#[test] +fn neo_hookean_square_survives_large_compression() { + let mut world = world_with_ground(); + let square = SoftBodyBuilder::grid(Vector::new(0.0, 0.5), Vector::splat(0.5), 5, 5) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: 1.0e4, + poisson_ratio: 0.45, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.03) + .surface_collider(ColliderBuilder::ball(0.03).friction(0.5)); + let handle = world.insert_soft_body(square); + // Plate bottom at 1.2 when lifted, 0.3 when down. + let plate_rest = Vector::new(0.0, 1.7); + let (plate, _) = world.insert( + RigidBodyBuilder::kinematic_position_based().translation(plate_rest), + ColliderBuilder::cuboid(2.0, 0.5), + ); + let inverted = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[handle]; + sb.cells() + .iter() + .filter(|c| { + let x: [Vector; 3] = + core::array::from_fn(|k| sb.particle_position(c.vertices[k] as usize)); + (x[1] - x[0]).perp_dot(x[2] - x[0]) < 0.0 + }) + .count() + }; + let dt = world.integration_parameters.dt; + let mut t: Real = 0.0; + let mut min_height = Real::MAX; + for _ in 0..(7.0 / dt) as usize { + t += dt; + let depth = if t < 2.0 { + t / 2.0 + } else if t < 3.0 { + 1.0 + } else if t < 5.0 { + (5.0 - t) / 2.0 + } else { + 0.0 + }; + world.bodies[plate] + .set_next_kinematic_translation(plate_rest - Vector::new(0.0, 0.9 * depth)); + world.step(); + assert_finite(&world, handle); + if (2.0..3.0).contains(&t) { + let sb = &world.soft_bodies[handle]; + let top = sb.particle_positions().map(|p| p.y).fold(0.0, Real::max); + min_height = min_height.min(top); + let inverted = inverted(&world); + assert!( + inverted <= 2, + "{inverted} inverted cells while squashed (t = {t})" + ); + } + } + assert!( + min_height < 0.35, + "the plate did not squash the square: {min_height}" + ); + assert_eq!(inverted(&world), 0, "inverted cells after release"); + let sb = &world.soft_bodies[handle]; + let area = sb.volume(); + assert!( + (area - sb.rest_volume()).abs() < 0.05 * sb.rest_volume(), + "area {area} vs rest {}", + sb.rest_volume() + ); +} + +/// The Neo-Hookean material must not depend on the substep count either: the equilibrium +/// compression of a soft column standing on its pinned base under its own weight, and its +/// small-oscillation period, are the same at 1, 4 and 16 substeps. +#[test] +fn neo_hookean_response_is_substep_invariant() { + let mut compressions = Vec::new(); + let mut periods = Vec::new(); + for substeps in [1usize, 4, 16] { + let mut world = PhysicsWorld::new(); + world.integration_parameters.num_solver_iterations = substeps; + let mut column = SoftBodyBuilder::grid(Vector::new(0.0, 0.5), Vector::splat(0.5), 3, 3) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: 5.0e2, + poisson_ratio: 0.3, + elastic_damping_ratio: 0.05, + ..Default::default() + }) + .particle_mass(1.0) + .can_sleep(false) + .no_surface_collider(); + let base: Vec = (0..column.positions.len() as u32) + .filter(|i| column.positions[*i as usize].y < 1.0e-3) + .collect(); + column = column.pinned_particles(base); + let handle = world.insert_soft_body(column); + let top: Vec = world.soft_bodies[handle] + .particle_positions() + .enumerate() + .filter(|(_, p)| p.y > 0.999) + .map(|(i, _)| i) + .collect(); + let height = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[handle]; + top.iter().map(|i| sb.particle_position(*i).y).sum::() / top.len() as Real + }; + for _ in 0..600 { + world.step(); + } + let equilibrium = height(&world); + compressions.push(1.0 - equilibrium); + for &i in &top { + world.soft_bodies[handle].set_particle_velocity(i, Vector::new(0.0, 0.3)); + } + let mut crossings = Vec::new(); + let mut prev = height(&world); + for step in 0..1000 { + world.step(); + let y = height(&world); + if prev > equilibrium && y <= equilibrium { + crossings.push(step as Real * world.integration_parameters.dt); + if crossings.len() == 3 { + break; + } + } + prev = y; + } + assert_eq!( + crossings.len(), + 3, + "no oscillation with {substeps} substeps" + ); + periods.push((crossings[2] - crossings[0]) / 2.0); + } + let spread = |v: &[Real]| { + let (min, max) = v + .iter() + .fold((Real::MAX, Real::MIN), |(a, b), x| (a.min(*x), b.max(*x))); + (max - min) / max + }; + assert!( + compressions.iter().all(|c| *c > 0.05), + "the column barely compresses: {compressions:?}" + ); + assert!( + spread(&compressions) < 0.08, + "the equilibrium compression depends on the substep count: {compressions:?}" + ); + assert!( + spread(&periods) < 0.08, + "the oscillation period depends on the substep count: {periods:?}" + ); +} + +/// A closed surface expels bodies that cross it: a small rigid ball spawned straddling a soft +/// square's edge from inside (and a particle of a second soft body likewise) come out instead +/// of being trapped, and the square itself stays whole. +#[test] +fn closed_surface_expels_intruders() { + let mut world = world_with_ground(); + world.gravity = Vector::ZERO; + let square = SoftBodyBuilder::grid(Vector::new(0.0, 3.0), Vector::splat(0.75), 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.05); + let square = world.insert_soft_body(square); + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.1, 3.68)), + ColliderBuilder::ball(0.1), + ); + let intruder = SoftBodyBuilder::new(vec![ + Vector::new(0.72, 3.1), + Vector::new(1.2, 3.0), + Vector::new(1.0, 3.5), + ]) + .cells(vec![[0, 1, 2]]) + .particle_radius(0.05) + .particle_mass(0.1); + let intruder = world.insert_soft_body(intruder); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, square); + let sb = &world.soft_bodies[square]; + let inside = |p: Vector| { + let com = sb.center_of_mass(); + (p - com).abs().max_element() < 0.75 - 0.05 + }; + let ball_pos = world.bodies[ball].translation(); + assert!( + !inside(ball_pos), + "rigid ball still inside the square: {ball_pos:?}" + ); + for p in world.soft_bodies[intruder].particles() { + assert!( + !inside(p.position()), + "intruder particle still inside: {:?}", + p.position() + ); + } + let area = sb.volume(); + assert!( + (area - sb.rest_volume()).abs() < 0.1 * sb.rest_volume(), + "square area {area} vs rest {}", + sb.rest_volume() + ); +} + +/// A shape-matched cloud returns to its rest shape after being scrambled. +#[test] +fn shape_matching_recovers_rest_shape() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let mut positions = Vec::new(); + for i in 0..4 { + for j in 0..4 { + positions.push(Vector::new(i as Real, j as Real) * 0.5); + } + } + let builder = SoftBodyBuilder::new(positions.clone()) + .shape_matching(true) + .softness(SpringCoefficients::new(10.0, 1.0)) + .no_surface_collider() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + { + let sb = &mut world.soft_bodies[handle]; + for (i, p) in positions.iter().enumerate() { + let x = ((i * 7919) % 100) as Real / 100.0 - 0.5; + let y = ((i * 104729) % 100) as Real / 100.0 - 0.5; + sb.set_particle_position(i, *p + Vector::new(x, y) * 0.6); + } + } + for _ in 0..600 { + world.step(); + } + let sb = &world.soft_bodies[handle]; + let mut max_err: Real = 0.0; + for a in 0..positions.len() { + for b in a + 1..positions.len() { + let rest = (positions[a] - positions[b]).length(); + let cur = (sb.particle_position(a) - sb.particle_position(b)).length(); + max_err = max_err.max((rest - cur).abs()); + } + } + assert!( + max_err < 0.05, + "shape not recovered, max distance error {max_err}" + ); +} + +/// Two-way coupling: a light box (0.36 kg) dropped just above a heavy damped ring blob (40 kg) +/// rests on its contact skin, not inside it, and squashes it. +#[test] +fn rigid_box_rests_on_blob() { + let mut world = world_with_ground(); + let blob = SoftBodyBuilder::disk(Vector::new(0.0, 1.0), 1.0, 40) + .softness(SpringCoefficients::new(30.0, 1.0)) + .linear_damping(0.5); + let handle = world.insert_soft_body(blob); + let skin = world.soft_bodies[handle].particle_radius(); + let (rb, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 2.4)), + ColliderBuilder::cuboid(0.3, 0.3), + ); + for _ in 0..600 { + world.step(); + } + assert_finite(&world, handle); + let box_y = world.bodies[rb].translation().y; + let sb = &world.soft_bodies[handle]; + let max_y = sb + .particle_positions() + .map(|p| p.y) + .fold(Real::MIN, Real::max); + assert!(max_y < 2.0, "blob was not squashed: {max_y}"); + let box_bottom = box_y - 0.3; + assert!( + (box_bottom - (max_y + skin)).abs() < 0.02, + "box bottom {box_bottom} is not resting on the blob's skin ({max_y} + {skin})" + ); + let speed = world.bodies[rb].linvel().length(); + assert!(speed < 0.01, "box did not settle: {speed} m/s"); +} + +/// A soft body sleeps as a unit, wakes when hit, and is removed cleanly. +#[test] +fn soft_body_sleeps_wakes_and_is_removed() { + let mut world = world_with_ground(); + let blob = SoftBodyBuilder::grid(Vector::new(0.0, 0.5), Vector::splat(0.5), 4, 4) + .softness(SpringCoefficients::new(30.0, 1.0)) + .linear_damping(0.5); + let handle = world.insert_soft_body(blob); + let num_bodies_before = world.bodies.len(); + for _ in 0..1200 { + world.step(); + } + assert!( + world.soft_bodies[handle].is_sleeping(), + "soft body did not fall asleep" + ); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 3.0)), + ColliderBuilder::ball(0.2), + ); + let mut woke = false; + for _ in 0..200 { + world.step(); + if !world.soft_bodies[handle].is_sleeping() { + woke = true; + break; + } + } + assert!(woke, "soft body was not woken by the ball"); + let sb = world.remove_soft_body(handle).unwrap(); + assert!(world.bodies.get(sb.root_body()).is_none()); + // One root body was removed by `remove_soft_body`, and one ball body was inserted since + // `num_bodies_before` was captured: the counts cancel out. + assert_eq!(world.bodies.len(), num_bodies_before); + for _ in 0..10 { + world.step(); + } +} + +/// A snapshot of a world with soft bodies round-trips through serde. +#[cfg(feature = "serde-serialize")] +#[test] +fn soft_bodies_serialize() { + let mut world = world_with_ground(); + let disk = SoftBodyBuilder::disk(Vector::new(0.0, 1.5), 0.5, 16); + let handle = world.insert_soft_body(disk); + for _ in 0..50 { + world.step(); + } + let bytes = bincode::serialize(&world).unwrap(); + let mut restored: PhysicsWorld = bincode::deserialize(&bytes).unwrap(); + for _ in 0..50 { + world.step(); + restored.step(); + } + let a = world.soft_bodies[handle].particle_position(3); + let b = restored.soft_bodies[handle].particle_position(3); + assert!( + (a - b).length() < 1.0e-3, + "restored world diverged: {a:?} vs {b:?}" + ); +} + +/// Surface (polyline) collisions: a box resting on a triangulated soft square neither sinks in +/// nor slides, and nothing creeps at rest. +#[test] +fn box_rests_on_surface_and_nothing_creeps() { + let mut world = world_with_ground(); + let square = SoftBodyBuilder::grid(Vector::new(0.0, 0.6), Vector::splat(0.6), 6, 6) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e3, + poisson_ratio: 0.35, + ..Default::default() + }) + .particle_mass(0.2); + let handle = world.insert_soft_body(square); + assert!(world.soft_bodies[handle].collision_mesh().is_some()); + let (rb, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.1, 2.0)), + ColliderBuilder::cuboid(0.2, 0.2).density(2.0), + ); + for _ in 0..600 { + world.step(); + } + assert_finite(&world, handle); + let box_pos = world.bodies[rb].translation(); + assert!( + box_pos.y > 1.2, + "box sank into the soft square: {box_pos:?}" + ); + assert!( + box_pos.y < 1.7, + "box floats above the soft square: {box_pos:?}" + ); + assert!( + (box_pos.x - 0.1).abs() < 0.05, + "box slid on the soft square: {box_pos:?}" + ); + let com = world.soft_bodies[handle].center_of_mass(); + assert!(com.x.abs() < 0.02, "soft square crept: {com:?}"); + for v in world.soft_bodies[handle].particle_velocities() { + assert!(v.length() < 0.01); + } +} + +/// Soft-vs-soft: a blob dropped on a soft square rests on top of it. +#[test] +fn blob_rests_on_blob() { + let mut world = world_with_ground(); + let bottom = SoftBodyBuilder::grid(Vector::new(0.0, 0.8), Vector::splat(0.8), 6, 6) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.1); + let bottom = world.insert_soft_body(bottom); + let top = SoftBodyBuilder::disk(Vector::new(0.1, 2.6), 0.6, 24) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.05); + let top = world.insert_soft_body(top); + for _ in 0..400 { + world.step(); + } + assert_finite(&world, top); + let top_min = world.soft_bodies[top] + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + let bottom_max = world.soft_bodies[bottom] + .particle_positions() + .map(|p| p.y) + .fold(Real::MIN, Real::max); + assert!( + top_min > 0.9, + "top blob fell through the bottom one: {top_min}" + ); + assert!( + top_min < bottom_max + 0.3, + "top blob floats: {top_min} vs {bottom_max}" + ); +} + +/// A strip pinched between two jelly squares, all colliding through their polylines, settles +/// and sleeps: the polyline-vs-particle constraints of both sides agree and see live deformation. +#[test] +fn strip_pinched_between_soft_squares_settles() { + let mut world = world_with_ground(); + let jelly = |center: Vector| { + SoftBodyBuilder::grid(center, Vector::splat(0.5), 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.08) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) + }; + let strip = SoftBodyBuilder::grid(Vector::new(0.0, 1.4), Vector::new(0.9, 0.06), 16, 2) + .cell_model(SoftBodyCellModel::Volume) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.03) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)); + let handles = [ + world.insert_soft_body(jelly(Vector::new(0.0, 0.6))), + world.insert_soft_body(strip), + world.insert_soft_body(jelly(Vector::new(0.0, 2.2))), + ]; + for _ in 0..900 { + world.step(); + } + for h in handles { + assert_finite(&world, h); + assert!( + world.soft_bodies[h].is_sleeping(), + "the sandwich did not settle within 15 s" + ); + } + let com = |h| world.soft_bodies[h].center_of_mass(); + assert!(com(handles[2]).y > com(handles[1]).y && com(handles[1]).y > com(handles[0]).y); + assert!( + com(handles[2]).y > 1.6, + "top square sank: {:?}", + com(handles[2]) + ); +} + +/// Edge-vs-edge (segment-vs-segment) contacts: a taut horizontal rope pushed sideways into a +/// taut vertical rope crosses it between the vertices of both (no vertex ever comes within the +/// vertex contact reach); the crossing segments must hold each other instead of passing through. +#[test] +fn crossing_ropes_hold_through_segment_contacts() { + let mut world = PhysicsWorld::new(); + let rope = |start: Vector, end: Vector| { + SoftBodyBuilder::rope(start, end, 5) + .softness(SpringCoefficients::new(60.0, 1.0)) + .particle_mass(0.05) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05)) + .pinned_particles([0, 4]) + }; + // Vertical rope at x = 1 (vertices at y = 0.5, 1, 1.5, 2, 2.5). + let vertical = world.insert_soft_body(rope(Vector::new(1.0, 0.5), Vector::new(1.0, 2.5))); + // Horizontal rope at y = 1.25 (vertices at x = -1.5, -1, ..., 0.5); its pinned ends move + // 0.75 right over 3 s so its last segment crosses the vertical rope's second segment + // 0.25 away from every vertex. + let horizontal = world.insert_soft_body(rope(Vector::new(-1.5, 1.25), Vector::new(0.5, 1.25))); + let pins: Vec<(usize, Vector)> = [0usize, 4] + .iter() + .map(|&i| (i, world.soft_bodies[horizontal].particle_position(i))) + .collect(); + for step in 0..600 { + let t = (step + 1) as Real / 60.0; + let shift = 0.75 * (t / 3.0).min(1.0); + for &(i, rest) in &pins { + world.soft_bodies[horizontal] + .set_particle_kinematic_target(i, rest + Vector::new(shift, 0.0)); + } + world.step(); + } + assert_finite(&world, vertical); + let v = &world.soft_bodies[vertical]; + let h = &world.soft_bodies[horizontal]; + // The vertical rope is bowed to the right by the crossing segment. + let bow = v.particle_position(2).x; + assert!(bow > 1.15, "the ropes passed through each other: bow {bow}"); + // The crossing segments stay two particle radii apart. + let (a0, a1) = (v.particle_position(1), v.particle_position(2)); + let (b0, b1) = (h.particle_position(3), h.particle_position(4)); + let (la, lb) = parry2d::query::details::closest_points_segment_segment_with_locations_nD( + (&a0, &a1), + (&b0, &b1), + ); + let pa = a0 * la.barycentric_coordinates()[0] + a1 * la.barycentric_coordinates()[1]; + let pb = b0 * lb.barycentric_coordinates()[0] + b1 * lb.barycentric_coordinates()[1]; + let gap = (pb - pa).length(); + assert!(gap > 0.08, "crossing segments too close: {gap}"); +} + +/// `SoftBodyBuilder::volumetric` fills a closed polygon with conforming triangular cells: a disk +/// filled with cells a fifth of its radius has about the disk's area, no inverted cell, and rests +/// on the ground. +#[test] +fn volumetric_disk_is_a_valid_soft_body() { + let n = 32; + let vertices: Vec = (0..n) + .map(|i| { + let a = i as Real / n as Real * core::f32::consts::TAU; + Vector::new(a.cos(), 1.5 + a.sin()) + }) + .collect(); + let indices: Vec<[u32; 2]> = (0..n).map(|i| [i as u32, ((i + 1) % n) as u32]).collect(); + let builder = SoftBodyBuilder::volumetric(&vertices, &indices, 0.2) + .expect("the disk is filled with cells") + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e4, + poisson_ratio: 0.3, + ..Default::default() + }) + .particle_mass(0.05); + let mut world = world_with_ground(); + let handle = world.insert_soft_body(builder); + let sb = &world.soft_bodies[handle]; + let area = sb.rest_volume(); + let disk_area = core::f32::consts::PI; + assert!( + (area - disk_area).abs() < 0.3 * disk_area, + "volumetric area {area} vs disk {disk_area}" + ); + for c in sb.cells() { + let x: [Vector; 3] = core::array::from_fn(|k| sb.particle_position(c.vertices[k] as usize)); + let a = (x[1] - x[0]).perp_dot(x[2] - x[0]) * 0.5; + assert!(a > 0.0, "inverted cell (area {a})"); + } + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let min_y = sb + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + assert!( + min_y > -0.05 && min_y < 0.3, + "the disk sank or floats: {min_y}" + ); +} + +/// A cell torn far past its rest shape recovers without snapping back at hundreds of m/s: the +/// strain error fed to the elastic bias is clamped, continuously through the inversion. +#[test] +fn torn_cell_recovers_without_snapping() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let strip = SoftBodyBuilder::grid(Vector::new(0.0, 5.0), Vector::new(1.2, 0.12), 13, 2) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.03) + .particle_radius(0.06) + .no_surface_collider(); + let handle = world.insert_soft_body(strip); + // Drag a middle particle 8 m away, through the strip (its cells end up inverted, with a + // 40x stretch), and let go. + let sb = &world.soft_bodies[handle]; + let torn = (0..sb.num_particles()) + .min_by(|&a, &b| { + let d = |i: usize| (sb.particle_position(i) - Vector::new(0.0, 5.12)).length(); + d(a).partial_cmp(&d(b)).unwrap() + }) + .unwrap(); + world.soft_bodies[handle].set_particle_position(torn, Vector::new(0.0, -3.0)); + let mut max_speed: Real = 0.0; + for _ in 0..600 { + world.step(); + for v in world.soft_bodies[handle].particle_velocities() { + max_speed = max_speed.max(v.length()); + } + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let mut inverted = 0; + let mut worst_ratio: Real = 0.0; + for c in sb.cells() { + let x: [Vector; 3] = core::array::from_fn(|k| sb.particle_position(c.vertices[k] as usize)); + let area = (x[1] - x[0]).perp_dot(x[2] - x[0]) * 0.5; + if area < 0.0 { + inverted += 1; + } + worst_ratio = worst_ratio.max((area / c.rest_volume - 1.0).abs()); + } + assert_eq!(inverted, 0, "cells still inverted"); + assert!( + worst_ratio < 0.2, + "a cell is still off its rest area by {worst_ratio}" + ); + // Uncapped, the recovery through the inversion peaked above 150 m/s. + assert!(max_speed < 60.0, "recovery snapped at {max_speed} m/s"); +} + +/// A closed surface only expels intruders through elements whose winding can be trusted: an +/// element belonging to an inverted cell has a mirrored winding, and a ball resting on it must +/// not be "expelled" through the body. +#[test] +fn inverted_cell_elements_do_not_expel() { + let mut world = world_with_ground(); + let square = SoftBodyBuilder::grid(Vector::new(0.0, 0.75), Vector::splat(0.75), 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e4, + poisson_ratio: 0.4, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.05); + let handle = world.insert_soft_body(square); + // Pin the top edge, its two middle particles swapped and raised: the segment between them + // runs backwards (its cell is inverted), so its winding normal points into the square, and + // it stands alone above the rest of the top edge. + let sb = &world.soft_bodies[handle]; + let n = sb.num_particles(); + let mut top: Vec = (0..n) + .filter(|&i| sb.particle_position(i).y > 1.49) + .collect(); + top.sort_by(|&a, &b| { + sb.particle_position(a) + .x + .partial_cmp(&sb.particle_position(b).x) + .unwrap() + }); + let (a, b) = (top[1], top[2]); + for &i in &top { + world.soft_bodies[handle].set_particle_pinned(i, true); + } + for (i, x) in [(a, 0.6), (b, -0.6)] { + world.soft_bodies[handle].set_particle_position(i, Vector::new(x, 1.9)); + } + // A ball resting on that segment only. + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 1.9 + 0.05 + 0.15)), + ColliderBuilder::ball(0.15), + ); + for _ in 0..120 { + world.step(); + } + assert_finite(&world, handle); + // Resting on the segment: 1.9 + skin 0.05 + radius 0.15 (a mirrored-winding expulsion + // pushed it down to 1.93). + let y = world.bodies[ball].translation().y; + assert!( + y > 2.05, + "the ball was pushed into the square through a mirrored element: y = {y}" + ); +} + +/// A pressurized body turned inside out is accepted as mirrored: its area target follows its +/// orientation and its outward normals flip with it, so it stays a blob and supports a box. +#[test] +fn inverted_blob_is_accepted_as_mirrored() { + let mut world = world_with_ground(); + let blob = SoftBodyBuilder::disk(Vector::new(0.0, 0.7), 0.6, 20) + .softness(SpringCoefficients::new(20.0, 1.0)) + .volume_factor(1.1) + .particle_mass(0.05) + .particle_radius(0.06); + let handle = world.insert_soft_body(blob); + // Mirror it (same shape, reversed winding). + { + let sb = &mut world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + let p = sb.particle_position(i); + sb.set_particle_position(i, Vector::new(-p.x, p.y)); + } + } + let (block, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 1.8)), + ColliderBuilder::cuboid(0.3, 0.2).density(0.3), + ); + for _ in 0..240 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let area = sb.volume(); + assert!( + (area.abs() - 1.1 * sb.rest_volume()).abs() < 0.15 * sb.rest_volume(), + "mirrored blob area {area} vs target {}", + 1.1 * sb.rest_volume() + ); + let y = world.bodies[block].translation().y; + assert!( + y > 1.0, + "the block did not rest on the mirrored blob: y = {y}" + ); +} + +/// A fixed thin pin inside a blob pressed on the ground by a plate cannot be expelled: the +/// expulsion is demoted to a quiet two-sided contact and the blob rests and sleeps. +#[test] +fn stuck_expulsion_settles() { + let mut world = world_with_ground(); + let blob = SoftBodyBuilder::disk(Vector::new(0.0, 0.6), 0.6, 24) + .softness(SpringCoefficients::new(20.0, 1.0)) + .volume_factor(1.1) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)); + let handle = world.insert_soft_body(blob); + // The pin, just above the blob's bottom, fully inside. + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, 0.18)), + ColliderBuilder::capsule_x(0.3, 0.04), + ); + // A plate lowered onto the blob during the first second, then held. + let (plate, _) = world.insert( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 1.5)), + ColliderBuilder::cuboid(1.5, 0.2), + ); + let dt = world.integration_parameters.dt; + let mut slept_at = None; + let mut max_speed_after_3s: Real = 0.0; + for step in 0..(10.0 / dt) as usize { + let t = step as Real * dt; + let y = 1.5 - 0.5 * t.min(1.0); + world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y)); + world.step(); + let sb = &world.soft_bodies[handle]; + if t > 3.0 { + for v in sb.particle_velocities() { + max_speed_after_3s = max_speed_after_3s.max(v.length()); + } + } + if sb.is_sleeping() { + slept_at = Some(t); + break; + } + } + assert_finite(&world, handle); + assert!( + max_speed_after_3s < 0.5, + "the blob keeps fighting the pin: {max_speed_after_3s} m/s" + ); + assert!( + slept_at.is_some_and(|t| t < 10.0), + "the blob never fell asleep (slept at {slept_at:?})" + ); +} + +/// A fully pinned rope (every particle kinematic) still collides through its polyline: a box +/// dropped on it rests on it instead of falling through (a body without a free particle used to +/// be left out of the solver's awake list, so its surface had no contact constraint). +#[test] +fn fully_pinned_rope_holds_a_box() { + let mut world = world_with_ground(); + let n = 11; + let rope = SoftBodyBuilder::rope(Vector::new(-0.5, 1.0), Vector::new(0.5, 1.0), n) + .pinned_particles(0..n as u32) + .surface_collider(ColliderBuilder::ball(0.03)); + let handle = world.insert_soft_body(rope); + let (block, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 2.0)), + ColliderBuilder::cuboid(0.15, 0.15), + ); + for _ in 0..240 { + world.step(); + } + assert_finite(&world, handle); + // Resting on the polyline: 1.0 + half-height 0.15 + the surface's skin (the particle radius). + let y = world.bodies[block].translation().y; + assert!(y > 1.14, "the box fell through the pinned rope: y = {y}"); + assert!(y < 1.3, "the box did not settle on the rope: y = {y}"); + let sb = &world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + assert!( + (sb.particle_position(i).y - 1.0).abs() < 1.0e-6, + "a pinned particle moved: {:?}", + sb.particle_position(i) + ); + } +} + +/// The pinned particles moved kinematically (the whole rope rising) lift the resting box up +/// with them. +#[test] +fn kinematic_pinned_rope_pushes_a_box() { + let mut world = world_with_ground(); + let n = 11; + let rope = SoftBodyBuilder::rope(Vector::new(-0.5, 1.0), Vector::new(0.5, 1.0), n) + .pinned_particles(0..n as u32) + .surface_collider(ColliderBuilder::ball(0.03)); + let handle = world.insert_soft_body(rope); + let (block, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 1.5)), + ColliderBuilder::cuboid(0.15, 0.15), + ); + for _ in 0..120 { + world.step(); + } + let rested = world.bodies[block].translation().y; + assert!( + rested > 1.14, + "the box fell through the pinned rope: y = {rested}" + ); + // Raise the rope at 0.5 m/s for two seconds. + let dt = world.integration_parameters.dt; + let num_particles = world.soft_bodies[handle].num_particles(); + let mut rope_y = 1.0; + for _ in 0..120 { + rope_y += 0.5 * dt; + for i in 0..num_particles { + let mut pos = world.soft_bodies[handle].particle_position(i); + pos.y = rope_y; + world.soft_bodies[handle].set_particle_kinematic_target(i, pos); + } + world.step(); + let y = world.bodies[block].translation().y; + assert!( + y > rope_y + 0.1, + "the rising rope passed through the box: box y = {y}, rope y = {rope_y}" + ); + } + assert_finite(&world, handle); + let y = world.bodies[block].translation().y; + assert!( + y > rope_y + 0.14 && y < rope_y + 0.35, + "the box does not ride the rope: box y = {y}, rope y = {rope_y}" + ); +} + +/// A triangulated strip pinned at both ends and pulled past its tear strain rips under the volume +/// model (edges tear) and the corotational one (cells tear on their principal strain); cracks +/// split particles without removing cells and the pieces keep consistent tables. +#[test] +fn strip_tears_when_pulled_apart() { + for model in [SoftBodyCellModel::Volume, SoftBodyCellModel::Corotational] { + let mut world = PhysicsWorld::new(); + let (nx, ny) = (13usize, 3usize); + let strip = SoftBodyBuilder::grid(Vector::new(0.0, 2.0), Vector::new(1.2, 0.2), nx, ny) + .cell_model(model) + .material(SoftBodyMaterial { + young_modulus: 5.0e3, + tear_strain: Some(0.4), + ..Default::default() + }) + .particle_mass(0.05); + let handle = world.insert_soft_body(strip); + let sb = &world.soft_bodies[handle]; + let left: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particle_position(i).x < -1.19) + .collect(); + let right: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particle_position(i).x > 1.19) + .collect(); + assert_eq!(left.len(), ny); + for &i in left.iter().chain(right.iter()) { + world.soft_bodies[handle].set_particle_pinned(i, true); + } + let rest: Vec = right + .iter() + .map(|&i| world.soft_bodies[handle].particle_position(i)) + .collect(); + for k in 0..240 { + let shift = 2.4 * (k as Real / 180.0).min(1.0); + for (&i, p) in right.iter().zip(rest.iter()) { + world.soft_bodies[handle] + .set_particle_kinematic_target(i, *p + Vector::new(shift, 0.0)); + } + world.step(); + assert_finite(&world, handle); + world.soft_bodies[handle].validate_topology().unwrap(); + } + let sb = &world.soft_bodies[handle]; + assert!( + ); + assert!(sb.num_particles() > num_particles, "{model:?}: no particle split"); + // Torn apart: no remaining cell edge is stretched past twice the tear strain. + let mut max_stretch: Real = 0.0; + for c in sb.cells() { + for a in 0..3 { + for b in a + 1..3 { + let (pa, pb) = ( + sb.particle_position(c.vertices[a] as usize), + sb.particle_position(c.vertices[b] as usize), + ); + let rest = (sb.particles()[c.vertices[a] as usize].rest_position() + - sb.particles()[c.vertices[b] as usize].rest_position()) + .length(); + max_stretch = max_stretch.max((pa - pb).length() / rest); + } + assert!( + max_stretch < 1.8, + "{model:?}: a cell is still stretched by {max_stretch}: the strip did not split" + ); + } +} + +/// A particle two pieces share alone is duplicated: tearing a bottom edge of a one-cell-thick +/// strip opens a crack at its first endpoint, and the top particle left joining the halves alone +/// splits too. No cell is removed; mass and rest area are conserved. +#[test] +fn tearing_splits_the_particle_two_fans_share() { + let mut world = world_with_ground(); + let grid = SoftBodyBuilder::grid(Vector::new(0.0, 1.0), Vector::new(1.0, 0.25), 5, 2) + let handle = world.insert_soft_body(grid); + world.step(); + let sb = &world.soft_bodies[handle]; + let idx = |i: u32, j: u32| i * ny + j; + .edges() + .iter() + assert!( + world + .soft_bodies + .tear(handle, &[], &torn, &mut world.bodies, &mut world.colliders) + ); + let sb = &world.soft_bodies[handle]; + sb.validate_topology().unwrap(); + let left: Vec = (0..=top).collect(); + let right: Vec = (top + 1..num_particles + 2).collect(); + assert_eq!(sb.connected_pieces(), vec![left, right]); + assert!((sb.mass() - mass).abs() < 1.0e-5); + assert!((sb.rest_measure() - measure).abs() < 1.0e-5); + for _ in 0..120 { + world.step(); + assert_finite(&world, handle); + } + world.soft_bodies[handle].validate_topology().unwrap(); +} + +/// `SoftBodyBuilder::trimesh` (2D): the triangles' vertices become the particles, their edges +/// springs, the boundary the surface (counter-clockwise, holes clockwise), and shape matching +/// keeps the shape once dropped. +#[test] +fn trimesh_soft_body_keeps_its_shape() { + let mut world = world_with_ground(); + // A square with a square hole: 8 vertices, 8 triangles (given clockwise on purpose). + let outer = [ + Vector::new(-1.0, -1.0), + Vector::new(1.0, -1.0), + Vector::new(1.0, 1.0), + Vector::new(-1.0, 1.0), + ]; + let inner = [ + Vector::new(-0.4, -0.4), + Vector::new(0.4, -0.4), + Vector::new(0.4, 0.4), + Vector::new(-0.4, 0.4), + ]; + let vertices: Vec = outer + .iter() + .chain(inner.iter()) + .map(|v| *v + Vector::new(0.0, 2.0)) + .collect(); + let mut triangles = Vec::new(); + for k in 0..4u32 { + let (a, b) = (k, (k + 1) % 4); + triangles.push([a, 4 + a, b]); + triangles.push([b, 4 + a, 4 + b]); + } + let square = SoftBodyBuilder::trimesh(vertices, triangles) + .unwrap() + .material(SoftBodyMaterial { + shape_matching_softness: SpringCoefficients::new(30.0, 1.0), + ..Default::default() + }) + .particle_mass(0.1); + let handle = world.insert_soft_body(square); + let sb = &world.soft_bodies[handle]; + assert!(sb.cluster(0).unwrap().shape_matching_enabled()); + assert_eq!(sb.boundary().len(), 8, "outer and inner boundary segments"); + assert_eq!(sb.edges().len(), 16); + assert!(sb.cells().is_empty()); + // Outer loop counter-clockwise, hole clockwise: a positive enclosed area of 4 - 0.64. + assert!( + (sb.rest_volume() - 3.36).abs() < 1.0e-4, + "{}", + sb.rest_volume() + ); + assert!(SoftBodyBuilder::trimesh(Vec::new(), Vec::new()).is_none()); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let pose = world.bodies[sb.cluster_proxy(0).unwrap()].position(); + let max_dev = sb + .particles() + .iter() + .map(|p| (pose * p.rest_position() - p.position()).length()) + .fold(0.0, Real::max); + assert!(max_dev < 0.05, "the square lost its shape: {max_dev}"); + let com = sb.center_of_mass(); + assert!( + (com.y - 1.0 - sb.particle_radius()).abs() < 0.1, + "the square did not rest on the ground: {com:?}" + ); +} + +/// `SoftBodyBuilder::polyline` (2D): segments as springs and surface, bending edges at every +/// two-segment vertex, shape matching; a closed counter-clockwise polyline has an inside. +#[test] +fn polyline_soft_body() { + let mut world = world_with_ground(); + let n = 12u32; + let ring: Vec = (0..n) + .map(|i| { + let a = i as Real / n as Real * core::f32::consts::TAU; + Vector::new(a.cos(), a.sin() + 2.0) + }) + .collect(); + let segments: Vec<[u32; 2]> = (0..n).map(|i| [i, (i + 1) % n]).collect(); + let ring = SoftBodyBuilder::polyline(ring, Some(segments)).unwrap(); + let handle = world.insert_soft_body(ring.particle_mass(0.05)); + let sb = &world.soft_bodies[handle]; + assert_eq!(sb.boundary().len(), 12); + assert_eq!(sb.edges().len(), 24, "12 structural + 12 bending edges"); + assert!(sb.rest_volume() > 3.0, "{}", sb.rest_volume()); + // An open strip: consecutive vertices, one bending edge per interior vertex. + let strip = SoftBodyBuilder::polyline( + vec![ + Vector::new(3.0, 3.0), + Vector::new(3.5, 3.0), + Vector::new(4.0, 3.0), + Vector::new(4.5, 3.0), + ], + None, + ) + .unwrap(); + let strip = world.insert_soft_body(strip.particle_mass(0.05)); + assert_eq!(world.soft_bodies[strip].edges().len(), 3 + 2); + assert!(SoftBodyBuilder::polyline(vec![Vector::ZERO], None).is_none()); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + assert_finite(&world, strip); + let sb = &world.soft_bodies[handle]; + let com = sb.center_of_mass(); + assert!( + (com.y - 1.0 - sb.particle_radius()).abs() < 0.15, + "the ring did not rest on the ground as a ring: {com:?}" + ); + let strip = &world.soft_bodies[strip]; + let strip_y = strip.center_of_mass().y; + assert!( + (strip_y - strip.particle_radius()).abs() < 0.05, + "the strip did not fall flat: {strip_y}" + ); +} diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs new file mode 100644 index 000000000..a8f73ab53 --- /dev/null +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -0,0 +1,2870 @@ +//! Soft-body regression tests: substep invariance of the material, ropes, cloth, volume +//! preservation, shape matching, tetrahedral bodies, two-way coupling, sleeping and removal. + +use rapier3d::prelude::*; + +fn world_with_ground() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(50.0, 0.5, 50.0).translation(Vector::new(0.0, -0.5, 0.0)), + ); + world +} + +/// The mesh a body wears as a skin (bound to its cells). +fn skinned_mesh(sb: &SoftBody) -> &SoftCollisionMesh { + sb.meshes() + .find(|mesh| mesh.is_skinned()) + .expect("the body has a skinned mesh") +} + +fn assert_finite(world: &PhysicsWorld, handle: SoftBodyHandle) { + let sb = &world.soft_bodies[handle]; + for p in sb.particles() { + assert!( + p.position().is_finite(), + "non-finite particle position {:?}", + p.position() + ); + } +} + +/// The period of a mass-spring pair set by its natural frequency must not depend on the +/// substep count: this is the property that separates the material model from position-based +/// `rigidity` knobs. +#[test] +fn edge_period_is_substep_invariant() { + let natural_frequency = 2.0; // Hz + let mut periods = Vec::new(); + for substeps in [1usize, 4, 16] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.num_solver_iterations = substeps; + world.integration_parameters.dt = 1.0 / 240.0; + // Two particles, one pinned, the free one starts stretched. + let builder = SoftBodyBuilder::new(vec![Vector::ZERO, Vector::new(1.0, 0.0, 0.0)]) + .edges(vec![[0, 1]]) + .pinned_particles([0]) + .softness(SpringCoefficients::new(natural_frequency, 0.0)) + .no_surface_collider() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + world.soft_bodies[handle].set_particle_position(1, Vector::new(1.2, 0.0, 0.0)); + + // Measure the time between the two first crossings of the rest length from above. + let mut crossings = Vec::new(); + let mut prev_len = 1.2; + for step in 0..2000 { + world.step(); + let len = world.soft_bodies[handle].particle_position(1).x; + if prev_len > 1.0 && len <= 1.0 { + crossings.push(step as Real * world.integration_parameters.dt); + if crossings.len() == 2 { + break; + } + } + prev_len = len; + } + assert_eq!( + crossings.len(), + 2, + "no oscillation with {substeps} substeps" + ); + periods.push(crossings[1] - crossings[0]); + } + let expected = 1.0 / natural_frequency; + for (i, period) in periods.iter().enumerate() { + assert!( + (period - expected).abs() < 0.08 * expected, + "period {period} (substeps case {i}) too far from {expected}" + ); + } + let (min, max) = periods + .iter() + .fold((Real::MAX, Real::MIN), |(a, b), p| (a.min(*p), b.max(*p))); + assert!( + (max - min) < 0.05 * expected, + "period depends on the substep count: {periods:?}" + ); +} + +/// A rope pinned at one end hangs under gravity: its length stays close to the rest length +/// and the free end ends up below the anchor. +#[test] +fn rope_hangs_from_anchor() { + let mut world = PhysicsWorld::new(); + let rope = SoftBodyBuilder::rope(Vector::ZERO, Vector::new(2.0, 0.0, 0.0), 21) + .pinned_particles([0]) + .softness(SpringCoefficients::new(60.0, 1.0)) + .linear_damping(1.0); + let handle = world.insert_soft_body(rope); + for _ in 0..600 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let end = sb.particle_position(20); + assert!(end.y < -1.9, "rope end did not fall: {end:?}"); + assert!( + end.x.abs() < 0.2, + "rope end did not settle under the anchor: {end:?}" + ); + let mut length = 0.0; + for i in 0..20 { + length += (sb.particle_position(i + 1) - sb.particle_position(i)).length(); + } + assert!((length - 2.0).abs() < 0.1, "rope length drifted: {length}"); +} + +/// A cloth pinned by two corners drapes without stretching much, and its particles are +/// simulated as rotation-locked rigid bodies. +#[test] +fn cloth_drapes() { + let mut world = PhysicsWorld::new(); + let n = 10; + let cloth = SoftBodyBuilder::cloth( + Vector::new(0.0, 2.0, 0.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .pinned_particles([0, (n - 1) as u32]) + .softness(SpringCoefficients::new(100.0, 1.0)); + let handle = world.insert_soft_body(cloth); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + // The far corner fell. + let far = sb.particle_position(n * n - 1); + assert!(far.y < 1.5, "cloth did not drape: {far:?}"); + // No edge stretched by more than 20% (the two top edges bear ~50 particle weights each: + // at 100 Hz that is a few percent of stretch). + for e in sb.edges() { + if e.kind != SoftBodyEdgeKind::Structural { + continue; + } + let len = (sb.particle_position(e.vertices[0] as usize) + - sb.particle_position(e.vertices[1] as usize)) + .length(); + assert!( + len < e.rest_length * 1.2, + "edge over-stretched: {len} vs {}", + e.rest_length + ); + } + let root = &world.bodies[sb.root_body()]; + assert!(root.is_rotation_locked().iter().all(|l| *l)); + assert!(root.is_translation_locked()); +} + +/// A hollow sphere with volume preservation dropped on the ground keeps its volume, and a +/// pressurized one inflates. +#[test] +fn sphere_keeps_its_volume() { + let mut world = world_with_ground(); + let sphere = SoftBodyBuilder::sphere(Vector::new(0.0, 1.5, 0.0), 1.0, 2) + .softness(SpringCoefficients::new(20.0, 1.0)); + let handle = world.insert_soft_body(sphere); + let rest = world.soft_bodies[handle].rest_volume(); + assert!(rest > 3.0, "icosphere volume too small: {rest}"); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let volume = world.soft_bodies[handle].volume(); + assert!( + (volume - rest).abs() < 0.15 * rest, + "volume drifted: {volume} vs {rest}" + ); + // The lowest particle rests on the ground (within the particle radius). + let min_y = world.soft_bodies[handle] + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + assert!( + min_y > -0.05 && min_y < 0.3, + "sphere sank or floats: {min_y}" + ); + + // Inflate. + world.soft_bodies[handle].set_volume_factor(1.5); + for _ in 0..300 { + world.step(); + } + let inflated = world.soft_bodies[handle].volume(); + assert!( + inflated > 1.25 * rest, + "sphere did not inflate: {inflated} vs {rest}" + ); +} + +/// A shape-matched cloud of particles returns to its rest shape after being randomized. +#[test] +fn shape_matching_recovers_rest_shape() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let mut positions = Vec::new(); + for i in 0..3 { + for j in 0..3 { + for k in 0..3 { + positions.push(Vector::new(i as Real, j as Real, k as Real) * 0.5); + } + } + } + let builder = SoftBodyBuilder::new(positions.clone()) + .shape_matching(true) + .softness(SpringCoefficients::new(10.0, 1.0)) + .no_surface_collider() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + // Scramble. + let sb = &mut world.soft_bodies[handle]; + for (i, p) in positions.iter().enumerate() { + let x = ((i * 7919) % 100) as Real / 100.0 - 0.5; + let y = ((i * 104729) % 100) as Real / 100.0 - 0.5; + let z = ((i * 1299709) % 100) as Real / 100.0 - 0.5; + sb.set_particle_position(i, *p + Vector::new(x, y, z) * 0.6); + } + for _ in 0..600 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + // Compare pairwise distances with the rest shape (the body may have translated/rotated). + let mut max_err: Real = 0.0; + for a in 0..positions.len() { + for b in a + 1..positions.len() { + let rest = (positions[a] - positions[b]).length(); + let cur = (sb.particle_position(a) - sb.particle_position(b)).length(); + max_err = max_err.max((rest - cur).abs()); + } + } + assert!( + max_err < 0.05, + "shape not recovered, max distance error {max_err}" + ); +} + +/// A tetrahedral box (corotational and volume models) dropped on the ground settles and keeps +/// its cells positively oriented and its volume. +#[test] +fn tet_box_settles() { + for model in [SoftBodyCellModel::Volume, SoftBodyCellModel::Corotational] { + let mut world = world_with_ground(); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 1.0, 0.0), Vector::splat(0.5), 4, 4, 4) + .cell_model(model) + .softness(SpringCoefficients::new(30.0, 1.0)); + let handle = world.insert_soft_body(cube); + let rest = world.soft_bodies[handle].rest_volume(); + assert!((rest - 1.0).abs() < 1.0e-3, "cube rest volume {rest}"); + for _ in 0..400 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let volume = sb.volume(); + assert!( + (volume - rest).abs() < 0.2 * rest, + "{model:?}: volume drifted: {volume} vs {rest}" + ); + for c in sb.cells() { + let x: [Vector; 4] = + core::array::from_fn(|k| sb.particle_position(c.vertices[k] as usize)); + let v = (x[1] - x[0]).dot((x[2] - x[0]).cross(x[3] - x[0])) / 6.0; + assert!(v > 0.0, "{model:?}: inverted cell (volume {v})"); + } + let min_y = sb + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + assert!( + min_y > -0.05, + "{model:?}: cube sank through the ground: {min_y}" + ); + } +} + +/// The corotational cells are stable at any stiffness: a very stiff cube (E = 1e8, 0.1 kg +/// particles) squeezed by 20% and dropped spinning settles and sleeps, and a free-floating one +/// keeps its angular momentum (the cells neither resist nor damp rotation). +#[test] +fn stiff_elastic_cube_is_stable() { + let cube = |young: Real| { + SoftBodyBuilder::cuboid(Vector::new(0.0, 3.0, 0.0), Vector::splat(0.75), 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + }; + let spin = |world: &mut PhysicsWorld, handle: SoftBodyHandle, squeeze: Real| { + let sb = &mut world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + let mut t = sb.particle_position(i); + t.y = 3.0 + (t.y - 3.0) * squeeze; + sb.set_particle_position(i, t); + let arm = t - Vector::new(0.0, 3.0, 0.0); + sb.set_particle_velocity(i, Vector::new(0.0, 5.0, 0.0).cross(arm)); + } + }; + for young in [3.0e4, 3.0e6, 1.0e8] { + let mut world = world_with_ground(); + let handle = world.insert_soft_body(cube(young)); + spin(&mut world, handle, 0.8); + for _ in 0..500 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let max_vel = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + assert!( + max_vel < 0.05, + "E = {young}: cube still moving at {max_vel} m/s" + ); + assert!(sb.is_sleeping(), "E = {young}: cube did not fall asleep"); + let volume = sb.volume(); + assert!( + (volume - sb.rest_volume()).abs() < 0.05 * sb.rest_volume(), + "E = {young}: volume {volume} vs rest {}", + sb.rest_volume() + ); + } + + let angular_momentum = |world: &PhysicsWorld, handle: SoftBodyHandle| { + let sb = &world.soft_bodies[handle]; + let com = sb.center_of_mass(); + sb.particles() + .iter() + .map(|p| (p.position() - com).cross(p.velocity() * p.mass())) + .fold(Vector::ZERO, |a, b| a + b) + }; + // With and without deformation damping (which must leave the rigid motion alone). + for damping in [0.0, 100.0] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let mut cube = cube(3.0e6); + cube.material.deformation_damping = damping; + let handle = world.insert_soft_body(cube); + spin(&mut world, handle, 1.0); + world.step(); + let l0 = angular_momentum(&world, handle); + for _ in 0..600 { + world.step(); + } + let l1 = angular_momentum(&world, handle); + assert!( + (l1 - l0).length() < 0.01 * l0.length(), + "damping {damping}: angular momentum drifted: {l0:?} -> {l1:?}" + ); + } +} + +/// The Neo-Hookean cells are as stable as the corotational ones: the same squeezed, spinning +/// cube dropped on the ground at E = 1e4, 1e6 and 1e8 settles, sleeps and keeps its volume. +#[test] +fn stiff_neo_hookean_cube_is_stable() { + for young in [1.0e4, 1.0e6, 1.0e8] { + let mut world = world_with_ground(); + let cube = + SoftBodyBuilder::cuboid(Vector::new(0.0, 3.0, 0.0), Vector::splat(0.75), 4, 4, 4) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1); + let handle = world.insert_soft_body(cube); + let sb = &mut world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + let mut t = sb.particle_position(i); + t.y = 3.0 + (t.y - 3.0) * 0.8; + sb.set_particle_position(i, t); + let arm = t - Vector::new(0.0, 3.0, 0.0); + sb.set_particle_velocity(i, Vector::new(0.0, 5.0, 0.0).cross(arm)); + } + for _ in 0..500 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let max_vel = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + assert!( + max_vel < 0.05, + "E = {young}: cube still moving at {max_vel} m/s" + ); + assert!(sb.is_sleeping(), "E = {young}: cube did not fall asleep"); + let volume = sb.volume(); + assert!( + (volume - sb.rest_volume()).abs() < 0.05 * sb.rest_volume(), + "E = {young}: volume {volume} vs rest {}", + sb.rest_volume() + ); + } +} + +/// A soft Neo-Hookean cube squashed to 30% of its height by a kinematic plate keeps its cells +/// positively oriented (up to a couple of transient flips while held) and recovers its volume +/// within 5% once the plate lifts, with no inverted cell left. +#[test] +fn neo_hookean_cube_survives_large_compression() { + let mut world = world_with_ground(); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 0.5, 0.0), Vector::splat(0.5), 4, 4, 4) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: 1.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.03) + .surface_collider(ColliderBuilder::ball(0.03).friction(0.5)); + let handle = world.insert_soft_body(cube); + // Plate bottom at 1.2 when lifted, 0.3 when down. + let plate_rest = Vector::new(0.0, 1.7, 0.0); + let (plate, _) = world.insert( + RigidBodyBuilder::kinematic_position_based().translation(plate_rest), + ColliderBuilder::cuboid(2.0, 0.5, 2.0), + ); + let inverted = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[handle]; + sb.cells() + .iter() + .filter(|c| { + let x: [Vector; 4] = + core::array::from_fn(|k| sb.particle_position(c.vertices[k] as usize)); + (x[1] - x[0]).dot((x[2] - x[0]).cross(x[3] - x[0])) < 0.0 + }) + .count() + }; + // Down to 30% of the height over 2 s, hold 1 s, up over 2 s, rest 2 s. + let dt = world.integration_parameters.dt; + let mut t: Real = 0.0; + let mut min_height = Real::MAX; + for _ in 0..(7.0 / dt) as usize { + t += dt; + let depth = if t < 2.0 { + t / 2.0 + } else if t < 3.0 { + 1.0 + } else if t < 5.0 { + (5.0 - t) / 2.0 + } else { + 0.0 + }; + world.bodies[plate] + .set_next_kinematic_translation(plate_rest - Vector::new(0.0, 0.9 * depth, 0.0)); + world.step(); + assert_finite(&world, handle); + if (2.0..3.0).contains(&t) { + let sb = &world.soft_bodies[handle]; + let top = sb.particle_positions().map(|p| p.y).fold(0.0, Real::max); + min_height = min_height.min(top); + let inverted = inverted(&world); + assert!( + inverted <= 2, + "{inverted} inverted cells while squashed (t = {t})" + ); + } + } + assert!( + min_height < 0.35, + "the plate did not squash the cube: {min_height}" + ); + assert_eq!(inverted(&world), 0, "inverted cells after release"); + let sb = &world.soft_bodies[handle]; + let volume = sb.volume(); + assert!( + (volume - sb.rest_volume()).abs() < 0.05 * sb.rest_volume(), + "volume {volume} vs rest {}", + sb.rest_volume() + ); +} + +/// The Neo-Hookean material must not depend on the substep count either: the equilibrium +/// compression of a soft column standing on its pinned base under its own weight (about 13% +/// strain), and its small-oscillation period, are the same at 1, 4 and 16 substeps. +#[test] +fn neo_hookean_response_is_substep_invariant() { + let mut compressions = Vec::new(); + let mut periods = Vec::new(); + for substeps in [1usize, 4, 16] { + let mut world = PhysicsWorld::new(); + world.integration_parameters.num_solver_iterations = substeps; + let mut column = + SoftBodyBuilder::cuboid(Vector::new(0.0, 0.5, 0.0), Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: 1.0e3, + poisson_ratio: 0.3, + elastic_damping_ratio: 0.05, + ..Default::default() + }) + .particle_mass(1.0) + .can_sleep(false) + .no_surface_collider(); + let base: Vec = (0..column.positions.len() as u32) + .filter(|i| column.positions[*i as usize].y < 1.0e-3) + .collect(); + column = column.pinned_particles(base); + let handle = world.insert_soft_body(column); + let top: Vec = world.soft_bodies[handle] + .particle_positions() + .enumerate() + .filter(|(_, p)| p.y > 0.999) + .map(|(i, _)| i) + .collect(); + let height = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[handle]; + top.iter().map(|i| sb.particle_position(*i).y).sum::() / top.len() as Real + }; + for _ in 0..600 { + world.step(); + } + let equilibrium = height(&world); + compressions.push(1.0 - equilibrium); + // Nudge the top layer and time the oscillation about the equilibrium. + for &i in &top { + world.soft_bodies[handle].set_particle_velocity(i, Vector::new(0.0, 0.3, 0.0)); + } + let mut crossings = Vec::new(); + let mut prev = height(&world); + for step in 0..1000 { + world.step(); + let y = height(&world); + if prev > equilibrium && y <= equilibrium { + crossings.push(step as Real * world.integration_parameters.dt); + if crossings.len() == 3 { + break; + } + } + prev = y; + } + assert_eq!( + crossings.len(), + 3, + "no oscillation with {substeps} substeps" + ); + periods.push((crossings[2] - crossings[0]) / 2.0); + } + let spread = |v: &[Real]| { + let (min, max) = v + .iter() + .fold((Real::MAX, Real::MIN), |(a, b), x| (a.min(*x), b.max(*x))); + (max - min) / max + }; + assert!( + compressions.iter().all(|c| *c > 0.1), + "the column barely compresses: {compressions:?}" + ); + assert!( + spread(&compressions) < 0.08, + "the equilibrium compression depends on the substep count: {compressions:?}" + ); + assert!( + spread(&periods) < 0.08, + "the oscillation period depends on the substep count: {periods:?}" + ); +} + +/// `deformation_damping` settles the residual bending of stiff slender bodies: a clamped stiff +/// beam swings for many seconds without it and stops within a few with it, while a stiff block +/// rocking about its base (a nearly rigid mode) needs a large rate to settle. +#[test] +fn deformation_damping_settles_stiff_bodies() { + let material = |damping: Real| SoftBodyMaterial { + young_modulus: 1.0e6, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + deformation_damping: damping, + ..Default::default() + }; + // Cantilever: a 3 m beam clamped at x = 0 (its first particle layer pinned). + let tip_speed_after = |damping: Real, seconds: Real| { + let mut world = PhysicsWorld::new(); + let beam = SoftBodyBuilder::cuboid( + Vector::new(1.5, 2.0, 0.0), + Vector::new(1.5, 0.25, 0.25), + 13, + 3, + 3, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(material(damping)) + .particle_mass(0.1) + .particle_radius(0.06); + let handle = world.insert_soft_body(beam); + let sb = &world.soft_bodies[handle]; + let clamped: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particle_position(i).x < 0.01) + .collect(); + let tip: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particle_position(i).x > 2.99) + .collect(); + for &i in &clamped { + world.soft_bodies[handle].set_particle_pinned(i, true); + } + for _ in 0..(seconds * 60.0) as usize { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + tip.iter() + .map(|&i| sb.particle_velocity(i).length()) + .fold(0.0, Real::max) + }; + let undamped = tip_speed_after(0.0, 6.0); + let damped = tip_speed_after(3.0, 6.0); + assert!( + undamped > 0.5, + "the undamped beam already stopped: {undamped}" + ); + assert!(damped < 0.01, "the damped beam still swings: {damped}"); + + // Standing block pushed sideways at the top. + let top_speed_after = |damping: Real| { + let mut world = world_with_ground(); + let block = SoftBodyBuilder::cuboid( + Vector::new(0.0, 1.5, 0.0), + Vector::new(0.25, 1.5, 0.25), + 3, + 13, + 3, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(material(damping)) + .particle_mass(0.1) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(1.0)); + let handle = world.insert_soft_body(block); + for _ in 0..120 { + world.step(); + } + let sb = &mut world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + let y = sb.particle_position(i).y; + sb.set_particle_velocity(i, Vector::new(0.4 * y / 3.0, 0.0, 0.0)); + } + for _ in 0..360 { + world.step(); + } + let sb = &world.soft_bodies[handle]; + sb.particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max) + }; + let undamped = top_speed_after(0.0); + let damped = top_speed_after(100.0); + assert!( + undamped > 0.1, + "the undamped block already stopped: {undamped}" + ); + assert!(damped < 0.01, "the damped block still sways: {damped}"); +} + +/// Two-way coupling: a rigid box dropped on a cloth pinned by its four corners is held up by +/// it (it does not fall through), and it deflects the cloth. +#[test] +fn rigid_box_rests_on_cloth() { + let mut world = PhysicsWorld::new(); + let n = 12; + let cloth = SoftBodyBuilder::cloth( + Vector::new(-0.55, 1.0, -0.55), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .pinned_particles([0, (n - 1) as u32, (n * (n - 1)) as u32, (n * n - 1) as u32]) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.1); + let handle = world.insert_soft_body(cloth); + let (rb, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 1.5, 0.0)), + ColliderBuilder::cuboid(0.15, 0.15, 0.15), + ); + for _ in 0..400 { + world.step(); + } + assert_finite(&world, handle); + let box_y = world.bodies[rb].translation().y; + assert!(box_y > 0.3, "box fell through the cloth: y = {box_y}"); + assert!(box_y < 1.2, "box did not settle on the cloth: y = {box_y}"); + let center = world.soft_bodies[handle].particle_position(n * n / 2 + n / 2); + assert!( + center.y < 0.95, + "cloth was not deflected by the box: {center:?}" + ); +} + +/// A soft body falls asleep as a unit once at rest, wakes up when hit, and is removed cleanly. +#[test] +fn soft_body_sleeps_wakes_and_is_removed() { + let mut world = world_with_ground(); + let blob = SoftBodyBuilder::cuboid(Vector::new(0.0, 0.5, 0.0), Vector::splat(0.5), 3, 3, 3) + .softness(SpringCoefficients::new(30.0, 1.0)) + .linear_damping(0.5); + let handle = world.insert_soft_body(blob); + let num_bodies_before = world.bodies.len(); + for _ in 0..1200 { + world.step(); + } + assert!( + world.soft_bodies[handle].is_sleeping(), + "soft body did not fall asleep" + ); + // The soft body's root body sleeps with it (atomic island). + assert!(world.bodies[world.soft_bodies[handle].root_body()].is_sleeping()); + // A ball dropped on it wakes it up. + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 3.0, 0.0)), + ColliderBuilder::ball(0.2), + ); + let mut woke = false; + for _ in 0..200 { + world.step(); + if !world.soft_bodies[handle].is_sleeping() { + woke = true; + break; + } + } + assert!(woke, "soft body was not woken by the ball"); + // Removal takes the root body with it. + let sb = world.remove_soft_body(handle).unwrap(); + assert!(world.bodies.get(sb.root_body()).is_none()); + assert_eq!(world.bodies.len(), num_bodies_before); + for _ in 0..10 { + world.step(); + } +} + +/// A snapshot of a world with soft bodies round-trips through serde. +#[cfg(feature = "serde-serialize")] +#[test] +fn soft_bodies_serialize() { + let mut world = world_with_ground(); + let rope = SoftBodyBuilder::rope(Vector::new(0.0, 2.0, 0.0), Vector::new(2.0, 2.0, 0.0), 10) + .pinned_particles([0]); + let handle = world.insert_soft_body(rope); + for _ in 0..50 { + world.step(); + } + let bytes = bincode::serialize(&world).unwrap(); + let mut restored: PhysicsWorld = bincode::deserialize(&bytes).unwrap(); + for _ in 0..50 { + world.step(); + restored.step(); + } + let a = world.soft_bodies[handle].particle_position(9); + let b = restored.soft_bodies[handle].particle_position(9); + assert!( + (a - b).length() < 1.0e-3, + "restored world diverged: {a:?} vs {b:?}" + ); +} + +/// Surface collisions: a rigid box resting on a tetrahedral cube's surface neither sinks +/// through it nor slides away, and the resting soft body has no residual velocity. +#[test] +fn box_rests_on_surface_and_nothing_creeps() { + let mut world = world_with_ground(); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 0.6, 0.0), Vector::splat(0.6), 5, 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e3, + poisson_ratio: 0.35, + ..Default::default() + }) + .particle_mass(0.2); + let handle = world.insert_soft_body(cube); + assert!(world.soft_bodies[handle].collision_mesh().is_some()); + let (rb, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.1, 2.0, 0.1)), + ColliderBuilder::cuboid(0.2, 0.2, 0.2).density(2.0), + ); + for _ in 0..600 { + world.step(); + } + assert_finite(&world, handle); + let box_pos = world.bodies[rb].translation(); + assert!(box_pos.y > 1.2, "box sank into the soft cube: {box_pos:?}"); + assert!( + box_pos.y < 1.7, + "box floats above the soft cube: {box_pos:?}" + ); + assert!( + (box_pos.x - 0.1).abs() < 0.05 && (box_pos.z - 0.1).abs() < 0.05, + "box slid on the soft cube: {box_pos:?}" + ); + // The box rests on the surface's contact skin (one particle radius above the top face), + // neither inside the skin nor above it. + let skin = world.soft_bodies[handle].particle_radius(); + let top_under_box = world.soft_bodies[handle] + .particle_positions() + .filter(|p| p.y > 1.0 && (p.x - 0.1).abs() < 0.3 && (p.z - 0.1).abs() < 0.3) + .map(|p| p.y) + .fold(Real::MIN, Real::max); + let box_bottom = box_pos.y - 0.2; + assert!( + (box_bottom - (top_under_box + skin)).abs() < 0.02, + "box bottom {box_bottom} is not on the skin ({top_under_box} + {skin})" + ); + let com = world.soft_bodies[handle].center_of_mass(); + assert!( + com.x.abs() < 0.02 && com.z.abs() < 0.02, + "soft cube crept: {com:?}" + ); + // At rest: every particle and the box are (nearly) still. + for p in world.soft_bodies[handle].particles() { + let v = p.velocity().length(); + assert!(v < 0.01, "residual particle velocity {v}"); + } + assert!(world.bodies[rb].linvel().length() < 0.01); + // The soft body's surface contacts report their force through the manifolds. + let surface = world.soft_bodies[handle].collision_mesh().unwrap().collider(); + let total_impulse: Real = world + .narrow_phase + .contact_pairs_with(surface) + .flat_map(|p| p.manifolds().iter()) + .flat_map(|m| m.points.iter()) + .map(|pt| pt.data.impulse) + .sum(); + assert!( + total_impulse > 0.0, + "no contact impulse reported on the surface" + ); +} + +/// Soft-vs-soft: a soft sphere dropped on a soft cube rests on top of it. +#[test] +fn soft_sphere_rests_on_soft_cube() { + let mut world = world_with_ground(); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 0.6, 0.0), Vector::splat(0.6), 5, 5, 5) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.2); + let cube = world.insert_soft_body(cube); + let sphere = SoftBodyBuilder::sphere(Vector::new(0.0, 2.2, 0.0), 0.5, 2) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.05); + let sphere = world.insert_soft_body(sphere); + for _ in 0..400 { + world.step(); + } + assert_finite(&world, cube); + assert_finite(&world, sphere); + let sphere_min = world.soft_bodies[sphere] + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + let cube_max = world.soft_bodies[cube] + .particle_positions() + .map(|p| p.y) + .fold(Real::MIN, Real::max); + assert!( + sphere_min > 0.9, + "sphere fell through the cube: {sphere_min}" + ); + assert!( + sphere_min < cube_max + 0.4, + "sphere floats above the cube: {sphere_min} vs {cube_max}" + ); +} + +/// A cloth pinched between two jelly cubes (all colliding through their surfaces) settles and +/// sleeps: the constraints of each surface against the other bodies' particles agree, and a +/// surface's constraints see its live deformation. +#[test] +fn cloth_pinched_between_soft_cubes_settles() { + let mut world = world_with_ground(); + let jelly = |center: Vector| { + SoftBodyBuilder::cuboid(center, Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.08) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) + }; + let cloth = SoftBodyBuilder::cloth( + Vector::new(-0.9, 1.4, -0.9), + Vector::new(0.12, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.12), + 16, + 16, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .particle_mass(0.02) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)); + let handles = [ + world.insert_soft_body(jelly(Vector::new(0.0, 0.6, 0.0))), + world.insert_soft_body(cloth), + world.insert_soft_body(jelly(Vector::new(0.0, 2.2, 0.0))), + ]; + for _ in 0..900 { + world.step(); + } + for h in handles { + assert_finite(&world, h); + assert!( + world.soft_bodies[h].is_sleeping(), + "the sandwich did not settle within 15 s" + ); + } + // Stacked: the top cube rests on the cloth on the bottom cube. + let com = |h| world.soft_bodies[h].center_of_mass(); + assert!(com(handles[2]).y > com(handles[1]).y && com(handles[1]).y > com(handles[0]).y); + assert!( + com(handles[2]).y > 1.6, + "top cube sank: {:?}", + com(handles[2]) + ); +} + +/// Self-collision: a strip pinned by both ends whose ends are brought together folds without +/// its two halves passing through each other. +#[test] +fn self_contacts_keep_folded_cloth_apart() { + let mut min_gap = Vec::new(); + for self_contacts in [false, true] { + let mut world = PhysicsWorld::new(); + let n = 40; + let strip = SoftBodyBuilder::cloth( + Vector::new(-2.0, 2.0, 0.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + 3, + ) + .pinned_particles([ + 0, + 1, + 2, + ((n - 1) * 3) as u32, + ((n - 1) * 3 + 1) as u32, + ((n - 1) * 3 + 2) as u32, + ]) + .self_contacts(self_contacts) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.05); + let handle = world.insert_soft_body(strip); + // Let it sag, then move the right end onto the left end. + for _ in 0..120 { + world.step(); + } + for step in 0..240 { + let t = (step as Real / 240.0).min(1.0); + for j in 0..3 { + let target = Vector::new(1.9 - 3.85 * t, 2.0, 0.1 * j as Real); + world.soft_bodies[handle].set_particle_kinematic_target((n - 1) * 3 + j, target); + } + world.step(); + } + for _ in 0..120 { + world.step(); + } + assert_finite(&world, handle); + // Distance between the two halves (first and last quarter of the strip). + let sb = &world.soft_bodies[handle]; + let mut gap: Real = Real::MAX; + for i in 0..n / 4 { + for k in 3 * n / 4..n { + for j in 0..3 { + let a = sb.particle_position(i * 3 + j); + let b = sb.particle_position(k * 3 + j); + gap = gap.min((a - b).length()); + } + } + } + min_gap.push(gap); + } + // Without self-contacts the halves pass through each other freely (gap ~0); with them the + // layers stay a particle radius apart. + assert!( + min_gap[1] > 0.035, + "self contacts did not keep the layers apart: {min_gap:?}" + ); + assert!( + min_gap[1] > min_gap[0] + 0.02, + "self contacts changed nothing: {min_gap:?}" + ); +} + +/// Edge-vs-edge contacts: two coarse thin strips pinned at their ends cross at 90° so that no +/// vertex of either projects on the other (their edges cross between vertices); a plate loading +/// the crossing must not push the upper strip through the lower one. +#[test] +fn crossing_strips_hold_through_edge_contacts() { + let mut world = world_with_ground(); + let strip = |origin: Vector, du: Vector, dv: Vector| { + SoftBodyBuilder::cloth(origin, du, dv, 7, 2) + .softness(SpringCoefficients::new(60.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(60.0, 1.0)) + }) + .particle_mass(0.05) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)) + .pinned_particles([0, 1, 12, 13]) + }; + // Along x at height 1 (vertices at x = -1.5, -1, ..., 1.5 and z = -0.1, 0.1). + let lower = world.insert_soft_body(strip( + Vector::new(-1.5, 1.0, -0.1), + Vector::new(0.5, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.2), + )); + // Along z just above (vertices at z = -1.75, ..., 1.25 and x = 0.15, 0.35): its edges cross + // the lower strip's edges 0.15 away from every vertex, beyond the vertex contact reach. + let upper = world.insert_soft_body(strip( + Vector::new(0.15, 1.05, -1.75), + Vector::new(0.0, 0.0, 0.5), + Vector::new(0.2, 0.0, 0.0), + )); + let (plate, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.25, 1.2, 0.0)), + ColliderBuilder::cuboid(0.25, 0.05, 0.25).density(100.0), + ); + for _ in 0..600 { + world.step(); + } + assert_finite(&world, upper); + let mid_y = |h: SoftBodyHandle| { + let sb = &world.soft_bodies[h]; + (sb.particle_position(6).y + sb.particle_position(7).y) * 0.5 + }; + let (upper_y, lower_y) = (mid_y(upper), mid_y(lower)); + assert!( + upper_y > lower_y + 0.05, + "the upper strip went through the lower one: {upper_y} vs {lower_y}" + ); + let plate_y = world.bodies[plate].translation().y; + assert!(plate_y > upper_y, "the plate fell through: {plate_y}"); +} + +/// Fast deformable bodies get speculative contacts over their coming motion (the colliders' +/// margins grow with the particles' speed): a cloth thrown down at 15 m/s lands on a thin +/// dynamic plate instead of tunneling through it. +#[test] +fn fast_cloth_does_not_tunnel_through_thin_dynamic_plate() { + let mut world = world_with_ground(); + for (x, z) in [(-2.0, -2.0), (2.0, -2.0), (-2.0, 2.0), (2.0, 2.0)] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 1.0, z)), + ColliderBuilder::cuboid(0.2, 1.0, 0.2), + ); + } + let (plate, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 2.02, 0.0)), + ColliderBuilder::cuboid(3.0, 0.01, 3.0).density(50.0), + ); + let cloth = SoftBodyBuilder::cloth( + Vector::new(-0.9, 3.0, -0.9), + Vector::new(0.12, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.12), + 16, + 16, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .particle_mass(0.02) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)); + let handle = world.insert_soft_body(cloth); + world.soft_bodies[handle].apply_impulse(Vector::new(0.0, -15.0, 0.0), true); + for _ in 0..120 { + world.step(); + } + assert_finite(&world, handle); + let plate_top = world.bodies[plate].translation().y + 0.01; + let min_y = world.soft_bodies[handle] + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + assert!( + min_y > plate_top, + "the cloth tunneled through the plate: {min_y} vs {plate_top}" + ); +} + +/// Inverted elastic cells recover instead of exploding: a corner particle of a stiff cube +/// (E = 1e8) teleported through the cube inverts its cells, which un-invert within a few steps +/// under the capped snap-back. +#[test] +fn inverted_cells_recover() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 3.0, 0.0), Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e8, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .no_surface_collider(); + let handle = world.insert_soft_body(cube); + world.step(); + let sb = &world.soft_bodies[handle]; + let corner = (0..sb.num_particles()) + .min_by(|&a, &b| { + let (pa, pb) = (sb.particle_position(a), sb.particle_position(b)); + (pa.x + pa.y + pa.z) + .partial_cmp(&(pb.x + pb.y + pb.z)) + .unwrap() + }) + .unwrap(); + let p = sb.particle_position(corner); + world.soft_bodies[handle].set_particle_position( + corner, + Vector::new(3.0, 3.0, 0.0) + (Vector::new(0.0, 3.0, 0.0) - p) * 0.6, + ); + let inverted = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[handle]; + sb.cells() + .iter() + .filter(|c| { + let x: [Vector; 4] = + core::array::from_fn(|k| sb.particle_position(c.vertices[k] as usize)); + (x[1] - x[0]).dot((x[2] - x[0]).cross(x[3] - x[0])) < 0.0 + }) + .count() + }; + world.step(); + assert!(inverted(&world) > 0, "the teleport did not invert any cell"); + for _ in 0..60 { + world.step(); + } + assert_finite(&world, handle); + assert_eq!(inverted(&world), 0, "cells still inverted after 1 s"); + let sb = &world.soft_bodies[handle]; + let max_v = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + assert!(max_v < 50.0, "the cube exploded: {max_v} m/s"); +} + +/// A shape that really is in several pieces is meshed as it is: the thickening that keeps a thin +/// limb attached must not run away on a body that can never come out whole. +#[test] +fn volumetric_keeps_genuinely_separate_pieces() { + let (ball, ball_indices) = Ball::new(1.0).to_trimesh(16, 16); + let mut vertices = Vec::new(); + let mut indices = Vec::new(); + + for center in [Vector::new(-3.0, 0.0, 0.0), Vector::new(3.0, 0.0, 0.0)] { + let offset = vertices.len() as u32; + vertices.extend(ball.iter().map(|p| *p + center)); + indices.extend( + ball_indices + .iter() + .map(|t| [t[0] + offset, t[1] + offset, t[2] + offset]), + ); + } + + let builder = SoftBodyBuilder::volumetric(&vertices, &indices, 0.25).expect("two balls"); + let mut world = world_with_ground(); + let handle = world.insert_soft_body(builder); + let sb = &world.soft_bodies[handle]; + + // Two balls of volume 4/3 pi: the pieces are 6 apart and stay two covers, each + // containing its ball with up to a cell-thick shell of overshoot. + let volume = sb.rest_volume(); + let expected = 2.0 * 4.0 / 3.0 * core::f32::consts::PI; + assert!( + volume > expected * 0.95 && volume < expected * 1.9, + "volume {volume} vs {expected}" + ); +} + +/// `SoftBodyBuilder::volumetric` fills a closed mesh with conforming tetrahedral cells: a ball +/// filled with cells a fifth of its radius has about the ball's volume, a closed surface, no +/// inverted cell, and stands on the ground. +#[test] +fn volumetric_ball_is_a_valid_soft_body() { + let (vertices, indices) = Ball::new(1.0).to_trimesh(16, 16); + let vertices: Vec = vertices + .iter() + .map(|p| *p + Vector::new(0.0, 1.5, 0.0)) + .collect(); + let builder = SoftBodyBuilder::volumetric(&vertices, &indices, 0.2) + .expect("the ball is filled with cells") + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e4, + poisson_ratio: 0.3, + ..Default::default() + }) + .particle_mass(0.05); + let mut world = world_with_ground(); + let handle = world.insert_soft_body(builder); + let sb = &world.soft_bodies[handle]; + let volume = sb.rest_volume(); + let ball_volume = 4.0 / 3.0 * core::f32::consts::PI; + // The cover contains the ball, overshooting by up to a cell-thick shell. + assert!( + volume > ball_volume * 0.95 && volume < ball_volume * 1.9, + "volumetric volume {volume} vs ball {ball_volume}" + ); + assert!(sb.collision_mesh().is_some(), "no surface"); + // Half the mean edge of the boundary, whose faces are whole lattice faces (edges 0.2 + // and 0.173 long): the cover cuts nothing. + assert!(sb.particle_radius() > 0.08 && sb.particle_radius() < 0.11); + for c in sb.cells() { + let x: [Vector; 4] = core::array::from_fn(|k| sb.particle_position(c.vertices[k] as usize)); + let v = (x[1] - x[0]).dot((x[2] - x[0]).cross(x[3] - x[0])) / 6.0; + assert!(v > 0.0, "inverted cell (volume {v})"); + } + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let min_y = sb + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + assert!( + min_y > -0.05 && min_y < 0.3, + "the ball sank or floats: {min_y}" + ); + let com = sb.center_of_mass(); + assert!(com.y > 0.8, "the ball collapsed: {com:?}"); +} + +/// `SoftBody::attach_particle`: a cloth hung by two corners from a dynamic bar follows the bar +/// (dropped, then swinging on a hinge) and pulls on it, and detaching (removing the joint) lets +/// it fall. +#[test] +fn attached_cloth_follows_its_body() { + let mut world = world_with_ground(); + // A bar hinged at its left end, above the ground. + let (bar, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(1.0, 3.0, 0.0)), + ColliderBuilder::cuboid(1.0, 0.05, 0.05).density(2.0), + ); + let (pivot, _) = world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, 3.0, 0.0)), + ColliderBuilder::ball(0.05), + ); + world.impulse_joints.insert( + pivot, + bar, + RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::new(-1.0, 0.0, 0.0)), + true, + ); + let n = 12; + let cloth = SoftBodyBuilder::cloth( + Vector::new(0.4, 2.9, -0.06), + Vector::new(0.12, 0.0, 0.0), + Vector::new(0.0, -0.12, 0.0), + n, + n, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.05); + let cloth = world.insert_soft_body(cloth); + // Attach the top corners (particles 0 and (n-1)*n: the top row is j = 0) to the bar. + let corners = [0usize, (n - 1) * n]; + for &i in &corners { + world.soft_bodies[cloth].attach_particle(i, bar, &world.bodies); + } + assert_eq!(world.soft_bodies[cloth].particle_attachments().len(), 2); + // The bar is a pendulum: its lowest point tells whether it swung, its pose at the end is + // phase-dependent. + let mut lowest = Real::MAX; + for _ in 0..180 { + world.step(); + lowest = lowest.min(world.bodies[bar].translation().y); + } + assert_finite(&world, cloth); + // The bar swung down (its free end went below the pivot) and the corners are still on it. + let bar_pos = *world.bodies[bar].position(); + assert!(lowest < 2.5, "bar did not swing: lowest y {lowest}"); + let sb = &world.soft_bodies[cloth]; + for &i in &corners { + let p = sb.particle_position(i); + let local = bar_pos.inverse_transform_point(p); + assert!( + local.y.abs() < 0.35 && local.x.abs() < 1.1, + "corner {i} left the bar: {local:?}" + ); + } + let hanging = sb.center_of_mass().y; + // Detach: the cloth falls to the ground. + for &i in &corners { + assert!(world.soft_bodies[cloth].detach_particle(i)); + } + assert!(world.soft_bodies[cloth].particle_attachments().is_empty()); + for _ in 0..240 { + world.step(); + } + let dropped = world.soft_bodies[cloth].center_of_mass().y; + assert!( + dropped < hanging - 0.5, + "the cloth did not fall once detached: {dropped} vs {hanging}" + ); +} + +/// Plasticity: a jelly block whose top is sheared past the yield keeps part of the lean once +/// released (its rest shape flowed), while an elastic one springs back. +#[test] +fn plastic_cells_keep_their_deformation() { + let lean_after = |plastic_yield: Real| { + let mut world = world_with_ground(); + let block = + SoftBodyBuilder::cuboid(Vector::new(0.0, 0.75, 0.0), Vector::splat(0.75), 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + plastic_yield, + plastic_creep: 100.0, + deformation_damping: 5.0, + ..Default::default() + }) + .particle_mass(0.1) + .surface_collider(ColliderBuilder::ball(0.15).friction(2.0)); + let handle = world.insert_soft_body(block); + // Pin the bottom layer, drag the top layer sideways by 60% of the height for a second, + // then release the top and let everything settle. + let sb = &world.soft_bodies[handle]; + let bottom: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particle_position(i).y < 0.01) + .collect(); + let top: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particle_position(i).y > 1.49) + .collect(); + for &i in bottom.iter().chain(top.iter()) { + world.soft_bodies[handle].set_particle_pinned(i, true); + } + let rest: Vec = top + .iter() + .map(|&i| world.soft_bodies[handle].particle_position(i)) + .collect(); + for step in 0..120 { + let shift = 0.9 * (step as Real / 60.0).min(1.0); + for (&i, p) in top.iter().zip(rest.iter()) { + world.soft_bodies[handle] + .set_particle_kinematic_target(i, *p + Vector::new(shift, 0.0, 0.0)); + } + world.step(); + } + for &i in &top { + world.soft_bodies[handle].set_particle_pinned(i, false); + } + for _ in 0..240 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let top_x: Real = + top.iter().map(|&i| sb.particle_position(i).x).sum::() / top.len() as Real; + let bottom_x: Real = bottom + .iter() + .map(|&i| sb.particle_position(i).x) + .sum::() + / bottom.len() as Real; + top_x - bottom_x + }; + let elastic = lean_after(0.0); + let plastic = lean_after(0.05); + assert!( + elastic.abs() < 0.15, + "the elastic block did not spring back: lean {elastic}" + ); + assert!( + plastic > 0.4, + "the plastic block did not keep its lean: {plastic} (elastic {elastic})" + ); +} + +/// Plastic flow is bounded: a clay slab stamped again and again by a kinematic wedge keeps +/// well-shaped cells (the flow of a repeatedly crushed cell is capped by `plastic_max`, it +/// cannot creep toward a sliver, invert and blow up), keeps its volume and stays calm. +#[test] +fn plastic_flow_is_bounded() { + let mut world = world_with_ground(); + let slab = SoftBodyBuilder::cuboid( + Vector::new(0.0, 0.4, 0.0), + Vector::new(2.4, 0.4, 1.4), + 13, + 3, + 8, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 3.0e4, + poisson_ratio: 0.35, + elastic_damping_ratio: 1.0, + plastic_yield: 0.02, + plastic_creep: 50.0, + deformation_damping: 4.0, + ..Default::default() + }) + .particle_mass(0.1) + .surface_collider(ColliderBuilder::ball(0.15).friction(0.8)); + let handle = world.insert_soft_body(slab); + let press_rest = Vector::new(-1.5, 2.2, 0.0); + let (press, _) = world.insert( + RigidBodyBuilder::kinematic_position_based().translation(press_rest), + ColliderBuilder::cuboid(0.35, 0.35, 0.35).rotation(Vector::new( + 0.0, + 0.0, + core::f32::consts::FRAC_PI_4, + )), + ); + // Four stamps, one per second, at four spots. + let dt = world.integration_parameters.dt; + let mut t: Real = 0.0; + for _ in 0..(12.0 / dt) as usize { + t += dt; + let cycle = (t / 3.0).floor(); + let phase = t - cycle * 3.0; + let depth = if phase < 1.0 { + phase + } else if phase < 2.0 { + 2.0 - phase + } else { + 0.0 + }; + world.bodies[press] + .set_next_kinematic_translation(press_rest + Vector::new(cycle, -depth, 0.0)); + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let mut inverted = 0; + for c in sb.cells() { + let x: [Vector; 4] = core::array::from_fn(|k| sb.particle_position(c.vertices[k] as usize)); + if (x[1] - x[0]).dot((x[2] - x[0]).cross(x[3] - x[0])) < 0.0 { + inverted += 1; + } + } + assert!(inverted <= 2, "{inverted} inverted cells after stamping"); + let volume = sb.volume(); + assert!( + (volume - sb.rest_volume()).abs() < 0.05 * sb.rest_volume(), + "slab volume {volume} vs rest {}", + sb.rest_volume() + ); + let max_speed = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + assert!( + max_speed < 1.0, + "the slab is still agitated: {max_speed} m/s" + ); +} + +/// A body whose cells keep flowing is kept awake: a plastic column creeping under its own +/// weight must not fall asleep mid-creep (its elastic twin, at rest, does sleep). +#[test] +fn creeping_body_stays_awake() { + let mut world = world_with_ground(); + let mut handles = vec![]; + for (i, plastic_yield) in [0.0, 0.05].into_iter().enumerate() { + let column = SoftBodyBuilder::cuboid( + Vector::new(i as Real * 3.0, 1.3, 0.0), + Vector::new(width, 1.2, width), + 3, + 12, + 3, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 1.0, + plastic_yield, + plastic_creep: 0.5, + deformation_damping: 4.0, + ..Default::default() + }) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(1.0)); + handles.push(world.insert_soft_body(column)); + } + let top = |world: &PhysicsWorld, h: SoftBodyHandle| { + world.soft_bodies[h] + .particle_positions() + .map(|p| p.y) + .fold(0.0, Real::max) + }; + for _ in 0..180 { + world.step(); + } + let plastic_top_early = top(&world, handles[1]); + for _ in 0..180 { + world.step(); + } + assert!( + world.soft_bodies[handles[0]].is_sleeping(), + "the elastic column did not sleep" + ); + assert!( + !world.soft_bodies[handles[1]].is_sleeping(), + "the creeping column fell asleep" + ); + let plastic_top = top(&world, handles[1]); + assert!( + plastic_top < plastic_top_early - 0.3, + "the plastic column did not keep creeping: top {plastic_top_early} -> {plastic_top}" + ); + assert!( + top(&world, handles[0]) > 2.2, + "the elastic column sagged: top {}", + top(&world, handles[0]) + ); +} + +/// Shape matching only resists deformation: a shape-matched cloud falls freely (its center of +/// mass follows `g t² / 2`), keeps spinning (only the velocity relative to the rigid motion is +/// damped), and a body driven by a moving target follows it without a speed-dependent lag. +#[test] +fn shape_matching_leaves_rigid_motion_alone() { + let mut positions = Vec::new(); + for i in 0..3 { + for j in 0..3 { + for k in 0..3 { + positions.push(Vector::new(i as Real, j as Real, k as Real) * 0.5); + } + } + } + let builder = || { + SoftBodyBuilder::new(positions.clone()) + .shape_matching(true) + .softness(SpringCoefficients::new(10.0, 1.0)) + .no_surface_collider() + .can_sleep(false) + }; + // Free fall. + let mut world = PhysicsWorld::new(); + let handle = world.insert_soft_body(builder()); + let com0 = world.soft_bodies[handle].center_of_mass(); + for _ in 0..60 { + world.step(); + } + let fallen = com0.y - world.soft_bodies[handle].center_of_mass().y; + let expected = 0.5 * 9.81; + assert!( + (fallen - expected).abs() < 0.1 * expected, + "shape-matched body fell {fallen} m in 1 s instead of {expected}" + ); + // Spin. + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let handle = world.insert_soft_body(builder()); + let com = world.soft_bodies[handle].center_of_mass(); + let omega = 2.0; + let sb = &mut world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + let r = sb.particle_position(i) - com; + sb.set_particle_velocity(i, Vector::new(0.0, omega, 0.0).cross(r)); + } + for _ in 0..120 { + world.step(); + } + let sb = &world.soft_bodies[handle]; + let (mut l, mut inertia) = (0.0, 0.0); + for p in sb.particles() { + let r = p.position() - sb.center_of_mass(); + let v = p.velocity(); + l += r.cross(v).y * p.mass(); + inertia += (r.x * r.x + r.z * r.z) * p.mass(); + } + let omega_after = l / inertia; + assert!( + (omega_after - omega).abs() < 0.1 * omega, + "the spin was damped: {omega_after} rad/s instead of {omega}" + ); + // Driven target moving at 3 m/s. + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let handle = world.insert_soft_body(builder()); + let start = world.soft_bodies[handle].center_of_mass(); + let mut t: Real = 0.0; + for _ in 0..180 { + t += world.integration_parameters.dt; + let target = start + Vector::new(3.0 * t, 0.0, 0.0); + world.soft_bodies[handle] + .cluster_mut(0) + .unwrap() + .set_shape_matching_target(Some(Pose::from_translation(target))); + world.step(); + } + let com = world.soft_bodies[handle].center_of_mass(); + let lag = (start.x + 3.0 * t) - com.x; + assert!( + lag.abs() < 0.15, + "the driven body lags {lag} m behind a target moving at 3 m/s" + ); +} + +/// Impact-adaptive substeps: a soft cube falling fast onto the ground raises its first +/// particle's `additional_solver_iterations` while approaching and hitting, then drops back at +/// rest; a rigid ball dropped on it raises it again (the rigid bodies' approach speed counts). +#[test] +fn fast_impacts_request_extra_substeps() { + let mut world = world_with_ground(); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 30.0, 0.0), Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e4, + poisson_ratio: 0.3, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05)); + let handle = world.insert_soft_body(cube); + let root = world.soft_bodies[handle].root_body(); + let dt = world.integration_parameters.dt; + + let mut max_extra = 0; + let mut extra_before_contact = 0; + for _ in 0..(5.0 / dt) as usize { + world.step(); + let extra = world.bodies[root].additional_solver_iterations(); + max_extra = max_extra.max(extra); + let bottom = world.soft_bodies[handle] + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + // Falling freely, well above the ground: no contact, no request. + if bottom > 3.0 { + extra_before_contact = extra_before_contact.max(extra); + } + } + assert_eq!( + extra_before_contact, 0, + "a free-falling body requested substeps" + ); + assert_eq!( + max_extra, world.integration_parameters.soft_bodies.max_extra_substeps, + "the impact (about 24 m/s: 8 substeps to travel one radius per substep) must request the maximum" + ); + assert_eq!( + world.bodies[root].additional_solver_iterations(), + 0, + "the resting cube must drop back to zero" + ); + assert_finite(&world, handle); + + // A heavy rigid ball dropped from 20 m onto the resting cube. + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 20.0, 0.0)), + ColliderBuilder::ball(0.3).density(2.0), + ); + let mut max_extra = 0; + for _ in 0..(3.0 / dt) as usize { + world.step(); + max_extra = max_extra.max(world.bodies[root].additional_solver_iterations()); + } + assert!( + max_extra >= 2, + "the rigid ball's impact (about 19 m/s) must request extra substeps, got {max_extra}" + ); + for _ in 0..(3.0 / dt) as usize { + world.step(); + } + assert_eq!(world.bodies[root].additional_solver_iterations(), 0); + assert_finite(&world, handle); +} + +/// A fully pinned cloth (every particle kinematic) still collides: a box dropped on it rests on +/// it instead of falling through (a body without a free particle used to be left out of the +/// solver's awake list, so its surface had no contact constraint). +#[test] +fn fully_pinned_cloth_holds_a_box() { + let mut world = world_with_ground(); + let n = 10; + let cloth = SoftBodyBuilder::cloth( + Vector::new(-0.45, 1.0, -0.45), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .pinned_particles(0..(n * n) as u32) + .surface_collider(ColliderBuilder::ball(0.05)); + let handle = world.insert_soft_body(cloth); + let (block, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 2.0, 0.0)), + ColliderBuilder::cuboid(0.15, 0.15, 0.15), + ); + for _ in 0..240 { + world.step(); + } + assert_finite(&world, handle); + // Resting on the surface: 1.0 + half-height 0.15 + the surface's skin (the particle radius). + let y = world.bodies[block].translation().y; + assert!(y > 1.14, "the box fell through the pinned cloth: y = {y}"); + assert!(y < 1.3, "the box did not settle on the cloth: y = {y}"); + let sb = &world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + assert!( + (sb.particle_position(i).y - 1.0).abs() < 1.0e-6, + "a pinned particle moved: {:?}", + sb.particle_position(i) + ); + } +} + +/// The pinned particles moved kinematically (the whole sheet rising) lift the resting box up +/// with them. +#[test] +fn kinematic_pinned_cloth_pushes_a_box() { + let mut world = world_with_ground(); + let n = 10; + let cloth = SoftBodyBuilder::cloth( + Vector::new(-0.45, 1.0, -0.45), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .pinned_particles(0..(n * n) as u32) + .surface_collider(ColliderBuilder::ball(0.05)); + let handle = world.insert_soft_body(cloth); + let (block, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 1.5, 0.0)), + ColliderBuilder::cuboid(0.15, 0.15, 0.15), + ); + for _ in 0..120 { + world.step(); + } + let rested = world.bodies[block].translation().y; + assert!( + rested > 1.14, + "the box fell through the pinned cloth: y = {rested}" + ); + // Raise the sheet at 0.5 m/s for two seconds. + let dt = world.integration_parameters.dt; + let mut sheet_y = 1.0; + for _ in 0..120 { + sheet_y += 0.5 * dt; + let sb = &mut world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + let mut pos = sb.particle_position(i); + pos.y = sheet_y; + sb.set_particle_kinematic_target(i, pos); + } + world.step(); + let y = world.bodies[block].translation().y; + assert!( + y > sheet_y + 0.1, + "the rising sheet passed through the box: box y = {y}, sheet y = {sheet_y}" + ); + } + assert_finite(&world, handle); + let y = world.bodies[block].translation().y; + assert!( + y > sheet_y + 0.14 && y < sheet_y + 0.35, + "the box does not ride the sheet: box y = {y}, sheet y = {sheet_y}" + ); +} + +/// Tearing: a cloth pinned at both ends and pulled apart past the material's tear strain rips +/// (the cracks open by splitting particles, no triangle is lost), the pieces keep simulating and +/// the derived tables stay consistent. +#[test] +fn cloth_tears_when_pulled_apart() { + let mut world = PhysicsWorld::new(); + let (nu, nv) = (12usize, 6usize); + let step = 0.2; + let idx = |i: usize, j: usize| i * nv + j; + let cloth = SoftBodyBuilder::cloth( + Vector::new(0.0, 2.0, 0.0), + Vector::new(step, 0.0, 0.0), + Vector::new(0.0, 0.0, step), + nu, + nv, + ) + .pinned_particles((0..nv).flat_map(|j| [idx(0, j) as u32, idx(nu - 1, j) as u32])) + .tear_strain(0.5) + .particle_mass(0.05); + let handle = world.insert_soft_body(cloth); + let num_surface = world.soft_bodies[handle].boundary().len(); + + // Pull the right column away by one cloth width over two seconds. + let rest: Vec = (0..nv) + .map(|j| world.soft_bodies[handle].particle_position(idx(nu - 1, j))) + .collect(); + let width = step * (nu - 1) as Real; + for k in 0..180 { + let shift = width * (k as Real / 120.0).min(1.0); + for (j, p) in rest.iter().enumerate() { + world.soft_bodies[handle] + .set_particle_kinematic_target(idx(nu - 1, j), *p + Vector::new(shift, 0.0, 0.0)); + } + world.step(); + assert_finite(&world, handle); + world.soft_bodies[handle].validate_topology().unwrap(); + } + let sb = &world.soft_bodies[handle]; + assert_eq!( + sb.boundary().len(), + num_surface, + "a cloth loses no triangle when it tears" + assert!( + sb.num_particles() > nu * nv, + "the crack did not split any particle" + ); + // The two pinned columns are now more than twice the rest width apart: what still ties + // them would be strained way past the threshold, so nothing does. The middle sagged under + // gravity instead of being held taut. + let mid = (0..nv) + .map(|j| sb.particle_position(idx(nu / 2, j)).y) + .fold(Real::MAX, Real::min); + assert!(mid < 1.9, "the torn cloth did not sag: {mid}"); + for e in sb.edges() { + let len = (sb.particle_position(e.vertices[0] as usize) + - sb.particle_position(e.vertices[1] as usize)) + .length(); + assert!( + len < e.rest_length * 1.6, + "an edge past the tear strain survived: {len} vs rest {}", + e.rest_length + ); + } +} + +/// A cloth crack opens across the torn edges: tearing every warp edge between two columns +/// splits one endpoint per edge along the plane perpendicular to it, cutting the cloth in two +/// with no triangle lost and the mass conserved; the unpinned piece falls away. +#[test] +fn tearing_splits_the_particles_a_crack_passes_through() { + let mut world = PhysicsWorld::new(); + let n = 6usize; + let idx = |i: usize, j: usize| (i * n + j) as u32; + let cloth = SoftBodyBuilder::cloth( + Vector::new(0.0, 2.0, 0.0), + Vector::new(0.2, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.2), + n, + n, + ) + .pinned_particles([idx(0, 0), idx(0, n - 1)]); + let handle = world.insert_soft_body(cloth); + world.step(); + let sb = &world.soft_bodies[handle]; + let num_particles = sb.num_particles(); + let num_surface = sb.boundary().len(); + // The warp edges between the columns `col` and `col + 1`. + let col = n / 2; + let warp: Vec = sb + .edges() + .iter() + .enumerate() + .filter(|(_, e)| { + (0..n).any(|j| { + let (a, b) = (idx(col, j), idx(col + 1, j)); + e.vertices == [a, b] || e.vertices == [b, a] + }) + }) + .map(|(i, _)| i as u32) + .collect(); + assert_eq!(warp.len(), n); + let torn = world.tear_soft_body(handle, &warp, &[]); + assert!(torn.is_some()); + let sb = &world.soft_bodies[handle]; + sb.validate_topology().unwrap(); + // One split per torn edge (its first endpoint, `(col, j)`), the triangles all still there. + assert_eq!(sb.num_particles(), num_particles + n); + assert_eq!(sb.boundary().len(), num_surface); + let mass: Real = sb.particles().iter().map(|p| p.mass()).sum(); + assert!( + (mass - num_particles as Real).abs() < 1.0e-4, + "mass changed: {mass}" + ); + // The left piece hangs from its pinned corners, the right one falls: the split particles + // drift apart. + for _ in 0..120 { + world.step(); + assert_finite(&world, handle); + } + let sb = &world.soft_bodies[handle]; + sb.validate_topology().unwrap(); + // The copies are appended in crack order: the copy of `(col, j)` is particle + // `num_particles + j`; the event says where each ended up. + let copy = num_particles + n / 2; + let gap = + (sb.particle_position(copy) - sb.particle_position(idx(col, n / 2) as usize)).length(); + assert!(gap > 0.05, "the split particles did not separate: {gap}"); +} + +/// A corotational jelly bar pulled apart past its tear strain splits: cracks open through its +/// particles (no cell is lost), the crack faces become surface, the tables stay consistent, and +/// both halves keep simulating. +#[test] +fn jelly_bar_tears_when_pulled_apart() { + let mut world = PhysicsWorld::new(); + let (nx, ny, nz) = (13usize, 3usize, 3usize); + let bar = SoftBodyBuilder::cuboid( + Vector::new(0.0, 2.0, 0.0), + Vector::new(1.2, 0.2, 0.2), + nx, + ny, + nz, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e3, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + tear_strain: Some(0.4), + ..Default::default() + }) + .particle_mass(0.05); + let handle = world.insert_soft_body(bar); + let sb = &world.soft_bodies[handle]; + let num_cells = sb.cells().len(); + let left: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particle_position(i).x < -1.19) + .collect(); + let right: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particle_position(i).x > 1.19) + .collect(); + assert_eq!(left.len(), ny * nz); + for &i in left.iter().chain(right.iter()) { + world.soft_bodies[handle].set_particle_pinned(i, true); + } + let rest: Vec = right + .iter() + .map(|&i| world.soft_bodies[handle].particle_position(i)) + .collect(); + for k in 0..240 { + let shift = 2.4 * (k as Real / 180.0).min(1.0); + for (&i, p) in right.iter().zip(rest.iter()) { + world.soft_bodies[handle] + .set_particle_kinematic_target(i, *p + Vector::new(shift, 0.0, 0.0)); + } + world.step(); + assert_finite(&world, handle); + world.soft_bodies[handle].validate_topology().unwrap(); + } + let sb = &world.soft_bodies[handle]; + assert!( + sb.cells().len() < num_cells, + "no cell tore ({num_cells} before and after)" + ); + assert!(!sb.boundary().is_empty()); + for c in sb.cells() { + for &v in &c.vertices { + assert!((v as usize) < sb.num_particles()); + } + } + // The bar is now torn: no remaining cell is stretched past twice the tear strain. + let mut max_stretch: Real = 0.0; + for c in sb.cells() { + for a in 0..4 { + for b in a + 1..4 { + let (pa, pb) = ( + sb.particle_position(c.vertices[a] as usize), + sb.particle_position(c.vertices[b] as usize), + ); + let rest = (sb.particles()[c.vertices[a] as usize].rest_position() + - sb.particles()[c.vertices[b] as usize].rest_position()) + .length(); + max_stretch = max_stretch.max((pa - pb).length() / rest); + } + } + assert!( + max_stretch < 1.8, + "a cell is still stretched by {max_stretch}: the bar did not split" + ); +} + +/// A torn soft body (split particles included) round-trips through serde. +#[cfg(feature = "serde-serialize")] +#[test] +fn torn_soft_bodies_serialize() { + let mut world = world_with_ground(); + let n = 5usize; + let idx = |i: usize, j: usize| (i * n + j) as u32; + let cloth = SoftBodyBuilder::cloth( + Vector::new(0.0, 1.0, 0.0), + Vector::new(0.2, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.2), + n, + n, + ) + .pinned_particles([idx(0, 0), idx(0, n - 1)]) + .tear_strain(0.5); + let handle = world.insert_soft_body(cloth); + world.step(); + let diagonal: Vec = world.soft_bodies[handle] + .edges() + .iter() + .enumerate() + .filter(|(_, e)| { + (0..n - 1).any(|k| { + let (a, b) = (idx(k, k), idx(k + 1, k + 1)); + e.vertices == [a, b] || e.vertices == [b, a] + }) + }) + .map(|(i, _)| i as u32) + .collect(); + for _ in 0..30 { + world.step(); + } + let bytes = bincode::serialize(&world).unwrap(); + let mut restored: PhysicsWorld = bincode::deserialize(&bytes).unwrap(); + restored.soft_bodies[handle].validate_topology().unwrap(); + assert_eq!( + restored.soft_bodies[handle].num_particles(), + world.soft_bodies[handle].num_particles() + ); + for _ in 0..30 { + world.step(); + restored.step(); + } + let last = world.soft_bodies[handle].num_particles() - 1; + let a = world.soft_bodies[handle].particle_position(last); + let b = restored.soft_bodies[handle].particle_position(last); + assert!( + (a - b).length() < 1.0e-3, + "restored world diverged: {a:?} vs {b:?}" + ); +} + +/// Load-adaptive substeps: a soft cube supporting a box many times its own weight requests +/// extra substeps for its island (the solve is under-converged under a heavy load), and drops +/// the request once the box is removed. +#[test] +fn heavy_load_requests_extra_substeps() { + let mut world = world_with_ground(); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 0.5, 0.0), Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .particle_mass(0.05) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05)); + let handle = world.insert_soft_body(cube); + let root = world.soft_bodies[handle].root_body(); + let dt = world.integration_parameters.dt; + for _ in 0..(2.0 / dt) as usize { + world.step(); + } + assert_eq!( + world.bodies[root].additional_solver_iterations(), + 0, + "a cube under its own weight only must not request substeps" + ); + // A 100 kg box (the cube weighs 1.35 kg) set down on it. + let (rb, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 1.5, 0.0)), + ColliderBuilder::cuboid(0.4, 0.4, 0.4).density(100.0 / 0.512), + ); + let mut max_extra = 0; + for _ in 0..(3.0 / dt) as usize { + world.step(); + max_extra = max_extra.max(world.bodies[root].additional_solver_iterations()); + } + assert!( + max_extra >= 1, + "the loaded cube must request extra substeps (got {max_extra})" + ); + assert!( + world.bodies[root].additional_solver_iterations() >= 1, + "the request must hold while the load stays" + ); + assert_finite(&world, handle); + world.remove_body(rb); + for _ in 0..(2.0 / dt) as usize { + world.step(); + } + assert_eq!(world.bodies[root].additional_solver_iterations(), 0); +} + +/// A cloth thrown at 15 m/s onto a resting balloon (a pressurized soft sphere) neither tunnels +/// through it nor crushes it flat: the speculative constraints and impact-adaptive substeps +/// hold, and the balloon keeps more than half of its volume at every step. +#[test] +fn fast_cloth_does_not_crush_a_balloon() { + let mut world = world_with_ground(); + let balloon = SoftBodyBuilder::sphere(Vector::new(0.0, 1.0, 0.0), 1.0, 2) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.05) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)); + let balloon = world.insert_soft_body(balloon); + for _ in 0..120 { + world.step(); + } + let rest = world.soft_bodies[balloon].rest_volume(); + let cloth = SoftBodyBuilder::cloth( + Vector::new(-0.9, 3.0, -0.9), + Vector::new(0.12, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.12), + 16, + 16, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .particle_mass(0.02) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)); + let cloth = world.insert_soft_body(cloth); + world.soft_bodies[cloth].apply_impulse(Vector::new(0.0, -15.0, 0.0), true); + let mut min_ratio: Real = 1.0; + for _ in 0..180 { + world.step(); + min_ratio = min_ratio.min(world.soft_bodies[balloon].volume() / rest); + } + assert_finite(&world, balloon); + assert_finite(&world, cloth); + assert!( + min_ratio > 0.5, + "the balloon was crushed to {min_ratio} of its volume" + ); + // No cloth particle ended up inside the balloon. + let com = world.soft_bodies[balloon].center_of_mass(); + let inside = world.soft_bodies[cloth] + .particle_positions() + .filter(|p| (*p - com).length() < 0.8) + .count(); + assert_eq!( + inside, 0, + "{inside} cloth particles ended up inside the balloon" + ); +} + +/// `SoftBodyBuilder::append` shifts the appended piece's indices (elements, pins, per-edge +/// overrides) past the first one's particles, and `add_edges` sews them: two cloth squares +/// appended and stitched along a shared edge hang from the first one's pins as one body. +#[test] +fn appended_pieces_are_one_soft_body() { + let mut world = PhysicsWorld::new(); + let n = 6usize; + let step = 0.1; + let a = SoftBodyBuilder::cloth( + Vector::new(0.0, 2.0, 0.0), + Vector::new(step, 0.0, 0.0), + Vector::new(0.0, 0.0, step), + n, + n, + ) + .pinned_particles([0, (n - 1) as u32]); + let b = SoftBodyBuilder::cloth( + Vector::new(0.0, 2.0, step * n as Real), + Vector::new(step, 0.0, 0.0), + Vector::new(0.0, 0.0, step), + n, + n, + ) + .tension_only(); + let (na, nb) = (a.particle_positions().len(), b.particle_positions().len()); + let (ea, eb) = (a.edges.len(), b.edges.len()); + let bends_a = a.bend_edges.len(); + let tension_b = b.tension_only_edges.len(); + // Seams: the last row of `a` (j = n - 1) to the first row of `b` (j = 0). + let seams: Vec<[u32; 2]> = (0..n) + .map(|i| [(i * n + n - 1) as u32, (na + i * n) as u32]) + .collect(); + let stitched = a.append(b).add_edges(seams.clone()); + assert_eq!(stitched.particle_positions().len(), na + nb); + assert_eq!(stitched.pinned, vec![0, (n - 1) as u32]); + assert_eq!(stitched.edges.len(), ea + eb + seams.len()); + // The appended piece's tension-only marks point at its own edges and bending edges. + assert_eq!(stitched.tension_only_edges.len(), tension_b); + for &i in &stitched.tension_only_edges { + let i = i as usize; + let edge = if i < stitched.edges.len() { + stitched.edges[i] + } else { + stitched.bend_edges[i - stitched.edges.len()] + }; + assert!( + edge.iter().all(|&v| v as usize >= na), + "override on a's edge {edge:?}" + ); + } + assert_eq!(stitched.bend_edges.len(), bends_a + tension_b - eb); + let handle = world.insert_soft_body(stitched.particle_mass(0.05)); + world.soft_bodies[handle].validate_topology().unwrap(); + for _ in 0..240 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + // The second square hangs below the first, held through the seams (not on the ground: no + // ground here, so it must simply not have fallen away). + let lowest = sb + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + assert!( + lowest > 0.9, + "the appended piece fell away: lowest y {lowest}" + ); + let seam_gap = seams + .iter() + .map(|s| { + (sb.particle_position(s[0] as usize) - sb.particle_position(s[1] as usize)).length() + }) + .fold(0.0, Real::max); + assert!(seam_gap < 0.2, "a seam opened: {seam_gap}"); +} + +/// A soft body given a non-finite particle (by the user, or by a non-finite force during a step) +/// is quarantined: disabled, reported, its velocities zeroed, the rest of the world unharmed; +/// fixing the particle and re-enabling the body brings it back. +#[test] +fn non_finite_soft_body_is_quarantined() { + let mut world = world_with_ground(); + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 0.5, 3.0)), + ColliderBuilder::ball(0.5), + ); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 0.5, 0.0), Vector::splat(0.5), 3, 3, 3) + .softness(SpringCoefficients::new(30.0, 1.0)); + let handle = world.insert_soft_body(cube); + let other = world.insert_soft_body(SoftBodyBuilder::cuboid( + Vector::new(3.0, 0.5, 0.0), + Vector::splat(0.5), + 3, + 3, + 3, + )); + for _ in 0..10 { + world.step(); + } + // A NaN position set by the user. + world.soft_bodies[handle].set_particle_position(0, Vector::new(Real::NAN, 0.0, 0.0)); + world.step(); + assert_eq!(world.physics_pipeline.quarantine().soft_bodies(), &[handle]); + assert!(!world.soft_bodies[handle].is_enabled()); + assert!(!world.bodies[world.soft_bodies[handle].root_body()].is_enabled()); + for _ in 0..30 { + world.step(); + assert!(world.physics_pipeline.quarantine().is_empty()); + assert!(world.bodies[ball].translation().is_finite()); + for p in world.soft_bodies[other].particles() { + assert!(p.position().is_finite() && p.velocity().is_finite()); + } + } + // Repaired and re-enabled: it simulates again. + world.soft_bodies[handle].set_particle_position(0, Vector::new(0.0, 0.5, 0.0)); + world.soft_bodies[handle].set_enabled(true); + let before = world.soft_bodies[handle].center_of_mass(); + for _ in 0..30 { + world.step(); + assert!(world.physics_pipeline.quarantine().is_empty()); + } + assert!(world.soft_bodies[handle].is_enabled()); + assert!(world.bodies[world.soft_bodies[handle].root_body()].is_enabled()); + let sb = &world.soft_bodies[handle]; + assert!(sb.particles().iter().all(|p| p.position().is_finite())); + assert!( + (sb.center_of_mass() - before).length() > 1.0e-3, + "the re-enabled body did not move" + ); + + // A non-finite force during a step: quarantined at the end of the step. + world.soft_bodies[other].add_particle_force(0, Vector::new(0.0, Real::INFINITY, 0.0), true); + world.step(); + assert_eq!(world.physics_pipeline.quarantine().soft_bodies(), &[other]); + assert!(!world.soft_bodies[other].is_enabled()); + for _ in 0..10 { + world.step(); + assert!(world.physics_pipeline.quarantine().is_empty()); + assert!(world.bodies[ball].translation().is_finite()); + } +} + +/// A disabled soft body is left where it is (no constraints, no contacts) and comes back when enabled. +#[test] +fn disabled_soft_body_is_left_alone() { + let mut world = world_with_ground(); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 2.0, 0.0), Vector::splat(0.5), 3, 3, 3); + let handle = world.insert_soft_body(cube); + world.soft_bodies[handle].set_enabled(false); + for _ in 0..60 { + world.step(); + } + let com = world.soft_bodies[handle].center_of_mass(); + assert!( + (com.y - 2.0).abs() < 1.0e-6, + "the disabled body moved: {com:?}" + ); + // A ball falls through the place of the disabled body's surface. + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 4.0, 0.0)), + ColliderBuilder::ball(0.2), + ); + for _ in 0..120 { + world.step(); + } + assert!(world.bodies[ball].translation().y < 1.0); + world.remove_body(ball); + world.soft_bodies[handle].set_enabled(true); + for _ in 0..120 { + world.step(); + } + let com = world.soft_bodies[handle].center_of_mass(); + assert!(com.y < 1.0, "the re-enabled body did not fall: {com:?}"); +} + +/// `SoftBodyBuilder::trimesh`: a mesh's vertices become the particles, its edges springs, its +/// interior edges dihedrals, and shape matching keeps the model's shape once dropped. +#[test] +fn trimesh_soft_body_keeps_its_shape() { + let mut world = world_with_ground(); + let (vertices, indices) = Cuboid::new(Vector::new(0.5, 0.4, 0.3)).to_trimesh(); + let vertices: Vec = vertices + .into_iter() + .map(|v| v + Vector::new(0.0, 2.0, 0.0)) + .collect(); + let cube = SoftBodyBuilder::trimesh(vertices, indices) + .unwrap() + .material(SoftBodyMaterial { + shape_matching_softness: SpringCoefficients::new(30.0, 1.0), + ..Default::default() + }) + .particle_mass(0.1); + let handle = world.insert_soft_body(cube); + let sb = &world.soft_bodies[handle]; + assert!(sb.cluster(0).unwrap().shape_matching_enabled()); + assert_eq!(sb.boundary().len(), 12); + assert_eq!(sb.edges().len(), 18); + assert_eq!(sb.dihedrals().len(), 18); + assert!(sb.cells().is_empty()); + assert!(SoftBodyBuilder::trimesh(Vec::new(), Vec::new()).is_none()); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let pose = world.bodies[sb.cluster_proxy(0).unwrap()].position(); + let max_dev = sb + .particles() + .iter() + .map(|p| (pose * p.rest_position() - p.position()).length()) + .fold(0.0, Real::max); + assert!(max_dev < 0.05, "the box lost its shape: {max_dev}"); + let com = sb.center_of_mass(); + assert!( + (com.y - 0.4 - sb.particle_radius()).abs() < 0.1, + "the box did not rest on the ground: {com:?}" + ); +} + +/// A skin is a linear map of the cell it rides: whatever affine motion the cell undergoes, the +/// skin undergoes the same one, for a vertex the cell does not cover just as much as for one +/// inside it. +#[test] +fn skin_follows_the_cells() { + // A single cell, and a skin with one vertex inside it, one on a face, one well outside it. + let cell = [Vector::ZERO, Vector::X, Vector::Y, Vector::Z]; + let skin_vertices = vec![ + Vector::new(0.2, 0.2, 0.2), + Vector::new(0.5, 0.5, 0.0), + Vector::new(1.5, 1.5, 1.5), + ]; + let builder = SoftBodyBuilder::new(cell.to_vec()) + .cells(vec![[0, 1, 2, 3]]) + .skin(skin_vertices.clone(), vec![[0, 1, 2]]) + .particle_mass(1.0); + + let mut world = world_with_ground(); + let handle = world.insert_soft_body(builder); + let sb = &world.soft_bodies[handle]; + let skin = skinned_mesh(sb); + + // Bound at rest: the skin sits exactly where it was given. + for (bound, rest) in skin.vertices().iter().zip(&skin_vertices) { + assert!((*bound - *rest).length() < 1.0e-5, "{bound:?} vs {rest:?}"); + } + + // Move the particles by an affine map, then let the body settle wherever it settles. + let rotation = Rotation::from_axis_angle(Vector::Z, 0.7); + let sb = &mut world.soft_bodies[handle]; + for (i, p) in cell.iter().enumerate() { + sb.set_particle_position( + i, + rotation * Vector::new(p.x * 2.0, p.y * 0.5, p.z) + Vector::new(3.0, -1.0, 2.0), + ); + } + + for _ in 0..5 { + world.step(); + + // The affine map the cell actually underwent, recovered from the particles themselves. + let sb = &world.soft_bodies[handle]; + let moved: Vec = sb.particle_positions().collect(); + let rest_frame = Matrix::from_cols(cell[1] - cell[0], cell[2] - cell[0], cell[3] - cell[0]); + let frame = Matrix::from_cols( + moved[1] - moved[0], + moved[2] - moved[0], + moved[3] - moved[0], + ); + let map = frame * rest_frame.inverse(); + + for (skinned, rest) in skinned_mesh(sb).vertices().iter().zip(&skin_vertices) { + let expected = map * (*rest - cell[0]) + moved[0]; + assert!( + (*skinned - expected).length() < 1.0e-4, + "skin vertex at {skinned:?}, expected {expected:?}" + ); + } + } +} + +/// `SoftBodyBuilder::volumetric_skinned` keeps the input mesh as the skin, whatever the cells +/// could resolve of it. +#[test] +fn volumetric_skinned_keeps_the_input_mesh() { + let (vertices, indices) = Ball::new(1.0).to_trimesh(24, 24); + let vertices: Vec = vertices + .iter() + .map(|p| *p + Vector::new(0.0, 3.0, 0.0)) + .collect(); + + let builder = SoftBodyBuilder::volumetric_skinned(&vertices, &indices, 0.4) + .expect("the ball is filled") + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e4, + poisson_ratio: 0.3, + ..Default::default() + }) + .particle_mass(0.05); + + let mut world = world_with_ground(); + let handle = world.insert_soft_body(builder); + let sb = &world.soft_bodies[handle]; + let skin = skinned_mesh(sb); + + assert_eq!(skin.vertices().len(), vertices.len()); + assert_eq!(skin.indices().len(), indices.len()); + // The cells are far coarser than the mesh they hold. + assert!( + sb.num_particles() < vertices.len(), + "{} particles for {} skin vertices", + sb.num_particles(), + vertices.len() + ); + + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + + // The skin came down with the cells, and is still a ball-sized mesh. + let sb = &world.soft_bodies[handle]; + let skin = skinned_mesh(sb); + let center = skin.vertices().iter().copied().sum::() / skin.vertices().len() as Real; + let radius = skin + .vertices() + .iter() + .map(|v| (*v - center).length()) + .fold(0.0, Real::max); + assert!(skin.vertices().iter().all(|v| v.is_finite())); + assert!(radius > 0.5 && radius < 1.6, "skin radius {radius}"); + assert!( + center.y < 2.0, + "the skin did not follow the fall: {center:?}" + ); +} + +/// The debug-renderer draws the cage of a skinned body: the cells are what is simulated, and are +/// otherwise invisible under the skin. +#[cfg(feature = "debug-render")] +#[test] +fn debug_renderer_draws_the_cage() { + use rapier3d::pipeline::{ + DebugColor, DebugRenderBackend, DebugRenderMode, DebugRenderObject, DebugRenderPipeline, + DebugRenderStyle, + }; + + #[derive(Default)] + struct CountLines { + soft_body_lines: usize, + min: Vector, + max: Vector, + } + + impl DebugRenderBackend for CountLines { + fn draw_line(&mut self, object: DebugRenderObject, a: Vector, b: Vector, _: DebugColor) { + if matches!(object, DebugRenderObject::SoftBody(..)) { + self.soft_body_lines += 1; + self.min = self.min.min(a).min(b); + self.max = self.max.max(a).max(b); + } + } + } + + let (vertices, indices) = Ball::new(1.0).to_trimesh(16, 16); + let vertices: Vec = vertices + .iter() + .map(|p| *p + Vector::new(0.0, 3.0, 0.0)) + .collect(); + let builder = SoftBodyBuilder::volumetric_skinned(&vertices, &indices, 0.4) + .expect("the ball is filled") + .cell_model(SoftBodyCellModel::Corotational) + .particle_mass(0.05); + + let mut world = world_with_ground(); + let handle = world.insert_soft_body(builder); + world.step(); + + let mut backend = CountLines { + min: Vector::splat(Real::MAX), + max: Vector::splat(-Real::MAX), + ..Default::default() + }; + let mut pipeline = + DebugRenderPipeline::new(DebugRenderStyle::default(), DebugRenderMode::SOFT_BODIES); + pipeline.render( + &mut backend, + &world.bodies, + &world.colliders, + &world.impulse_joints, + &world.multibody_joints, + &world.narrow_phase, + &world.soft_bodies, + ); + + // Every cage edge once, however many cells share it. + let sb = &world.soft_bodies[handle]; + let cells = sb.cells().len(); + let mut edges = std::collections::HashSet::new(); + for cell in sb.cells() { + for a in 0..4 { + for b in a + 1..4 { + let (x, y) = (cell.vertices[a], cell.vertices[b]); + edges.insert([x.min(y), x.max(y)]); + } + } + } + assert!(cells > 0); + assert_eq!(backend.soft_body_lines, edges.len()); + assert!( + edges.len() * 3 < cells * 6, + "{} edges vs {} cell sides: the cage was not deduplicated", + edges.len(), + cells * 6 + ); + + // And it is drawn where the body is, not at the origin. + assert!( + backend.min.y > 1.0 && backend.max.y < 5.0, + "the cage was drawn at {:?}..{:?}", + backend.min, + backend.max + ); +} + +/// A skinned body rests on its skin, not on the cells holding it: the cage is dilated and +/// coarse, so a body colliding through it would hover above the ground. +#[test] +fn skin_contacts_rest_on_the_skin() { + let settle = |skin_collision: bool| -> (Real, Real) { + let (vertices, indices) = Ball::new(1.0).to_trimesh(24, 24); + let vertices: Vec = vertices + .iter() + .map(|p| *p + Vector::new(0.0, 2.0, 0.0)) + .collect(); + let builder = SoftBodyBuilder::volumetric_skinned(&vertices, &indices, 0.7) + .expect("the ball is filled") + .skin_collision(skin_collision) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e5, + poisson_ratio: 0.3, + ..Default::default() + }) + .particle_mass(0.05) + .particle_radius(0.02); + + let mut world = world_with_ground(); + let handle = world.insert_soft_body(builder); + for _ in 0..400 { + world.step(); + } + assert_finite(&world, handle); + + let sb = &world.soft_bodies[handle]; + let skin = skinned_mesh(sb); + let lowest_skin = skin + .vertices() + .iter() + .map(|v| v.y) + .fold(Real::MAX, Real::min); + let lowest_cell = sb + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + (lowest_skin, lowest_cell) + }; + + // Colliding through the skin: the skin is what touches the ground. + let (skin_y, cell_y) = settle(true); + assert!( + skin_y > -0.02 && skin_y < 0.15, + "the skin should rest on the ground, lowest vertex at {skin_y}" + ); + + // Colliding through the cells: the cells are what touches the ground, and the cover's + // cage contains the ball (it circumscribes the curvature), so the model hovers above + // the floor by however much the cage overshoots. + let (cage_skin_y, cage_cell_y) = settle(false); + assert!( + cage_skin_y > 0.02, + "colliding through the cells should keep the model off the floor, at {cage_skin_y}" + ); + assert!( + skin_y < cage_skin_y - 0.02, + "the skin sits at {skin_y} on itself against {cage_skin_y} on the cells" + ); + // Either way the body came down and did not sink. Colliding through the skin, the + // cage has no collider and hangs below the resting skin by its overshoot (up to a + // 0.7 cell), which is not sinking. + for y in [skin_y, cage_skin_y, cage_cell_y] { + assert!(y > -0.3, "something sank through the ground: {y}"); + } + assert!(cell_y > -1.0, "the cage fell through the ground: {cell_y}"); +} + +/// A soft body meets another soft body on its skin: `soft_sphere_rests_on_soft_cube` again, with +/// the cube wearing its own boundary as a skin and colliding through it. The vertex-vs-surface +/// pass has to read the mesh each side collides through for the sphere to find the cube at all. +#[test] +fn skinned_bodies_collide_with_each_other() { + let settle = |skinned: bool| -> (Real, Real) { + let half = Vector::splat(0.6); + let center = Vector::new(0.0, 0.6, 0.0); + let mut cube = SoftBodyBuilder::cuboid(center, half, 5, 5, 5) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.2); + if skinned { + // The lattice fills the box exactly, so its own boundary is the skin. + let (vertices, indices) = Cuboid::new(half).to_trimesh(); + let vertices: Vec = vertices.iter().map(|p| *p + center).collect(); + cube = cube.skin(vertices, indices); + } + let sphere = SoftBodyBuilder::sphere(Vector::new(0.0, 2.2, 0.0), 0.5, 2) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.05); + + let mut world = world_with_ground(); + let cube = world.insert_soft_body(cube); + let sphere = world.insert_soft_body(sphere); + assert_eq!( + world.soft_bodies[cube] + .meshes() + .any(|m| m.is_skinned()), + skinned + ); + for _ in 0..400 { + world.step(); + } + assert_finite(&world, cube); + assert_finite(&world, sphere); + + let sphere_bottom = world.soft_bodies[sphere] + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + let cube_top = world.soft_bodies[cube] + .particle_positions() + .map(|p| p.y) + .fold(-Real::MAX, Real::max); + (sphere_bottom, cube_top) + }; + + let (plain_bottom, plain_top) = settle(false); + let (skinned_bottom, skinned_top) = settle(true); + + // The sphere rests on the cube either way, and the skin puts it in the same place as the + // cells do, the two meshes being the same box. + for (bottom, top) in [(plain_bottom, plain_top), (skinned_bottom, skinned_top)] { + assert!(bottom > 0.9, "the sphere fell through the cube to {bottom}"); + assert!( + bottom < top + 0.4, + "the sphere floats at {bottom} over a cube topped at {top}" + ); + } + assert!( + (skinned_bottom - plain_bottom).abs() < 0.15, + "the sphere rests at {skinned_bottom} on the skin against {plain_bottom} on the cells" + ); +} + +/// Reproducer for a known bug (see `docs/soft-soft-overlap-bug.md`): two soft bodies inserted +/// with overlapping contact skins are thrown apart instead of settling. Ignored because it +/// fails (the per-pair de-overlap budget that fixed it lives on the experimental branch). +#[test] +#[ignore = "known bug: soft-vs-soft bodies inserted within each other's contact skins explode"] +fn soft_bodies_inserted_overlapping_settle() { + let cube = |y: Real| { + SoftBodyBuilder::cuboid(Vector::new(0.0, y, 0.0), Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + poisson_ratio: 0.3, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.2) + }; + + // The two particle radii add up to 0.5, so anything below that starts inside the skins. + for gap in [0.1, 0.3, 0.7] { + let mut world = world_with_ground(); + let lower = world.insert_soft_body(cube(0.55)); + let upper = world.insert_soft_body(cube(0.55 + 1.0 + gap)); + let start = world.soft_bodies[upper] + .particle_positions() + .map(|p| p.y) + .fold(-Real::MAX, Real::max); + let mut peak: Real = 0.0; + + for _ in 0..400 { + world.step(); + peak = peak.max( + world.soft_bodies[upper] + .particle_positions() + .map(|p| p.y) + .fold(-Real::MAX, Real::max), + ); + } + + let top = world.soft_bodies[lower] + .particle_positions() + .map(|p| p.y) + .fold(-Real::MAX, Real::max); + let bottom = world.soft_bodies[upper] + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + // Settling out of the authored overlaps necessarily lifts the top: up to 0.4 of + // pair overlap plus 0.2 of ground-skin overlap. The bound leaves 0.15 of dynamic + // overshoot on top of that. + assert!( + peak < start + 0.75, + "gap {gap}: the upper cube was thrown from {start} up to {peak}" + ); + assert!( + bottom > top - 0.25, + "gap {gap}: the upper cube ended at {bottom}, under a top at {top}" + ); + } +} + +/// `SoftBodyBuilder::volumetric_with` prices a fine boundary against a coarse interior: the +/// cover's subdivision refines the boundary cells only, so the same boundary resolution costs +/// fewer cells than a uniform fill, and the body still behaves. +#[test] +fn volumetric_subdivision_costs_fewer_cells() { + use rapier3d::parry::transformation::VolumeMeshParameters; + + let (vertices, indices) = Ball::new(1.0).to_trimesh(30, 30); + let vertices: Vec = vertices + .iter() + .map(|p| *p + Vector::new(0.0, 2.0, 0.0)) + .collect(); + + let build = |cell_size: Real, subdivisions: u32| { + let mut params = VolumeMeshParameters::new(cell_size); + params.cover_subdivisions = subdivisions; + SoftBodyBuilder::volumetric_with(&vertices, &indices, ¶ms) + .expect("the ball is filled") + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e4, + poisson_ratio: 0.3, + ..Default::default() + }) + .particle_mass(0.05) + }; + + // The same 0.12 boundary, once as a uniform fill and once as a coarse fill whose + // boundary is subdivided down to it. + let uniform = build(0.12, 0); + let subdivided = build(0.24, 1); + assert!( + subdivided.cells.len() * 3 < uniform.cells.len() * 2, + "subdividing saved little: {} cells against {}", + subdivided.cells.len(), + uniform.cells.len() + ); + + // The boundary is meshed just as finely either way, so the two bodies are the same size. + let mut world = world_with_ground(); + let handles = [ + world.insert_soft_body(uniform), + world.insert_soft_body(subdivided), + ]; + let volumes: Vec = handles + .iter() + .map(|h| world.soft_bodies[*h].rest_volume()) + .collect(); + let ball = 4.0 / 3.0 * core::f32::consts::PI; + for volume in &volumes { + // The cover overshoots the ball by up to a boundary cell. + assert!( + *volume > ball * 0.95 && *volume < ball * 1.9, + "volume {volume} vs {ball}" + ); + } + + for _ in 0..300 { + world.step(); + } + for handle in handles { + assert_finite(&world, handle); + let lowest = world.soft_bodies[handle] + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + assert!(lowest > -0.1 && lowest < 0.6, "the ball ended at {lowest}"); + } +} diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 14e561083..e8afed865 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -55,6 +55,17 @@ mod s2d_high_mass_ratio_3; mod s2d_joint_grid; mod s2d_pyramid; mod sensor2; +mod soft_blobs2; +mod soft_bodies2; +mod soft_jelly2; +// The letters come from a tessellated SVG (usvg), which doesn't build for wasm. +#[cfg(not(target_arch = "wasm32"))] +mod soft_letters2; +mod soft_pile2; +mod soft_plasticity2; +mod soft_surface2; +mod soft_tearing2; +mod soft_thin_features2; mod stress_tests; // Tessellates an SVG with usvg, which doesn't build for wasm. #[cfg(not(target_arch = "wasm32"))] @@ -87,6 +98,7 @@ pub async fn main() { const DYNAMICS: &str = "Dynamics"; const JOINTS: &str = "Joints"; const CONTROLS: &str = "Controls"; + const SOFT: &str = "Soft bodies"; const DEBUG: &str = "Debug"; const S2D: &str = "Inspired by Solver 2D"; const STRESS: &str = "Stress tests"; @@ -122,6 +134,17 @@ pub async fn main() { JOINTS, "Joint motor position", joint_motor_position2::run; JOINTS, "Inverse kinematics", inverse_kinematics2::run; JOINTS, "Multi Pendulum", multi_pendulum2::run; + // ── Soft bodies ───────────────────────────────────────────────────── + SOFT, "Soft bodies", soft_bodies2::run; + SOFT, "Blobs", soft_blobs2::run; + SOFT, "Jelly", soft_jelly2::run; + SOFT, "Deformable polylines", soft_surface2::run; + SOFT, "Soft pile", soft_pile2::run; + SOFT, "Thin features", soft_thin_features2::run; + #[cfg(not(target_arch = "wasm32"))] + SOFT, "Soft letters", soft_letters2::run; + SOFT, "Plasticity", soft_plasticity2::run; + SOFT, "Tearing", soft_tearing2::run; // ── Controls ──────────────────────────────────────────────────────── CONTROLS, "Character controller", character_controller2::run; // ── Debug ─────────────────────────────────────────────────────────── diff --git a/examples2d/soft_blobs2.rs b/examples2d/soft_blobs2.rs new file mode 100644 index 000000000..012f6e0a4 --- /dev/null +++ b/examples2d/soft_blobs2.rs @@ -0,0 +1,66 @@ +//! Many pressurized blobs poured into a container, with self-contacts enabled on one of them +//! so it can fold onto itself. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Container. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(20.0, 0.5), + ); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-20.0, 30.0)), + ColliderBuilder::cuboid(0.5, 30.0), + ); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(20.0, 30.0)), + ColliderBuilder::cuboid(0.5, 30.0), + ); + + /* + * Blobs. + */ + let mut k = 0; + for j in 0..60 { + for i in 0..19 { + let radius = 0.45 + 0.1 * ((i + j) % 3) as Real; + let x = -19.0 + i as Real * 2.0 + (j % 2) as Real * 0.5; + let y = 3.0 + j as Real * 2.0; + // Self-contacts keep a hard squeeze from pushing the loop through itself (a + // crossed loop is a degenerate pressure vessel that stays crushed under the pile). + let blob = SoftBodyBuilder::disk(Vector::new(x, y), radius, 20) + .softness(SpringCoefficients::new(60.0, 3.0)) + .volume_factor(1.05) + .self_contacts(true) + .particle_mass(0.05); + world.insert_soft_body(blob); + k += 1; + } + } + let _ = k; + + /* + * A long floppy strip with self-contacts, dropped on the pile. + */ + let strip = SoftBodyBuilder::grid(Vector::new(0.0, 16.0), Vector::new(3.0, 0.15), 40, 3) + .softness(SpringCoefficients::new(120.0, 1.0)) + .self_contacts(true) + .particle_mass(0.1); + world.insert_soft_body(strip); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 6.0), 30.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/soft_bodies2.rs b/examples2d/soft_bodies2.rs new file mode 100644 index 000000000..57994894b --- /dev/null +++ b/examples2d/soft_bodies2.rs @@ -0,0 +1,107 @@ +//! Soft-body showcase (2D): pressurized blobs, a rope holding a weight, a jelly square and a +//! hanging chain of soft polygons. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground and walls. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(15.0, 0.5), + ); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-15.0, 5.0)), + ColliderBuilder::cuboid(0.5, 5.0), + ); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(15.0, 5.0)), + ColliderBuilder::cuboid(0.5, 5.0), + ); + + /* + * Pressurized blobs of various sizes. + */ + for i in 0..5 { + let radius = 0.6 + 0.15 * i as Real; + let blob = SoftBodyBuilder::disk( + Vector::new(-10.0 + i as Real * 2.5, 2.0 + i as Real), + radius, + 24, + ) + .softness(SpringCoefficients::new(20.0, 1.0)) + .volume_factor(1.1) + .self_contacts(true) + .particle_mass(0.05); + world.insert_soft_body(blob); + } + + /* + * Jelly squares (corotational and Neo-Hookean elastic cells) and one with per-cell area + * constraints. + */ + let jelly = SoftBodyBuilder::grid(Vector::new(2.0, 1.2), Vector::splat(1.0), 6, 6) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 3.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.2); + world.insert_soft_body(jelly); + let jelly = SoftBodyBuilder::grid(Vector::new(8.0, 1.2), Vector::splat(1.0), 6, 6) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: 3.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.2); + world.insert_soft_body(jelly); + let jelly = SoftBodyBuilder::grid(Vector::new(5.0, 1.2), Vector::splat(1.0), 6, 6) + .cell_model(SoftBodyCellModel::Volume) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.2); + world.insert_soft_body(jelly); + + /* + * A rope hanging from a fixed anchor, holding a rigid box. + */ + let rope = SoftBodyBuilder::rope(Vector::new(8.0, 9.0), Vector::new(12.0, 9.0), 25) + .pinned_particles([0]) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.05); + let rope_handle = world.insert_soft_body(rope); + let last_pos = world.soft_bodies[rope_handle].particle_position(24); + let (weight, _) = world.insert( + RigidBodyBuilder::dynamic().translation(last_pos + Vector::new(0.0, -0.4)), + ColliderBuilder::cuboid(0.3, 0.3).density(2.0), + ); + world.soft_bodies[rope_handle].attach_particle(24, weight, &world.bodies); + + /* + * A stack of rigid boxes for the blobs to knock down. + */ + for i in 0..6 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(-4.0, 0.3 + 0.6 * i as Real)), + ColliderBuilder::cuboid(0.3, 0.3), + ); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 4.0), 30.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/soft_jelly2.rs b/examples2d/soft_jelly2.rs new file mode 100644 index 000000000..4bf09e80b --- /dev/null +++ b/examples2d/soft_jelly2.rs @@ -0,0 +1,88 @@ +//! Jelly (2D): triangulated squares of increasing stiffness stacked in a pyramid, a bridge of +//! soft slabs pinned at both ends supporting rigid boxes, and a shape-matched blob driven along a +//! path. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(20.0, 0.5), + ); + + /* + * A pyramid of jelly squares, softer toward the top. + */ + let levels = 4; + for level in 0..levels { + let young = 2.0e4 / (1.0 + level as Real * 1.5); + for i in 0..(levels - level) { + let x = -8.0 + (i as Real - (levels - level) as Real * 0.5 + 0.5) * 1.6; + let y = 0.75 + level as Real * 1.5; + let square = SoftBodyBuilder::grid(Vector::new(x, y), Vector::splat(0.75), 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1); + world.insert_soft_body(square); + } + } + + /* + * A soft bridge pinned at both ends, with rigid boxes dropped on it. + */ + let bridge = SoftBodyBuilder::grid(Vector::new(3.0, 3.0), Vector::new(4.0, 0.2), 41, 3) + .cell_model(SoftBodyCellModel::Volume) + .softness(SpringCoefficients::new(30.0, 1.0)) + .pinned_particles([0, 1, 2, 120, 121, 122]) + .particle_mass(0.05); + world.insert_soft_body(bridge); + for i in 0..6 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.5 + i as Real, 5.0 + i as Real)), + ColliderBuilder::cuboid(0.25, 0.25), + ); + } + + /* + * A shape-matched blob driven along a circle: a squishy kinematic body pushing rigid boxes. + */ + let driven = SoftBodyBuilder::disk(Vector::new(8.0, 4.0), 0.8, 24) + .shape_matching(true) + .softness(SpringCoefficients::new(15.0, 1.0)) + .volume_preservation(false) + .gravity_scale(0.0) + .particle_mass(0.2) + .can_sleep(false); + let driven = world.insert_soft_body(driven); + for i in 0..8 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(6.5 + 0.5 * i as Real, 0.25)), + ColliderBuilder::cuboid(0.2, 0.2), + ); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 3.0), 30.0); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + t += world.integration_parameters.dt; + let center = Vector::new(8.0 + 2.5 * t.cos(), 1.0 + 1.5 * (2.0 * t).sin().abs()); + world.soft_bodies[driven] + .cluster_mut(0) + .unwrap() + .set_shape_matching_target(Some(Pose::from_parts(center, Rotation::new(t)))); + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/soft_letters2.rs b/examples2d/soft_letters2.rs new file mode 100644 index 000000000..30e271b9d --- /dev/null +++ b/examples2d/soft_letters2.rs @@ -0,0 +1,72 @@ +//! The "rapier" letters of the trimesh demo as soft bodies: each glyph outline is filled with +//! cells by `SoftBodyBuilder::volumetric` and dropped into a bin, one row per stiffness, from +//! jelly to nearly rigid. Every letter collides through its deformable polyline surface. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Bin (same as the trimesh demo). + */ + let ground_size = 25.0; + world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(ground_size, 1.2), + ); + for x in [-ground_size, ground_size] { + world.insert( + RigidBodyBuilder::fixed() + .rotation(std::f32::consts::FRAC_PI_2) + .translation(Vector::new(x, ground_size)), + ColliderBuilder::cuboid(ground_size, 1.2), + ); + } + + /* + * Soft letters: one row per Young's modulus. + */ + let cell_size = 2.4; + let letters: Vec<_> = crate::utils::svg::rapier_logo() + .iter() + .filter_map(|(vtx, idx)| { + let outline = crate::utils::svg::outline(idx); + let letter = SoftBodyBuilder::volumetric(vtx, &outline, cell_size)?; + Some((letter.positions, letter.cells)) + }) + .collect(); + let stiffnesses = [1.0e3, 5.0e3, 1.0e4, 5.0e4, 1.0e5, 5.0e5, 1.0e6, 5.0e6]; + for (row, young) in stiffnesses.into_iter().rev().enumerate() { + for (ith, (positions, cells)) in letters.iter().enumerate() { + let offset = Vector::new(ith as Real * 8.0 - 22.0, 12.0 + row as Real * 11.0); + let letter = SoftBodyBuilder::new(positions.iter().map(|p| *p + offset).collect()) + .cells(cells.clone()) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + // The stiff letters sway on their feet (solver compliance, undamped by + // the material constraints): damp their deformation, the more the stiffer. + deformation_damping: (young / 4.0e4).min(50.0), + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.15) + .self_contacts(true); + world.insert_soft_body(letter); + } + } + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 20.0), 17.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/soft_pile2.rs b/examples2d/soft_pile2.rs new file mode 100644 index 000000000..47e8dc848 --- /dev/null +++ b/examples2d/soft_pile2.rs @@ -0,0 +1,115 @@ +//! Stress test: a big pile of deformable objects (jelly squares of various stiffnesses, +//! pressurized blobs, floppy strips, ropes) poured into a bin with a few rigid boxes and thin +//! pins in the way. Everything collides through deformable polylines. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Bin, with thin pins across it. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(9.0, 0.5), + ); + for x in [-9.0, 9.0] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 12.0)), + ColliderBuilder::cuboid(0.5, 12.0), + ); + } + for i in 0..5 { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-6.0 + i as Real * 3.0, 6.0)), + ColliderBuilder::capsule_x(0.6, 0.1), + ); + } + + /* + * Layers of soft objects. + */ + let mut k = 0usize; + for layer in 0..40 { + for i in 0..6 { + let x = -7.0 + i as Real * 2.8 + (layer % 2) as Real * 1.0; + let y = 9.0 + layer as Real * 2.6; + match k % 5 { + 0 | 3 => { + let young = 3.0e3 * (1.0 + (k % 7) as Real * 4.0); + let square = SoftBodyBuilder::grid(Vector::new(x, y), Vector::splat(0.6), 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.08) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)); + world.insert_soft_body(square); + } + 1 => { + let blob = SoftBodyBuilder::disk(Vector::new(x, y), 0.6, 20) + .softness(SpringCoefficients::new(20.0, 1.0)) + .volume_factor(1.1) + .self_contacts(true) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)); + world.insert_soft_body(blob); + } + 2 => { + let strip = + SoftBodyBuilder::grid(Vector::new(x, y), Vector::new(1.2, 0.12), 13, 2) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)); + world.insert_soft_body(strip); + } + _ => { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(x, y)), + ColliderBuilder::cuboid(0.4, 0.4).density(0.5), + ); + } + } + k += 1; + } + } + + /* + * Ropes thrown on top. + */ + for i in 0..3 { + let rope = SoftBodyBuilder::rope( + Vector::new(-6.0 + i as Real, 36.0 + i as Real), + Vector::new(4.0 + i as Real, 37.0 + i as Real), + 30, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.03) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.6)); + world.insert_soft_body(rope); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 10.0), 20.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/soft_plasticity2.rs b/examples2d/soft_plasticity2.rs new file mode 100644 index 000000000..8f4ccedb6 --- /dev/null +++ b/examples2d/soft_plasticity2.rs @@ -0,0 +1,144 @@ +//! Plasticity (2D): elastic cells whose rest shape flows once their strain exceeds a yield +//! (`SoftBodyMaterial::plastic_yield` / `plastic_creep`), shown on squares hit by disks, a +//! stamped clay slab, a creeping column and clay disks splatting on a wall. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground, a wall for the clay disks, and a shelf for the press. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(16.0, 0.5), + ); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(13.0, 2.0)), + ColliderBuilder::cuboid(0.2, 2.0).friction(0.8), + ); + + let clay = |young: Real, plastic_yield: Real, plastic_creep: Real| SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.35, + elastic_damping_ratio: 1.0, + plastic_yield, + plastic_creep, + deformation_damping: 4.0, + ..Default::default() + }; + + /* + * The yield ladder: identical squares, from purely elastic (left) to very plastic (right), + * each hit by the same heavy disk. + */ + for (i, plastic_yield) in [0.0, 0.2, 0.08, 0.02].into_iter().enumerate() { + let x = -13.0 + i as Real * 2.4; + let square = SoftBodyBuilder::grid(Vector::new(x, 0.75), Vector::splat(0.75), 6, 6) + .cell_model(SoftBodyCellModel::Corotational) + .material(clay(1.0e4, plastic_yield, 20.0)) + .particle_mass(0.1) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + world.insert_soft_body(square); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(x, 6.0)), + ColliderBuilder::ball(0.4).density(5.0), + ); + } + + /* + * A clay slab stamped by a kinematic press: the imprints stay after the press lifts. + */ + let slab = SoftBodyBuilder::grid(Vector::new(0.0, 0.5), Vector::new(3.0, 0.5), 25, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(clay(3.0e4, 0.02, 50.0)) + .particle_mass(0.1) + .surface_collider(ColliderBuilder::ball(0.12).friction(0.8)); + world.insert_soft_body(slab); + let press_rest = Vector::new(-2.0, 2.4); + let (press, _) = world.insert( + RigidBodyBuilder::kinematic_position_based().translation(press_rest), + ColliderBuilder::cuboid(0.4, 0.4) + .rotation(core::f32::consts::FRAC_PI_4) + .friction(0.5), + ); + + /* + * Two soft columns under their own weight: the elastic one stands, the plastic one creeps, + * leans and collapses into a heap (a body whose cells keep flowing is kept awake). + */ + for (i, plastic_yield) in [0.0, 0.04].into_iter().enumerate() { + let x = 5.5 + i as Real * 1.5; + let column = SoftBodyBuilder::grid(Vector::new(x, 1.2), Vector::new(0.3, 1.2), 3, 12) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 1.0, + plastic_yield, + plastic_creep: 0.5, + deformation_damping: 4.0, + ..Default::default() + }) + .particle_mass(0.2) + .surface_collider(ColliderBuilder::ball(0.1).friction(1.0)); + world.insert_soft_body(column); + } + + /* + * Clay disks thrown at the wall. + */ + let n = 24; + let indices: Vec<[u32; 2]> = (0..n).map(|i| [i as u32, ((i + 1) % n) as u32]).collect(); + for i in 0..3 { + let center = Vector::new(11.5 - i as Real * 1.5, 1.0 + i as Real * 0.5); + let vertices: Vec = (0..n) + .map(|k| { + let a = k as Real / n as Real * core::f32::consts::TAU; + center + Vector::new(a.cos(), a.sin()) * 0.5 + }) + .collect(); + if let Some(disk) = SoftBodyBuilder::volumetric(&vertices, &indices, 0.15) { + let disk = disk + .cell_model(SoftBodyCellModel::Corotational) + .material(clay(1.0e4, 0.03, 60.0)) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.075).friction(0.8)); + let handle = world.insert_soft_body(disk); + let sb = &mut world.soft_bodies[handle]; + for k in 0..sb.num_particles() { + sb.set_particle_velocity(k, Vector::new(10.0, 1.0)); + } + } + } + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 3.0), 30.0); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + t += world.integration_parameters.dt; + // The press stamps a new spot every 3 seconds: down for a second, up for a second, + // then slides to the next spot. + let period = 3.0; + let cycle = (t / period).floor(); + let phase = t - cycle * period; + let x = press_rest.x + (cycle % 5.0) * 1.0; + let depth = 1.0; + let y = press_rest.y + - if phase < 1.0 { + depth * phase + } else if phase < 2.0 { + depth * (2.0 - phase) + } else { + 0.0 + }; + world.bodies[press].set_next_kinematic_translation(Vector::new(x, y)); + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/soft_surface2.rs b/examples2d/soft_surface2.rs new file mode 100644 index 000000000..c71579976 --- /dev/null +++ b/examples2d/soft_surface2.rs @@ -0,0 +1,129 @@ +//! Deformable polyline collisions: soft bodies colliding through their boundary polyline (a +//! hammock catching small balls, a pinned bridge, stacked jelly squares, a self-colliding strip). + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground and walls. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(22.0, 0.5), + ); + for x in [-22.0, 22.0] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 2.0)), + ColliderBuilder::cuboid(0.5, 3.0), + ); + } + + /* + * A hammock with a coarse particle spacing (0.3) under a rain of balls smaller than that + * spacing: they rest on the segments instead of falling between the particles. + */ + { + let x0 = -12.0; + let hammock = + SoftBodyBuilder::grid(Vector::new(x0 + 3.0, 5.0), Vector::new(3.0, 0.15), 21, 2) + .cell_model(SoftBodyCellModel::Volume) + .pinned_particles([0, 1, 40, 41]) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)); + world.insert_soft_body(hammock); + + for i in 0..12 { + for h in 0..4 { + let pos = Vector::new( + x0 + 0.8 + i as Real * 0.4 + (h % 2) as Real * 0.15, + 7.0 + h as Real * 0.5, + ); + world.insert( + RigidBodyBuilder::dynamic().translation(pos), + ColliderBuilder::ball(0.08).density(2.0), + ); + } + } + } + + /* + * A soft bridge loaded with thin pins standing on their tips: with surface collisions the + * pins push on the segments between the particles. + */ + let bridge = SoftBodyBuilder::grid(Vector::new(2.0, 3.0), Vector::new(3.0, 0.2), 31, 3) + .cell_model(SoftBodyCellModel::Volume) + .pinned_particles([0, 1, 2, 90, 91, 92]) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.05) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.8)); + world.insert_soft_body(bridge); + for i in 0..8 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(-0.4 + i as Real * 0.65, 4.5)), + ColliderBuilder::cuboid(0.03, 0.4).density(3.0), + ); + } + + /* + * A stack of jelly squares topped by a pressurized blob: soft-vs-soft contacts go through + * the surfaces. + */ + for i in 0..3 { + let square = SoftBodyBuilder::grid( + Vector::new(8.0, 0.75 + i as Real * 1.6), + Vector::splat(0.7), + 5, + 5, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.8)); + world.insert_soft_body(square); + } + let blob = SoftBodyBuilder::disk(Vector::new(8.0, 5.0), 0.8, 24) + .softness(SpringCoefficients::new(20.0, 1.0)) + .volume_factor(1.1) + .particle_mass(0.05) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)); + world.insert_soft_body(blob); + + /* + * A long strip with self-contacts dropped on a peg: it folds and piles up on itself. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(14.0, 1.0)), + ColliderBuilder::cuboid(0.15, 1.0), + ); + let strip = SoftBodyBuilder::grid(Vector::new(14.0, 9.0), Vector::new(4.0, 0.1), 60, 2) + .cell_model(SoftBodyCellModel::Volume) + .softness(SpringCoefficients::new(30.0, 1.0)) + .self_contacts(true) + .particle_mass(0.02) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)); + world.insert_soft_body(strip); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 4.0), 18.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/soft_tearing2.rs b/examples2d/soft_tearing2.rs new file mode 100644 index 000000000..230b1cad0 --- /dev/null +++ b/examples2d/soft_tearing2.rs @@ -0,0 +1,116 @@ +//! Tearing (2D, `SoftBodyMaterial::tear_strain`): a pinned jelly bridge broken by a heavy disk, a +//! hanging strip shot through by a fast disk, and a jelly bar pulled apart by its kinematic ends. +//! Cracks split particles and shed pieces as new soft bodies; the panel sets `min_piece`. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(30.0, 0.5), + ); + // A wall catching the disk shot through the strip. + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(16.0, 3.0)), + ColliderBuilder::cuboid(0.3, 3.0), + ); + + let jelly = |young: Real, tear: Real| SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + tear_strain: Some(tear), + ..Default::default() + }; + + /* + * A bridge pinned at both ends: the heavy disk dropped on its middle breaks it. + */ + let (nx, ny) = (31usize, 6usize); + let idx = |i: usize, j: usize| (i * ny + j) as u32; + // Stiff and thick enough to support its own weight with strains under 0.15 (a slender bridge + // of soft jelly sagged past the tear strain by itself). + let bridge = SoftBodyBuilder::grid(Vector::new(-8.0, 4.0), Vector::new(3.0, 0.5), nx, ny) + .cell_model(SoftBodyCellModel::Corotational) + .material(jelly(1.0e6, 0.35)) + .pinned_particles((0..ny).flat_map(|j| [idx(0, j), idx(nx - 1, j)])) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + world.insert_soft_body(bridge); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(-8.0, 9.0)), + ColliderBuilder::ball(0.6).density(20.0), + ); + + /* + * A strip hanging from its top edge, with a fast disk shot through it. + */ + let (nx, ny) = (7usize, 41usize); + let idx = |i: usize, j: usize| (i * ny + j) as u32; + let curtain = SoftBodyBuilder::grid(Vector::new(2.0, 5.0), Vector::new(0.45, 3.0), nx, ny) + .cell_model(SoftBodyCellModel::Corotational) + .material(jelly(3.0e4, 0.35)) + .pinned_particles((0..nx).map(|i| idx(i, ny - 1))) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.8)); + world.insert_soft_body(curtain); + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(-4.0, 4.5)) + .linvel(Vector::new(20.0, 0.0)), + ColliderBuilder::ball(0.4).density(10.0), + ); + + /* + * A jelly bar whose two ends are pinned: the right end is pulled away until the bar snaps. + */ + let bar = SoftBodyBuilder::grid(Vector::new(-3.0, 1.0), Vector::new(2.0, 0.4), 21, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(jelly(5.0e4, 0.4)) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + let bar = world.insert_soft_body(bar); + let sb = &world.soft_bodies[bar]; + let ends: Vec<(usize, Vector, bool)> = (0..sb.num_particles()) + .filter_map(|i| { + let p = sb.particle_position(i); + if p.x < -4.99 { + Some((i, p, false)) + } else if p.x > -1.01 { + Some((i, p, true)) + } else { + None + } + }) + .collect(); + for &(i, _, _) in &ends { + world.soft_bodies[bar].set_particle_pinned(i, true); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 4.0), 40.0); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + t += world.integration_parameters.dt; + // The right end of the bar starts moving after a second, at half a meter per + // second, and stops once the bar has doubled its length. + let shift = ((t - 1.0).max(0.0) * 0.5).min(4.0); + let sb = &mut world.soft_bodies[bar]; + for &(i, rest, right) in &ends { + if right { + sb.set_particle_kinematic_target(i, rest + Vector::new(shift, 0.0)); + } + } + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/soft_thin_features2.rs b/examples2d/soft_thin_features2.rs new file mode 100644 index 000000000..be6917434 --- /dev/null +++ b/examples2d/soft_thin_features2.rs @@ -0,0 +1,130 @@ +//! Stress test (2D): deformable bodies against thin rigid features and vice versa (beds of +//! nails, thin pins, raining needles, thin plates, a stretched strip under a thin rod). + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(40.0, 0.5), + ); + + let jelly = |center: Vector, half: Real, n: usize, young: Real| { + SoftBodyBuilder::grid(center, Vector::splat(half), n, n) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.7)) + }; + let strip = |center: Vector, half: Vector, nx: usize| { + SoftBodyBuilder::grid(center, half, nx, 2) + .cell_model(SoftBodyCellModel::Volume) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.03) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)) + }; + let blob = |center: Vector, radius: Real| { + SoftBodyBuilder::disk(center, radius, 24) + .softness(SpringCoefficients::new(20.0, 1.0)) + .volume_factor(1.2) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) + }; + + /* + * Bed of nails: jelly squares of two stiffnesses and a blob land on it, a strip drapes over. + */ + for i in 0..24 { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-16.0 + i as Real * 0.5, 0.6)), + ColliderBuilder::capsule_y(0.6, 0.03), + ); + } + world.insert_soft_body(jelly(Vector::new(-14.5, 3.5), 0.75, 5, 3.0e3)); + world.insert_soft_body(jelly(Vector::new(-11.5, 3.5), 0.75, 5, 5.0e4)); + world.insert_soft_body(blob(Vector::new(-8.5, 3.5), 0.8)); + world.insert_soft_body(strip(Vector::new(-6.0, 6.0), Vector::new(2.5, 0.1), 31)); + + /* + * Needle rain on a hammock (strip pinned at both ends) and on a jelly block. + */ + let hammock = + strip(Vector::new(0.0, 4.0), Vector::new(3.0, 0.1), 31).pinned_particles([0, 1, 60, 61]); + world.insert_soft_body(hammock); + world.insert_soft_body(jelly(Vector::new(6.5, 0.9), 0.9, 6, 2.0e4)); + for i in 0..10 { + for j in 0..3 { + for cx in [0.0, 6.5] { + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new( + cx - 2.0 + i as Real * 0.45, + 8.0 + j as Real * 1.2, + )) + .rotation(0.4 * (i + j) as Real), + ColliderBuilder::capsule_y(0.5, 0.02).density(3.0), + ); + } + } + } + + /* + * Thin plates falling on a blob. + */ + world.insert_soft_body(blob(Vector::new(11.0, 0.9), 0.9)); + for i in 0..4 { + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(11.0, 3.5 + i as Real * 0.5)) + .rotation(0.15 * i as Real), + ColliderBuilder::cuboid(0.9, 0.015).density(1.0), + ); + } + + /* + * A clamped strip stretched to twice its length and released, with a rod resting on it. + */ + let (sx, sy) = (2.5, 0.1); + let stretched = + strip(Vector::new(16.0, 3.0), Vector::new(sx, sy), 31).pinned_particles([0, 1, 60, 61]); + let stretched = world.insert_soft_body(stretched); + let right_rest: Vec = [60usize, 61] + .iter() + .map(|&i| world.soft_bodies[stretched].particle_position(i)) + .collect(); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(16.0, 4.0)), + ColliderBuilder::capsule_x(0.8, 0.03).density(2.0), + ); + let animate = move |world: &mut PhysicsWorld, t: Real| { + let stretch = 2.5 * (1.0 - (0.5 * t).cos()); + let sb = &mut world.soft_bodies[stretched]; + for (&i, rest) in [60usize, 61].iter().zip(right_rest.iter()) { + sb.set_particle_kinematic_target(i, *rest + Vector::new(stretch, 0.0)); + } + }; + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 4.0), 22.0); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + t += world.integration_parameters.dt; + animate(&mut world, t); + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/utils/svg.rs b/examples2d/utils/svg.rs index 9ac30ba05..7d207e963 100644 --- a/examples2d/utils/svg.rs +++ b/examples2d/utils/svg.rs @@ -36,6 +36,25 @@ const RAPIER_SVG_STR: &str = r#" "#; +/// The outline of a tessellated shape: the edges belonging to a single triangle, which for a fill +/// tessellation are the shape's contours, its holes included. +/// +/// That closed polyline is what `SoftBodyBuilder::volumetric` fills with simulation cells. +pub fn outline(triangles: &[[u32; 3]]) -> Vec<[u32; 2]> { + let mut count: std::collections::HashMap<[u32; 2], u32> = Default::default(); + for tri in triangles { + for k in 0..3 { + let (a, b) = (tri[k], tri[(k + 1) % 3]); + *count.entry([a.min(b), a.max(b)]).or_insert(0) += 1; + } + } + triangles + .iter() + .flat_map(|tri| (0..3).map(move |k| [tri[k], tri[(k + 1) % 3]])) + .filter(|e| count[&[e[0].min(e[1]), e[0].max(e[1])]] == 1) + .collect() +} + pub fn rapier_logo() -> Vec<(Vec, Vec<[u32; 3]>)> { tessellate_svg_str(RAPIER_SVG_STR) } @@ -208,3 +227,62 @@ struct GpuVertex { position: [f32; 2], prim_id: u32, } + +#[cfg(test)] +mod tests { + use super::*; + + /// Each glyph outline closes, and filling it at the resolution the demo uses keeps its area + /// and gives well-shaped cells. + #[test] + fn logo_outlines_are_closed_and_fillable() { + for (i, (vtx, idx)) in rapier_logo().iter().enumerate() { + let outline = outline(idx); + + // Every vertex of a closed polyline is used by exactly two segments, once as the + // start and once as the end. + let mut degree: std::collections::HashMap = Default::default(); + for e in &outline { + degree.entry(e[0]).or_default().0 += 1; + degree.entry(e[1]).or_default().1 += 1; + } + for (v, (out, inc)) in °ree { + assert_eq!((*out, *inc), (1, 1), "glyph {i}: vertex {v} is a dead end"); + } + + let tessellated: Real = idx + .iter() + .map(|t| { + let (a, b, c) = (vtx[t[0] as usize], vtx[t[1] as usize], vtx[t[2] as usize]); + ((b - a).perp_dot(c - a) * 0.5).abs() + }) + .sum(); + + let mesh = SoftBodyBuilder::volumetric(vtx, &outline, 0.4) + .unwrap_or_else(|| panic!("glyph {i} could not be filled")); + let mut filled = 0.0; + let mut min_angle = Real::MAX; + + for cell in &mesh.cells { + let p = cell.map(|v| mesh.positions[v as usize]); + filled += (p[1] - p[0]).perp_dot(p[2] - p[0]) * 0.5; + + for k in 0..3 { + let (a, b, c) = (p[k], p[(k + 1) % 3], p[(k + 2) % 3]); + let angle = (b - a) + .normalize() + .dot((c - a).normalize()) + .clamp(-1.0, 1.0) + .acos(); + min_angle = min_angle.min(angle.to_degrees()); + } + } + + assert!( + (filled - tessellated).abs() < tessellated * 0.01, + "glyph {i}: filled {filled} vs tessellated {tessellated}" + ); + assert!(min_angle > 25.0, "glyph {i}: smallest angle {min_angle}"); + } + } +} diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 84c8f9b42..80bac13df 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -76,6 +76,18 @@ mod primitives3; mod restitution3; mod rope_joints3; mod sensor3; +mod soft_bodies3; +mod soft_cloth3; +mod soft_cloth_stress3; +mod soft_dress3; +mod soft_jelly3; +mod soft_pile3; +mod soft_plasticity3; +mod soft_surface3; +mod soft_tearing3; +mod soft_thin_features3; +#[cfg(not(target_arch = "wasm32"))] +mod soft_trimesh3; mod spring_joints3; mod stress_tests; mod trimesh3; @@ -109,6 +121,7 @@ pub async fn main() { const DYNAMICS: &str = "Dynamics"; const JOINTS: &str = "Joints"; const CONTROLS: &str = "Controls"; + const SOFT: &str = "Soft bodies"; const DEBUG: &str = "Debug"; const ROBOTICS: &str = "Robotics"; const STRESS: &str = "Stress tests"; @@ -149,6 +162,19 @@ pub async fn main() { JOINTS, "Spring Joints", spring_joints3::run; JOINTS, "Joint Motor Position", joint_motor_position3::run; JOINTS, "Inverse kinematics", inverse_kinematics3::run; + // ── Soft bodies ───────────────────────────────────────────────────── + SOFT, "Soft bodies", soft_bodies3::run; + SOFT, "Cloth", soft_cloth3::run; + SOFT, "Jelly", soft_jelly3::run; + SOFT, "Deformable trimeshes", soft_surface3::run; + SOFT, "Soft pile", soft_pile3::run; + SOFT, "Thin features", soft_thin_features3::run; + SOFT, "Cloth stress", soft_cloth_stress3::run; + SOFT, "Plasticity", soft_plasticity3::run; + SOFT, "Tearing", soft_tearing3::run; + SOFT, "Dancing dress", soft_dress3::run; + #[cfg(not(target_arch = "wasm32"))] + SOFT, "Soft trimeshes", soft_trimesh3::run; // ── Controls ──────────────────────────────────────────────────────── CONTROLS, "Character controller", character_controller3::run; CONTROLS, "Vehicle controller", vehicle_controller3::run; diff --git a/examples3d/soft_bodies3.rs b/examples3d/soft_bodies3.rs new file mode 100644 index 000000000..68b411585 --- /dev/null +++ b/examples3d/soft_bodies3.rs @@ -0,0 +1,103 @@ +//! Soft-body showcase: a pinned cloth catching a box, a pressurized balloon, jelly cubes +//! (corotational, Neo-Hookean) and a volume-preserving one, and a rope holding a rigid weight. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(12.0, 0.1, 12.0), + ); + + /* + * A cloth pinned by its four corners, with a box dropped on it. + */ + let n = 24; + let cloth = SoftBodyBuilder::cloth( + Vector::new(-3.5, 2.5, -1.2), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .pinned_particles([0, (n - 1) as u32, (n * (n - 1)) as u32, (n * n - 1) as u32]) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.05); + world.insert_soft_body(cloth); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(-2.35, 4.0, 0.0)), + ColliderBuilder::cuboid(0.3, 0.3, 0.3).density(0.5), + ); + + /* + * A balloon: hollow sphere with pressure (global volume preservation). + */ + let balloon = SoftBodyBuilder::sphere(Vector::new(0.5, 3.0, 0.0), 0.8, 2) + .softness(SpringCoefficients::new(15.0, 1.0)) + .volume_factor(1.2) + .particle_mass(0.05); + world.insert_soft_body(balloon); + + /* + * Jelly cubes: one corotational, one Neo-Hookean, one with per-cell volume constraints. + */ + let jelly = SoftBodyBuilder::cuboid(Vector::new(3.0, 1.0, 1.5), Vector::splat(0.6), 5, 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.2); + world.insert_soft_body(jelly); + let jelly = SoftBodyBuilder::cuboid(Vector::new(3.0, 1.0, 4.5), Vector::splat(0.6), 5, 5, 5) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: 2.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.2); + world.insert_soft_body(jelly); + let jelly = SoftBodyBuilder::cuboid(Vector::new(3.0, 1.0, -1.5), Vector::splat(0.6), 5, 5, 5) + .cell_model(SoftBodyCellModel::Volume) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.2); + world.insert_soft_body(jelly); + + /* + * A rope hanging from a fixed anchor, holding a rigid ball attached by a joint. + */ + let rope = SoftBodyBuilder::rope(Vector::new(-0.5, 5.0, 3.0), Vector::new(2.5, 5.0, 3.0), 30) + .pinned_particles([0]) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.05); + let rope_handle = world.insert_soft_body(rope); + let last_pos = world.soft_bodies[rope_handle].particle_position(29); + let (weight, _) = world.insert( + RigidBodyBuilder::dynamic().translation(last_pos + Vector::new(0.0, -0.3, 0.0)), + ColliderBuilder::ball(0.25).density(2.0), + ); + world.soft_bodies[rope_handle].attach_particle(29, weight, &world.bodies); + + /* + * Set up the testbed. + */ + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(9.0, 6.0, 12.0), Vec3::new(0.0, 1.5, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/soft_cloth3.rs b/examples3d/soft_cloth3.rs new file mode 100644 index 000000000..c0ba12b5e --- /dev/null +++ b/examples3d/soft_cloth3.rs @@ -0,0 +1,75 @@ +//! Cloth draped over rigid obstacles, and a hanging curtain pushed by a rolling ball. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(12.0, 0.1, 12.0), + ); + + /* + * A sheet dropped over a rigid ball and a rigid box. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-1.0, 1.0, 0.0)), + ColliderBuilder::ball(1.0), + ); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(1.5, 0.6, 0.0)), + ColliderBuilder::cuboid(0.6, 0.6, 0.6), + ); + let n = 40; + let sheet = SoftBodyBuilder::cloth( + Vector::new(-3.0, 3.0, -2.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n + 20, + n, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .particle_mass(0.02) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.8)); + world.insert_soft_body(sheet); + + /* + * A curtain pinned along its top edge, with a heavy ball rolling into it. + */ + let curtain = SoftBodyBuilder::cloth( + Vector::new(-2.0, 4.0, 5.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, -0.1, 0.0), + 40, + 30, + ) + .pinned_particles((0..40).map(|i| (i * 30) as u32)) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.02); + world.insert_soft_body(curtain); + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 0.5, 9.0)) + .linvel(Vector::new(0.0, 0.0, -6.0)), + ColliderBuilder::ball(0.5).density(3.0), + ); + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(8.0, 6.0, 14.0), Vec3::new(0.0, 1.5, 2.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/soft_cloth_stress3.rs b/examples3d/soft_cloth_stress3.rs new file mode 100644 index 000000000..9f361886f --- /dev/null +++ b/examples3d/soft_cloth_stress3.rs @@ -0,0 +1,128 @@ +//! Stress test for cloth: a stack of sheets sliding down a slope against each other, a banner +//! stretched to twice its length and released periodically, and a strip twisted around its axis +//! by a rotating clamp (self contacts on) until it winds into a rope. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), + ColliderBuilder::cuboid(30.0, 0.5, 30.0), + ); + let cloth = |origin: Vector, du: Vector, dv: Vector, nu: usize, nv: usize| { + SoftBodyBuilder::cloth(origin, du, dv, nu, nv) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .particle_mass(0.02) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.5)) + }; + + /* + * Sheets sliding down a slope, on top of each other, into a stopper. + */ + let slope = 0.5; + world.insert( + RigidBodyBuilder::fixed() + .translation(Vector::new(-6.0, 2.0, 0.0)) + .rotation(Vector::new(0.0, 0.0, -slope)), + ColliderBuilder::cuboid(5.0, 0.2, 4.0).friction(0.4), + ); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-0.6, 0.6, 0.0)), + ColliderBuilder::cuboid(0.2, 0.6, 4.0), + ); + let rot = Rotation::from_axis_angle(Vector::Z, -slope); + for k in 0..6 { + let origin = Vector::new(-6.0, 2.0, 0.0) + + rot * Vector::new(-3.5 + k as Real * 0.3, 0.3 + k as Real * 0.12, -1.5); + world.insert_soft_body(cloth( + origin, + rot * Vector::new(0.15, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.15), + 21, + 21, + )); + } + + /* + * A banner pinned along both ends; the right end is pulled away and brought back. + */ + let (nu, nv) = (31, 13); + let banner_origin = Vector::new(2.0, 4.0, -4.0); + let left: Vec = (0..nv).map(|j| j as u32).collect(); + let right: Vec = (0..nv).map(|j| ((nu - 1) * nv + j) as u32).collect(); + let banner = world.insert_soft_body( + cloth( + banner_origin, + Vector::new(0.15, 0.0, 0.0), + Vector::new(0.0, -0.15, 0.0), + nu, + nv, + ) + .pinned_particles(left.iter().chain(right.iter()).copied()), + ); + let banner_right_rest: Vec = right + .iter() + .map(|&i| world.soft_bodies[banner].particle_position(i as usize)) + .collect(); + + /* + * A strip pinned top and bottom; the bottom clamp rotates about the vertical axis. + */ + let (su, sv) = (11, 41); + let strip_origin = Vector::new(9.0, 6.5, -0.75); + let top: Vec = (0..su).map(|i| (i * sv) as u32).collect(); + let bottom: Vec = (0..su).map(|i| (i * sv + sv - 1) as u32).collect(); + let strip = world.insert_soft_body( + cloth( + strip_origin, + Vector::new(0.0, 0.0, 0.15), + Vector::new(0.0, -0.15, 0.0), + su, + sv, + ) + .pinned_particles(top.iter().chain(bottom.iter()).copied()) + .self_contacts(true), + ); + let strip_axis = Vector::new(9.0, 0.0, 0.0); + let strip_bottom_rest: Vec = bottom + .iter() + .map(|&i| world.soft_bodies[strip].particle_position(i as usize)) + .collect(); + + // Drives the pinned clamps: stretch/release the banner, twist the strip. + let animate = move |world: &mut PhysicsWorld, t: Real| { + let stretch = 1.5 * (1.0 - (0.5 * t).cos()); + let banner_sb = &mut world.soft_bodies[banner]; + for (&i, rest) in right.iter().zip(banner_right_rest.iter()) { + banner_sb + .set_particle_kinematic_target(i as usize, *rest + Vector::new(stretch, 0.0, 0.0)); + } + let twist = Rotation::from_axis_angle(Vector::Y, 0.8 * t); + let strip_sb = &mut world.soft_bodies[strip]; + for (&i, rest) in bottom.iter().zip(strip_bottom_rest.iter()) { + let target = strip_axis + twist * (*rest - strip_axis); + strip_sb.set_particle_kinematic_target(i as usize, target); + } + }; + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(2.0, 8.0, 18.0), Vec3::new(1.5, 3.0, 0.0)); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + t += world.integration_parameters.dt; + animate(&mut world, t); + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/soft_dress3.rs b/examples3d/soft_dress3.rs new file mode 100644 index 000000000..6210b86b7 --- /dev/null +++ b/examples3d/soft_dress3.rs @@ -0,0 +1,238 @@ +//! A kinematic mannequin (capsules driven by a procedural dance) wearing a cloth skirt +//! (`SoftBodyBuilder::cloth_tube`) pinned at the waist and a sleeveless t-shirt tube pinned to +//! the torso; the cloth collides with the body and, for the skirt, with itself. + +use kiss3d::color::Color; +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +const UPPER_ARM: Real = 0.34; +const FOREARM: Real = 0.32; +const THIGH: Real = 0.5; +const SHIN: Real = 0.5; +const TORSO_RADIUS: Real = 0.15; + +/// The body parts, in the order of `Frame::poses`. +#[derive(Clone, Copy)] +enum Part { + Pelvis, + Torso, + Head, + UpperArm(usize), + Forearm(usize), + Thigh(usize), + Shin(usize), +} + +impl Part { + fn index(self) -> usize { + match self { + Part::Pelvis => 0, + Part::Torso => 1, + Part::Head => 2, + Part::UpperArm(i) => 3 + i, + Part::Forearm(i) => 5 + i, + Part::Thigh(i) => 7 + i, + Part::Shin(i) => 9 + i, + } + } +} + +const NUM_PARTS: usize = 11; + +/// The pose placing a capsule (along its local Y) from `joint` along `dir`. +fn limb_pose(joint: Vector, dir: Vector, half_length: Real) -> Pose { + let dir = dir.normalize(); + Pose::from_parts( + joint + dir * half_length, + Rotation::from_rotation_arc(Vector::Y, dir), + ) +} + +/// The mannequin's pose at time `t`: the dance makes the pelvis sway sideways and bounce on +/// the beat while the body slowly turns; the legs step, the arms swing. +fn frame_at(t: Real) -> [Pose; NUM_PARTS] { + let beat = 2.0 * t; + let yaw = 0.7 * (0.35 * t).sin() + 0.25 * t; + let sway = 0.25 * beat.sin(); + let bounce = 0.04 * (2.0 * beat).sin().abs(); + let roll = 0.12 * beat.sin(); + let turn = Rotation::from_rotation_y(yaw); + let hips_rot = turn * Rotation::from_rotation_z(roll); + let hips = Vector::new(0.0, 0.95 + bounce, 0.0) + turn * Vector::new(sway, 0.0, 0.0); + let up = hips_rot * Vector::Y; + let side = hips_rot * Vector::X; + let forward = hips_rot * Vector::Z; + + let mut poses = [Pose::IDENTITY; NUM_PARTS]; + // Pelvis and torso, leaning a little into the sway; the head on top. + let lean = Rotation::from_axis_angle(forward, -0.5 * roll); + let torso_rot = lean * hips_rot; + let torso_center = hips + up * 0.42; + poses[Part::Pelvis.index()] = Pose::from_parts(hips, hips_rot); + poses[Part::Torso.index()] = Pose::from_parts(torso_center, torso_rot); + let neck = torso_center + torso_rot * Vector::Y * 0.28; + poses[Part::Head.index()] = Pose::from_parts(neck + up * 0.13, torso_rot); + + // Legs: thighs swing back and forth alternately, the knees bend on the forward swing. + for (i, s) in [1.0, -1.0].iter().enumerate() { + let hip = hips + side * (0.11 * s) - up * 0.05; + let phase = if i == 0 { 0.0 } else { core::f32::consts::PI }; + let swing = 0.45 * (beat + phase).sin(); + let thigh_dir = (-up * swing.cos() + forward * swing.sin() + side * (0.05 * s)).normalize(); + let knee = hip + thigh_dir * THIGH; + let bend = 0.9 * swing.max(0.0); + let shin_dir = -up * (swing - bend).cos() + forward * (swing - bend).sin(); + poses[Part::Thigh(i).index()] = limb_pose(hip, thigh_dir, THIGH * 0.5); + poses[Part::Shin(i).index()] = limb_pose(knee, shin_dir, SHIN * 0.5); + } + + // Arms: raised sideways and swinging, the elbows bent. + for (i, s) in [1.0, -1.0].iter().enumerate() { + let shoulder = neck - up * 0.06 + torso_rot * Vector::X * (0.22 * s); + let raise = 0.6 + 0.6 * (beat + if i == 0 { 0.0 } else { 1.5 }).sin(); + let arm_dir = (side * (s * raise.cos()) - up * raise.sin() * 0.6 + + forward * 0.2 * (0.7 * beat).sin()) + .normalize(); + let elbow = shoulder + arm_dir * UPPER_ARM; + let fore_dir = (arm_dir + up * 0.9 + forward * 0.5).normalize(); + poses[Part::UpperArm(i).index()] = limb_pose(shoulder, arm_dir, UPPER_ARM * 0.5); + poses[Part::Forearm(i).index()] = limb_pose(elbow, fore_dir, FOREARM * 0.5); + } + poses +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(10.0, 0.1, 10.0), + ); + + /* + * The mannequin: kinematic capsules (a pelvis, a torso, a head, arms and legs in two + * segments), placed at the dance's first frame and animated below. + */ + let skin = Color::new(0.93, 0.8, 0.68, 1.0); + let frame0 = frame_at(0.0); + let shapes: [ColliderBuilder; NUM_PARTS] = [ + ColliderBuilder::capsule_x(0.08, 0.13), + ColliderBuilder::capsule_y(0.18, TORSO_RADIUS), + ColliderBuilder::ball(0.12), + ColliderBuilder::capsule_y(UPPER_ARM * 0.5 - 0.03, 0.05), + ColliderBuilder::capsule_y(UPPER_ARM * 0.5 - 0.03, 0.05), + ColliderBuilder::capsule_y(FOREARM * 0.5 - 0.03, 0.045), + ColliderBuilder::capsule_y(FOREARM * 0.5 - 0.03, 0.045), + ColliderBuilder::capsule_y(THIGH * 0.5 - 0.05, 0.085), + ColliderBuilder::capsule_y(THIGH * 0.5 - 0.05, 0.085), + ColliderBuilder::capsule_y(SHIN * 0.5 - 0.04, 0.065), + ColliderBuilder::capsule_y(SHIN * 0.5 - 0.04, 0.065), + ]; + let mut parts = Vec::with_capacity(NUM_PARTS); + for (i, shape) in shapes.into_iter().enumerate() { + let (body, _) = world.insert( + RigidBodyBuilder::kinematic_position_based().pose(frame0[i]), + shape.friction(0.4), + ); + viewer.set_initial_body_color(body, skin); + parts.push(body); + } + + /* + * The clothes: tubes of cloth whose first ring is pinned and driven with a body part. + */ + let mut pins: Vec<(SoftBodyHandle, usize, Part, Vector)> = Vec::new(); + let mut piece = |world: &mut PhysicsWorld, + viewer: &mut TestbedViewer, + tube: SoftBodyBuilder, + num_around: usize, + part: Part, + self_contacts: bool, + color: Color| { + let handle = world.insert_soft_body( + tube.pinned_particles(0..num_around as u32) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(80.0, 1.0)) + }) + .particle_mass(0.01) + .particle_radius(0.02) + .self_contacts(self_contacts) + .surface_collider(ColliderBuilder::ball(0.02).friction(0.5)), + ); + viewer.set_initial_soft_body_color(handle, color); + // The pinned ring's offsets, in the frame of the part driving it. + let part_pose = frame0[part.index()]; + let sb = &world.soft_bodies[handle]; + for k in 0..num_around { + pins.push(( + handle, + k, + part, + part_pose.inverse_transform_point(sb.particle_position(k)), + )); + } + }; + let red = Color::new(0.75, 0.15, 0.3, 1.0); + // The skirt: from a ring around the waist (pinned to the pelvis) flaring down to the hem. + let pelvis = frame0[Part::Pelvis.index()]; + let waist = pelvis * Vector::new(0.0, 0.1, 0.0); + let hem = Vector::new(waist.x, 0.25, waist.z); + piece( + &mut world, + viewer, + SoftBodyBuilder::cloth_tube(waist, hem - waist, 0.2, 0.62, 56, 18), + 56, + Part::Pelvis, + true, + red, + ); + // The t-shirt: a sleeveless tube from the shoulders down to just above the pelvis, hugging + // the torso capsule at the top (its radius plus a small gap) and widening toward the hem, + // its top ring pinned to the torso. + let white = Color::new(0.95, 0.95, 0.9, 1.0); + let torso = frame0[Part::Torso.index()]; + let shirt_top = torso * Vector::new(0.0, 0.26, 0.0); + let shirt_bottom = torso * Vector::new(0.0, -0.26, 0.0); + piece( + &mut world, + viewer, + SoftBodyBuilder::cloth_tube( + shirt_top, + shirt_bottom - shirt_top, + TORSO_RADIUS + 0.012, + TORSO_RADIUS + 0.09, + 48, + 16, + ), + 48, + Part::Torso, + false, + white, + ); + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(3.0, 2.0, 4.0), Vec3::new(0.0, 0.9, 0.0)); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if !viewer.simulating() { + continue; + } + t += world.integration_parameters.dt; + let frame = frame_at(t); + for (i, body) in parts.iter().enumerate() { + world.bodies[*body].set_next_kinematic_position(frame[i]); + } + for (handle, i, part, offset) in &pins { + world.soft_bodies[*handle] + .set_particle_kinematic_target(*i, frame[part.index()] * *offset); + } + world.step(); + } + Ok(()) +} diff --git a/examples3d/soft_jelly3.rs b/examples3d/soft_jelly3.rs new file mode 100644 index 000000000..9ff317e90 --- /dev/null +++ b/examples3d/soft_jelly3.rs @@ -0,0 +1,143 @@ +//! Jelly: a stack of tetrahedral cubes of increasing stiffness, balloons piled in a box, a +//! shape-matched cube animated toward a moving pose, and tetrahedralized (ball, capsule) jelly +//! bodies. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground and a container for the balloons. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(12.0, 0.1, 12.0), + ); + for (dx, dz, hx, hz) in [ + (-2.5, 0.0, 0.1, 2.5), + (2.5, 0.0, 0.1, 2.5), + (0.0, -2.5, 2.5, 0.1), + (0.0, 2.5, 2.5, 0.1), + ] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(dx + 5.0, 1.0, dz)), + ColliderBuilder::cuboid(hx, 1.0, hz), + ); + } + + /* + * A stack of elastic cubes, softer at the top. + */ + for (i, young) in [1.0e4, 4.0e3, 1.5e3].iter().enumerate() { + let cube = SoftBodyBuilder::cuboid( + Vector::new(-4.0, 0.7 + 1.5 * i as Real, 0.0), + Vector::splat(0.6), + 5, + 5, + 5, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: *young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1); + world.insert_soft_body(cube); + } + + /* + * Balloons dropped in the container. + */ + for i in 0..6 { + let x = 5.0 + ((i % 3) as Real - 1.0) * 1.2; + let z = ((i / 3) as Real - 0.5) * 1.2; + let balloon = SoftBodyBuilder::sphere(Vector::new(x, 2.0 + i as Real * 1.5, z), 0.6, 2) + .softness(SpringCoefficients::new(15.0, 1.0)) + .volume_factor(1.1) + .particle_mass(0.03); + world.insert_soft_body(balloon); + } + + /* + * A shape-matched particle cube driven toward an animated pose: it behaves like a squishy + * kinematic body. + */ + let driven = SoftBodyBuilder::cuboid(Vector::splat(0.45), Vector::splat(0.45), 4, 4, 4) + .shape_matching(true) + .softness(SpringCoefficients::new(15.0, 1.0)) + .particle_radius(0.1) + .particle_mass(0.2) + .gravity_scale(0.0) + .can_sleep(false); + let driven = world.insert_soft_body(driven); + + /* + * Volumetric jelly: a ball and a capsule filled with tetrahedral cells from their boundary + * meshes (`SoftBodyBuilder::volumetric`), dropped behind the stack. + */ + let jelly = |builder: SoftBodyBuilder, young: Real| { + builder + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.7)) + }; + let (vertices, indices) = Ball::new(0.7).to_trimesh(16, 16); + let vertices: Vec = vertices + .iter() + .map(|p| *p + Vector::new(-4.0, 3.0, -4.0)) + .collect(); + if let Some(ball) = SoftBodyBuilder::volumetric(&vertices, &indices, 0.2) { + world.insert_soft_body(jelly(ball, 1.0e4)); + } + let (vertices, indices) = Capsule::new_y(0.6, 0.45).to_trimesh(12, 12); + let vertices: Vec = vertices + .iter() + .map(|p| Rotation::from_axis_angle(Vector::Z, 1.2) * *p + Vector::new(-4.0, 6.0, -4.0)) + .collect(); + if let Some(capsule) = SoftBodyBuilder::volumetric(&vertices, &indices, 0.2) { + world.insert_soft_body(jelly(capsule, 3.0e4)); + } + + /* + * Rigid boxes to be shoved around by the driven cube. + */ + for i in 0..8 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new( + -0.5 + (i % 4) as Real * 0.5, + 0.25, + 4.0 + (i / 4) as Real * 0.5, + )), + ColliderBuilder::cuboid(0.2, 0.2, 0.2), + ); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(8.0, 7.0, 14.0), Vec3::new(0.0, 1.5, 1.0)); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + t += world.integration_parameters.dt; + // Circle around the rigid boxes at height 0.6, spinning. + let center = Vector::new(2.0 * t.cos(), 0.6, 4.0 + 2.0 * t.sin()); + let rotation = Rotation::from_axis_angle(Vector::Y, 2.0 * t); + world.soft_bodies[driven] + .cluster_mut(0) + .unwrap() + .set_shape_matching_target(Some(Pose::from_parts(center, rotation))); + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/soft_pile3.rs b/examples3d/soft_pile3.rs new file mode 100644 index 000000000..8f68ebab5 --- /dev/null +++ b/examples3d/soft_pile3.rs @@ -0,0 +1,106 @@ +//! Stress test: a big pile of jelly cubes, pressurized balloons and ropes poured into a bin with +//! a few rigid crates, mostly soft-vs-soft contacts under load. Cloth is left out: crumpled +//! cloth pinched in a pile does not settle yet. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Bin. + */ + let half = 5.0; + let (h, _) = world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), + ColliderBuilder::cuboid(half + 0.5, 0.5, half + 0.5), + ); + viewer.set_body_color(h, [0.6, 0.7, 1.0, 0.3].into(), false); + for (dx, dz) in [(1.0, 0.0), (-1.0, 0.0), (0.0, 1.0), (0.0, -1.0)] { + let (hx, hz) = if dx != 0.0 { + (0.25, half + 0.5) + } else { + (half + 0.5, 0.25) + }; + let (h, _) = world.insert( + RigidBodyBuilder::fixed().translation(Vector::new( + dx * (half + 0.25), + 4.0, + dz * (half + 0.25), + )), + ColliderBuilder::cuboid(hx, 4.0, hz), + ); + viewer.set_body_color(h, [0.6, 0.7, 1.0, 0.3].into(), false); + } + + /* + * Layers of soft objects, dropped from increasing heights. + */ + let mut k = 0usize; + for layer in 0..40 { + for i in 0..3 { + for j in 0..3 { + let x = -3.0 + i as Real * 3.0 + (layer % 2) as Real * 0.7; + let z = -3.0 + j as Real * 3.0 + (layer % 3) as Real * 0.5; + let y = 3.0 + layer as Real * 2.5; + match k % 5 { + 0 | 2 => { + // Pressurized balloons. + let balloon = SoftBodyBuilder::sphere(Vector::new(x, y, z), 0.6, 1) + .softness(SpringCoefficients::new(15.0, 1.0)) + .volume_factor(1.15) + .particle_mass(0.03) + .particle_radius(0.08) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.6)); + world.insert_soft_body(balloon); + } + 1 | 3 => { + // Smaller, softer balloons. + let balloon = SoftBodyBuilder::sphere(Vector::new(x, y, z), 0.45, 1) + .softness(SpringCoefficients::new(10.0, 1.0)) + .volume_factor(1.3) + .particle_mass(0.03) + .particle_radius(0.07) + .surface_collider(ColliderBuilder::ball(0.07).friction(0.6)); + world.insert_soft_body(balloon); + } + _ => { + // Rigid crates in the mix. + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(x, y, z)), + ColliderBuilder::cuboid(0.4, 0.4, 0.4).density(0.5), + ); + } + } + k += 1; + } + } + } + + /* + * A couple of ropes thrown on top. + */ + for i in 0..3 { + let z = -2.0 + i as Real * 2.0; + let rope = SoftBodyBuilder::rope( + Vector::new(-3.5, 24.0 + i as Real, z), + Vector::new(3.5, 24.0 + i as Real, z + 0.5), + 30, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.03) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.6)); + world.insert_soft_body(rope); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(14.0, 12.0, 14.0), Vec3::new(0.0, 4.0, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/soft_plasticity3.rs b/examples3d/soft_plasticity3.rs new file mode 100644 index 000000000..eb0e41504 --- /dev/null +++ b/examples3d/soft_plasticity3.rs @@ -0,0 +1,154 @@ +//! Plasticity: elastic cells whose rest shape flows once their strain exceeds a yield +//! (`SoftBodyMaterial::plastic_yield` / `plastic_creep`): blocks of increasing plasticity hit by +//! heavy balls, a stamped clay slab, a creeping column beside an elastic one, clay balls on a wall. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground and a wall for the clay balls. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(14.0, 0.1, 14.0), + ); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(9.0, 2.0, 5.0)), + ColliderBuilder::cuboid(0.2, 2.0, 3.0).friction(0.8), + ); + + let clay = |young: Real, plastic_yield: Real, plastic_creep: Real| SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.35, + elastic_damping_ratio: 1.0, + plastic_yield, + plastic_creep, + deformation_damping: 4.0, + ..Default::default() + }; + + /* + * The yield ladder: identical blocks, from purely elastic (left) to very plastic (right), + * each hit by the same heavy ball. + */ + for (i, plastic_yield) in [0.0, 0.2, 0.08, 0.02].into_iter().enumerate() { + let x = -7.0 + i as Real * 2.6; + let block = SoftBodyBuilder::cuboid(Vector::new(x, 0.6, -4.0), Vector::splat(0.6), 5, 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(clay(1.0e4, plastic_yield, 20.0)) + .particle_mass(0.1) + .surface_collider(ColliderBuilder::ball(0.12).friction(0.8)); + world.insert_soft_body(block); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(x, 5.0, -4.0)), + ColliderBuilder::ball(0.4).density(5.0), + ); + } + + /* + * A clay slab stamped by a kinematic press: the imprints stay after the press lifts. + */ + let slab = SoftBodyBuilder::cuboid( + Vector::new(0.0, 0.4, 1.5), + Vector::new(2.4, 0.4, 1.4), + 13, + 3, + 8, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(clay(3.0e4, 0.02, 50.0)) + .particle_mass(0.1) + .surface_collider(ColliderBuilder::ball(0.15).friction(0.8)); + world.insert_soft_body(slab); + let press_rest = Vector::new(-1.5, 2.2, 1.5); + let (press, _) = world.insert( + RigidBodyBuilder::kinematic_position_based().translation(press_rest), + ColliderBuilder::cuboid(0.35, 0.35, 0.35) + .rotation(Vector::new(0.0, 0.0, core::f32::consts::FRAC_PI_4)) + .friction(0.5), + ); + + /* + * Two soft columns under their own weight: the elastic one stands, the plastic one creeps, + * leans and collapses into a heap (a body whose cells keep flowing is kept awake). + */ + for (i, plastic_yield) in [0.0, 0.05].into_iter().enumerate() { + let x = -6.0 + i as Real * 2.0; + let column = SoftBodyBuilder::cuboid( + Vector::new(x, 1.2, 5.5), + Vector::new(0.3, 1.2, 0.3), + 3, + 12, + 3, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 1.0, + plastic_yield, + plastic_creep: 0.5, + deformation_damping: 4.0, + ..Default::default() + }) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(1.0)); + world.insert_soft_body(column); + } + + /* + * Clay balls thrown at the wall. + */ + let (vertices, indices) = Ball::new(0.5).to_trimesh(12, 12); + for i in 0..3 { + let center = Vector::new( + 4.0 - i as Real * 1.5, + 2.0 + i as Real * 0.3, + 3.5 + i as Real * 1.5, + ); + let vertices: Vec = vertices.iter().map(|p| *p + center).collect(); + if let Some(ball) = SoftBodyBuilder::volumetric(&vertices, &indices, 0.2) { + let ball = ball + .cell_model(SoftBodyCellModel::Corotational) + .material(clay(1.0e4, 0.03, 60.0)) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + let handle = world.insert_soft_body(ball); + let sb = &mut world.soft_bodies[handle]; + for k in 0..sb.num_particles() { + sb.set_particle_velocity(k, Vector::new(10.0, 1.0, 0.0)); + } + } + } + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(4.0, 9.0, 16.0), Vec3::new(0.0, 0.5, 0.0)); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + t += world.integration_parameters.dt; + // The press stamps a new spot every 3 seconds: down for a second, up for a second, + // then slides to the next spot. + let period = 3.0; + let cycle = (t / period).floor(); + let phase = t - cycle * period; + let x = press_rest.x + (cycle % 4.0) * 1.0; + let depth = 1.0; + let y = press_rest.y + - if phase < 1.0 { + depth * phase + } else if phase < 2.0 { + depth * (2.0 - phase) + } else { + 0.0 + }; + world.bodies[press].set_next_kinematic_translation(Vector::new(x, y, press_rest.z)); + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/soft_surface3.rs b/examples3d/soft_surface3.rs new file mode 100644 index 000000000..0bbb75f69 --- /dev/null +++ b/examples3d/soft_surface3.rs @@ -0,0 +1,158 @@ +//! Deformable trimesh collisions: soft bodies colliding through their surface mesh. A coarse +//! trampoline catches a rain of balls smaller than its particle spacing, a tarp drapes over thin +//! poles, soft cubes stack on each other, and a self-colliding ribbon piles up on a pedestal. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -1.0, 0.0)), + ColliderBuilder::cuboid(40.0, 1.0, 40.0), + ); + + /* + * A trampoline with a coarse particle spacing (0.2) under a rain of balls smaller than + * that spacing: they rest on the triangles instead of falling between the particles. + */ + let n = 20; + { + let x0 = -2.0; + let edge = |i: usize, j: usize| i == 0 || j == 0 || i == n - 1 || j == n - 1; + let pinned = (0..n * n) + .filter(|&id| edge(id / n, id % n)) + .map(|id| id as u32); + let trampoline = SoftBodyBuilder::cloth( + Vector::new(x0, 3.0, -2.0), + Vector::new(0.2, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.2), + n, + n, + ) + .pinned_particles(pinned) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.05) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)); + world.insert_soft_body(trampoline); + + // A rain of balls smaller than the particle spacing. + for i in 0..6 { + for j in 0..6 { + for h in 0..3 { + let pos = Vector::new( + x0 + 0.8 + i as Real * 0.5 + (h % 2) as Real * 0.2, + 5.0 + h as Real * 0.6, + -1.2 + j as Real * 0.5 + (h % 2) as Real * 0.2, + ); + world.insert( + RigidBodyBuilder::dynamic().translation(pos), + ColliderBuilder::ball(0.08).density(2.0), + ); + } + } + } + } + + /* + * A tarp draped over a row of thin poles and a sharp-edged bar: with surface collisions the + * poles push on the triangles, not only on the particles. + */ + for i in 0..5 { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-3.0 + i as Real * 1.5, 0.5, 6.0)), + ColliderBuilder::capsule_y(0.5, 0.08), + ); + } + world.insert( + RigidBodyBuilder::fixed() + .translation(Vector::new(0.0, 1.0, 8.0)) + .rotation(Vector::new(core::f32::consts::FRAC_PI_4, 0.0, 0.0)), + ColliderBuilder::cuboid(3.5, 0.4, 0.4), + ); + let tarp = SoftBodyBuilder::cloth( + Vector::new(-4.0, 3.0, 5.0), + Vector::new(0.2, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.2), + 40, + 25, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .particle_mass(0.02) + .particle_radius(0.04) + .surface_collider(ColliderBuilder::ball(0.04).friction(0.8)); + world.insert_soft_body(tarp); + + /* + * A stack of soft cubes: soft-vs-soft contacts go through the surfaces. + */ + for i in 0..3 { + let cube = SoftBodyBuilder::cuboid( + Vector::new(9.0, 0.8 + i as Real * 1.7, 6.0), + Vector::splat(0.75), + 4, + 4, + 4, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 6.0e3, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.8)); + world.insert_soft_body(cube); + } + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(9.0, 6.5, 6.0)), + ColliderBuilder::cuboid(0.4, 0.4, 0.4).density(2.0), + ); + + /* + * A long ribbon with self-contacts dropped on a small pedestal: it folds and piles up on + * itself instead of passing through its own layers. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-9.0, 0.75, 6.0)), + ColliderBuilder::cuboid(0.4, 0.75, 0.4), + ); + let ribbon = SoftBodyBuilder::cloth( + Vector::new(-13.0, 4.0, 5.6), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + 80, + 8, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(2.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .self_contacts(true) + .particle_mass(0.02) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)); + world.insert_soft_body(ribbon); + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(0.0, 12.0, 24.0), Vec3::new(0.0, 1.5, 3.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/soft_tearing3.rs b/examples3d/soft_tearing3.rs new file mode 100644 index 000000000..b4a1f8334 --- /dev/null +++ b/examples3d/soft_tearing3.rs @@ -0,0 +1,134 @@ +//! Tearing (`SoftBodyMaterial::tear_strain`): a pinned sheet ripped by a dropped ball, a curtain +//! shot through by a ball, and a jelly bar pulled apart by its kinematic ends. Cloth springs are +//! stiff (100 Hz) so only impacts pass the 0.4 tear strain; the panel sets `min_piece`. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(30.0, 0.1, 30.0), + ); + + /* + * A sheet pinned along its border: the heavy ball dropped on it rips through. + */ + let n = 40usize; + let border = (0..n * n).filter_map(|k| { + let (i, j) = (k / n, k % n); + (i == 0 || j == 0 || i == n - 1 || j == n - 1).then_some(k as u32) + }); + let sheet = SoftBodyBuilder::cloth( + Vector::new(-6.0, 3.0, -2.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .pinned_particles(border) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + tear_strain: Some(0.4), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(100.0, 1.0)) + }) + .particle_mass(0.02) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.8)); + world.insert_soft_body(sheet); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(-4.0, 6.0, 0.0)), + ColliderBuilder::ball(0.6).density(30.0), + ); + + /* + * A curtain pinned along its top edge, with a heavy ball shot through it. + */ + let curtain = SoftBodyBuilder::cloth( + Vector::new(0.0, 4.5, 4.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, -0.1, 0.0), + 50, + 40, + ) + .pinned_particles((0..50).map(|i| (i * 40) as u32)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + tear_strain: Some(0.2), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(100.0, 1.0)) + }) + .particle_mass(0.02); + world.insert_soft_body(curtain); + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(2.5, 2.5, 12.0)) + .linvel(Vector::new(0.0, 0.0, -25.0)), + ColliderBuilder::cuboid(0.3, 0.3, 0.3) + .rotation(Vector::new(0.5, 0.5, 0.5)) + .density(20.0), + ); + + /* + * A jelly bar whose two ends are pinned: the right end is pulled away until the bar snaps. + */ + let bar = SoftBodyBuilder::cuboid( + Vector::new(3.0, 1.0, -4.0), + Vector::new(2.0, 0.4, 0.4), + 21, + 5, + 5, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + // Stiff enough not to tear from sagging between the clamps (E = 5e3 did). + young_modulus: 5.0e4, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + tear_strain: Some(0.4), + ..Default::default() + }) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + let bar = world.insert_soft_body(bar); + let sb = &world.soft_bodies[bar]; + let ends: Vec<(usize, Vector, bool)> = (0..sb.num_particles()) + .filter_map(|i| { + let p = sb.particle_position(i); + if p.x < 1.01 { + Some((i, p, false)) + } else if p.x > 4.99 { + Some((i, p, true)) + } else { + None + } + }) + .collect(); + for &(i, _, _) in &ends { + world.soft_bodies[bar].set_particle_pinned(i, true); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(9.0, 8.0, 16.0), Vec3::new(0.0, 1.5, 1.0)); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + t += world.integration_parameters.dt; + // The right end of the bar starts moving after a second, at half a meter per + // second, and stops once the bar has doubled its length. + let shift = ((t - 1.0).max(0.0) * 0.5).min(4.0); + let sb = &mut world.soft_bodies[bar]; + for &(i, rest, right) in &ends { + if right { + sb.set_particle_kinematic_target(i, rest + Vector::new(shift, 0.0, 0.0)); + } + } + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/soft_thin_features3.rs b/examples3d/soft_thin_features3.rs new file mode 100644 index 000000000..f3640838d --- /dev/null +++ b/examples3d/soft_thin_features3.rs @@ -0,0 +1,170 @@ +//! Stress test: deformable bodies against thin rigid features (nails, knife edges, a wire grid) +//! and thin rigid bodies (needles, plates, a rod) falling on soft bodies. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), + ColliderBuilder::cuboid(30.0, 0.5, 30.0), + ); + + let jelly = |center: Vector, half: Real, n: usize, young: Real| { + SoftBodyBuilder::cuboid(center, Vector::splat(half), n, n, n) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.7)) + }; + let cloth = |origin: Vector, nu: usize, nv: usize| { + SoftBodyBuilder::cloth( + origin, + Vector::new(0.12, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.12), + nu, + nv, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .particle_mass(0.02) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)) + }; + + /* + * Bed of nails: a jelly block, a balloon and a cloth land on it. + */ + for i in 0..10 { + for j in 0..10 { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new( + -8.0 + i as Real * 0.45, + 0.6, + -2.0 + j as Real * 0.45, + )), + ColliderBuilder::capsule_y(0.6, 0.03), + ); + } + } + world.insert_soft_body(jelly(Vector::new(-7.0, 3.5, -1.0), 0.75, 5, 2.0e4)); + world.insert_soft_body( + SoftBodyBuilder::sphere(Vector::new(-4.8, 3.5, 1.2), 0.7, 2) + .softness(SpringCoefficients::new(15.0, 1.0)) + .volume_factor(1.2) + .particle_mass(0.03) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)), + ); + world.insert_soft_body(cloth(Vector::new(-8.5, 5.5, -2.5), 30, 30)); + + /* + * Knife edges: thin vertical plates; a soft cube dropped across them. + */ + for i in 0..4 { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-1.5, 0.75, -3.0 + i as Real * 0.8)), + ColliderBuilder::cuboid(1.2, 0.75, 0.015), + ); + } + world.insert_soft_body(jelly(Vector::new(-1.5, 3.0, -1.8), 0.9, 5, 5.0e3)); + + /* + * Wire grid: a cloth sags through it. + */ + for i in 0..7 { + let t = -1.8 + i as Real * 0.6; + world.insert( + RigidBodyBuilder::fixed() + .translation(Vector::new(t, 2.5, 5.0)) + .rotation(Vector::new(core::f32::consts::FRAC_PI_2, 0.0, 0.0)), + ColliderBuilder::capsule_y(1.8, 0.02), + ); + world.insert( + RigidBodyBuilder::fixed() + .translation(Vector::new(0.0, 2.5, 5.0 + t)) + .rotation(Vector::new(0.0, 0.0, core::f32::consts::FRAC_PI_2)), + ColliderBuilder::capsule_y(1.8, 0.02), + ); + } + world.insert_soft_body(cloth(Vector::new(-1.5, 4.5, 3.5), 26, 26).self_contacts(true)); + + /* + * Needle rain on a trampoline (cloth pinned along its edges) and on a jelly block. + */ + let n = 26; + let edge = |i: usize, j: usize| i == 0 || j == 0 || i == n - 1 || j == n - 1; + let pinned = (0..n * n) + .filter(|&id| edge(id / n, id % n)) + .map(|id| id as u32); + world.insert_soft_body( + cloth(Vector::new(3.0, 2.5, -3.5), n, n) + .pinned_particles(pinned) + .softness(SpringCoefficients::new(40.0, 1.0)), + ); + world.insert_soft_body(jelly(Vector::new(4.5, 0.9, 3.0), 0.9, 5, 3.0e4)); + for i in 0..6 { + for j in 0..6 { + for (cx, cz, h) in [(4.5, -2.0, 6.0), (4.5, 3.0, 5.0)] { + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new( + cx - 1.0 + i as Real * 0.4, + h + (i + j) as Real * 0.3, + cz - 1.0 + j as Real * 0.4, + )) + .rotation(Vector::new(0.3 * i as Real, 0.0, 0.2 * j as Real)), + ColliderBuilder::capsule_y(0.5, 0.02).density(3.0), + ); + } + } + } + + /* + * Thin plates falling flat on a jelly cube, and a long thin rod across a balloon. + */ + world.insert_soft_body(jelly(Vector::new(9.0, 0.9, -2.0), 0.9, 5, 1.0e4)); + for i in 0..4 { + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(9.0, 3.5 + i as Real * 0.5, -2.0)) + .rotation(Vector::new(0.1 * i as Real, 0.5 * i as Real, 0.05)), + ColliderBuilder::cuboid(0.8, 0.015, 0.8).density(1.0), + ); + } + world.insert_soft_body( + SoftBodyBuilder::sphere(Vector::new(9.0, 1.0, 3.0), 0.9, 2) + .softness(SpringCoefficients::new(15.0, 1.0)) + .volume_factor(1.2) + .particle_mass(0.03) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)), + ); + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(9.0, 3.5, 3.0)) + .rotation(Vector::new(0.0, 0.0, core::f32::consts::FRAC_PI_2)), + ColliderBuilder::capsule_y(2.5, 0.03).density(2.0), + ); + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(2.0, 10.0, 18.0), Vec3::new(1.0, 1.5, 1.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/soft_trimesh3.rs b/examples3d/soft_trimesh3.rs new file mode 100644 index 000000000..90dc3d2b5 --- /dev/null +++ b/examples3d/soft_trimesh3.rs @@ -0,0 +1,475 @@ +//! The dynamic trimeshes demo's models as volumetric soft bodies: the lattice mesher fills each +//! boundary mesh with corotational tetrahedral cells and the model is worn as the drawn skin. The +//! settings panel picks the cover smoothing, crust, subdivision and FEM variants. + +use obj::raw::object::Polygon; +use rapier_testbed3d::TestbedViewer; +use rapier_testbed3d::settings::StringDisplayMode; +use rapier3d::parry::bounding_volume; +use rapier3d::prelude::*; +use std::fs::File; +use std::io::BufReader; + +/// How a model is filled with cells; the settings panel edits one of these. +struct Meshing { + /// The size of the elements (the boundary refines below it, see `cover_subdivisions`). + cell_size: Real, + /// Whether only the model's surface is covered: a hollow shell that takes the model as + /// it is, open meshes included. + crust: bool, + /// Shrink-wrap iterations flattening the cover's staircase (cover and crust). + cover_smoothing: u32, + /// How close to the model the wrap may pull the boundary, as a fraction of the local + /// cell size: the standoff floor of the smoothed cover. + cover_guard: Real, + /// Halvings below the cell size for cells crossing the model's boundary (cover and crust). + cover_subdivisions: u32, +} + +impl Default for Meshing { + fn default() -> Self { + Self { + cell_size: 2.0, + crust: false, + cover_smoothing: 20, + cover_guard: 0.15, + cover_subdivisions: 1, + } + } +} + +impl Meshing { + /// The size of the cells on the model's boundary: the cell size, halved per subdivision. + fn boundary_cell(&self) -> Real { + self.cell_size / (1 << self.cover_subdivisions) as Real + } +} + +const BODY_COLOR: [f32; 4] = [0.85, 0.35, 0.3, 1.0]; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + /* + * Example settings. + */ + let settings = viewer.example_settings_mut(); + + // Overwritten with the real count once the bodies are built; registered here so the + // readout sits at the top of the panel. + settings.set_label("Tetrahedra", "..."); + + let mut model_options = vec!["All models".to_string()]; + model_options.extend(models().iter().map(|path| { + path.rsplit('/') + .next() + .unwrap_or(path) + .trim_end_matches(".obj") + .to_string() + })); + let selected_model = + settings.get_or_set_string_with("Model", 0, model_options, StringDisplayMode::List); + + let defaults = Meshing::default(); + let meshing = Meshing { + cell_size: settings.get_or_set_f32("Cell size", defaults.cell_size, 0.4..=4.0), + crust: settings.get_or_set_bool("Crust (surface shell only)", defaults.crust), + cover_smoothing: settings.get_or_set_u32( + "Cover smoothing", + defaults.cover_smoothing, + 0..=50, + ), + cover_guard: settings.get_or_set_f32("Cover guard", defaults.cover_guard, 0.02..=0.5), + cover_subdivisions: settings.get_or_set_u32( + "Cover subdivision", + defaults.cover_subdivisions, + 0..=3, + ), + }; + // Drop the stale `Enclosure` entry left over from an earlier three-way choice. + settings.remove("Enclosure"); + + let wear_skin = settings.get_or_set_bool("Wear the model as a skin", true); + let skin_collision = if wear_skin { + settings.get_or_set_bool("Skin collisions", false) + } else { + settings.remove("Skin collisions"); + false + }; + let cell_model = settings.get_or_set_string( + "Cell model", + 0, + vec![ + "Corotational".to_string(), + "Neo-Hookean".to_string(), + "Volume".to_string(), + ], + ); + let young_modulus = if cell_model == 2 { + // The volume cells have no elastic modulus to tune. + settings.remove("Young modulus"); + 1.0e5 + } else { + settings.get_or_set_f32("Young modulus", 1.0e5, 1.0e4..=1.0e6) + }; + let self_contacts = settings.get_or_set_bool("Self contacts", false); + + let mut world = PhysicsWorld::new(); + + /* + * Wavy ground (as in the dynamic trimeshes demo). + */ + let nsubdivs = 100; + let heights = Array2::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + -(i as f32 * 40.0 / (nsubdivs as f32) / 2.0).cos() + - (j as f32 * 40.0 / (nsubdivs as f32) / 2.0).cos() + }); + let heightfield = HeightField::new(heights, Vector::new(100.0, 2.0, 100.0)); + let mut trimesh = TriMesh::from(heightfield); + let _ = trimesh.set_flags(TriMeshFlags::FIX_INTERNAL_EDGES); + world.insert_collider(ColliderBuilder::new(SharedShape::new(trimesh)), None); + + /* + * The models, as volumetric soft bodies. + */ + let geoms: Vec = models() + .into_iter() + .enumerate() + .filter(|(igeom, _)| selected_model == 0 || *igeom + 1 == selected_model) + .map(|(_, path)| path) + .collect(); + + let ngeoms = geoms.len(); + let width = (ngeoms as f32).sqrt().ceil().max(1.0) as usize; + let shift_y = 8.0f32; + let shift_xz = 9.0f32; + let mut total_cells = 0; + let mut total_bodies = 0; + + for (igeom, obj_path) in geoms.into_iter().enumerate() { + let Some((vertices, indices)) = load_obj(&obj_path) else { + continue; + }; + let Some(filled) = fill_lattice(&vertices, &indices, &meshing) else { + continue; + }; + + let x = (igeom % width) as f32 * shift_xz - (width - 1) as f32 * shift_xz / 2.0; + let y = (igeom / width) as f32 * shift_y + 7.0; + + // The skin has to be worn before the body is moved: `translated` moves the + // particles and whatever skin is already there, together. + let filled = if wear_skin { + filled + .skin(vertices.clone(), indices.clone()) + .skin_collision(skin_collision) + } else { + filled + }; + + let model = filled + .translated(Vector::new(x, y, 0.0)) + .cell_model(match cell_model { + 1 => SoftBodyCellModel::NeoHookean, + 2 => SoftBodyCellModel::Volume, + _ => SoftBodyCellModel::Corotational, + }) + .material(SoftBodyMaterial { + young_modulus, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.5, + // The models sway on their feet (solver compliance, undamped by the material + // constraints): damp their deformation. + deformation_damping: 2.5, + ..Default::default() + }) + .particle_mass(0.05) + // A quarter of the boundary cell size: the subdivision is what sets the + // boundary's resolution, so the contact skin follows it. + .particle_radius(meshing.boundary_cell() * 0.25) + .self_contacts(self_contacts) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.6)); + + total_cells += model.cells.len(); + total_bodies += 1; + let handle = world.insert_soft_body(model); + viewer.set_initial_soft_body_color(handle, BODY_COLOR.into()); + } + + viewer.example_settings_mut().set_label( + "Tetrahedra", + format!("{total_cells} in {total_bodies} bodies"), + ); + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(60.0, 40.0, 60.0), Vec3::new(0.0, 5.0, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} + +/// Loads an OBJ file as a shared-vertex triangle mesh, centered and scaled to a 10-unit +/// diagonal (as in the dynamic trimeshes demo). +fn load_obj(path: &str) -> Option<(Vec, Vec<[u32; 3]>)> { + let input = BufReader::new(File::open(path).ok()?); + let model = obj::raw::parse_obj(input).ok()?; + let mut vertices: Vec = model + .positions + .iter() + .map(|v| Vector::new(v.0, v.1, v.2)) + .collect(); + let flat: Vec = model + .polygons + .into_iter() + .flat_map(|p| match p { + Polygon::P(idx) => idx.into_iter(), + Polygon::PT(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(), + Polygon::PN(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(), + Polygon::PTN(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(), + }) + .collect(); + let aabb = + bounding_volume::details::point_cloud_aabb(&Pose::IDENTITY, vertices.iter().copied()); + let center = aabb.center(); + let diag = (aabb.maxs - aabb.mins).length(); + vertices + .iter_mut() + .for_each(|p| *p = (*p - Vector::new(center.x, center.y, center.z)) * 10.0 / diag); + let indices: Vec<[u32; 3]> = flat + .chunks(3) + .map(|idx| [idx[0] as u32, idx[1] as u32, idx[2] as u32]) + .collect(); + Some((vertices, indices)) +} + +/// The lattice mesher's parameters for the demo's settings. +fn lattice_params(meshing: &Meshing) -> rapier3d::parry::transformation::VolumeMeshParameters { + use rapier3d::parry::transformation::MeshEnclosure; + + let mut params = rapier3d::parry::transformation::VolumeMeshParameters::new(meshing.cell_size); + params.enclosure = if meshing.crust { + MeshEnclosure::Crust + } else { + MeshEnclosure::Cover + }; + params.cover_smoothing = meshing.cover_smoothing; + params.cover_guard = meshing.cover_guard; + params.cover_subdivisions = meshing.cover_subdivisions; + params +} + +/// Fills a model with the lattice mesher; the crust enclosure takes a model the lattice refuses +/// (not closed) as it is. Shared by the demo and the test that checks it, so they cannot drift. +fn fill_lattice( + vertices: &[Vector], + indices: &[[u32; 3]], + meshing: &Meshing, +) -> Option { + SoftBodyBuilder::volumetric_with(vertices, indices, &lattice_params(meshing)) +} + +/// The models of the dynamic trimeshes demo. `hornbug.obj` is not closed: it only gets +/// cells through the crust enclosure. +const OPEN_MODEL: &str = "assets/3d/hornbug.obj"; + +fn models() -> Vec { + vec![ + "assets/3d/camel_decimated.obj".to_string(), + "assets/3d/chair.obj".to_string(), + "assets/3d/cup_decimated.obj".to_string(), + "assets/3d/dilo_decimated.obj".to_string(), + "assets/3d/tstTorusModel2.obj".to_string(), + "assets/3d/feline_decimated.obj".to_string(), + "assets/3d/genus3_decimated.obj".to_string(), + "assets/3d/tstTorusModel.obj".to_string(), + "assets/3d/octopus_decimated.obj".to_string(), + "assets/3d/rabbit_decimated.obj".to_string(), + "assets/3d/rust_logo_simplified.obj".to_string(), + "assets/3d/screwdriver_decimated.obj".to_string(), + "assets/3d/table.obj".to_string(), + "assets/3d/tstTorusModel3.obj".to_string(), + OPEN_MODEL.to_string(), + ] +} + +#[cfg(test)] +mod tests { + use super::*; + use std::time::Instant; + + /// The connected components of a cell mesh, largest first. + fn components(positions: &[Vector], cells: &[[u32; 4]]) -> Vec { + let mut parent: Vec = (0..positions.len()).collect(); + fn find(parent: &mut [usize], mut i: usize) -> usize { + while parent[i] != i { + parent[i] = parent[parent[i]]; + i = parent[i]; + } + i + } + for cell in cells { + for k in 1..4 { + let (a, b) = ( + find(&mut parent, cell[0] as usize), + find(&mut parent, cell[k] as usize), + ); + parent[a] = b; + } + } + let mut sizes: std::collections::HashMap = Default::default(); + for cell in cells { + *sizes + .entry(find(&mut parent, cell[0] as usize)) + .or_insert(0) += 1; + } + let mut sizes: Vec = sizes.into_values().collect(); + sizes.sort_unstable_by(|a, b| b.cmp(a)); + sizes + } + + /// The volume of a cell, positive when it is the right way round. + fn cell_volume(positions: &[Vector], cell: [u32; 4]) -> Real { + let [a, b, c, d] = cell.map(|index| positions[index as usize]); + (b - a).cross(c - a).dot(d - a) / 6.0 + } + + /// Every closed model fills whole with the demo's default lattice settings (smoothed + /// cover) and the grid settles on the ground without exploding; the open model is + /// refused by default and shelled by the crust. + #[test] + fn demo_models_are_filled_and_settle() { + let mut world = PhysicsWorld::new(); + world.insert_collider( + ColliderBuilder::cuboid(100.0, 1.0, 100.0).translation(Vector::new(0.0, -1.0, 0.0)), + None, + ); + + let meshing = Meshing::default(); + let geoms = models(); + let ngeoms = geoms.len(); + let width = (ngeoms as f32).sqrt() as usize; + let mut cells = 0; + let mut handles = Vec::new(); + let mut labels: Vec = Vec::new(); + let build = Instant::now(); + + for (igeom, obj_path) in geoms.into_iter().enumerate() { + let path = format!("{}/../{obj_path}", env!("CARGO_MANIFEST_DIR")); + let (vertices, indices) = load_obj(&path).unwrap_or_else(|| panic!("{path} missing")); + + let filled = fill_lattice(&vertices, &indices, &meshing); + if obj_path == OPEN_MODEL { + // The open model is refused by default; the crust takes it. + assert!(filled.is_none(), "{obj_path} was expected to be refused"); + + let crusted = fill_lattice( + &vertices, + &indices, + &Meshing { + crust: true, + ..Meshing::default() + }, + ) + .unwrap_or_else(|| panic!("{obj_path} could not be crusted")); + assert!( + crusted + .cells + .iter() + .all(|cell| cell_volume(&crusted.positions, *cell) > 0.0) + ); + println!("{obj_path:44} crust {:>5} cells", crusted.cells.len()); + continue; + } + + let filled = filled.unwrap_or_else(|| panic!("{path} could not be filled")); + + // A model that comes out in pieces falls apart on screen. The lattice mesher + // thickens the shape until it does not, so every closed model comes out whole. + let parts = components(&filled.positions, &filled.cells); + assert_eq!( + parts.len(), + 1, + "{obj_path} came out in {} pieces", + parts.len() + ); + println!("{obj_path:44} {:>5} cells", filled.cells.len()); + + let filled = filled.skin(vertices.clone(), indices.clone()); + cells += filled.cells.len(); + + // The model is worn as a skin, so nothing of it is lost to the cells' + // resolution. + let skin = filled.skin.as_ref().expect("skinned"); + assert_eq!(skin.0.len(), vertices.len()); + + let offset = Vector::new( + (igeom % width) as f32 * 9.0 - width as f32 * 9.0 / 2.0, + (igeom / width) as f32 * 8.0 + 7.0, + 0.0, + ); + labels.push(obj_path.clone()); + handles.push( + world.insert_soft_body( + filled + .translated(offset) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.5, + deformation_damping: 2.5, + ..Default::default() + }) + .particle_mass(0.05) + .particle_radius(meshing.boundary_cell() * 0.25) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.6)), + ), + ); + } + + println!( + "{} bodies over {ngeoms} models, {cells} cells, built in {:?}", + handles.len(), + build.elapsed() + ); + + for _ in 0..600 { + world.step(); + } + + for (handle, label) in handles.iter().zip(&labels) { + let handle = *handle; + let body = &world.soft_bodies[handle]; + let skin = body + .meshes() + .find(|mesh| mesh.is_skinned()) + .expect("skinned"); + assert!( + skin.vertices().iter().all(|v| v.is_finite()), + "{label}: the skin went non-finite" + ); + let lowest = skin.vertices().iter().map(|v| v.y).fold(f32::MAX, f32::min); + assert!( + lowest < 7.0, + "{label}: the skin did not come down with the cells" + ); + + let max_v = body + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, f32::max); + assert!( + max_v.is_finite() && max_v < 20.0, + "{label} exploded: {max_v} m/s" + ); + let min_y = body + .particle_positions() + .map(|p| p.y) + .fold(f32::MAX, f32::min); + assert!(min_y > -0.5, "{label} sank through the ground: {min_y}"); + } + } +} From 257579f325167797b6cd063e77fda792dfb00c45 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 29 Aug 2026 18:01:34 +0200 Subject: [PATCH 03/32] feat(soft-body): add the FEM solver behind the `fem` feature, with demos, tests and a bench --- crates/rapier2d-f64/Cargo.toml | 7 + crates/rapier2d/Cargo.toml | 7 + crates/rapier2d/tests/soft_bodies.rs | 168 ++++ crates/rapier3d-f64/Cargo.toml | 7 + crates/rapier3d/Cargo.toml | 7 + crates/rapier3d/examples/fem_bench.rs | 184 +++++ crates/rapier3d/tests/soft_bodies.rs | 745 ++++++++++++++++++ .../tests/thread_count_determinism.rs | 48 +- crates/rapier_testbed2d-f64/Cargo.toml | 3 + crates/rapier_testbed2d/Cargo.toml | 3 + crates/rapier_testbed3d-f64/Cargo.toml | 3 + crates/rapier_testbed3d/Cargo.toml | 3 + examples2d/Cargo.toml | 3 +- examples2d/all_examples2.rs | 2 + examples2d/soft_fem2.rs | 98 +++ examples3d/Cargo.toml | 3 +- examples3d/all_examples3.rs | 2 + examples3d/soft_fem3.rs | 105 +++ examples3d/soft_trimesh3.rs | 16 + src/dynamics/integration_parameters.rs | 2 + src/dynamics/soft_body/mod.rs | 2 + src/dynamics/soft_body/soft_body.rs | 4 + src/dynamics/soft_body/soft_body_accessors.rs | 26 + .../soft_body_builder/soft_body_builder.rs | 3 + .../soft_body_builder_settings.rs | 7 + .../soft_body_builder_shapes.rs | 2 + src/dynamics/soft_body/soft_body_elements.rs | 16 + .../soft_body_set_insert_remove.rs | 2 + src/dynamics/soft_body/soft_body_settings.rs | 44 ++ src/dynamics/solver/mod.rs | 2 + src/dynamics/solver/soft_constraint/mod.rs | 3 +- .../soft_elastic_constraint.rs | 17 + .../soft_element_linalg.rs | 12 + .../soft_neo_hookean.rs | 12 +- .../soft_scalar_constraint.rs | 45 ++ .../soft_element_constraint_assembly.rs | 51 +- src/dynamics/solver/soft_fem/mod.rs | 10 + src/dynamics/solver/soft_fem/soft_fem_set.rs | 175 ++++ .../solver/soft_fem/soft_fem_skyline.rs | 348 ++++++++ .../solver/soft_fem/soft_fem_sparse.rs | 372 +++++++++ src/dynamics/solver/soft_fem/system/mod.rs | 11 + .../system/soft_fem_system_assemble.rs | 260 ++++++ .../system/soft_fem_system_elements.rs | 107 +++ .../system/soft_fem_system_prepare.rs | 237 ++++++ .../solver/staged_island_solver/init.rs | 13 + .../solver/staged_island_solver/mod.rs | 18 +- .../solver/staged_island_solver/solve.rs | 1 + .../solver/staged_island_solver/worker.rs | 54 ++ 48 files changed, 3245 insertions(+), 25 deletions(-) create mode 100644 crates/rapier3d/examples/fem_bench.rs create mode 100644 examples2d/soft_fem2.rs create mode 100644 examples3d/soft_fem3.rs create mode 100644 src/dynamics/solver/soft_fem/mod.rs create mode 100644 src/dynamics/solver/soft_fem/soft_fem_set.rs create mode 100644 src/dynamics/solver/soft_fem/soft_fem_skyline.rs create mode 100644 src/dynamics/solver/soft_fem/soft_fem_sparse.rs create mode 100644 src/dynamics/solver/soft_fem/system/mod.rs create mode 100644 src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs create mode 100644 src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs create mode 100644 src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index ace823f3c..7b64eacf6 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -49,6 +49,13 @@ std = [ "thiserror/std", "serde?/std", ] +# Adds an alternative FEM solver for soft bodies, selected per body with +# `SoftBodyBuilder::solver(SoftBodySolver::Fem)`. Instead of resolving elasticity +# as sequential impulse rows, the FEM path assembles the element forces and +# tangent stiffness of a strain-energy density and takes one implicit Euler step +# per substep, solving `(M + h D + h^2 K) dv = b` over the whole body. Off by +# default: it adds per-body solver state and is still experimental. +fem = [] parallel = ["dep:rayon", "std", "parry2d-f64/parallel"] # Drops the `Sync` requirement from callbacks (`PhysicsHooks`, `EventHandler`). # As a side effect, this also removes the dedicated threadpool API from the physics world diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index 930ed4551..366e833e9 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -50,6 +50,13 @@ std = [ "wide/std", "serde?/std", ] +# Adds an alternative FEM solver for soft bodies, selected per body with +# `SoftBodyBuilder::solver(SoftBodySolver::Fem)`. Instead of resolving elasticity +# as sequential impulse rows, the FEM path assembles the element forces and +# tangent stiffness of a strain-energy density and takes one implicit Euler step +# per substep, solving `(M + h D + h^2 K) dv = b` over the whole body. Off by +# default: it adds per-body solver state and is still experimental. +fem = [] parallel = ["dep:rayon", "std", "parry2d/parallel"] # Drops the `Sync` requirement from callbacks (`PhysicsHooks`, `EventHandler`). # As a side effect, this also removes the dedicated threadpool API from the physics world diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index eb0fbeb6d..32c6d9987 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -1372,3 +1372,171 @@ fn polyline_soft_body() { "the strip did not fall flat: {strip_y}" ); } + +// --------------------------------------------------------------------------------------------- +// The FEM soft-body solver (`fem` cargo feature, `SoftBodySolver::Fem`). +// --------------------------------------------------------------------------------------------- + +/// A FEM soft body with no contacts falls exactly like a point mass, and does not deform. +#[cfg(feature = "fem")] +#[test] +fn fem_free_fall_is_exact() { + let mut world = PhysicsWorld::new(); + world.integration_parameters.num_solver_iterations = 1; + let builder = SoftBodyBuilder::grid(Vector::new(0.0, 10.0), Vector::splat(0.5), 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .solver(SoftBodySolver::Fem) + .without_colliders() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + let start = world.soft_bodies[handle].particle_position(0); + let steps = 60; + for _ in 0..steps { + world.step(); + } + let dt = world.integration_parameters.dt; + let expected = world.gravity.y * dt * dt * (steps * (steps + 1) / 2) as Real; + let sb = &world.soft_bodies[handle]; + let drop = sb.particle_position(0).y - start.y; + assert!( + (drop - expected).abs() < 1.0e-4 * expected.abs(), + "free fall {drop} vs expected {expected}" + ); + for i in 0..sb.num_particles() { + let v = sb.particle_velocity(i) - sb.particle_velocity(0); + assert!(v.length() < 1.0e-4, "particle {i} drifted at {v:?}"); + } +} + +/// A stiff FEM square dropped on the ground settles, keeps its area and does not explode, at any +/// Young's modulus. +#[cfg(feature = "fem")] +#[test] +fn fem_stiff_square_is_stable() { + for young in [3.0e4, 3.0e6, 1.0e8] { + let mut world = world_with_ground(); + let square = SoftBodyBuilder::grid(Vector::new(0.0, 3.0), Vector::splat(0.75), 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .particle_mass(0.1); + let handle = world.insert_soft_body(square); + for _ in 0..500 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let max_vel = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + assert!( + max_vel < 0.2, + "E = {young}: square still moving at {max_vel} m/s" + ); + let area = sb.volume(); + assert!( + (area - sb.rest_volume()).abs() < 0.1 * sb.rest_volume(), + "E = {young}: area {area} vs rest {}", + sb.rest_volume() + ); + } +} + +/// The FEM path's static deflection does not depend on the substep count. +#[cfg(feature = "fem")] +#[test] +fn fem_deflection_is_substep_invariant() { + let (length, thickness, young) = (2.0, 0.2, 2.0e6); + let settle = |substeps: usize| -> Real { + let mut world = PhysicsWorld::new(); + world.integration_parameters.num_solver_iterations = substeps; + let builder = SoftBodyBuilder::grid( + Vector::new(length * 0.5, 0.0), + Vector::new(length * 0.5, thickness * 0.5), + 13, + 3, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.0, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .mass(2.0) + .without_colliders() + .can_sleep(false); + let pinned: Vec = builder + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x < 1.0e-4) + .map(|(i, _)| i as u32) + .collect(); + let tip: Vec = builder + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x > length - 1.0e-4) + .map(|(i, _)| i) + .collect(); + let handle = world.insert_soft_body(builder.pinned_particles(pinned)); + for _ in 0..1500 { + world.step(); + } + let sb = &world.soft_bodies[handle]; + tip.iter().map(|&i| sb.particle_position(i).y).sum::() / tip.len() as Real + }; + let deflections: Vec = [1usize, 2, 8].iter().map(|s| settle(*s)).collect(); + let reference = deflections[2]; + assert!(reference < -1.0e-3, "the beam did not deflect: {reference}"); + for (k, d) in deflections.iter().enumerate() { + assert!( + (d - reference).abs() < 0.05 * reference.abs(), + "FEM deflection depends on the substep count: {deflections:?} (case {k})" + ); + } +} + +/// Contacts against a FEM soft body: a rigid box dropped on one settles on its surface. +#[cfg(feature = "fem")] +#[test] +fn fem_box_rests_on_soft_body() { + let mut world = world_with_ground(); + let square = SoftBodyBuilder::grid(Vector::new(0.0, 0.6), Vector::splat(0.6), 6, 6) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e3, + poisson_ratio: 0.35, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .particle_mass(0.2); + let handle = world.insert_soft_body(square); + let (rb, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.05, 2.0)), + ColliderBuilder::cuboid(0.2, 0.2).density(2.0), + ); + for _ in 0..600 { + world.step(); + } + assert_finite(&world, handle); + let box_pos = world.bodies[rb].translation(); + assert!( + box_pos.y > 1.1, + "box sank into the soft square: {box_pos:?}" + ); + assert!( + box_pos.y < 1.7, + "box floats above the soft square: {box_pos:?}" + ); + assert!(world.bodies[rb].linvel().length() < 0.05); +} diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml index 1b31f1330..42da077dd 100644 --- a/crates/rapier3d-f64/Cargo.toml +++ b/crates/rapier3d-f64/Cargo.toml @@ -48,6 +48,13 @@ std = [ "thiserror/std", "serde?/std", ] +# Adds an alternative FEM solver for soft bodies, selected per body with +# `SoftBodyBuilder::solver(SoftBodySolver::Fem)`. Instead of resolving elasticity +# as sequential impulse rows, the FEM path assembles the element forces and +# tangent stiffness of a strain-energy density and takes one implicit Euler step +# per substep, solving `(M + h D + h^2 K) dv = b` over the whole body. Off by +# default: it adds per-body solver state and is still experimental. +fem = [] parallel = ["dep:rayon", "std", "parry3d-f64/parallel"] # Drops the `Sync` requirement from callbacks (`PhysicsHooks`, `EventHandler`). # As a side effect, this also removes the dedicated threadpool API from the physics world diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml index cfbf8362a..fbb641d10 100644 --- a/crates/rapier3d/Cargo.toml +++ b/crates/rapier3d/Cargo.toml @@ -49,6 +49,13 @@ std = [ "wide/std", "serde?/std", ] +# Adds an alternative FEM solver for soft bodies, selected per body with +# `SoftBodyBuilder::solver(SoftBodySolver::Fem)`. Instead of resolving elasticity +# as sequential impulse rows, the FEM path assembles the element forces and +# tangent stiffness of a strain-energy density and takes one implicit Euler step +# per substep, solving `(M + h D + h^2 K) dv = b` over the whole body. Off by +# default: it adds per-body solver state and is still experimental. +fem = [] parallel = ["dep:rayon", "std", "parry3d/parallel"] # Drops the `Sync` requirement from callbacks (`PhysicsHooks`, `EventHandler`). # As a side effect, this also removes the dedicated threadpool API from the physics world diff --git a/crates/rapier3d/examples/fem_bench.rs b/crates/rapier3d/examples/fem_bench.rs new file mode 100644 index 000000000..eba050418 --- /dev/null +++ b/crates/rapier3d/examples/fem_bench.rs @@ -0,0 +1,184 @@ +//! `cargo run -p rapier3d --features fem --release --example fem_bench`: A/B of the FEM soft-body +//! solver against the constraint rows; reports per scene and solver the median step time and a +//! quality figure (a loaded beam's static deflection or a settling body's residual velocity). + +#[cfg(feature = "fem")] +use rapier3d::prelude::*; +#[cfg(feature = "fem")] +use std::time::Instant; + +#[cfg(not(feature = "fem"))] +fn main() { + println!("this bench needs --features fem"); +} + +#[cfg(feature = "fem")] +fn main() { + println!( + "{:<22} {:<12} {:>7} {:>10} {:>12} {:>12}", + "scene", "solver", "substeps", "ms/step", "deflection", "max |v|" + ); + for substeps in [1usize, 4] { + for solver in [SoftBodySolver::Constraints, SoftBodySolver::Fem] { + cantilever(solver, substeps); + } + } + for solver in [SoftBodySolver::Constraints, SoftBodySolver::Fem] { + jelly_pile(solver); + cloth(solver); + } +} + +#[cfg(feature = "fem")] +fn report( + scene: &str, + solver: SoftBodySolver, + substeps: usize, + ms: f64, + deflection: Real, + v: Real, +) { + let name = match solver { + SoftBodySolver::Constraints => "constraints", + SoftBodySolver::Fem => "fem", + }; + println!("{scene:<22} {name:<12} {substeps:>7} {ms:>10.3} {deflection:>12.5} {v:>12.5}"); +} + +/// Median of the per-step times of `steps` steps, discarding the first tenth (warm-up). +#[cfg(feature = "fem")] +fn run(world: &mut PhysicsWorld, steps: usize) -> f64 { + let mut times = Vec::with_capacity(steps); + for _ in 0..steps { + let start = Instant::now(); + world.step(); + times.push(start.elapsed().as_secs_f64() * 1000.0); + } + let skip = steps / 10; + let mut tail: Vec = times[skip..].to_vec(); + tail.sort_by(|a, b| a.partial_cmp(b).unwrap()); + tail[tail.len() / 2] +} + +/// A stiff cantilever under its own weight: the deflection is the quality figure (the analytic +/// Euler-Bernoulli value is -0.0491 m). +#[cfg(feature = "fem")] +fn cantilever(solver: SoftBodySolver, substeps: usize) { + let (length, thickness, young) = (2.0, 0.2, 1.2e7); + let mut world = PhysicsWorld::new(); + world.integration_parameters.num_solver_iterations = substeps; + let half = Vector::new(length * 0.5, thickness * 0.5, thickness * 0.5); + let builder = SoftBodyBuilder::cuboid(Vector::new(length * 0.5, 0.0, 0.0), half, 9, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.0, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .solver(solver) + .mass(8.0) + .without_colliders() + .can_sleep(false); + let pinned: Vec = builder + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x < 1.0e-4) + .map(|(i, _)| i as u32) + .collect(); + let tip: Vec = builder + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x > length - 1.0e-4) + .map(|(i, _)| i) + .collect(); + let handle = world.insert_soft_body(builder.pinned_particles(pinned)); + let ms = run(&mut world, 1500); + let sb = &world.soft_bodies[handle]; + let deflection = + tip.iter().map(|&i| sb.particle_position(i).y).sum::() / tip.len() as Real; + let max_v = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + report("stiff cantilever", solver, substeps, ms, deflection, max_v); +} + +/// Nine jelly cubes dropped in a pile: the throughput case (contacts and self contacts). +#[cfg(feature = "fem")] +fn jelly_pile(solver: SoftBodySolver) { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(12.0, 0.1, 12.0), + ); + for i in 0..9 { + let x = ((i % 3) as Real - 1.0) * 1.0; + let z = ((i / 3) as Real - 1.0) * 1.0; + let cube = SoftBodyBuilder::cuboid( + Vector::new(x, 0.55 + i as Real * 1.05, z), + Vector::splat(0.5), + 4, + 4, + 4, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 3.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .solver(solver) + .can_sleep(false) + .particle_mass(0.1); + world.insert_soft_body(cube); + } + // Let the pile land before timing it: the drop itself is not the throughput case. + for _ in 0..400 { + world.step(); + } + let ms = run(&mut world, 600); + let max_v = world + .soft_bodies + .iter() + .flat_map(|(_, sb)| sb.particles().iter()) + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + report("jelly pile (9 cubes)", solver, 4, ms, 0.0, max_v); +} + +/// A pinned cloth: distance and dihedral elements only, no cells. +#[cfg(feature = "fem")] +fn cloth(solver: SoftBodySolver) { + let mut world = PhysicsWorld::new(); + let n = 30; + let cloth = SoftBodyBuilder::cloth( + Vector::new(0.0, 3.0, 0.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .pinned_particles([0, (n - 1) as u32]) + .softness(SpringCoefficients::new(100.0, 1.0)) + .solver(solver) + .can_sleep(false) + .particle_mass(0.02); + let handle = world.insert_soft_body(cloth); + let ms = run(&mut world, 600); + let sb = &world.soft_bodies[handle]; + let max_v = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + let lowest = sb + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + report("cloth 30x30", solver, 4, ms, lowest, max_v); +} diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index a8f73ab53..c1c1594e1 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -2868,3 +2868,748 @@ fn volumetric_subdivision_costs_fewer_cells() { assert!(lowest > -0.1 && lowest < 0.6, "the ball ended at {lowest}"); } } + +// --------------------------------------------------------------------------------------------- +// The FEM soft-body solver (`fem` cargo feature, `SoftBodySolver::Fem`). +// --------------------------------------------------------------------------------------------- + +/// A corotational bar built as a soft body, optionally FEM-solved, cantilevered from its `x = 0` +/// face. Returns the builder and the particles of its free end. +#[cfg(feature = "fem")] +fn fem_cantilever( + solver: SoftBodySolver, + young: Real, + length: Real, + thickness: Real, + nx: usize, + nt: usize, +) -> (SoftBodyBuilder, Vec) { + let half = Vector::new(length * 0.5, thickness * 0.5, thickness * 0.5); + let builder = SoftBodyBuilder::cuboid(Vector::new(length * 0.5, 0.0, 0.0), half, nx, nt, nt) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.0, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .solver(solver) + .without_colliders() + .can_sleep(false); + // Pin every particle of the root face. + let pinned: Vec = builder + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x < 1.0e-4) + .map(|(i, _)| i as u32) + .collect(); + let tip: Vec = builder + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x > length - 1.0e-4) + .map(|(i, _)| i) + .collect(); + (builder.pinned_particles(pinned), tip) +} + +/// The mean height of a cantilever's free end (it starts flat at `y = 0`). +#[cfg(feature = "fem")] +fn cantilever_tip(world: &PhysicsWorld, handle: SoftBodyHandle, tip: &[usize]) -> Real { + let sb = &world.soft_bodies[handle]; + tip.iter().map(|&i| sb.particle_position(i).y).sum::() / tip.len() as Real +} + +/// A FEM soft body with no elements and no contacts must fall exactly like a point mass: the +/// implicit elastic step must not disturb free fall. +#[cfg(feature = "fem")] +#[test] +fn fem_free_fall_is_exact() { + let mut world = PhysicsWorld::new(); + world.integration_parameters.num_solver_iterations = 1; + let builder = SoftBodyBuilder::cuboid(Vector::new(0.0, 10.0, 0.0), Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .solver(SoftBodySolver::Fem) + .without_colliders() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + let start = world.soft_bodies[handle].particle_position(0); + let steps = 60; + for _ in 0..steps { + world.step(); + } + let dt = world.integration_parameters.dt; + // Semi-implicit Euler over `steps` steps: Δy = g Σ_{k=1..n} k dt² = g dt² n(n+1)/2. + let expected = world.gravity.y * dt * dt * (steps * (steps + 1) / 2) as Real; + let sb = &world.soft_bodies[handle]; + let drop = sb.particle_position(0).y - start.y; + assert!( + (drop - expected).abs() < 1.0e-4 * expected.abs(), + "free fall {drop} vs expected {expected}" + ); + // And no internal motion at all. + for i in 0..sb.num_particles() { + let v = sb.particle_velocity(i) - sb.particle_velocity(0); + assert!(v.length() < 1.0e-4, "particle {i} drifted at {v:?}"); + } +} + +/// A cantilever's tip deflection under its own weight must match the Euler-Bernoulli value +/// `δ = q L⁴ / (8 E I)` within the discretization error of a coarse mesh. +#[cfg(feature = "fem")] +#[test] +fn fem_cantilever_matches_euler_bernoulli() { + let (length, thickness, young) = (2.0, 0.2, 1.2e7); + let density = 100.0; + let mut world = PhysicsWorld::new(); + world.integration_parameters.num_solver_iterations = 4; + let mass = density * length * thickness * thickness; + let (builder, tip_particles) = + fem_cantilever(SoftBodySolver::Fem, young, length, thickness, 9, 3); + let handle = world.insert_soft_body(builder.mass(mass)); + for _ in 0..1200 { + world.step(); + } + assert_finite(&world, handle); + let tip = cantilever_tip(&world, handle, &tip_particles); + // q = weight per unit length, I = b h³ / 12. + let q = mass * -world.gravity.y / length; + let inertia = thickness * thickness * thickness * thickness / 12.0; + let expected = -q * length * length * length * length / (8.0 * young * inertia); + // Linear tetrahedra lock in bending, so a mesh three elements thick reads stiffer than the + // analytic beam; the point of the test is the order of magnitude and the scaling, not an + // exact match. + let ratio = tip / expected; + assert!( + (0.2..1.1).contains(&ratio), + "tip deflection {tip} vs Euler-Bernoulli {expected} (ratio {ratio})" + ); +} + +/// A stiff FEM cube dropped on the ground settles without exploding, and keeps its volume. +#[cfg(feature = "fem")] +#[test] +fn fem_stiff_cube_is_stable() { + for young in [3.0e4, 3.0e6, 1.0e8] { + let mut world = world_with_ground(); + let cube = + SoftBodyBuilder::cuboid(Vector::new(0.0, 3.0, 0.0), Vector::splat(0.75), 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .particle_mass(0.1); + let handle = world.insert_soft_body(cube); + for _ in 0..500 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let max_vel = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + assert!( + max_vel < 0.2, + "E = {young}: cube still moving at {max_vel} m/s" + ); + let volume = sb.volume(); + assert!( + (volume - sb.rest_volume()).abs() < 0.1 * sb.rest_volume(), + "E = {young}: volume {volume} vs rest {}", + sb.rest_volume() + ); + } +} + +/// The FEM path's static deflection must not depend on the substep count: elasticity is solved +/// globally, not relaxed. The constraint path's does (it converges toward the same answer as the +/// sweeps pile up), which is the whole point of the FEM solver. +#[cfg(feature = "fem")] +#[test] +fn fem_deflection_is_substep_invariant() { + let (length, thickness) = (2.0, 0.2); + let mass = 8.0; + let settle = |solver: SoftBodySolver, substeps: usize, young: Real| -> Real { + let mut world = PhysicsWorld::new(); + world.integration_parameters.num_solver_iterations = substeps; + let (builder, tip) = fem_cantilever(solver, young, length, thickness, 9, 3); + let handle = world.insert_soft_body(builder.mass(mass)); + for _ in 0..1500 { + world.step(); + } + cantilever_tip(&world, handle, &tip) + }; + + // A stiff beam: the FEM deflection must be the same at 1, 2 and 8 substeps. + let fem: Vec = [1, 2, 8] + .iter() + .map(|s| settle(SoftBodySolver::Fem, *s, 1.2e7)) + .collect(); + let reference = fem[2]; + for (k, d) in fem.iter().enumerate() { + assert!( + (d - reference).abs() < 0.05 * reference.abs(), + "FEM deflection depends on the substep count: {fem:?} (case {k})" + ); + } + + // The same comparison on the constraint path, at a modulus it survives at one substep: it + // is much softer there than at eight substeps. This is the gap the FEM path closes, asserted + // so the comparison does not silently rot. + let constraints_1 = settle(SoftBodySolver::Constraints, 1, 3.0e5); + let constraints_8 = settle(SoftBodySolver::Constraints, 8, 3.0e5); + assert!( + (constraints_1 - constraints_8).abs() > 0.2 * constraints_8.abs(), + "the constraint path became substep-invariant: {constraints_1} vs {constraints_8}" + ); + let fem_1 = settle(SoftBodySolver::Fem, 1, 3.0e5); + let fem_8 = settle(SoftBodySolver::Fem, 8, 3.0e5); + assert!( + (fem_1 - fem_8).abs() < 0.05 * fem_8.abs(), + "FEM deflection depends on the substep count: {fem_1} vs {fem_8}" + ); +} + +/// Contacts against a FEM soft body: a rigid box dropped on it settles on the surface and both +/// come to rest. The contact impulses reach the whole body through the propagate stage. +#[cfg(feature = "fem")] +#[test] +fn fem_box_rests_on_soft_body() { + let mut world = world_with_ground(); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 0.6, 0.0), Vector::splat(0.6), 5, 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e3, + poisson_ratio: 0.35, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .particle_mass(0.2); + let handle = world.insert_soft_body(cube); + let (rb, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.1, 2.0, 0.1)), + ColliderBuilder::cuboid(0.2, 0.2, 0.2).density(2.0), + ); + for _ in 0..600 { + world.step(); + } + assert_finite(&world, handle); + let box_pos = world.bodies[rb].translation(); + assert!(box_pos.y > 1.2, "box sank into the soft cube: {box_pos:?}"); + assert!( + box_pos.y < 1.7, + "box floats above the soft cube: {box_pos:?}" + ); + for p in world.soft_bodies[handle].particles() { + let v = p.velocity().length(); + assert!(v < 0.05, "residual particle velocity {v}"); + } + assert!(world.bodies[rb].linvel().length() < 0.05); + let com = world.soft_bodies[handle].center_of_mass(); + assert!( + com.x.abs() < 0.05 && com.z.abs() < 0.05, + "soft cube crept: {com:?}" + ); +} + +/// A FEM soft body attached to a kinematic rigid body follows it, and the whole body comes along +/// through its elasticity. +#[cfg(feature = "fem")] +#[test] +fn fem_attachment_follows_its_body() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let (anchor, _) = world.insert( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 0.0, 0.0)), + ColliderBuilder::ball(0.05).density(0.0), + ); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.5, 0.0, 0.0), Vector::splat(0.5), 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .without_colliders() + .can_sleep(false) + .particle_mass(0.05); + let handle = world.insert_soft_body(cube); + // Attach every particle of the near face. + let attached: Vec = (0..world.soft_bodies[handle].num_particles()) + .filter(|&i| world.soft_bodies[handle].particle_position(i).x < 1.0e-4) + .collect(); + assert!(!attached.is_empty()); + for &i in &attached { + let bodies = &world.bodies; + world.soft_bodies[handle].attach_particle(i, anchor, bodies); + } + // Drag the anchor along +y. + for step in 0..400 { + let y = (step + 1) as Real * 0.005; + world.bodies[anchor].set_next_kinematic_translation(Vector::new(0.0, y, 0.0)); + world.step(); + } + assert_finite(&world, handle); + let anchor_y = world.bodies[anchor].translation().y; + let com = world.soft_bodies[handle].center_of_mass(); + assert!( + (com.y - anchor_y).abs() < 0.2, + "the soft body did not follow its anchor: com {com:?}, anchor y {anchor_y}" + ); +} + +/// Two-way coupling: a free-floating FEM soft body hit by a rigid body conserves the total +/// linear momentum of the pair. +#[cfg(feature = "fem")] +#[test] +fn fem_coupling_conserves_momentum() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 0.0, 0.0), Vector::splat(0.5), 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + elastic_damping_ratio: 0.2, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .can_sleep(false) + .particle_mass(0.05); + let handle = world.insert_soft_body(cube); + let (rb, _) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(-2.0, 0.0, 0.0)) + .linvel(Vector::new(4.0, 0.0, 0.0)) + .gravity_scale(0.0), + ColliderBuilder::ball(0.2).density(1.0), + ); + let momentum = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[handle]; + let soft: Vector = sb + .particles() + .iter() + .map(|p| p.velocity() * p.mass()) + .fold(Vector::ZERO, |a, b| a + b); + soft + world.bodies[rb].linvel() * world.bodies[rb].mass() + }; + world.step(); + let p0 = momentum(&world); + for _ in 0..400 { + world.step(); + } + assert_finite(&world, handle); + let p1 = momentum(&world); + assert!( + (p1 - p0).length() < 0.1 * p0.length(), + "momentum drifted: {p0:?} -> {p1:?}" + ); +} + +/// An impact on a stiff FEM body is absorbed by the whole body, not bounced off the few +/// particles the contacts touch: the constraints see the body through its augmented mass and +/// its elastic response; with lumped particle masses the same box rebounds at 19.9 of 20 m/s. +#[cfg(feature = "fem")] +#[test] +fn fem_impact_is_absorbed_by_the_whole_body() { + let mut world = PhysicsWorld::new(); + let plank = SoftBodyBuilder::cuboid( + Vector::new(0.0, 1.0, 0.0), + Vector::new(1.5, 0.1, 0.4), + 13, + 3, + 5, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e6, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .can_sleep(false) + .particle_mass(0.02); + // Pinned at both ends. + let pinned: Vec = plank + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x.abs() > 1.4) + .map(|(i, _)| i as u32) + .collect(); + let handle = world.insert_soft_body(plank.pinned_particles(pinned)); + let impact_speed = 20.0; + let (rb, _) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 1.6, 0.0)) + .linvel(Vector::new(0.0, -impact_speed, 0.0)) + .gravity_scale(0.0), + ColliderBuilder::cuboid(0.25, 0.25, 0.25).density(8.0), + ); + let mut rebound: Real = 0.0; + for _ in 0..120 { + world.step(); + rebound = rebound.max(world.bodies[rb].linvel().y); + } + assert_finite(&world, handle); + assert!( + rebound < 0.4 * impact_speed, + "the box bounced off the FEM plank at {rebound} m/s (impact {impact_speed})" + ); + assert!(rebound > 0.0, "the box went through the plank"); +} + +/// The FEM path's distance elements reproduce the constraint path's springs: the period of a +/// mass-spring pair is still set by the material's natural frequency, whatever the substep +/// count. +#[cfg(feature = "fem")] +#[test] +fn fem_edge_period_is_substep_invariant() { + let natural_frequency = 2.0; // Hz + let mut periods = Vec::new(); + for substeps in [1usize, 4, 16] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.num_solver_iterations = substeps; + world.integration_parameters.dt = 1.0 / 240.0; + let builder = SoftBodyBuilder::new(vec![Vector::ZERO, Vector::new(1.0, 0.0, 0.0)]) + .edges(vec![[0, 1]]) + .pinned_particles([0]) + .softness(SpringCoefficients::new(natural_frequency, 0.0)) + .solver(SoftBodySolver::Fem) + .without_colliders() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + world.soft_bodies[handle].set_particle_position(1, Vector::new(1.2, 0.0, 0.0)); + + let mut crossings = Vec::new(); + let mut prev_len = 1.2; + for step in 0..2000 { + world.step(); + let len = world.soft_bodies[handle].particle_position(1).x; + if prev_len > 1.0 && len <= 1.0 { + crossings.push(step as Real * world.integration_parameters.dt); + if crossings.len() == 2 { + break; + } + } + prev_len = len; + } + assert_eq!( + crossings.len(), + 2, + "no oscillation with {substeps} substeps" + ); + periods.push(crossings[1] - crossings[0]); + } + let expected = 1.0 / natural_frequency; + for (i, period) in periods.iter().enumerate() { + assert!( + (period - expected).abs() < 0.08 * expected, + "period {period} (substeps case {i}) too far from {expected}" + ); + } +} + +/// A FEM rope pinned at one end hangs from its anchor without stretching much: the distance +/// elements bear the load. +#[cfg(feature = "fem")] +#[test] +fn fem_rope_hangs_from_anchor() { + let mut world = PhysicsWorld::new(); + let rope = SoftBodyBuilder::rope(Vector::ZERO, Vector::new(2.0, 0.0, 0.0), 21) + .pinned_particles([0]) + .softness(SpringCoefficients::new(300.0, 1.0)) + .solver(SoftBodySolver::Fem) + .without_colliders() + .can_sleep(false); + let handle = world.insert_soft_body(rope); + for _ in 0..600 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let mut length = 0.0; + for i in 1..sb.num_particles() { + length += (sb.particle_position(i) - sb.particle_position(i - 1)).length(); + } + assert!( + (length - 2.0).abs() < 0.1, + "the FEM rope stretched to {length}" + ); + let tip = sb.particle_position(sb.num_particles() - 1); + assert!(tip.y < -1.5, "the rope did not hang: tip {tip:?}"); +} + +/// A FEM cloth (distance elements plus dihedral bending) pinned at two corners drapes without +/// over-stretching. +#[cfg(feature = "fem")] +#[test] +fn fem_cloth_drapes() { + let mut world = PhysicsWorld::new(); + let n = 10; + let cloth = SoftBodyBuilder::cloth( + Vector::new(0.0, 2.0, 0.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .pinned_particles([0, (n - 1) as u32]) + .softness(SpringCoefficients::new(100.0, 1.0)) + .solver(SoftBodySolver::Fem); + let handle = world.insert_soft_body(cloth); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let far = sb.particle_position(n * n - 1); + assert!(far.y < 1.5, "the FEM cloth did not drape: {far:?}"); + for e in sb.edges() { + if e.kind != SoftBodyEdgeKind::Structural { + continue; + } + let len = (sb.particle_position(e.vertices[0] as usize) + - sb.particle_position(e.vertices[1] as usize)) + .length(); + assert!( + len < e.rest_length * 1.2, + "edge over-stretched: {len} vs {}", + e.rest_length + ); + } +} + +/// The FEM path uses the stable Neo-Hookean energy too: a stiff cube dropped on the ground +/// settles and keeps its volume, and one squeezed to a fraction of its rest size recovers. +#[cfg(feature = "fem")] +#[test] +fn fem_neo_hookean_cube_is_stable() { + for young in [1.0e4, 1.0e6, 1.0e8] { + let mut world = world_with_ground(); + let cube = + SoftBodyBuilder::cuboid(Vector::new(0.0, 3.0, 0.0), Vector::splat(0.75), 4, 4, 4) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .particle_mass(0.1); + let handle = world.insert_soft_body(cube); + for _ in 0..500 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let max_vel = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + assert!( + max_vel < 0.2, + "E = {young}: cube still moving at {max_vel} m/s" + ); + let volume = sb.volume(); + assert!( + (volume - sb.rest_volume()).abs() < 0.1 * sb.rest_volume(), + "E = {young}: volume {volume} vs rest {}", + sb.rest_volume() + ); + } +} + +/// A FEM Neo-Hookean cube crushed to a fifth of its height recovers its rest shape: the energy +/// is defined past the collapse, so no cell stays inverted. +#[cfg(feature = "fem")] +#[test] +fn fem_neo_hookean_cube_survives_large_compression() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let cube = SoftBodyBuilder::cuboid(Vector::ZERO, Vector::splat(0.5), 4, 4, 4) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + poisson_ratio: 0.3, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .without_colliders() + .can_sleep(false) + .particle_mass(0.1); + let handle = world.insert_soft_body(cube); + let rest_volume = world.soft_bodies[handle].rest_volume(); + { + let sb = &mut world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + let mut p = sb.particle_position(i); + p.y *= 0.2; + sb.set_particle_position(i, p); + } + } + for _ in 0..1500 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let volume = sb.volume(); + assert!( + (volume - rest_volume).abs() < 0.1 * rest_volume, + "the crushed cube did not recover: volume {volume} vs rest {rest_volume}" + ); + // No cell left inverted. + for c in sb.cells() { + let x: [Vector; 4] = core::array::from_fn(|k| sb.particle_position(c.vertices[k] as usize)); + let volume = (x[1] - x[0]).dot((x[2] - x[0]).cross(x[3] - x[0])) / 6.0; + assert!( + volume * c.rest_volume > 0.0, + "a cell stayed inverted: {volume} vs rest {}", + c.rest_volume + ); + } +} + +/// Plasticity on the FEM path: a block sheared past its yield keeps its lean, an elastic one +/// springs back. +#[cfg(feature = "fem")] +#[test] +fn fem_plastic_cells_keep_their_deformation() { + let lean_after = |plastic_yield: Real| { + let mut world = world_with_ground(); + let block = + SoftBodyBuilder::cuboid(Vector::new(0.0, 0.75, 0.0), Vector::splat(0.75), 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + plastic_yield, + plastic_creep: 100.0, + deformation_damping: 5.0, + ..Default::default() + }) + .solver(SoftBodySolver::Fem) + .particle_mass(0.1) + .collider_template(ColliderBuilder::ball(0.15).friction(2.0)); + let handle = world.insert_soft_body(block); + let sb = &world.soft_bodies[handle]; + let bottom: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particle_position(i).y < 0.01) + .collect(); + let top: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particle_position(i).y > 1.49) + .collect(); + for &i in bottom.iter().chain(top.iter()) { + world.soft_bodies[handle].set_particle_pinned(i, true); + } + let rest: Vec = top + .iter() + .map(|&i| world.soft_bodies[handle].particle_position(i)) + .collect(); + for step in 0..120 { + let shift = 0.9 * (step as Real / 60.0).min(1.0); + for (&i, p) in top.iter().zip(rest.iter()) { + world.soft_bodies[handle] + .set_particle_kinematic_target(i, *p + Vector::new(shift, 0.0, 0.0)); + } + world.step(); + } + for &i in &top { + world.soft_bodies[handle].set_particle_pinned(i, false); + } + for _ in 0..240 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let top_x: Real = + top.iter().map(|&i| sb.particle_position(i).x).sum::() / top.len() as Real; + let bottom_x: Real = bottom + .iter() + .map(|&i| sb.particle_position(i).x) + .sum::() + / bottom.len() as Real; + top_x - bottom_x + }; + let elastic = lean_after(0.0); + let plastic = lean_after(0.05); + assert!( + elastic.abs() < 0.15, + "the elastic block did not spring back: lean {elastic}" + ); + assert!( + plastic > 0.4, + "the plastic block did not keep its lean: {plastic} (elastic {elastic})" + ); +} + +/// Tearing on the FEM path: a pinned FEM cloth pulled apart tears, and the topology change is +/// picked up by the solver (the sparsity pattern is rebuilt). +#[cfg(feature = "fem")] +#[test] +fn fem_cloth_tears_when_pulled_apart() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let n = 12; + let cloth = SoftBodyBuilder::cloth( + Vector::new(0.0, 0.0, 0.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .softness(SpringCoefficients::new(200.0, 1.0)) + .tear_strain(0.15) + .solver(SoftBodySolver::Fem) + .without_colliders() + .can_sleep(false); + let handle = world.insert_soft_body(cloth); + let left: Vec = (0..n).map(|k| k * n).collect(); + let right: Vec = (0..n).map(|k| k * n + n - 1).collect(); + for &i in left.iter().chain(right.iter()) { + world.soft_bodies[handle].set_particle_pinned(i, true); + } + let rest_left: Vec = left + .iter() + .map(|&i| world.soft_bodies[handle].particle_position(i)) + .collect(); + let rest_right: Vec = right + .iter() + .map(|&i| world.soft_bodies[handle].particle_position(i)) + .collect(); + let triangles_before = world.soft_bodies[handle].boundary().len(); + let version_before = world.soft_bodies[handle].topology_version(); + for step in 0..400 { + let pull = 0.004 * step as Real; + for (&i, p) in left.iter().zip(&rest_left) { + world.soft_bodies[handle] + .set_particle_kinematic_target(i, *p - Vector::new(pull, 0.0, 0.0)); + } + for (&i, p) in right.iter().zip(&rest_right) { + world.soft_bodies[handle] + .set_particle_kinematic_target(i, *p + Vector::new(pull, 0.0, 0.0)); + } + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + assert!( + "the FEM cloth did not tear: no particle split" + ); + assert!( + sb.topology_version() > version_before, + "the topology version was not bumped" + ); +} diff --git a/crates/rapier3d/tests/thread_count_determinism.rs b/crates/rapier3d/tests/thread_count_determinism.rs index c9c8eadba..82b1d3117 100644 --- a/crates/rapier3d/tests/thread_count_determinism.rs +++ b/crates/rapier3d/tests/thread_count_determinism.rs @@ -35,6 +35,8 @@ impl EventHandler for EventLog { _total_force_magnitude: Real, ) { } + + fn handle_soft_body_tear_event(&self, _soft_bodies: &SoftBodySet, _event: &SoftBodyTearEvent) {} } /// One bit-faithful snapshot per body, sorted by handle. Float `Debug` output is @@ -61,6 +63,22 @@ fn snapshot(bodies: &RigidBodySet) -> Vec<(u32, String)> { out } +/// One bit-faithful snapshot per soft-body particle, in soft-body then particle order. +fn soft_snapshot(soft_bodies: &SoftBodySet) -> Vec { + let mut out = Vec::new(); + for (h, sb) in soft_bodies.iter() { + for (i, p) in sb.particles().iter().enumerate() { + out.push(format!( + "{:?}/{i} {:?} {:?}", + h.into_raw_parts().0, + p.position(), + p.velocity() + )); + } + } + out +} + /// FNV-1a over the serialized broad-phase + narrow-phase. /// /// The body snapshots above are float state; this is stored *order and layout* — @@ -91,7 +109,7 @@ fn phase_checksum(_bf: &BroadPhaseBvh, _nf: &NarrowPhase) -> u64 { /// collider removal and wake impulse, to exercise pair creation/deletion, /// touching transitions, sleeping and island edits) and records a snapshot /// after every step. -type StepRecord = (Vec<(u32, String)>, Vec, u64); +type StepRecord = (Vec<(u32, String)>, Vec, u64, Vec); fn run_sim(num_threads: usize, num_steps: usize) -> Vec { let pool = rapier3d::rayon::ThreadPoolBuilder::new() @@ -187,6 +205,17 @@ fn run_sim(num_threads: usize, num_steps: usize) -> Vec { .shape_matching(true) .particle_mass(0.05); soft_bodies.insert(balloon, &mut bodies, &mut colliders); + // A FEM-solved jelly cube: the FEM stages (predict, snapshot, propagate, writeback) are + // claimed per body, so their results must not depend on the worker that ran them. + #[cfg(feature = "fem")] + { + let fem_jelly = + SoftBodyBuilder::cuboid(Vector::new(4.0, 5.0, -4.0), Vector::splat(0.6), 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .solver(SoftBodySolver::Fem) + .particle_mass(0.2); + soft_bodies.insert(fem_jelly, &mut bodies, &mut colliders); + } let events = EventLog::default(); let mut snapshots = Vec::new(); @@ -230,7 +259,12 @@ fn run_sim(num_threads: usize, num_steps: usize) -> Vec { } else { 0 }; - snapshots.push((snapshot(&bodies), step_events, checksum)); + snapshots.push(( + snapshot(&bodies), + step_events, + checksum, + soft_snapshot(&soft_bodies), + )); } snapshots } @@ -242,8 +276,14 @@ fn identical_results_for_any_worker_count() { for num_threads in [2, 8] { let run = run_sim(num_threads, STEPS); for step in 0..STEPS { - let ((base_state, base_events, base_sum), (run_state, run_events, run_sum)) = - (&base[step], &run[step]); + let ( + (base_state, base_events, base_sum, base_soft), + (run_state, run_events, run_sum, run_soft), + ) = (&base[step], &run[step]); + assert_eq!( + base_soft, run_soft, + "soft-body particle divergence at step {step}, {num_threads} threads vs 1" + ); assert_eq!( base_events, run_events, "event-sequence divergence at step {step}, {num_threads} threads vs 1" diff --git a/crates/rapier_testbed2d-f64/Cargo.toml b/crates/rapier_testbed2d-f64/Cargo.toml index 564307960..d16041ac5 100644 --- a/crates/rapier_testbed2d-f64/Cargo.toml +++ b/crates/rapier_testbed2d-f64/Cargo.toml @@ -29,6 +29,9 @@ clippy = { needless_lifetimes = "allow" } default = ["dim2"] dim2 = [] parallel = ["rapier/parallel", "num_cpus"] +# Enables the experimental FEM soft-body solver, selected per body with +# `SoftBodyBuilder::solver(SoftBodySolver::Fem)`. +fem = ["rapier/fem"] profiler_ui = ["dep:puffin_egui", "profiling/profile-with-puffin"] # See https://github.com/dimforge/rapier/issues/760. unstable-puffin-pr-235 = [] diff --git a/crates/rapier_testbed2d/Cargo.toml b/crates/rapier_testbed2d/Cargo.toml index 30928fa05..480bd9792 100644 --- a/crates/rapier_testbed2d/Cargo.toml +++ b/crates/rapier_testbed2d/Cargo.toml @@ -29,6 +29,9 @@ clippy = { needless_lifetimes = "allow" } default = ["dim2"] dim2 = [] parallel = ["rapier/parallel", "num_cpus"] +# Enables the experimental FEM soft-body solver, selected per body with +# `SoftBodyBuilder::solver(SoftBodySolver::Fem)`. +fem = ["rapier/fem"] profiler_ui = ["dep:puffin_egui", "profiling/profile-with-puffin"] # See https://github.com/dimforge/rapier/issues/760. unstable-puffin-pr-235 = [] diff --git a/crates/rapier_testbed3d-f64/Cargo.toml b/crates/rapier_testbed3d-f64/Cargo.toml index ddc80dd25..ca00b73f6 100644 --- a/crates/rapier_testbed3d-f64/Cargo.toml +++ b/crates/rapier_testbed3d-f64/Cargo.toml @@ -29,6 +29,9 @@ clippy = { needless_lifetimes = "allow" } default = ["dim3"] dim3 = [] parallel = ["rapier/parallel", "num_cpus"] +# Enables the experimental FEM soft-body solver, selected per body with +# `SoftBodyBuilder::solver(SoftBodySolver::Fem)`. +fem = ["rapier/fem"] profiler_ui = ["dep:puffin_egui", "profiling/profile-with-puffin"] # See https://github.com/dimforge/rapier/issues/760. unstable-puffin-pr-235 = [] diff --git a/crates/rapier_testbed3d/Cargo.toml b/crates/rapier_testbed3d/Cargo.toml index 4aeb0b138..997f96db8 100644 --- a/crates/rapier_testbed3d/Cargo.toml +++ b/crates/rapier_testbed3d/Cargo.toml @@ -29,6 +29,9 @@ clippy = { needless_lifetimes = "allow" } default = ["dim3"] dim3 = [] parallel = ["rapier/parallel", "num_cpus"] +# Enables the experimental FEM soft-body solver, selected per body with +# `SoftBodyBuilder::solver(SoftBodySolver::Fem)`. +fem = ["rapier/fem"] profiler_ui = ["dep:puffin_egui", "profiling/profile-with-puffin"] # See https://github.com/dimforge/rapier/issues/760. unstable-puffin-pr-235 = [] diff --git a/examples2d/Cargo.toml b/examples2d/Cargo.toml index a420fd182..593e99d94 100644 --- a/examples2d/Cargo.toml +++ b/examples2d/Cargo.toml @@ -22,10 +22,11 @@ usvg.workspace = true [dependencies.rapier_testbed2d] workspace = true -#features = ["profiler_ui"] +features = ["fem"] [dependencies.rapier2d] workspace = true +features = ["fem"] [[bin]] name = "all_examples2" diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index e8afed865..f78102894 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -57,6 +57,7 @@ mod s2d_pyramid; mod sensor2; mod soft_blobs2; mod soft_bodies2; +mod soft_fem2; mod soft_jelly2; // The letters come from a tessellated SVG (usvg), which doesn't build for wasm. #[cfg(not(target_arch = "wasm32"))] @@ -145,6 +146,7 @@ pub async fn main() { SOFT, "Soft letters", soft_letters2::run; SOFT, "Plasticity", soft_plasticity2::run; SOFT, "Tearing", soft_tearing2::run; + SOFT, "Soft FEM", soft_fem2::run; // ── Controls ──────────────────────────────────────────────────────── CONTROLS, "Character controller", character_controller2::run; // ── Debug ─────────────────────────────────────────────────────────── diff --git a/examples2d/soft_fem2.rs b/examples2d/soft_fem2.rs new file mode 100644 index 000000000..7363ead67 --- /dev/null +++ b/examples2d/soft_fem2.rs @@ -0,0 +1,98 @@ +//! The FEM soft-body solver (2D) next to the constraint solver: the same bodies twice, solved by +//! [`SoftBodySolver::Fem`] on the lower row and by the constraint solver on the upper one. +//! At four substeps the FEM cantilever and plank hold their static deflection; the others sag. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +/// Vertical distance between the FEM row and the constraint-solver row. +const ROW: Real = 6.0; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(30.0, 0.5), + ); + + let solvers = [SoftBodySolver::Fem, SoftBodySolver::Constraints]; + + for (row, solver) in solvers.iter().enumerate() { + let y = 2.0 + row as Real * ROW; + + /* + * A stiff cantilever bolted to a wall. + */ + let (length, thickness) = (4.0, 0.4); + let beam = SoftBodyBuilder::grid( + Vector::new(length * 0.5 - 8.0, y), + Vector::new(length * 0.5, thickness * 0.5), + 17, + 3, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e6, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .mass(8.0); + let pinned: Vec = beam + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x < -8.0 + 1.0e-4) + .map(|(i, _)| i as u32) + .collect(); + world.insert_soft_body(beam.pinned_particles(pinned).solver(*solver)); + + /* + * A plank pinned at both ends, loaded by a heavy box dropped in its middle. + */ + let plank = SoftBodyBuilder::grid(Vector::new(2.0, y), Vector::new(3.0, 0.25), 21, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 4.0e6, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .mass(20.0); + let pinned: Vec = plank + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| (p.x - 2.0).abs() > 2.9) + .map(|(i, _)| i as u32) + .collect(); + world.insert_soft_body(plank.pinned_particles(pinned).solver(*solver)); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(2.0, y + 2.5)), + ColliderBuilder::cuboid(0.5, 0.5).density(20.0), + ); + + /* + * A Neo-Hookean jelly square dropped on the plank. + */ + let jelly = SoftBodyBuilder::grid(Vector::new(9.0, y + 1.0), Vector::splat(0.8), 5, 5) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1); + world.insert_soft_body(jelly.solver(*solver)); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(1.0, 5.0), 25.0); + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index 96ae53e97..023cf9d9f 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -25,10 +25,11 @@ kiss3d = { workspace = true, features = ["rt_switcher"] } [dependencies.rapier_testbed3d] workspace = true -#features = ["profiler_ui"] +features = ["fem"] [dependencies.rapier3d] workspace = true +features = ["fem"] [dependencies.rapier3d-urdf] workspace = true diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 80bac13df..26fe1bfda 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -80,6 +80,7 @@ mod soft_bodies3; mod soft_cloth3; mod soft_cloth_stress3; mod soft_dress3; +mod soft_fem3; mod soft_jelly3; mod soft_pile3; mod soft_plasticity3; @@ -173,6 +174,7 @@ pub async fn main() { SOFT, "Plasticity", soft_plasticity3::run; SOFT, "Tearing", soft_tearing3::run; SOFT, "Dancing dress", soft_dress3::run; + SOFT, "Soft FEM", soft_fem3::run; #[cfg(not(target_arch = "wasm32"))] SOFT, "Soft trimeshes", soft_trimesh3::run; // ── Controls ──────────────────────────────────────────────────────── diff --git a/examples3d/soft_fem3.rs b/examples3d/soft_fem3.rs new file mode 100644 index 000000000..92097d61a --- /dev/null +++ b/examples3d/soft_fem3.rs @@ -0,0 +1,105 @@ +//! The FEM soft-body solver, side by side with the constraint solver: the same bodies twice, +//! solved by [`SoftBodySolver::Fem`] on the left (blue) and by the constraint solver on the right. +//! At four substeps the FEM cantilevers and planks hold their deflection; the constraint ones sag. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +/// Distance between the FEM column (at `-Z`) and the constraint column (at `+Z`). +const COLUMN: Real = 3.5; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(20.0, 0.1, 20.0), + ); + + let solvers = [SoftBodySolver::Fem, SoftBodySolver::Constraints]; + + for (column, solver) in solvers.iter().enumerate() { + let z = if column == 0 { -COLUMN } else { COLUMN }; + + /* + * A stiff cantilever bolted to a wall: the deflection under its own weight. + */ + let (length, thickness) = (3.0, 0.3); + let beam = SoftBodyBuilder::cuboid( + Vector::new(length * 0.5 - 5.0, 3.0, z), + Vector::new(length * 0.5, thickness * 0.5, thickness * 0.5), + 13, + 3, + 3, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e6, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .mass(12.0); + let pinned: Vec = beam + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x < -5.0 + 1.0e-4) + .map(|(i, _)| i as u32) + .collect(); + world.insert_soft_body(beam.pinned_particles(pinned).solver(*solver)); + + /* + * A plank pinned at both ends, loaded by a heavy box dropped in its middle. + */ + let plank = SoftBodyBuilder::cuboid( + Vector::new(1.0, 2.0, z), + Vector::new(2.0, 0.15, 0.6), + 17, + 3, + 5, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 4.0e6, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .mass(20.0); + let pinned: Vec = plank + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| (p.x - 1.0).abs() > 1.9) + .map(|(i, _)| i as u32) + .collect(); + world.insert_soft_body(plank.pinned_particles(pinned).solver(*solver)); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(1.0, 4.5, z)), + ColliderBuilder::cuboid(0.35, 0.35, 0.35).density(30.0), + ); + + /* + * A Neo-Hookean jelly cube dropped on the ground: the two solvers agree here. + */ + let jelly = SoftBodyBuilder::cuboid(Vector::new(6.0, 2.0, z), Vector::splat(0.7), 5, 5, 5) + .cell_model(SoftBodyCellModel::NeoHookean) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1); + world.insert_soft_body(jelly.solver(*solver)); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(2.0, 8.0, 16.0), Vec3::new(0.5, 2.0, 0.0)); + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/soft_trimesh3.rs b/examples3d/soft_trimesh3.rs index 90dc3d2b5..0f0b36c15 100644 --- a/examples3d/soft_trimesh3.rs +++ b/examples3d/soft_trimesh3.rs @@ -94,6 +94,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { settings.remove("Skin collisions"); false }; + // The FEM path is opt-in: without the `fem` feature the switch is hidden and every body + // runs the constraint solver. + let fem_solver = settings.get_or_set_bool("FEM solver", false); let cell_model = settings.get_or_set_string( "Cell model", 0, @@ -187,6 +190,11 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { .particle_radius(meshing.boundary_cell() * 0.25) .self_contacts(self_contacts) .surface_collider(ColliderBuilder::ball(0.1).friction(0.6)); + let model = model.solver(if fem_solver { + SoftBodySolver::Fem + } else { + SoftBodySolver::Constraints + }); total_cells += model.cells.len(); total_bodies += 1; @@ -409,6 +417,14 @@ mod tests { (igeom / width) as f32 * 8.0 + 7.0, 0.0, ); + // With the FEM path compiled in, alternate the solver over the models so the + // demo's switch is exercised by the same settle run. + let filled = filled.solver(if igeom % 2 == 1 { + SoftBodySolver::Fem + } else { + SoftBodySolver::Constraints + }); + labels.push(obj_path.clone()); handles.push( world.insert_soft_body( diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 6cdfe8301..683138b10 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -160,6 +160,8 @@ impl + Copy> SpringCoefficients { } } + pub propagate_in_relax_pass: bool, + propagate_in_relax_pass: true, /// Configuration parameters that control the physics simulation quality and behavior. /// /// These parameters affect how the physics engine advances time, resolves collisions, and diff --git a/src/dynamics/soft_body/mod.rs b/src/dynamics/soft_body/mod.rs index 01b069026..2fb69250c 100644 --- a/src/dynamics/soft_body/mod.rs +++ b/src/dynamics/soft_body/mod.rs @@ -7,6 +7,8 @@ pub use self::collision_mesh::{ }; #[cfg(feature = "dim3")] pub use self::soft_body_elements::SoftBodyDihedral; +#[cfg(feature = "fem")] +pub use self::soft_body_elements::SoftBodySolver; pub use self::soft_body::{SOFT_BODY_MAX_CONSTRAINT_PARTICLES, SoftBody}; pub use self::soft_body_elements::{ SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyEdgeKind, SoftBodyParticle, diff --git a/src/dynamics/soft_body/soft_body.rs b/src/dynamics/soft_body/soft_body.rs index 728855c7c..3e0d3da19 100644 --- a/src/dynamics/soft_body/soft_body.rs +++ b/src/dynamics/soft_body/soft_body.rs @@ -66,6 +66,10 @@ pub struct SoftBody { pub(crate) boundary_element_cells: Vec, pub(crate) material: SoftBodyMaterial, pub(crate) cell_model: SoftBodyCellModel, + /// Which solver simulates this body's elasticity. + #[cfg(feature = "fem")] + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) solver: SoftBodySolver, /// Whether the volume pieces are constrained (only set while there is at least one piece). pub(crate) volume_preservation: bool, /// The pieces of material enclosed by a closed boundary, each with its own volume constraint diff --git a/src/dynamics/soft_body/soft_body_accessors.rs b/src/dynamics/soft_body/soft_body_accessors.rs index f39ae73cb..f25110d2c 100644 --- a/src/dynamics/soft_body/soft_body_accessors.rs +++ b/src/dynamics/soft_body/soft_body_accessors.rs @@ -122,6 +122,32 @@ impl SoftBody { self.cell_model } + /// Which solver simulates this soft body's elasticity. + #[cfg(feature = "fem")] + pub fn solver(&self) -> SoftBodySolver { + self.solver + } + + /// Selects the solver simulating this soft body's elasticity (see [`SoftBodySolver`]). + #[cfg(feature = "fem")] + pub fn set_solver(&mut self, solver: SoftBodySolver) { + self.solver = solver; + self.modified = true; + } + + /// Whether this soft body's elasticity is solved by the FEM path. + #[inline] + pub(crate) fn uses_fem(&self) -> bool { + #[cfg(feature = "fem")] + { + self.solver == SoftBodySolver::Fem + } + #[cfg(not(feature = "fem"))] + { + false + } + } + /// Whether the global area/volume preservation row is enabled. pub fn volume_preservation_enabled(&self) -> bool { self.volume_preservation diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs index 1d2498216..e2607b5a8 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs @@ -73,6 +73,9 @@ pub struct SoftBodyBuilder { pub material: SoftBodyMaterial, /// The constitutive model of the cells. pub cell_model: SoftBodyCellModel, + /// Which solver simulates the body's elasticity. + #[cfg(feature = "fem")] + pub solver: super::SoftBodySolver, /// Whether area/volume preservation is enabled (one constraint per piece of material enclosed /// by a closed surface). pub volume_preservation: bool, diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs index 385817deb..c5c6b5b47 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs @@ -252,6 +252,13 @@ impl SoftBodyBuilder { self } + /// Selects the solver simulating the body's elasticity (see [`super::SoftBodySolver`]). + #[cfg(feature = "fem")] + pub fn solver(mut self, solver: super::SoftBodySolver) -> Self { + self.solver = solver; + self + } + /// Enables area/volume preservation: one constraint per piece of material enclosed by a /// closed surface (see [`crate::dynamics::SoftBody::volume_pieces`]). pub fn volume_preservation(mut self, enabled: bool) -> Self { diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs index 5e2bc0897..151df0a26 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs @@ -35,6 +35,8 @@ impl SoftBodyBuilder { surface: Vec::new(), material: SoftBodyMaterial::default(), cell_model: SoftBodyCellModel::default(), + #[cfg(feature = "fem")] + solver: super::SoftBodySolver::default(), volume_preservation: false, volume_factor: 1.0, shape_matching: false, diff --git a/src/dynamics/soft_body/soft_body_elements.rs b/src/dynamics/soft_body/soft_body_elements.rs index 2ac97dbfc..f3ca53f0d 100644 --- a/src/dynamics/soft_body/soft_body_elements.rs +++ b/src/dynamics/soft_body/soft_body_elements.rs @@ -143,6 +143,22 @@ pub struct SoftBodyDihedral { pub(crate) color: u8, } +/// Which solver simulates a soft body's elasticity (requires the `fem` cargo feature). +#[cfg(feature = "fem")] +#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum SoftBodySolver { + /// Every elastic element becomes constraints, warm-started and swept Gauss-Seidel by color once + /// per substep with the contacts and joints. Cheap and robust, but converged only as far as the + /// sweep count: a stiff body keeps a residual compliance, a load crosses a long body slowly. + #[default] + Constraints, + /// Implicit Euler elasticity: forces and tangent stiffness from the strain-energy density, + /// `(M + h D + h² K) Δv = b` solved over the whole body once per substep; constraints see the + /// body through its augmented mass. Costs a factorization per step and a solve per constraint. + Fem, +} + /// The constitutive model of a soft body's cells (triangles in 2D, tetrahedra in 3D). #[derive(Copy, Clone, Debug, PartialEq, Eq, Default)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] 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 339f60a3d..99c7c6e4e 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 @@ -41,6 +41,8 @@ impl SoftBodySet { boundary_element_cells: Vec::new(), material: soft_body.material, cell_model: soft_body.cell_model, + #[cfg(feature = "fem")] + solver: soft_body.solver, volume_preservation: false, volume_factor: 1.0, rest_com: soft_body.rest_com, diff --git a/src/dynamics/soft_body/soft_body_settings.rs b/src/dynamics/soft_body/soft_body_settings.rs index 4b9434cdb..5a3b09be1 100644 --- a/src/dynamics/soft_body/soft_body_settings.rs +++ b/src/dynamics/soft_body/soft_body_settings.rs @@ -28,4 +28,48 @@ pub struct SoftBodiesSettings { /// contact softness natural frequencies for the soft-body contacts (default: `4.0`); they run /// stiffer because a contact's effective mass is a few particles' worth, not the whole body's. pub contact_stiffening: Real, + /// Tuning of the FEM soft-body solver (see + /// [`SoftBodySolver::Fem`](crate::dynamics::SoftBodySolver::Fem)); only read by the soft + /// bodies that select it. + #[cfg(feature = "fem")] + pub fem: SoftFemParameters, +} + +impl Default for SoftBodiesSettings { + fn default() -> Self { + Self { + recovery: SoftRecoverySettings::default(), + resweep_strain: 0.75, + max_extra_substeps: 4, + contact_stiffening: 4.0, + #[cfg(feature = "fem")] + fem: SoftFemParameters::default(), + } + } +} + +/// Tuning of the FEM soft-body solver (see +/// [`SoftBodySolver::Fem`](crate::dynamics::SoftBodySolver::Fem)), which solves a linear system +/// per substep by conjugate gradient and factorizes it once per step (see `max_dense_dofs`). +#[cfg(feature = "fem")] +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftFemParameters { + /// Relative residual at which the conjugate gradient stops (default: `1.0e-5`). + pub linear_tolerance: Real, + /// Hard cap on the conjugate-gradient iterations, whatever the residual (default: `256`). + /// + /// A very stiff, finely meshed body can need several hundred iterations to converge; the + /// truncated step it gets instead is under-relaxed (safe), only slower to settle. + pub max_linear_iterations: usize, + /// +} +#[cfg(feature = "fem")] +impl Default for SoftFemParameters { + fn default() -> Self { + Self { + linear_tolerance: 1.0e-5, + max_linear_iterations: 20, + } + } } diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs index dde9bfe8f..3fa989f5a 100644 --- a/src/dynamics/solver/mod.rs +++ b/src/dynamics/solver/mod.rs @@ -12,6 +12,8 @@ mod interaction_groups; mod joint_constraint; pub(crate) mod manifold_store; mod soft_constraint; +#[cfg(feature = "fem")] +pub(crate) mod soft_fem; mod solver_body; pub(crate) mod solver_contact_graph; mod staged_island_solver; diff --git a/src/dynamics/solver/soft_constraint/mod.rs b/src/dynamics/solver/soft_constraint/mod.rs index dd61bbbcd..9adf2b01f 100644 --- a/src/dynamics/solver/soft_constraint/mod.rs +++ b/src/dynamics/solver/soft_constraint/mod.rs @@ -3,8 +3,9 @@ pub(crate) use self::soft_constraints_set::SoftConstraintsSet; mod soft_attachment; -mod soft_constraints_set; +pub(crate) mod soft_constraints_set; mod soft_contact; mod soft_contact_assembly; mod soft_contact_chunks; +pub(crate) mod soft_element_constraint; mod soft_element_constraint_assembly; diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs index 276e5aea6..76d42e66f 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs @@ -201,6 +201,9 @@ impl SoftElasticConstraint { } pub fn max_tensile_strain(&self) -> Real { + max_tensile_strain(&self.strain) + } + /// Deviatoric coefficients of a cell from its inverse rest matrix. pub fn coefficients(inv_rest_matrix: &Matrix) -> [Vector; MAX_CONSTRAINT_PARTICLES] { let constraints = inv_rest_matrix.transpose(); @@ -215,6 +218,20 @@ impl SoftElasticConstraint { /// Gradient of strain row `r` w.r.t. particle `p`'s velocity, in the cell frame: /// `½ (coeffs[p][b] e_a + coeffs[p][a] e_b)`. #[inline] + pub(crate) fn strain_gradient( + coeffs: &[Vector; MAX_CONSTRAINT_PARTICLES], + r: usize, + p: usize, + ) -> Vector { + let (a, b) = strain_pairs()[r]; + let mut g = Vector::ZERO; + if a == b { + g[a] = coeffs[p][a]; + } else { + g[a] = 0.5 * coeffs[p][b]; + g[b] = 0.5 * coeffs[p][a]; + } + g } /// The strain-constraint gradients of particle `p` rotated by `rotation`: the columns `R gᵣ[p]`. diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_element_linalg.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_element_linalg.rs index 66794b5bb..566138e9d 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_element_linalg.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_element_linalg.rs @@ -69,6 +69,18 @@ pub(crate) fn max_symmetric_eigenvalue(m: &Matrix) -> Real { m.symmetric_eigenvalues().max_element() } +/// The outer product `a bᵀ`. +#[inline] +pub(crate) fn outer_product(a: Vector, b: Vector) -> Matrix { + #[cfg(feature = "dim2")] + { + Matrix::from_cols(a * b.x, a * b.y) + } + #[cfg(feature = "dim3")] + { + Matrix::from_cols(a * b.x, a * b.y, a * b.z) + } +} /// The cofactor matrix `∂det(F)/∂F`. #[inline] diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs index 99e131069..0a3e6f919 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs @@ -28,17 +28,18 @@ pub(crate) struct NeoHookeanConstraint { pub dt_grad: StrainVector, } -/// Strain change (any component) past which a Neo-Hookean row recomputes its tangent stiffness. +/// Strain change (any component) past which a Neo-Hookean constraint recomputes its tangent stiffness. +pub(crate) const STIFFNESS_UPDATE_STRAIN: Real = 0.02; + impl NeoHookeanConstraint { - /// Rest stiffness of strain row `r` per unit rest volume: `2μ + λ` on the diagonal rows, - /// `4μ` on the shear rows (the linear-elastic block, which the Neo-Hookean Hessian - /// reproduces at rest). + /// Rest stiffness of strain row `r` per unit rest volume: `2μ + λ` on the diagonal rows, `4μ` + /// on the shear rows (the linear-elastic block, reproduced by the Neo-Hookean Hessian at rest). #[inline] pub fn rest_stiffness(mu: Real, lambda: Real, r: usize) -> Real { if r < DIM { 2.0 * mu + lambda } else { 4.0 * mu } } - /// Refreshes `M = dt (dt K + D)` and the gradient `dt g` at `strain`, and the block inverse + /// Updates `M = dt (dt K + D)`, the gradient `dt g` at `strain`, and the block inverse /// `(M A + I)⁻¹` into `inv_a`. pub(super) fn update( &mut self, @@ -99,6 +100,7 @@ impl NeoHookeanConstraint { } /// `S = I + ε`, `κ = λ'(J - 1) - μ` and the gradient `V₀ ∂Ψ/∂ε` at the strain `ε`. + pub(crate) fn gradient( mu: Real, lambda: Real, strain: &StrainVector, diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs index 542f377df..d0258a6e7 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs @@ -65,6 +65,43 @@ pub(crate) struct SoftScalarConstraint { pub impulse_bounds: [Real; 2], } +/// Gradients of the dihedral angle between the triangles `(p0, p1, p2)` and `(p0, p1, p3)` +/// (Müller et al., 2007); returns the angle minus `rest`. +#[cfg(feature = "dim3")] +pub(crate) fn dihedral_gradients(pos: &[Vector; 4], rest: Real, grad: &mut [Vector; 4]) -> Real { + let p1 = pos[1] - pos[0]; + let p2 = pos[2] - pos[0]; + let p3 = pos[3] - pos[0]; + let n1_raw = p1.cross(p2); + let n2_raw = p1.cross(p3); + let (l1, l2) = (n1_raw.length(), n2_raw.length()); + if l1 < 1.0e-9 || l2 < 1.0e-9 { + *grad = [Vector::ZERO; 4]; + return 0.0; + } + let n1 = n1_raw / l1; + let n2 = n2_raw / l2; + let d = n1.dot(n2).clamp(-1.0, 1.0); + let angle = d.acos(); + let sin = (1.0 - d * d).sqrt(); + if sin < 1.0e-6 { + // Flat configuration: the angle is stationary, no useful gradient. + *grad = [Vector::ZERO; 4]; + return angle - rest; + } + // The q_i are minus the gradients of d = n1·n2, so ∇C = q / sin (C = acos(d) - rest). + let q3 = (p1.cross(n2) + n1.cross(p1) * d) / l1; + let q4 = (p1.cross(n1) + n2.cross(p1) * d) / l2; + let q2 = -(p2.cross(n2) + n1.cross(p2) * d) / l1 - (p3.cross(n1) + n2.cross(p3) * d) / l2; + let q1 = -q2 - q3 - q4; + let scale = 1.0 / sin; + grad[0] = q1 * scale; + grad[1] = q2 * scale; + grad[2] = q3 * scale; + grad[3] = q4 * scale; + angle - rest +} + impl SoftScalarConstraint { /// Whether this constraint is strained beyond `min_strain` (relative error of a distance constraint; the /// other kinds never are): the criterion of the re-sweep after the contacts. @@ -127,8 +164,16 @@ impl SoftScalarConstraint { } } + /// Gradients of the dihedral angle of this constraint's four particles. #[cfg(feature = "dim3")] fn dihedral_gradients(&mut self) -> Real { + let pos = [self.pos[0], self.pos[1], self.pos[2], self.pos[3]]; + let mut grad = [Vector::ZERO; 4]; + let c = dihedral_gradients(&pos, self.rest, &mut grad); + self.grad[..4].copy_from_slice(&grad); + c + } + /// Applies the accumulated impulse to the particle velocities. #[inline] pub fn warmstart(&self, bodies: &mut SolverBodies) { diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs index 7888e6c28..d013fdfe7 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs @@ -490,19 +490,22 @@ impl SoftConstraintsSet { let (edges, dihedrals, cells) = element_ranges(sb, range); #[cfg(feature = "dim2")] let _ = dihedrals; - for e in &sb.edges { - counts[e.color as usize] += 1; - } - #[cfg(feature = "dim3")] - for d in &sb.dihedrals { - counts[d.color as usize] += 1; + // The FEM solver integrates the distance and bending elements itself; they get no constraint. + if !sb.uses_fem() { + for e in &sb.edges[edges] { + counts[e.color as usize] += 1; + } + #[cfg(feature = "dim3")] + for d in &sb.dihedrals[dihedrals] { + counts[d.color as usize] += 1; + } } let (mu, _) = sb.material.lame_parameters(); for c in &sb.cells[cells] { match sb.cell_model { SoftBodyCellModel::Volume => counts[c.color as usize] += 1, SoftBodyCellModel::Corotational | SoftBodyCellModel::NeoHookean => { - if mu > 0.0 && elastic_cell_terms(sb, slots, c).2 > 0.0 { + if !sb.uses_fem() && mu > 0.0 && elastic_cell_terms(sb, slots, c).2 > 0.0 { } } } @@ -599,7 +602,35 @@ impl SoftConstraintsSet { } }; - for (ei, e) in sb.edges.iter().enumerate() { + let element_constraints = !sb.uses_fem(); + for (ei, e) in sb + .edges + .iter() + .enumerate() + .take(edges.end) + .skip(edges.start) + .filter(|_| element_constraints) + { + let (erp, cfm) = match (e.softness, e.kind) { + (Some(softness), _) => coeffs_of(&softness), + (None, SoftBodyEdgeKind::Structural) => (edge_erp, edge_cfm), + (None, SoftBodyEdgeKind::Bend) => (bend_erp, bend_cfm), + }; + let mut constraint = base_constraint( + SoftScalarConstraintKind::Distance, + SoftScalarConstraintWriteback::Edge, + ei, + &e.vertices, + e.rest_length, + erp, + cfm, + e.impulse, + ); + if e.tension_only { + constraint.impulse_bounds = [0.0, Real::MAX]; + constraint.impulse = constraint.impulse.max(0.0); + } + out.push_constraint(e.color, constraint); } #[cfg(feature = "dim3")] @@ -640,6 +671,10 @@ impl SoftConstraintsSet { out.push_constraint(c.color, constraint); } SoftBodyCellModel::Corotational | SoftBodyCellModel::NeoHookean => { + if sb.uses_fem() { + // Integrated implicitly by the FEM solver instead (see `soft_fem`). + continue; + } let neo_hookean = sb.cell_model == SoftBodyCellModel::NeoHookean; // Constraint-space stiffnesses 2μV₀ / 4μV₀ (diagonal / shear rows) and λV₀ // (volumetric, at rest) map to natural frequencies via the effective masses, diff --git a/src/dynamics/solver/soft_fem/mod.rs b/src/dynamics/solver/soft_fem/mod.rs new file mode 100644 index 000000000..e58b08485 --- /dev/null +++ b/src/dynamics/solver/soft_fem/mod.rs @@ -0,0 +1,10 @@ +//! The FEM soft-body solver, an opt-in alternative to the soft constraints: a +//! [`SoftBodySolver::Fem`](crate::dynamics::SoftBodySolver::Fem) body solves `A Δv = b` with +//! `A = M + h D + h² K` each substep; constraints stay in the staged solver, acting via `A⁻¹Jᵀ`. + +mod soft_fem_set; +mod soft_fem_skyline; +mod soft_fem_sparse; +mod system; + +pub(crate) use soft_fem_set::SoftFemSet; diff --git a/src/dynamics/solver/soft_fem/soft_fem_set.rs b/src/dynamics/solver/soft_fem/soft_fem_set.rs new file mode 100644 index 000000000..cd6366da7 --- /dev/null +++ b/src/dynamics/solver/soft_fem/soft_fem_set.rs @@ -0,0 +1,175 @@ +//! The FEM systems of the soft bodies solved by the staged island solver: the per-substep +//! predict stage's bodies, and the responses of the step's constraints. + +use super::system::SoftFemSystem; +use crate::alloc_prelude::*; +use crate::dynamics::solver::soft_constraint::SoftConstraintsSet; +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::dynamics::{SoftBody, SoftBodyHandle, SoftBodySet, SoftFemParameters}; +use crate::math::Real; +use core::ops::Range; +use parry::utils::hashmap::HashMap; + +/// One awake soft body solved by the FEM path, during one island solve. +struct ActiveFem { + /// The soft body, valid for the solve's scope (same contract as `AwakeSoftBody::ptr`). + body: *mut SoftBody, + /// Its system, owned by `SoftFemSet::systems` and boxed so the pointer stays valid. + system: *mut SoftFemSystem, + /// The substep length of the body's solve group. + dt: Real, +} + +// SAFETY: the pointers are only dereferenced during the solve, where the stage discipline gives +// exactly one worker access to a given index (same contract as `AwakeSoftBody`). +unsafe impl Send for ActiveFem {} +unsafe impl Sync for ActiveFem {} + +/// The FEM systems of every soft body that selected the FEM solver, looked up by handle and kept +/// across steps (the sparsity pattern and element tables are the expensive part; values refill +/// every substep); `active` holds the awake ones of the island being solved, group-major. +#[derive(Default)] +pub(crate) struct SoftFemSet { + systems: HashMap>, + active: Vec, + /// Slice of `active` belonging to each substep group. + groups: Vec>, +} + +impl SoftFemSet { + pub fn new() -> Self { + Self::default() + } + + pub fn is_empty(&self) -> bool { + self.active.is_empty() + } + + /// The `active` range of a substep group. + pub fn group(&self, group: usize) -> Range { + self.groups + .get(group) + .cloned() + .unwrap_or(self.active.len()..self.active.len()) + } + + /// Collects the island's awake FEM soft bodies (after `SoftConstraintsSet::assemble` laid out + /// the particle slots) and updates their systems' step-constant state; the factorization runs + /// in the parallel response stage ([`Self::compute_responses`]). + pub fn assemble( + &mut self, + soft_constraints: &SoftConstraintsSet, + soft_bodies: &SoftBodySet, + num_groups: usize, + group_dt: impl Fn(usize) -> Real, + ) { + self.active.clear(); + self.groups.clear(); + self.groups.resize(num_groups.max(1), 0..0); + if soft_constraints.is_empty() { + // Bodies whose soft body was removed would otherwise keep their system forever; the + // sweep is only worth its cost once the map has grown well past what is in use. + if self.systems.len() > 64 { + self.systems.retain(|h, _| soft_bodies.get(*h).is_some()); + } + return; + } + + let mut group_starts = vec![0usize; self.groups.len() + 1]; + for awake in &soft_constraints.awake { + // SAFETY: read-only access during assembly, same contract as the constraint assembly. + let sb = unsafe { &*awake.ptr }; + if !awake.frozen && sb.uses_fem() { + group_starts[awake.group as usize + 1] += 1; + } + } + for k in 1..group_starts.len() { + group_starts[k] += group_starts[k - 1]; + } + for (g, range) in self.groups.iter_mut().enumerate() { + *range = group_starts[g]..group_starts[g + 1]; + } + + // `awake` is already sorted by group, so a single pass keeps the group-major order. + for awake in &soft_constraints.awake { + let sb = unsafe { &*awake.ptr }; + if awake.frozen || !sb.uses_fem() { + continue; + } + let slots = + &soft_constraints.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let system = self.systems.entry(awake.handle).or_default(); + system.prepare(sb, slots); + self.active.push(ActiveFem { + body: awake.ptr, + system: &mut **system as *mut SoftFemSystem, + dt: group_dt(awake.group as usize), + }); + } + } + + /// The implicit elastic step of `active[index]` (one substep). + /// # Safety + /// The caller must guarantee exclusive access to this index (one claiming worker per index + /// per stage) and that the body's solver-body slots are not written concurrently. + pub unsafe fn predict( + &self, + index: usize, + bodies: &mut SolverBodies, + params: &SoftFemParameters, + ) { + let entry = &self.active[index]; + let system = unsafe { &mut *entry.system }; + system.predict( + bodies, + entry.dt, + params.linear_tolerance, + params.max_linear_iterations, + ); + } + + /// Records `active[index]`'s velocities at the start of a solve pass. + /// + /// # Safety + /// Same contract as [`Self::predict`]. + pub unsafe fn snapshot(&self, index: usize, bodies: &SolverBodies) { + let entry = &self.active[index]; + let system = unsafe { &mut *entry.system }; + system.snapshot(bodies); + } + + /// Propagates the impulses `active[index]` received during a solve pass through its + /// elasticity. + /// + /// # Safety + /// Same contract as [`Self::predict`]. + pub unsafe fn propagate( + &self, + index: usize, + bodies: &mut SolverBodies, + params: &SoftFemParameters, + ) { + let entry = &self.active[index]; + let system = unsafe { &mut *entry.system }; + system.propagate( + bodies, + params.linear_tolerance, + params.max_linear_iterations, + ); + } + + /// Writes the per-cell state kept across steps back to the soft bodies. + /// + /// # Safety + /// Same contract as [`Self::predict`]. + pub unsafe fn writeback(&self, index: usize, step_dt: Real) { + let entry = &self.active[index]; + let system = unsafe { &*entry.system }; + let sb = unsafe { &mut *entry.body }; + system.writeback(sb, step_dt); + } + + pub fn num_active(&self) -> usize { + self.active.len() + } +} diff --git a/src/dynamics/solver/soft_fem/soft_fem_skyline.rs b/src/dynamics/solver/soft_fem/soft_fem_skyline.rs new file mode 100644 index 000000000..2c3dc371c --- /dev/null +++ b/src/dynamics/solver/soft_fem/soft_fem_skyline.rs @@ -0,0 +1,348 @@ +//! The direct factorization of a FEM step matrix: a skyline (envelope) Cholesky under a reverse +//! Cuthill-McKee ordering of the particles. Cholesky fill-in stays inside the envelope of the +//! permuted matrix: factorization and solves cost its size, a few times a mesh matrix's non-zeros. + +use crate::alloc_prelude::*; +use crate::math::{DIM, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::soft_fem_sparse::BlockMatrix; + +/// The skyline Cholesky factorization of a symmetric positive-definite block matrix, in the +/// ordering computed from its pattern (see the module docs). The ordering and the envelope are +/// built once per pattern; [`Self::factorize`] refills and refactorizes for the current values. +#[derive(Clone, Debug)] +pub(crate) struct SkylineCholesky { + /// Permuted block index to particle. + perm: Vec, + /// Particle to permuted block index. + inv_perm: Vec, + /// Per permuted degree of freedom, the first column of its envelope row. + first: Vec, + /// Start of every row's envelope in `values` (row `i` holds the columns `first[i]..=i`). + row_start: Vec, + /// The factor `L` (lower triangle, row by row over the envelope). + values: Vec, + /// The permuted right-hand side, then solution, of a solve. + scratch: Vec, +} + +impl SkylineCholesky { + /// The ordering and envelope of `pattern` (its values are ignored). + pub fn new(pattern: &BlockMatrix) -> Self { + let n = pattern.num_rows(); + // The particle adjacency, without the diagonal. + let mut offsets = Vec::with_capacity(n + 1); + let mut adjacency: Vec = Vec::new(); + offsets.push(0u32); + for r in 0..n { + pattern.for_each_block_of_row(r as u32, |c, _| { + if c as usize != r { + adjacency.push(c); + } + }); + offsets.push(adjacency.len() as u32); + } + let neighbors = |v: usize| &adjacency[offsets[v] as usize..offsets[v + 1] as usize]; + let perm = reverse_cuthill_mckee(n, neighbors); + let mut inv_perm = vec![0u32; n]; + for (i, &p) in perm.iter().enumerate() { + inv_perm[p as usize] = i as u32; + } + // The envelope: a block row starts at its lowest permuted neighbor. + let mut first = Vec::with_capacity(n * DIM); + let mut row_start = Vec::with_capacity(n * DIM + 1); + row_start.push(0usize); + for (i, &p) in perm.iter().enumerate() { + let mut first_block = i; + for &c in neighbors(p as usize) { + first_block = first_block.min(inv_perm[c as usize] as usize); + } + for d in 0..DIM { + let row = i * DIM + d; + let col = first_block * DIM; + first.push(col as u32); + row_start.push(row_start[row] + (row - col + 1)); + } + } + let envelope = row_start[n * DIM]; + Self { + perm, + inv_perm, + first, + row_start, + values: vec![0.0; envelope], + scratch: vec![0.0; n * DIM], + } + } + + /// The number of stored factor entries. + #[cfg(test)] + pub fn envelope_len(&self) -> usize { + self.values.len() + } + + /// Factorizes `matrix` (same pattern as the one this was built from). Returns `false` when + /// the matrix is not positive definite (the factor is then unusable). + pub fn factorize(&mut self, matrix: &BlockMatrix) -> bool { + let Self { + inv_perm, + first, + row_start, + values, + .. + } = self; + let n_dofs = first.len(); + values.fill(0.0); + // The lower triangle of the permuted matrix into the envelope. + for r in 0..matrix.num_rows() { + let i = inv_perm[r] as usize; + matrix.for_each_block_of_row(r as u32, |c, block| { + let j = inv_perm[c as usize] as usize; + if j > i { + return; + } + for d in 0..DIM { + let row = i * DIM + d; + let base = row_start[row] - first[row] as usize; + for e in 0..DIM { + let col = j * DIM + e; + if col <= row { + values[base + col] = block.col(e)[d]; + } + } + } + }); + } + // Row-oriented Cholesky: row `i` of `L` from the rows above it, all inside the envelope. + for i in 0..n_dofs { + let fi = first[i] as usize; + let ri = row_start[i]; + for j in fi..i { + let fj = first[j] as usize; + let rj = row_start[j]; + let k0 = fi.max(fj); + let mut sum = values[ri + j - fi]; + if k0 < j { + let a = &values[ri + k0 - fi..ri + j - fi]; + let b = &values[rj + k0 - fj..rj + j - fj]; + sum -= a.iter().zip(b).map(|(x, y)| x * y).sum::(); + } + values[ri + j - fi] = sum / values[rj + j - fj]; + } + let mut diag = values[ri + i - fi]; + for k in fi..i { + let l = values[ri + k - fi]; + diag -= l * l; + } + if !(diag > 0.0) { + return false; + } + values[ri + i - fi] = diag.sqrt(); + } + true + } + + /// Solves `A x = b` with the last factorization (`b` and `x` per particle). + pub fn solve(&mut self, b: &[Vector], x: &mut [Vector]) { + let Self { + perm, + first, + row_start, + values, + scratch, + .. + } = self; + let n_dofs = first.len(); + for (i, &p) in perm.iter().enumerate() { + for d in 0..DIM { + scratch[i * DIM + d] = b[p as usize][d]; + } + } + // Forward: L y = b. + for i in 0..n_dofs { + let fi = first[i] as usize; + let ri = row_start[i]; + let mut sum = scratch[i]; + if fi < i { + let row = &values[ri..ri + i - fi]; + sum -= row.iter().zip(&scratch[fi..i]).map(|(l, y)| l * y).sum::(); + } + scratch[i] = sum / values[ri + i - fi]; + } + // Backward: Lᵀ x = y. + for i in (0..n_dofs).rev() { + let fi = first[i] as usize; + let ri = row_start[i]; + let xi = scratch[i] / values[ri + i - fi]; + scratch[i] = xi; + let row = &values[ri..ri + i - fi]; + for (y, l) in scratch[fi..i].iter_mut().zip(row) { + *y -= l * xi; + } + } + for (i, &p) in perm.iter().enumerate() { + for d in 0..DIM { + x[p as usize][d] = scratch[i * DIM + d]; + } + } + } +} + +/// The reverse Cuthill-McKee ordering of a graph (new index to node): a breadth-first order +/// from a pseudo-peripheral node, neighbors by ascending degree, reversed; one component +/// after another. Deterministic (ties broken by node index). +fn reverse_cuthill_mckee<'a>(n: usize, neighbors: impl Fn(usize) -> &'a [u32]) -> Vec { + let degree = |v: usize| neighbors(v).len(); + let mut order: Vec = Vec::with_capacity(n); + let mut visited = vec![false; n]; + let mut levels = vec![u32::MAX; n]; + let mut queue: Vec = Vec::new(); + let mut sorted: Vec = Vec::new(); + // Breadth-first levels from `start` (over the unvisited nodes): the last level's node of + // least degree, and the eccentricity. + let bfs = |start: usize, levels: &mut Vec, queue: &mut Vec, visited: &[bool]| { + levels.fill(u32::MAX); + queue.clear(); + queue.push(start as u32); + levels[start] = 0; + let mut head = 0; + while head < queue.len() { + let v = queue[head] as usize; + head += 1; + for &w in neighbors(v) { + let w = w as usize; + if !visited[w] && levels[w] == u32::MAX { + levels[w] = levels[v] + 1; + queue.push(w as u32); + } + } + } + let last = levels[queue[queue.len() - 1] as usize]; + let far = queue + .iter() + .copied() + .filter(|&v| levels[v as usize] == last) + .min_by_key(|&v| (degree(v as usize), v)) + .unwrap_or(start as u32); + (far as usize, last) + }; + let mut done = 0; + while done < n { + // The component's start: a pseudo-peripheral node found from its least-degree node. + let mut start = (0..n) + .filter(|&v| !visited[v]) + .min_by_key(|&v| (degree(v), v)) + .unwrap(); + let (mut far, mut ecc) = bfs(start, &mut levels, &mut queue, &visited); + for _ in 0..4 { + let (next, next_ecc) = bfs(far, &mut levels, &mut queue, &visited); + if next_ecc <= ecc { + break; + } + start = far; + far = next; + ecc = next_ecc; + } + let _ = (far, ecc); + // Cuthill-McKee over the component. + let component_start = order.len(); + order.push(start as u32); + visited[start] = true; + let mut head = component_start; + while head < order.len() { + let v = order[head] as usize; + head += 1; + sorted.clear(); + sorted.extend(neighbors(v).iter().copied().filter(|&w| !visited[w as usize])); + sorted.sort_unstable_by_key(|&w| (degree(w as usize), w)); + for &w in &sorted { + if !visited[w as usize] { + visited[w as usize] = true; + order.push(w); + } + } + } + done = order.len(); + } + order.reverse(); + order +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::dynamics::solver::soft_constraint::soft_element_constraint::outer_product; + use crate::math::Matrix; + + /// A grid-shaped SPD block system (with a pinned row) solved through the skyline + /// factorization must satisfy the system to solver accuracy, whatever the ordering did. + #[test] + fn skyline_solve_matches_the_system() { + let (w, h) = (7usize, 5usize); + let n = w * h; + let id = |x: usize, y: usize| (y * w + x) as u32; + let mut pairs: Vec<(u32, u32)> = Vec::new(); + for y in 0..h { + for x in 0..w { + if x + 1 < w { + pairs.push((id(x, y), id(x + 1, y))); + } + if y + 1 < h { + pairs.push((id(x, y), id(x, y + 1))); + } + if x + 1 < w && y + 1 < h { + pairs.push((id(x, y), id(x + 1, y + 1))); + } + } + } + let mut matrix = BlockMatrix::from_pairs(n, pairs.iter().copied()); + let mut seed = 987654321u64; + let mut random = || { + seed = seed.wrapping_mul(6364136223846793005).wrapping_add(1); + ((seed >> 33) as Real) / (u32::MAX as Real / 2.0) - 1.0 + }; + for r in 0..n { + let idx = matrix.diagonal_index(r as u32) as usize; + matrix.blocks[idx] = Matrix::IDENTITY * (1.0 + random().abs()); + } + for &(a, b) in &pairs { + let mut dir = Vector::ZERO; + for d in 0..DIM { + dir[d] = random(); + } + let dir = dir.normalize(); + let block = outer_product(dir, dir) * (1.0 + random().abs()); + for (r, c) in [(a, b), (b, a)] { + let idx = matrix.block_index(r, c) as usize; + matrix.blocks[idx] += -block; + } + for r in [a, b] { + let idx = matrix.diagonal_index(r) as usize; + matrix.blocks[idx] += block; + } + } + matrix.make_dirichlet(id(3, 2)); + + let mut skyline = SkylineCholesky::new(&matrix); + let mut seen = vec![false; n]; + for &p in &skyline.perm { + assert!(!core::mem::replace(&mut seen[p as usize], true)); + } + assert!(skyline.envelope_len() < n * DIM * (n * DIM + 1) / 2); + assert!(skyline.factorize(&matrix)); + + let rhs: Vec = (0..n).map(|_| Vector::ONE * random()).collect(); + let mut x = vec![Vector::ZERO; n]; + skyline.solve(&rhs, &mut x); + let mut ax = vec![Vector::ZERO; n]; + matrix.mul(&x, &mut ax); + for k in 0..n { + let err = (ax[k] - rhs[k]).length(); + assert!(err < 1.0e-4, "residual {err} at row {k}"); + } + assert_eq!(x[id(3, 2) as usize], rhs[id(3, 2) as usize]); + } +} diff --git a/src/dynamics/solver/soft_fem/soft_fem_sparse.rs b/src/dynamics/solver/soft_fem/soft_fem_sparse.rs new file mode 100644 index 000000000..f0801ab95 --- /dev/null +++ b/src/dynamics/solver/soft_fem/soft_fem_sparse.rs @@ -0,0 +1,372 @@ +//! The block-sparse matrix and the preconditioned conjugate gradient behind the FEM soft-body +//! solver. A soft body's system matrix `A = M + h D + h² K` is symmetric positive definite with a +//! `DIM × DIM` block per particle pair sharing an element, stored as block-CSR (both triangles). + +use crate::alloc_prelude::*; +use crate::math::{Matrix, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// A block-sparse symmetric matrix with `DIM × DIM` blocks, stored in block-CSR with both +/// triangles; the sparsity pattern is built once from the element graph and reused across steps, +/// only the block values being refilled (`block_index` resolves a `(row, col)` pair to its slot). +#[derive(Clone, Debug, Default)] +pub(crate) struct BlockMatrix { + /// Start of each row's blocks in `cols`/`blocks` (length `num_rows + 1`). + row_offsets: Vec, + /// Column of every stored block, ascending within a row. + cols: Vec, + /// Slot of every row's diagonal block. + diagonal: Vec, + /// The block values, parallel to `cols`. + pub blocks: Vec, +} + +impl BlockMatrix { + /// Builds the pattern of a `num_rows × num_rows` block matrix: the diagonal blocks plus the + /// off-diagonal `pairs` (either order, symmetrized). The values are left zeroed. + pub fn from_pairs(num_rows: usize, pairs: impl Iterator + Clone) -> Self { + let mut counts = vec![1u32; num_rows + 1]; + for (a, b) in pairs.clone() { + if a != b { + counts[a as usize] += 1; + counts[b as usize] += 1; + } + } + // Prefix sum into the row offsets (with room for the duplicates, pruned below). + let mut row_offsets = Vec::with_capacity(num_rows + 1); + let mut total = 0u32; + for c in &counts[..num_rows] { + row_offsets.push(total); + total += *c; + } + row_offsets.push(total); + + let mut cols = vec![u32::MAX; total as usize]; + let mut cursors: Vec = row_offsets[..num_rows].to_vec(); + for r in 0..num_rows { + cols[cursors[r] as usize] = r as u32; + cursors[r] += 1; + } + for (a, b) in pairs { + if a != b { + cols[cursors[a as usize] as usize] = b; + cursors[a as usize] += 1; + cols[cursors[b as usize] as usize] = a; + cursors[b as usize] += 1; + } + } + + // Sort and deduplicate each row, compacting in place. + let mut out_cols = Vec::with_capacity(total as usize); + let mut out_offsets = Vec::with_capacity(num_rows + 1); + let mut diagonal = Vec::with_capacity(num_rows); + for r in 0..num_rows { + out_offsets.push(out_cols.len() as u32); + let row = &mut cols[row_offsets[r] as usize..row_offsets[r + 1] as usize]; + row.sort_unstable(); + let mut previous = u32::MAX; + for &c in row.iter() { + if c != previous { + if c == r as u32 { + diagonal.push(out_cols.len() as u32); + } + out_cols.push(c); + previous = c; + } + } + } + out_offsets.push(out_cols.len() as u32); + + let blocks = vec![Matrix::ZERO; out_cols.len()]; + Self { + row_offsets: out_offsets, + cols: out_cols, + diagonal, + blocks, + } + } + + pub fn num_rows(&self) -> usize { + self.row_offsets.len().saturating_sub(1) + } + + /// The slot of the `(row, col)` block, or `u32::MAX` if the pattern has none. + pub fn block_index(&self, row: u32, col: u32) -> u32 { + let start = self.row_offsets[row as usize] as usize; + let end = self.row_offsets[row as usize + 1] as usize; + match self.cols[start..end].binary_search(&col) { + Ok(k) => (start + k) as u32, + Err(_) => u32::MAX, + } + } + + /// The slot of the `row`-th diagonal block. + #[inline] + pub fn diagonal_index(&self, row: u32) -> u32 { + self.diagonal[row as usize] + } + + pub fn clear_values(&mut self) { + self.blocks.fill(Matrix::ZERO); + } + + /// `out = A · x`. + pub fn mul(&self, x: &[Vector], out: &mut [Vector]) { + debug_assert_eq!(x.len(), self.num_rows()); + for (r, o) in out.iter_mut().enumerate() { + let start = self.row_offsets[r] as usize; + let end = self.row_offsets[r + 1] as usize; + let mut acc = Vector::ZERO; + for k in start..end { + acc += self.blocks[k] * x[self.cols[k] as usize]; + } + *o = acc; + } + } + + /// Turns row `row` into the identity row of a Dirichlet (pinned) degree of freedom: its + /// diagonal block becomes the identity and every other block of the row and of the matching + /// column is zeroed, which keeps the matrix symmetric. + pub fn make_dirichlet(&mut self, row: u32) { + let start = self.row_offsets[row as usize] as usize; + let end = self.row_offsets[row as usize + 1] as usize; + for k in start..end { + let col = self.cols[k]; + self.blocks[k] = if col == row { + Matrix::IDENTITY + } else { + Matrix::ZERO + }; + if col != row { + let mirror = self.block_index(col, row); + if mirror != u32::MAX { + self.blocks[mirror as usize] = Matrix::ZERO; + } + } + } + } +} + +/// Workspace and stopping rules of the block-Jacobi preconditioned conjugate gradient. +#[derive(Clone, Debug, Default)] +pub(crate) struct ConjugateGradient { + /// Inverse of every diagonal block (the preconditioner). + preconditioner: Vec, + residual: Vec, + direction: Vec, + /// `A · direction`. + a_direction: Vec, + /// The preconditioned residual. + z: Vec, +} + +impl ConjugateGradient { + /// Updates the block-Jacobi preconditioner from the matrix's diagonal blocks. Must be + /// called after every change of the matrix values, before [`Self::solve`]. + pub fn update_preconditioner(&mut self, matrix: &BlockMatrix) { + let n = matrix.num_rows(); + self.preconditioner.resize(n, Matrix::IDENTITY); + for (r, p) in self.preconditioner.iter_mut().enumerate() { + let block = matrix.blocks[matrix.diagonal_index(r as u32) as usize]; + // The diagonal blocks are SPD (every free particle has mass), but a degenerate + // element can still make one singular: fall back to the identity there rather than + // poisoning the whole solve with a non-finite preconditioner. + let inv = block.inverse(); + *p = if inv.is_finite() { + inv + } else { + Matrix::IDENTITY + }; + } + } + + /// Solves `A x = b` from the guess in `x` with fixed-order reductions (worker-independent + /// result), returning the iteration count; stops on `‖r‖ ≤ tolerance · ‖b‖` or after + /// `max_iterations`, then scales the solution by its energy minimizer, clamped to `[0, 1]`. + pub fn solve( + &mut self, + matrix: &BlockMatrix, + b: &[Vector], + x: &mut [Vector], + tolerance: Real, + max_iterations: usize, + ) -> usize { + let n = matrix.num_rows(); + debug_assert_eq!(b.len(), n); + debug_assert_eq!(x.len(), n); + self.residual.resize(n, Vector::ZERO); + self.direction.resize(n, Vector::ZERO); + self.a_direction.resize(n, Vector::ZERO); + self.z.resize(n, Vector::ZERO); + + let b_norm2: Real = b.iter().map(|v| v.length_squared()).sum(); + if b_norm2 == 0.0 { + x.fill(Vector::ZERO); + return 0; + } + let threshold = tolerance * tolerance * b_norm2; + + matrix.mul(x, &mut self.residual); + for (r, bi) in self.residual.iter_mut().zip(b) { + *r = *bi - *r; + } + let mut residual_norm2: Real = self.residual.iter().map(|v| v.length_squared()).sum(); + if residual_norm2 <= threshold { + return 0; + } + for (k, z) in self.z.iter_mut().enumerate() { + *z = self.preconditioner[k] * self.residual[k]; + } + self.direction.copy_from_slice(&self.z); + let mut rz: Real = self + .residual + .iter() + .zip(&self.z) + .map(|(r, z)| r.dot(*z)) + .sum(); + + for iteration in 0..max_iterations { + matrix.mul(&self.direction, &mut self.a_direction); + let denominator: Real = self + .direction + .iter() + .zip(&self.a_direction) + .map(|(p, ap)| p.dot(*ap)) + .sum(); + if denominator.partial_cmp(&0.0) != Some(core::cmp::Ordering::Greater) { + // A non-positive curvature means the matrix is not SPD (a degenerate element, or + // a stiffness that slipped past the flooring): stop with what we have. + self.rescale(matrix, b, x); + return iteration; + } + let alpha = rz / denominator; + for k in 0..n { + x[k] += self.direction[k] * alpha; + self.residual[k] -= self.a_direction[k] * alpha; + } + residual_norm2 = self.residual.iter().map(|v| v.length_squared()).sum(); + if residual_norm2 <= threshold { + return iteration + 1; + } + for (k, z) in self.z.iter_mut().enumerate() { + *z = self.preconditioner[k] * self.residual[k]; + } + let rz_next: Real = self + .residual + .iter() + .zip(&self.z) + .map(|(r, z)| r.dot(*z)) + .sum(); + let beta = rz_next / rz; + rz = rz_next; + for k in 0..n { + self.direction[k] = self.z[k] + self.direction[k] * beta; + } + } + self.rescale(matrix, b, x); + max_iterations + } + + /// Scales `x` by the minimizer of `½ xᵀAx − bᵀx` along it, clamped to `[0, 1]`. + fn rescale(&mut self, matrix: &BlockMatrix, b: &[Vector], x: &mut [Vector]) { + matrix.mul(x, &mut self.a_direction); + let numerator: Real = b.iter().zip(x.iter()).map(|(bi, xi)| bi.dot(*xi)).sum(); + let denominator: Real = x + .iter() + .zip(&self.a_direction) + .map(|(xi, ax)| xi.dot(*ax)) + .sum(); + if denominator <= 0.0 { + x.fill(Vector::ZERO); + return; + } + let alpha = (numerator / denominator).clamp(0.0, 1.0); + if alpha != 1.0 { + for xi in x.iter_mut() { + *xi *= alpha; + } + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::dynamics::solver::soft_constraint::soft_element_constraint::outer_product; + + /// A small SPD block system solved by the conjugate gradient must match a dense reference + /// solve. + #[test] + fn conjugate_gradient_matches_dense_solve() { + let n = 12; + // A chain: every particle couples with its two neighbours. + let pairs: Vec<(u32, u32)> = (0..n as u32 - 1).map(|i| (i, i + 1)).collect(); + let mut matrix = BlockMatrix::from_pairs(n, pairs.iter().copied()); + + // Diagonally dominant SPD blocks: an isotropic mass plus a spring per neighbour. + let mut seed = 12345u64; + let mut random = || { + seed = seed.wrapping_mul(6364136223846793005).wrapping_add(1); + ((seed >> 33) as Real) / (u32::MAX as Real / 2.0) - 1.0 + }; + for r in 0..n { + let idx = matrix.diagonal_index(r as u32) as usize; + matrix.blocks[idx] = Matrix::IDENTITY * (2.0 + random().abs()); + } + for &(a, b) in &pairs { + let dir = Vector::splat(1.0).normalize(); + let k = 1.0 + random().abs(); + let block = outer_product(dir, dir) * k; + for (r, c) in [(a, b), (b, a)] { + let idx = matrix.block_index(r, c) as usize; + matrix.blocks[idx] += -block; + } + for r in [a, b] { + let idx = matrix.diagonal_index(r) as usize; + matrix.blocks[idx] += block; + } + } + + let rhs: Vec = (0..n) + .map(|_| Vector::splat(0.0) + Vector::ONE * random()) + .collect(); + let mut x = vec![Vector::ZERO; n]; + let mut cg = ConjugateGradient::default(); + cg.update_preconditioner(&matrix); + cg.solve(&matrix, &rhs, &mut x, 1.0e-10, 1000); + + // Residual check (the dense reference here is the matrix itself). + let mut ax = vec![Vector::ZERO; n]; + matrix.mul(&x, &mut ax); + for k in 0..n { + let err = (ax[k] - rhs[k]).length(); + assert!(err < 1.0e-4, "residual {err} at row {k}"); + } + } + + /// A Dirichlet row must keep its right-hand side and decouple from its neighbours. + #[test] + fn dirichlet_rows_are_decoupled() { + let pairs = [(0u32, 1u32), (1, 2)]; + let mut matrix = BlockMatrix::from_pairs(3, pairs.iter().copied()); + for r in 0..3 { + let idx = matrix.diagonal_index(r) as usize; + matrix.blocks[idx] = Matrix::IDENTITY * 4.0; + } + for &(a, b) in &pairs { + for (r, c) in [(a, b), (b, a)] { + let idx = matrix.block_index(r, c) as usize; + matrix.blocks[idx] = Matrix::IDENTITY * -1.0; + } + } + matrix.make_dirichlet(1); + let x = [Vector::ONE, Vector::ONE * 2.0, Vector::ONE * 3.0]; + let mut out = [Vector::ZERO; 3]; + matrix.mul(&x, &mut out); + assert_eq!(out[1], Vector::ONE * 2.0); + assert_eq!(out[0], Vector::ONE * 4.0); + assert_eq!(out[2], Vector::ONE * 12.0); + } +} diff --git a/src/dynamics/solver/soft_fem/system/mod.rs b/src/dynamics/solver/soft_fem/system/mod.rs new file mode 100644 index 000000000..1365d3cd9 --- /dev/null +++ b/src/dynamics/solver/soft_fem/system/mod.rs @@ -0,0 +1,11 @@ +//! The per-soft-body FEM system: element state, the assembly of `A = M + h D + h² K`, and the +//! substep's two solves (the implicit elastic predictor, the constraint impulses' propagation). + +mod soft_fem_system_assemble; +mod soft_fem_system_elements; +mod soft_fem_system_prepare; +mod soft_fem_system_response; +#[cfg(test)] +mod tests; + +pub(crate) use soft_fem_system_elements::SoftFemSystem; diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs new file mode 100644 index 000000000..9838e5e8e --- /dev/null +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs @@ -0,0 +1,260 @@ +//! The per-substep work: reading the particle state, assembling `A` and the elastic force, the predictor solve and the writeback to the soft body. + +use super::SoftFemSystem; +use crate::alloc_prelude::*; +use crate::dynamics::SoftBody; +use crate::dynamics::soft_body::soft_body_shape_matching::extract_rotation; +use crate::dynamics::solver::soft_constraint::soft_constraints_set::plastic_flow; +#[cfg(feature = "dim3")] +use crate::dynamics::solver::soft_constraint::soft_element_constraint::dihedral_gradients; +use crate::dynamics::solver::soft_constraint::soft_element_constraint::{ + MAX_CONSTRAINT_PARTICLES, NeoHookeanConstraint, STIFFNESS_UPDATE_STRAIN, SoftElasticConstraint, + StrainJacobian, outer_product, strain_rows_of, +}; +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::math::{Matrix, Real, Vector}; +use crate::utils::RotationOps; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +impl SoftFemSystem { + /// Reads the particles' state from their solver bodies. + fn read_state(&mut self, bodies: &SolverBodies) { + for (k, &slot) in self.slots.iter().enumerate() { + if slot != u32::MAX { + self.position[k] = bodies.get_pose(slot).translation; + self.velocity[k] = bodies.get_vel(slot).linear; + } + } + } + + /// Assembles `A = M + (h² + h β) K` and the elastic force at the current positions; pinned + /// degrees of freedom become Dirichlet rows, with their coupling to the free rows + /// (`Σ_{q pinned} A_pq v_q`) kept for the right-hand side; a step's first substep reuses it. + pub(super) fn assemble_operator(&mut self, dt: Real) { + self.matrix.clear_values(); + for (k, m) in self.mass.iter().enumerate() { + let idx = self.matrix.diagonal_index(k as u32) as usize; + self.matrix.blocks[idx] = Matrix::IDENTITY * *m; + } + self.force.fill(Vector::ZERO); + + for (ci, cell) in self.cells.iter_mut().enumerate() { + if cell.mu <= 0.0 { + continue; + } + let x: [Vector; MAX_CONSTRAINT_PARTICLES] = + core::array::from_fn(|k| self.position[cell.vertices[k] as usize]); + let f = SoftBody::cell_edge_matrix(x) * cell.inv_rest_matrix; + cell.rotation = extract_rotation(f, cell.rotation); + let s = cell.rotation.inverse().to_mat() * f; + cell.strain = strain_rows_of(&(s - Matrix::IDENTITY)); + + let gradient = if cell.neo_hookean { + // Stable Neo-Hookean (Smith et al. 2018): the energy is defined and bounded + // below for inverted cells, and its tangent is floored mode-wise at the rest + // curvature so one implicit step per substep needs no line search. + let lambda = cell.lambda + cell.mu; + let (s, kappa, gradient) = NeoHookeanConstraint::gradient(cell.mu, lambda, &cell.strain); + let stale = (cell.strain - cell.tangent_strain) + .iter() + .any(|d| d.abs() > STIFFNESS_UPDATE_STRAIN); + if stale { + cell.tangent = NeoHookeanConstraint::hessian(cell.mu, lambda, &s, kappa); + cell.tangent_strain = cell.strain; + } + gradient + } else { + // Linear elasticity in the cell's rotation-free frame: a constant tangent. + cell.tangent * cell.strain + }; + + // World-frame strain gradients: column `r` of `jacobians[p]` is `R gᵣ[p]`. + let rotation = cell.rotation.to_mat(); + let jacobians: [StrainJacobian; MAX_CONSTRAINT_PARTICLES] = core::array::from_fn(|p| { + SoftElasticConstraint::strain_jacobian(&cell.coeffs, p, &rotation) + }); + + // Elastic force `f_p = −Σᵣ g̃ᵣ[p] gradᵣ`. + for p in 0..MAX_CONSTRAINT_PARTICLES { + let fp: Vector = (jacobians[p] * gradient).into(); + self.force[cell.vertices[p] as usize] -= fp; + } + + // Tangent `K_pq = Σᵣₛ Hᵣₛ g̃ᵣ[p] g̃ₛ[q]ᵀ = J_p H J_qᵀ`, scaled by `h² + h β` (the + // stiffness and its Rayleigh damping share the same matrix); `K_qp = K_pqᵀ`. + let scale = dt * dt + dt * cell.beta; + let blocks = &self.cell_blocks[ci]; + for q in 0..MAX_CONSTRAINT_PARTICLES { + let hq = cell.tangent * jacobians[q].transpose(); + for p in 0..=q { + let slot = blocks[p * MAX_CONSTRAINT_PARTICLES + q]; + if slot == u32::MAX { + continue; + } + let block: Matrix = (jacobians[p] * hq).into(); + let block = block * scale; + self.matrix.blocks[slot as usize] += block; + if p != q { + let mirror = blocks[q * MAX_CONSTRAINT_PARTICLES + p]; + self.matrix.blocks[mirror as usize] += block.transpose(); + } + } + } + } + + self.assemble_springs(dt); + #[cfg(feature = "dim3")] + self.assemble_dihedrals(dt); + // The pinned particles' coupling to the free rows (`A_pq = A_qpᵀ`), then their Dirichlet + // rows. + self.pinned_coupling.clear(); + self.pinned_coupling.resize(self.mass.len(), Vector::ZERO); + for (k, &pinned) in self.pinned.iter().enumerate() { + if pinned { + let v = self.velocity[k]; + let coupling = &mut self.pinned_coupling; + self.matrix.for_each_block_of_row(k as u32, |col, block| { + if col as usize != k { + coupling[col as usize] += block.transpose() * v; + } + }); + self.matrix.make_dirichlet(k as u32); + } + } + self.cg.update_preconditioner(&self.matrix); + } + + /// The predictor's right-hand side `b = h f − (A − M) v` from the assembled operator and + /// the current velocities. + fn assemble_rhs(&mut self, dt: Real) { + self.matrix.mul(&self.velocity, &mut self.rhs); + for k in 0..self.rhs.len() { + self.rhs[k] = self.force[k] * dt - (self.rhs[k] + self.pinned_coupling[k]) + + self.velocity[k] * self.mass[k]; + } + for (k, &pinned) in self.pinned.iter().enumerate() { + if pinned { + self.rhs[k] = Vector::ZERO; + } + } + } + + /// Accumulates the distance elements' force and Gauss-Newton tangent. + fn assemble_springs(&mut self, dt: Real) { + for spring in &self.springs { + if spring.stiffness <= 0.0 { + continue; + } + let a = spring.vertices[0] as usize; + let b = spring.vertices[1] as usize; + let d = self.position[b] - self.position[a]; + let length = d.length(); + if length < 1.0e-9 { + continue; + } + let c = length - spring.rest_length; + if spring.tension_only && c <= 0.0 { + continue; + } + let n = d / length; + let force = n * (spring.stiffness * c); + self.force[a] += force; + self.force[b] -= force; + + let block = outer_product(n, n) * (spring.stiffness * (dt * dt + dt * spring.beta)); + for (k, &slot) in spring.blocks.iter().enumerate() { + if slot != u32::MAX { + // Diagonal blocks (k = 0, 3) get `+k nnᵀ`, off-diagonal ones `−k nnᵀ`. + let sign = if k == 0 || k == 3 { 1.0 } else { -1.0 }; + self.matrix.blocks[slot as usize] += block * sign; + } + } + } + } + + /// Accumulates the dihedral elements' force and Gauss-Newton tangent. + #[cfg(feature = "dim3")] + fn assemble_dihedrals(&mut self, dt: Real) { + for dihedral in &self.dihedrals { + if dihedral.stiffness <= 0.0 { + continue; + } + let pos: [Vector; 4] = + core::array::from_fn(|k| self.position[dihedral.vertices[k] as usize]); + let mut grad = [Vector::ZERO; 4]; + let c = dihedral_gradients(&pos, dihedral.rest_angle, &mut grad); + for k in 0..4 { + self.force[dihedral.vertices[k] as usize] -= grad[k] * (dihedral.stiffness * c); + } + let scale = dihedral.stiffness * (dt * dt + dt * dihedral.beta); + for p in 0..4 { + for q in 0..4 { + let slot = dihedral.blocks[p * 4 + q]; + if slot != u32::MAX { + self.matrix.blocks[slot as usize] += + outer_product(grad[p], grad[q]) * scale; + } + } + } + } + } + + /// The implicit elastic step of one substep: assemble, solve `A Δv = b`, and add `Δv` to the + /// particles' solver velocities. Returns the conjugate-gradient iteration count. + pub fn predict( + &mut self, + bodies: &mut SolverBodies, + dt: Real, + tolerance: Real, + max_iterations: usize, + ) -> usize { + self.read_state(bodies); + // The step-start factorization assembled this operator at these same positions (the + // substep only changed the velocities): reused by the first substep. + if !core::mem::take(&mut self.operator_fresh) { + self.assemble_operator(dt); + } + self.assemble_rhs(dt); + // Started from zero, never warm-started: a truncated CG converges the smooth modes first, + // so a partial solve is an under-relaxed step, while a stale start leaves high-frequency + // content the truncation never removes and pumps energy into a stiff body. + self.delta.fill(Vector::ZERO); + let iterations = self.cg.solve( + &self.matrix, + &self.rhs, + &mut self.delta, + tolerance, + max_iterations, + ); + for (k, &slot) in self.slots.iter().enumerate() { + if slot != u32::MAX && !self.pinned[k] { + bodies.vels[slot as usize].linear += self.delta[k]; + } + } + iterations + } + + /// Writes back what the soft body keeps across steps: each cell's warm-started polar rotation, + /// the step's plastic flow, and the tearing marks of elements strained past `tear_strain` + /// (the FEM counterpart of `SoftConstraintsSet::writeback_constraints`). + pub fn writeback(&self, sb: &mut SoftBody, step_dt: Real) { + let material = sb.material; + let plastic = material.plastic_yield > 0.0 && material.plastic_creep > 0.0; + let mut flowing = false; + let mut torn = false; + // The interior strength reads the particles' flags: taken before the cells are borrowed. + out.rotation = cell.rotation; + if cell.mu <= 0.0 { + continue; + } + if plastic && plastic_flow(out, &cell.strain, &material, step_dt) { + flowing = true; + } + } + } + sb.plastic_flowing |= flowing; + sb.tearing_pending |= torn; + } +} diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs new file mode 100644 index 000000000..af4be9bdb --- /dev/null +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs @@ -0,0 +1,107 @@ +//! The element state of a FEM soft body and the `SoftFemSystem` that owns it. + +use super::super::soft_fem_sparse::{BlockMatrix, ConjugateGradient}; +use crate::alloc_prelude::*; +use crate::dynamics::solver::soft_constraint::soft_element_constraint::{ + MAX_CONSTRAINT_PARTICLES, StrainMatrix, StrainVector, +}; +use crate::math::{Matrix, Real, Rotation, Vector}; + +/// Number of `DIM × DIM` blocks a cell writes into the system matrix. +pub(super) const CELL_BLOCKS: usize = MAX_CONSTRAINT_PARTICLES * MAX_CONSTRAINT_PARTICLES; + +/// One finite element (a triangle in 2D, a tetrahedron in 3D) of a FEM soft body: +/// [`SoftFemSystem::prepare`] sets its per-step constants (material, shape functions, Rayleigh +/// coefficient), [`SoftFemSystem::assemble`] its rotation, strain, force and tangent per substep. +#[derive(Copy, Clone, Debug)] +pub(super) struct FemCell { + pub(super) vertices: [u32; MAX_CONSTRAINT_PARTICLES], + /// `∂F_cj/∂(x_a)_c`: the constraints of `Dm⁻¹`, particle 0 minus their sum. + pub(super) coeffs: [Vector; MAX_CONSTRAINT_PARTICLES], + pub(super) inv_rest_matrix: Matrix, + /// `μ V₀` and `λ V₀` (Lamé parameters scaled by the rest volume). + pub(super) mu: Real, + pub(super) lambda: Real, + /// Rayleigh damping coefficient `β` (the damping matrix of the cell is `β K`). + pub(super) beta: Real, + /// Polar rotation of the deformation gradient, frozen over the substep and warm-started + /// from the previous one. + pub(super) rotation: Rotation, + /// Corotated strain `sym(RᵀF) - I` in the strain-row coordinates. + pub(super) strain: StrainVector, + /// Whether the cell carries the stable Neo-Hookean energy instead of the linear-elastic + /// (corotational) one. + pub(super) neo_hookean: bool, + /// The tangent `V₀ ∂²Ψ/∂ε²` last computed, and the strain it was computed at + /// (`Real::MAX`: never). Constant for the corotational model; the Neo-Hookean one + /// re-linearizes when the strain moved by more than `STIFFNESS_REFRESH_STRAIN`. + pub(super) tangent: StrainMatrix, + pub(super) tangent_strain: StrainVector, +} + +/// One distance element (a structural or bending edge) of a FEM soft body. +/// +/// Energy `½ k (|d| − L)²`, with the Gauss-Newton tangent `k ∇C ∇Cᵀ` (the indefinite `C ∂²C` +/// term dropped: an implicit step needs a positive semi-definite stiffness). +#[derive(Copy, Clone, Debug)] +pub(super) struct FemSpring { + pub(super) vertices: [u32; 2], + /// Slots of the element's four blocks, row-major. + pub(super) blocks: [u32; 4], + pub(super) rest_length: Real, + pub(super) stiffness: Real, + pub(super) beta: Real, + /// A rope segment: no stiffness under compression. + pub(super) tension_only: bool, +} + +/// One dihedral bending element of a FEM soft body (3D): energy `½ k (θ − θ₀)²`, Gauss-Newton +/// tangent, gradients shared with the constraint path. +#[cfg(feature = "dim3")] +#[derive(Copy, Clone, Debug)] +pub(super) struct FemDihedral { + pub(super) vertices: [u32; 4], + /// Slots of the element's sixteen blocks, row-major. + pub(super) blocks: [u32; 16], + pub(super) rest_angle: Real, + pub(super) stiffness: Real, + pub(super) beta: Real, +} + +/// The FEM system of one soft body: the block-sparse `A`, its conjugate-gradient workspace, and +/// the per-particle vectors the two solves work on. The sparsity pattern is built once from the +/// element graph and kept across steps; the values are refilled every substep. +#[derive(Clone, Debug, Default)] +pub(crate) struct SoftFemSystem { + /// Topology version the pattern was built for (bumped by tearing). + pub(super) topology_version: u32, + pub(super) matrix: BlockMatrix, + pub(super) cg: ConjugateGradient, + pub(super) cells: Vec, + /// Slots of every cell's `(DIM+1)²` blocks, row-major over its particles. + pub(super) cell_blocks: Vec<[u32; CELL_BLOCKS]>, + pub(super) volume_cells: Vec, + pub(super) springs: Vec, + #[cfg(feature = "dim3")] + pub(super) dihedrals: Vec, + /// Particle masses (`0` for a pinned particle: its constraint is a Dirichlet constraint). + pub(super) mass: Vec, + /// Solver-body slot of every particle. + pub(super) slots: Vec, + /// Whether the particle is pinned (`inv_mass == 0`). + pub(super) pinned: Vec, + pub(super) position: Vec, + pub(super) velocity: Vec, + /// Elastic force of the current configuration. + pub(super) force: Vec, + /// `Σ_{q pinned} A_pq v_q` per free row: the pinned particles' right-hand-side term, kept when + /// their rows become Dirichlet rows. + pub(super) pinned_coupling: Vec, + /// The operator (`matrix`, `force`, `pinned_coupling`) was just assembled at the current + /// positions by the step-start factorization: the first substep's predict reuses it. + pub(super) operator_fresh: bool, + pub(super) rhs: Vec, + /// Solution of the last predictor solve (the warm start of the next one). + pub(super) delta: Vec, + pub(super) step_matrix: BlockMatrix, +} diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs new file mode 100644 index 000000000..694299440 --- /dev/null +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs @@ -0,0 +1,237 @@ +//! The step-constant preparation: the sparsity pattern, the per-element material and shape functions, and the particle state. + +use super::SoftFemSystem; +use super::soft_fem_system_elements::{CELL_BLOCKS, FemCell, FemSpring, FemVolumeCell}; +#[cfg(feature = "dim3")] +use super::soft_fem_system_elements::FemDihedral; +use super::super::soft_fem_sparse::BlockMatrix; +#[cfg(feature = "dim3")] +use crate::dynamics::solver::soft_constraint::soft_element_constraint::dihedral_gradients; +use crate::dynamics::solver::soft_constraint::soft_element_constraint::{ + MAX_CONSTRAINT_PARTICLES, STRAIN_ROWS, SoftElasticConstraint, StrainMatrix, StrainVector, +}; +use crate::dynamics::{SoftBody, SoftBodyCellModel, SoftBodyEdgeKind, SpringCoefficients}; +use crate::math::{DIM, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +impl SoftFemSystem { + /// Updates everything that is constant over a step: the sparsity pattern (rebuilt when the + /// topology changed), the per-cell material and shape functions, and the particle masses. + pub fn prepare(&mut self, sb: &SoftBody, slots: &[u32]) { + let n = sb.particles.len(); + let rebuild = self.matrix.num_rows() != n + || self.cells.len() != sb.cells.len() + || self.springs.len() != sb.edges.len() + || self.topology_version != sb.topology_version(); + if rebuild { + self.topology_version = sb.topology_version(); + self.rebuild_pattern(sb); + self.direct = None; + } + + self.mass.clear(); + self.pinned.clear(); + self.position.clear(); + self.velocity.clear(); + for p in &sb.particles { + self.mass.push(if p.inv_mass == 0.0 { 0.0 } else { p.mass }); + self.pinned.push(p.inv_mass == 0.0); + self.position.push(p.position); + self.velocity.push(p.velocity); + } + self.slots.clear(); + self.slots.extend_from_slice(slots); + self.force.resize(n, Vector::ZERO); + self.rhs.resize(n, Vector::ZERO); + self.delta.resize(n, Vector::ZERO); + let (mu, lambda) = sb.material.lame_parameters(); + let zeta = sb.material.elastic_damping_ratio; + let elastic = matches!( + sb.cell_model, + SoftBodyCellModel::Corotational | SoftBodyCellModel::NeoHookean + ); + self.cells.clear(); + for c in &sb.cells { + // A degenerate cell, one with no material, or the `Volume` cell model (a volume + // element instead, see `prepare_volume_cells`): no elastic element. + let volume = if elastic && mu > 0.0 { + c.rest_volume.abs() + } else { + 0.0 + }; + let coeffs = SoftElasticConstraint::coefficients(&c.inv_rest_matrix); + let inv_mass: [Real; MAX_CONSTRAINT_PARTICLES] = + core::array::from_fn(|k| sb.particles[c.vertices[k] as usize].inv_mass); + let beta = Self::rayleigh_coefficient(&coeffs, &inv_mass, mu, lambda, volume, zeta); + self.cells.push(FemCell { + vertices: c.vertices, + coeffs, + inv_rest_matrix: c.inv_rest_matrix, + mu: mu * volume, + lambda: lambda * volume, + beta, + rotation: c.rotation, + strain: StrainVector::zeros(), + neo_hookean: sb.cell_model == SoftBodyCellModel::NeoHookean, + tangent: Self::corotational_hessian(mu * volume, lambda * volume), + tangent_strain: StrainVector::repeat(Real::MAX), + }); + } + + self.prepare_springs(sb); + #[cfg(feature = "dim3")] + self.prepare_dihedrals(sb); + /// Rebuilds the sparsity pattern from the element graph and caches every element's block + /// slots. + fn rebuild_pattern(&mut self, sb: &SoftBody) { + let n = sb.particles.len(); + let cell_pairs = sb.cells.iter().flat_map(|c| { + (0..MAX_CONSTRAINT_PARTICLES).flat_map(move |a| { + (a + 1..MAX_CONSTRAINT_PARTICLES).map(move |b| (c.vertices[a], c.vertices[b])) + }) + }); + let edge_pairs = sb.edges.iter().map(|e| (e.vertices[0], e.vertices[1])); + #[cfg(feature = "dim3")] + let dihedral_pairs = sb.dihedrals.iter().flat_map(|d| { + (0..4).flat_map(move |a| (a + 1..4).map(move |b| (d.vertices[a], d.vertices[b]))) + }); + #[cfg(feature = "dim2")] + let dihedral_pairs = core::iter::empty(); + self.matrix = + BlockMatrix::from_pairs(n, cell_pairs.chain(edge_pairs).chain(dihedral_pairs)); + + self.cell_blocks.clear(); + for c in &sb.cells { + let mut blocks = [u32::MAX; CELL_BLOCKS]; + for a in 0..MAX_CONSTRAINT_PARTICLES { + for b in 0..MAX_CONSTRAINT_PARTICLES { + blocks[a * MAX_CONSTRAINT_PARTICLES + b] = + self.matrix.block_index(c.vertices[a], c.vertices[b]); + } + } + self.cell_blocks.push(blocks); + } + } + + /// Updates the distance elements: `k = ω² / w₀` with the effective mass `w₀` taken at rest + /// reproduces the constraint path's spring exactly. + fn prepare_springs(&mut self, sb: &SoftBody) { + self.springs.clear(); + for e in &sb.edges { + let softness = e.softness.unwrap_or(match e.kind { + SoftBodyEdgeKind::Structural => sb.material.edge_softness, + SoftBodyEdgeKind::Bend => sb.material.bend_softness, + }); + let w = sb.particles[e.vertices[0] as usize].inv_mass + + sb.particles[e.vertices[1] as usize].inv_mass; + let (stiffness, beta) = Self::spring_stiffness(&softness, w); + let blocks = core::array::from_fn(|k| { + self.matrix + .block_index(e.vertices[k / 2], e.vertices[k % 2]) + }); + self.springs.push(FemSpring { + vertices: e.vertices, + blocks, + rest_length: e.rest_length, + stiffness, + beta, + tension_only: e.tension_only, + }); + } + } + + /// Updates the dihedral bending elements. + #[cfg(feature = "dim3")] + fn prepare_dihedrals(&mut self, sb: &SoftBody) { + self.dihedrals.clear(); + let softness = sb.material.bend_softness; + for d in &sb.dihedrals { + let pos: [Vector; 4] = + core::array::from_fn(|k| sb.particles[d.vertices[k] as usize].position); + let mut grad = [Vector::ZERO; 4]; + dihedral_gradients(&pos, d.rest_angle, &mut grad); + let w: Real = (0..4) + .map(|k| sb.particles[d.vertices[k] as usize].inv_mass * grad[k].length_squared()) + .sum(); + let (stiffness, beta) = Self::spring_stiffness(&softness, w); + let blocks = core::array::from_fn(|k| { + self.matrix + .block_index(d.vertices[k / 4], d.vertices[k % 4]) + }); + self.dihedrals.push(FemDihedral { + vertices: d.vertices, + blocks, + rest_angle: d.rest_angle, + stiffness, + beta, + }); + } + } + + /// The stiffness `k = ω²/w` and Rayleigh coefficient `β = 2ζ/ω` of a spring-like element with + /// rest effective mass `w`; a fully pinned element gets no stiffness. + fn spring_stiffness(softness: &SpringCoefficients, w: Real) -> (Real, Real) { + if w <= 0.0 || softness.natural_frequency <= 0.0 { + return (0.0, 0.0); + } + let omega = softness.angular_frequency(); + let stiffness = omega * omega / w; + let beta = if softness.damping_ratio > 0.0 { + 2.0 * softness.damping_ratio / omega + } else { + 0.0 + }; + (stiffness, beta) + } + + /// The Rayleigh coefficient `β = 2ζ / ω` of a cell, `ω` the mean natural frequency of its rest + /// strain modes; a single coefficient per cell keeps the damping matrix `β K` positive + /// semi-definite, as the implicit solve needs. + fn rayleigh_coefficient( + coeffs: &[Vector; MAX_CONSTRAINT_PARTICLES], + inv_mass: &[Real; MAX_CONSTRAINT_PARTICLES], + mu: Real, + lambda: Real, + volume: Real, + zeta: Real, + ) -> Real { + if zeta <= 0.0 { + return 0.0; + } + let block = SoftElasticConstraint::strain_block(coeffs, inv_mass); + let mut sum = 0.0; + let mut count = 0; + for r in 0..STRAIN_ROWS { + let stiffness = Self::rest_stiffness(mu, lambda, r) * volume; + let omega = (stiffness * block[(r, r)]).sqrt(); + if omega > 0.0 { + sum += omega; + count += 1; + } + } + if count == 0 { + return 0.0; + } + 2.0 * zeta * count as Real / sum + } + + /// Rest stiffness of strain row `r` per unit rest volume of the linear-elastic energy + /// `Ψ = μ‖ε‖² + λ/2 tr(ε)²`: `2μ + λ` on the diagonal rows, `4μ` on the shear rows. + #[inline] + fn rest_stiffness(mu: Real, lambda: Real, r: usize) -> Real { + if r < DIM { 2.0 * mu + lambda } else { 4.0 * mu } + } + + /// The linear-elastic Hessian `V₀ ∂²Ψ/∂ε²` in strain-constraint coordinates (constant, the + /// corotational model's tangent). + fn corotational_hessian(mu: Real, lambda: Real) -> StrainMatrix { + let mut h = StrainMatrix::zeros(); + h.fixed_view_mut::(0, 0).fill(lambda); + for r in 0..STRAIN_ROWS { + h[(r, r)] = Self::rest_stiffness(mu, lambda, r); + } + h + } +} diff --git a/src/dynamics/solver/staged_island_solver/init.rs b/src/dynamics/solver/staged_island_solver/init.rs index 07c0637e7..29536d150 100644 --- a/src/dynamics/solver/staged_island_solver/init.rs +++ b/src/dynamics/solver/staged_island_solver/init.rs @@ -163,6 +163,17 @@ impl StagedIslandSolver { ); } + // FEM soft bodies: their systems are updated once the particle slots exist (the + // sparsity pattern and the element tables persist across steps). + #[cfg(feature = "fem")] + { + let groups = &self.groups; + self.soft_fem + .assemble(&self.soft_constraints, soft_bodies, groups.len(), |gi| { + groups[gi].dt + }); + } + // The persistent solver contact graph already holds two-body manifolds grouped by // (color, contact count) — colors touch pairwise-disjoint bodies, buckets slice straight // into SIMD chunks — and multibody manifolds apart; nothing to categorize/sort per step. @@ -564,6 +575,8 @@ impl StagedIslandSolver { joint_constraints: &mut self.joint_constraints as *mut _, contact_constraints: set as *mut _, soft_constraints: &mut self.soft_constraints as *mut _, + #[cfg(feature = "fem")] + soft_fem: &self.soft_fem as *const _, coulomb_builders: set.simd_velocity_coulomb_constraints_builder.as_mut_ptr(), coulomb_constraints: set.simd_velocity_coulomb_constraints.as_mut_ptr(), #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/staged_island_solver/mod.rs b/src/dynamics/solver/staged_island_solver/mod.rs index 0d31d7879..b8cf873b5 100644 --- a/src/dynamics/solver/staged_island_solver/mod.rs +++ b/src/dynamics/solver/staged_island_solver/mod.rs @@ -193,6 +193,10 @@ struct SharedCtx<'a> { /// The soft-body constraints of the awake soft bodies (empty when the step has none: every soft /// stage is then skipped, identically on every worker). soft_constraints: *mut SoftConstraintsSet, + /// The FEM systems of the awake soft bodies that selected the FEM solver (empty when the + /// step has none: every FEM stage is then skipped, identically on every worker). + #[cfg(feature = "fem")] + soft_fem: *const crate::dynamics::solver::soft_fem::SoftFemSet, coulomb_builders: *mut ContactWithCoulombFrictionBuilder, coulomb_constraints: *mut ContactWithCoulombFriction, @@ -225,6 +229,8 @@ pub(crate) struct StagedIslandSolver { pub contact_constraints: ContactConstraintsSet, pub joint_constraints: JointConstraintsSet, pub soft_constraints: SoftConstraintsSet, + #[cfg(feature = "fem")] + pub soft_fem: crate::dynamics::solver::soft_fem::SoftFemSet, pub velocity_solver: VelocitySolver, /// The SIMD chunk layout: segments over the persistent bucket slices (and /// over `overflow_chunk_refs`), in global chunk-id order. @@ -256,11 +262,11 @@ pub(crate) struct StagedIslandSolver { /// Constraint-row range of each SIMD joint chunk. joint_chunk_rows: Vec>, /// Workspace: (row signature, joint index) of a color's SIMD-eligible joints. - joint_sig_scratch: Vec<(u32, JointIndex)>, + joint_sig_workspace: Vec<(u32, JointIndex)>, /// Scalar joints solved by worker 0: joints without a wide row formulation /// (motors, coupled limits), extra-solver-iterations joints, and the joints /// of colors too small to parallelize. - joint_overflow_scratch: Vec, + joint_overflow_workspace: Vec, /// Joint builders from colors too small to parallelize: solved by worker 0. joint_overflow_range: Range, /// Workspace: the overflow-color manifolds handed to the greedy body-mask @@ -305,19 +311,21 @@ impl StagedIslandSolver { contact_constraints: ContactConstraintsSet::new(), joint_constraints: JointConstraintsSet::new(), soft_constraints: SoftConstraintsSet::new(), + #[cfg(feature = "fem")] + soft_fem: crate::dynamics::solver::soft_fem::SoftFemSet::new(), velocity_solver: VelocitySolver::new(), chunk_segments: Vec::new(), overflow_chunk_refs: Vec::new(), color_ranges: Vec::new(), groups: Vec::new(), grouped_chunk_refs: Vec::new(), - overflow_scratch: Vec::new(), + overflow_workspace: Vec::new(), joint_colors: ParallelInteractionGroups::new(), joint_color_ranges: Vec::new(), joint_chunk_lanes: Vec::new(), joint_chunk_rows: Vec::new(), - joint_sig_scratch: Vec::new(), - joint_overflow_scratch: Vec::new(), + joint_sig_workspace: Vec::new(), + joint_overflow_workspace: Vec::new(), joint_overflow_range: 0..0, joint_rows: Vec::new(), staged_joints_valid: false, diff --git a/src/dynamics/solver/staged_island_solver/solve.rs b/src/dynamics/solver/staged_island_solver/solve.rs index b11cce6df..9c128d3db 100644 --- a/src/dynamics/solver/staged_island_solver/solve.rs +++ b/src/dynamics/solver/staged_island_solver/solve.rs @@ -335,6 +335,7 @@ pub(super) unsafe fn solve_pass( } stage = sync.sync(stage, 1); } + stage } diff --git a/src/dynamics/solver/staged_island_solver/worker.rs b/src/dynamics/solver/staged_island_solver/worker.rs index 6146acab2..8f1409f7f 100644 --- a/src/dynamics/solver/staged_island_solver/worker.rs +++ b/src/dynamics/solver/staged_island_solver/worker.rs @@ -205,6 +205,11 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { // case) skip the end-of-step restitution stages entirely. let has_bouncy = unsafe { &*ctx.any_bouncy }.load(Ordering::Relaxed); let has_soft = !unsafe { &*ctx.soft_constraints }.is_empty(); + // Soft bodies solved by the FEM path: their implicit elastic step is one stage per substep, + // and their pass stages live in `solve_pass`. Read-only during the solve, so every worker + // takes the same branches. + #[cfg(feature = "fem")] + let has_fem = !unsafe { &*ctx.soft_fem }.is_empty(); for (group_index, group) in ctx.groups.iter().enumerate() { // This group's substep parameters: identical to `base_params` except // for the substep dt. Everything dt-derived downstream (soft erp/cfm, @@ -296,6 +301,33 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { stage = sync.sync(stage, stage_work); } + /* + * Stage: the FEM soft bodies' implicit elastic step (parallel over the group's FEM + * bodies): assemble `A = M + h D + h² K` and solve `A Δv = h f - (A - M) v`. One body + * per claim; nothing else touches its system or particle slots during this stage. + */ + #[cfg(feature = "fem")] + if has_fem { + let soft_fem = unsafe { &*ctx.soft_fem }; + let fem_bodies = soft_fem.group(group_index); + let stage_work = fem_bodies.len(); + let mut done = 0; + while let Some(claimed) = sync.claim(stage, &fem_bodies, 1, worker_id) { + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + let claimed_len = claimed.len(); + for index in claimed { + // SAFETY: one worker per claimed index; a body writes only its own + // particles (soft bodies share no particle). + unsafe { + soft_fem.predict(index, solver_bodies, ¶ms.soft_bodies.fem); + } + } + done += claimed_len; + } + sync.complete(stage, done, stage_work); + stage = sync.sync(stage, stage_work); + } + /* * Stage: update constraints from builders (parallel; contact chunks first, then * scalar joint builders — each writes only its own constraint slot/rows). @@ -861,6 +893,28 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { stage = sync.sync(stage, stage_work); } + /* + * Stage: write the FEM soft bodies' per-cell state back (the warm-started polar rotation, + * the plastic flow and the tearing marks), parallel over the FEM bodies. + */ + #[cfg(feature = "fem")] + if has_fem { + let set = unsafe { &*ctx.soft_fem }; + let all_bodies = 0..set.num_active(); + let stage_work = all_bodies.len(); + let mut done = 0; + while let Some(claimed) = sync.claim(stage, &all_bodies, 1, worker_id) { + let claimed_len = claimed.len(); + for index in claimed { + // SAFETY: one worker per claimed index; a body writes only its own cells. + unsafe { set.writeback(index, ctx.base_params.dt) }; + } + done += claimed_len; + } + sync.complete(stage, done, stage_work); + stage = sync.sync(stage, stage_work); + } + /* * Stage: write velocities and poses back to the rigid-bodies (parallel over * claimed island bodies; each writes its own rigid-body). The multibody From e41746b55639da09e557f27779b04848a73d10f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 30 Aug 2026 17:15:18 +0200 Subject: [PATCH 04/32] feat(soft-body): add clusters: SoftFrame proxies, per-cluster shape matching, material sugar and impulse joints on clusters --- crates/rapier2d/tests/miri_scenes.rs | 1 + crates/rapier2d/tests/soft_bodies.rs | 146 ++++ crates/rapier3d/tests/miri_scenes.rs | 3 +- crates/rapier3d/tests/soft_bodies.rs | 11 +- crates/rapier3d/tests/soft_body_clusters.rs | 520 +++++++++++++ crates/rapier3d/tests/soft_body_joints.rs | 473 ++++++++++++ .../rapier3d/tests/solver_graph_stale_refs.rs | 1 + .../tests/thread_count_determinism.rs | 1 + examples2d/add_remove2.rs | 1 + examples3d/debug_add_remove_collider3.rs | 1 + examples3d/debug_dynamic_collider_add3.rs | 8 +- examples3d/fountain3.rs | 1 + src/dynamics/island_manager/local_split.rs | 15 + src/dynamics/island_manager/manager.rs | 20 + src/dynamics/island_manager/persistent.rs | 76 +- src/dynamics/island_manager/substep_groups.rs | 5 + .../joint/multibody_joint/multibody.rs | 10 +- src/dynamics/rigid_body.rs | 139 +++- src/dynamics/rigid_body_components.rs | 48 +- src/dynamics/rigid_body_handle.rs | 5 + src/dynamics/rigid_body_set.rs | 14 +- src/dynamics/soft_body/mod.rs | 5 +- src/dynamics/soft_body/soft_body.rs | 6 + .../soft_body_builder/soft_body_build.rs | 1 + src/dynamics/soft_body/soft_body_cluster.rs | 685 ++++++++++++++++++ src/dynamics/soft_body/soft_body_elements.rs | 4 + src/dynamics/soft_body/soft_body_material.rs | 5 + .../soft_body_set/soft_body_set_clusters.rs | 163 +++++ .../soft_body_set_insert_remove.rs | 40 + .../soft_body_set/soft_body_set_proxies.rs | 11 + .../soft_body_set/soft_body_set_step_sync.rs | 2 + .../generic_joint_constraint_builder.rs | 6 +- .../joint_constraint_builder.rs | 14 +- src/dynamics/solver/joint_constraint/mod.rs | 46 ++ .../soft_constraint_prepare.rs | 153 ++++ .../soft_constraint_rigid_coupling.rs | 140 +++- .../soft_constraint_writeback.rs | 15 + .../soft_constraints_set/soft_constraints.rs | 1 + .../soft_constraints_set.rs | 31 + .../soft_element_constraint_assembly.rs | 51 +- .../system/soft_fem_system_prepare.rs | 1 + .../solver/staged_island_solver/init.rs | 17 + .../solver/staged_island_solver/joints.rs | 68 +- .../solver/staged_island_solver/solve.rs | 44 +- .../solver/staged_island_solver/worker.rs | 6 +- src/geometry/collider.rs | 20 +- src/geometry/collider_components.rs | 4 + src/geometry/collider_set.rs | 23 +- .../soft_contacts/soft_contacts_edge_pass.rs | 1 + .../debug_render_pipeline.rs | 4 +- .../debug_render_style.rs | 3 + src/pipeline/physics_pipeline/test.rs | 8 +- src/pipeline/physics_world.rs | 40 +- 53 files changed, 3031 insertions(+), 86 deletions(-) create mode 100644 crates/rapier3d/tests/soft_body_clusters.rs create mode 100644 crates/rapier3d/tests/soft_body_joints.rs create mode 100644 src/dynamics/soft_body/soft_body_cluster.rs create mode 100644 src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs diff --git a/crates/rapier2d/tests/miri_scenes.rs b/crates/rapier2d/tests/miri_scenes.rs index 29b94fe4c..7d859436e 100644 --- a/crates/rapier2d/tests/miri_scenes.rs +++ b/crates/rapier2d/tests/miri_scenes.rs @@ -219,6 +219,7 @@ fn body_removal_midrun() { &mut w.colliders, &mut w.impulse_joints, &mut w.multibody_joints, + &mut w.soft_bodies, true, ); w.run(60); diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index 32c6d9987..2e76ac8c1 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -1540,3 +1540,149 @@ fn fem_box_rests_on_soft_body() { ); assert!(world.bodies[rb].linvel().length() < 0.05); } + +/// 2D twin of `violently_dragged_one_particle_cluster_stays_bounded` (rapier3d's +/// soft_body_joints.rs): the 2D reduced inertia is a scalar whose plain inverse exploded the +/// same way on a one-particle cluster's roundoff inertia. +#[test] +fn violently_dragged_one_particle_cluster_stays_bounded() { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(30.0, 0.5), + ); + let h = world.insert_soft_body( + SoftBodyBuilder::grid(Vector::new(0.0, 1.11), Vector::splat(0.5), 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.8, + ..Default::default() + }) + .particle_mass(0.08), + ); + let corner = Vector::new(0.5, 1.61); + let nearest = (0..world.soft_bodies[h].num_particles()) + .min_by(|&a, &b| { + let body = &world.soft_bodies[h]; + let da = (body.particle_position(a) - corner).length_squared(); + let db = (body.particle_position(b) - corner).length_squared(); + da.partial_cmp(&db).unwrap() + }) + .unwrap() as u32; + let anchor = world.soft_bodies[h].particle_position(nearest as usize); + let cluster = world.add_soft_body_cluster(h, &[nearest]).unwrap(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let mouse_body = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); + let joint: GenericJoint = GenericJointBuilder::new(JointAxesMask::empty()) + .motor_position(JointAxis::LinX, 0.0, 1000.0, 50.0) + .motor_position(JointAxis::LinY, 0.0, 1000.0, 50.0) + .into(); + world.insert_impulse_joint(mouse_body, proxy, joint); + + let mut t: Real = 0.0; + let mut peak: Real = 0.0; + for _ in 0..150 { + t += world.integration_parameters.dt; + let target = anchor + Vector::new(3.0 * (15.0 * t).cos(), 1.5 + 1.5 * (15.0 * t).sin()); + world.bodies[mouse_body].set_next_kinematic_translation(target); + world.bodies[proxy].wake_up(true); + world.step(); + let body = &world.soft_bodies[h]; + let v = (0..body.num_particles()) + .map(|i| body.particle_velocity(i).length()) + .fold(0.0, Real::max); + peak = peak.max(v); + } + assert!(world.quarantine().is_empty()); + assert!(peak < 500.0, "particle speeds diverged: peak {peak}"); +} + +/// With self-contacts a pressurized blob cannot be squeezed through itself into a crossed +/// figure-8 loop: it survives a grab-crush against the floor and re-inflates once released. +#[test] +fn self_contact_blob_survives_grab_crush() { + let area_ratio = |world: &PhysicsWorld, sb: SoftBodyHandle| -> Real { + let body = &world.soft_bodies[sb]; + let mut area = 0.0; + let mut rest = 0.0; + for seg in body.boundary() { + let a = body.particle_position(seg[0] as usize); + let b = body.particle_position(seg[1] as usize); + area += 0.5 * (a.x * b.y - a.y * b.x); + let ra = body.particles()[seg[0] as usize].rest_position(); + let rb = body.particles()[seg[1] as usize].rest_position(); + rest += 0.5 * (ra.x * rb.y - ra.y * rb.x); + } + area / rest + }; + + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(4.0, 0.5), + ); + let mut blobs = Vec::new(); + for j in 0..3 { + for i in 0..3 { + let x = -1.2 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.6 + j as Real * 1.1; + blobs.push( + world.insert_soft_body( + SoftBodyBuilder::disk(Vector::new(x, y), 0.5, 20) + .softness(SpringCoefficients::new(20.0, 1.0)) + .volume_factor(1.05) + .self_contacts(true) + .particle_mass(0.05), + ), + ); + } + } + for _ in 0..240 { + world.step(); + } + + // Grab a bottom-row blob's particle like the testbed mouse does, and park the target far + // underground: the position-error-scaled motor crushes the blob against the floor. + let victim = blobs[1]; + let grabbed = 0u32; + let anchor = world.soft_bodies[victim].particle_position(grabbed as usize); + let cluster = world.add_soft_body_cluster(victim, &[grabbed]).unwrap(); + let proxy = world.soft_bodies[victim].cluster_proxy(cluster).unwrap(); + let mouse = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); + let joint: GenericJoint = GenericJointBuilder::new(JointAxesMask::empty()) + .motor_position(JointAxis::LinX, 0.0, 1000.0, 50.0) + .motor_position(JointAxis::LinY, 0.0, 1000.0, 50.0) + .into(); + world.insert_impulse_joint(mouse, proxy, joint); + for step in 0..900 { + let tt = step as Real / 60.0; + world.bodies[mouse].set_next_kinematic_translation(Vector::new( + anchor.x + 1.5 * (3.0 * tt).sin(), + -8.0, + )); + world.bodies[proxy].wake_up(true); + world.step(); + } + // The crush itself must never cross the loop (the signed area keeps its sign). + assert!( + area_ratio(&world, victim) > 0.0, + "the loop crossed despite self-contacts: ratio {}", + area_ratio(&world, victim) + ); + + world.remove_body(mouse); + world.remove_soft_body_cluster(victim, cluster); + for _ in 0..600 { + world.step(); + } + assert!(world.quarantine().is_empty()); + // Every blob re-inflates: no stuck figure-8, no permanently crushed neighbor. + for &b in &blobs { + let r = area_ratio(&world, b); + assert!(r > 0.6, "a blob stayed crushed after release: ratio {r}"); + } +} diff --git a/crates/rapier3d/tests/miri_scenes.rs b/crates/rapier3d/tests/miri_scenes.rs index e574e5dbd..d08123488 100644 --- a/crates/rapier3d/tests/miri_scenes.rs +++ b/crates/rapier3d/tests/miri_scenes.rs @@ -280,6 +280,7 @@ fn body_removal_midrun() { &mut w.colliders, &mut w.impulse_joints, &mut w.multibody_joints, + &mut w.soft_bodies, true, ); w.run(60); @@ -294,7 +295,7 @@ fn body_removal_midrun() { /// Kinematic solver bodies: a dynamic box riding a velocity-based kinematic platform. #[test] -fn kinematic_platform_carries_box() { +fn kinematic_platform_transports_box() { let mut w = World::new(); let platform = w .bodies diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index c1c1594e1..f37e0605b 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -156,9 +156,16 @@ fn cloth_drapes() { e.rest_length ); } + // The root body is the whole-body cluster's proxy: a soft frame whose pose and mass are + // derived from the particles (its pose tracks the cloth's best-fit frame). let root = &world.bodies[sb.root_body()]; - assert!(root.is_rotation_locked().iter().all(|l| *l)); - assert!(root.is_translation_locked()); + assert!(root.is_soft_frame()); + // The frame's origin is the free particles' centroid (the two pinned corners are left + // out), near but not equal to the full center of mass. + assert!( + (root.position().translation - sb.center_of_mass()).length() < 0.5, + "the root frame does not track the cloth" + ); } /// A hollow sphere with volume preservation dropped on the ground keeps its volume, and a diff --git a/crates/rapier3d/tests/soft_body_clusters.rs b/crates/rapier3d/tests/soft_body_clusters.rs new file mode 100644 index 000000000..49aec3b2c --- /dev/null +++ b/crates/rapier3d/tests/soft_body_clusters.rs @@ -0,0 +1,520 @@ +//! Soft-body clusters and their proxy rigid bodies: lifecycle (add/remove clusters, remove +//! proxies, remove colliders), reference-counted particle ownership, and the island coupling of +//! a soft body's proxies. + +use rapier3d::prelude::*; + +fn world_with_ground() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(50.0, 0.5, 50.0).translation(Vector::new(0.0, -0.5, 0.0)), + ); + world +} + +fn cloth(world: &mut PhysicsWorld) -> SoftBodyHandle { + world.insert_soft_body(SoftBodyBuilder::cloth( + Vector::new(0.0, 2.0, 0.0), + Vector::X * 0.2, + Vector::Z * 0.2, + 6, + 6, + )) +} + +fn rope(world: &mut PhysicsWorld) -> SoftBodyHandle { + world.insert_soft_body(SoftBodyBuilder::rope( + Vector::new(0.0, 2.0, 0.0), + Vector::X * 0.2, + 8, + )) +} + +#[test] +fn root_body_is_the_whole_body_cluster_proxy() { + let mut world = world_with_ground(); + let h = cloth(&mut world); + let sb = &world.soft_bodies[h]; + assert_eq!(sb.num_live_clusters(), 1); + assert_eq!(sb.cluster_proxy(0), Some(sb.root_body())); + assert_eq!( + sb.cluster(0).unwrap().particles().len(), + sb.num_particles() + ); + let rb = &world.bodies[sb.root_body()]; + assert_eq!(rb.body_type(), RigidBodyType::SoftFrame); + assert!(rb.is_soft_frame()); + assert_eq!(rb.soft_body(), Some(h)); + assert_eq!(rb.soft_cluster(), Some(0)); +} + +#[test] +fn soft_frame_setters_are_ignored() { + let mut world = world_with_ground(); + let h = cloth(&mut world); + let root = world.soft_bodies[h].root_body(); + // The proxy's pose is the cluster's derived frame; the setters must not disturb it. + let pose_before = *world.bodies[root].position(); + let vels_before = world.bodies[root].linvel(); + let rb = &mut world.bodies[root]; + rb.set_translation(Vector::new(10.0, 0.0, 0.0), true); + rb.set_linvel(Vector::new(5.0, 0.0, 0.0), true); + rb.set_body_type(RigidBodyType::Fixed, true); + let rb = &world.bodies[root]; + assert_eq!(*rb.position(), pose_before); + assert_eq!(rb.linvel(), vels_before); + assert_eq!(rb.body_type(), RigidBodyType::SoftFrame); + // A regular body cannot be converted to a soft frame either. + let other = world.insert_body(RigidBodyBuilder::dynamic()); + world.bodies[other].set_body_type(RigidBodyType::SoftFrame, true); + assert_eq!(world.bodies[other].body_type(), RigidBodyType::Dynamic); +} + +#[test] +fn removing_the_root_proxy_removes_the_soft_body() { + let mut world = world_with_ground(); + let h = cloth(&mut world); + let root = world.soft_bodies[h].root_body(); + for _ in 0..5 { + world.step(); + } + world.remove_body(root); + // The zombie of old: the soft body must be gone, not left unsimulated. + assert!(world.soft_bodies.get(h).is_none()); + assert_eq!(world.colliders.len(), 1); // The ground. + for _ in 0..5 { + world.step(); + } +} + +#[test] +fn removing_the_root_proxy_of_a_rope_then_stepping_is_clean() { + let mut world = world_with_ground(); + let h = rope(&mut world); + world.step(); + let root = world.soft_bodies[h].root_body(); + world.remove_body(root); + assert!(world.soft_bodies.get(h).is_none()); + assert_eq!(world.colliders.len(), 1); + world.step(); +} + +#[test] +fn removing_a_corner_cluster_is_non_destructive() { + let mut world = world_with_ground(); + let h = cloth(&mut world); + let num_particles = world.soft_bodies[h].num_particles(); + let num_edges = world.soft_bodies[h].edges().len(); + let cluster = world + .soft_bodies + .add_cluster(h, &[0, 1, 6], &mut world.bodies, &mut world.colliders) + .unwrap(); + assert_eq!(world.soft_bodies[h].num_live_clusters(), 2); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + assert_eq!(world.bodies[proxy].soft_cluster(), Some(cluster)); + world.step(); + + // While the whole-body cluster exists, removing a user cluster deletes nothing. + let report = world + .soft_bodies + .remove_cluster( + h, + cluster, + &mut world.islands, + &mut world.bodies, + &mut world.colliders, + &mut world.impulse_joints, + &mut world.multibody_joints, + ) + .unwrap(); + assert_eq!(report.particles, 0); + assert_eq!(report.edges, 0); + assert!(!report.soft_body_removed); + assert!(world.bodies.get(proxy).is_none()); + assert_eq!(world.soft_bodies[h].num_live_clusters(), 1); + assert_eq!(world.soft_bodies[h].num_particles(), num_particles); + assert_eq!(world.soft_bodies[h].edges().len(), num_edges); + world.step(); +} + +#[test] +fn removing_the_whole_body_cluster_keeps_the_corner() { + let mut world = world_with_ground(); + let h = cloth(&mut world); + let num_particles = world.soft_bodies[h].num_particles(); + // A corner cluster over the particles of the first row. + let corner: Vec = (0..6).collect(); + let cluster = world + .soft_bodies + .add_cluster(h, &corner, &mut world.bodies, &mut world.colliders) + .unwrap(); + let corner_proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + world.step(); + + // Removing cluster 0 deletes everything the corner does not cover. + let report = world + .soft_bodies + .remove_cluster( + h, + 0, + &mut world.islands, + &mut world.bodies, + &mut world.colliders, + &mut world.impulse_joints, + &mut world.multibody_joints, + ) + .unwrap(); + assert_eq!(report.particles, num_particles - corner.len()); + assert!(!report.soft_body_removed); + let sb = &world.soft_bodies[h]; + assert_eq!(sb.num_particles(), corner.len()); + assert_eq!(sb.num_live_clusters(), 1); + // The root body re-points to the surviving cluster's proxy. + assert_eq!(sb.root_body(), corner_proxy); + sb.validate_topology().unwrap(); + for _ in 0..10 { + world.step(); + } + // Removing the last cluster removes the body. + let report = world + .soft_bodies + .remove_cluster( + h, + cluster, + &mut world.islands, + &mut world.bodies, + &mut world.colliders, + &mut world.impulse_joints, + &mut world.multibody_joints, + ) + .unwrap(); + assert!(report.soft_body_removed); + assert_eq!(report.particles, corner.len()); + assert!(world.soft_bodies.get(h).is_none()); + world.step(); +} + +#[test] +fn removing_the_surface_collider_clears_the_back_reference() { + let mut world = world_with_ground(); + let h = cloth(&mut world); + world.step(); + let co = world.soft_bodies[h].collision_mesh().unwrap().collider(); + world.remove_collider(co); + assert!(world.soft_bodies[h].collision_mesh().is_none()); + for _ in 0..5 { + world.step(); + } +} + +#[test] +fn cluster_proxies_share_the_island_and_sleep_as_a_unit() { + let mut world = world_with_ground(); + let h = cloth(&mut world); + let cluster = world + .soft_bodies + .add_cluster(h, &[0, 1, 2], &mut world.bodies, &mut world.colliders) + .unwrap(); + let root = world.soft_bodies[h].root_body(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + // Let the cloth fall asleep on the ground. + for _ in 0..2000 { + world.step(); + if world.soft_bodies[h].is_sleeping() { + break; + } + } + assert!(world.soft_bodies[h].is_sleeping(), "cloth never slept"); + assert!(world.bodies[root].is_sleeping()); + assert!(world.bodies[proxy].is_sleeping(), "proxy left awake alone"); + assert_eq!( + world.islands.persistent_island_of(&world.bodies, root), + world.islands.persistent_island_of(&world.bodies, proxy), + ); +} + +#[test] +fn tear_duplicates_join_their_source_clusters() { + let mut world = world_with_ground(); + let h = world.insert_soft_body( + SoftBodyBuilder::cloth( + Vector::new(0.0, 2.0, 0.0), + Vector::X * 0.2, + Vector::Z * 0.2, + 6, + 6, + ) + .tear_strain(0.2), + ); + world.step(); + let before = world.soft_bodies[h].num_particles(); + // Tear a middle edge immediately. + let torn = world.tear_soft_body(h, &[30], &[]); + assert!(torn); + let sb = &world.soft_bodies[h]; + let after = sb.num_particles(); + // Whatever was duplicated must be covered by cluster 0 (refcounts stay consistent). + assert_eq!(sb.cluster(0).unwrap().particles().len(), after); + sb.validate_topology().unwrap(); + let _ = before; + for _ in 0..5 { + world.step(); + } +} + +/// The surface collider's vertices live in the cluster frame: a soft body far from the origin +/// keeps its contact precision (world-sized coordinates never enter the vertex data). +#[test] +fn distant_soft_body_rests_cleanly() { + let far = Vector::new(2000.0, 0.0, 2000.0); + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(50.0, 0.5, 50.0).translation(far + Vector::new(0.0, -0.5, 0.0)), + ); + let h = world.insert_soft_body(SoftBodyBuilder::cloth( + far + Vector::new(0.0, 0.3, 0.0), + Vector::X * 0.2, + Vector::Z * 0.2, + 6, + 6, + )); + for _ in 0..600 { + world.step(); + } + let sb = &world.soft_bodies[h]; + for p in sb.particles() { + assert!(p.position().is_finite()); + assert!( + p.position().y > -0.2 && p.position().y < 0.5, + "particle at odd height: {:?}", + p.position() + ); + } + // The collider's world pose holds the distance; its local frame stays small. + let root = sb.root_body(); + assert!((world.bodies[root].position().translation - far).length() < 5.0); +} + +/// A user collider attached to a cluster proxy rides the cluster's frame, collides with the +/// world, and never collides with its own soft body's surface. +#[test] +fn user_collider_on_a_proxy_rides_the_frame() { + let mut world = world_with_ground(); + let h = world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(0.0, 0.6, 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) + .collider_template(ColliderBuilder::ball(0.05)), + ); + let root = world.soft_bodies[h].root_body(); + // A small bracket embedded in the jelly (overlapping its own surface: the same-soft-body + // filter must keep them from fighting). + let co = world.colliders.insert_with_parent( + ColliderBuilder::cuboid(0.1, 0.1, 0.1).density(0.1), + root, + &mut world.bodies, + ); + for _ in 0..300 { + world.step(); + } + let sb = &world.soft_bodies[h]; + for p in sb.particles() { + assert!(p.position().is_finite()); + } + // The jelly rests on the ground instead of being pushed out by its own bracket. + let com = sb.center_of_mass(); + assert!( + com.y > 0.2 && com.y < 0.8, + "the jelly did not settle (com {com:?})" + ); + // The bracket's collider follows the frame. + let co_pos = world.colliders[co].position().translation; + assert!((co_pos - world.bodies[root].position().translation).length() < 0.3); +} + +/// Per-cluster shape matching: two clusters of one cloth are held rigid independently while the +/// rest of the cloth stays soft. +#[test] +fn per_cluster_shape_matching_holds_two_regions() { + let mut world = world_with_ground(); + let h = world.insert_soft_body( + SoftBodyBuilder::cloth( + Vector::new(0.0, 1.0, 0.0), + Vector::X * 0.2, + Vector::Z * 0.2, + 6, + 6, + ) + .pinned_particles([0, 5]), + ); + // Two disjoint clusters (two triangles of particles), shape-matched. + let ca = world + .soft_bodies + .add_cluster(h, &[14, 15, 20], &mut world.bodies, &mut world.colliders) + .unwrap(); + let cb = world + .soft_bodies + .add_cluster(h, &[27, 28, 33], &mut world.bodies, &mut world.colliders) + .unwrap(); + let rest = |world: &PhysicsWorld, ids: [usize; 3]| -> [Real; 3] { + let sb = &world.soft_bodies[h]; + let d01 = (sb.particle_position(ids[0]) - sb.particle_position(ids[1])).length(); + let d12 = (sb.particle_position(ids[1]) - sb.particle_position(ids[2])).length(); + let d02 = (sb.particle_position(ids[0]) - sb.particle_position(ids[2])).length(); + [d01, d12, d02] + }; + let before_a = rest(&world, [14, 15, 20]); + world.soft_bodies[h].enable_cluster_shape_matching(ca, true); + world.soft_bodies[h].enable_cluster_shape_matching(cb, true); + for _ in 0..240 { + world.step(); + } + let sb = &world.soft_bodies[h]; + for p in sb.particles() { + assert!(p.position().is_finite()); + } + // The shape-matched triangles keep their rest edge lengths within a few percent while the + // cloth as a whole drapes. + let after_a = rest(&world, [14, 15, 20]); + for (b, a) in before_a.iter().zip(after_a.iter()) { + assert!( + (a - b).abs() < 0.15 * b, + "cluster region deformed: {before_a:?} -> {after_a:?}" + ); + } +} + +/// The cluster material sugar: a stiffness-scaled region of a jelly compresses less than the +/// rest under the same load. +#[test] +fn cluster_stiffness_scale_stiffens_a_region() { + let mut world = world_with_ground(); + let h = world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(0.0, 0.5, 0.0), Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e2, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.05) + .collider_template(ColliderBuilder::ball(0.05)), + ); + // A soft reference world with the identical body, unscaled. + let mut soft_world = world_with_ground(); + let h_soft = soft_world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(0.0, 0.5, 0.0), Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e2, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.05) + .collider_template(ColliderBuilder::ball(0.05)), + ); + // Stiffen the whole body through the whole-body cluster: it must sag less than the soft + // reference under its own weight. + world.soft_bodies[h].set_cluster_stiffness_scale(0, 50.0); + for _ in 0..300 { + world.step(); + soft_world.step(); + } + let top = |world: &PhysicsWorld, h: SoftBodyHandle| -> Real { + world.soft_bodies[h] + .particle_positions() + .map(|p| p.y) + .fold(0.0, |a: Real, b| a.max(b)) + }; + let stiff_top = top(&world, h); + let soft_top = top(&soft_world, h_soft); + assert!( + stiff_top > soft_top + 0.02, + "the scaled body did not stiffen: stiff {stiff_top} vs soft {soft_top}" + ); +} + +/// The fixed/kinematic mass-point sugar: pinning a cluster holds its region in place, and a +/// kinematic target moves the whole region rigidly (with the matching velocities), dragging the +/// rest of the body. +#[test] +fn pinned_cluster_drives_its_region_kinematically() { + let mut world = PhysicsWorld::new(); + // A vertical cloth banner; its top edge is the driven cluster. + let h = world.insert_soft_body(SoftBodyBuilder::cloth( + Vector::new(0.0, 2.0, 0.0), + Vector::X * 0.2, + Vector::Y * -0.2, + 6, + 6, + )); + let top_edge: Vec = (0..6).collect(); + let grip = world + .soft_bodies + .add_cluster(h, &top_edge, &mut world.bodies, &mut world.colliders) + .unwrap(); + world.soft_bodies[h].set_cluster_pinned(grip, true); + for &v in &top_edge { + assert!(world.soft_bodies[h].particles()[v as usize].is_pinned()); + } + // Pinned without a target: the region holds while the rest drapes. + let before: Vec = top_edge + .iter() + .map(|&v| world.soft_bodies[h].particle_position(v as usize)) + .collect(); + for _ in 0..120 { + world.step(); + } + for (i, &v) in top_edge.iter().enumerate() { + let now = world.soft_bodies[h].particle_position(v as usize); + assert!( + (now - before[i]).length() < 1.0e-4, + "pinned cluster moved: {:?} -> {now:?}", + before[i] + ); + } + + // Drive the grip sideways: the whole row travels rigidly, the cloth follows. + let grip_proxy = world.soft_bodies[h].cluster_proxy(grip).unwrap(); + let home = world.bodies[grip_proxy].position().translation; + let shift = Vector::new(1.5, 0.0, 0.0); + let steps = 120; + for k in 0..steps { + let s = (k + 1) as Real / steps as Real; + world.soft_bodies[h] + .set_cluster_kinematic_target(grip, Pose::from_translation(home + shift * s)); + world.step(); + } + // The row arrived, rigidly (edge lengths preserved). + let d0 = (world.soft_bodies[h].particle_position(0) + - world.soft_bodies[h].particle_position(5)) + .length(); + assert!( + (d0 - 1.0).abs() < 1.0e-3, + "the driven row deformed (width {d0})" + ); + let mid = world.soft_bodies[h].particle_position(2); + assert!( + (mid.x - (before[2].x + shift.x)).abs() < 0.05, + "the driven row did not reach its target ({mid:?})" + ); + // The free part of the cloth was dragged along. + let bottom = world.soft_bodies[h].particle_position(32); + assert!(bottom.x > 0.7, "the cloth did not follow its grip ({bottom:?})"); + for p in world.soft_bodies[h].particles() { + assert!(p.position().is_finite()); + } +} diff --git a/crates/rapier3d/tests/soft_body_joints.rs b/crates/rapier3d/tests/soft_body_joints.rs new file mode 100644 index 000000000..d9117cf71 --- /dev/null +++ b/crates/rapier3d/tests/soft_body_joints.rs @@ -0,0 +1,473 @@ +//! Impulse joints attached to soft-body clusters through their proxy rigid bodies: soft-rigid, +//! soft-soft and soft-multibody coupling, angular DOFs on full-rank clusters, the angular strip +//! on rank-deficient ones, momentum conservation of the gather/scatter, and wake-up. + +use rapier3d::prelude::*; + +fn world_with_ground() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(50.0, 0.5, 50.0).translation(Vector::new(0.0, -0.5, 0.0)), + ); + world +} + +fn cloth_at(world: &mut PhysicsWorld, origin: Vector) -> SoftBodyHandle { + world.insert_soft_body(SoftBodyBuilder::cloth( + origin, + Vector::X * 0.2, + Vector::Z * 0.2, + 6, + 6, + )) +} + +fn jelly_at(world: &mut PhysicsWorld, center: Vector) -> SoftBodyHandle { + world.insert_soft_body( + SoftBodyBuilder::cuboid(center, Vector::splat(0.4), 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) + .collider_template(ColliderBuilder::ball(0.05)), + ) +} + +/// A one-particle cluster jointed (spherically) to a kinematic mover: the cloth corner follows +/// it, two-way like an attachment but through the ordinary joint pipeline. +#[test] +fn spherical_joint_holds_a_cloth_corner_on_a_moving_body() { + let mut world = world_with_ground(); + let h = cloth_at(&mut world, Vector::new(0.0, 2.0, 0.0)); + let corner_pos = world.soft_bodies[h].particle_position(0); + let cluster = world + .soft_bodies + .add_cluster(h, &[0], &mut world.bodies, &mut world.colliders) + .unwrap(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + // The proxy's frame sits at the cluster's centroid: the corner particle. + assert!((world.bodies[proxy].position().translation - corner_pos).length() < 1.0e-6); + + let mover = world.insert_body( + RigidBodyBuilder::kinematic_velocity_based() + .translation(corner_pos) + .linvel(Vector::new(0.5, 0.0, 0.0)), + ); + let joint = SphericalJointBuilder::new() + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::ZERO); + world.insert_impulse_joint(mover, proxy, joint); + + for _ in 0..120 { + world.step(); + } + let anchor = world.bodies[mover].position().translation; + let corner = world.soft_bodies[h].particle_position(0); + assert!( + (corner - anchor).length() < 0.05, + "corner {corner:?} strayed from its anchor {anchor:?}" + ); + // The mover travelled: the corner really followed a moving target. + assert!(anchor.x > 0.9); + // And the rest of the cloth hangs from it rather than falling. + assert!(world.soft_bodies[h].particle_position(35).y > 0.0); +} + +/// A rope joint on the whole-body cluster limits how far a jelly can fall. +#[test] +fn rope_joint_limits_a_jelly_fall() { + let mut world = PhysicsWorld::new(); + let h = jelly_at(&mut world, Vector::new(0.0, 0.0, 0.0)); + let root = world.soft_bodies[h].root_body(); + let anchor = world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(0.0, 2.0, 0.0))); + let joint = RopeJointBuilder::new(3.0) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::ZERO); + world.insert_impulse_joint(anchor, root, joint); + for _ in 0..300 { + world.step(); + } + let com = world.soft_bodies[h].center_of_mass(); + let dist = (com - Vector::new(0.0, 2.0, 0.0)).length(); + assert!( + dist < 3.3, + "the jelly fell past its rope limit (dist {dist})" + ); + assert!(dist > 2.0, "the jelly did not fall at all (dist {dist})"); +} + +/// An angular motor on a full-rank cluster spins the jelly about the joint axis: the frame's +/// angular velocity is scattered into a real rigid rotation of the particles. +#[test] +fn revolute_motor_spins_a_jelly() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let h = jelly_at(&mut world, Vector::new(0.0, 0.0, 0.0)); + let root = world.soft_bodies[h].root_body(); + let com = world.soft_bodies[h].center_of_mass(); + let anchor = world.insert_body(RigidBodyBuilder::fixed().translation(com)); + let joint = RevoluteJointBuilder::new(Vector::Y) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::ZERO) + .motor_velocity(2.0, 50.0); + world.insert_impulse_joint(anchor, root, joint); + + for _ in 0..120 { + world.step(); + } + // The gathered frame angular velocity approaches the motor target about Y. + let w = world.bodies[root].angvel(); + assert!( + w.y > 1.0, + "the motor failed to spin the jelly (angvel {w:?})" + ); + // And it is a real particle motion, not proxy bookkeeping: a border particle moves. + let speed: Real = world.soft_bodies[h] + .particle_velocities() + .map(|v| v.length()) + .fold(0.0, |a: Real, b| a.max(b)); + assert!(speed > 0.4, "particles do not move (max speed {speed})"); + // The com stays pinned by the revolute joint. + let com2 = world.soft_bodies[h].center_of_mass(); + assert!((com2 - com).length() < 0.1); +} + +/// A position motor drives the jelly's frame to a target angle and holds it there. +#[test] +fn revolute_position_motor_reaches_its_target() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let h = jelly_at(&mut world, Vector::new(0.0, 0.0, 0.0)); + let root = world.soft_bodies[h].root_body(); + let com = world.soft_bodies[h].center_of_mass(); + let anchor = world.insert_body(RigidBodyBuilder::fixed().translation(com)); + let target = 1.0; + let joint = RevoluteJointBuilder::new(Vector::Y) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::ZERO) + .motor_position(target, 100.0, 20.0); + world.insert_impulse_joint(anchor, root, joint); + for _ in 0..400 { + world.step(); + } + // The frame's rotation about Y approaches the target. + let rot = world.bodies[root].position().rotation; + let (axis, angle) = rot.to_axis_angle(); + let signed = angle * axis.y.signum(); + assert!( + (signed - target).abs() < 0.25, + "target {target}, reached {signed} (axis {axis:?}, angle {angle})" + ); +} + +/// A soft-soft spherical joint conserves the linear momentum of the pair (the gather/scatter is +/// momentum-exact and the joint only exchanges impulses between the two clusters). +#[test] +fn soft_soft_joint_conserves_momentum() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let a = jelly_at(&mut world, Vector::new(0.0, 0.0, 0.0)); + let b = jelly_at(&mut world, Vector::new(1.2, 0.0, 0.0)); + let root_a = world.soft_bodies[a].root_body(); + let root_b = world.soft_bodies[b].root_body(); + let joint = SphericalJointBuilder::new() + .local_anchor1(Vector::new(0.6, 0.0, 0.0)) + .local_anchor2(Vector::new(-0.6, 0.0, 0.0)); + world.insert_impulse_joint(root_a, root_b, joint); + + world.soft_bodies[a].apply_impulse(Vector::new(1.5, 0.0, 0.0), true); + let momentum = |world: &PhysicsWorld, h: SoftBodyHandle| -> Vector { + world.soft_bodies[h] + .particles() + .iter() + .map(|p| p.velocity() * p.mass()) + .fold(Vector::ZERO, |a, b| a + b) + }; + let p0 = momentum(&world, a) + momentum(&world, b); + for _ in 0..120 { + world.step(); + } + let p1 = momentum(&world, a) + momentum(&world, b); + assert!( + (p1 - p0).length() < 0.05 * p0.length().max(1.0), + "momentum drifted: {p0:?} -> {p1:?}" + ); + // And the joint dragged the second jelly along. + assert!(momentum(&world, b).x > 0.2); +} + +/// The angular axes of a joint on a rank-deficient (single-particle) cluster are stripped: a +/// fixed joint behaves as a point weld, and the other body keeps spinning freely. +#[test] +fn angular_axes_are_stripped_on_a_point_cluster() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let h = cloth_at(&mut world, Vector::new(0.0, 2.0, 0.0)); + let cluster = world + .soft_bodies + .add_cluster(h, &[0], &mut world.bodies, &mut world.colliders) + .unwrap(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let corner = world.soft_bodies[h].particle_position(0); + // No collider: the spinner would otherwise be expelled by the cloth's own surface + // contacts, which is contact behavior, not the joint's. + let spinner = world.insert_body( + RigidBodyBuilder::dynamic() + .translation(corner) + .angvel(Vector::new(0.0, 3.0, 0.0)) + .additional_mass_properties(MassProperties::from_ball(2.0, 0.05)), + ); + let joint = FixedJointBuilder::new() + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::ZERO); + world.insert_impulse_joint(spinner, proxy, joint); + for _ in 0..60 { + world.step(); + } + // The spinner's rotation must remain free: a zero-inertia particle cannot define an + // orientation, and the strip keeps the joint from freezing the spinner instead. + let w = world.bodies[spinner].angvel(); + assert!( + w.y > 2.5, + "the fixed joint froze the spinner's rotation (angvel {w:?})" + ); + // While its position stays welded to the corner. + let d = (world.bodies[spinner].position().translation + - world.soft_bodies[h].particle_position(0)) + .length(); + assert!(d < 0.08, "the point weld drifted ({d})"); +} + +/// Inserting a joint to a sleeping soft body wakes it, and the joint couples their islands. +#[test] +fn joint_insertion_wakes_the_soft_body() { + let mut world = world_with_ground(); + let h = cloth_at(&mut world, Vector::new(0.0, 0.05, 0.0)); + for _ in 0..2000 { + world.step(); + if world.soft_bodies[h].is_sleeping() { + break; + } + } + assert!(world.soft_bodies[h].is_sleeping(), "cloth never slept"); + let root = world.soft_bodies[h].root_body(); + let (body, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(3.0, 3.0, 0.0)), + ColliderBuilder::ball(0.2), + ); + let joint = RopeJointBuilder::new(4.0); + world.insert_impulse_joint(body, root, joint); + world.step(); + assert!(!world.soft_bodies[h].is_sleeping()); + assert!(!world.bodies[body].is_sleeping()); +} + +/// A soft cluster jointed to a multibody link: the joint goes through the generic (jacobian) +/// path and still holds. +#[test] +fn soft_cluster_jointed_to_a_multibody() { + let mut world = PhysicsWorld::new(); + // A two-link multibody arm hanging from a fixed root. + let root = world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(0.0, 3.0, 0.0))); + let (link1, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(1.0, 3.0, 0.0)), + ColliderBuilder::cuboid(0.4, 0.1, 0.1).density(1.0), + ); + let j1 = RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::new(-1.0, 0.0, 0.0)); + world + .multibody_joints + .insert(root, link1, j1, true) + .unwrap(); + + let h = jelly_at(&mut world, Vector::new(2.0, 3.0, 0.0)); + let sb_root = world.soft_bodies[h].root_body(); + let joint = SphericalJointBuilder::new() + .local_anchor1(Vector::new(0.5, 0.0, 0.0)) + .local_anchor2(Vector::new(-0.5, 0.0, 0.0)); + world.insert_impulse_joint(link1, sb_root, joint); + + for _ in 0..240 { + world.step(); + } + // The jelly hangs from the arm instead of free-falling. + let com = world.soft_bodies[h].center_of_mass(); + assert!( + com.y > 0.5, + "the jelly fell as if unjointed (com {com:?})" + ); + assert!(com.is_finite()); +} + +/// The joint pipeline works on a FEM-solved body too: `SoftFemSystem::propagate` answers the +/// scattered joint impulse with the whole body's elasticity. +#[cfg(feature = "fem")] +#[test] +fn joint_on_a_fem_body_holds() { + let mut world = PhysicsWorld::new(); + let h = world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(0.0, 0.0, 0.0), Vector::splat(0.4), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .solver(SoftBodySolver::Fem) + .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) + .collider_template(ColliderBuilder::ball(0.05)), + ); + let root = world.soft_bodies[h].root_body(); + let anchor = world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(0.0, 2.0, 0.0))); + let joint = RopeJointBuilder::new(3.0) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::ZERO); + world.insert_impulse_joint(anchor, root, joint); + for _ in 0..300 { + world.step(); + } + let com = world.soft_bodies[h].center_of_mass(); + assert!(com.is_finite()); + let dist = (com - Vector::new(0.0, 2.0, 0.0)).length(); + assert!(dist < 3.3, "the FEM jelly fell past its rope (dist {dist})"); +} + +/// A NaN injected into a jointed soft body lands in the quarantine: the body is disabled, the +/// joint partner survives, the world keeps stepping. +#[test] +fn quarantine_contains_a_nan_on_a_jointed_cluster() { + let mut world = world_with_ground(); + let h = cloth_at(&mut world, Vector::new(0.0, 1.0, 0.0)); + let root = world.soft_bodies[h].root_body(); + let (partner, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(1.0, 1.0, 0.0)), + ColliderBuilder::ball(0.1), + ); + world.insert_impulse_joint(partner, root, RopeJointBuilder::new(2.0)); + for _ in 0..10 { + world.step(); + } + world.soft_bodies[h].set_particle_velocity(0, Vector::NAN); + for _ in 0..10 { + world.step(); + } + assert!(!world.soft_bodies[h].is_enabled(), "the NaN body must be quarantined"); + assert!(world.bodies[partner].position().translation.is_finite()); +} + +/// Tearing a jointed cloth keeps the joint anchored to its (possibly renumbered) cluster: the +/// duplicated particles join their source's clusters and the proxy stays live. +#[test] +fn tearing_keeps_cluster_joints_alive() { + let mut world = world_with_ground(); + let h = world.insert_soft_body( + SoftBodyBuilder::cloth( + Vector::new(0.0, 2.0, 0.0), + Vector::X * 0.2, + Vector::Z * 0.2, + 6, + 6, + ) + .tear_strain(0.2), + ); + let cluster = world + .soft_bodies + .add_cluster(h, &[0, 1, 6, 7], &mut world.bodies, &mut world.colliders) + .unwrap(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let anchor = world.insert_body( + RigidBodyBuilder::fixed().translation(world.bodies[proxy].position().translation), + ); + world.insert_impulse_joint(anchor, proxy, SphericalJointBuilder::new()); + world.step(); + // Tear a couple of middle edges. + let torn = world.tear_soft_body(h, &[28, 30], &[]); + assert!(torn); + world.soft_bodies[h].validate_topology().unwrap(); + for _ in 0..60 { + world.step(); + } + // The jointed corner still hangs near its anchor while the cloth drapes. + let d = (world.bodies[proxy].position().translation + - world.bodies[anchor].position().translation) + .length(); + assert!(d < 0.3, "the jointed cluster strayed after the tear ({d})"); + for p in world.soft_bodies[h].particles() { + assert!(p.position().is_finite()); + } +} + +/// A violently dragged mouse-joint grab of a one-particle cluster keeps bounded particle speeds +/// (the cluster's roundoff inertia is pseudo-inverted with an absolute noise floor). +#[test] +fn violently_dragged_one_particle_cluster_stays_bounded() { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(18.0, 0.1, 18.0), + ); + let h = world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(0.0, 0.61, 0.0), Vector::splat(0.5), 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.8, + ..Default::default() + }) + .particle_mass(0.08) + .particle_radius(0.06) + .collider_template(ColliderBuilder::ball(0.06)), + ); + // Grab the top corner particle, like the testbed's mouse grab does. + let corner = Vector::new(0.5, 1.1, 0.5); + let nearest = (0..world.soft_bodies[h].num_particles()) + .min_by(|&a, &b| { + let body = &world.soft_bodies[h]; + let da = (body.particle_position(a) - corner).length_squared(); + let db = (body.particle_position(b) - corner).length_squared(); + da.partial_cmp(&db).unwrap() + }) + .unwrap() as u32; + let anchor = world.soft_bodies[h].particle_position(nearest as usize); + let cluster = world.add_soft_body_cluster(h, &[nearest]).unwrap(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let mouse_body = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); + let joint: GenericJoint = GenericJointBuilder::new(JointAxesMask::empty()) + .motor_position(JointAxis::LinX, 0.0, 1000.0, 50.0) + .motor_position(JointAxis::LinY, 0.0, 1000.0, 50.0) + .motor_position(JointAxis::LinZ, 0.0, 1000.0, 50.0) + .into(); + world.insert_impulse_joint(mouse_body, proxy, joint); + + // A violent wide circular wave (~45 length-units/s of hand speed). + let mut t: Real = 0.0; + let mut peak: Real = 0.0; + for _ in 0..150 { + t += world.integration_parameters.dt; + let target = + anchor + Vector::new(3.0 * (15.0 * t).cos(), 1.5 + 1.5 * (15.0 * t).sin(), 0.0); + world.bodies[mouse_body].set_next_kinematic_translation(target); + world.bodies[proxy].wake_up(true); + world.step(); + let body = &world.soft_bodies[h]; + let v = (0..body.num_particles()) + .map(|i| body.particle_velocity(i).length()) + .fold(0.0, Real::max); + peak = peak.max(v); + } + assert!(world.quarantine().is_empty()); + // The drag itself moves at ~45; anything far past the joint's following speed is the + // roundoff-inertia divergence (it reached 1e13 before the fix). + assert!(peak < 500.0, "particle speeds diverged: peak {peak}"); +} diff --git a/crates/rapier3d/tests/solver_graph_stale_refs.rs b/crates/rapier3d/tests/solver_graph_stale_refs.rs index f81294847..8f884a05b 100644 --- a/crates/rapier3d/tests/solver_graph_stale_refs.rs +++ b/crates/rapier3d/tests/solver_graph_stale_refs.rs @@ -71,6 +71,7 @@ fn body_churn_keeps_solver_graph_exact() { &mut world.colliders, &mut world.impulse_joints, &mut world.multibody_joints, + &mut world.soft_bodies, true, ); } diff --git a/crates/rapier3d/tests/thread_count_determinism.rs b/crates/rapier3d/tests/thread_count_determinism.rs index 82b1d3117..16812a704 100644 --- a/crates/rapier3d/tests/thread_count_determinism.rs +++ b/crates/rapier3d/tests/thread_count_determinism.rs @@ -228,6 +228,7 @@ fn run_sim(num_threads: usize, num_steps: usize) -> Vec { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, true, ); } diff --git a/examples2d/add_remove2.rs b/examples2d/add_remove2.rs index 57041b8b6..c73ba132c 100644 --- a/examples2d/add_remove2.rs +++ b/examples2d/add_remove2.rs @@ -62,6 +62,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { &mut world.colliders, &mut world.impulse_joints, &mut world.multibody_joints, + &mut world.soft_bodies, true, ); diff --git a/examples3d/debug_add_remove_collider3.rs b/examples3d/debug_add_remove_collider3.rs index 339cf4656..d2c13811a 100644 --- a/examples3d/debug_add_remove_collider3.rs +++ b/examples3d/debug_add_remove_collider3.rs @@ -43,6 +43,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { removed_collider_handle, &mut world.islands, &mut world.bodies, + &mut world.soft_bodies, true, ) .unwrap(); diff --git a/examples3d/debug_dynamic_collider_add3.rs b/examples3d/debug_dynamic_collider_add3.rs index c9138ad0a..b7f38abca 100644 --- a/examples3d/debug_dynamic_collider_add3.rs +++ b/examples3d/debug_dynamic_collider_add3.rs @@ -80,7 +80,13 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { world .colliders - .remove(*handle, &mut world.islands, &mut world.bodies, true); + .remove( + *handle, + &mut world.islands, + &mut world.bodies, + &mut world.soft_bodies, + true, + ); } extra_colliders.clear(); diff --git a/examples3d/fountain3.rs b/examples3d/fountain3.rs index 44b26bb9b..1ef6b879f 100644 --- a/examples3d/fountain3.rs +++ b/examples3d/fountain3.rs @@ -70,6 +70,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { &mut world.colliders, &mut world.impulse_joints, &mut world.multibody_joints, + &mut world.soft_bodies, true, ); viewer.remove_body(*handle); diff --git a/src/dynamics/island_manager/local_split.rs b/src/dynamics/island_manager/local_split.rs index 10c1209e1..af0ef2c4e 100644 --- a/src/dynamics/island_manager/local_split.rs +++ b/src/dynamics/island_manager/local_split.rs @@ -118,6 +118,11 @@ impl IslandGraph<'_> { for attachment in sb.particle_attachments() { visit(attachment.body); } + for (_, cluster) in sb.live_clusters() { + if cluster.proxy() != body { + visit(cluster.proxy()); + } + } } for (sb_handle, _) in self.soft_bodies.attached_to(body) { if let Some(sb) = self.soft_bodies.get(*sb_handle) { @@ -175,6 +180,16 @@ impl IslandGraph<'_> { ordinal, })); } + let members = sb + .live_clusters() + .filter(|(_, c)| self.is_member(c.proxy())) + .count(); + for ordinal in 0..members.saturating_sub(1) as u32 { + f(IncidentLink::Joint(JointLinkKey::SoftProxyChain { + soft_body, + ordinal, + })); + } } } for (sb_handle, ordinal) in self.soft_bodies.attached_to(body) { diff --git a/src/dynamics/island_manager/manager.rs b/src/dynamics/island_manager/manager.rs index 7abcbbbc1..afa81e262 100644 --- a/src/dynamics/island_manager/manager.rs +++ b/src/dynamics/island_manager/manager.rs @@ -209,9 +209,29 @@ impl IslandManager { for attachment in sb.particle_attachments() { self.wake_for_link(bodies, root, attachment.body); } + let mut any_awake = false; + let mut sleeping = Vec::new(); + for (_, cluster) in sb.live_clusters() { + if let Some(rb) = bodies.get(cluster.proxy()) { + if rb.is_enabled() { + if rb.activation().sleeping { + sleeping.push(cluster.proxy()); + } else { + any_awake = true; + } + } + } + } + if any_awake { + for proxy in sleeping { + self.wake_up(bodies, proxy, true); + } + } } self.persistent .update_soft_body_attachments(bodies, soft_bodies, handle); + self.persistent + .update_soft_body_proxy_chain(bodies, soft_bodies, handle); } /// Wakes the sleeping side of a new link when the other side is awake. diff --git a/src/dynamics/island_manager/persistent.rs b/src/dynamics/island_manager/persistent.rs index f392faa9d..31f41cb4d 100644 --- a/src/dynamics/island_manager/persistent.rs +++ b/src/dynamics/island_manager/persistent.rs @@ -47,6 +47,10 @@ pub(crate) enum JointLinkKey { /// generation): a link between the soft body's root body and the rigid body the particle is /// attached to, rebuilt whenever the soft body's attachments change. SoftAttachment { soft_body: u64, ordinal: u32 }, + /// The `ordinal`-th link of the internal connectivity chain of the soft body at `soft_body`'s + /// cluster proxies. A soft body sleeps and wakes as a unit, so its proxies are chained in + /// cluster order, rebuilt whenever a cluster is added or removed. + SoftProxyChain { soft_body: u64, ordinal: u32 }, } /// Packs a multibody arena index (index + generation) into a stable key. @@ -134,6 +138,9 @@ fn serialize_joint_link_locs( JointLinkKey::SoftAttachment { soft_body, ordinal } => { (2u8, *soft_body, *ordinal as u64) } + JointLinkKey::SoftProxyChain { soft_body, ordinal } => { + (3u8, *soft_body, *ordinal as u64) + } }, s, ) @@ -159,16 +166,16 @@ pub(crate) struct PersistentIslands { pub(super) joint_link_locs: HashMap, /// The edges unlinked since the last [`Self::resolve_removals`]. pub(super) removal_journal: Vec, - /// Scratch for the local split search. + /// Workspace for the local split search. #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(super) local_split: super::local_split::LocalSplitScratch, + pub(super) local_split: super::local_split::LocalSplitWorkspace, /// Split candidate chosen last step (the sleepiest island that lost /// constraints), consumed by [`Self::run_pending_split`] this step. pub(super) split_island: Option, - /// Union-find & counting scratch for the split. + /// Union-find & counting workspace for the split. #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(super) split_scratch: SplitScratch, - /// Per-step sleep-scan scratch, indexed by island id: `(stamp, all_bodies_eligible_so_far)`. + pub(super) split_workspace: SplitWorkspace, + /// Per-step sleep-scan workspace, indexed by island id: `(stamp, all_bodies_eligible_so_far)`. /// Stamped so the scan is O(active bodies), never O(total islands) — sleeping islands are /// never touched. #[cfg_attr(feature = "serde-serialize", serde(skip))] @@ -638,6 +645,64 @@ impl PersistentIslands { } } + /// Updates a soft body's proxy chain: unlinks the old one, then (if the soft body still + /// exists) chains its live cluster proxies in cluster order. A soft body's proxies must + /// share one island (its sleep unit); the whole-body proxy alone needs no link. + pub fn update_soft_body_proxy_chain( + &mut self, + bodies: &mut RigidBodySet, + soft_bodies: &SoftBodySet, + handle: SoftBodyHandle, + ) { + self.unlink_soft_body_proxy_chain(handle); + + let Some(sb) = soft_bodies.get(handle) else { + return; + }; + let raw = soft_body_key(handle); + let mut prev: Option = None; + let mut ordinal = 0; + for (_, cluster) in sb.live_clusters() { + let proxy = cluster.proxy(); + let is_member = bodies + .get(proxy) + .is_some_and(|rb| !rb.is_fixed() && rb.is_enabled()); + if !is_member { + continue; + } + if let Some(prev) = prev { + self.link_joint( + bodies, + JointLinkKey::SoftProxyChain { + soft_body: raw, + ordinal, + }, + prev, + proxy, + ); + ordinal += 1; + } + prev = Some(proxy); + } + } + + /// Unlinks a soft body's proxy-chain links (ordinals are dense from 0). + pub fn unlink_soft_body_proxy_chain(&mut self, handle: SoftBodyHandle) { + let raw = soft_body_key(handle); + let mut ordinal = 0; + loop { + let key = JointLinkKey::SoftProxyChain { + soft_body: raw, + ordinal, + }; + if !self.joint_link_locs.contains_key(&key) { + break; + } + self.unlink_joint(key); + ordinal += 1; + } + } + /// Unlinks a soft body's attachment links (ordinals are dense from 0). pub fn unlink_soft_body_attachments(&mut self, handle: SoftBodyHandle) { let raw = soft_body_key(handle); @@ -710,6 +775,7 @@ impl PersistentIslands { let sb_handles: Vec = soft_bodies.iter().map(|(h, _)| h).collect(); for handle in sb_handles { self.update_soft_body_attachments(bodies, soft_bodies, handle); + self.update_soft_body_proxy_chain(bodies, soft_bodies, handle); } // Merging propagated `sleeping &=`, so an island is `sleeping` iff diff --git a/src/dynamics/island_manager/substep_groups.rs b/src/dynamics/island_manager/substep_groups.rs index 15bd76e00..449111264 100644 --- a/src/dynamics/island_manager/substep_groups.rs +++ b/src/dynamics/island_manager/substep_groups.rs @@ -125,6 +125,11 @@ impl IslandManager { ws.uf.union(root, s); } } + for (_, cluster) in sb.live_clusters() { + if let Some(s) = slot(cluster.proxy()) { + ws.uf.union(root, s); + } + } } // Effective counts per body = max of each over its component; non-dynamic awake bodies diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 5ce4ddad0..53f36807d 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1639,11 +1639,11 @@ impl Multibody { let jb2 = &self.body_jacobians[link2.internal_id]; // Use the (overwritten below) W·J slot as workspace for J1ᵀ·f1. - let (mut out_j, mut scratch) = + let (mut out_j, mut workspace) = jacobians.rows_range_pair_mut(*j_id..*j_id + self.ndofs, wj_id..wj_id + self.ndofs); jb2.tr_mul_to(force2.as_vector(), &mut out_j); - jb1.tr_mul_to(force1.as_vector(), &mut scratch); - out_j.axpy(-1.0, &scratch, 1.0); + jb1.tr_mul_to(force1.as_vector(), &mut workspace); + out_j.axpy(-1.0, &workspace, 1.0); // Cancellation guard: the reference scale is the magnitude of the dot-product operands, // not their results (which may be pure cancellation noise when the direction isn’t @@ -1811,7 +1811,7 @@ mod test { use crate::math::{Real, SPATIAL_DIM}; use crate::prelude::{ ColliderSet, MultibodyJointHandle, MultibodyJointSet, RevoluteJoint, RigidBodyBuilder, - RigidBodySet, + RigidBodySet, SoftBodySet }; use na::{DVector, RowDVector}; @@ -1898,6 +1898,7 @@ mod test { let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut islands = IslandManager::new(); + let mut soft_bodies = SoftBodySet::new(); let num_links = 100; let mut handles = vec![]; @@ -1939,6 +1940,7 @@ mod test { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, true, ); } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 2482c8638..ccb693d7a 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -71,10 +71,22 @@ pub struct RigidBody { serde(default = "crate::dynamics::SoftBodyHandle::invalid") )] pub(crate) soft_body: crate::dynamics::SoftBodyHandle, + /// The cluster this soft-frame proxy stands for, in its soft body's cluster list + /// (`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). + pub(crate) soft_motion_margin: Real, /// User-defined data associated to this rigid-body. pub user_data: u128, } +#[cfg(feature = "serde-serialize")] +fn invalid_soft_cluster() -> u32 { + u32::MAX +} + impl Default for RigidBody { fn default() -> Self { Self::new() @@ -101,6 +113,8 @@ impl RigidBody { user_data: 0, additional_solver_iterations: 0, soft_body: crate::dynamics::SoftBodyHandle::invalid(), + soft_cluster: u32::MAX, + soft_motion_margin: 0.0, } } @@ -108,6 +122,7 @@ impl RigidBody { self.colliders.0 = Vec::new(); self.ids = Default::default(); self.soft_body = crate::dynamics::SoftBodyHandle::invalid(); + self.soft_cluster = u32::MAX; } /// Copy all the characteristics from `other` to `self`. @@ -145,7 +160,9 @@ impl RigidBody { dominance, enabled, additional_solver_iterations, - soft_body: _soft_body, // The soft-body marker belongs to the root body, not to its state. + soft_body: _soft_body, // The soft-body markers belong to the proxy, not to its state. + soft_cluster: _soft_cluster, + soft_motion_margin: _soft_motion_margin, // Engine-managed, like the markers. user_data, } = other; @@ -181,6 +198,18 @@ impl RigidBody { (!self.soft_body.is_invalid()).then_some(self.soft_body) } + /// The index of the cluster this soft-frame proxy stands for in its soft body's cluster + /// list, if any (see [`Self::soft_body`]). + pub fn soft_cluster(&self) -> Option { + (self.soft_cluster != u32::MAX).then_some(self.soft_cluster) + } + + /// Whether this rigid body is the proxy of a soft-body cluster + /// ([`RigidBodyType::SoftFrame`]). + pub fn is_soft_frame(&self) -> bool { + self.body_type.is_soft_frame() + } + /// Set the additional number of solver substeps run for the simulation island containing this /// rigid-body (default: 0). Each extra substep re-derives the soft constraint bias at a smaller /// timestep, improving accuracy for stiff couplings (joint chains, high mass-ratio stacks). The @@ -266,6 +295,11 @@ impl RigidBody { /// Sets the type of this rigid-body. pub fn set_body_type(&mut self, status: RigidBodyType, wake_up: bool) { + if self.is_soft_frame() || status.is_soft_frame() { + // Soft-frame proxies are created and removed with their cluster only: a body + // cannot be turned into one, and a proxy cannot change kind. + return; + } if status != self.body_type { self.changes.insert(RigidBodyChanges::TYPE); self.body_type = status; @@ -319,6 +353,11 @@ impl RigidBody { /// Sets the axes along which this rigid-body cannot translate or rotate. #[inline] pub fn set_locked_axes(&mut self, locked_axes: LockedAxes, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if locked_axes != self.mprops.flags { if self.is_dynamic_or_kinematic() && wake_up { self.wake_up(true); @@ -341,6 +380,11 @@ impl RigidBody { /// Use for characters that shouldn't tip over, or objects that should only slide. #[inline] pub fn lock_rotations(&mut self, locked: bool, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if locked != self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED) { if self.is_dynamic_or_kinematic() && wake_up { self.wake_up(true); @@ -362,6 +406,11 @@ impl RigidBody { allow_rotations_z: bool, wake_up: bool, ) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) == allow_rotations_x || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) == allow_rotations_y || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) == allow_rotations_z @@ -406,6 +455,11 @@ impl RigidBody { /// Use for rotating platforms, turrets, or objects fixed in space. #[inline] pub fn lock_translations(&mut self, locked: bool, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if locked != self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED) { if self.is_dynamic_or_kinematic() && wake_up { self.wake_up(true); @@ -427,6 +481,11 @@ impl RigidBody { #[cfg(feature = "dim3")] allow_translation_z: bool, wake_up: bool, ) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } #[cfg(feature = "dim2")] if self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) != allow_translation_x && self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) != allow_translation_y @@ -605,6 +664,11 @@ impl RigidBody { /// Updated automatically at next physics step or call `recompute_mass_properties_from_colliders()`. #[inline] pub fn set_additional_mass(&mut self, additional_mass: Real, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } self.do_set_additional_mass_properties( RigidBodyAdditionalMassProps::Mass(additional_mass), wake_up, @@ -629,6 +693,11 @@ impl RigidBody { /// put to sleep because it did not move for a while. #[inline] pub fn set_additional_mass_properties(&mut self, props: MassProperties, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } self.do_set_additional_mass_properties( RigidBodyAdditionalMassProps::MassProps(props), wake_up, @@ -676,8 +745,9 @@ impl RigidBody { /// Checks if this is a dynamic body (moves via forces and collisions). /// /// Dynamic bodies are fully simulated and respond to gravity, forces, and collisions. + /// Soft-frame proxies ([`RigidBodyType::SoftFrame`]) count as dynamic. pub fn is_dynamic(&self) -> bool { - self.body_type == RigidBodyType::Dynamic + self.body_type.is_dynamic() } /// Checks if this is a kinematic body (moves via direct velocity/position control). @@ -754,6 +824,11 @@ impl RigidBody { /// bodies[body].set_gravity_scale(2.0, true); // Extra heavy /// ``` pub fn set_gravity_scale(&mut self, scale: Real, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if self.forces.gravity_scale != scale { if wake_up && self.activation.sleeping { self.changes.insert(RigidBodyChanges::SLEEP); @@ -919,6 +994,11 @@ impl RigidBody { /// bodies[body].set_linvel(Vector::new(5.0, 0.0, 0.0), true); /// ``` pub fn set_linvel(&mut self, linvel: Vector, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if self.vels.linvel != linvel { match self.body_type { RigidBodyType::Dynamic | RigidBodyType::KinematicVelocityBased => { @@ -927,7 +1007,9 @@ impl RigidBody { self.wake_up(true) } } - RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {} + RigidBodyType::Fixed + | RigidBodyType::KinematicPositionBased + | RigidBodyType::SoftFrame => {} } } } @@ -938,6 +1020,11 @@ impl RigidBody { /// put to sleep because it did not move for a while. #[cfg(feature = "dim2")] pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if self.vels.angvel != angvel { match self.body_type { RigidBodyType::Dynamic | RigidBodyType::KinematicVelocityBased => { @@ -946,7 +1033,9 @@ impl RigidBody { self.wake_up(true) } } - RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {} + RigidBodyType::Fixed + | RigidBodyType::KinematicPositionBased + | RigidBodyType::SoftFrame => {} } } } @@ -957,6 +1046,11 @@ impl RigidBody { /// put to sleep because it did not move for a while. #[cfg(feature = "dim3")] pub fn set_angvel(&mut self, angvel: AngVector, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if self.vels.angvel != angvel { match self.body_type { RigidBodyType::Dynamic | RigidBodyType::KinematicVelocityBased => { @@ -965,7 +1059,9 @@ impl RigidBody { self.wake_up(true) } } - RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {} + RigidBodyType::Fixed + | RigidBodyType::KinematicPositionBased + | RigidBodyType::SoftFrame => {} } } } @@ -1002,6 +1098,11 @@ impl RigidBody { /// * `wake_up` - If `true`, prevents the body from immediately going back to sleep #[inline] pub fn set_translation(&mut self, translation: Vector, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if self.pos.position.translation != translation || self.pos.next_position.translation != translation { @@ -1030,6 +1131,11 @@ impl RigidBody { /// ⚠️ **Warning**: This teleports the rotation, ignoring physics! See [`set_translation()`](Self::set_translation) for details. #[inline] pub fn set_rotation(&mut self, rotation: Rotation, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if self.pos.position.rotation != rotation || self.pos.next_position.rotation != rotation { self.changes.insert(RigidBodyChanges::POSITION); self.pos.position.rotation = rotation; @@ -1052,6 +1158,11 @@ impl RigidBody { /// /// Use for respawning, level transitions, or resetting positions. pub fn set_position(&mut self, pos: Pose, wake_up: bool) { + if self.is_soft_frame() { + // A soft-frame proxy's state is derived from its cluster's particles: this + // setter is ignored for it. + return; + } if self.pos.position != pos || self.pos.next_position != pos { self.changes.insert(RigidBodyChanges::POSITION); self.pos.position = pos; @@ -1223,7 +1334,7 @@ impl RigidBody { /// /// Only affects dynamic bodies (does nothing for kinematic/fixed bodies). pub fn add_force(&mut self, force: Vector, wake_up: bool) { - if force != Vector::ZERO && self.body_type == RigidBodyType::Dynamic { + if force != Vector::ZERO && self.body_type.is_dynamic() { self.forces.user_force += force; if wake_up { @@ -1241,7 +1352,7 @@ impl RigidBody { /// Only affects dynamic bodies. #[cfg(feature = "dim2")] pub fn add_torque(&mut self, torque: Real, wake_up: bool) { - if !torque.is_zero() && self.body_type == RigidBodyType::Dynamic { + if !torque.is_zero() && self.body_type.is_dynamic() { self.forces.user_torque += torque; if wake_up { @@ -1258,7 +1369,7 @@ impl RigidBody { /// Only affects dynamic bodies. #[cfg(feature = "dim3")] pub fn add_torque(&mut self, torque: Vector, wake_up: bool) { - if torque != Vector::ZERO && self.body_type == RigidBodyType::Dynamic { + if torque != Vector::ZERO && self.body_type.is_dynamic() { self.forces.user_torque += torque; if wake_up { @@ -1280,7 +1391,7 @@ impl RigidBody { /// /// Only affects dynamic bodies. pub fn add_force_at_point(&mut self, force: Vector, point: Vector, wake_up: bool) { - if force != Vector::ZERO && self.body_type == RigidBodyType::Dynamic { + if force != Vector::ZERO && self.body_type.is_dynamic() { self.forces.user_force += force; self.forces.user_torque += (point - self.mprops.world_com).gcross(force); @@ -1318,7 +1429,7 @@ impl RigidBody { /// Only affects dynamic bodies (does nothing for kinematic/fixed bodies). #[profiling::function] pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { - if impulse != Vector::ZERO && self.body_type == RigidBodyType::Dynamic { + if impulse != Vector::ZERO && self.body_type.is_dynamic() { self.vels.linvel += impulse * self.mprops.effective_inv_mass; if wake_up { @@ -1333,7 +1444,7 @@ impl RigidBody { #[cfg(feature = "dim2")] #[profiling::function] pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) { - if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic { + if !torque_impulse.is_zero() && self.body_type.is_dynamic() { self.vels.angvel += self.mprops.effective_world_inv_inertia * torque_impulse; if wake_up { @@ -1349,7 +1460,7 @@ impl RigidBody { #[cfg(feature = "dim3")] #[profiling::function] pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { - if torque_impulse != Vector::ZERO && self.body_type == RigidBodyType::Dynamic { + if torque_impulse != Vector::ZERO && self.body_type.is_dynamic() { self.vels.angvel += self.mprops.effective_world_inv_inertia * torque_impulse; if wake_up { @@ -1389,7 +1500,7 @@ impl RigidBody { /// This is the sum of all `add_force()` calls since the last physics step. /// Returns zero for non-dynamic bodies. pub fn user_force(&self) -> Vector { - if self.body_type == RigidBodyType::Dynamic { + if self.body_type.is_dynamic() { self.forces.user_force } else { Vector::ZERO @@ -1401,7 +1512,7 @@ impl RigidBody { /// This is the sum of all `add_torque()` calls since the last physics step. /// Returns zero for non-dynamic bodies. pub fn user_torque(&self) -> AngVector { - if self.body_type == RigidBodyType::Dynamic { + if self.body_type.is_dynamic() { self.forces.user_torque } else { #[cfg(feature = "dim2")] diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index 882ea6709..576c81cb4 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -53,6 +53,11 @@ pub enum RigidBodyType { /// Use for: Moving platforms, elevators, doors, player-controlled characters (when you want /// direct control rather than physics-based movement). KinematicVelocityBased = 3, + + /// The proxy rigid body of a soft-body cluster, created internally, one per cluster, by + /// [`crate::dynamics::SoftBodySet::add_cluster`]; never built directly. Pose, velocity and mass + /// derive from the particles (setters, collider masses ignored); removal removes its cluster. + SoftFrame = 4, // Semikinematic, // A kinematic that performs automatic CCD with the fixed environment to avoid traversing it? // Disabled, } @@ -64,8 +69,16 @@ impl RigidBodyType { } /// Is this rigid-body dynamic (i.e. can move and be affected by forces)? + /// + /// Soft-frame proxies count as dynamic: they move, have (derived) mass, and take part in + /// every dynamic interaction. pub fn is_dynamic(self) -> bool { - self == RigidBodyType::Dynamic + self == RigidBodyType::Dynamic || self == RigidBodyType::SoftFrame + } + + /// Is this rigid-body the proxy of a soft-body cluster? + pub fn is_soft_frame(self) -> bool { + self == RigidBodyType::SoftFrame } /// Is this rigid-body kinematic (i.e. can move but is unaffected by forces)? @@ -425,6 +438,12 @@ impl RigidBodyMassProps { body_type: RigidBodyType, position: &Pose, ) { + if body_type.is_soft_frame() { + // A soft-frame proxy's mass properties are the cluster's reduced mass matrix, + // maintained by the soft-body sync; its colliders contribute none. + self.recompute_max_extent(colliders, attached_colliders); + return; + } let added_mprops = self .additional_local_mprops .as_ref() @@ -526,6 +545,11 @@ impl RigidBodyMassProps { /// Update the world-space mass properties of `self`, taking into account the new position. pub fn update_world_mass_properties(&mut self, body_type: RigidBodyType, position: &Pose) { + if body_type.is_soft_frame() { + // A soft-frame proxy's effective mass properties are the cluster's reduced mass + // matrix, written directly by the soft-body sync: nothing to derive here. + return; + } self.world_com = self.local_mprops.world_com(position); self.effective_inv_mass = Vector::splat(self.local_mprops.inv_mass); self.effective_world_inv_inertia = self.local_mprops.world_inv_inertia(&position.rotation); @@ -1227,12 +1251,16 @@ impl RigidBodyColliders { rb_ccd.ccd_thickness = rb_ccd.ccd_thickness.min(co_shape.ccd_thickness()); } - let mass_properties = co_mprops - .mass_properties(&**co_shape) - .transform_by(&co_parent.pos_wrt_parent); self.0.push(co_handle); - rb_mprops.local_mprops += mass_properties; - rb_mprops.update_world_mass_properties(rb_type, &rb_pos.position); + // A soft-frame proxy's mass is derived from its cluster's particles: its colliders + // contribute none. + if !rb_type.is_soft_frame() { + let mass_properties = co_mprops + .mass_properties(&**co_shape) + .transform_by(&co_parent.pos_wrt_parent); + rb_mprops.local_mprops += mass_properties; + rb_mprops.update_world_mass_properties(rb_type, &rb_pos.position); + } } /// Update the positions of all the colliders attached to this rigid-body. @@ -1433,6 +1461,14 @@ impl RigidBodyActivation { } let can_sleep = match body_type { + // A soft-frame proxy's pose is derived from its cluster's particles (fit noise + // included), so the drift rule applies to the fastest particle instead + // (`sq_linvel` is that speed squared, see `IslandManager::update_body_energy`). + RigidBodyType::SoftFrame => { + let linear_threshold = self.normalized_linear_threshold * length_unit; + self.normalized_linear_threshold >= 0.0 + && sq_linvel * 0.25 < linear_threshold * linear_threshold + } RigidBodyType::Dynamic => { let linear_threshold = self.normalized_linear_threshold * length_unit; let prev_pose = core::mem::replace(&mut self.sleep_prev_pose, *pose); diff --git a/src/dynamics/rigid_body_handle.rs b/src/dynamics/rigid_body_handle.rs index 69200e523..fd1ab44a6 100644 --- a/src/dynamics/rigid_body_handle.rs +++ b/src/dynamics/rigid_body_handle.rs @@ -22,4 +22,9 @@ impl RigidBodyHandle { crate::INVALID_U32, )) } + + /// Whether this handle is the sentinel returned by [`Self::invalid`]. + pub fn is_invalid(self) -> bool { + self == Self::invalid() + } } diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index c36652d2a..1e6c7d989 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -1,7 +1,7 @@ use crate::data::{Arena, HasModifiedFlag, ModifiedObjects}; use crate::dynamics::{ ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBody, RigidBodyBuilder, - RigidBodyChanges, RigidBodyHandle, + RigidBodyChanges, RigidBodyHandle, SoftBodySet, }; use crate::geometry::ColliderSet; use core::ops::{Index, IndexMut}; @@ -207,8 +207,18 @@ impl RigidBodySet { colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, + soft_bodies: &mut SoftBodySet, remove_attached_colliders: bool, ) -> Option { + // Removing a soft-frame proxy removes its cluster: the particles only it covers, the + // elements attached to them, and their colliders. Dissolved before the body removal so the + // proxy (and its parenting of the cluster's colliders) is still intact meanwhile. + if let Some(rb) = self.bodies.get(handle.0) { + if let (Some(sb_handle), Some(cluster)) = (rb.soft_body(), rb.soft_cluster()) { + soft_bodies.on_proxy_removed(sb_handle, cluster, islands, self, colliders); + } + } + let rb = self.bodies.remove(handle.0)?; /* * Update active sets. @@ -220,7 +230,7 @@ impl RigidBodySet { */ if remove_attached_colliders { for collider in rb.colliders() { - colliders.remove(*collider, islands, self, false); + colliders.remove(*collider, islands, self, soft_bodies, false); } } else { // If we don’t remove the attached colliders, simply detach them. diff --git a/src/dynamics/soft_body/mod.rs b/src/dynamics/soft_body/mod.rs index 2fb69250c..2f9ac337a 100644 --- a/src/dynamics/soft_body/mod.rs +++ b/src/dynamics/soft_body/mod.rs @@ -1,6 +1,8 @@ //! Soft bodies: deformable bodies made of particles linked by elastic constraints, solved //! together with rigid bodies, contacts and joints. +pub use self::soft_body_cluster::{SoftBodyCluster, SoftClusterRemoval}; +pub(crate) use self::soft_body_cluster::{inertia_noise_floor, pseudo_inverse_inertia}; pub use self::collision_mesh::{ SoftBindingError, SoftCollisionMesh, SoftMeshBinding, SoftMeshBindingMode, SoftMeshCellBinding, SoftMeshId, SoftMeshMapping, SoftMeshRef, @@ -26,11 +28,12 @@ pub(crate) use self::soft_body_contacts::{SoftEdgeContact, SoftVertexContact}; #[allow(unused_imports)] use self::soft_body_elements::{CELL_IMPULSES}; #[allow(unused_imports)] -use self::soft_body_material::{default_true}; +use self::soft_body_material::{default_true, one}; pub(crate) mod collision_mesh; mod soft_body; mod soft_body_accessors; mod soft_body_builder; +mod soft_body_cluster; mod soft_body_coloring; mod soft_body_contacts; mod soft_body_elements; diff --git a/src/dynamics/soft_body/soft_body.rs b/src/dynamics/soft_body/soft_body.rs index 3e0d3da19..8e6cb7e0a 100644 --- a/src/dynamics/soft_body/soft_body.rs +++ b/src/dynamics/soft_body/soft_body.rs @@ -31,6 +31,12 @@ pub struct SoftBody { /// colliders (invalid until the soft body is inserted in a set): the whole-body cluster's /// proxy, re-pointed to another live cluster's proxy if that cluster is removed. pub(crate) root_body: RigidBodyHandle, + /// The clusters of this soft body (index 0: the whole-body cluster, created at insertion). + /// Removed clusters leave a dead slot so indices stay stable. + pub(crate) clusters: Vec, + /// Number of live clusters referencing each particle: a particle is removed when it drops + /// to zero. + pub(crate) cluster_refs: Vec, /// The particles attached to rigid bodies. pub(crate) attachments: Vec, /// Whether the soft body is currently asleep (mirrors the root body's state at the end of 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 fe0804184..0b04ab5f1 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 @@ -91,6 +91,7 @@ impl SoftBodyBuilder { plastic_stretch: Matrix::IDENTITY, impulses: [0.0; super::super::CELL_IMPULSES], rotation: Rotation::IDENTITY, + stiffness_scale: 1.0, color: 0, torn: false, // Surface: given, or the boundary of the cells. diff --git a/src/dynamics/soft_body/soft_body_cluster.rs b/src/dynamics/soft_body/soft_body_cluster.rs new file mode 100644 index 000000000..955df4dc6 --- /dev/null +++ b/src/dynamics/soft_body/soft_body_cluster.rs @@ -0,0 +1,685 @@ +//! Soft-body clusters: weighted particle sets, each backed by a proxy rigid body +//! ([`crate::dynamics::RigidBodyType::SoftFrame`]) standing for it in the islands and joints. +//! Index 0 is the whole-body cluster; removing a cluster deletes the particles it alone covers. + +use crate::alloc_prelude::*; +use crate::dynamics::RigidBodyHandle; +use crate::math::{AngVector, AngularInertia, DIM, Pose, Real, Rotation, Vector}; +use crate::utils::{AngularInertiaOps, CrossProduct}; + +use super::SoftBody; +use super::soft_body_shape_matching::extract_rotation; + +/// A cluster of a soft body: a set of its particles backed by a proxy rigid body +/// ([`crate::dynamics::RigidBodyType::SoftFrame`]) carrying its frame, gathered velocity and +/// reduced mass. A rank-deficient one (one particle, or collinear in 3D) has no angular response. +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftBodyCluster { + /// The particles of the cluster (sorted, unique). Empty for a removed cluster slot. + pub(crate) particles: Vec, + /// The cluster's proxy rigid body (invalid for a removed cluster slot). + pub(crate) proxy: RigidBodyHandle, + /// Warm-started rotation of the cluster's frame fit. + // NOTE: read once the frames go live (the joint phase); stored from the start so the + // serialization format is stable. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) rotation: Rotation, + /// The cell whose vertices are exactly this cluster's particles, if any: the frame rotation + /// is then the cell's corotational rotation (exact and free) instead of a shape-match fit. + #[cfg_attr(feature = "serde-serialize", serde(default = "invalid_cell"))] + pub(crate) cell: u32, + /// Whether this cluster's particles are shape-matched toward the cluster's frame. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) shape_matching: bool, + pub(crate) shape_matching_target: Option, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) prev_shape_matching_target: Option, + /// The rigid velocity gathered from the particles when the proxy was last refreshed: the + /// proxy's `rb.vels` was set to exactly this, so `rb.vels - last_gather` is the external + /// impulse the user applied to the proxy since (scattered by the solver's first pass). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) last_gather: (Vector, AngVector), + /// Warm-start impulses of this cluster's shape-matching rows, parallel to `particles` + /// (empty until the cluster is shape-matched). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) shape_impulses: Vec, +} + +#[cfg(feature = "serde-serialize")] +fn invalid_cell() -> u32 { + u32::MAX +} + +impl SoftBodyCluster { + pub fn new(particles: Vec, proxy: RigidBodyHandle, shape_matching: bool) -> Self { + SoftBodyCluster { + particles, + proxy, + rotation: Rotation::IDENTITY, + cell: u32::MAX, + shape_matching, + shape_matching_target: None, + prev_shape_matching_target: None, + last_gather: Default::default(), + shape_impulses: vec![], + } + } + + /// Whether this cluster slot is live (not removed). + #[inline] + pub fn is_live(&self) -> bool { + !self.proxy.is_invalid() + } + + /// The particles of this cluster (sorted, unique). + pub fn particles(&self) -> &[u32] { + &self.particles + } + + /// The cluster's proxy rigid body. + pub fn proxy(&self) -> RigidBodyHandle { + self.proxy + } + + /// Whether this cluster's particles are shape-matched toward its frame. + pub fn shape_matching_enabled(&self) -> bool { + self.shape_matching + } + + pub fn set_shape_matching_target(&mut self, target: Option) { + self.shape_matching_target = target; + } +} + +/// What [`crate::dynamics::SoftBodySet::remove_cluster`] (or removing a proxy rigid body) +/// deleted: the particles only the removed cluster covered, and everything attached to them. +#[derive(Copy, Clone, Debug, Default, PartialEq, Eq)] +pub struct SoftClusterRemoval { + /// Particles removed (they belonged to no other cluster). + pub particles: usize, + /// Distance constraints removed with them. + pub edges: usize, + /// Cells removed with them. + pub cells: usize, + /// Dihedral bending constraints removed with them (3D). + pub dihedrals: usize, + /// Surface elements removed with them. + pub surface_elements: usize, + /// Particle attachments removed with them. + pub attachments: usize, + /// Whether the soft body itself was removed (its last cluster is gone). + pub soft_body_removed: bool, +} + +impl SoftBody { + /// The clusters of this soft body (index 0 is the whole-body cluster created at insertion). + /// Removed cluster slots stay as dead entries (`SoftBodyCluster::is_live`); indices are + /// stable. + pub fn clusters(&self) -> &[SoftBodyCluster] { + &self.clusters + } + + /// The `i`-th cluster, if it exists and is live. + pub fn cluster(&self, i: u32) -> Option<&SoftBodyCluster> { + self.clusters.get(i as usize).filter(|c| c.is_live()) + } + + /// The `i`-th cluster, if it exists and is live. + pub fn cluster_mut(&mut self, i: u32) -> Option<&mut SoftBodyCluster> { + let result = self.clusters.get_mut(i as usize).filter(|c| c.is_live()); + self.modified |= result.is_some(); + result + } + + /// The proxy rigid body of the `i`-th cluster, if it is live. + pub fn cluster_proxy(&self, i: u32) -> Option { + self.cluster(i).map(|c| c.proxy) + } + + /// Iterates over the live clusters as `(index, cluster)`. + pub fn live_clusters(&self) -> impl Iterator { + self.clusters + .iter() + .enumerate() + .filter(|(_, c)| c.is_live()) + .map(|(i, c)| (i as u32, c)) + } + + /// Number of live clusters. + pub fn num_live_clusters(&self) -> usize { + self.clusters.iter().filter(|c| c.is_live()).count() + } + + /// Enables or disables shape matching of the `i`-th cluster's particles toward the cluster's + /// frame (independent from the whole-body [`Self::enable_shape_matching`]). + pub fn enable_cluster_shape_matching(&mut self, i: u32, enabled: bool) { + if let Some(c) = self.clusters.get_mut(i as usize) { + if c.is_live() && c.shape_matching != enabled { + c.shape_matching = enabled; + self.modified = true; + } + } + } + + /// Scales the material stiffness (Young modulus) of every cell fully contained in the + /// `i`-th cluster: regional materials without a separate body. Cells straddling the + /// cluster's boundary are left as they are. + pub fn set_cluster_stiffness_scale(&mut self, i: u32, scale: Real) { + let Some(cluster) = self.clusters.get(i as usize).filter(|c| c.is_live()) else { + return; + }; + let contained: Vec = self + .cells + .iter() + .map(|c| { + c.vertices + .iter() + .all(|v| cluster.particles.binary_search(v).is_ok()) + }) + .collect(); + for (c, inside) in self.cells.iter_mut().zip(contained) { + if inside { + c.stiffness_scale = scale.max(0.0); + } + } + self.modified = true; + } + + /// Overrides the softness of every structural or bending edge fully contained in the `i`-th + /// cluster (`None` restores the body material's): regional stiffness for cloth and ropes. + pub fn set_cluster_edge_softness( + &mut self, + i: u32, + softness: Option>, + ) { + let Some(cluster) = self.clusters.get(i as usize).filter(|c| c.is_live()) else { + return; + }; + let contained: Vec = self + .edges + .iter() + .map(|e| { + e.vertices + .iter() + .all(|v| cluster.particles.binary_search(v).is_ok()) + }) + .collect(); + for (e, inside) in self.edges.iter_mut().zip(contained) { + if inside { + e.softness = softness; + } + } + self.modified = true; + } + + /// Pins (or releases) every particle of the `i`-th cluster at once: the cluster becomes a + /// fixed (kinematic) region dragging the rest of the body, driven along a path with + /// [`Self::set_cluster_kinematic_target`]; particles keep their individual pinned semantics. + pub fn set_cluster_pinned(&mut self, i: u32, pinned: bool) { + let Some(cluster) = self.clusters.get(i as usize).filter(|c| c.is_live()) else { + return; + }; + for &v in cluster.particles.clone().iter() { + self.set_particle_pinned(v as usize, pinned); + } + } + + /// Moves the pinned particles of the `i`-th cluster rigidly toward `pose` over the next step: + /// each pinned particle's target is the cluster's rest shape placed at `pose`, with matching + /// velocities; free particles are unaffected (pin them with [`Self::set_cluster_pinned`]). + pub fn set_cluster_kinematic_target(&mut self, i: u32, pose: Pose) { + let Some(cluster) = self.clusters.get(i as usize).filter(|c| c.is_live()) else { + return; + }; + // Rest centroid of the cluster (nominal masses): the target pose places the rest shape. + let mut nominal = 0.0; + let mut rest_com = Vector::ZERO; + for &v in &cluster.particles { + let p = &self.particles[v as usize]; + nominal += p.mass; + rest_com += p.rest_position * p.mass; + } + if nominal > 0.0 { + rest_com /= nominal; + } + for &v in cluster.particles.clone().iter() { + let offset = self.particles[v as usize].rest_position - rest_com; + self.set_particle_kinematic_target(v as usize, pose * offset); + } + } + + /// Recomputes the per-particle cluster reference counts from the cluster lists. + pub(crate) fn rebuild_cluster_refs(&mut self) { + self.cluster_refs.clear(); + self.cluster_refs.resize(self.particles.len(), 0); + for c in &self.clusters { + if c.is_live() { + for &v in &c.particles { + if let Some(r) = self.cluster_refs.get_mut(v as usize) { + *r += 1; + } + } + } + } + } + + /// The cell whose vertices are exactly `particles` (sorted), if any: such a cluster's frame + /// rotation is the cell's own corotational rotation. + pub(crate) fn matching_cell(&self, particles: &[u32]) -> u32 { + if particles.len() != DIM + 1 { + return u32::MAX; + } + for (i, cell) in self.cells.iter().enumerate() { + let mut vs = cell.vertices; + vs.sort_unstable(); + if vs == particles[..DIM + 1] { + return i as u32; + } + } + u32::MAX + } + + /// Registers freshly duplicated particles (a tear split) into the clusters of their source + /// particle, and extends the reference counts. `duplicated` is `(copy, source)` pairs. + pub(crate) fn inherit_cluster_membership(&mut self, duplicated: &[(u32, u32)]) { + self.cluster_refs.resize(self.particles.len(), 0); + for &(copy, source) in duplicated { + for c in &mut self.clusters { + if c.is_live() && c.particles.binary_search(&source).is_ok() { + if let Err(pos) = c.particles.binary_search(©) { + c.particles.insert(pos, copy); + if !c.shape_impulses.is_empty() { + // Keep the warm shape impulses aligned with the particle list. + c.shape_impulses.insert(pos.min(c.shape_impulses.len()), Vector::ZERO); + } + if let Some(r) = self.cluster_refs.get_mut(copy as usize) { + *r += 1; + } + } + } + } + } + // Duplication changes the cell list; the element frames must follow their cells. + self.update_cluster_cells(); + } + + /// Re-derives every cluster's `cell` match (cells were removed, added or renumbered). + pub(crate) fn update_cluster_cells(&mut self) { + let mut matches = Vec::new(); + for c in &self.clusters { + matches.push(if c.is_live() { + self.matching_cell(&c.particles) + } else { + u32::MAX + }); + } + for (c, cell) in self.clusters.iter_mut().zip(matches) { + c.cell = cell; + } + } + + /// Removes every particle whose `dead` flag is set (they must belong to no live cluster), the + /// elements touching them and the attachments on them, compacting the remaining indices. + /// Returns the removal counts and the old-to-new remap (`u32::MAX` if removed; empty if none). + pub(crate) fn remove_dead_particles(&mut self, dead: &[bool]) -> (SoftClusterRemoval, Vec) { + let mut counts = SoftClusterRemoval::default(); + if !dead.contains(&true) { + return (counts, Vec::new()); + } + debug_assert_eq!(dead.len(), self.particles.len()); + + // Old -> new particle index (u32::MAX for the removed ones). + let mut remap = vec![u32::MAX; self.particles.len()]; + let mut next = 0u32; + for (i, &d) in dead.iter().enumerate() { + if !d { + remap[i] = next; + next += 1; + } + } + counts.particles = self.particles.len() - next as usize; + + let alive = |vs: &[u32]| vs.iter().all(|&v| !dead[v as usize]); + + // Elements: drop the ones touching a dead particle, remap the rest. + let before = self.edges.len(); + self.edges.retain(|e| alive(&e.vertices)); + counts.edges = before - self.edges.len(); + for e in &mut self.edges { + for v in &mut e.vertices { + *v = remap[*v as usize]; + } + } + + // Cells need their own remap for the skin bindings. + let mut cell_remap = vec![u32::MAX; self.cells.len()]; + let mut next_cell = 0u32; + for (i, c) in self.cells.iter().enumerate() { + if alive(&c.vertices) { + cell_remap[i] = next_cell; + next_cell += 1; + } + } + let before = self.cells.len(); + self.cells.retain(|c| alive(&c.vertices)); + counts.cells = before - self.cells.len(); + for c in &mut self.cells { + for v in &mut c.vertices { + *v = remap[*v as usize]; + } + } + + #[cfg(feature = "dim3")] + { + let before = self.dihedrals.len(); + self.dihedrals.retain(|d| alive(&d.vertices)); + counts.dihedrals = before - self.dihedrals.len(); + for d in &mut self.dihedrals { + for v in &mut d.vertices { + *v = remap[*v as usize]; + } + } + } + + let before = self.boundary.len(); + self.boundary.retain(|s| alive(s)); + counts.surface_elements = before - self.boundary.len(); + for s in &mut self.boundary { + for v in s.iter_mut() { + *v = remap[*v as usize]; + } + } + + // Attachments anchored to a dead particle die with it. + let before = self.attachments.len(); + self.attachments.retain(|a| !dead[a.particle as usize]); + counts.attachments = before - self.attachments.len(); + for a in &mut self.attachments { + a.particle = remap[a.particle as usize]; + } + if counts.attachments > 0 { + self.attachments_modified = true; + } + self.attachments_modified |= counts.particles > 0; + + // Live clusters: drop dead entries (there should be none) and remap; the warm shape + // impulses follow their particles. + for c in &mut self.clusters { + if c.is_live() { + if c.shape_impulses.len() == c.particles.len() { + let mut keep = c.particles.iter().map(|&v| !dead[v as usize]); + c.shape_impulses.retain(|_| keep.next().unwrap()); + } else { + c.shape_impulses.clear(); + } + c.particles.retain(|&v| !dead[v as usize]); + for v in &mut c.particles { + *v = remap[*v as usize]; + } + } + } + + // The particles themselves, and the per-particle tables. + let mut keep = dead.iter().map(|d| !d); + self.particles.retain(|_| keep.next().unwrap()); + self.rebuild_cluster_refs(); + + // The meshes follow the renumbered particles and cells; their contact caches hold stale + // ids. + let mut meshes = core::mem::take(&mut self.meshes); + for mesh in &mut meshes { + mesh.remap_topology(self, &cell_remap); + } + self.meshes = meshes; + + self.boundary_closed = super::soft_body_builder::surface_is_closed(&self.boundary); + self.rebuild_mesh_tables(); + self.update_cluster_cells(); + self.recolor(); + self.modified = true; + self.topology_version = self.topology_version.wrapping_add(1); + (counts, remap) + } + + /// Weighted centroid of the cluster's particles (current positions) and their total mass. + pub(crate) fn cluster_com_and_mass(&self, cluster: &SoftBodyCluster) -> (Vector, Real) { + let mut com = Vector::ZERO; + let mut mass = 0.0; + for &v in &cluster.particles { + let p = &self.particles[v as usize]; + com += p.position * p.mass; + mass += p.mass; + } + if mass > 0.0 { + com /= mass; + } + (com, mass) + } + + /// The current frame and reduced mass matrix of the `ci`-th cluster: origin at the free + /// particles' mass-weighted centroid (every particle's when all are pinned), rotation from the + /// matching cell (else the Kabsch fit onto the rest positions), mass from the free particles. + pub(crate) fn cluster_frame(&self, ci: usize) -> Option { + let cluster = self.clusters.get(ci)?; + if !cluster.is_live() { + return None; + } + + // Dynamics: the free particles' centroid, momentum and reduced mass matrix. + let mut mass = 0.0; + let mut com = Vector::ZERO; + for &v in &cluster.particles { + let p = &self.particles[v as usize]; + if p.inv_mass > 0.0 { + mass += p.mass; + com += p.position * p.mass; + } + } + let inv_mass = crate::utils::inv(mass); + let origin = if mass > 0.0 { + com * inv_mass + } else { + // Fully pinned: the frame still needs an origin. + let (com_all, _) = self.cluster_com_and_mass(cluster); + com_all + }; + + let mut linvel = Vector::ZERO; + let mut angmom = AngVector::default(); + #[cfg(feature = "dim2")] + let mut inertia: AngularInertia = 0.0; + #[cfg(feature = "dim3")] + let mut inertia = parry::utils::SdpMatrix3::zero(); + for &v in &cluster.particles { + let p = &self.particles[v as usize]; + if p.inv_mass > 0.0 { + let r = p.position - origin; + linvel += p.velocity * p.mass; + angmom += r.gcross(p.velocity * p.mass); + #[cfg(feature = "dim2")] + { + inertia += p.mass * r.length_squared(); + } + #[cfg(feature = "dim3")] + { + let d = p.mass * r.length_squared(); + inertia = parry::utils::SdpMatrix3 { + m11: inertia.m11 + d - p.mass * r.x * r.x, + m12: inertia.m12 - p.mass * r.x * r.y, + m13: inertia.m13 - p.mass * r.x * r.z, + m22: inertia.m22 + d - p.mass * r.y * r.y, + m23: inertia.m23 - p.mass * r.y * r.z, + m33: inertia.m33 + d - p.mass * r.z * r.z, + }; + } + } + } + // Pseudo-inverse with an eigenvalue threshold: a rank-deficient cluster has a singular + // reduced inertia whose naive inverse explodes; those directions lose their angular + // response instead (a fully degenerate cluster gets its angular joint axes stripped). + let inv_inertia = pseudo_inverse_inertia(inertia, inertia_noise_floor(mass, origin)); + let linvel = linvel * inv_mass; + let angvel = inv_inertia.transform_vector(angmom); + + // Rotation: exact (the matching cell's corotational rotation), or the Kabsch fit of the + // particles onto their rest positions (see the plan's 2.6: for a one-cell cluster the + // two differ by O(strain x rest anisotropy), so the exact one is preferred). + let rotation = if let Some(cell) = self.cells.get(cluster.cell as usize) { + cell.rotation + } else { + let mut rest_com = Vector::ZERO; + let mut nominal_mass = 0.0; + for &v in &cluster.particles { + let p = &self.particles[v as usize]; + rest_com += p.rest_position * p.mass; + nominal_mass += p.mass; + } + if nominal_mass > 0.0 { + rest_com /= nominal_mass; + } + let mut fit_com = Vector::ZERO; + for &v in &cluster.particles { + let p = &self.particles[v as usize]; + fit_com += p.position * p.mass; + } + if nominal_mass > 0.0 { + fit_com /= nominal_mass; + } + let mut a = crate::math::Matrix::ZERO; + for &v in &cluster.particles { + let p = &self.particles[v as usize]; + let x = (p.position - fit_com) * p.mass; + let q = p.rest_position - rest_com; + #[cfg(feature = "dim2")] + { + a += crate::math::Matrix::from_cols(x * q.x, x * q.y); + } + #[cfg(feature = "dim3")] + { + a += crate::math::Matrix::from_cols(x * q.x, x * q.y, x * q.z); + } + } + extract_rotation(a, cluster.rotation) + }; + + Some(ClusterFrame { + pose: Pose::from_parts(origin, rotation), + inv_mass, + inv_inertia, + linvel, + angvel, + }) + } +} + +impl SoftBody { + /// Solver-side gather of a cluster's rigid state from its particles' solver bodies: `(com, + /// linvel, angvel)` of the free particles, with the frozen per-step `inv_inertia` (world axes + /// about the fresh com). `pos`/`vel` map a particle index to its solver state. + pub(crate) fn gather_cluster_velocity( + &self, + ci: usize, + inv_inertia: &AngularInertia, + pos: impl Fn(u32) -> Vector, + vel: impl Fn(u32) -> Vector, + ) -> (Vector, Vector, AngVector) { + let cluster = &self.clusters[ci]; + let mut mass = 0.0; + let mut com = Vector::ZERO; + for &v in &cluster.particles { + let p = &self.particles[v as usize]; + if p.inv_mass > 0.0 { + mass += p.mass; + com += pos(v) * p.mass; + } + } + if mass > 0.0 { + com /= mass; + } + let inv_mass = crate::utils::inv(mass); + let mut linvel = Vector::ZERO; + let mut angmom = AngVector::default(); + for &v in &cluster.particles { + let p = &self.particles[v as usize]; + if p.inv_mass > 0.0 { + let r = pos(v) - com; + linvel += vel(v) * p.mass; + angmom += r.gcross(vel(v) * p.mass); + } + } + ( + com, + linvel * inv_mass, + inv_inertia.transform_vector(angmom), + ) + } +} + +/// The per-step frame and reduced mass matrix of a cluster (see `SoftBody::cluster_frame`). +#[derive(Copy, Clone, Debug)] +pub(crate) struct ClusterFrame { + /// Frame pose: origin at the free particles' weighted centroid, rotation from the cell or + /// the Kabsch fit. + pub pose: Pose, + /// Inverse of the free particles' total mass (`0`: immovable). + pub inv_mass: Real, + /// Inverse of the reduced angular inertia about the origin (world axes; zero when + /// rank-deficient). + pub inv_inertia: AngularInertia, + /// Mass-weighted mean velocity of the free particles. + pub linvel: Vector, + /// Angular velocity of the rigid fit: `I⁻¹ Σ rᵢ × mᵢvᵢ`. + pub angvel: AngVector, +} + +/// Absolute noise floor of a cluster's reduced inertia: a zero-extent cluster (one free particle, +/// a point-collapsed one) has a roundoff inertia whose inverse fits huge angular velocities, so +/// an eigenvalue below the mass put at `sqrt(machine epsilon)` of the coordinate scale is noise. +pub(crate) fn inertia_noise_floor(mass: Real, com: Vector) -> Real { + let noise_extent = Real::EPSILON.sqrt() * (1.0 + com.length()); + mass * noise_extent * noise_extent +} + +/// Pseudo-inverse of a reduced angular inertia: directions whose eigenvalue is negligible next +/// to the largest one, or below the absolute floor `abs_floor` (see [`inertia_noise_floor`]), +/// are zeroed instead of inverted (a singular inertia's naive inverse explodes). +#[cfg(feature = "dim2")] +pub(crate) fn pseudo_inverse_inertia(inertia: AngularInertia, abs_floor: Real) -> AngularInertia { + if inertia <= abs_floor { + 0.0 + } else { + crate::utils::inv(inertia) + } +} + +/// See the 2D overload. +#[cfg(feature = "dim3")] +pub(crate) fn pseudo_inverse_inertia(inertia: AngularInertia, abs_floor: Real) -> AngularInertia { + const RELATIVE_EPS: Real = 1.0e-5; + let m = na::Matrix3::new( + inertia.m11, inertia.m12, inertia.m13, inertia.m12, inertia.m22, inertia.m23, inertia.m13, + inertia.m23, inertia.m33, + ); + let eig = m.symmetric_eigen(); + let max = eig.eigenvalues.amax(); + if max <= abs_floor { + return parry::utils::SdpMatrix3::zero(); + } + let mut inv = na::Matrix3::zeros(); + for k in 0..3 { + let lambda = eig.eigenvalues[k]; + if lambda > (max * RELATIVE_EPS).max(abs_floor) { + let u = eig.eigenvectors.column(k); + inv += u * u.transpose() / lambda; + } + } + parry::utils::SdpMatrix3 { + m11: inv[(0, 0)], + m12: inv[(0, 1)], + m13: inv[(0, 2)], + m22: inv[(1, 1)], + m23: inv[(1, 2)], + m33: inv[(2, 2)], + } +} diff --git a/src/dynamics/soft_body/soft_body_elements.rs b/src/dynamics/soft_body/soft_body_elements.rs index f3ca53f0d..ee53ea6ae 100644 --- a/src/dynamics/soft_body/soft_body_elements.rs +++ b/src/dynamics/soft_body/soft_body_elements.rs @@ -202,6 +202,10 @@ pub struct SoftBodyCell { /// Rotation of the polar decomposition of the cell's deformation gradient at the last step /// (warm start of the corotational rows' rotation extraction). pub(crate) rotation: Rotation, + /// Multiplier of the material's Young modulus for this cell (default `1.0`): per-region + /// stiffness, usually set through [`crate::dynamics::SoftBody::set_cluster_stiffness_scale`]. + #[cfg_attr(feature = "serde-serialize", serde(default = "one"))] + pub stiffness_scale: Real, /// Parallel solve color. pub(crate) color: u8, /// Set when the cell was strained past the material's `tear_strain` during the last step diff --git a/src/dynamics/soft_body/soft_body_material.rs b/src/dynamics/soft_body/soft_body_material.rs index 9432d3f9f..3db8b5490 100644 --- a/src/dynamics/soft_body/soft_body_material.rs +++ b/src/dynamics/soft_body/soft_body_material.rs @@ -82,6 +82,11 @@ pub(super) fn default_true() -> bool { true } +#[cfg(feature = "serde-serialize")] +pub(super) fn one() -> Real { + 1.0 +} + impl Default for SoftBodyMaterial { fn default() -> Self { Self { diff --git a/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs b/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs new file mode 100644 index 000000000..691a3597c --- /dev/null +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs @@ -0,0 +1,163 @@ +//! The cluster paths of a `SoftBodySet`: adding, removing and dissolving clusters. +use crate::alloc_prelude::*; +use crate::dynamics::{ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyHandle, RigidBodySet}; +use crate::geometry::ColliderSet; +use crate::dynamics::soft_body::{SoftBodyCluster, SoftBodyHandle, SoftClusterRemoval}; +use crate::math::Rotation; +use super::soft_body_set_proxies::spawn_proxy; +use super::{SoftBodyIslandEvent, SoftBodySet}; + +impl SoftBodySet { + /// Adds a cluster to a soft body: a set of its particles (invalid indices ignored, duplicates + /// merged) backed by a fresh [`crate::dynamics::RigidBodyType::SoftFrame`] proxy body that + /// joints and colliders attach to. Returns its stable index, `None` if nothing valid remains. + pub fn add_cluster( + &mut self, + handle: SoftBodyHandle, + particles: &[u32], + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) -> Option { + let sb = self.bodies.get_mut(handle.0)?; + let n = sb.particles.len() as u32; + let mut list: Vec = particles.iter().copied().filter(|&v| v < n).collect(); + list.sort_unstable(); + list.dedup(); + if list.is_empty() { + return None; + } + let index = sb.clusters.len() as u32; + let proxy = spawn_proxy(&sb.particle_settings, sb.user_data, handle, index, bodies); + for &v in &list { + sb.cluster_refs[v as usize] += 1; + } + let cell = sb.matching_cell(&list); + sb.clusters.push(SoftBodyCluster { + particles: list, + proxy, + rotation: Rotation::IDENTITY, + cell, + shape_matching: false, + shape_matching_target: None, + prev_shape_matching_target: None, + last_gather: Default::default(), + shape_impulses: Vec::new(), + }); + sb.modified = true; + // Update right away: a joint attached before the next step must see the cluster's + // real frame and reduced mass. + Self::update_cluster_proxies(sb, bodies, colliders); + self.island_events.push(SoftBodyIslandEvent { handle }); + Some(index) + } + + /// Removes the `cluster`-th cluster of a soft body, its proxy rigid body, and the particles + /// only this cluster covered (with their elements and attachments); removing the last cluster + /// removes the body. Returns what was deleted, or `None` if the body or cluster is missing. + #[allow(clippy::too_many_arguments)] + pub fn remove_cluster( + &mut self, + handle: SoftBodyHandle, + cluster: u32, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + ) -> Option { + let (counts, proxy) = self.dissolve_cluster(handle, cluster, islands, bodies, colliders)?; + // The proxy body itself, with its own colliders and joints (the dissolution above + // tombstoned the cluster, so the removal hook no-ops). + bodies.remove( + proxy, + islands, + colliders, + impulse_joints, + multibody_joints, + self, + true, + ); + Some(counts) + } + + /// The proxy-removal hook of [`RigidBodySet::remove`]: dissolves the cluster the removed + /// proxy stood for. The proxy body itself (and its colliders and joints) is removed by the + /// caller. + pub(crate) fn on_proxy_removed( + &mut self, + handle: SoftBodyHandle, + cluster: u32, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) { + let _ = self.dissolve_cluster(handle, cluster, islands, bodies, colliders); + } + + /// Dissolves a cluster: tombstones it, removes the particles it uniquely covered (with their + /// elements, attachments and particle-ball colliders), re-points the root body if needed, and + /// removes the body with its last cluster. Keeps the proxy body; returns counts and its handle. + fn dissolve_cluster( + &mut self, + handle: SoftBodyHandle, + cluster: u32, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) -> Option<(SoftClusterRemoval, RigidBodyHandle)> { + // First pass, on the soft body alone: tombstone the cluster and remove the particles + // only it covered (with their elements and attachments). + let (mut counts, remap, dead, proxy, root, last) = { + let sb = self.bodies.get_mut(handle.0)?; + let c = sb.clusters.get_mut(cluster as usize)?; + if !c.is_live() { + return None; + } + + // Reference counts: the particles only this cluster covered die. + let mut dead = alloc::vec![false; sb.particles.len()]; + for &v in &particles { + if let Some(r) = sb.cluster_refs.get_mut(v as usize) { + *r = r.saturating_sub(1); + if *r == 0 { + dead[v as usize] = true; + } + } + } + let (counts, remap) = sb.remove_dead_particles(&dead); + + // The root body follows the first live cluster when cluster 0 goes. + if sb.root_body == proxy { + let next_root = sb + .live_clusters() + .next() + .map(|(_, c)| c.proxy) + .unwrap_or(RigidBodyHandle::invalid()); + sb.root_body = next_root; + } + let last = sb.num_live_clusters() == 0; + (counts, remap, dead, proxy, sb.root_body, last) + }; + counts.soft_body_removed = last; + + if last { + // The last cluster: the soft body goes with it (its proxy is removed by the caller). + let sb = self.bodies.remove(handle.0).unwrap(); + islands.persistent.unlink_soft_body_attachments(handle); + islands.persistent.unlink_soft_body_proxy_chain(handle); + self.attached_to_stale |= !sb.attachments.is_empty(); + return Some((counts, proxy)); + } + + if counts.particles > 0 { + // The surface shape lost the dead particles' vertices: rebuild it. + Self::update_colliders(&self.bodies[handle.0], bodies, colliders, true); + } + self.attached_to_stale |= counts.attachments > 0; + self.island_events.push(SoftBodyIslandEvent { handle }); + if let Some(rb) = bodies.get_mut(root) { + rb.wake_up(true); + } + Some((counts, proxy)) + } +} 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 99c7c6e4e..1d931738c 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 @@ -26,6 +26,8 @@ impl SoftBodySet { // Reserve the handle first: the root body stores it. let handle = SoftBodyHandle(self.bodies.insert_with(|_| SoftBody { particles: Vec::new(), + clusters: Vec::new(), + cluster_refs: Vec::new(), root_body: RigidBodyHandle::invalid(), attachments: Vec::new(), sleeping: false, @@ -61,6 +63,27 @@ impl SoftBodySet { user_data: 0, })); + soft_body.root_body = + spawn_proxy(&soft_body.particle_settings, soft_body.user_data, handle, 0, bodies); + soft_body.clusters = alloc::vec![SoftBodyCluster { + particles: (0..soft_body.particles.len() as u32).collect(), + proxy: soft_body.root_body, + rotation: Rotation::IDENTITY, + cell: u32::MAX, + shape_matching: soft_body_builder.shape_matching, + shape_matching_target: None, + prev_shape_matching_target: None, + last_gather: Default::default(), + shape_impulses: Vec::new(), + }]; + soft_body.cluster_refs = alloc::vec![1; soft_body.particles.len()]; + // The whole-body frame first: the colliders are expressed in it. + Self::update_cluster_proxies(&mut soft_body, bodies, colliders); + let frame = bodies + .get(soft_body.root_body) + .map(|rb| rb.pos.position) + .unwrap_or(Pose::IDENTITY); + // The deformable meshes' colliders attach to the root body (so they link islands and wake // the soft body), with vertices in the whole-body cluster's frame (the proxy's pose holds // the rigid motion). They collide with everything, fixed and kinematic colliders included. @@ -81,7 +104,24 @@ impl SoftBodySet { multibody_joints: &mut MultibodyJointSet, ) -> Option { let soft_body = self.bodies.remove(handle.0)?; + // Unlink the attachment and proxy-chain links while the proxies still exist. islands.persistent.unlink_soft_body_attachments(handle); + islands.persistent.unlink_soft_body_proxy_chain(handle); self.attached_to_stale |= !soft_body.attachments.is_empty(); + for cluster in &soft_body.clusters { + if cluster.is_live() { + // The soft body is out of the arena already, so the proxy-removal hook no-ops. + bodies.remove( + cluster.proxy, + islands, + colliders, + impulse_joints, + multibody_joints, + self, + true, + ); + } + } + Some(soft_body) } } 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 befc912bb..17069eead 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 @@ -77,8 +77,19 @@ pub(super) fn rebuilt_surface_shape( .additional_solver_iterations(settings.additional_solver_iterations) .can_sleep(settings.can_sleep) .dominance_group(settings.dominance_group) + .user_data(user_data) .build(); let rb_handle = bodies.insert(rb); + // Stamped after insertion: `insert` resets the internal references. The body type is set + // directly: `SoftFrame` cannot be built or converted to through the public API. + let rb = bodies.index_mut_internal(rb_handle); + rb.body_type = crate::dynamics::RigidBodyType::SoftFrame; + rb.soft_body = handle; + rb.soft_cluster = cluster; + #[cfg(feature = "dim3")] + { + rb.forces.gyroscopic_forces_enabled = false; + } rb_handle } 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 aba0e24f8..02f24d0e0 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 @@ -1,4 +1,6 @@ /// refreshes the attachment index and island links of the bodies whose attachments changed. + /// Refreshes every live cluster's proxy rigid body from the current particles: its pose is + // Dead zone: the frame fit carries a little noise even at rest (the least-squares // so a one-refresh-stale proxy pose is fine. /// Refreshes every soft body's derived state at the end of a step: the surface orientation, // Every body reads its own particles and writes only itself: updated in parallel, then diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs index a7aae8d97..3eddbe613 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -119,7 +119,9 @@ impl JointGenericExternalConstraintBuilder { // Each row appends `2 * multibodies_ndof` entries (jacobian and weighted jacobian, both // sides); reserve exactly the rows this joint emits (motor plus limit on an axis gives two, // so more than `SPATIAL_DIM`), counted after `strip_soft_frame_angular_axes` runs. - let num_rows = joint_num_constraints(joint); + let mut masked_data = joint.data; + super::strip_soft_frame_angular_axes(&mut masked_data, rb1, rb2); + let num_rows = crate::dynamics::solver::joint_data_num_constraints(&masked_data); let required_jacobian_len = *j_id + multibodies_ndof * 2 * num_rows; *j_id += multibodies_ndof * 2 * num_rows; @@ -130,7 +132,7 @@ impl JointGenericExternalConstraintBuilder { jacobians.resize_vertically_mut(required_jacobian_len, 0.0); } - let mut joint_data = joint.data; + let mut joint_data = masked_data; // NOTE: multibody links are positioned by `link.local_to_world` which is // the body-frame (origin-centered) pose, unlike regular dynamic // bodies whose solver pose is com-centered. So the com shift from diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 3ccdbd31d..142048229 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -58,14 +58,18 @@ impl JointConstraintBuilder { // Since solver body poses are given in center-of-mass space, // we need to transform the anchors to that space. out_builder.joint.transform_to_solver_body_space(rb1, rb2); + // A rank-deficient soft-frame side has no orientation: its angular axes are + // stripped (the reserved row count must match the stripped lowering). + super::strip_soft_frame_angular_axes(&mut out_builder.joint, rb1, rb2); - *out_constraint_id += joint_num_constraints(joint); + *out_constraint_id += + crate::dynamics::solver::joint_data_num_constraints(&out_builder.joint); } - /// Refreshes the warm-start seeds (the impulses written back at the end of + /// Updates the warm-start seeds (the impulses written back at the end of /// the previous step) of a builder recycled across steps by the persistent /// joint assembly. - pub fn refresh_warmstart_seeds(&mut self, joints_all: &[crate::dynamics::JointGraphEdge]) { + pub fn update_warmstart_seeds(&mut self, joints_all: &[crate::dynamics::JointGraphEdge]) { let joint = &joints_all[self.joint_id].weight; self.prev_dof_impulses = joint.impulses; for i in 0..SPATIAL_DIM { @@ -329,10 +333,10 @@ impl JointConstraintBuilderSimd { *out_constraint_id += joint_num_constraints(joint[0]); } - /// Refreshes the warm-start seeds (the impulses written back at the end of + /// Updates the warm-start seeds (the impulses written back at the end of /// the previous step) of a builder recycled across steps by the persistent /// joint assembly. - pub fn refresh_warmstart_seeds(&mut self, joints_all: &[crate::dynamics::JointGraphEdge]) { + pub fn update_warmstart_seeds(&mut self, joints_all: &[crate::dynamics::JointGraphEdge]) { let joint = array![|ii| &joints_all[self.joint_id[ii]].weight]; self.prev_dof_impulses = core::array::from_fn(|axis| array![|ii| joint[ii].impulses[axis]].into()); diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs index 1da2352d6..e8104c98e 100644 --- a/src/dynamics/solver/joint_constraint/mod.rs +++ b/src/dynamics/solver/joint_constraint/mod.rs @@ -18,3 +18,49 @@ mod joint_constraint_builder; mod joint_constraint_helper; mod joint_constraints_set; mod joint_velocity_constraint; + +/// Whether this rigid body is a soft-frame proxy with no angular response (a rank-deficient +/// cluster: one particle, or a collinear one in 3D): its zero reduced inverse inertia would turn +/// an angular joint row into a one-sided constraint freezing the *other* body's orientation. +pub(crate) fn soft_frame_angular_degenerate(rb: &crate::dynamics::RigidBody) -> bool { + if !rb.is_soft_frame() { + return false; + } + let ii = &rb.mass_properties().effective_world_inv_inertia; + #[cfg(feature = "dim2")] + { + *ii == 0.0 + } + #[cfg(feature = "dim3")] + { + ii.m11 == 0.0 + && ii.m12 == 0.0 + && ii.m13 == 0.0 + && ii.m22 == 0.0 + && ii.m23 == 0.0 + && ii.m33 == 0.0 + } +} + +/// Whether the joint constrains any angular axis (locked, limited, motorized or coupled). +pub(crate) fn joint_uses_angular_axes(data: &crate::dynamics::GenericJoint) -> bool { + let ang = crate::dynamics::JointAxesMask::ANG_AXES; + !((data.locked_axes | data.limit_axes | data.motor_axes | data.coupled_axes) & ang).is_empty() +} + +/// Strips the angular axes from a joint's lowered data when one side is an angular-degenerate +/// soft frame (see [`soft_frame_angular_degenerate`]): the joint keeps its translational +/// behavior, and the documented rule is that a rank-deficient cluster has no orientation. +pub(crate) fn strip_soft_frame_angular_axes( + data: &mut crate::dynamics::GenericJoint, + rb1: &crate::dynamics::RigidBody, + rb2: &crate::dynamics::RigidBody, +) { + if soft_frame_angular_degenerate(rb1) || soft_frame_angular_degenerate(rb2) { + let ang = crate::dynamics::JointAxesMask::ANG_AXES; + data.locked_axes &= !ang; + data.limit_axes &= !ang; + data.motor_axes &= !ang; + data.coupled_axes &= !ang; + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs index 25cd45b99..327132a4e 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs @@ -40,6 +40,159 @@ impl SoftConstraintsSet { } }; + // Per-cluster shape matching: each shape-matched cluster gets its own best-fit frame + // and pulls only its particles toward it. + if !awake.awake_clusters.is_empty() { + let vel = |i: usize| -> Vector { + let s = slots[i]; + if s == u32::MAX { + sb.particles[i].velocity + } else { + bodies.get_vel(s).linear + } + }; + let constraints = &mut shape_constraints[awake.shape_constraints.clone()]; + for k in 0..awake.awake_clusters.len() { + let awake_cluster = &mut awake.awake_clusters[k]; + let cluster = &sb.clusters[awake_cluster.cluster as usize]; + + if let Some(target_pose) = cluster.shape_matching_target { + // The shape-matching follows a kinematic path (step-constant: set once). + if !update { + continue; + } + // The mid-pose converges measurably faster toward the target; the target + // or the previous pose alone over-shoots or undershoots. + let mid_pose = cluster + .prev_shape_matching_target + .map(|prev| prev.lerp(&target_pose, 0.5)) + .unwrap_or(target_pose); + awake_cluster.rotation = target_pose.rotation; + + for constraint in constraints.iter_mut() { + if constraint.cluster != awake_cluster.cluster { + continue; + } + let particle = constraint.particle as usize; + constraint.goal = mid_pose * sb.particles[particle].rest_position; + constraint.goal_vel = awake_cluster.target_linvel + + awake_cluster + .target_angvel + .gcross(constraint.goal - mid_pose.translation); + } + continue; + } + + if update { + // The fit of the poses: rotation and centroids, then the inertia of the + // free particles about their own center of mass (the velocity fit below + // reads it every pass). + let mut mass = 0.0; + let mut rest_com = Vector::ZERO; + let mut com = Vector::ZERO; + for &v in cluster.particles() { + let p = &sb.particles[v as usize]; + mass += p.mass; + rest_com += p.rest_position * p.mass; + com += pos(v as usize) * p.mass; + } + if mass > 0.0 { + rest_com /= mass; + com /= mass; + } + let mut a = Matrix::ZERO; + for &v in cluster.particles() { + let p = &sb.particles[v as usize]; + a += outer((pos(v as usize) - com) * p.mass, p.rest_position - rest_com); + } + let rot = crate::dynamics::soft_body::soft_body_shape_matching::extract_rotation( + a, + awake_cluster.rotation, + ); + let mut dyn_mass = 0.0; + let mut dyn_com = Vector::ZERO; + for &v in cluster.particles() { + let p = &sb.particles[v as usize]; + if p.inv_mass > 0.0 { + dyn_mass += p.mass; + dyn_com += pos(v as usize) * p.mass; + } + } + dyn_com *= crate::utils::inv(dyn_mass); + #[cfg(feature = "dim2")] + let mut inertia: crate::math::AngularInertia = 0.0; + #[cfg(feature = "dim3")] + let mut inertia = parry::utils::SdpMatrix3::zero(); + for &v in cluster.particles() { + let p = &sb.particles[v as usize]; + if p.inv_mass > 0.0 { + let r = pos(v as usize) - dyn_com; + #[cfg(feature = "dim2")] + { + inertia += p.mass * r.length_squared(); + } + #[cfg(feature = "dim3")] + { + let d = p.mass * r.length_squared(); + inertia = parry::utils::SdpMatrix3 { + m11: inertia.m11 + d - p.mass * r.x * r.x, + m12: inertia.m12 - p.mass * r.x * r.y, + m13: inertia.m13 - p.mass * r.x * r.z, + m22: inertia.m22 + d - p.mass * r.y * r.y, + m23: inertia.m23 - p.mass * r.y * r.z, + m33: inertia.m33 + d - p.mass * r.z * r.z, + }; + } + } + } + let floor = crate::dynamics::soft_body::inertia_noise_floor(dyn_mass, dyn_com); + awake_cluster.rotation = rot; + awake_cluster.com = com; + awake_cluster.rest_com = rest_com; + awake_cluster.dyn_com = dyn_com; + awake_cluster.inv_inertia = + crate::dynamics::soft_body::pseudo_inverse_inertia(inertia, floor); + for constraint in constraints.iter_mut() { + if constraint.cluster != awake_cluster.cluster { + continue; + } + let particle = constraint.particle as usize; + constraint.goal = + com + rot * (sb.particles[particle].rest_position - rest_com); + } + } + + // Rigid-fit velocity of the cluster (free particles): the constraints damp relative + // to it, not to the world. The angular momentum is taken about the free particles' + // center of mass, so the linear part drops out of it. + let dyn_com = awake_cluster.dyn_com; + let mut dyn_mass = 0.0; + let mut vcom = Vector::ZERO; + let mut angmom = crate::math::AngVector::default(); + for &v in cluster.particles() { + let p = &sb.particles[v as usize]; + if p.inv_mass > 0.0 { + let velocity = vel(v as usize); + dyn_mass += p.mass; + vcom += velocity * p.mass; + angmom += (pos(v as usize) - dyn_com).gcross(velocity * p.mass); + } + } + vcom *= crate::utils::inv(dyn_mass); + let omega = awake_cluster.inv_inertia.transform_vector(angmom); + for constraint in constraints.iter_mut() { + if constraint.cluster != awake_cluster.cluster { + continue; + } + let particle = constraint.particle as usize; + constraint.goal_vel = vcom + omega.gcross(pos(particle) - dyn_com); + } + } + } + + if !update { + return; + } if let Some(vi) = awake.volume_constraint { let vc = &mut volume_constraints[vi]; let grads = &mut volume_grads[vc.grads.clone()]; diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs index f0ae32b41..a54a67922 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs @@ -12,10 +12,142 @@ use super::super::soft_element_constraint::MAX_CONSTRAINT_PARTICLES; use super::*; impl SoftConstraintsSet { - /// Builds the particle attachment rows of the awake soft bodies (group-major, so it runs - /// after [`Self::assemble`] laid the groups out): the particle's slot against the rigid - /// body's (a fixed or non-simulated body anchors the particle to a world point). - /// `group_dt(group)` is the group's substep length (the rows' softness). + /// Collects the step's active clusters, group-major (after [`Self::assemble`] laid the groups + /// out): those whose proxy has an enabled joint (`jointed_proxies`) or a pending external + /// impulse or force. The gather stage feeds their proxy slot; the scatter stage writes back. + pub fn assemble_clusters( + &mut self, + bodies: &crate::dynamics::RigidBodySet, + colliders: &crate::geometry::ColliderSet, + solver_bodies: &SolverBodies, + jointed_proxies: &parry::utils::hashmap::HashMap, + ) { + self.clusters.clear(); + for gi in 0..self.groups.len() { + let start = self.clusters.len(); + for ai in self.groups[gi].awake.clone() { + let awake = &self.awake[ai]; + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + for (ci, cluster) in sb.clusters.iter().enumerate() { + if !cluster.is_live() { + continue; + } + let Some(rb) = bodies.get(cluster.proxy) else { + continue; + }; + let slot = rb.ids.active_set_id; + if slot == u32::MAX { + continue; + } + let external = rb.vels.linvel != cluster.last_gather.0 + || rb.vels.angvel != cluster.last_gather.1 + || rb.forces.user_force != Vector::ZERO + || rb.forces.user_torque != crate::math::AngVector::default(); + // A proxy with a collider of its own (not the soft-managed surface or + // balls) takes rigid contacts on its slot: those impulses must be scattered. + let has_user_collider = rb.colliders().iter().any(|co| { + colliders + .get(*co) + .is_some_and(|co| !co.is_deformable_collider()) + }); + if !jointed_proxies.contains_key(&slot) && !external && !has_user_collider { + continue; + } + let inv_inertia = rb.mprops.effective_world_inv_inertia; + let (com, _, _) = sb.gather_cluster_velocity( + ci, + &inv_inertia, + |v| solver_bodies.get_pose(slots[v as usize]).translation, + |v| solver_bodies.get_vel(slots[v as usize]).linear, + ); + // The proxy slot starts at `rb.vels`, the sync-time gather: using that same + // gather as the reference scatters the external impulses applied since first. + let mut ref_vel = SolverVel::zero(); + ref_vel.linear = cluster.last_gather.0; + ref_vel.angular = cluster.last_gather.1; + self.clusters.push(SoftClusterRecord { + awake: ai as u32, + cluster: ci as u32, + slot, + com, + ref_vel, + }); + } + } + self.groups[gi].clusters = start..self.clusters.len(); + } + } + + /// The gather stage of one pass (worker 0, serial): re-derives each active cluster's rigid + /// velocity and centroid from its particles' solver state and sets the proxy slot to + /// `fresh + (slot - ref_vel)`, so the pending external and joint impulses stay. + pub fn gather_clusters(&mut self, group: usize, bodies: &mut SolverBodies) { + let range = self.groups[group].clusters.clone(); + for record in &mut self.clusters[range] { + let awake = &self.awake[record.awake as usize]; + // SAFETY: worker-0 exclusive stage. + let sb = unsafe { &*awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let inv_inertia = bodies.get_pose(record.slot).ii; + let (com, linvel, angvel) = sb.gather_cluster_velocity( + record.cluster as usize, + &inv_inertia, + |v| bodies.get_pose(slots[v as usize]).translation, + |v| bodies.get_vel(slots[v as usize]).linear, + ); + record.com = com; + let slot = record.slot as usize; + let vels = &mut bodies.vels[slot]; + vels.linear += linvel - record.ref_vel.linear; + vels.angular += angvel - record.ref_vel.angular; + record.ref_vel.linear = linvel; + record.ref_vel.angular = angvel; + // The frame origin follows the particles exactly (the integrated proxy pose is only + // an estimate of the same motion). + bodies.poses[slot].translation = com; + } + } + + /// The scatter stage of one pass (worker 0, serial): distributes each active cluster's + /// proxy-slot velocity change since the gather onto its free particles as the rigid field + /// `Δv + Δω × r`; `Σ mᵢΔvᵢ` is the impulse the joints applied to the slot. + pub fn scatter_clusters(&mut self, group: usize, bodies: &mut SolverBodies) { + let range = self.groups[group].clusters.clone(); + for record in &mut self.clusters[range] { + let slot_vels = bodies.get_vel(record.slot); + let dlin = slot_vels.linear - record.ref_vel.linear; + let dang = slot_vels.angular - record.ref_vel.angular; + if dlin == Vector::ZERO && dang == crate::math::AngVector::default() { + continue; + } + let awake = &self.awake[record.awake as usize]; + // SAFETY: worker-0 exclusive stage; overlapping clusters are fine, the stage is + // serial. + let sb = unsafe { &*awake.ptr }; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + for &v in &sb.clusters[record.cluster as usize].particles { + let p = &sb.particles[v as usize]; + if p.inv_mass > 0.0 { + let slot = slots[v as usize] as usize; + if slot < bodies.len() { + let r = bodies.poses[slot].translation - record.com; + bodies.vels[slot].linear += dlin + dang.gcross(r); + } + } + } + record.ref_vel.linear = slot_vels.linear; + record.ref_vel.angular = slot_vels.angular; + } + } + + /// Whether the given group has active clusters (the gather/scatter stages exist). + #[inline] + pub fn group_has_clusters(&self, group: usize) -> bool { + !self.groups[group].clusters.is_empty() + } + pub fn assemble_attachments( &mut self, island_id: usize, 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 713da0f73..0949ee581 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 @@ -101,6 +101,21 @@ impl SoftConstraintsSet { for awake in &self.awake { // SAFETY: serial stage, exclusive access. let sb = unsafe { &mut *awake.ptr }; + if !awake.awake_clusters.is_empty() { + for awake_cluster in &awake.awake_clusters { + let cluster = &mut sb.clusters[awake_cluster.cluster as usize]; + cluster.rotation = awake_cluster.rotation; + cluster + .shape_impulses + .resize(cluster.particles.len(), Vector::ZERO); + } + for constraint in &self.shape_constraints[awake.shape_constraints.clone()] { + let cluster = &mut sb.clusters[constraint.cluster as usize]; + if let Ok(k) = cluster.particles.binary_search(&constraint.particle) { + cluster.shape_impulses[k] = constraint.impulse; + } + } + } if let Some(vi) = awake.volume_constraint { sb.volume_impulse = crate::utils::canonicalize_zero(self.volume_constraints[vi].impulse); } diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs index 25e7c36ee..10b4d0c3f 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs @@ -15,6 +15,7 @@ use super::super::soft_attachment::FemAttachment; #[derive(Copy, Clone, Debug)] pub(crate) struct SoftShapeConstraint { pub particle: u32, + pub cluster: u32, pub solver_id: u32, pub im: Vector, pub goal: Vector, 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 283b1c528..ad7ef0aa3 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 @@ -46,6 +46,9 @@ pub(crate) struct AwakeSoftBody { /// Largest normal approach speed of the rigid bodies met by the surface this step (`None`: /// no contact row 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: + /// `(cluster index, rotation)`, refreshed by the per-pass prepare. + pub awake_clusters: Vec, } // SAFETY: the raw pointer is only dereferenced under the staged solver's stage discipline. unsafe impl Send for AwakeSoftBody {} @@ -94,6 +97,8 @@ pub(crate) struct SoftGroupLayout { pub slots: Range, /// Range into `attachments`. pub attachments: Range, + /// Range into `clusters`. + pub clusters: Range, /// Index range into `color_ranges`. pub colors: Range, /// Constraints solved serially by worker 0 (overflow color). @@ -111,7 +116,31 @@ pub(crate) struct SoftGroupLayout { pub contact_serial: Range, /// Whether an awake body of the group damps its deformation. pub damping: bool, + /// Whether the group's shape constraints must be solved serially: per-cluster shape matching can + /// give one particle several constraints (whole body + clusters), which must not be claimed by + /// different workers. + pub shape_serial: bool, } + +/// One active soft-body cluster for the step: its proxy's solver slot is a virtual rigid body whose +/// velocity is gathered from the cluster's particles before the joints solve and whose velocity +/// change is scattered back after. A cluster is active when a joint, impulse or force acts on it. +#[derive(Copy, Clone)] +pub(crate) struct SoftClusterRecord { + /// Index of the cluster's soft body in `awake`. + pub awake: u32, + /// Index of the cluster in its soft body's cluster list. + pub cluster: u32, + /// The proxy's solver-body slot. + pub slot: u32, + /// Weighted centroid of the free particles at the last gather (the scatter's torque + /// reference). + pub com: Vector, + /// The proxy-slot velocity already accounted in the particles: the scatter distributes + /// `slot velocity - ref_vel`, the gather re-derives it. + pub ref_vel: SolverVel, +} + /// The soft-body constraints of one step. #[derive(Default)] pub(crate) struct SoftConstraintsSet { @@ -132,6 +161,8 @@ pub(crate) struct SoftConstraintsSet { pub contacts: Vec, /// The particle attachments of the awake soft bodies, group-major. pub attachments: Vec, + /// The active clusters (a joint or an external impulse acts on their proxy), group-major. + pub clusters: Vec, /// The contact rows chunked by body pair (see `soft_contact_chunks`): row indices into /// `contacts` chunk by chunk, the chunks' ranges into them (group-major, parallel colors /// then the serial tail), and the chunk ranges of the parallel colors. diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs index d013fdfe7..7c8bc431b 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs @@ -246,6 +246,9 @@ impl SoftConstraintsSet { }; g.colors = colors_start..self.color_ranges.len(); g.shape_constraints = shape_start..shape_cursor; + g.shape_serial = self.awake[awake_start..awake_cursor] + .iter() + .any(|a| !a.awake_clusters.is_empty()); g.volume_constraints = volume_start..self.volume_constraints.len(); g.damping = damping; } @@ -465,6 +468,7 @@ impl SoftConstraintsSet { plastic_flow: AtomicBool::new(false), torn: AtomicBool::new(false), contact_approach_speed: None, + awake_clusters, }); } } @@ -505,7 +509,11 @@ impl SoftConstraintsSet { match sb.cell_model { SoftBodyCellModel::Volume => counts[c.color as usize] += 1, SoftBodyCellModel::Corotational | SoftBodyCellModel::NeoHookean => { - if !sb.uses_fem() && mu > 0.0 && elastic_cell_terms(sb, slots, c).2 > 0.0 { + if !sb.uses_fem() + && mu * c.stiffness_scale > 0.0 + && elastic_cell_terms(sb, slots, c).2 > 0.0 + { + elastic_counts[c.color as usize] += 1; } } } @@ -524,6 +532,17 @@ impl SoftConstraintsSet { let sb = unsafe { &*awake.ptr }; let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; let mut count = 0; + for cluster in &sb.clusters { + if cluster.is_live() && cluster.shape_matching { + count += cluster + .particles() + .iter() + .filter(|&&v| { + slots[v as usize] != u32::MAX && sb.particles[v as usize].inv_mass != 0.0 + }) + .count(); + } + } count } @@ -681,6 +700,8 @@ impl SoftConstraintsSet { // ω² = k · Σ w |∇C|²; Neo-Hookean damping/snap-back caps use (2μ + λ)V₀ / 4μV₀. let vol = c.rest_volume.abs(); let (coeffs, im, w_vol) = elastic_cell_terms(sb, slots, c); + // Per-cell stiffness scale (regional materials through the clusters). + let (mu, lambda) = (mu * c.stiffness_scale, lambda * c.stiffness_scale); if w_vol <= 0.0 || mu <= 0.0 { // Every particle pinned, a degenerate cell, or no material. continue; @@ -805,6 +826,34 @@ impl SoftConstraintsSet { let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; let coeffs_of = |c: &SpringCoefficients| (c.erp_inv_dt(dt), c.cfm_coeff(dt)); let (shape_erp, shape_cfm) = coeffs_of(&sb.material.shape_matching_softness); + // Per-cluster shape-matching constraints, in cluster order (warm-started from the cluster's + // own impulse store). + for (ci, cluster) in sb.clusters.iter().enumerate() { + if !cluster.is_live() || !cluster.shape_matching { + continue; + } + for (k, &v) in cluster.particles().iter().enumerate() { + let pi = v as usize; + let p = &sb.particles[pi]; + let s = slots[pi]; + if s == u32::MAX || p.inv_mass == 0.0 { + continue; + } + out.push_shape_constraint(SoftShapeConstraint { + particle: v, + cluster: ci as u32, + solver_id: s, + im: Vector::splat(p.inv_mass), + goal: p.position, + goal_vel: Vector::ZERO, + erp_inv_dt: shape_erp, + cfm_coeff: shape_cfm, + inv_lhs: Vector::ZERO, + cfm_gain: Vector::ZERO, + rhs: Vector::ZERO, + impulse: cluster.shape_impulses.get(k).copied().unwrap_or(Vector::ZERO), + }); + } } } } diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs index 694299440..9b3456447 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs @@ -61,6 +61,7 @@ impl SoftFemSystem { } else { 0.0 }; + let (mu, lambda) = (mu * c.stiffness_scale, lambda * c.stiffness_scale); let coeffs = SoftElasticConstraint::coefficients(&c.inv_rest_matrix); let inv_mass: [Real; MAX_CONSTRAINT_PARTICLES] = core::array::from_fn(|k| sb.particles[c.vertices[k] as usize].inv_mass); diff --git a/src/dynamics/solver/staged_island_solver/init.rs b/src/dynamics/solver/staged_island_solver/init.rs index 29536d150..d432c4fd0 100644 --- a/src/dynamics/solver/staged_island_solver/init.rs +++ b/src/dynamics/solver/staged_island_solver/init.rs @@ -214,6 +214,23 @@ impl StagedIslandSolver { ); self.soft_constraints .assemble_attachments(island_id, bodies, group_dt); + + // Active clusters: the proxies with a joint this step (or a pending external + // impulse) get the per-pass gather/scatter stages. + let mut jointed_proxies: parry::utils::hashmap::HashMap = Default::default(); + for ji in joint_indices { + let joint = &impulse_joints[*ji].weight; + for handle in [joint.body1, joint.body2] { + if let Some(rb) = bodies.get(handle) { + if rb.is_soft_frame() && rb.ids.active_set_id != u32::MAX { + jointed_proxies.insert(rb.ids.active_set_id, ()); + } + } + } + } + let vs = &self.velocity_solver; + self.soft_constraints + .assemble_clusters(bodies, colliders, &vs.solver_bodies, &jointed_proxies); } // Avoid spawning more workers than there is work to distribute. diff --git a/src/dynamics/solver/staged_island_solver/joints.rs b/src/dynamics/solver/staged_island_solver/joints.rs index 2c5772d0c..f702f1225 100644 --- a/src/dynamics/solver/staged_island_solver/joints.rs +++ b/src/dynamics/solver/staged_island_solver/joints.rs @@ -12,6 +12,7 @@ use crate::dynamics::solver::reset_buffer; use crate::dynamics::{ JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyHandle, RigidBodySet, }; +use crate::dynamics::solver::joint_constraint::{joint_uses_angular_axes, soft_frame_angular_degenerate}; use parry::math::SIMD_WIDTH; use super::{GroupJointRanges, JOINT_BATCH, LAYOUT_REF_WORKERS, StagedIslandSolver}; @@ -65,20 +66,20 @@ impl StagedIslandSolver { .velocity_constraints_builder .par_iter_mut() .with_min_len(64) - .for_each(|builder| builder.refresh_warmstart_seeds(impulse_joints)); + .for_each(|builder| builder.update_warmstart_seeds(impulse_joints)); joints .simd_velocity_constraints_builder .par_iter_mut() .with_min_len(16) - .for_each(|builder| builder.refresh_warmstart_seeds(impulse_joints)); + .for_each(|builder| builder.update_warmstart_seeds(impulse_joints)); } #[cfg(not(feature = "parallel"))] { for builder in &mut joints.velocity_constraints_builder { - builder.refresh_warmstart_seeds(impulse_joints); + builder.update_warmstart_seeds(impulse_joints); } for builder in &mut joints.simd_velocity_constraints_builder { - builder.refresh_warmstart_seeds(impulse_joints); + builder.update_warmstart_seeds(impulse_joints); } } } @@ -116,7 +117,7 @@ impl StagedIslandSolver { self.joint_color_ranges.clear(); self.joint_chunk_lanes.clear(); self.joint_chunk_rows.clear(); - self.joint_overflow_scratch.clear(); + self.joint_overflow_workspace.clear(); self.staged_group_joint_layout.clear(); // Calibrated for LAYOUT_REF_WORKERS, NOT the pool size (see that constant's docs). @@ -181,30 +182,36 @@ impl StagedIslandSolver { let colors_start = self.joint_color_ranges.len(); let chunks_start = self.joint_chunk_lanes.len(); for (color_bit, subset) in ¶llel_by_group[gi] { - self.joint_sig_scratch.clear(); + self.joint_sig_workspace.clear(); for joint_i in subset { let joint = &impulse_joints[*joint_i].weight; - if joint.data.supports_simd_constraints() { - self.joint_sig_scratch + + // No SIMD for joints whose angular part may be masked out; rare enough. + let ang_maskable = joint_uses_angular_axes( + &joint.data, + ) && (soft_frame_angular_degenerate(&bodies[joint.body1]) + || soft_frame_angular_degenerate(&bodies[joint.body2])); + if joint.data.supports_simd_constraints() && !ang_maskable { + self.joint_sig_workspace .push((joint.data.simd_row_signature(), *joint_i)); } else { scalar_by_group[gi].push(*joint_i); } } - self.joint_sig_scratch.sort_by_key(|(sig, _)| *sig); + self.joint_sig_workspace.sort_by_key(|(sig, _)| *sig); let chunk_start = self.joint_chunk_lanes.len(); let mut run_start = 0; - while run_start < self.joint_sig_scratch.len() { - let sig = self.joint_sig_scratch[run_start].0; + while run_start < self.joint_sig_workspace.len() { + let sig = self.joint_sig_workspace[run_start].0; let mut run_end = run_start + 1; - while run_end < self.joint_sig_scratch.len() - && self.joint_sig_scratch[run_end].0 == sig + while run_end < self.joint_sig_workspace.len() + && self.joint_sig_workspace[run_end].0 == sig { run_end += 1; } for chunk in - self.joint_sig_scratch[run_start..run_end].chunks(SIMD_WIDTH) + self.joint_sig_workspace[run_start..run_end].chunks(SIMD_WIDTH) { let mut lanes = [chunk[0].1; SIMD_WIDTH]; for (l, (_, joint_i)) in chunk.iter().enumerate() { @@ -334,7 +341,7 @@ impl StagedIslandSolver { self.joint_color_ranges.clear(); self.joint_chunk_lanes.clear(); self.joint_chunk_rows.clear(); - self.joint_overflow_scratch.clear(); + self.joint_overflow_workspace.clear(); // Calibrated for LAYOUT_REF_WORKERS, NOT the pool size (see that constant's docs). let min_color_joints = JOINT_BATCH * LAYOUT_REF_WORKERS / 2; @@ -350,34 +357,39 @@ impl StagedIslandSolver { // Color 128 is the "couldn't color" bucket: always serial overflow. let parallel = color_bit < 128 && color.len() >= min_color_joints; if !parallel { - self.joint_overflow_scratch.extend_from_slice(color); + self.joint_overflow_workspace.extend_from_slice(color); continue; } { - self.joint_sig_scratch.clear(); + self.joint_sig_workspace.clear(); for joint_i in color { let joint = &impulse_joints[*joint_i].weight; - if joint.data.supports_simd_constraints() { - self.joint_sig_scratch + // No SIMD for joints whose angular part may be masked out; rare enough. + let ang_maskable = joint_uses_angular_axes( + &joint.data, + ) && (soft_frame_angular_degenerate(&bodies[joint.body1]) + || soft_frame_angular_degenerate(&bodies[joint.body2])); + if joint.data.supports_simd_constraints() && !ang_maskable { + self.joint_sig_workspace .push((joint.data.simd_row_signature(), *joint_i)); } else { - self.joint_overflow_scratch.push(*joint_i); + self.joint_overflow_workspace.push(*joint_i); } } - self.joint_sig_scratch.sort_by_key(|(sig, _)| *sig); + self.joint_sig_workspace.sort_by_key(|(sig, _)| *sig); let chunk_start = self.joint_chunk_lanes.len(); let mut run_start = 0; - while run_start < self.joint_sig_scratch.len() { - let sig = self.joint_sig_scratch[run_start].0; + while run_start < self.joint_sig_workspace.len() { + let sig = self.joint_sig_workspace[run_start].0; let mut run_end = run_start + 1; - while run_end < self.joint_sig_scratch.len() - && self.joint_sig_scratch[run_end].0 == sig + while run_end < self.joint_sig_workspace.len() + && self.joint_sig_workspace[run_end].0 == sig { run_end += 1; } - for chunk in self.joint_sig_scratch[run_start..run_end].chunks(SIMD_WIDTH) { + for chunk in self.joint_sig_workspace[run_start..run_end].chunks(SIMD_WIDTH) { let mut lanes = [chunk[0].1; SIMD_WIDTH]; for (l, (_, joint_i)) in chunk.iter().enumerate() { lanes[l] = *joint_i; @@ -459,12 +471,12 @@ impl StagedIslandSolver { unsafe { reset_buffer( &mut joints.velocity_constraints_builder, - self.joint_overflow_scratch.len(), + self.joint_overflow_workspace.len(), ); } let overflow_start = num_builders; - for joint_i in &self.joint_overflow_scratch { + for joint_i in &self.joint_overflow_workspace { let joint = &impulse_joints[*joint_i].weight; let row_start = num_rows; JointConstraintBuilder::generate( diff --git a/src/dynamics/solver/staged_island_solver/solve.rs b/src/dynamics/solver/staged_island_solver/solve.rs index 9c128d3db..108f0583e 100644 --- a/src/dynamics/solver/staged_island_solver/solve.rs +++ b/src/dynamics/solver/staged_island_solver/solve.rs @@ -59,6 +59,22 @@ pub(super) unsafe fn solve_pass( stage = sync.sync(stage, stage_work); } + /* + * Stage: gather the active soft-body clusters (worker 0, serial): each cluster's proxy slot is + * updated from its particles' velocities (keeping unscattered impulses) so the joints below + * pull on the cluster's true rigid state. Runs in both passes, like the joints. + */ + let has_clusters = !soft_ref.is_empty() && soft_ref.group_has_clusters(group_index); + if has_clusters { + if worker_id == 0 { + let soft = unsafe { &mut *ctx.soft_constraints }; + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + soft.gather_clusters(group_index, solver_bodies); + sync.complete(stage, 1, 1); + } + stage = sync.sync(stage, 1); + } + // Optionally, friction can be solved only in the unbiased pass // ("no friction when applying bias"), unless there is no unbiased pass. let solve_friction = wo_bias @@ -336,6 +352,21 @@ pub(super) unsafe fn solve_pass( stage = sync.sync(stage, 1); } + /* + * Stage: scatter the active clusters' proxy velocity changes (this pass's joint impulses and + * any external impulse) onto their particles as a rigid field (worker 0, serial: overlapping + * clusters share particles). + */ + if has_clusters { + if worker_id == 0 { + let soft = unsafe { &mut *ctx.soft_constraints }; + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + soft.scatter_clusters(group_index, solver_bodies); + sync.complete(stage, 1, 1); + } + stage = sync.sync(stage, 1); + } + stage } @@ -383,7 +414,7 @@ unsafe fn solve_soft_constraints( stage = sync.sync(stage, virt.end); } - if !sg.shape_rows.is_empty() { + if !sg.shape_constraints.is_empty() && !sg.shape_serial { let mut done = 0; while let Some(claimed) = sync.claim( stage, @@ -402,6 +433,17 @@ unsafe fn solve_soft_constraints( } sync.complete(stage, done, sg.shape_constraints.len()); stage = sync.sync(stage, sg.shape_constraints.len()); + } else if !sg.shape_constraints.is_empty() { + // Per-cluster shape matching can give one particle several constraints: solved serially by + // worker 0 (cluster shape constraints are few). + if worker_id == 0 { + let soft = unsafe { &mut *ctx.soft_constraints }; + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + for constraint_id in sg.shape_constraints.clone() { + } + sync.complete(stage, 1, 1); + } + stage = sync.sync(stage, 1); } if sg.serial.len() > 0 || !sg.volume_rows.is_empty() { diff --git a/src/dynamics/solver/staged_island_solver/worker.rs b/src/dynamics/solver/staged_island_solver/worker.rs index 8f1409f7f..944fe63c2 100644 --- a/src/dynamics/solver/staged_island_solver/worker.rs +++ b/src/dynamics/solver/staged_island_solver/worker.rs @@ -950,7 +950,9 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { // NOTE: if it's a position-based kinematic body, don't writeback as we want // to preserve exactly the value given by the user (it might not be // exactly equal to the integrated position because of rounding errors). - if rb.body_type != RigidBodyType::KinematicPositionBased { + // A soft-frame proxy's pose is not integrated either: the soft-body sync + // re-derives it exactly from the particles at the end of the step. + if rb.body_type != RigidBodyType::KinematicPositionBased && !rb.is_soft_frame() { let local_com = -rb.mprops.local_mprops.local_com; rb.pos.next_position = solver_poses.pose().prepend_translation(local_com); } @@ -958,7 +960,7 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { // Capture the post-solve velocity used by the CCD sweep — for *every* dynamic // body (fast bodies get CCD vs fixed colliders by default), skipped entirely // when CCD is globally off (`max_ccd_substeps == 0`) so those users pay nothing. - if base_params.max_ccd_substeps != 0 && rb.is_dynamic() { + if base_params.max_ccd_substeps != 0 && rb.is_dynamic() && !rb.is_soft_frame() { rb.ccd_vels = rb .pos .interpolate_velocity(base_params.inv_dt(), rb.local_center_of_mass()); diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 0940e5745..d0ef780bf 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -99,11 +99,25 @@ impl Collider { /// Moves a soft-body particle ball to its particle's world position (a simulation-driven /// motion, like a deformation: no wake-up, no narrow-phase workspace invalidation). `frame` /// is the parent proxy's pose: the ball's local pose is kept consistent with it. - pub(crate) fn deform_position(&mut self, position: Vector) { + pub(crate) fn deform_position(&mut self, frame: &Pose, position: Vector) { + let world = Pose::from_translation(position); if let Some(parent) = self.parent.as_mut() { - parent.pos_wrt_parent = Pose::from_translation(position); + parent.pos_wrt_parent = frame.inverse() * world; } - self.pos = ColliderPosition(Pose::from_translation(position)); + self.pos = ColliderPosition(world); + self.changes.insert(ColliderChanges::DEFORMED); + } + + /// Moves a soft-body surface collider along with its cluster proxy (`frame` is the proxy's + /// pose, the collider keeps its pose relative to it), as a deformation: no wake-up, no + /// workspace invalidation. The shape's vertices are expressed in the resulting frame. + pub(crate) fn deform_pose(&mut self, frame: Pose) { + let pos_wrt_parent = self + .parent + .as_ref() + .map_or(Pose::IDENTITY, |parent| parent.pos_wrt_parent); + self.pos = ColliderPosition(frame * pos_wrt_parent); + self.changes.insert(ColliderChanges::DEFORMED); } pub(crate) fn effective_contact_force_event_threshold(&self) -> Real { diff --git a/src/geometry/collider_components.rs b/src/geometry/collider_components.rs index b2e9b862e..d61979323 100644 --- a/src/geometry/collider_components.rs +++ b/src/geometry/collider_components.rs @@ -336,6 +336,10 @@ impl ActiveCollisionTypes { // // Because that test must be symmetric, we perform two similar tests by swapping // rb_type1 and rb_type2. + + // Soft-frame proxies collide as dynamic bodies. + let rb_type1 = if rb_type1.is_soft_frame() { RigidBodyType::Dynamic } else { rb_type1 }; + let rb_type2 = if rb_type2.is_soft_frame() { RigidBodyType::Dynamic } else { rb_type2 }; ((self.bits() >> (rb_type1 as u32 * 4)) & 0b0000_1111) & (1 << rb_type2 as u32) != 0 || ((self.bits() >> (rb_type2 as u32 * 4)) & 0b0000_1111) & (1 << rb_type1 as u32) != 0 } diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 1e2ea3bf6..7d304cf1f 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -1,7 +1,7 @@ use crate::alloc_prelude::*; use crate::data::arena::Arena; use crate::data::{HasModifiedFlag, ModifiedObjects}; -use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet}; +use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet, SoftBodySet}; use crate::geometry::{Collider, ColliderChanges, ColliderHandle, ColliderParent}; use crate::math::Pose; use core::ops::{Index, IndexMut}; @@ -335,6 +335,27 @@ impl ColliderSet { /// } /// ``` pub fn remove( + &mut self, + handle: ColliderHandle, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + soft_bodies: &mut SoftBodySet, + wake_up: bool, + ) -> Option { + let collider = self.remove_internal(handle, islands, bodies, wake_up)?; + // A removed deformable collider takes its mesh with it: the soft body keeps simulating, + // without that mesh's collisions. + if let Some(sb_handle) = collider.soft_body_surface() { + if let Some(sb) = soft_bodies.get_mut(sb_handle) { + sb.clear_mesh_collider(handle); + } + } + Some(collider) + } + + /// Like [`Self::remove`], for internal callers that maintain the soft-body back-references + /// themselves. + pub(crate) fn remove_internal( &mut self, handle: ColliderHandle, islands: &mut IslandManager, diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs index 9bf282654..9cdf2aa05 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs @@ -76,6 +76,7 @@ impl EdgeScan<'_> { for &v in element { aabb.take_point(mesh.cached_vertex(v as usize)); } + let aabb = aabb.loosened(self.reach).transform_by(&self.other_inv_pose); for j in self.other_bvh.intersect_aabb(&aabb) { let i = i as u32; if self.is_self { diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs index 07b9d81ed..5871e2e55 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs @@ -418,7 +418,9 @@ impl DebugRenderPipeline { let coeff = self.body_color_multiplier(parent, co.is_enabled(), true); let c = match parent.body_type { RigidBodyType::Fixed => self.style.collider_fixed_color, - RigidBodyType::Dynamic => self.style.collider_dynamic_color, + RigidBodyType::Dynamic | RigidBodyType::SoftFrame => { + self.style.collider_dynamic_color + } RigidBodyType::KinematicPositionBased | RigidBodyType::KinematicVelocityBased => { self.style.collider_kinematic_color diff --git a/src/pipeline/debug_render_pipeline/debug_render_style.rs b/src/pipeline/debug_render_pipeline/debug_render_style.rs index 0c5cd9d77..6eeeade84 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_style.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_style.rs @@ -59,6 +59,8 @@ pub struct DebugRenderStyle { pub contact_normal_length: Real, /// The color of the soft bodies' elements (structural edges, cell edges). pub soft_body_element_color: DebugColor, + /// Color of the soft-body cluster frames (the joint anchors' axes). + pub soft_body_frame_color: DebugColor, /// The color of the colliders' [`Aabb`](crate::geometry::Aabb)s. pub collider_aabb_color: DebugColor, } @@ -86,6 +88,7 @@ impl Default for DebugRenderStyle { contact_normal_color: [0.0, 1.0, 1.0, 1.0], contact_normal_length: 0.3, soft_body_element_color: [200.0, 0.8, 0.5, 1.0], + soft_body_frame_color: [40.0, 0.9, 0.6, 1.0], collider_aabb_color: [124.0, 1.0, 0.4, 1.0], } } diff --git a/src/pipeline/physics_pipeline/test.rs b/src/pipeline/physics_pipeline/test.rs index 622a606e6..d69a5ac34 100644 --- a/src/pipeline/physics_pipeline/test.rs +++ b/src/pipeline/physics_pipeline/test.rs @@ -86,6 +86,7 @@ fn rigid_body_removal_before_step() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, true, ); } @@ -111,6 +112,7 @@ fn rigid_body_removal_before_step() { #[test] fn rigid_body_removal_snapshot_handle_determinism() { let mut colliders = ColliderSet::new(); + let mut soft_bodies = SoftBodySet::new(); let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); let mut islands = IslandManager::new(); @@ -127,6 +129,7 @@ fn rigid_body_removal_snapshot_handle_determinism() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, true, ); bodies.remove( @@ -135,6 +138,7 @@ fn rigid_body_removal_snapshot_handle_determinism() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, true, ); bodies.remove( @@ -143,6 +147,7 @@ fn rigid_body_removal_snapshot_handle_determinism() { &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, true, ); @@ -283,13 +288,14 @@ fn collider_removal_before_step() { let b_handle = bodies.insert(body); let collider = ColliderBuilder::ball(1.0).build(); let c_handle = colliders.insert_with_parent(collider, b_handle, &mut bodies); - colliders.remove(c_handle, &mut islands, &mut bodies, true); + colliders.remove(c_handle, &mut islands, &mut bodies, &mut soft_bodies, true); bodies.remove( b_handle, &mut islands, &mut colliders, &mut impulse_joints, &mut multibody_joints, + &mut soft_bodies, true, ); diff --git a/src/pipeline/physics_world.rs b/src/pipeline/physics_world.rs index a9ce0778e..5bdd583de 100644 --- a/src/pipeline/physics_world.rs +++ b/src/pipeline/physics_world.rs @@ -223,6 +223,7 @@ impl PhysicsWorld { &mut self.colliders, &mut self.impulse_joints, &mut self.multibody_joints, + &mut self.soft_bodies, true, ) } @@ -286,7 +287,13 @@ impl PhysicsWorld { /// Returns the removed collider, or `None` if the handle was invalid. pub fn remove_collider(&mut self, handle: ColliderHandle) -> Option { self.colliders - .remove(handle, &mut self.islands, &mut self.bodies, true) + .remove( + handle, + &mut self.islands, + &mut self.bodies, + &mut self.soft_bodies, + true, + ) } // ── Impulse joints ────────────────────────────────────────────────── @@ -410,6 +417,37 @@ impl PhysicsWorld { ) } + /// Adds a cluster to a soft body: a set of its particles backed by a fresh proxy rigid + /// body ([`RigidBodyType::SoftFrame`]) that impulse joints and colliders can attach to. + /// Returns the cluster's index (see [`SoftBodySet::add_cluster`]). + pub fn add_soft_body_cluster( + &mut self, + handle: SoftBodyHandle, + particles: &[u32], + ) -> Option { + self.soft_bodies + .add_cluster(handle, particles, &mut self.bodies, &mut self.colliders) + } + + /// Removes a soft body's cluster, its proxy rigid body, and the particles only that + /// cluster covered (see [`SoftBodySet::remove_cluster`]). Removing the proxy from the + /// [`RigidBodySet`] is equivalent. + pub fn remove_soft_body_cluster( + &mut self, + handle: SoftBodyHandle, + cluster: u32, + ) -> Option { + self.soft_bodies.remove_cluster( + handle, + cluster, + &mut self.islands, + &mut self.bodies, + &mut self.colliders, + &mut self.impulse_joints, + &mut self.multibody_joints, + ) + } + /// Iterate over every soft body in the world. pub fn soft_bodies(&self) -> impl Iterator { self.soft_bodies.iter() From 2e215bdaf76c2e991d773930c3b47b6606e207dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 3 Sep 2026 11:13:09 +0200 Subject: [PATCH 05/32] feat(testbed): add soft joints demos, mouse dragging of dynamic objects and a Debug tab, fix the camera view axis --- examples2d/all_examples2.rs | 2 + examples2d/soft_joints2.rs | 286 ++++++++++++++++++++++++++++ examples3d/all_examples3.rs | 2 + examples3d/soft_joints3.rs | 358 +++++++++++++++++++++++++++++++++++ src_testbed/debug_render.rs | 68 ++++++- src_testbed/grab.rs | 252 ++++++++++++++++++++++++ src_testbed/lib.rs | 1 + src_testbed/mouse.rs | 5 + src_testbed/testbed/state.rs | 2 + src_testbed/ui.rs | 206 +++++++++++++++++++- src_testbed/viewer.rs | 122 ++++++++++-- 11 files changed, 1278 insertions(+), 26 deletions(-) create mode 100644 examples2d/soft_joints2.rs create mode 100644 examples3d/soft_joints3.rs create mode 100644 src_testbed/grab.rs diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index f78102894..6a89a92b8 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -59,6 +59,7 @@ mod soft_blobs2; mod soft_bodies2; mod soft_fem2; mod soft_jelly2; +mod soft_joints2; // The letters come from a tessellated SVG (usvg), which doesn't build for wasm. #[cfg(not(target_arch = "wasm32"))] mod soft_letters2; @@ -147,6 +148,7 @@ pub async fn main() { SOFT, "Plasticity", soft_plasticity2::run; SOFT, "Tearing", soft_tearing2::run; SOFT, "Soft FEM", soft_fem2::run; + SOFT, "Soft joints", soft_joints2::run; // ── Controls ──────────────────────────────────────────────────────── CONTROLS, "Character controller", character_controller2::run; // ── Debug ─────────────────────────────────────────────────────────── diff --git a/examples2d/soft_joints2.rs b/examples2d/soft_joints2.rs new file mode 100644 index 000000000..f301e75f5 --- /dev/null +++ b/examples2d/soft_joints2.rs @@ -0,0 +1,286 @@ +//! Every 2D impulse-joint type attached to soft bodies through their cluster proxies: joints on +//! jellies, a motorized hinge between two clusters of one soft bar, a multibody arm plus a rope +//! joint on one jelly, and a kinematic cluster waved with `set_cluster_kinematic_target`. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(30.0, 0.5), + ); + + let jelly = |center: Vector, half: Vector, young: Real| { + SoftBodyBuilder::grid(center, half, 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.8, + ..Default::default() + }) + .particle_mass(0.08) + .particle_radius(0.06) + .collider_template(ColliderBuilder::ball(0.06).friction(0.6)) + }; + + /* + * Revolute + velocity motor: a jelly spun at its whole-body frame. + */ + let spinner = world.insert_soft_body(jelly(Vector::new(-12.0, 2.0), Vector::splat(0.6), 8.0e3)); + let spinner_root = world.soft_bodies[spinner].root_body(); + let com = world.soft_bodies[spinner].center_of_mass(); + let pivot = world.insert_body(RigidBodyBuilder::fixed().translation(com)); + world.insert_impulse_joint( + pivot, + spinner_root, + RevoluteJointBuilder::new().motor_velocity(1.5, 60.0), + ); + + /* + * Fixed: a rigid plate welded onto a jelly's top-edge cluster; the full-rank edge cluster + * holds the plate's orientation as the jelly wobbles. + */ + let wobbler = world.insert_soft_body(jelly(Vector::new(-8.0, 0.61), Vector::splat(0.6), 2.5e3)); + // The top edge of the particle grid: the row with the highest y. + let top_edge: Vec = { + let sb = &world.soft_bodies[wobbler]; + let max_y = sb + .particle_positions() + .map(|p| p.y) + .fold(0.0, |a: Real, b| a.max(b)); + (0..sb.num_particles() as u32) + .filter(|&v| (sb.particle_position(v as usize).y - max_y).abs() < 1.0e-3) + .collect() + }; + let top_cluster = world + .add_soft_body_cluster(wobbler, &top_edge) + .expect("top-edge cluster"); + let top_proxy = world.soft_bodies[wobbler].cluster_proxy(top_cluster).unwrap(); + let top_pos = world.bodies[top_proxy].position().translation; + let (plate, _) = world.insert( + RigidBodyBuilder::dynamic().translation(top_pos + Vector::new(0.0, 0.12)), + ColliderBuilder::cuboid(0.7, 0.06).density(0.4), + ); + world.insert_impulse_joint( + plate, + top_proxy, + FixedJointBuilder::new().local_anchor1(Vector::new(0.0, -0.12)), + ); + // A small crate dropped on the plate to make it wobble. + world.insert( + RigidBodyBuilder::dynamic().translation(top_pos + Vector::new(0.3, 1.4)), + ColliderBuilder::cuboid(0.15, 0.15).density(1.5), + ); + + /* + * Prismatic + limits + position motor: a jelly shuttled along a rail between two stops + * (the motor target oscillates in the render loop). + */ + let shuttle = world.insert_soft_body(jelly(Vector::new(-3.0, 0.85), Vector::splat(0.4), 6.0e3)); + let shuttle_root = world.soft_bodies[shuttle].root_body(); + let shuttle_com = world.soft_bodies[shuttle].center_of_mass(); + let rail = world.insert_body(RigidBodyBuilder::fixed().translation(shuttle_com)); + let rail_joint = world.insert_impulse_joint( + rail, + shuttle_root, + PrismaticJointBuilder::new(Vector::X) + .limits([-1.8, 1.8]) + .motor_position(0.0, 40.0, 8.0), + ); + + /* + * Rope: two jellies chained over a ledge, one dragging the other. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(1.5, 1.0)), + ColliderBuilder::cuboid(1.2, 1.0), + ); + let anchor_jelly = world.insert_soft_body(jelly(Vector::new(1.5, 2.6), Vector::splat(0.5), 1.2e4)); + let hanging_jelly = + world.insert_soft_body(jelly(Vector::new(3.8, 2.6), Vector::splat(0.5), 1.2e4)); + world.insert_impulse_joint( + world.soft_bodies[anchor_jelly].root_body(), + world.soft_bodies[hanging_jelly].root_body(), + RopeJointBuilder::new(2.2), + ); + + /* + * Spring: a bungee jelly bouncing under a gantry. + */ + let bungee = world.insert_soft_body(jelly(Vector::new(6.5, 3.2), Vector::splat(0.45), 6.0e3)); + let gantry = world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(6.5, 5.5))); + world.insert_impulse_joint( + gantry, + world.soft_bodies[bungee].root_body(), + SpringJointBuilder::new(1.2, 25.0, 1.5), + ); + + /* + * Pin-slot: a jelly bead sliding and spinning along a vertical pole, caught by the slot + * limits. + */ + let pole_pos = Vector::new(9.5, 2.5); + // The pole is visual only: the bead's particles would otherwise rest on it. + world.insert( + RigidBodyBuilder::fixed().translation(pole_pos), + ColliderBuilder::cuboid(0.05, 2.5).collision_groups(InteractionGroups::none()), + ); + let bead = world.insert_soft_body(jelly(pole_pos + Vector::new(0.0, 1.7), Vector::splat(0.35), 8.0e3)); + let pole = world.insert_body(RigidBodyBuilder::fixed().translation(pole_pos)); + world.insert_impulse_joint( + pole, + world.soft_bodies[bead].root_body(), + PinSlotJointBuilder::new(Vector::Y).limits([-1.8, 1.8]), + ); + + /* + * Same-soft-body joint: a motorized revolute hinges two disjoint half clusters of one soft + * bar at its middle, so the body folds and flaps at its own hinge. + */ + let bar = world.insert_soft_body( + SoftBodyBuilder::grid(Vector::new(13.5, 3.0), Vector::new(1.0, 0.22), 9, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.35, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .particle_mass(0.08) + .particle_radius(0.06) + .collider_template(ColliderBuilder::ball(0.06)), + ); + let (left_half, right_half): (Vec, Vec) = { + let sb = &world.soft_bodies[bar]; + let mid = sb.center_of_mass().x; + let mut left = Vec::new(); + let mut right = Vec::new(); + for v in 0..sb.num_particles() as u32 { + if sb.particle_position(v as usize).x < mid - 1.0e-3 { + left.push(v); + } else if sb.particle_position(v as usize).x > mid + 1.0e-3 { + right.push(v); + } + } + (left, right) + }; + let left_cluster = world.add_soft_body_cluster(bar, &left_half).expect("left half"); + let right_cluster = world + .add_soft_body_cluster(bar, &right_half) + .expect("right half"); + let left_proxy = world.soft_bodies[bar].cluster_proxy(left_cluster).unwrap(); + let right_proxy = world.soft_bodies[bar].cluster_proxy(right_cluster).unwrap(); + let bar_com = world.soft_bodies[bar].center_of_mass(); + let left_pos = world.bodies[left_proxy].position().translation; + let right_pos = world.bodies[right_proxy].position().translation; + // Hold the bar's left half in the air, then flap the right half about the middle hinge. + let bar_anchor = world.insert_body(RigidBodyBuilder::fixed().translation(left_pos)); + world.insert_impulse_joint(bar_anchor, left_proxy, FixedJointBuilder::new()); + let flap_joint = world.insert_impulse_joint( + left_proxy, + right_proxy, + RevoluteJointBuilder::new() + .local_anchor1(bar_com - left_pos) + .local_anchor2(bar_com - right_pos) + .motor_position(0.0, 80.0, 10.0), + ); + + /* + * Multibody + rigid on one soft body: a jelly hanging from a two-link multibody arm + * while a second joint (a rope) ties the same jelly to a rigid crate on the ground. + */ + let arm_root = world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(17.0, 6.0))); + let (link1, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(18.2, 6.0)), + ColliderBuilder::capsule_x(0.5, 0.08).density(2.0), + ); + world + .multibody_joints + .insert( + arm_root, + link1, + RevoluteJointBuilder::new() + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::new(-1.2, 0.0)), + true, + ) + .expect("arm joint 1"); + let (link2, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(19.4, 6.0)), + ColliderBuilder::capsule_x(0.5, 0.08).density(2.0), + ); + world + .multibody_joints + .insert( + link1, + link2, + RevoluteJointBuilder::new() + .local_anchor1(Vector::new(0.6, 0.0)) + .local_anchor2(Vector::new(-0.6, 0.0)), + true, + ) + .expect("arm joint 2"); + let pendulum = world.insert_soft_body(jelly(Vector::new(20.2, 5.2), Vector::splat(0.5), 5.0e3)); + let pendulum_root = world.soft_bodies[pendulum].root_body(); + world.insert_impulse_joint( + link2, + pendulum_root, + RevoluteJointBuilder::new() + .local_anchor1(Vector::new(0.7, 0.0)) + .local_anchor2(Vector::new(0.0, 0.6)), + ); + let (crate_body, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(20.2, 0.3)), + ColliderBuilder::cuboid(0.3, 0.3).density(0.5), + ); + world.insert_impulse_joint(pendulum_root, crate_body, RopeJointBuilder::new(4.2)); + + /* + * Kinematic cluster (no joint): a soft rope whose pinned top cluster is waved rigidly. + */ + let strand = world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(-16.0, 5.5), Vector::new(-16.0, 1.5), 20) + .particle_mass(0.05), + ); + let grip: Vec = vec![0, 1]; + let strand_grip = world + .add_soft_body_cluster(strand, &grip) + .expect("strand grip cluster"); + world.soft_bodies[strand].set_cluster_pinned(strand_grip, true); + let grip_home = world.soft_bodies[strand] + .cluster_proxy(strand_grip) + .map(|proxy| world.bodies[proxy].position().translation) + .unwrap_or_default(); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(2.0, 3.0), 30.0); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + t += world.integration_parameters.dt; + // The prismatic motor shuttles between its stops. + if let Some(j) = world.impulse_joints.get_mut(rail_joint, true) { + j.data + .set_motor_position(JointAxis::LinX, 1.5 * (0.6 * t).sin(), 40.0, 8.0); + } + // The same-body hinge flaps. + if let Some(j) = world.impulse_joints.get_mut(flap_joint, true) { + j.data + .set_motor_position(JointAxis::AngX, 0.8 * (1.4 * t).sin(), 80.0, 10.0); + } + // The strand's grip waves. + let grip_pose = Pose::from_parts( + grip_home + Vector::new(1.2 * (0.7 * t).sin(), 0.15 * (1.9 * t).sin()), + Rotation::new(0.5 * (1.1 * t).sin()), + ); + world.soft_bodies[strand].set_cluster_kinematic_target(strand_grip, grip_pose); + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 26fe1bfda..2b52079fb 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -82,6 +82,7 @@ mod soft_cloth_stress3; mod soft_dress3; mod soft_fem3; mod soft_jelly3; +mod soft_joints3; mod soft_pile3; mod soft_plasticity3; mod soft_surface3; @@ -167,6 +168,7 @@ pub async fn main() { SOFT, "Soft bodies", soft_bodies3::run; SOFT, "Cloth", soft_cloth3::run; SOFT, "Jelly", soft_jelly3::run; + SOFT, "Soft joints", soft_joints3::run; SOFT, "Deformable trimeshes", soft_surface3::run; SOFT, "Soft pile", soft_pile3::run; SOFT, "Thin features", soft_thin_features3::run; diff --git a/examples3d/soft_joints3.rs b/examples3d/soft_joints3.rs new file mode 100644 index 000000000..42828f120 --- /dev/null +++ b/examples3d/soft_joints3.rs @@ -0,0 +1,358 @@ +//! Every impulse-joint type attached to soft bodies through their cluster proxies: revolute, +//! spherical, fixed, prismatic, rope, spring and generic joints between jellies, cloths and rigid +//! or kinematic bodies, plus a same-soft-body hinge, a multibody arm and a kinematic cluster. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(18.0, 0.1, 18.0), + ); + + let jelly = |center: Vector, half: Vector, young: Real| { + SoftBodyBuilder::cuboid(center, half, 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.8, + ..Default::default() + }) + .particle_mass(0.08) + .particle_radius(0.06) + .collider_template(ColliderBuilder::ball(0.06).friction(0.6)) + }; + + /* + * Revolute + velocity motor: a jelly spun about Y at its whole-body frame. + */ + let spinner = world.insert_soft_body(jelly( + Vector::new(-6.0, 1.6, -4.0), + Vector::splat(0.5), + 8.0e3, + )); + let spinner_root = world.soft_bodies[spinner].root_body(); + let com = world.soft_bodies[spinner].center_of_mass(); + let pivot = world.insert_body(RigidBodyBuilder::fixed().translation(com)); + world.insert_impulse_joint( + pivot, + spinner_root, + RevoluteJointBuilder::new(Vector::Y).motor_velocity(1.5, 60.0), + ); + + /* + * Spherical: a cloth's corner cluster follows a circling kinematic mover (two-way: the + * joint feels the cloth's weight). + */ + let cloth = world.insert_soft_body(SoftBodyBuilder::cloth( + Vector::new(0.0, 2.6, 2.5), + Vector::X * 0.16, + Vector::Z * 0.16, + 12, + 12, + )); + let corner = world + .add_soft_body_cluster(cloth, &[0, 1, 12]) + .expect("corner cluster"); + let corner_proxy = world.soft_bodies[cloth].cluster_proxy(corner).unwrap(); + let corner_pos = world.bodies[corner_proxy].position().translation; + let mover = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(corner_pos)); + world.insert_impulse_joint(mover, corner_proxy, SphericalJointBuilder::new()); + + /* + * Fixed: a rigid plate welded flat onto a jelly's top-face cluster; the full-rank face + * cluster lets the weld hold the plate's orientation as the jelly wobbles. + */ + let wobbler = world.insert_soft_body(jelly( + Vector::new(-2.5, 0.61, -4.0), + Vector::splat(0.6), + 2.5e3, + )); + // The top face of a 4x4x4 particle cuboid: the 16 particles of the highest layer. + let top_face: Vec = { + let sb = &world.soft_bodies[wobbler]; + let max_y = sb + .particle_positions() + .map(|p| p.y) + .fold(0.0, |a: Real, b| a.max(b)); + (0..sb.num_particles() as u32) + .filter(|&v| (sb.particle_position(v as usize).y - max_y).abs() < 1.0e-3) + .collect() + }; + let top_cluster = world + .add_soft_body_cluster(wobbler, &top_face) + .expect("top-face cluster"); + let top_proxy = world.soft_bodies[wobbler].cluster_proxy(top_cluster).unwrap(); + let top_pos = world.bodies[top_proxy].position().translation; + let (plate, _) = world.insert( + RigidBodyBuilder::dynamic().translation(top_pos + Vector::new(0.0, 0.12, 0.0)), + ColliderBuilder::cuboid(0.7, 0.06, 0.7).density(0.4), + ); + world.insert_impulse_joint( + plate, + top_proxy, + FixedJointBuilder::new().local_anchor1(Vector::new(0.0, -0.12, 0.0)), + ); + // A small crate dropped on the plate to make it wobble. + world.insert( + RigidBodyBuilder::dynamic().translation(top_pos + Vector::new(0.3, 1.4, 0.2)), + ColliderBuilder::cuboid(0.15, 0.15, 0.15).density(1.5), + ); + + /* + * Prismatic + limits + position motor: a jelly shuttled along a rail between two stops + * (the motor target oscillates in the render loop). + */ + let shuttle = world.insert_soft_body(jelly( + Vector::new(2.0, 0.85, -4.0), + Vector::splat(0.4), + 6.0e3, + )); + let shuttle_root = world.soft_bodies[shuttle].root_body(); + let shuttle_com = world.soft_bodies[shuttle].center_of_mass(); + let rail = world.insert_body(RigidBodyBuilder::fixed().translation(shuttle_com)); + let rail_joint = world.insert_impulse_joint( + rail, + shuttle_root, + PrismaticJointBuilder::new(Vector::X) + .limits([-1.8, 1.8]) + .motor_position(0.0, 40.0, 8.0), + ); + + /* + * Rope: two jellies chained over a ledge, one dragging the other. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-1.0, 1.0, -9.0)), + ColliderBuilder::cuboid(1.6, 1.0, 1.2), + ); + let anchor_jelly = world.insert_soft_body(jelly( + Vector::new(-1.0, 2.6, -9.0), + Vector::splat(0.5), + 1.2e4, + )); + let hanging_jelly = world.insert_soft_body(jelly( + Vector::new(1.6, 2.6, -9.0), + Vector::splat(0.5), + 1.2e4, + )); + world.insert_impulse_joint( + world.soft_bodies[anchor_jelly].root_body(), + world.soft_bodies[hanging_jelly].root_body(), + RopeJointBuilder::new(2.2), + ); + + /* + * Spring: a bungee jelly bouncing under a gantry. + */ + let bungee = world.insert_soft_body(jelly( + Vector::new(5.5, 3.2, 2.5), + Vector::splat(0.45), + 6.0e3, + )); + let gantry = + world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(5.5, 5.5, 2.5))); + world.insert_impulse_joint( + gantry, + world.soft_bodies[bungee].root_body(), + SpringJointBuilder::new(1.2, 25.0, 1.5), + ); + + /* + * Generic: a jelly bead on a pole, translations locked to the pole's Y axis (limited along + * it) and every rotation free: a 3D pin-slot joint spelled as a generic joint. + */ + let pole_base = Vector::new(8.5, 0.0, -4.0); + // The pole is visual only: the bead's particles would otherwise rest on it. + world.insert( + RigidBodyBuilder::fixed().translation(pole_base + Vector::new(0.0, 2.5, 0.0)), + ColliderBuilder::cylinder(2.5, 0.05).collision_groups(InteractionGroups::none()), + ); + let bead = world.insert_soft_body(jelly( + pole_base + Vector::new(0.0, 4.2, 0.0), + Vector::splat(0.35), + 8.0e3, + )); + let pole = world + .insert_body(RigidBodyBuilder::fixed().translation(pole_base + Vector::new(0.0, 2.5, 0.0))); + world.insert_impulse_joint( + pole, + world.soft_bodies[bead].root_body(), + GenericJointBuilder::new(JointAxesMask::LIN_X | JointAxesMask::LIN_Z) + .local_axis1(Vector::Y) + .local_axis2(Vector::Y) + .limits(JointAxis::LinY, [-1.8, 1.8]), + ); + + /* + * Same-soft-body joint: one soft bar whose two disjoint half clusters are hinged by a + * motorized revolute at its middle, so the body folds and flaps at its own hinge. + */ + let bar = world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(2.5, 2.2, 5.5), Vector::new(1.0, 0.22, 0.4), 7, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.35, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .particle_mass(0.08) + .particle_radius(0.06) + .collider_template(ColliderBuilder::ball(0.06)), + ); + let (left_half, right_half): (Vec, Vec) = { + let sb = &world.soft_bodies[bar]; + let mid = sb.center_of_mass().x; + let mut left = Vec::new(); + let mut right = Vec::new(); + for v in 0..sb.num_particles() as u32 { + if sb.particle_position(v as usize).x < mid - 1.0e-3 { + left.push(v); + } else if sb.particle_position(v as usize).x > mid + 1.0e-3 { + right.push(v); + } + } + (left, right) + }; + let left_cluster = world.add_soft_body_cluster(bar, &left_half).expect("left half"); + let right_cluster = world + .add_soft_body_cluster(bar, &right_half) + .expect("right half"); + let left_proxy = world.soft_bodies[bar].cluster_proxy(left_cluster).unwrap(); + let right_proxy = world.soft_bodies[bar].cluster_proxy(right_cluster).unwrap(); + let bar_com = world.soft_bodies[bar].center_of_mass(); + let left_pos = world.bodies[left_proxy].position().translation; + let right_pos = world.bodies[right_proxy].position().translation; + // Hold the bar's left half in the air, then flap the right half about the middle hinge. + let bar_anchor = world.insert_body(RigidBodyBuilder::fixed().translation(left_pos)); + world.insert_impulse_joint(bar_anchor, left_proxy, FixedJointBuilder::new()); + let flap_joint = world.insert_impulse_joint( + left_proxy, + right_proxy, + RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(bar_com - left_pos) + .local_anchor2(bar_com - right_pos) + .motor_position(0.0, 80.0, 10.0), + ); + + /* + * Multibody + rigid on one soft body: a jelly hanging from a two-link multibody arm (the + * generic joint path) while a rope joint ties the same jelly to a rigid crate on the ground. + */ + let arm_root = + world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(7.0, 5.0, 6.0))); + let (link1, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(8.2, 5.0, 6.0)), + ColliderBuilder::capsule_x(0.5, 0.08).density(2.0), + ); + world + .multibody_joints + .insert( + arm_root, + link1, + RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::new(-1.2, 0.0, 0.0)), + true, + ) + .expect("arm joint 1"); + let (link2, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(9.4, 5.0, 6.0)), + ColliderBuilder::capsule_x(0.5, 0.08).density(2.0), + ); + world + .multibody_joints + .insert( + link1, + link2, + RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(Vector::new(0.6, 0.0, 0.0)) + .local_anchor2(Vector::new(-0.6, 0.0, 0.0)), + true, + ) + .expect("arm joint 2"); + let pendulum = world.insert_soft_body(jelly( + Vector::new(10.2, 4.2, 6.0), + Vector::splat(0.5), + 5.0e3, + )); + let pendulum_root = world.soft_bodies[pendulum].root_body(); + world.insert_impulse_joint( + link2, + pendulum_root, + SphericalJointBuilder::new() + .local_anchor1(Vector::new(0.7, 0.0, 0.0)) + .local_anchor2(Vector::new(0.0, 0.6, 0.0)), + ); + let (crate_body, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(10.2, 0.3, 6.0)), + ColliderBuilder::cuboid(0.3, 0.3, 0.3).density(0.5), + ); + world.insert_impulse_joint( + pendulum_root, + crate_body, + RopeJointBuilder::new(3.2), + ); + + /* + * Kinematic cluster (no joint): a banner whose pinned top-edge cluster is waved rigidly. + */ + 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, + )); + let top_edge: Vec = (0..14).collect(); + let banner_grip = world + .add_soft_body_cluster(banner, &top_edge) + .expect("banner grip cluster"); + world.soft_bodies[banner].set_cluster_pinned(banner_grip, true); + let grip_home = world.soft_bodies[banner] + .cluster_proxy(banner_grip) + .map(|proxy| world.bodies[proxy].position().translation) + .unwrap_or_default(); + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(13.0, 9.0, 15.0), Vec3::new(0.0, 1.5, 0.0)); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + t += world.integration_parameters.dt; + // The spherical joint's kinematic mover circles. + let target = corner_pos + + Vector::new( + 1.2 * (0.8 * t).sin(), + 0.4 * (1.3 * t).sin(), + 1.2 * (0.8 * t).cos() - 1.2, + ); + world.bodies[mover].set_next_kinematic_translation(target); + // The prismatic motor shuttles between its stops. + if let Some(j) = world.impulse_joints.get_mut(rail_joint, true) { + j.data + .set_motor_position(JointAxis::LinX, 1.5 * (0.6 * t).sin(), 40.0, 8.0); + } + // 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); + } + // The banner's grip waves. + let grip_pose = Pose::from_parts( + grip_home + Vector::new(1.5 * (0.7 * t).sin(), 0.2 * (1.9 * t).sin(), 0.0), + Rotation::from_axis_angle(Vector::X, 0.35 * (1.1 * t).sin()), + ); + world.soft_bodies[banner].set_cluster_kinematic_target(banner_grip, grip_pose); + world.step(); + } + } + Ok(()) +} diff --git a/src_testbed/debug_render.rs b/src_testbed/debug_render.rs index d3c79ec3b..290b5ed31 100644 --- a/src_testbed/debug_render.rs +++ b/src_testbed/debug_render.rs @@ -106,7 +106,7 @@ pub fn debug_render_scene( } /// Convert HSLA color to RGB -fn hsla_to_rgb(h: f32, s: f32, l: f32, a: f32) -> [f32; 4] { +pub(crate) fn hsla_to_rgb(h: f32, s: f32, l: f32, a: f32) -> [f32; 4] { if s == 0.0 { return [l, l, l, a]; } @@ -144,3 +144,69 @@ fn hue_to_rgb(p: f32, q: f32, t: f32) -> f32 { p } } + +/// The inverse of [`hsla_to_rgb`]: an RGBA color as the HSLA the debug-render style stores (hue +/// in degrees, the rest in `0..=1`). +pub(crate) fn rgb_to_hsla(rgba: [f32; 4]) -> DebugColor { + let [r, g, b, a] = rgba; + let max = r.max(g).max(b); + let min = r.min(g).min(b); + let l = (max + min) / 2.0; + let delta = max - min; + + if delta <= f32::EPSILON { + return [0.0, 0.0, l, a]; + } + + let s = if l < 0.5 { + delta / (max + min) + } else { + delta / (2.0 - max - min) + }; + let h = if max == r { + ((g - b) / delta).rem_euclid(6.0) + } else if max == g { + (b - r) / delta + 2.0 + } else { + (r - g) / delta + 4.0 + }; + + // Hue is an angle: a value a rounding error short of a full turn is zero, not 359.999. + let h = (h * 60.0).rem_euclid(360.0); + let h = if h > 360.0 - 1.0e-3 { 0.0 } else { h }; + [h, s, l, a] +} + +#[cfg(test)] +mod tests { + use super::{hsla_to_rgb, rgb_to_hsla}; + + /// The color picker shows a style color as RGB and writes it back as HSLA: the pair must + /// round-trip, or editing one color would drift every time the tab is opened. + #[test] + fn hsla_round_trips_through_rgb() { + for hsla in [ + [340.0, 1.0, 0.3, 1.0], + [20.0, 1.0, 0.3, 1.0], + [30.0, 1.0, 0.4, 0.5], + [0.0, 1.0, 0.5, 1.0], + [120.0, 0.5, 0.75, 1.0], + [240.0, 0.25, 0.5, 0.25], + [359.0, 0.9, 0.1, 1.0], + ] { + let rgba = hsla_to_rgb(hsla[0], hsla[1], hsla[2], hsla[3]); + assert!( + rgba.iter().all(|c| (0.0..=1.0).contains(c)), + "{hsla:?} left the unit cube: {rgba:?}" + ); + let back = rgb_to_hsla(rgba); + assert!( + (back[0] - hsla[0]).abs() < 0.1 + && (back[1] - hsla[1]).abs() < 1.0e-3 + && (back[2] - hsla[2]).abs() < 1.0e-3 + && (back[3] - hsla[3]).abs() < 1.0e-6, + "{hsla:?} came back as {back:?}" + ); + } + } +} diff --git a/src_testbed/grab.rs b/src_testbed/grab.rs new file mode 100644 index 000000000..c6bfb1620 --- /dev/null +++ b/src_testbed/grab.rs @@ -0,0 +1,252 @@ +//! Mouse grabbing: left-click-drag any dynamic object (rigid body, multibody link, or soft +//! body) to pull it around with a soft mouse joint, box2d-style. The grab spawns an invisible +//! kinematic body following the cursor, tied to the picked body by a motor-only joint. + +#![allow(clippy::useless_conversion)] // Conversions are needed for switching between f32/f64. +#![allow(clippy::unnecessary_cast)] + +use crate::mouse::SceneMouse; +use rapier::dynamics::{RigidBodyBuilder, RigidBodyHandle, SoftBodyHandle}; +use rapier::math::{Real, Vector}; +use rapier::pipeline::PhysicsWorld; +use rapier::prelude::{ + GenericJoint, GenericJointBuilder, JointAxesMask, JointAxis, QueryFilter, QueryFilterFlags, +}; + +#[cfg(feature = "dim3")] +use rapier::prelude::Ray; + +/// Spring gains of the mouse joint, roughly a 5 Hz near-critically-damped spring. +const GRAB_STIFFNESS: Real = 1000.0; +const GRAB_DAMPING: Real = 50.0; + +#[derive(Default)] +pub struct MouseGrab { + grabbed: Option, +} + +struct Grabbed { + /// The body the joint pulled at first: the picked body itself, or the temporary cluster's + /// proxy (a tear may have moved the joint to another proxy since, see [`Grabbed::pulled`]). + body: RigidBodyHandle, + /// Invisible kinematic body following the cursor. + mouse_body: RigidBodyHandle, + /// The mouse joint. + soft_cluster: Option<(SoftBodyHandle, u32)>, + /// Anchor of the plane the cursor ray is intersected with while dragging. + #[cfg(feature = "dim3")] + plane_point: Vector, +} + +impl MouseGrab { + pub fn active(&self) -> bool { + self.grabbed.is_some() + } + + /// World-space endpoints of the mouse joint, for drawing a cue line. + pub fn cue_line(&self, world: &PhysicsWorld) -> Option<(Vector, Vector)> { + let g = self.grabbed.as_ref()?; + let mouse = world.bodies.get(g.mouse_body)?.position().translation; + let body = world.bodies.get(g.body)?.position().translation; + Some((mouse, body)) + } + + /// Drops the grab state without touching the world (the world was replaced). + pub fn forget(&mut self) { + self.grabbed = None; + } + + /// Picks the dynamic object under the cursor and attaches the mouse joint to it. + /// Returns `true` if something was grabbed. + pub fn try_grab(&mut self, mouse: &SceneMouse, world: &mut PhysicsWorld) -> bool { + if self.grabbed.is_some() { + return false; + } + let Some((body_handle, grab_point)) = pick(mouse, world) else { + return false; + }; + if !world.bodies[body_handle].is_dynamic() { + return false; + } + + let grabbed = if let Some(sb_handle) = world.bodies[body_handle].soft_body() { + // Soft body: drag its nearest particle through a temporary one-particle cluster. + // The single particle makes the cluster rank-deficient, so the joint's angular + // axes are stripped automatically and this acts as a pure point pull. + let sb = &world.soft_bodies[sb_handle]; + let mut nearest = 0u32; + let mut best_dist = Real::MAX; + for (i, p) in sb.particle_positions().enumerate() { + let dist = (p - grab_point).length_squared(); + if dist < best_dist { + best_dist = dist; + nearest = i as u32; + } + } + let anchor = sb.particle_position(nearest as usize); + let Some(cluster) = world.add_soft_body_cluster(sb_handle, &[nearest]) else { + return false; + }; + let Some(proxy) = world.soft_bodies[sb_handle].cluster_proxy(cluster) else { + world.remove_soft_body_cluster(sb_handle, cluster); + return false; + }; + let mouse_body = world + .insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); + world.insert_impulse_joint(mouse_body, proxy, mouse_joint(Vector::ZERO)); + Grabbed { + body: proxy, + mouse_body, + soft_cluster: Some((sb_handle, cluster)), + #[cfg(feature = "dim3")] + plane_point: anchor, + } + } else { + let local_anchor = world.bodies[body_handle] + .position() + .inverse_transform_point(grab_point); + let mouse_body = world + .insert_body(RigidBodyBuilder::kinematic_position_based().translation(grab_point)); + world.insert_impulse_joint(mouse_body, body_handle, mouse_joint(local_anchor)); + Grabbed { + body: body_handle, + mouse_body, + soft_cluster: None, + #[cfg(feature = "dim3")] + plane_point: grab_point, + } + }; + + if let Some(rb) = world.bodies.get_mut(grabbed.body) { + rb.wake_up(true); + } + self.grabbed = Some(grabbed); + true + } + + /// Moves the kinematic anchor to the cursor. In 3D the cursor ray is intersected with the + /// camera-facing plane through the initial grab point, keeping the drag depth constant. + pub fn update( + &mut self, + mouse: &SceneMouse, + #[cfg(feature = "dim3")] camera_fwd: Vector, + world: &mut PhysicsWorld, + ) { + let Some(g) = &self.grabbed else { + return; + }; + // The example may have removed the body (or the whole soft body) while we held it. + if world.bodies.get(g.body).is_none() || world.bodies.get(g.mouse_body).is_none() { + self.release(world); + return; + } + + #[cfg(feature = "dim2")] + let target = match mouse.point { + Some(pt) => Vector::new(pt.x as Real, pt.y as Real), + None => return, + }; + #[cfg(feature = "dim3")] + let target = { + let Some((orig, dir)) = mouse.ray else { + return; + }; + let orig = Vector::new(orig.x as Real, orig.y as Real, orig.z as Real); + let dir = Vector::new(dir.x as Real, dir.y as Real, dir.z as Real); + let denom = dir.dot(camera_fwd); + if denom.abs() < 1.0e-6 { + return; + } + let t = (g.plane_point - orig).dot(camera_fwd) / denom; + if t <= 0.0 { + return; + } + orig + dir * t + }; + + world.bodies[g.mouse_body].set_next_kinematic_translation(target); + world.bodies[g.body].wake_up(true); + } + + /// Removes the mouse joint, its kinematic anchor, and any temporary cluster proxy (the one + /// picked, and the one a tear handed the joint to). Removing a proxy dissolves its cluster + /// in whichever soft body holds it. + pub fn release(&mut self, world: &mut PhysicsWorld) { + let Some(g) = self.grabbed.take() else { + return; + }; + if let Some(rb) = world.bodies.get_mut(g.body) { + rb.wake_up(true); + } + // Removing the body also removes the attached mouse joint. + world.remove_body(g.mouse_body); + if let Some((sb_handle, cluster)) = g.soft_cluster { + if world.soft_bodies.get(sb_handle).is_some() { + world.remove_soft_body_cluster(sb_handle, cluster); + } + } +} + +/// A motor-only joint: nothing locked, spring position motors driving the anchors together on +/// every linear axis. Rotation stays completely free. +fn mouse_joint(local_anchor2: Vector) -> GenericJoint { + let joint = GenericJointBuilder::new(JointAxesMask::empty()) + .local_anchor2(local_anchor2) + .motor_position(JointAxis::LinX, 0.0, GRAB_STIFFNESS, GRAB_DAMPING) + .motor_position(JointAxis::LinY, 0.0, GRAB_STIFFNESS, GRAB_DAMPING); + #[cfg(feature = "dim3")] + let joint = joint.motor_position(JointAxis::LinZ, 0.0, GRAB_STIFFNESS, GRAB_DAMPING); + joint.into() +} + +fn query_filter() -> QueryFilter<'static> { + QueryFilter::from(QueryFilterFlags::ONLY_DYNAMIC | QueryFilterFlags::EXCLUDE_SENSORS) +} + +/// The dynamic body under the cursor and the world-space grab point on it. +#[cfg(feature = "dim2")] +fn pick(mouse: &SceneMouse, world: &PhysicsWorld) -> Option<(RigidBodyHandle, Vector)> { + let pt = mouse.point?; + let point = Vector::new(pt.x as Real, pt.y as Real); + let query_pipeline = world.broad_phase.as_query_pipeline( + world.narrow_phase.query_dispatcher(), + &world.bodies, + &world.colliders, + query_filter(), + ); + // An exact hit inside a solid collider. A 2D soft body's surface polyline is oriented, so + // parry classifies its interior (holes included) with the vertex pseudo-normals. + let inside = query_pipeline.intersect_point(point).find_map(|(handle, co)| { + // An open soft surface (rope, cloth) only has an outward side, not an interior. + if let Some(mesh_ref) = co.deformable_mesh_ref() { + let mesh = world.soft_bodies.get(mesh_ref.body)?.mesh_of(handle)?; + if !mesh.is_closed() { + return None; + } + } + co.parent() + }); + if let Some(body) = inside { + return Some((body, point)); + } + + // Otherwise the nearest boundary within a cursor-sized tolerance: soft bodies expose thin + // particle balls and surfaces that an exact test misses. + let (handle, proj) = query_pipeline.project_point(point, mouse.pick_radius as Real, true)?; + Some((world.colliders[handle].parent()?, proj.point)) +} + +#[cfg(feature = "dim3")] +fn pick(mouse: &SceneMouse, world: &PhysicsWorld) -> Option<(RigidBodyHandle, Vector)> { + let (orig, dir) = mouse.ray?; + let ray = Ray::new(orig.into(), dir.into()); + let query_pipeline = world.broad_phase.as_query_pipeline( + world.narrow_phase.query_dispatcher(), + &world.bodies, + &world.colliders, + query_filter(), + ); + let (handle, toi) = query_pipeline.cast_ray(&ray, Real::MAX, true)?; + let body = world.colliders[handle].parent()?; + Some((body, ray.origin + ray.dir * toi)) +} diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs index 388434714..1b079ef07 100644 --- a/src_testbed/lib.rs +++ b/src_testbed/lib.rs @@ -20,6 +20,7 @@ pub use kiss3d::event::Key as KeyCode; pub use egui; mod debug_render; +mod grab; mod graphics; mod mouse; pub mod physics; diff --git a/src_testbed/mouse.rs b/src_testbed/mouse.rs index aa29fce32..59d0ffbc7 100644 --- a/src_testbed/mouse.rs +++ b/src_testbed/mouse.rs @@ -4,6 +4,9 @@ use glamx::Vec2; pub struct SceneMouse { #[cfg(feature = "dim2")] pub point: Option, + /// World-space size of a ~10-pixel picking tolerance at the current zoom (2D only). + #[cfg(feature = "dim2")] + pub pick_radius: f32, #[cfg(feature = "dim3")] pub ray: Option<(glamx::Vec3, glamx::Vec3)>, } @@ -28,6 +31,8 @@ impl SceneMouse { let window_size = Vec2::new(window_size.0 as f32, window_size.1 as f32); let world_pos = camera.unproject(cursor, window_size); self.point = Some(world_pos); + let offset = camera.unproject(cursor + Vec2::new(10.0, 0.0), window_size); + self.pick_radius = (offset - world_pos).length(); } else { self.point = None; } diff --git a/src_testbed/testbed/state.rs b/src_testbed/testbed/state.rs index 7d3e6da74..e19278af5 100644 --- a/src_testbed/testbed/state.rs +++ b/src_testbed/testbed/state.rs @@ -78,6 +78,8 @@ pub enum UiTab { Examples, Settings, Performance, + /// What the debug renderer draws, and how. + DebugRender, } /// Information about an example for UI display. diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 9470993dc..52ce83e39 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -127,6 +127,7 @@ pub(crate) fn update_ui( ui.selectable_value(&mut state.selected_tab, UiTab::Examples, "Examples"); ui.selectable_value(&mut state.selected_tab, UiTab::Settings, "Settings"); ui.selectable_value(&mut state.selected_tab, UiTab::Performance, "Performance"); + ui.selectable_value(&mut state.selected_tab, UiTab::DebugRender, "Debug"); }); ui.separator(); @@ -141,11 +142,14 @@ pub(crate) fn update_ui( examples_tab(ui, state); } UiTab::Settings => { - settings_tab(ui, state, world, debug_render); + settings_tab(ui, state, world); } UiTab::Performance => { performance_tab(ui, state, world); } + UiTab::DebugRender => { + debug_render_tab(ui, debug_render); + } }); ui.separator(); @@ -300,12 +304,7 @@ fn examples_tab(ui: &mut Ui, state: &mut TestbedState) { } } -fn settings_tab( - ui: &mut Ui, - state: &mut TestbedState, - world: &mut PhysicsWorld, - debug_render: &mut DebugRenderPipelineResource, -) { +fn settings_tab(ui: &mut Ui, state: &mut TestbedState, world: &mut PhysicsWorld) { let integration_parameters = &mut world.integration_parameters; // ───────────────────────────────────────────────────────────────── @@ -325,8 +324,19 @@ fn settings_tab( .set(TestbedStateFlags::DRAW_SURFACES, draw_surfaces); } - ui.checkbox(&mut debug_render.enabled, "Debug render") - .on_hover_text("Show debug wireframes and contacts."); + #[cfg(feature = "dim3")] + { + let mut smooth = state + .flags + .contains(TestbedStateFlags::SMOOTH_MESH_COLLIDERS); + if ui + .changed() + { + state + .flags + .set(TestbedStateFlags::SMOOTH_MESH_COLLIDERS, smooth); + } + } // ───────────────────────────────────────────────────────────────── // SIMULATION @@ -561,6 +571,182 @@ fn settings_tab( } } +/// One of the debug renderer's colors, as a color picker. The style stores HSLA; the picker +/// works in sRGB, so the value round-trips through [`crate::debug_render::rgb_to_hsla`]. +fn debug_color_picker(ui: &mut Ui, label: &str, color: &mut rapier::pipeline::DebugColor) { + let rgba = crate::debug_render::hsla_to_rgb(color[0], color[1], color[2], color[3]); + let mut srgba = rgba.map(|c| (c.clamp(0.0, 1.0) * 255.0).round() as u8); + ui.horizontal(|ui| { + if ui.color_edit_button_srgba_unmultiplied(&mut srgba).changed() { + *color = crate::debug_render::rgb_to_hsla(srgba.map(|c| c as f32 / 255.0)); + } + ui.label(label); + }); +} + +/// One of the debug renderer's color multipliers (a per-HSLA-component scale, not a color). +fn debug_multiplier_row(ui: &mut Ui, label: &str, color: &mut rapier::pipeline::DebugColor) { + ui.horizontal(|ui| { + for (component, prefix) in color.iter_mut().zip(["h ", "s ", "l ", "a "]) { + ui.add( + egui::DragValue::new(component) + .speed(0.01) + .range(0.0..=1.0) + .prefix(prefix), + ); + } + ui.label(label); + }); +} + +/// What the debug renderer draws, and how it draws it. +fn debug_render_tab(ui: &mut Ui, debug_render: &mut DebugRenderPipelineResource) { + use rapier::pipeline::DebugRenderMode; + + ui.checkbox(&mut debug_render.enabled, "Debug render") + .on_hover_text("Draw the physics state over the scene (wireframes, joints, contacts)."); + ui.add_space(8.0); + + // ───────────────────────────────────────────────────────────────── + // WHAT TO DRAW + // ───────────────────────────────────────────────────────────────── + ui.label(RichText::new("What to draw").strong()); + ui.add_space(2.0); + + { + // The composite `JOINTS` flag is left out: its two halves are here. + const FLAGS: &[(DebugRenderMode, &str, &str)] = &[ + ( + DebugRenderMode::COLLIDER_SHAPES, + "Collider shapes", + "The colliders' outlines, colored by their body type.", + ), + ( + DebugRenderMode::COLLIDER_AABBS, + "Collider AABBs", + "The bounding boxes the broad phase sees.", + ), + ( + DebugRenderMode::RIGID_BODY_AXES, + "Rigid-body axes", + "The local frame of every rigid body, at its center of mass.", + ), + ( + DebugRenderMode::IMPULSE_JOINTS, + "Impulse joints", + "The anchors of the impulse joints, and their separation.", + ), + ( + DebugRenderMode::MULTIBODY_JOINTS, + "Multibody joints", + "The anchors of the multibody joints, and their separation.", + ), + ( + DebugRenderMode::CONTACTS, + "Contacts", + "The geometric contact points and their normals.", + ), + ( + DebugRenderMode::SOLVER_CONTACTS, + "Solver contacts", + "The contact points the solver actually used this step.", + ), + ( + DebugRenderMode::SOFT_BODIES, + "Soft bodies", + "The soft bodies' elements (structural and cell edges), their cluster frames, \ + and their soft-vs-soft contacts.", + ), + ]; + + for (flag, label, hover) in FLAGS { + let mut on = debug_render.pipeline.mode.contains(*flag); + if ui.checkbox(&mut on, *label).on_hover_text(*hover).changed() { + debug_render.pipeline.mode.set(*flag, on); + } + } + + ui.horizontal(|ui| { + if ui.button("All").clicked() { + debug_render.pipeline.mode = DebugRenderMode::all(); + } + if ui.button("None").clicked() { + debug_render.pipeline.mode = DebugRenderMode::empty(); + } + if ui.button("Default").clicked() { + debug_render.pipeline.mode = DebugRenderMode::default(); + } + }); + + ui.add_space(8.0); + + // ───────────────────────────────────────────────────────────── + // STYLE + // ───────────────────────────────────────────────────────────── + ui.label(RichText::new("Style").strong()); + ui.add_space(2.0); + + let style = &mut debug_render.pipeline.style; + ui.add(Slider::new(&mut style.subdivisions, 2..=64).text("Subdivisions")) + .on_hover_text("Segments approximating a curved shape (balls, capsules, cones)."); + ui.add(Slider::new(&mut style.border_subdivisions, 1..=32).text("Border subdivisions")) + .on_hover_text("Segments approximating the rounded border of a round shape."); + ui.add( + Slider::new(&mut style.rigid_body_axes_length, 0.0..=2.0).text("Axes length"), + ) + .on_hover_text("Length of the rigid-body axes."); + ui.add( + Slider::new(&mut style.contact_normal_length, 0.0..=1.0).text("Normal length"), + ) + .on_hover_text("Length of the contact normals."); + + ui.collapsing("Colors", |ui| { + debug_color_picker(ui, "Dynamic colliders", &mut style.collider_dynamic_color); + debug_color_picker(ui, "Fixed colliders", &mut style.collider_fixed_color); + debug_color_picker(ui, "Kinematic colliders", &mut style.collider_kinematic_color); + debug_color_picker(ui, "Parentless colliders", &mut style.collider_parentless_color); + debug_color_picker(ui, "Collider AABBs", &mut style.collider_aabb_color); + debug_color_picker(ui, "Impulse joint anchors", &mut style.impulse_joint_anchor_color); + debug_color_picker( + ui, + "Impulse joint separation", + &mut style.impulse_joint_separation_color, + ); + debug_color_picker( + ui, + "Multibody joint anchors", + &mut style.multibody_joint_anchor_color, + ); + debug_color_picker( + ui, + "Multibody joint separation", + &mut style.multibody_joint_separation_color, + ); + debug_color_picker(ui, "Contact depth", &mut style.contact_depth_color); + debug_color_picker(ui, "Contact normals", &mut style.contact_normal_color); + debug_color_picker(ui, "Soft-body elements", &mut style.soft_body_element_color); + debug_color_picker(ui, "Soft-body frames", &mut style.soft_body_frame_color); + + ui.add_space(4.0); + ui.label("Multipliers (per HSLA component)"); + debug_multiplier_row(ui, "Sleeping", &mut style.sleep_color_multiplier); + debug_multiplier_row(ui, "Sleep-ready", &mut style.sleep_eligible_color_multiplier); + debug_multiplier_row(ui, "Disabled", &mut style.disabled_color_multiplier); + if ui.button("Reset colors").clicked() { + let default = rapier::pipeline::DebugRenderStyle::default(); + // The scalars above are edited separately: only the colors are reset. + *style = rapier::pipeline::DebugRenderStyle { + subdivisions: style.subdivisions, + border_subdivisions: style.border_subdivisions, + rigid_body_axes_length: style.rigid_body_axes_length, + contact_normal_length: style.contact_normal_length, + ..default + }; + } + }); + } +} + fn performance_tab(ui: &mut Ui, state: &TestbedState, world: &PhysicsWorld) { // ───────────────────────────────────────────────────────────────── // SCENE INFO @@ -571,7 +757,7 @@ fn performance_tab(ui: &mut Ui, state: &TestbedState, world: &PhysicsWorld) { let num_contacts: usize = world .narrow_phase .contact_pairs() - .map(|pair| pair.manifolds.iter().map(|m| m.points.len()).sum::()) + .map(|pair| pair.manifolds().iter().map(|m| m.points.len()).sum::()) .sum(); let num_sleeping = world diff --git a/src_testbed/viewer.rs b/src_testbed/viewer.rs index f1db6c5df..298d641ec 100644 --- a/src_testbed/viewer.rs +++ b/src_testbed/viewer.rs @@ -36,6 +36,7 @@ use glamx::Vec3; use crate::Camera; use crate::debug_render::{DebugRenderPipelineResource, debug_render_scene}; +use crate::grab::MouseGrab; use crate::graphics::{GraphicsManager, RenderMaterial}; use crate::mouse::SceneMouse; use crate::physics::{restore_world, snapshot_world}; @@ -58,6 +59,7 @@ pub struct TestbedViewer { graphics: GraphicsManager, camera: Camera, scene_mouse: SceneMouse, + grab: MouseGrab, keys: KeysState, debug_render: DebugRenderPipelineResource, state: TestbedState, @@ -116,6 +118,7 @@ impl TestbedViewer { graphics: GraphicsManager::new(), camera, scene_mouse: SceneMouse::new(), + grab: MouseGrab::default(), keys: KeysState::default(), debug_render: DebugRenderPipelineResource::default(), state, @@ -147,12 +150,13 @@ impl TestbedViewer { return false; } - self.handle_events(); - let cursor_pos = self.window.cursor_pos(); self.scene_mouse .update_from_window(cursor_pos, self.window.size().into(), &self.camera); + self.handle_events(world); + self.update_grab(world); + self.handle_action_flags(world); self.handle_sleep_settings(world); self.autosave(); @@ -234,6 +238,8 @@ impl TestbedViewer { /// between examples). A restart / backend / solver-parameter switch preserves the camera /// and the user's setting edits; selecting a different example resets both. pub fn clear_scene(&mut self) { + // The grabbed handles belong to the world being discarded. + self.grab.forget(); self.graphics.clear(); self.state.transition = None; self.state.snapshot = None; @@ -294,16 +300,37 @@ impl TestbedViewer { /// down its local -z, so `rot * z` points from the scene back toward the eye. #[cfg(feature = "dim3")] pub fn camera_rotation(&self) -> na::UnitQuaternion { - let rot_x = na::UnitQuaternion::from_axis_angle(&na::Vector3::y_axis(), self.camera.at().x); - let rot_y = - na::UnitQuaternion::from_axis_angle(&(-na::Vector3::x_axis()), self.camera.at().y); - rot_x * rot_y + use kiss3d::camera::Camera3d; + let eye = self.camera.eye(); + let at = self.camera.at(); + let backward = na::Vector3::new(eye.x - at.x, eye.y - at.y, eye.z - at.z); + let Some(backward) = na::Unit::try_new(backward, 1.0e-6) else { + return na::UnitQuaternion::identity(); + }; + let backward = backward.into_inner(); + let up = na::Vector3::new( + self.state.up_axis.x as f32, + self.state.up_axis.y as f32, + self.state.up_axis.z as f32, + ); + // Degenerate up (looking straight along it): substitute any non-parallel axis. + let up = if up.dot(&backward).abs() > 0.999 { + let alt = na::Vector3::x(); + if alt.dot(&backward).abs() > 0.999 { + na::Vector3::y() + } else { + alt + } + } else { + up + }; + na::UnitQuaternion::face_towards(&backward, &up) } /// Camera forward direction, from the eye toward the look-at point (3D only). #[cfg(feature = "dim3")] pub fn camera_fwd_dir(&self) -> na::Vector3 { - self.camera_rotation() * na::Vector3::z() + self.camera_rotation() * -na::Vector3::z() } /// Recenters the camera so the whole scene fills the viewport (deferred to @@ -481,13 +508,48 @@ impl TestbedViewer { // ───────────────────────────── internals ──────────────────────────────── - fn handle_events(&mut self) { - // Collect first so we can mutate `self` freely inside the loop. - let events: Vec = self.window.events().iter().map(|e| e.value).collect(); + fn handle_events(&mut self, world: &mut PhysicsWorld) { + // Mouse events are handled live so a grab can inhibit them (an inhibited event never + // reaches the camera). Key events are collected and processed after the loop, since + // their handlers borrow `self` as a whole. + let egui_wants_pointer = { + let ctx = self.window.egui_context(); + ctx.egui_wants_pointer_input() || ctx.is_pointer_over_egui() + }; + + let mut key_events = Vec::new(); + for mut event in self.window.events().iter() { + match event.value { + WindowEvent::MouseButton(kiss3d::event::MouseButton::Button1, action, _) => { + match action { + Action::Press => { + if !egui_wants_pointer + && self.grab.try_grab(&self.scene_mouse, world) + { + event.inhibited = true; + } + } + Action::Release => { + if self.grab.active() { + self.grab.release(world); + event.inhibited = true; + } + } + } + } + // The cameras poll the button state on cursor moves, so freezing the camera + // during a grab means swallowing the cursor events themselves. + WindowEvent::CursorPos(..) if self.grab.active() => { + event.inhibited = true; + } + WindowEvent::Key(key, action, _) => key_events.push((key, action)), + _ => {} + } + } - for value in events { - match value { - WindowEvent::Key(key, Action::Press, _) => { + for (key, action) in key_events { + match action { + Action::Press => { if !self.keys.pressed_keys.contains(&key) { self.keys.pressed_keys.push(key); } @@ -498,7 +560,7 @@ impl TestbedViewer { _ => {} } } - WindowEvent::Key(key, Action::Release, _) => { + Action::Release => { self.keys.pressed_keys.retain(|k| *k != key); match key { Key::T => { @@ -516,11 +578,41 @@ impl TestbedViewer { _ => {} } } - _ => {} } } } + /// Retargets the mouse joint's kinematic anchor and draws the grab cue line. + fn update_grab(&mut self, world: &mut PhysicsWorld) { + #[cfg(feature = "dim3")] + { + let fwd = self.camera_fwd_dir(); + let fwd = rapier::math::Vector::new(fwd.x as _, fwd.y as _, fwd.z as _); + self.grab.update(&self.scene_mouse, fwd, world); + } + #[cfg(feature = "dim2")] + self.grab.update(&self.scene_mouse, world); + + if let Some((a, b)) = self.grab.cue_line(world) { + let color = Color::new(0.9, 0.4, 0.1, 1.0); + #[cfg(feature = "dim3")] + self.window.draw_line( + glamx::Vec3::new(a.x as f32, a.y as f32, a.z as f32), + glamx::Vec3::new(b.x as f32, b.y as f32, b.z as f32), + color, + 4.0, + false, + ); + #[cfg(feature = "dim2")] + self.window.draw_line_2d( + glamx::Vec2::new(a.x as f32, a.y as f32), + glamx::Vec2::new(b.x as f32, b.y as f32), + color, + 4.0, + ); + } + } + fn request_restart(&mut self) { self.state.preserve_settings_on_switch = true; self.state.transition = Some(Transition::Switch); From 2c3c41c2d50fb9409cf0498dbb5646f18ec92ab9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 4 Sep 2026 13:35:16 +0200 Subject: [PATCH 06/32] feat(soft-body): bind collision meshes to clusters with `ColliderSet::insert_deformable`, wire meshes, CCD targets and mesh rendering --- .../tests/pseudo_normals_debug_render.rs | 63 +++ crates/rapier2d/tests/soft_bodies.rs | 4 +- crates/rapier3d/examples/fem_bench.rs | 2 +- .../tests/pseudo_normals_debug_render.rs | 52 ++ crates/rapier3d/tests/soft_bodies.rs | 16 +- crates/rapier3d/tests/soft_body_clusters.rs | 128 ++++- crates/rapier3d/tests/soft_body_joints.rs | 6 +- crates/rapier3d/tests/soft_body_meshes.rs | 452 ++++++++++++++++++ crates/rapier3d/tests/soft_wires.rs | 172 +++++++ examples2d/soft_joints2.rs | 4 +- examples3d/all_examples3.rs | 2 + examples3d/soft_joints3.rs | 4 +- examples3d/soft_meshes3.rs | 262 ++++++++++ examples3d/soft_trimesh3.rs | 71 +++ src/dynamics/ccd/ccd_solver.rs | 12 +- src/dynamics/ccd/sweeps.rs | 50 +- .../collision_mesh/collision_mesh.rs | 149 ++++++ .../collision_mesh_accessors.rs | 285 +++++++++++ .../collision_mesh/collision_mesh_binding.rs | 238 +++++++++ .../collision_mesh/collision_mesh_geometry.rs | 1 + .../collision_mesh/collision_mesh_topology.rs | 375 +++++++++++++++ src/dynamics/soft_body/collision_mesh/mod.rs | 22 + .../soft_body_builder/soft_body_build.rs | 43 ++ .../soft_body_builder/soft_body_builder.rs | 4 + .../soft_body_builder_mesh_helpers.rs | 53 ++ .../soft_body_builder_settings.rs | 34 +- .../soft_body_builder_shapes.rs | 5 + src/dynamics/soft_body/soft_body_cluster.rs | 50 +- src/dynamics/soft_body/soft_body_meshes.rs | 150 ++++++ .../soft_body_set/soft_body_set_clusters.rs | 1 + .../soft_body_set_insert_remove.rs | 28 ++ .../soft_body_set/soft_body_set_proxies.rs | 51 ++ .../soft_body_set/soft_body_set_step_sync.rs | 1 + .../soft_constraint_rigid_coupling.rs | 19 + .../soft_contact_assembly_body_contacts.rs | 40 ++ .../soft_contact_assembly_edge_contacts.rs | 1 + .../soft_contact_assembly_workspace.rs | 16 + src/geometry/collider_set.rs | 103 +++- src/geometry/narrow_phase/pair_update.rs | 18 + .../soft_contacts/soft_contacts_edge_pass.rs | 8 + .../debug_render_pipeline.rs | 115 +++++ .../debug_render_style.rs | 9 + src/pipeline/physics_pipeline/substep.rs | 6 +- src/pipeline/physics_world.rs | 26 +- src_testbed/graphics.rs | 184 ++++--- src_testbed/graphics/graphics_polyline.rs | 181 +++++++ src_testbed/graphics/graphics_soft_bodies.rs | 315 ++++++++++++ src_testbed/ui.rs | 25 + src_testbed/viewer.rs | 10 + 49 files changed, 3737 insertions(+), 129 deletions(-) create mode 100644 crates/rapier2d/tests/pseudo_normals_debug_render.rs create mode 100644 crates/rapier3d/tests/pseudo_normals_debug_render.rs create mode 100644 crates/rapier3d/tests/soft_body_meshes.rs create mode 100644 crates/rapier3d/tests/soft_wires.rs create mode 100644 examples3d/soft_meshes3.rs create mode 100644 src/dynamics/soft_body/collision_mesh/collision_mesh.rs create mode 100644 src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs create mode 100644 src/dynamics/soft_body/collision_mesh/collision_mesh_binding.rs create mode 100644 src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs create mode 100644 src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs create mode 100644 src/dynamics/soft_body/collision_mesh/mod.rs create mode 100644 src_testbed/graphics/graphics_polyline.rs create mode 100644 src_testbed/graphics/graphics_soft_bodies.rs diff --git a/crates/rapier2d/tests/pseudo_normals_debug_render.rs b/crates/rapier2d/tests/pseudo_normals_debug_render.rs new file mode 100644 index 000000000..49436097d --- /dev/null +++ b/crates/rapier2d/tests/pseudo_normals_debug_render.rs @@ -0,0 +1,63 @@ +//! The debug-renderer draws a polyline's pseudo-normals when it has them, and nothing otherwise. +#![cfg(feature = "debug-render")] + +use rapier2d::parry::shape::{Polyline, PolylineFlags}; +use rapier2d::pipeline::{ + DebugRenderBackend, DebugRenderMode, DebugRenderObject, DebugRenderPipeline, DebugRenderStyle, +}; +use rapier2d::prelude::*; + +#[derive(Default)] +struct LineCollector { + lines: Vec<(Vector, Vector)>, +} + +impl DebugRenderBackend for LineCollector { + fn draw_line(&mut self, _: DebugRenderObject, a: Vector, b: Vector, _: [f32; 4]) { + self.lines.push((a, b)); + } +} + +fn render_square_pseudo_normals(flags: PolylineFlags) -> (LineCollector, usize) { + // A counter-clockwise square, so the pseudo-normals point outward. + let vertices = vec![ + Vector::new(-1.0, -1.0), + Vector::new(1.0, -1.0), + Vector::new(1.0, 1.0), + Vector::new(-1.0, 1.0), + ]; + let indices = vec![[0, 1], [1, 2], [2, 3], [3, 0]]; + let num_vertices = vertices.len(); + let polyline = Polyline::with_flags(vertices, Some(indices), flags); + + let bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + colliders.insert(ColliderBuilder::new(SharedShape::new(polyline))); + + // Only `PSEUDO_NORMALS`: the pseudo-normals must not depend on the shapes being drawn. + let mut pipeline = + DebugRenderPipeline::new(DebugRenderStyle::default(), DebugRenderMode::PSEUDO_NORMALS); + let mut backend = LineCollector::default(); + pipeline.render_colliders(&mut backend, &bodies, &colliders); + (backend, num_vertices) +} + +#[test] +fn oriented_polyline_draws_one_normal_per_vertex() { + let (backend, num_vertices) = render_square_pseudo_normals(PolylineFlags::ORIENTED); + assert_eq!(backend.lines.len(), num_vertices); + + let len = DebugRenderStyle::default().pseudo_normal_length; + for (a, b) in &backend.lines { + let normal = *b - *a; + assert!((normal.length() - len).abs() < 1.0e-5); + // A square's corner pseudo-normals are its outward diagonals. + assert!(normal.dot(*a) > 0.0); + } +} + +#[test] +fn polyline_without_pseudo_normals_draws_nothing() { + let (backend, _) = render_square_pseudo_normals(PolylineFlags::empty()); + assert!(backend.lines.is_empty()); +} diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index 2e76ac8c1..ae98a64cb 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -1386,7 +1386,7 @@ fn fem_free_fall_is_exact() { let builder = SoftBodyBuilder::grid(Vector::new(0.0, 10.0), Vector::splat(0.5), 4, 4) .cell_model(SoftBodyCellModel::Corotational) .solver(SoftBodySolver::Fem) - .without_colliders() + .no_surface_collider() .can_sleep(false); let handle = world.insert_soft_body(builder); let start = world.soft_bodies[handle].particle_position(0); @@ -1472,7 +1472,7 @@ fn fem_deflection_is_substep_invariant() { }) .solver(SoftBodySolver::Fem) .mass(2.0) - .without_colliders() + .no_surface_collider() .can_sleep(false); let pinned: Vec = builder .particle_positions() diff --git a/crates/rapier3d/examples/fem_bench.rs b/crates/rapier3d/examples/fem_bench.rs index eba050418..1e99a3dd9 100644 --- a/crates/rapier3d/examples/fem_bench.rs +++ b/crates/rapier3d/examples/fem_bench.rs @@ -78,7 +78,7 @@ fn cantilever(solver: SoftBodySolver, substeps: usize) { }) .solver(solver) .mass(8.0) - .without_colliders() + .no_surface_collider() .can_sleep(false); let pinned: Vec = builder .particle_positions() diff --git a/crates/rapier3d/tests/pseudo_normals_debug_render.rs b/crates/rapier3d/tests/pseudo_normals_debug_render.rs new file mode 100644 index 000000000..2f40f6c2e --- /dev/null +++ b/crates/rapier3d/tests/pseudo_normals_debug_render.rs @@ -0,0 +1,52 @@ +//! The debug-renderer draws a mesh's pseudo-normals when it has them, and nothing otherwise. +#![cfg(feature = "debug-render")] + +use rapier3d::parry::shape::TriMeshFlags; +use rapier3d::pipeline::{ + DebugRenderBackend, DebugRenderMode, DebugRenderObject, DebugRenderPipeline, DebugRenderStyle, +}; +use rapier3d::prelude::*; + +#[derive(Default)] +struct LineCollector { + lines: Vec<(Vector, Vector)>, +} + +impl DebugRenderBackend for LineCollector { + fn draw_line(&mut self, _: DebugRenderObject, a: Vector, b: Vector, _: [f32; 4]) { + self.lines.push((a, b)); + } +} + +fn render_cube_pseudo_normals(flags: TriMeshFlags) -> (LineCollector, usize, usize) { + let (vtx, idx) = Cuboid::new(Vector::splat(0.5)).to_trimesh(); + let (num_vertices, num_triangles) = (vtx.len(), idx.len()); + + let bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + colliders.insert(ColliderBuilder::trimesh_with_flags(vtx, idx, flags).unwrap()); + + // Only `PSEUDO_NORMALS`: the pseudo-normals must not depend on the shapes being drawn. + let mut pipeline = + DebugRenderPipeline::new(DebugRenderStyle::default(), DebugRenderMode::PSEUDO_NORMALS); + let mut backend = LineCollector::default(); + pipeline.render_colliders(&mut backend, &bodies, &colliders); + (backend, num_vertices, num_triangles) +} + +#[test] +fn oriented_trimesh_draws_one_normal_per_vertex_and_triangle_edge() { + let (backend, num_vertices, num_triangles) = render_cube_pseudo_normals(TriMeshFlags::ORIENTED); + assert_eq!(backend.lines.len(), num_vertices + 3 * num_triangles); + + let len = DebugRenderStyle::default().pseudo_normal_length; + for (a, b) in &backend.lines { + assert!(((*b - *a).length() - len).abs() < 1.0e-5); + } +} + +#[test] +fn trimesh_without_pseudo_normals_draws_nothing() { + let (backend, ..) = render_cube_pseudo_normals(TriMeshFlags::empty()); + assert!(backend.lines.is_empty()); +} diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index f37e0605b..57357d487 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -2901,7 +2901,7 @@ fn fem_cantilever( ..Default::default() }) .solver(solver) - .without_colliders() + .no_surface_collider() .can_sleep(false); // Pin every particle of the root face. let pinned: Vec = builder @@ -2938,7 +2938,7 @@ fn fem_free_fall_is_exact() { let builder = SoftBodyBuilder::cuboid(Vector::new(0.0, 10.0, 0.0), Vector::splat(0.5), 3, 3, 3) .cell_model(SoftBodyCellModel::Corotational) .solver(SoftBodySolver::Fem) - .without_colliders() + .no_surface_collider() .can_sleep(false); let handle = world.insert_soft_body(builder); let start = world.soft_bodies[handle].particle_position(0); @@ -3144,7 +3144,7 @@ fn fem_attachment_follows_its_body() { ..Default::default() }) .solver(SoftBodySolver::Fem) - .without_colliders() + .no_surface_collider() .can_sleep(false) .particle_mass(0.05); let handle = world.insert_soft_body(cube); @@ -3291,7 +3291,7 @@ fn fem_edge_period_is_substep_invariant() { .pinned_particles([0]) .softness(SpringCoefficients::new(natural_frequency, 0.0)) .solver(SoftBodySolver::Fem) - .without_colliders() + .no_surface_collider() .can_sleep(false); let handle = world.insert_soft_body(builder); world.soft_bodies[handle].set_particle_position(1, Vector::new(1.2, 0.0, 0.0)); @@ -3335,7 +3335,7 @@ fn fem_rope_hangs_from_anchor() { .pinned_particles([0]) .softness(SpringCoefficients::new(300.0, 1.0)) .solver(SoftBodySolver::Fem) - .without_colliders() + .no_surface_collider() .can_sleep(false); let handle = world.insert_soft_body(rope); for _ in 0..600 { @@ -3453,7 +3453,7 @@ fn fem_neo_hookean_cube_survives_large_compression() { ..Default::default() }) .solver(SoftBodySolver::Fem) - .without_colliders() + .no_surface_collider() .can_sleep(false) .particle_mass(0.1); let handle = world.insert_soft_body(cube); @@ -3509,7 +3509,7 @@ fn fem_plastic_cells_keep_their_deformation() { }) .solver(SoftBodySolver::Fem) .particle_mass(0.1) - .collider_template(ColliderBuilder::ball(0.15).friction(2.0)); + .surface_collider(ColliderBuilder::ball(0.15).friction(2.0)); let handle = world.insert_soft_body(block); let sb = &world.soft_bodies[handle]; let bottom: Vec = (0..sb.num_particles()) @@ -3580,7 +3580,7 @@ fn fem_cloth_tears_when_pulled_apart() { .softness(SpringCoefficients::new(200.0, 1.0)) .tear_strain(0.15) .solver(SoftBodySolver::Fem) - .without_colliders() + .no_surface_collider() .can_sleep(false); let handle = world.insert_soft_body(cloth); let left: Vec = (0..n).map(|k| k * n).collect(); diff --git a/crates/rapier3d/tests/soft_body_clusters.rs b/crates/rapier3d/tests/soft_body_clusters.rs index 49aec3b2c..ef5a61654 100644 --- a/crates/rapier3d/tests/soft_body_clusters.rs +++ b/crates/rapier3d/tests/soft_body_clusters.rs @@ -313,7 +313,7 @@ fn user_collider_on_a_proxy_rides_the_frame() { }) .particle_mass(0.1) .particle_radius(0.05) - .collider_template(ColliderBuilder::ball(0.05)), + .surface_collider(ColliderBuilder::ball(0.05)), ); let root = world.soft_bodies[h].root_body(); // A small bracket embedded in the jelly (overlapping its own surface: the same-soft-body @@ -409,7 +409,7 @@ fn cluster_stiffness_scale_stiffens_a_region() { }) .particle_mass(0.1) .particle_radius(0.05) - .collider_template(ColliderBuilder::ball(0.05)), + .surface_collider(ColliderBuilder::ball(0.05)), ); // A soft reference world with the identical body, unscaled. let mut soft_world = world_with_ground(); @@ -424,7 +424,7 @@ fn cluster_stiffness_scale_stiffens_a_region() { }) .particle_mass(0.1) .particle_radius(0.05) - .collider_template(ColliderBuilder::ball(0.05)), + .surface_collider(ColliderBuilder::ball(0.05)), ); // Stiffen the whole body through the whole-body cluster: it must sag less than the soft // reference under its own weight. @@ -518,3 +518,125 @@ fn pinned_cluster_drives_its_region_kinematically() { assert!(p.position().is_finite()); } } + +/// A jelly cube with a stiff corotational material, on the ground. +fn jelly(world: &mut PhysicsWorld) -> SoftBodyHandle { + world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(0.0, 0.6, 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)), + ) +} + +/// The particles of a soft body above `y`. +fn particles_above(world: &PhysicsWorld, handle: SoftBodyHandle, y: Real) -> Vec { + world.soft_bodies[handle] + .particles() + .iter() + .enumerate() + .filter(|(_, p)| p.position().y > y) + .map(|(i, _)| i as u32) + .collect() +} + +#[test] +fn a_rigid_collider_on_a_sub_cluster_does_not_fight_its_own_body() { + let mut world = world_with_ground(); + let h = jelly(&mut world); + let top = particles_above(&world, h, 0.6); + assert!(!top.is_empty()); + let cluster = world + .soft_bodies + .add_cluster(h, &top, &mut world.bodies, &mut world.colliders) + .unwrap(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + // A bracket embedded in the jelly, on a proxy that is *not* the parent of its surface + // collider: only the same-soft-body filter can keep the two apart. + let bracket = world.colliders.insert_with_parent( + ColliderBuilder::cuboid(0.2, 0.2, 0.2).density(0.1), + proxy, + &mut world.bodies, + ); + let surface = world.soft_bodies[h].collision_mesh().unwrap().collider(); + + for _ in 0..300 { + world.step(); + } + + assert!( + !world + .narrow_phase + .contact_pair(bracket, surface) + .is_some_and(|pair| pair.has_any_active_contact()), + "a body's own bracket collided with its surface" + ); + let sb = &world.soft_bodies[h]; + assert!(sb.particle_positions().all(|p| p.is_finite())); + let com = sb.center_of_mass(); + assert!( + com.y > 0.2 && com.y < 0.8, + "the jelly did not settle (com {com:?})" + ); +} + +#[test] +fn a_rigid_collider_on_a_cluster_brings_the_world_to_the_particles() { + let mut world = world_with_ground(); + // A soft jelly, so the load's deflection is visible. + let h = world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(0.0, 0.6, 0.0), Vector::splat(0.5), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e3, + 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 top = particles_above(&world, h, 0.6); + let cluster = world + .soft_bodies + .add_cluster(h, &top, &mut world.bodies, &mut world.colliders) + .unwrap(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + // A plate on the jelly's top cluster, and a heavy box dropped on it. + world.colliders.insert_with_parent( + ColliderBuilder::cuboid(0.5, 0.05, 0.5).translation(Vector::new(0.0, 0.55, 0.0)), + proxy, + &mut world.bodies, + ); + for _ in 0..100 { + world.step(); + } + let before = world.soft_bodies[h].center_of_mass().y; + + let (weight, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 2.5, 0.0)), + ColliderBuilder::cuboid(0.3, 0.3, 0.3).density(20.0), + ); + for _ in 0..300 { + world.step(); + } + + let sb = &world.soft_bodies[h]; + assert!(sb.particle_positions().all(|p| p.is_finite())); + let after = sb.center_of_mass().y; + assert!( + after < before - 0.02, + "the load on the plate did not reach the particles: {before} -> {after}" + ); + // The weight rests on the plate rather than falling through the jelly. + let weight_y = world.bodies[weight].translation().y; + assert!(weight_y > 0.2, "the weight fell through: {weight_y}"); +} diff --git a/crates/rapier3d/tests/soft_body_joints.rs b/crates/rapier3d/tests/soft_body_joints.rs index d9117cf71..ba8189d4d 100644 --- a/crates/rapier3d/tests/soft_body_joints.rs +++ b/crates/rapier3d/tests/soft_body_joints.rs @@ -35,7 +35,7 @@ fn jelly_at(world: &mut PhysicsWorld, center: Vector) -> SoftBodyHandle { }) .particle_mass(0.1) .particle_radius(0.05) - .collider_template(ColliderBuilder::ball(0.05)), + .surface_collider(ColliderBuilder::ball(0.05)), ) } @@ -324,7 +324,7 @@ fn joint_on_a_fem_body_holds() { }) .particle_mass(0.1) .particle_radius(0.05) - .collider_template(ColliderBuilder::ball(0.05)), + .surface_collider(ColliderBuilder::ball(0.05)), ); let root = world.soft_bodies[h].root_body(); let anchor = world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(0.0, 2.0, 0.0))); @@ -426,7 +426,7 @@ fn violently_dragged_one_particle_cluster_stays_bounded() { }) .particle_mass(0.08) .particle_radius(0.06) - .collider_template(ColliderBuilder::ball(0.06)), + .surface_collider(ColliderBuilder::ball(0.06)), ); // Grab the top corner particle, like the testbed's mouse grab does. let corner = Vector::new(0.5, 1.1, 0.5); diff --git a/crates/rapier3d/tests/soft_body_meshes.rs b/crates/rapier3d/tests/soft_body_meshes.rs new file mode 100644 index 000000000..a071b7a63 --- /dev/null +++ b/crates/rapier3d/tests/soft_body_meshes.rs @@ -0,0 +1,452 @@ +//! Collision meshes owned by a soft body's clusters: binding a mesh to a cluster +//! (`ColliderSet::insert_deformable`), what is rejected, and how several meshes of one body +//! interact. + +use rapier3d::parry::shape::{Ball, Cuboid, TriMeshFlags}; +use rapier3d::prelude::*; + +fn world_with_ground() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(50.0, 0.5, 50.0).translation(Vector::new(0.0, -0.5, 0.0)), + ); + world +} + +/// A soft cube of 3x3x3 particles, above the ground. +fn cube(world: &mut PhysicsWorld) -> SoftBodyHandle { + world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(0.0, 1.5, 0.0), Vector::splat(0.5), 3, 3, 3) + .particle_mass(0.2), + ) +} + +/// A deformable mesh authored in world space, to be attached to `parent`: placed by the +/// inverse of the parent's pose. +fn world_mesh( + world: &PhysicsWorld, + vertices: Vec, + indices: Vec<[u32; 3]>, + parent: RigidBodyHandle, +) -> ColliderBuilder { + ColliderBuilder::trimesh_with_flags(vertices, indices, TriMeshFlags::DEFORMABLE) + .expect("a valid mesh") + .position(world.bodies[parent].position().inverse()) +} + +/// The body's own boundary, in world space: a mesh that sits exactly on its particles, to be +/// attached to `parent`. +fn boundary_mesh( + world: &PhysicsWorld, + handle: SoftBodyHandle, + parent: RigidBodyHandle, +) -> ColliderBuilder { + let sb = &world.soft_bodies[handle]; + let vertices: Vec = sb.particle_positions().collect(); + let indices: Vec<[u32; 3]> = sb.boundary().to_vec(); + world_mesh(world, vertices, indices, parent) +} + +#[test] +fn a_second_mesh_on_the_whole_body_cluster_follows_the_particles() { + let mut world = world_with_ground(); + let handle = cube(&mut world); + let proxy = world.soft_bodies[handle].root_body(); + let collider = world + .insert_deformable( + boundary_mesh(&world, handle, proxy), + SoftMeshBinding::direct_by_position(1.0e-4), + proxy, + ) + .expect("the mesh binds to the whole-body cluster"); + + assert_eq!(world.soft_bodies[handle].meshes().count(), 2); + let before = world.soft_bodies[handle] + .mesh_of(collider) + .unwrap() + .vertex_positions(&world.soft_bodies[handle]) + .map(|v| v.y) + .fold(Real::MAX, Real::min); + + for _ in 0..120 { + world.step(); + } + + let sb = &world.soft_bodies[handle]; + let mesh = sb.mesh_of(collider).expect("the mesh is still there"); + let after = mesh + .vertex_positions(sb) + .map(|v| v.y) + .fold(Real::MAX, Real::min); + assert!( + mesh.vertex_positions(sb).all(|v| v.is_finite()), + "the mesh went non-finite" + ); + assert!( + after < before - 0.1, + "the mesh did not come down with the particles: {before} -> {after}" + ); + // It rests on the ground with its particles rather than sinking. + assert!(after > -0.2, "the mesh sank through the ground: {after}"); +} + +#[test] +fn two_meshes_of_one_cluster_never_collide_with_each_other() { + let mut world = world_with_ground(); + let handle = cube(&mut world); + let proxy = world.soft_bodies[handle].root_body(); + let first = world.soft_bodies[handle] + .collision_mesh() + .unwrap() + .collider(); + let second = world + .insert_deformable( + boundary_mesh(&world, handle, proxy), + // Even asking for self contacts: two meshes of one cluster describe the same + // particles, so they must not fight each other. + SoftMeshBinding::direct_by_position(1.0e-4).self_contacts(true), + proxy, + ) + .unwrap(); + + for _ in 0..60 { + world.step(); + } + + assert!( + world.narrow_phase.contact_pair(first, second).is_none(), + "two meshes of one cluster reported a contact pair" + ); + // Both still meet the ground. + for collider in [first, second] { + assert!( + world + .narrow_phase + .contact_pairs_with(collider) + .any(|p| p.has_any_active_contact()), + "a mesh lost its contacts with the world" + ); + } +} + +#[test] +fn a_mesh_bound_outside_its_cluster_is_rejected() { + let mut world = world_with_ground(); + let handle = cube(&mut world); + // A cluster covering a single particle: every other particle is outside it. + let cluster = world + .soft_bodies + .add_cluster(handle, &[0], &mut world.bodies, &mut world.colliders) + .expect("the cluster is created"); + let proxy = world.soft_bodies[handle].cluster_proxy(cluster).unwrap(); + + // Particle 1 is not in the cluster, so no vertex may ride it. + let err = world + .insert_deformable( + boundary_mesh(&world, handle, proxy), + SoftMeshBinding::direct(vec![1; world.soft_bodies[handle].num_particles()]), + proxy, + ) + .unwrap_err(); + assert_eq!( + err, + SoftBindingError::VertexOutsideCluster { + vertex: 0, + particle: 1 + }, + "{err}" + ); + + // A binding by position finds no particle of that cluster near most vertices. + let err = world + .insert_deformable( + boundary_mesh(&world, handle, proxy), + SoftMeshBinding::direct_by_position(1.0e-4), + proxy, + ) + .unwrap_err(); + assert!( + matches!(err, SoftBindingError::UnmatchedVertex { .. }), + "{err}" + ); + assert_eq!(world.soft_bodies[handle].cluster(cluster).unwrap().meshes().count(), 0); +} + +#[test] +fn a_bad_parent_or_shape_is_rejected() { + let mut world = world_with_ground(); + let handle = cube(&mut world); + let (rigid, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(5.0, 5.0, 5.0)), + ColliderBuilder::ball(0.2), + ); + + // Not a cluster proxy. + let err = world + .insert_deformable( + boundary_mesh(&world, handle, rigid), + SoftMeshBinding::direct_by_position(1.0e-4), + rigid, + ) + .unwrap_err(); + assert_eq!(err, SoftBindingError::NotAClusterProxy); + + // Not a mesh. + let proxy = world.soft_bodies[handle].root_body(); + let err = world + .insert_deformable( + ColliderBuilder::ball(0.5), + SoftMeshBinding::direct_by_position(1.0e-4), + proxy, + ) + .unwrap_err(); + assert_eq!(err, SoftBindingError::UnsupportedShape); + + // A mesh without the deformable flag: its vertices could not be moved in place. + let sb = &world.soft_bodies[handle]; + let err = world + .insert_deformable( + ColliderBuilder::trimesh(sb.particle_positions().collect(), sb.boundary().to_vec()) + .unwrap() + .position(world.bodies[proxy].position().inverse()), + SoftMeshBinding::direct_by_position(1.0e-4), + proxy, + ) + .unwrap_err(); + assert_eq!(err, SoftBindingError::NotDeformable); + + // Both sets are untouched by a rejected insertion. + assert_eq!(world.soft_bodies[handle].meshes().count(), 1); + assert_eq!(world.bodies[proxy].colliders().len(), 1); +} + +/// The collider is attached like any other: its shape object, flags and pose relative to the +/// proxy are kept, and its vertices are rewritten in its own frame. +#[test] +fn a_deformable_collider_keeps_its_shape_and_pose() { + let mut world = world_with_ground(); + let handle = cube(&mut world); + let proxy = world.soft_bodies[handle].root_body(); + let sb = &world.soft_bodies[handle]; + let vertices: Vec = sb.particle_positions().collect(); + let indices: Vec<[u32; 3]> = sb.boundary().to_vec(); + let flags = TriMeshFlags::DEFORMABLE | TriMeshFlags::ORIENTED; + let given = world.bodies[proxy].position().inverse(); + let collider = world + .insert_deformable( + ColliderBuilder::trimesh_with_flags(vertices, indices, flags) + .unwrap() + .position(given), + SoftMeshBinding::direct_by_position(1.0e-4), + proxy, + ) + .unwrap(); + + let check = |world: &PhysicsWorld| { + let co = &world.colliders[collider]; + let wrt_parent = co.position_wrt_parent().unwrap(); + assert!( + (wrt_parent.translation - given.translation).length() < 1.0e-6, + "the pose relative to the proxy was changed: {wrt_parent:?} vs {given:?}" + ); + let trimesh = co.shape().as_trimesh().expect("still a trimesh"); + assert_eq!(trimesh.flags(), flags, "the shape's flags were changed"); + // The shape, through the collider's pose, is where the mesh's vertices are. + let sb = &world.soft_bodies[handle]; + let mesh = sb.mesh_of(collider).unwrap(); + for (local, expected) in trimesh.vertices().iter().zip(mesh.vertex_positions(sb)) { + let world_vertex = *co.position() * *local; + assert!( + (world_vertex - expected).length() < 1.0e-4, + "the shape drifted from its mesh: {world_vertex:?} vs {expected:?}" + ); + } + }; + check(&world); + for _ in 0..60 { + world.step(); + } + check(&world); +} + +#[test] +fn removing_a_deformable_collider_removes_its_mesh() { + let mut world = world_with_ground(); + let handle = cube(&mut world); + let proxy = world.soft_bodies[handle].root_body(); + let collider = world + .insert_deformable( + boundary_mesh(&world, handle, proxy), + SoftMeshBinding::direct_by_position(1.0e-4), + proxy, + ) + .unwrap(); + assert_eq!(world.soft_bodies[handle].meshes().count(), 2); + + world.remove_collider(collider); + assert_eq!(world.soft_bodies[handle].meshes().count(), 1); + assert!(world.soft_bodies[handle].mesh_of(collider).is_none()); + + for _ in 0..30 { + world.step(); + } + // The body keeps simulating through the mesh it has left. + assert!( + world.soft_bodies[handle] + .particle_positions() + .all(|p| p.is_finite()) + ); +} + +#[test] +fn a_mesh_on_a_sub_cluster_binds_to_its_own_particles() { + let mut world = world_with_ground(); + let handle = cube(&mut world); + // Three particles of the cube's lower face. + let lower: Vec = world.soft_bodies[handle] + .particles() + .iter() + .enumerate() + .filter(|(_, p)| p.position().y < 1.4) + .map(|(i, _)| i as u32) + .take(3) + .collect(); + assert_eq!(lower.len(), 3); + let cluster = world + .soft_bodies + .add_cluster(handle, &lower, &mut world.bodies, &mut world.colliders) + .unwrap(); + let proxy = world.soft_bodies[handle].cluster_proxy(cluster).unwrap(); + + // A quad over those four particles. + let sb = &world.soft_bodies[handle]; + let vertices: Vec = lower.iter().map(|&i| sb.particle_position(i as usize)).collect(); + let patch = world_mesh(&world, vertices, vec![[0, 1, 2]], proxy); + let collider = world + .insert_deformable( + patch, + SoftMeshBinding::direct_by_position(1.0e-4), + proxy, + ) + .expect("the patch binds to the lower-face cluster"); + + let mesh = world.soft_bodies[handle].mesh_of(collider).unwrap(); + assert_eq!(mesh.id().cluster, cluster); + assert_eq!(mesh.vertex_count(), 3); + + for _ in 0..120 { + world.step(); + } + let sb = &world.soft_bodies[handle]; + let mesh = sb.mesh_of(collider).unwrap(); + assert!(mesh.vertex_positions(sb).all(|v| v.is_finite())); + // The patch stayed on the particles it was bound to. + for (vertex, &particle) in mesh.vertex_positions(sb).zip(&lower) { + let expected = sb.particle_position(particle as usize); + assert!( + (vertex - expected).length() < 1.0e-4, + "the patch left its particle: {vertex:?} vs {expected:?}" + ); + } +} + +/// A tear rewires the cells to the split particles; a skinned mesh must follow its cells instead +/// of freezing where it was (its bindings used to be left untouched, so every vertex went +/// dangling). +#[test] +fn a_skinned_mesh_follows_its_cells_through_a_tear() { + let (vertices, indices) = Ball::new(1.0).to_trimesh(16, 16); + let vertices: Vec = vertices + .iter() + .map(|p| *p + Vector::new(0.0, 3.0, 0.0)) + .collect(); + let mut world = world_with_ground(); + let handle = world.insert_soft_body( + SoftBodyBuilder::volumetric_skinned(&vertices, &indices, 0.4) + .expect("the ball is filled") + .cell_model(SoftBodyCellModel::Corotational) + .skin_collision(true) + .particle_mass(0.05), + ); + + // Where every skin vertex sits just before the tear. + world.step(); + let before: Vec = { + let sb = &world.soft_bodies[handle]; + sb.collision_mesh().unwrap().vertex_positions(sb).collect() + }; + + // Tear a cell of the ball: a crack opens through it, under the skin. + let cells_before = world.soft_bodies[handle].cells().len(); + world.soft_bodies[handle].tear_cell(0); + world.step(); + assert!( + world.soft_bodies[handle].cells().len() < cells_before, + "nothing was torn" + ); + + let sb = &world.soft_bodies[handle]; + let mesh = sb.collision_mesh().expect("the skin is still the collision mesh"); + // Every vertex still rides a live cell. + match mesh.binding() { + SoftMeshMapping::Skinned { bindings } => { + for (vertex, binding) in bindings.iter().enumerate() { + assert!( + (binding.cell as usize) < sb.cells().len(), + "skin vertex {vertex} lost its cell" + ); + } + } + other => panic!("the collision mesh is not skinned: {other:?}"), + } + assert!(mesh.vertex_positions(sb).all(|v| v.is_finite())); + // A vertex left on a renumbered cell jumps across the body; every one of them must still be + // where it was, one step later. + assert_eq!(mesh.vertex_count(), before.len()); + let moved = mesh + .vertex_positions(sb) + .zip(&before) + .map(|(now, then)| (now - *then).length()) + .fold(0.0, Real::max); + assert!(moved < 0.1, "a skin vertex jumped {moved} through the tear"); +} + +/// `insert_deformable` takes the mesh in world space: a skinned mesh must stay where it was +/// given, not be displaced by the body's rest frame. +#[test] +fn a_skinned_mesh_binds_where_it_is_given() { + let mut world = world_with_ground(); + // At rest and far from the origin: binding against the rest positions (which are relative + // to the rest center of mass) lands the mesh a whole `center` away as soon as it is + // updated from the particles. + world.gravity = Vector::ZERO; + let center = Vector::new(-3.0, 1.5, 2.0); + let handle = world.insert_soft_body( + SoftBodyBuilder::cuboid(center, Vector::splat(0.5), 3, 3, 3).particle_mass(0.2), + ); + let (vertices, indices) = Cuboid::new(Vector::splat(0.7)).to_trimesh(); + let given: Vec = vertices.iter().map(|v| *v + center).collect(); + let proxy = world.soft_bodies[handle].root_body(); + let collider = world + .insert_deformable( + world_mesh(&world, given.clone(), indices, proxy), + SoftMeshBinding::skinned(), + proxy, + ) + .expect("the mesh binds to the whole-body cluster"); + + // The cached vertices are the input until the first update: step first. + for _ in 0..5 { + world.step(); + } + + let sb = &world.soft_bodies[handle]; + let mesh = sb.mesh_of(collider).unwrap(); + for (bound, given) in mesh.vertex_positions(sb).zip(&given) { + assert!( + (bound - *given).length() < 1.0e-4, + "the mesh was bound at {bound:?} instead of {given:?}" + ); + } +} diff --git a/crates/rapier3d/tests/soft_wires.rs b/crates/rapier3d/tests/soft_wires.rs new file mode 100644 index 000000000..bb9d1c308 --- /dev/null +++ b/crates/rapier3d/tests/soft_wires.rs @@ -0,0 +1,172 @@ +//! Wire collision meshes: a soft body whose collision geometry is a polyline (a rope in 3D) +//! rather than a surface. + +use rapier3d::prelude::*; + +fn world_with_ground() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(50.0, 0.5, 50.0).translation(Vector::new(0.0, -0.5, 0.0)), + ); + world +} + +fn rope(world: &mut PhysicsWorld, from: Vector, to: Vector, n: usize) -> SoftBodyHandle { + world.insert_soft_body(SoftBodyBuilder::rope(from, to, n).particle_mass(0.05)) +} + +fn lowest(world: &PhysicsWorld, handle: SoftBodyHandle) -> Real { + world.soft_bodies[handle] + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min) +} + +#[test] +fn a_rope_collides_through_a_polyline() { + let mut world = world_with_ground(); + let handle = rope( + &mut world, + Vector::new(-1.0, 2.0, 0.0), + Vector::new(1.0, 2.0, 0.0), + 12, + ); + + let sb = &world.soft_bodies[handle]; + let mesh = sb.collision_mesh().expect("the rope has a collision mesh"); + assert!(mesh.is_wire(), "the rope's mesh is not a wire"); + assert_eq!(mesh.arity(), 2); + assert!(!mesh.is_closed(), "a wire encloses nothing"); + assert_eq!( + world.colliders[mesh.collider()].shape().shape_type(), + ShapeType::Polyline + ); + + for _ in 0..300 { + world.step(); + } + let sb = &world.soft_bodies[handle]; + assert!(sb.particle_positions().all(|p| p.is_finite())); + let low = lowest(&world, handle); + assert!( + low > -0.05 && low < 0.2, + "the rope did not come to rest on the ground: {low}" + ); +} + +#[test] +fn two_ropes_collide_with_each_other() { + let mut world = world_with_ground(); + // A rope resting on the ground, and another dropped across it. + let lower = rope( + &mut world, + Vector::new(-1.0, 0.1, 0.0), + Vector::new(1.0, 0.1, 0.0), + 12, + ); + let upper = rope( + &mut world, + Vector::new(0.0, 1.0, -1.0), + Vector::new(0.0, 1.0, 1.0), + 12, + ); + + for _ in 0..400 { + world.step(); + } + + for handle in [lower, upper] { + assert!( + world.soft_bodies[handle] + .particle_positions() + .all(|p| p.is_finite()) + ); + } + // The upper rope rests *on* the lower one instead of passing through it: at the crossing, + // it stays above the lower rope's own height. + let upper_middle = world.soft_bodies[upper].particle_position(6).y; + let lower_top = lowest(&world, lower); + assert!( + upper_middle > lower_top + 0.05, + "the ropes passed through each other: {upper_middle} vs {lower_top}" + ); +} + +#[test] +fn a_rope_rests_on_a_cloth() { + let mut world = world_with_ground(); + // A cloth pinned by its corners, with a rope dropped on it. + let cloth = world.insert_soft_body( + SoftBodyBuilder::cloth( + Vector::new(-0.6, 1.0, -0.6), + Vector::X * 0.3, + Vector::Z * 0.3, + 5, + 5, + ) + .pinned_particles([0, 4, 20, 24]) + .particle_mass(0.05) + .self_contacts(true), + ); + let rope = rope( + &mut world, + Vector::new(-0.4, 1.6, 0.0), + Vector::new(0.4, 1.6, 0.0), + 10, + ); + + for _ in 0..400 { + world.step(); + } + + assert!( + world.soft_bodies[rope] + .particle_positions() + .all(|p| p.is_finite()) + ); + let rope_low = lowest(&world, rope); + let cloth_low = lowest(&world, cloth); + assert!( + rope_low > cloth_low - 0.05, + "the rope fell through the cloth: rope {rope_low}, cloth {cloth_low}" + ); + assert!(rope_low < 1.6, "the rope never landed: {rope_low}"); +} + +/// The wire's collider holds the *deformed* geometry, not a frozen copy moved rigidly by +/// the cluster frame: a rope pinned at both ends sags, and its shape sags with it. +#[test] +fn a_wire_collider_follows_the_particles() { + let mut world = world_with_ground(); + let handle = world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(-1.0, 2.0, 0.0), Vector::new(1.0, 2.0, 0.0), 12) + .pinned_particles([0, 11]) + .particle_mass(0.05), + ); + let collider = world.soft_bodies[handle] + .collision_mesh() + .unwrap() + .collider(); + + for _ in 0..200 { + world.step(); + } + + let sb = &world.soft_bodies[handle]; + // It actually sagged, or the test would pass on a rigid rope. + let middle = sb.particle_position(6).y; + assert!(middle < 1.9, "the rope did not sag: {middle}"); + + let co = &world.colliders[collider]; + let polyline = co.shape().as_polyline().expect("a wire is a polyline"); + assert_eq!(polyline.vertices().len(), sb.num_particles()); + for (i, vertex) in polyline.vertices().iter().enumerate() { + let world_vertex = *co.position() * *vertex; + let particle = sb.particle_position(i); + assert!( + (world_vertex - particle).length() < 1.0e-4, + "vertex {i} sits at {world_vertex:?} instead of its particle {particle:?}" + ); + } +} diff --git a/examples2d/soft_joints2.rs b/examples2d/soft_joints2.rs index f301e75f5..11c69f00c 100644 --- a/examples2d/soft_joints2.rs +++ b/examples2d/soft_joints2.rs @@ -24,7 +24,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { }) .particle_mass(0.08) .particle_radius(0.06) - .collider_template(ColliderBuilder::ball(0.06).friction(0.6)) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) }; /* @@ -152,7 +152,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { }) .particle_mass(0.08) .particle_radius(0.06) - .collider_template(ColliderBuilder::ball(0.06)), + .surface_collider(ColliderBuilder::ball(0.06)), ); let (left_half, right_half): (Vec, Vec) = { let sb = &world.soft_bodies[bar]; diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 2b52079fb..309fc59ac 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -83,6 +83,7 @@ mod soft_dress3; mod soft_fem3; mod soft_jelly3; mod soft_joints3; +mod soft_meshes3; mod soft_pile3; mod soft_plasticity3; mod soft_surface3; @@ -169,6 +170,7 @@ pub async fn main() { SOFT, "Cloth", soft_cloth3::run; SOFT, "Jelly", soft_jelly3::run; SOFT, "Soft joints", soft_joints3::run; + SOFT, "Cluster meshes", soft_meshes3::run; SOFT, "Deformable trimeshes", soft_surface3::run; SOFT, "Soft pile", soft_pile3::run; SOFT, "Thin features", soft_thin_features3::run; diff --git a/examples3d/soft_joints3.rs b/examples3d/soft_joints3.rs index 42828f120..848d6ac4d 100644 --- a/examples3d/soft_joints3.rs +++ b/examples3d/soft_joints3.rs @@ -24,7 +24,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { }) .particle_mass(0.08) .particle_radius(0.06) - .collider_template(ColliderBuilder::ball(0.06).friction(0.6)) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) }; /* @@ -204,7 +204,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { }) .particle_mass(0.08) .particle_radius(0.06) - .collider_template(ColliderBuilder::ball(0.06)), + .surface_collider(ColliderBuilder::ball(0.06)), ); let (left_half, right_half): (Vec, Vec) = { let sb = &world.soft_bodies[bar]; diff --git a/examples3d/soft_meshes3.rs b/examples3d/soft_meshes3.rs new file mode 100644 index 000000000..96bcae0df --- /dev/null +++ b/examples3d/soft_meshes3.rs @@ -0,0 +1,262 @@ +//! Collision meshes owned by clusters: a jelly wearing a fine skin over its coarse cells, a +//! jelly whose top cluster supports a rigid plate, and a jelly split into two clusters that +//! collide with each other. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::parry::shape::{Ball, TriMeshFlags}; +use rapier3d::prelude::*; + +/// A corotational jelly cube of `n` particles per side. +fn jelly(center: Vector, half_extents: Real, n: usize, young: Real) -> SoftBodyBuilder { + SoftBodyBuilder::cuboid(center, Vector::splat(half_extents), n, n, n) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.7)) +} + +/// The three jellies of this demo: skinned, plated, split. +fn build_world() -> anyhow::Result<(PhysicsWorld, [SoftBodyHandle; 3])> { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(12.0, 0.1, 12.0), + ); + + /* + * A jelly wearing a second, skinned ball-shaped sensor. + */ + let center = Vector::new(-3.0, 1.2, 0.0); + let skinned = world.insert_soft_body(jelly(center, 0.6, 4, 2.0e2)); + // The mesh is authored in world space, where the body is: placed by the inverse of the + // proxy's pose, like any collider attached to a body. + let (vertices, indices) = Ball::new(1.2).to_trimesh(20, 20); + let vertices: Vec = vertices.iter().map(|v| *v + center).collect(); + let proxy = world.soft_bodies[skinned].root_body(); + world.insert_deformable( + ColliderBuilder::trimesh_with_flags(vertices, indices, TriMeshFlags::DEFORMABLE)? + .position(world.bodies[proxy].position().inverse()) + .sensor(true), + SoftMeshBinding::skinned(), + proxy, + )?; + + /* + * A jelly whose top cluster supports a rigid plate: the load on the plate reaches the + * particles through the cluster's frame. + */ + let center = Vector::new(0.0, 1.2, 0.0); + let plated = world.insert_soft_body(jelly(center, 0.6, 4, 4.0e2)); + let top: Vec = world.soft_bodies[plated] + .particles() + .iter() + .enumerate() + .filter(|(_, p)| p.position().y > center.y + 0.3) + .map(|(i, _)| i as u32) + .collect(); + let cluster = world + .soft_bodies + .add_cluster(plated, &top, &mut world.bodies, &mut world.colliders) + .expect("the top cluster"); + let proxy = world.soft_bodies[plated].cluster_proxy(cluster).unwrap(); + world.colliders.insert_with_parent( + ColliderBuilder::cuboid(0.7, 0.05, 0.7).translation(Vector::new(0.0, 0.65, 0.0)), + proxy, + &mut world.bodies, + ); + + /* + * A jelly split into two clusters, each with a mesh of its own: they collide with each + * other (both opted into self contacts) because their clusters share no particle. + */ + let center = Vector::new(3.0, 1.2, 0.0); + let split = world.insert_soft_body(jelly(center, 0.6, 4, 4.0e2).no_surface_collider()); + let (left, right): (Vec, Vec) = world.soft_bodies[split] + .particles() + .iter() + .enumerate() + .map(|(i, p)| (i as u32, p.position().x < center.x)) + .fold((Vec::new(), Vec::new()), |mut sides, (i, is_left)| { + if is_left { + sides.0.push(i); + } else { + sides.1.push(i); + } + sides + }); + for half in [left, right] { + let cluster = world + .soft_bodies + .add_cluster(split, &half, &mut world.bodies, &mut world.colliders) + .expect("a half cluster"); + let proxy = world.soft_bodies[split].cluster_proxy(cluster).unwrap(); + let sb = &world.soft_bodies[split]; + // The closed boundary of the cells the half owns: its outer skin plus a cap over the + // cut, whose vertices are particles *inside* the body. A direct binding takes those + // like any other. + let (vertices, indices) = cluster_surface(sb, &half); + world.insert_deformable( + ColliderBuilder::trimesh_with_flags(vertices, indices, TriMeshFlags::DEFORMABLE)? + .position(world.bodies[proxy].position().inverse()) + .friction(0.7), + // Both halves opt in: their clusters share no particle, so their meshes may meet. + SoftMeshBinding::direct(half.clone()).self_contacts(true), + proxy, + )?; + } + + /* + * Something to drop on all three. + */ + for (i, x) in [-3.0, 0.0, 3.0].iter().enumerate() { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(*x, 4.0 + i as Real, 0.0)), + ColliderBuilder::cuboid(0.3, 0.3, 0.3).density(4.0), + ); + } + + Ok((world, [skinned, plated, split])) +} + +/// The closed surface of the cells a set of particles owns: every face that no second owned +/// cell shares, oriented outward, over those particles as vertices. A capped half of a body has +/// some surface vertices that sit inside the body rather than on its boundary. +fn cluster_surface(sb: &SoftBody, particles: &[u32]) -> (Vec, Vec<[u32; 3]>) { + use std::collections::HashMap; + + let mut vertex_of = vec![u32::MAX; sb.num_particles()]; + for (vertex, &particle) in particles.iter().enumerate() { + vertex_of[particle as usize] = vertex as u32; + } + let vertices: Vec = particles + .iter() + .map(|&i| sb.particle_position(i as usize)) + .collect(); + + // Every face of every owned cell, keyed by its sorted vertices: a face shared by two of + // them is interior to the half. + let mut faces: HashMap<[u32; 3], ([u32; 3], Vector, usize)> = HashMap::new(); + for cell in sb.cells() { + let cell = cell.vertices; + if !cell.iter().all(|v| vertex_of[*v as usize] != u32::MAX) { + continue; + } + let centroid = cell + .iter() + .map(|&v| sb.particle_position(v as usize)) + .sum::() + / 4.0; + for k in 0..4 { + let face = [cell[(k + 1) % 4], cell[(k + 2) % 4], cell[(k + 3) % 4]]; + let mut key = face; + key.sort_unstable(); + let entry = faces.entry(key).or_insert((face, centroid, 0)); + entry.2 += 1; + } + } + + let indices = faces + .values() + .filter(|(_, _, count)| *count == 1) + .map(|(face, centroid, _)| { + let p = face.map(|v| sb.particle_position(v as usize)); + // Outward winding: the face's normal must point away from its cell. + let normal = (p[1] - p[0]).cross(p[2] - p[0]); + let face = if normal.dot((p[0] + p[1] + p[2]) / 3.0 - *centroid) < 0.0 { + [face[0], face[2], face[1]] + } else { + *face + }; + face.map(|v| vertex_of[v as usize]) + }) + .collect(); + + (vertices, indices) +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let (mut world, _) = build_world()?; + viewer.set_world(&mut world); + viewer.look_at(Vector::new(-6.0, 4.0, 8.0), Vector::new(0.0, 1.0, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} + +#[cfg(test)] +mod tests { + use super::*; + + /// Each half of the split jelly is capped over the cut: a closed surface whose vertices + /// include particles from inside the body. + #[test] + fn the_split_jelly_halves_are_closed() { + let (world, [_, _, split]) = build_world().expect("the demo builds"); + let sb = &world.soft_bodies[split]; + let on_boundary = + |particle: u32| sb.boundary().iter().any(|element| element.contains(&particle)); + + let halves: Vec<_> = sb.meshes().filter(|mesh| mesh.id().cluster != 0).collect(); + assert_eq!(halves.len(), 2); + for mesh in halves { + assert!(mesh.is_closed(), "a half's surface is not closed"); + let SoftMeshMapping::Direct { particles } = mesh.binding() else { + panic!("a half's mesh is not bound to particles"); + }; + // The cap over the cut rides particles the body's own boundary does not include. + assert!( + particles.iter().copied().any(|p| !on_boundary(p)), + "no vertex of the cap rides an interior particle" + ); + // And it encloses a volume: the winding is outward and consistent. + let volume: Real = mesh + .indices() + .iter() + .map(|t| { + let p = t.map(|v| mesh.vertex(sb, v as usize)); + p[0].dot(p[1].cross(p[2])) / 6.0 + }) + .sum(); + assert!(volume > 0.05, "the half encloses no volume: {volume}"); + } + } + + /// The three cluster-mesh scenes settle: the meshes follow their particles, the plate + /// bears its load, and nothing explodes. + #[test] + fn cluster_meshes_settle() { + let (mut world, handles) = build_world().expect("the demo builds"); + for _ in 0..400 { + world.step(); + } + for handle in handles { + let sb = &world.soft_bodies[handle]; + assert!( + sb.particle_positions().all(|p| p.is_finite()), + "a body went non-finite" + ); + let com = sb.center_of_mass(); + assert!( + com.y > 0.0 && com.y < 3.0, + "a body left the scene (com {com:?})" + ); + for mesh in sb.meshes() { + assert!( + mesh.vertex_positions(sb).all(|v| v.is_finite()), + "a mesh went non-finite" + ); + } + } + } +} diff --git a/examples3d/soft_trimesh3.rs b/examples3d/soft_trimesh3.rs index 0f0b36c15..062b315d2 100644 --- a/examples3d/soft_trimesh3.rs +++ b/examples3d/soft_trimesh3.rs @@ -488,4 +488,75 @@ mod tests { assert!(min_y > -0.5, "{label} sank through the ground: {min_y}"); } } + + /// A model worn as a skin the body does not collide through is a drawn mesh (skinned, no + /// collider) drawn in place of the cells' boundary; a colliding skin gets a collider and + /// drops the boundary, and a body with no skin only has its boundary. + #[test] + fn a_worn_skin_is_a_drawn_mesh() { + let path = format!("{}/../{}", env!("CARGO_MANIFEST_DIR"), models()[0]); + let (vertices, indices) = load_obj(&path).unwrap_or_else(|| panic!("{path} missing")); + let meshing = Meshing::default(); + + // The three settings the demo offers, and what each leaves for the renderer: + // (wear the model as a skin, skin collisions) -> (drawn skins, colliding meshes). + for (wear_skin, skin_collision, drawn, colliding) in [ + (true, false, 1, 1), + (true, true, 0, 1), + (false, false, 0, 1), + ] { + let mut world = PhysicsWorld::new(); + let filled = fill_lattice(&vertices, &indices, &meshing).expect("filled"); + let filled = if wear_skin { + filled + .skin(vertices.clone(), indices.clone()) + .skin_collision(skin_collision) + } else { + filled + }; + let handle = world.insert_soft_body( + filled + .particle_mass(0.05) + .particle_radius(meshing.boundary_cell() * 0.25) + .surface_collider(ColliderBuilder::ball(0.1)), + ); + let body = &world.soft_bodies[handle]; + let label = format!("wear_skin={wear_skin}, skin_collision={skin_collision}"); + + let drawn_skins: Vec<_> = body + .meshes() + .filter(|mesh| mesh.is_skinned() && !mesh.collision_enabled()) + .collect(); + assert_eq!(drawn_skins.len(), drawn, "{label}: drawn skins"); + for skin in &drawn_skins { + // What the renderer builds its node from: the model's own geometry, with no + // collider to pick it up through the collider pass. + assert_eq!( + skin.collider(), + ColliderSet::invalid_handle(), + "{label}: the skin has a collider" + ); + assert_eq!( + skin.vertex_count(), + vertices.len(), + "{label}: skin vertices" + ); + assert_eq!( + skin.indices().len(), + indices.len(), + "{label}: skin triangles" + ); + assert!( + skin.vertex_positions(body).all(|v| v.is_finite()), + "{label}: the skin is not positioned" + ); + } + + let with_collider = body + .meshes() + .filter(|mesh| mesh.collision_enabled()) + .count(); + assert_eq!(with_collider, colliding, "{label}: colliding meshes"); + } + } } diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index da33fa17d..32676dee6 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,5 +1,5 @@ use crate::alloc_prelude::*; -use crate::dynamics::{IntegrationParameters, IslandManager, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, IslandManager, RigidBodySet, SoftBodySet}; use crate::geometry::{ BroadPhaseBvh, Collider, ColliderHandle, ColliderSet, CollisionEvent, NarrowPhase, }; @@ -10,8 +10,8 @@ use crate::prelude::{ActiveEvents, CollisionEventFlags}; use parry::query::sweep_toi::Sweep; use super::sweeps::{ - BodyContinuousResult, CcdTargets, PseudoHitMode, collect_fixed_targets, is_bullet, - map_bodies_parallel, sweep_fast_body, + BodyContinuousResult, CcdTargets, PseudoHitMode, collect_fixed_targets, collect_soft_targets, + is_bullet, map_bodies_parallel, sweep_fast_body, }; /// Continuous Collision Detection solver preventing fast objects from tunneling: @@ -168,6 +168,7 @@ impl CCDSolver { islands: &IslandManager, bodies: &mut RigidBodySet, colliders: &ColliderSet, + soft_bodies: &SoftBodySet, broad_phase: &mut BroadPhaseBvh, narrow_phase: &NarrowPhase, hooks: &dyn PhysicsHooks, @@ -209,8 +210,11 @@ impl CCDSolver { collect_fixed_targets(bodies, colliders, prediction), )); } + // Soft-body meshes deform every step without any scene change, so they are + // gathered fresh (one AABB per mesh, not a collider scan). + let soft = collect_soft_targets(soft_bodies, colliders, prediction); let targets = match &self.fixed_targets_cache.as_ref().unwrap().1 { - Some(fixed) => CcdTargets::FixedList(fixed), + Some(fixed) => CcdTargets::Lists { fixed, soft: &soft }, None => CcdTargets::FullBvh(bvh), }; let results = map_bodies_parallel(&non_bullets, hooks, |handle, hooks| { diff --git a/src/dynamics/ccd/sweeps.rs b/src/dynamics/ccd/sweeps.rs index f98eae590..3f576afd8 100644 --- a/src/dynamics/ccd/sweeps.rs +++ b/src/dynamics/ccd/sweeps.rs @@ -3,7 +3,7 @@ //! and the per-body continuous solve. use crate::alloc_prelude::*; -use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet, SoftBodySet}; use crate::geometry::{Collider, ColliderHandle, ColliderSet}; use crate::math::{Pose, Real, Vector}; use crate::parry::bounding_volume::Aabb; @@ -25,6 +25,13 @@ fn is_fixed_target(rb: Option<&RigidBody>) -> bool { rb.map(|b| b.is_fixed()).unwrap_or(true) } +/// Is `co` a target of the automatic (non-bullet) tier: fixed geometry, plus the deformable +/// collision meshes of soft bodies (surfaces of no thickness, never swept, taken at their +/// start-of-step geometry). +fn is_auto_target(rb: Option<&RigidBody>, co: &Collider) -> bool { + is_fixed_target(rb) || co.is_deformable_collider() +} + /// Is `rb` a bullet (a dynamic body with the full-CCD upgrade)? Bullets never sweep against /// other bullets. pub(super) fn is_bullet(rb: &RigidBody) -> bool { @@ -34,11 +41,11 @@ pub(super) fn is_bullet(rb: &RigidBody) -> bool { /// The target-selection rule for a fast body `rb1`: a non-bullet fast body only sweeps /// against automatic targets (fixed geometry and soft-body meshes); a bullet sweeps against /// every body type except other bullets. -fn tier_allows(rb1: &RigidBody, rb2: Option<&RigidBody>) -> bool { +fn tier_allows(rb1: &RigidBody, rb2: Option<&RigidBody>, co2: &Collider) -> bool { if is_bullet(rb1) { !rb2.map(is_bullet).unwrap_or(false) } else { - is_fixed_target(rb2) + is_auto_target(rb2, co2) } } @@ -132,7 +139,12 @@ fn intersect_swept_aabb<'a>( pub(super) enum CcdTargets<'a> { /// Flat lists of the automatic targets and their (slightly fattened) AABBs: the cached /// fixed colliders, and the soft-body collision meshes (rebuilt every step, as they deform). - FixedList(&'a [(ColliderHandle, Aabb)]), + Lists { + /// The fixed colliders. + fixed: &'a [(ColliderHandle, Aabb)], + /// The soft-body collision meshes. + soft: &'a [(ColliderHandle, Aabb)], + }, /// The full broad-phase BVH. FullBvh(&'a Bvh), } @@ -163,6 +175,28 @@ pub(super) fn collect_fixed_targets( Some(fixed) } +/// Collects the enabled soft-body collision meshes and their AABBs, fattened by the prediction +/// (speculative) distance. Rebuilt every step: unlike the fixed targets, a mesh deforms with its +/// particles without any scene change. +pub(super) fn collect_soft_targets( + soft_bodies: &SoftBodySet, + colliders: &ColliderSet, + prediction_distance: Real, +) -> Vec<(ColliderHandle, Aabb)> { + let mut soft = Vec::new(); + for (_, sb) in soft_bodies.iter() { + for mesh in sb.meshes() { + let ch = mesh.collider(); + let Some(co) = colliders.get(ch).filter(|co| co.is_enabled()) else { + continue; + }; + let aabb = co.shape.compute_aabb(&co.pos).loosened(prediction_distance); + soft.push((ch, aabb)); + } + } + soft +} + /// A convex piece of the fast collider: its point-cloud proxy plus its own sweep (the /// collider's sweep composed with the piece's local pose for compound children). struct FastSubShape<'a> { @@ -554,7 +588,7 @@ pub(super) fn sweep_fast_body( return; // Skip same body. } let rb2 = bh2.and_then(|h| bodies.get(h)); - if !tier_allows(rb1, rb2) { + if !tier_allows(rb1, rb2, co2) { return; } if !co1.flags.collision_groups.test(co2.flags.collision_groups) { @@ -595,8 +629,8 @@ pub(super) fn sweep_fast_body( }; match targets { - CcdTargets::FixedList(fixed) => { - for (ch2, aabb2) in fixed { + CcdTargets::Lists { fixed, soft } => { + for (ch2, aabb2) in fixed.iter().chain(soft.iter()) { if aabb2.intersects(&swept_aabb) { handle_candidate(*ch2, &colliders[*ch2]); } @@ -632,7 +666,7 @@ impl PhysicsHooks for HookProbe { ) -> Option { self.0.store(true, core::sync::atomic::Ordering::Relaxed); // Irrelevant: reaching this discards the whole pass in favour of the serial redo. - Some(crate::geometry::SolverFlags::COMPUTE_IMPULSES) + Some(crate::geometry::SolverFlags::COMPUTE_RIGID_IMPULSES) } } diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh.rs new file mode 100644 index 000000000..de0c611c3 --- /dev/null +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh.rs @@ -0,0 +1,149 @@ +//! The collision-mesh types: the mesh itself, its vertex mapping, its id and the remap tables. + +use crate::alloc_prelude::*; +use crate::geometry::ColliderHandle; +use crate::math::{DIM, Real, Vector}; + +use super::super::{SoftEdgeContact, SoftVertexContact}; +use super::super::{SoftOverlapState, SoftOverlapWarm, SoftVolumeContact}; + +/// Where a skinned collision-mesh vertex rides: the cell holding it, and its barycentric +/// coordinates in that cell. The coordinates are not clamped, so a vertex the cells do not cover +/// is bound to the closest one through an extrapolation and follows it. +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftMeshCellBinding { + /// The cell holding the vertex. + pub cell: u32, + /// Barycentric coordinates of the vertex in that cell, summing to one. + pub weights: [Real; DIM + 1], +} + +/// Where a collision mesh lives in its soft body: the cluster owning it, and its slot in that +/// cluster. Stable for the mesh's whole life (removed meshes leave a dead slot). +#[derive(Copy, Clone, Debug, Default, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftMeshId { + /// The cluster owning the mesh. + pub cluster: u32, + /// The mesh's slot in that cluster. + pub mesh: u32, +} + +/// A collision mesh of a given soft body: what a deformable collider points back to. +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftMeshRef { + /// The soft body owning the mesh. + pub body: crate::dynamics::SoftBodyHandle, + /// Where the mesh sits in it. + pub id: SoftMeshId, +} + +/// How a collision mesh's vertices are mapped onto a soft body's computational mesh, as +/// resolved at insertion (see [`crate::dynamics::SoftMeshBinding`] for the request). +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum SoftMeshMapping { + /// Vertex `i` of the mesh is the particle `particles[i]`: the mesh moves with the particles + /// one for one. + Direct { + /// The particle backing each vertex. + particles: Vec, + }, + /// Vertex `i` of the mesh rides a cell of the computational mesh (cage simulation): the + /// cells only have to contain the mesh, not resolve it. + Skinned { + /// Where each vertex rides. + bindings: Vec, + }, +} + +/// A mesh a soft body collides through: a polyline (2D) or triangle mesh (3D) whose vertices +/// follow the body's particles, and the collider holding it. A skinned mesh carries thin features +/// the coarse cells cannot: its vertices are barycentric combinations of the particles. +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftCollisionMesh { + /// How the vertices follow the computational mesh. + pub(crate) binding: SoftMeshMapping, + /// The mesh's elements: segments in 2D, triangles in 3D. + pub(crate) indices: Vec<[u32; DIM]>, + /// World-space vertices of a skinned mesh, updated from the particles after every step + /// (empty for a direct mesh, which reads its particles). + pub(crate) vertices: Vec, + /// Surface 1-ring of every vertex (CSR over `ring`): the vertices sharing an element with + /// it. Self contacts against those elements are excluded. + pub(crate) ring_offsets: Vec, + pub(crate) ring: Vec, + /// Elements incident to every vertex (CSR over `vertex_elements`). + pub(crate) vertex_elements_offsets: Vec, + pub(crate) vertex_elements: Vec, + /// The mesh's unique edges (`[a, b]` with `a < b`) and the edge ids of every element (3D: + /// the triangle edges; the segments are their own edges in 2D). + #[cfg(feature = "dim3")] + pub(crate) edges: Vec<[u32; 2]>, + #[cfg(feature = "dim3")] + pub(crate) element_edges: Vec<[u32; DIM]>, + /// The element owning each edge (the first one containing it): edge-vs-edge tests of an + /// element pair only involve the edges each element owns, so every edge pair is tested once. + #[cfg(feature = "dim3")] + pub(crate) edge_owners: Vec, + /// Whether the mesh is closed (every segment vertex / triangle edge shared by exactly two + /// elements): its contacts with other bodies are then oriented outward. + pub(crate) closed: bool, + /// Signed area/volume enclosed by the mesh at rest: with `inverted`, it says which way the + /// winding points. + pub(crate) rest_signed_volume: Real, + /// Whether the closed mesh is currently inside out (its signed volume, updated with the + /// particles, has the opposite sign to the rest one): the winding is mirrored as a whole, so + /// the outward normals flip with it. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) inverted: bool, + /// Whether the closed mesh's signed volume is currently so far below its rest magnitude + /// (inside the orientation hysteresis band) that the loop is likely self-crossed: part of + /// its winding is then mirrored and no global flag can say where the outside is. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) orientation_unreliable: bool, + /// The cell owning each element (`u32::MAX`: none), for a direct mesh: the winding of an + /// element whose cell is inverted no longer tells where the outside is. Empty for a skinned + /// mesh, which has no cell of its own to check. + pub(crate) element_cells: Vec, + /// Where this mesh lives in its soft body (its cluster and slot). + pub(crate) id: SoftMeshId, + /// Whether this mesh is the computational mesh's boundary: it is re-derived from it after + /// every topology change, instead of being remapped. + pub(crate) follows_boundary: bool, + /// Whether this mesh is a collision mesh (`false`: it is only drawn, and has no + /// collider). + pub(crate) collision_enabled: bool, + /// The collider holding this mesh's shape (invalid until the soft body is inserted in a + /// set, like [`SoftBody::root_body`], and for a mesh that is only drawn). + pub(crate) collider: ColliderHandle, + /// Whether this mesh collides with itself. + pub(crate) self_contacts: bool, + /// Edge-vs-edge contacts of the last step (self contacts and contacts with other soft + /// bodies' meshes, owned by the body with the smaller handle): warm-start state. + pub(crate) edge_contacts: Vec, + /// Vertex-vs-surface contacts of the last step against this mesh (self contacts and the + /// other soft bodies' vertices): warm-start state. + pub(crate) vertex_contacts: Vec, + /// Bumped whenever this mesh's elements change (a tear): renderers keying their mesh on it + /// rebuild it on change. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) topology_version: u32, + /// The world-space vertices as of the last [`Self::refresh_vertex_cache`] (before every + /// narrow-phase update): the contact passes read this contiguous copy instead of walking + /// the particle array. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) vertex_cache: Vec, +} + +/// How a topology change renumbered a soft body's cells and particles (old index to new, +/// `u32::MAX` removed, empty when unchanged), the split particles as `(copy, source)`, and the +/// segments a cut inserted into as `[a, b, p, q]` (`(a, b)` became `(a, p)` and `(q, b)`). +#[derive(Copy, Clone, Debug, Default)] +pub(crate) struct SoftTopologyRemap<'a> { + pub cells: &'a [u32], + pub particles: &'a [u32], +} diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs new file mode 100644 index 000000000..e87502c99 --- /dev/null +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs @@ -0,0 +1,285 @@ +//! Read accessors of a collision mesh: identity, elements and edges, vertices and the particles +//! a contact at a vertex acts through. + +use crate::geometry::ColliderHandle; +use crate::math::{DIM, Real, Vector}; + +use super::{SoftBody, SoftCollisionMesh, SoftMeshCellBinding, SoftMeshId, SoftMeshMapping}; + +impl SoftCollisionMesh { + /// Where this mesh lives in its soft body. + pub fn id(&self) -> SoftMeshId { + self.id + } + + /// Records where this mesh was put (called when it joins a cluster). + pub(crate) fn set_id(&mut self, id: SoftMeshId) { + self.id = id; + } + + /// The collider holding this mesh's shape (invalid for a mesh that is only drawn). + pub fn collider(&self) -> ColliderHandle { + self.collider + } + + /// Whether this mesh collides (`false`: it is only drawn). + pub fn collision_enabled(&self) -> bool { + self.collision_enabled + } + + /// The mesh's elements: segments in 2D, triangles in 3D, or segments in 3D for a wire + /// (`u32::MAX` fills the unused slot then; see [`Self::element`]). + pub fn indices(&self) -> &[[u32; DIM]] { + &self.indices + } + + /// The vertices of the `i`-th element: two for a segment, `DIM` for a facet. + #[inline] + pub fn element(&self, i: usize) -> &[u32] { + super::soft_body_builder::element_vertices(&self.indices[i]) + } + + /// How many vertices this mesh's elements have: `2` for a wire (a rope in 3D, and every + /// mesh in 2D), `DIM` for a surface. + #[inline] + pub fn arity(&self) -> usize { + self.indices.first().map_or(DIM, |e| { + super::soft_body_builder::element_vertices(e).len() + }) + } + + /// Whether this mesh is a wire (segments in 3D): a curve, with no inside and no facets. + #[inline] + pub fn is_wire(&self) -> bool { + self.arity() < DIM + } + + /// The edges the `i`-th element owns: its three edges for a triangle, itself for a segment. + pub(crate) fn element_edge_ids(&self, i: usize) -> impl Iterator + '_ { + #[cfg(feature = "dim3")] + let ids: &[u32] = if self.is_wire() { + &[] + } else { + super::soft_body_builder::element_vertices(&self.element_edges[i]) + }; + #[cfg(feature = "dim2")] + let ids: &[u32] = &[]; + let own = ids.is_empty().then_some(i as u32); + own.into_iter().chain(ids.iter().copied()) + } + + /// The two vertices of an edge id (see [`Self::element_edge_ids`]). + pub(crate) fn edge_vertices(&self, edge: u32) -> [u32; 2] { + #[cfg(feature = "dim3")] + if !self.is_wire() { + return self.edges[edge as usize]; + } + let element = self.element(edge as usize); + [element[0], element[1]] + } + + /// The element owning an edge: a segment owns itself. + pub(crate) fn edge_owner(&self, edge: u32) -> u32 { + #[cfg(feature = "dim3")] + if !self.is_wire() { + return self.edge_owners[edge as usize]; + } + edge + } + + /// How this mesh's vertices follow the computational mesh. + pub fn binding(&self) -> &SoftMeshMapping { + &self.binding + } + + /// Whether this mesh rides the body's cells rather than its particles. + pub fn is_skinned(&self) -> bool { + matches!(self.binding, SoftMeshMapping::Skinned { .. }) + } + + /// Whether this mesh is closed (its contacts with other bodies are then oriented outward). + pub fn is_closed(&self) -> bool { + self.closed + } + + /// Whether this mesh collides with itself. + pub fn self_contacts_enabled(&self) -> bool { + self.self_contacts + } + + /// The number of vertices of this mesh. + pub fn vertex_count(&self) -> usize { + match &self.binding { + SoftMeshMapping::Direct { particles } => particles.len(), + SoftMeshMapping::Skinned { .. } => self.vertices.len(), + } + } + + /// The cached world-space vertices of a skinned mesh (empty for a direct mesh, whose + /// vertices are its particles: see [`Self::vertex_positions`]). + pub fn vertices(&self) -> &[Vector] { + &self.vertices + } + + /// The world-space position of every vertex of this mesh. + pub fn vertex_positions<'a>( + &'a self, + body: &'a SoftBody, + ) -> impl ExactSizeIterator + 'a { + (0..self.vertex_count()).map(move |i| self.vertex(body, i)) + } + + /// The world position of the `i`-th vertex. + pub fn vertex(&self, body: &SoftBody, i: usize) -> Vector { + match &self.binding { + SoftMeshMapping::Direct { particles } => { + body.particles[particles[i] as usize].position + } + SoftMeshMapping::Skinned { .. } => self.vertices[i], + } + } + + /// The `i`-th vertex as cached by [`Self::refresh_vertex_cache`] (the narrow-phase passes + /// read the cache, refreshed right before every narrow-phase update). + #[inline] + pub(crate) fn cached_vertex(&self, i: usize) -> Vector { + self.vertex_cache[i] + } + + /// Refreshes the vertex cache from the particles (see [`Self::cached_vertex`]). + pub(crate) fn refresh_vertex_cache(&mut self, particles: &[super::super::SoftBodyParticle]) { + match &self.binding { + SoftMeshMapping::Direct { particles: map } => { + self.vertex_cache.clear(); + self.vertex_cache + .extend(map.iter().map(|&p| particles[p as usize].position)); + } + SoftMeshMapping::Skinned { .. } => { + self.vertex_cache.clear(); + self.vertex_cache.extend_from_slice(&self.vertices); + } + } + } + + /// The world velocity of the `i`-th vertex: a skinned vertex moves with the cell holding it. + pub fn vertex_velocity(&self, body: &SoftBody, i: usize) -> Vector { + match &self.binding { + SoftMeshMapping::Direct { particles } => { + body.particles[particles[i] as usize].velocity + } + SoftMeshMapping::Skinned { bindings } => { + let binding = &bindings[i]; + let Some(cell) = body.cells.get(binding.cell as usize) else { + return Vector::ZERO; + }; + let mut velocity = Vector::ZERO; + for (vid, weight) in cell.vertices.iter().zip(&binding.weights) { + velocity += body.particles[*vid as usize].velocity * *weight; + } + velocity + } + } + } + + /// Whether the `i`-th vertex belongs to any element of this mesh. + pub(crate) fn vertex_on_surface(&self, i: usize) -> bool { + match ( + self.vertex_elements_offsets.get(i), + self.vertex_elements_offsets.get(i + 1), + ) { + (Some(start), Some(end)) => end > start, + _ => false, + } + } + + /// The particles a contact at the `i`-th vertex acts through, and their weights: the + /// vertex's own particle for a direct mesh, the particles of the cell holding it for a + /// skinned one. + &self, + body: &SoftBody, + i: usize, + ) -> ([u32; DIM + 1], [Real; DIM + 1]) { + match &self.binding { + SoftMeshMapping::Direct { particles } => { + let mut ids = [u32::MAX; DIM + 1]; + let mut weights = [0.0; DIM + 1]; + ids[0] = particles[i]; + weights[0] = 1.0; + (ids, weights) + } + SoftMeshMapping::Skinned { bindings } => { + match body.cells.get(bindings[i].cell as usize) { + Some(cell) => (cell.vertices, bindings[i].weights), + None => ([u32::MAX; DIM + 1], [0.0; DIM + 1]), + } + } + } + } + + /// The particles a contact acts through and their weights (the element's own vertices for a + /// direct mesh, the bound cell's particles for a skinned one), plus the point the constraint + /// tracks; `weights` are the contact point's barycentric coordinates in `element`. + pub(crate) fn contact_anchors( + &self, + body: &SoftBody, + vertices: &[u32], + weights: &[Real], + point: Vector, + ) -> ([u32; DIM + 1], [Real; DIM + 1], Vector) { + let pad = |vertices: &[u32]| { + let mut ids = [u32::MAX; DIM + 1]; + let mut ws = [0.0; DIM + 1]; + let len = vertices.len().min(DIM + 1); + ids[..len].copy_from_slice(&vertices[..len]); + ws[..len].copy_from_slice(&weights[..len]); + (ids, ws, point) + }; + + match &self.binding { + SoftMeshMapping::Direct { particles } => { + let mut ids = [u32::MAX; DIM + 1]; + let mut ws = [0.0; DIM + 1]; + for k in 0..vertices.len().min(DIM + 1) { + ids[k] = particles[vertices[k] as usize]; + ws[k] = weights[k]; + } + (ids, ws, point) + } + SoftMeshMapping::Skinned { bindings } => self + .skinned_contact_anchors(body, bindings, vertices, weights, point) + .unwrap_or_else(|| pad(vertices)), + } + } + + fn skinned_contact_anchors( + &self, + body: &SoftBody, + bindings: &[SoftMeshCellBinding], + vertices: &[u32], + weights: &[Real], + point: Vector, + ) -> Option<([u32; DIM + 1], [Real; DIM + 1], Vector)> { + let first = bindings.get(*vertices.first()? as usize)?; + let shared = vertices + .iter() + .all(|v| bindings[*v as usize].cell == first.cell); + + if shared { + let cell = body.cells.get(first.cell as usize)?; + let mut blended = [0.0; DIM + 1]; + for (v, weight) in vertices.iter().zip(weights) { + for (w, bw) in blended.iter_mut().zip(&bindings[*v as usize].weights) { + *w += bw * weight; + } + } + return Some((cell.vertices, blended, point)); + } + + let heaviest = (0..vertices.len()).max_by(|a, b| weights[*a].total_cmp(&weights[*b]))?; + let vertex = vertices[heaviest] as usize; + let binding = &bindings[vertex]; + let cell = body.cells.get(binding.cell as usize)?; + Some((cell.vertices, binding.weights, self.vertices[vertex])) + } + +} diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_binding.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_binding.rs new file mode 100644 index 000000000..ba21b2426 --- /dev/null +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_binding.rs @@ -0,0 +1,238 @@ +//! Binding a collision mesh to a soft body's cells: the requested binding modes, the errors, and +//! the barycentric bindings themselves. + +use crate::alloc_prelude::*; +use crate::math::{DIM, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::{SoftBody, SoftBodyCell, SoftMeshCellBinding}; + +/// The barycentric coordinates of `point` in the cell `[a, b, c(, d)]`, extrapolating outside it. +/// +/// `None` for a degenerate cell, which has no frame to express the point in. +fn barycentric(point: Vector, cell: [Vector; DIM + 1]) -> Option<[Real; DIM + 1]> { + let frame = SoftBody::cell_edge_matrix(cell); + if SoftBody::cell_volume(cell).abs() <= Real::EPSILON { + return None; + } + + let coords = frame.inverse() * (point - cell[0]); + let mut weights = [0.0; DIM + 1]; + weights[0] = 1.0; + + for k in 0..DIM { + weights[k + 1] = coords[k]; + weights[0] -= coords[k]; + } + + Some(weights) +} + +/// The squared distance from `point` to the cell, zero inside it. +fn distance_squared_to_cell(point: Vector, cell: [Vector; DIM + 1]) -> Real { + use parry::query::PointQuery; + + #[cfg(feature = "dim2")] + { + let triangle = parry::shape::Triangle::new(cell[0], cell[1], cell[2]); + (triangle.project_local_point(point, true).point - point).length_squared() + } + #[cfg(feature = "dim3")] + { + // The distance to the closest of the four faces, zero when the point is behind all of + // them. + // TODO: refactor this into `Tetrahedron`. + let faces = [ + [cell[0], cell[2], cell[1]], + [cell[0], cell[1], cell[3]], + [cell[0], cell[3], cell[2]], + [cell[1], cell[2], cell[3]], + ]; + let mut inside = true; + let mut closest = Real::MAX; + + for face in faces { + let triangle = parry::shape::Triangle::new(face[0], face[1], face[2]); + let normal = (face[1] - face[0]).cross(face[2] - face[0]); + inside &= (point - face[0]).dot(normal) <= 0.0; + closest = closest + .min((triangle.project_local_point(point, true).point - point).length_squared()); + } + + if inside { 0.0 } else { closest } + } +} + +/// Binds a mesh to a soft body's cells: for every vertex, the closest cell and the vertex's +/// barycentric coordinates in it. +pub(crate) fn bind_to_cells( + vertices: &[Vector], + indices: &[[u32; DIM]], + cells: &[SoftBodyCell], + positions: &[Vector], +) -> Option> { + use parry::bounding_volume::Aabb; + use parry::partitioning::{Bvh, BvhBuildStrategy}; + use parry::query::PointQuery; + + if vertices.is_empty() || indices.is_empty() || cells.is_empty() { + return None; + } + + let cell_points = |cell: &SoftBodyCell| cell.vertices.map(|vid| positions[vid as usize]); + let leaves: Vec = cells + .iter() + .map(|cell| Aabb::from_points(cell_points(cell).iter().copied())) + .collect(); + let bvh = Bvh::from_leaves(BvhBuildStrategy::Binned, &leaves); + + let mut bindings = Vec::with_capacity(vertices.len()); + + for vertex in vertices { + // The closest cell: exact at the leaves, bounded by the node Aabbs on the way down. + let closest = bvh.find_best( + Real::MAX, + |node, _| node.aabb().distance_to_local_point(*vertex, true), + |cell, _| { + let points = cell_points(&cells[cell as usize]); + Some(distance_squared_to_cell(*vertex, points).sqrt()) + }, + ); + + let (cell, _) = closest?; + let binding = match barycentric(*vertex, cell_points(&cells[cell as usize])) { + Some(weights) => SoftMeshCellBinding { cell, weights }, + // The closest cell can be a sliver too flat to invert: the closest cell that is not + // holds the vertex instead (this checks all the cells so it's slow, bet it should + // almost never happen unless the input mesh is malformed). + None => { + let mut by_distance: Vec<(Real, u32)> = cells + .iter() + .enumerate() + .map(|(id, cell)| { + ( + distance_squared_to_cell(*vertex, cell_points(cell)), + id as u32, + ) + }) + .collect(); + by_distance.sort_unstable_by(|a, b| a.0.total_cmp(&b.0)); + by_distance.iter().find_map(|(_, cell)| { + let weights = barycentric(*vertex, cell_points(&cells[*cell as usize]))?; + Some(SoftMeshCellBinding { + cell: *cell, + weights, + }) + })? + } + }; + bindings.push(binding); + } + + Some(bindings) +} + +/// How a collision mesh should be bound to its cluster, requested at insertion +/// ([`crate::geometry::ColliderSet::insert_deformable`]). +#[derive(Clone, Debug)] +pub enum SoftMeshBindingMode { + /// Vertex `i` of the mesh is the particle `particles[i]` of the cluster. + Direct { + /// The particle backing each vertex. + particles: Vec, + }, + /// Each vertex is the particle of the cluster closest to it, within `eps`: the common case, + /// where the collision mesh was authored from the same vertices as the computational mesh. + DirectByPosition { + /// How far a vertex may sit from its particle. + eps: Real, + }, + /// Each vertex rides the cell of the cluster holding it (cage simulation): the cells only + /// have to contain the mesh, not resolve it. + Skinned, +} + +/// How a deformable collider follows its soft body, and the collision settings that are not the +/// collider's own. +#[derive(Clone, Debug)] +pub struct SoftMeshBinding { + pub(crate) mode: SoftMeshBindingMode, + pub(crate) self_contacts: bool, +} + +impl SoftMeshBinding { + /// Binds vertex `i` of the mesh to `particles[i]`, which must belong to the cluster. + pub fn direct(particles: Vec) -> Self { + Self::with_mode(SoftMeshBindingMode::Direct { particles }) + } + + /// Binds every vertex to the particle of the cluster closest to it, within `eps`. + pub fn direct_by_position(eps: Real) -> Self { + Self::with_mode(SoftMeshBindingMode::DirectByPosition { eps }) + } + + /// Binds every vertex to the cell of the cluster holding it (cage simulation). + pub fn skinned() -> Self { + Self::with_mode(SoftMeshBindingMode::Skinned) + } + + fn with_mode(mode: SoftMeshBindingMode) -> Self { + Self { + mode, + self_contacts: false, + } + } + + /// Enables collisions of this mesh with itself. + pub fn self_contacts(mut self, enabled: bool) -> Self { + self.self_contacts = enabled; + self + } +} + +/// Why a mesh could not be bound to a cluster +/// ([`crate::geometry::ColliderSet::insert_deformable`]). +#[derive(Copy, Clone, Debug, PartialEq, Eq, thiserror::Error)] +pub enum SoftBindingError { + /// The parent is not a live cluster proxy ([`crate::dynamics::SoftBodyCluster::proxy`]). + #[error("the parent body is not a soft-body cluster proxy")] + NotAClusterProxy, + /// The collider's shape is not a polyline (2D) or triangle mesh (3D). + #[error("a deformable collider must be a polyline or a trimesh")] + UnsupportedShape, + /// The collider's shape does not have the deformable flag (`TriMeshFlags::DEFORMABLE`, + /// `PolylineFlags::DEFORMABLE`), so its vertices cannot be moved in place. + #[error("a deformable collider's shape must have the deformable flag")] + NotDeformable, + /// The mesh has no vertex, or no element. + #[error("the mesh has no vertex or no element")] + DegenerateMesh, + /// A direct binding names a particle that does not belong to the cluster. + #[error("vertex {vertex} is bound to particle {particle}, which is not in the cluster")] + VertexOutsideCluster { + /// The mesh vertex. + vertex: u32, + /// The particle it names. + particle: u32, + }, + /// A direct binding by position found no particle of the cluster close enough to a vertex. + #[error("no particle of the cluster is close enough to vertex {vertex}")] + UnmatchedVertex { + /// The mesh vertex. + vertex: u32, + }, + /// A skinned binding found no cell of the cluster to hold a vertex. + #[error("no cell of the cluster can hold vertex {vertex}")] + UnboundVertex { + /// The mesh vertex. + vertex: u32, + }, +} + +/// The binding of `position` to the closest cell of `body` accepted by `allowed` (`None` without +/// any such cell). +// TODO: optimize neighrest neighbor search with the BVH? +pub(super) fn closest_cell_binding(body: &SoftBody, position: Vector) -> Option { +} diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs new file mode 100644 index 000000000..96266b2f2 --- /dev/null +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs @@ -0,0 +1 @@ + /// carries a spurious normal. diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs new file mode 100644 index 000000000..36e8aecfd --- /dev/null +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs @@ -0,0 +1,375 @@ +//! Construction of a collision mesh, its topology tables, and the per-step update of its +//! vertices, orientation and collider shape. + +use crate::alloc_prelude::*; +use crate::geometry::ColliderHandle; +use crate::math::{DIM, Pose, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::collision_mesh_binding::{bind_to_cells, closest_cell_binding}; +use super::{ + SoftBody, SoftBodyCell, SoftCollisionMesh, SoftMeshId, SoftMeshMapping, SoftTopologyRemap, + surface_element_cells, surface_is_closed, surface_rings, surface_vertex_elements, +}; + +impl SoftCollisionMesh { + /// The mesh a body collides through when it collides through its own boundary: one vertex + /// per particle (interior particles included, so vertex indices are particle indices), the + /// cells' boundary as elements. + pub(crate) fn boundary( + body: &SoftBody, + self_contacts: bool, + collision_enabled: bool, + ) -> Self { + let num_vertices = body.particles.len(); + let indices = body.boundary.clone(); + let (ring_offsets, ring) = surface_rings(num_vertices, &indices); + let (vertex_elements_offsets, vertex_elements) = + surface_vertex_elements(num_vertices, &indices); + #[cfg(feature = "dim3")] + let (edges, element_edges, edge_owners) = + super::soft_body_builder::surface_edge_table(&indices); + + Self { + binding: SoftMeshMapping::Direct { + particles: (0..num_vertices as u32).collect(), + }, + id: SoftMeshId { cluster: 0, mesh: 0 }, + follows_boundary: true, + closed: surface_is_closed(&indices), + rest_signed_volume: SoftBody::boundary_volume(&indices, |i| { + body.particles[i as usize].rest_position + }), + element_cells: Vec::new(), + indices, + vertices: Vec::new(), + ring_offsets, + ring, + vertex_elements_offsets, + vertex_elements, + #[cfg(feature = "dim3")] + edges, + #[cfg(feature = "dim3")] + element_edges, + #[cfg(feature = "dim3")] + edge_owners, + inverted: false, + orientation_unreliable: false, + collision_enabled, + collider: ColliderHandle::invalid(), + self_contacts, + edge_contacts: Vec::new(), + vertex_contacts: Vec::new(), + topology_version: 0, + } + } + + /// A mesh bound to a cluster through `mapping`, whose vertices are at `vertices` when the + /// binding is taken. + pub(crate) fn new( + mapping: SoftMeshMapping, + indices: Vec<[u32; DIM]>, + vertices: Vec, + body: &SoftBody, + self_contacts: bool, + ) -> Self { + let num_vertices = vertices.len(); + let (ring_offsets, ring) = surface_rings(num_vertices, &indices); + let (vertex_elements_offsets, vertex_elements) = + surface_vertex_elements(num_vertices, &indices); + #[cfg(feature = "dim3")] + let (edges, element_edges, edge_owners) = + super::soft_body_builder::surface_edge_table(&indices); + let skinned = matches!(mapping, SoftMeshMapping::Skinned { .. }); + // A wire has no facet, so no owning cell to read a winding from. + let wire = indices + .first() + .is_some_and(|e| super::soft_body_builder::element_vertices(e).len() < DIM); + + Self { + closed: surface_is_closed(&indices), + // A wire encloses nothing: its "volume" is not a quantity. + rest_signed_volume: if wire { + 0.0 + } else { + SoftBody::boundary_volume(&indices, |i| vertices[i as usize]) + }, + element_cells: if skinned || wire { + Vec::new() + } else { + surface_element_cells(&indices, &body.cells) + }, + binding: mapping, + id: SoftMeshId::default(), + follows_boundary: false, + indices, + // A direct mesh reads its particles; only a skinned one caches its vertices. + vertices: if skinned { vertices } else { Vec::new() }, + ring_offsets, + ring, + vertex_elements_offsets, + vertex_elements, + #[cfg(feature = "dim3")] + edges, + #[cfg(feature = "dim3")] + element_edges, + #[cfg(feature = "dim3")] + edge_owners, + inverted: false, + orientation_unreliable: false, + collision_enabled: true, + collider: ColliderHandle::invalid(), + self_contacts, + edge_contacts: Vec::new(), + vertex_contacts: Vec::new(), + topology_version: 0, + } + } + + /// A wire a body collides through: its segments over the particles, one vertex per particle + /// (vertex indices are particle indices, like the boundary mesh). A wire is a curve that + /// encloses nothing, so it is never closed and its contacts are always two-sided. + #[cfg(feature = "dim3")] + pub(crate) fn wire( + body: &SoftBody, + segments: &[[u32; 2]], + self_contacts: bool, + collision_enabled: bool, + ) -> Self { + let indices: Vec<[u32; DIM]> = segments + .iter() + .map(|s| core::array::from_fn(|k| if k < 2 { s[k] } else { u32::MAX })) + .collect(); + let mut mesh = Self::new( + SoftMeshMapping::Direct { + particles: (0..body.particles.len() as u32).collect(), + }, + indices, + body.particles.iter().map(|p| p.position).collect(), + body, + self_contacts, + ); + mesh.collision_enabled = collision_enabled; + mesh + } + + /// A mesh bound to the body's cells: for every vertex, the closest cell and the vertex's + /// barycentric coordinates in it. `positions` are the particle positions the cells are in + /// when the binding is taken, which the mesh's vertices are expected to match. + pub(crate) fn skinned( + vertices: &[Vector], + indices: &[[u32; DIM]], + cells: &[SoftBodyCell], + positions: &[Vector], + self_contacts: bool, + collision_enabled: bool, + ) -> Option { + let bindings = bind_to_cells(vertices, indices, cells, positions)?; + let (ring_offsets, ring) = surface_rings(vertices.len(), indices); + let (vertex_elements_offsets, vertex_elements) = + surface_vertex_elements(vertices.len(), indices); + #[cfg(feature = "dim3")] + let (edges, element_edges, edge_owners) = super::soft_body_builder::surface_edge_table(indices); + + Some(Self { + binding: SoftMeshMapping::Skinned { bindings }, + id: SoftMeshId { cluster: 0, mesh: 0 }, + follows_boundary: false, + closed: surface_is_closed(indices), + rest_signed_volume: SoftBody::boundary_volume(indices, |i| vertices[i as usize]), + element_cells: Vec::new(), + indices: indices.to_vec(), + vertices: vertices.to_vec(), + ring_offsets, + ring, + vertex_elements_offsets, + vertex_elements, + #[cfg(feature = "dim3")] + edges, + #[cfg(feature = "dim3")] + element_edges, + #[cfg(feature = "dim3")] + edge_owners, + inverted: false, + orientation_unreliable: false, + collision_enabled, + collider: ColliderHandle::invalid(), + self_contacts, + edge_contacts: Vec::new(), + vertex_contacts: Vec::new(), + topology_version: 0, + }) + } + + /// The vertices of this mesh's collider shape, expressed in the collider's frame (`pose` is + /// its world pose: the cluster proxy's pose composed with the collider's pose relative to + /// it). A rigid motion of the body moves the frame, not the vertices. + pub(crate) fn local_vertices(&self, body: &SoftBody, pose: &Pose) -> Vec { + (0..self.vertex_count()) + .map(|i| pose.inverse_transform_point(self.vertex(body, i))) + .collect() + } + + /// Moves the vertices of this mesh's collider shape to the current positions, in place: + /// parry refits the shape's BVH. + pub(crate) fn deform_shape( + &self, + body: &SoftBody, + pose: &Pose, + shape: &mut dyn parry::shape::Shape, + ) { + let write = |vertices: &mut [Vector]| { + debug_assert_eq!(vertices.len(), self.vertex_count()); + for (i, v) in vertices.iter_mut().enumerate() { + *v = pose.inverse_transform_point(self.vertex(body, i)); + } + }; + // A polyline in either dimension (a 2D surface, or a wire in 3D), a trimesh in 3D. + if let Some(polyline) = shape.as_polyline_mut() { + polyline.update_vertices(write); + return; + } + #[cfg(feature = "dim3")] + if let Some(trimesh) = shape.as_trimesh_mut() { + trimesh.update_vertices(write); + } + } + + /// Moves a skinned mesh to the current particle positions (a direct mesh reads them + /// directly and has nothing to update). + pub(crate) fn update(&mut self, cells: &[SoftBodyCell], particles: &[super::SoftBodyParticle]) { + let SoftMeshMapping::Skinned { bindings } = &self.binding else { + return; + }; + for (vertex, binding) in self.vertices.iter_mut().zip(bindings) { + let Some(cell) = cells.get(binding.cell as usize) else { + continue; + }; + let mut skinned = Vector::ZERO; + for (vid, weight) in cell.vertices.iter().zip(&binding.weights) { + skinned += particles[*vid as usize].position * *weight; + } + *vertex = skinned; + } + } + + /// Updates the inside-out state of a closed mesh from the current vertex positions. + pub(crate) fn update_orientation(&mut self, body: &SoftBody) { + if !self.closed || self.rest_signed_volume == 0.0 { + return; + } + // With hysteresis: a crumpled body hovering around a zero volume must not flip its + // orientation (contact normals) every step. + let volume = SoftBody::boundary_volume(&self.indices, |i| self.vertex(body, i as usize)); + let ratio = volume / self.rest_signed_volume; + if ratio < -0.25 { + self.inverted = true; + } else if ratio > 0.25 { + self.inverted = false; + } + // Inside the band the loop is likely self-crossed (an O squeezed into an 8): part of the + // winding is mirrored, so no global orientation is trustworthy. + self.orientation_unreliable = ratio.abs() <= 0.25; + } + + /// A counter bumped whenever this mesh's topology changes: a renderer keying its mesh on it + /// rebuilds only what changed. + pub fn topology_version(&self) -> u32 { + self.topology_version + } + + /// Rebuilds the tables derived from the elements after a topology change (a tear). + pub(crate) fn rebuild_tables(&mut self, body: &SoftBody) { + self.topology_version = self.topology_version.wrapping_add(1); + let num_vertices = self.vertex_count(); + let (offsets, ring) = surface_rings(num_vertices, &self.indices); + self.ring_offsets = offsets; + self.ring = ring; + let (offsets, elements) = surface_vertex_elements(num_vertices, &self.indices); + self.vertex_elements_offsets = offsets; + self.vertex_elements = elements; + self.closed = surface_is_closed(&self.indices); + if !self.is_skinned() && !self.follows_boundary { + self.element_cells = surface_element_cells(&self.indices, &body.cells); + } + #[cfg(feature = "dim3")] + { + let (edges, element_edges, owners) = + super::soft_body_builder::surface_edge_table(&self.indices); + self.edges = edges; + self.element_edges = element_edges; + self.edge_owners = owners; + } + } + + /// Follows the body through a topology change (tear, cut, cluster removal): a boundary mesh is + /// re-derived, a direct mesh follows its particles ([`follow_insertions`], [`follow_splits`]), + /// a skinned mesh follows its cells and comes apart along the cracks ([`follow_pieces`]). + pub(crate) fn remap_topology(&mut self, body: &SoftBody, remap: &SoftTopologyRemap) { + match &mut self.binding { + SoftMeshMapping::Skinned { bindings } => { + if !remap.cells.is_empty() { + for binding in bindings.iter_mut() { + binding.cell = remap + .cells + .get(binding.cell as usize) + .copied() + .unwrap_or(u32::MAX); + } + } + let dangling: Vec = bindings + .iter() + .enumerate() + .filter(|(_, binding)| body.cells.get(binding.cell as usize).is_none()) + .map(|(i, _)| i) + .collect(); + for vertex in dangling { + let position = self.vertices[vertex]; + if let Some(binding) = closest_cell_binding(body, position) { + bindings[vertex] = binding; + } + } + } + SoftMeshMapping::Direct { particles } if self.follows_boundary => { + self.indices = body.boundary.clone(); + *particles = (0..body.particles.len() as u32).collect(); + self.rest_signed_volume = SoftBody::boundary_volume(&self.indices, |i| { + body.particles[i as usize].rest_position + }); + } + SoftMeshMapping::Direct { particles } => { + if remap.particles.is_empty() { + return; + } + let mut vertex_remap = vec![u32::MAX; particles.len()]; + let mut kept = Vec::with_capacity(particles.len()); + for (vertex, particle) in particles.iter().enumerate() { + let new = remap.particles.get(*particle as usize).copied(); + if let Some(new) = new.filter(|p| *p != u32::MAX) { + vertex_remap[vertex] = kept.len() as u32; + kept.push(new); + } + } + *particles = kept; + self.indices.retain(|element| { + element + .iter() + .all(|v| vertex_remap[*v as usize] != u32::MAX) + }); + for element in &mut self.indices { + for v in element.iter_mut() { + *v = vertex_remap[*v as usize]; + } + } + } + } + } + + /// Clears the contact state kept across steps (a topology change invalidated its ids). + pub(crate) fn clear_contacts(&mut self) { + self.edge_contacts.clear(); + self.vertex_contacts.clear(); + } +} diff --git a/src/dynamics/soft_body/collision_mesh/mod.rs b/src/dynamics/soft_body/collision_mesh/mod.rs new file mode 100644 index 000000000..8a2534518 --- /dev/null +++ b/src/dynamics/soft_body/collision_mesh/mod.rs @@ -0,0 +1,22 @@ +//! The mesh a soft body meets the world through, as opposed to the computational mesh +//! ([`SoftBody`]'s particles, edges and cells) it is simulated on. It maps onto that mesh +//! ([`SoftMeshMapping`]) directly (vertex `i` is a particle) or by skinning (riding a cell). + +mod collision_mesh; +mod collision_mesh_accessors; +mod collision_mesh_binding; +mod collision_mesh_geometry; +mod collision_mesh_topology; + +pub use self::collision_mesh_binding::{SoftBindingError, SoftMeshBinding, SoftMeshBindingMode}; +pub(crate) use self::collision_mesh_binding::bind_to_cells; +pub use self::collision_mesh::{ + SoftCollisionMesh, SoftMeshCellBinding, SoftMeshId, SoftMeshMapping, SoftMeshRef, +}; +pub(crate) use self::collision_mesh::SoftTopologyRemap; +pub(super) use super::SoftBodyParticle; +pub(super) use super::soft_body_builder; +pub(super) use super::soft_body_builder::{ + surface_element_cells, surface_is_closed, surface_rings, surface_vertex_elements, +}; +pub(super) use super::{SoftBody, SoftBodyCell}; 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 0b04ab5f1..4f024bdcc 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 @@ -94,6 +94,8 @@ impl SoftBodyBuilder { stiffness_scale: 1.0, color: 0, torn: false, + }); + } // Surface: given, or the boundary of the cells. let surface = if !self.surface.is_empty() { self.surface.clone() @@ -186,4 +188,45 @@ impl SoftBodyBuilder { } } +impl SoftBodyBuilder { + /// The meshes of `body`, built at insertion once it has its whole-body cluster: its skin, + /// bound to the cells at the rest positions, and its boundary, kept only when the body collides + /// through it (a body with a collision skin collides through the skin alone). + pub(crate) fn build_meshes(&self, body: &SoftBody) -> Vec { + let has_collider = self.collider_template.is_some(); + let skin = self.skin.as_ref().and_then(|(vertices, indices)| { + SoftCollisionMesh::skinned( + vertices, + indices, + &body.cells, + &self.positions, + self.self_contacts, + has_collider && self.skin_collision, + ) + }); + let skin_collides = skin.as_ref().is_some_and(|mesh| mesh.collision_enabled()); + let mut meshes = Vec::new(); + if let Some(skin) = skin { + meshes.push(skin); + } + // A wire replaces the boundary mesh: a body made of segments has no surface. + #[cfg(feature = "dim3")] + if !skin_collides && !self.wire.is_empty() { + meshes.push(SoftCollisionMesh::wire( + body, + &self.wire, + self.self_contacts, + has_collider, + )); + return meshes; + } + if !skin_collides { + meshes.push(SoftCollisionMesh::boundary( + body, + self.self_contacts, + has_collider, + )); + } + meshes + } } diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs index e2607b5a8..415ca2497 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs @@ -95,6 +95,10 @@ pub struct SoftBodyBuilder { /// Mesh held by the cells, if any: `(vertices, elements)` in world space, bound to the /// cells at build time (see [`super::SoftBodySkin`]). pub skin: Option<(Vec, Vec<[u32; DIM]>)>, + /// The segments the body collides through when it has no surface: a rope or a wire (3D + /// only; in 2D the surface is already made of segments). + #[cfg(feature = "dim3")] + pub wire: Vec<[u32; 2]>, /// Whether the body meets the world through its skin rather than through its cells' boundary /// (off by default, and no effect without a skin). pub skin_collision: bool, diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs index b52f7c44b..39ddbbf3c 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs @@ -40,7 +40,45 @@ pub(super) fn mean_edge_length(positions: &[Vector], edges: &[[u32; 2]]) -> Real / edges.len() as Real } +/// An element's vertices: its slots up to the first unused one (`u32::MAX`), so a segment +/// element of a wire mesh in 3D reads as the two vertices it has. +pub(crate) fn element_vertices(element: &[u32; DIM]) -> &[u32] { + let used = element + .iter() + .position(|v| *v == u32::MAX) + .unwrap_or(element.len()); + &element[..used] +} + +pub(crate) fn surface_is_closed(surface: &[[u32; DIM]]) -> bool { + if surface.is_empty() { + return false; + } + // Only a mesh of full facets encloses anything: a wire (segments in 3D) is a curve. + if surface + .iter() + .any(|element| element_vertices(element).len() != DIM) + { + return false; + } + let mut counts: HashMap<[u32; DIM - 1], u32> = HashMap::default(); + for element in surface { + for k in 0..DIM { + // The facet opposite to vertex k, as a sorted key. + let mut facet = [0u32; DIM - 1]; + let mut n = 0; + for (j, &v) in element.iter().enumerate() { + if j != k { + facet[n] = v; + n += 1; + } + } + facet.sort_unstable(); + *counts.entry(facet).or_insert(0) += 1; + } + } counts.values().all(|&c| c == 2) +} /// The cell owning each surface element (`u32::MAX` when no cell contains its vertices). pub(crate) fn surface_element_cells(surface: &[[u32; DIM]], cells: &[SoftBodyCell]) -> Vec { @@ -85,6 +123,20 @@ pub(crate) fn surface_edge_table( let mut element_edges = Vec::with_capacity(surface.len()); for (ei, element) in surface.iter().enumerate() { let mut e = [u32::MAX; DIM]; + // A segment element is its own single edge (2D, and a wire in 3D). + let arity = element_vertices(element).len(); + if arity < DIM { + let (a, b) = (element[0], element[1]); + let key = [a.min(b), a.max(b)]; + let id = *ids.entry(key).or_insert_with(|| { + edges.push(key); + owners.push(ei as u32); + edges.len() as u32 - 1 + }); + e[0] = id; + element_edges.push(e); + continue; + } for k in 0..DIM { let a = element[(k + 1) % DIM]; let b = element[(k + 2) % DIM]; @@ -128,6 +180,7 @@ pub(crate) fn surface_vertex_elements( pub(crate) fn surface_rings(num_particles: usize, surface: &[[u32; DIM]]) -> (Vec, Vec) { let mut rings: Vec> = vec![Vec::new(); num_particles]; for element in surface { + let element = element_vertices(element); for &a in element { for &b in element { if a != b { diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs index c5c6b5b47..a70d2bbbb 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs @@ -195,31 +195,31 @@ impl SoftBodyBuilder { self } - /// Sets the mesh the cells carry: the body's default collision mesh is then that mesh, - /// bound to the cells ([`crate::dynamics::SoftMeshMapping::Skinned`]), instead of the cells' boundary. - /// - /// The vertices are in world space, in the pose the cells are built in; each is bound to the - /// cell closest to it. The mesh adds no particle and no constraint row, so it can be as - /// detailed as wanted, and it can hold features the cells are far too coarse to resolve. + /// Sets the mesh the cells hold: the body's default collision mesh is then that mesh, bound to + /// the cells ([`crate::dynamics::SoftMeshMapping::Skinned`]) instead of the cells' boundary. + /// The vertices are in world space in the cells' build pose, each bound to the closest cell. pub fn skin(mut self, vertices: Vec, indices: Vec<[u32; DIM]>) -> Self { self.skin = Some((vertices, indices)); self } /// Whether the body's skin is its collision mesh rather than its cells' boundary (off by - /// default, and no effect without a skin). Further meshes, skinned or not, are added after - /// insertion with [`crate::geometry::ColliderSet::insert_deformable`]. - /// - /// A skin is what the body looks like, so it is also what the body could collide as: the - /// contact is then resolved through the particles of the cell carrying the touched part of - /// the skin, and the skin needs to be at least as fine as the cells. Off, collisions stay on - /// the cells, which is coarser but is what the expulsion and internal-feature rules were - /// written for. + /// default, and no effect without a skin); on, a contact is resolved through the particles of + /// the cell holding the touched part of the skin, which must be at least as fine as the cells. pub fn skin_collision(mut self, enabled: bool) -> Self { self.skin_collision = enabled; self } + /// Sets the segments the body collides through: a rope or a wire, whose collision mesh is a + /// polyline rather than a surface (3D only; in 2D [`Self::surface`] already takes segments). + /// A wire encloses nothing, so its contacts are two-sided and it never expels anything. + #[cfg(feature = "dim3")] + pub fn wire(mut self, segments: Vec<[u32; 2]>) -> Self { + self.wire = segments; + self + } + /// Sets the computational mesh's boundary elements (segments in 2D, triangles in 3D), /// oriented outward: what the volume constraint integrates over, and the geometry of the default /// collision mesh. @@ -293,6 +293,12 @@ impl SoftBodyBuilder { self } + /// Configures the collider of the body's default collision mesh (its cells' boundary, or its + /// skin under [`Self::skin_collision`]): friction, restitution, groups, events. The shape is + /// the mesh's own; the density is ignored (mass from [`Self::mass`] or [`Self::masses`]). + pub fn surface_collider(mut self, collider: ColliderBuilder) -> Self { + self.collider_template = Some(collider); + self } /// Gives the body no default collision mesh collider: it collides through the meshes added diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs index 151df0a26..094a3aab6 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs @@ -43,6 +43,8 @@ impl SoftBodyBuilder { self_contacts: false, particle_radius: 0.01, skin: None, + #[cfg(feature = "dim3")] + wire: Vec::new(), skin_collision: false, collider_template: Some(ColliderBuilder::ball(0.05).density(0.0)), particle_settings: SoftBodyParticleSettings::default(), @@ -184,8 +186,11 @@ impl SoftBodyBuilder { .collect(); let segment = (end - start).length() / (num_particles - 1) as Real; let builder = Self::new(positions).particle_radius(segment * 0.5); + // The rope collides through its own segments: a polyline surface in 2D, a wire in 3D. #[cfg(feature = "dim2")] let builder = builder.surface(edges.clone()); + #[cfg(feature = "dim3")] + let builder = builder.wire(edges.clone()); builder.edges(edges).bend_edges(bend_edges) } diff --git a/src/dynamics/soft_body/soft_body_cluster.rs b/src/dynamics/soft_body/soft_body_cluster.rs index 955df4dc6..11998b004 100644 --- a/src/dynamics/soft_body/soft_body_cluster.rs +++ b/src/dynamics/soft_body/soft_body_cluster.rs @@ -35,15 +35,19 @@ pub struct SoftBodyCluster { pub(crate) shape_matching_target: Option, #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) prev_shape_matching_target: Option, - /// The rigid velocity gathered from the particles when the proxy was last refreshed: the + /// The rigid velocity gathered from the particles when the proxy was last updated: the /// proxy's `rb.vels` was set to exactly this, so `rb.vels - last_gather` is the external /// impulse the user applied to the proxy since (scattered by the solver's first pass). #[cfg_attr(feature = "serde-serialize", serde(default))] pub(crate) last_gather: (Vector, AngVector), - /// Warm-start impulses of this cluster's shape-matching rows, parallel to `particles` + /// Warm-start impulses of this cluster's shape-matching constraints, parallel to `particles` /// (empty until the cluster is shape-matched). #[cfg_attr(feature = "serde-serialize", serde(default))] pub(crate) shape_impulses: Vec, + /// The collision meshes this cluster owns (see [`super::SoftCollisionMesh`]). Removed + /// meshes leave a `None` slot so ids stay stable. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) meshes: Vec>, } #[cfg(feature = "serde-serialize")] @@ -63,6 +67,7 @@ impl SoftBodyCluster { prev_shape_matching_target: None, last_gather: Default::default(), shape_impulses: vec![], + meshes: Vec::new(), } } @@ -82,6 +87,16 @@ impl SoftBodyCluster { self.proxy } + /// The collision meshes this cluster carries (dead slots skipped). + pub fn meshes(&self) -> impl Iterator { + self.meshes.iter().flatten() + } + + /// The `i`-th mesh of this cluster, if it is live. + pub fn mesh(&self, i: u32) -> Option<&super::SoftCollisionMesh> { + self.meshes.get(i as usize).and_then(|mesh| mesh.as_ref()) + } + /// Whether this cluster's particles are shape-matched toward its frame. pub fn shape_matching_enabled(&self) -> bool { self.shape_matching @@ -151,6 +166,24 @@ impl SoftBody { self.clusters.iter().filter(|c| c.is_live()).count() } + /// Whether two of this body's clusters share no particle (a merge walk over their sorted + /// particle lists; a dead or missing cluster shares nothing). + pub(crate) fn clusters_are_disjoint(&self, a: u32, b: u32) -> bool { + let (Some(a), Some(b)) = (self.cluster(a), self.cluster(b)) else { + return true; + }; + let (mut i, mut j) = (0, 0); + let (a, b) = (a.particles(), b.particles()); + while i < a.len() && j < b.len() { + match a[i].cmp(&b[j]) { + core::cmp::Ordering::Less => i += 1, + core::cmp::Ordering::Greater => j += 1, + core::cmp::Ordering::Equal => return false, + } + } + true + } + /// Enables or disables shape matching of the `i`-th cluster's particles toward the cluster's /// frame (independent from the whole-body [`Self::enable_shape_matching`]). pub fn enable_cluster_shape_matching(&mut self, i: u32, enabled: bool) { @@ -427,11 +460,14 @@ impl SoftBody { // The meshes follow the renumbered particles and cells; their contact caches hold stale // ids. - let mut meshes = core::mem::take(&mut self.meshes); - for mesh in &mut meshes { - mesh.remap_topology(self, &cell_remap); - } - self.meshes = meshes; + let remap_tables = super::collision_mesh::SoftTopologyRemap { + cells: &cell_remap, + particles: &remap, + }; + self.for_each_mesh_mut(|body, mesh| { + mesh.remap_topology(body, &remap_tables); + mesh.clear_contacts(); + }); self.boundary_closed = super::soft_body_builder::surface_is_closed(&self.boundary); self.rebuild_mesh_tables(); diff --git a/src/dynamics/soft_body/soft_body_meshes.rs b/src/dynamics/soft_body/soft_body_meshes.rs index 58bf7da5e..7ef111afc 100644 --- a/src/dynamics/soft_body/soft_body_meshes.rs +++ b/src/dynamics/soft_body/soft_body_meshes.rs @@ -31,6 +31,153 @@ impl SoftBody { self.meshes().find(|mesh| mesh.collision_enabled()) } + /// The mesh with the given id, mutably. + pub(crate) fn mesh_mut(&mut self, id: SoftMeshId) -> Option<&mut SoftCollisionMesh> { + self.clusters + .get_mut(id.cluster as usize)? + .meshes + .get_mut(id.mesh as usize)? + .as_mut() + } + + /// The mesh held by `collider`, mutably. + pub(crate) fn mesh_of_mut( + &mut self, + collider: crate::geometry::ColliderHandle, + ) -> Option<&mut SoftCollisionMesh> { + self.clusters + .iter_mut() + .flat_map(|cluster| cluster.meshes.iter_mut().flatten()) + .find(|mesh| mesh.collider() == collider) + } + + /// Binds a mesh to the cluster `cluster`, without inserting it: the geometry is given in + /// world space, as it sits when the binding is taken. + pub(crate) fn bind_mesh( + &self, + cluster: u32, + binding: &super::SoftMeshBinding, + vertices: Vec, + indices: Vec<[u32; DIM]>, + ) -> Result { + use super::{SoftBindingError, SoftMeshBindingMode, SoftMeshMapping}; + let cluster = self + .cluster(cluster) + .ok_or(SoftBindingError::NotAClusterProxy)?; + let in_cluster = |particle: u32| cluster.particles().binary_search(&particle).is_ok(); + + let mapping = match &binding.mode { + SoftMeshBindingMode::Direct { particles } => { + if particles.len() != vertices.len() { + return Err(SoftBindingError::DegenerateMesh); + } + for (vertex, &particle) in particles.iter().enumerate() { + if particle as usize >= self.particles.len() || !in_cluster(particle) { + return Err(SoftBindingError::VertexOutsideCluster { + vertex: vertex as u32, + particle, + }); + } + } + SoftMeshMapping::Direct { + particles: particles.clone(), + } + } + SoftMeshBindingMode::DirectByPosition { eps } => { + let mut particles = Vec::with_capacity(vertices.len()); + for (vertex, position) in vertices.iter().enumerate() { + let closest = cluster + .particles() + .iter() + .map(|&p| (p, (self.particles[p as usize].position - *position).length())) + .filter(|(_, d)| *d <= *eps) + .min_by(|a, b| a.1.total_cmp(&b.1)); + let (particle, _) = closest.ok_or(SoftBindingError::UnmatchedVertex { + vertex: vertex as u32, + })?; + particles.push(particle); + } + SoftMeshMapping::Direct { particles } + } + SoftMeshBindingMode::Skinned => { + // Only the cells the cluster fully owns may hold the mesh: a vertex must not + // move particles its cluster does not have. + let cells: Vec = (0..self.cells.len()) + .filter(|&c| self.cells[c].vertices.iter().all(|v| in_cluster(*v))) + .collect(); + let owned: Vec = cells.iter().map(|&c| self.cells[c]).collect(); + // The mesh is given in world space, so it binds against where the particles + // are now (`rest_position` is relative to the rest center of mass). + let positions: Vec = + self.particles.iter().map(|p| p.position).collect(); + let bindings = super::collision_mesh::bind_to_cells( + &vertices, &indices, &owned, &positions, + ) + .ok_or(SoftBindingError::UnboundVertex { vertex: 0 })?; + // `bind_to_cells` indexes the cells it was given: back to the body's own ids. + SoftMeshMapping::Skinned { + bindings: bindings + .into_iter() + .map(|mut binding| { + binding.cell = cells[binding.cell as usize] as u32; + binding + }) + .collect(), + } + } + }; + + Ok(SoftCollisionMesh::new( + mapping, + indices, + vertices, + self, + binding.self_contacts, + )) + } + + /// Puts a bound mesh in its cluster, reusing a dead slot when there is one. + pub(crate) fn push_mesh(&mut self, cluster: u32, mut mesh: SoftCollisionMesh) -> SoftMeshId { + let meshes = &mut self.clusters[cluster as usize].meshes; + let slot = meshes + .iter() + .position(|slot| slot.is_none()) + .unwrap_or(meshes.len()); + let id = SoftMeshId { + cluster, + mesh: slot as u32, + }; + mesh.set_id(id); + if slot == meshes.len() { + meshes.push(Some(mesh)); + } else { + meshes[slot] = Some(mesh); + } + self.modified = true; + id + } + + /// Records the collider a freshly inserted mesh got. + pub(crate) fn set_mesh_collider( + &mut self, + id: SoftMeshId, + collider: crate::geometry::ColliderHandle, + ) { + if let Some(mesh) = self.mesh_mut(id) { + mesh.collider = collider; + } + } + + /// Removes the mesh `id` from its cluster, leaving a dead slot. + pub(crate) fn remove_mesh(&mut self, id: SoftMeshId) -> Option { + self.clusters + .get_mut(id.cluster as usize)? + .meshes + .get_mut(id.mesh as usize)? + .take() + } + + /// Runs `f` on every mesh of this body, with the body itself in hand. pub(crate) fn for_each_mesh_mut(&mut self, mut f: impl FnMut(&SoftBody, &mut SoftCollisionMesh)) { let mut clusters = core::mem::take(&mut self.clusters); for cluster in &mut clusters { @@ -38,6 +185,9 @@ impl SoftBody { f(self, mesh); } } + self.clusters = clusters; + } + /// Updates the meshes' cached vertices from the current particle positions. pub(crate) fn update_meshes(&mut self) { self.for_each_mesh_mut(|body, mesh| mesh.update(&body.cells, &body.particles)); diff --git a/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs b/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs index 691a3597c..7713c6187 100644 --- a/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs @@ -35,6 +35,7 @@ impl SoftBodySet { sb.clusters.push(SoftBodyCluster { particles: list, proxy, + meshes: Vec::new(), rotation: Rotation::IDENTITY, cell, shape_matching: false, 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 1d931738c..2721bf309 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 @@ -65,6 +65,7 @@ impl SoftBodySet { soft_body.root_body = spawn_proxy(&soft_body.particle_settings, soft_body.user_data, handle, 0, bodies); + let meshes = soft_body_builder.build_meshes(&soft_body); soft_body.clusters = alloc::vec![SoftBodyCluster { particles: (0..soft_body.particles.len() as u32).collect(), proxy: soft_body.root_body, @@ -75,6 +76,7 @@ impl SoftBodySet { prev_shape_matching_target: None, last_gather: Default::default(), shape_impulses: Vec::new(), + meshes: meshes.into_iter().map(Some).collect(), }]; soft_body.cluster_refs = alloc::vec![1; soft_body.particles.len()]; // The whole-body frame first: the colliders are expressed in it. @@ -88,6 +90,32 @@ impl SoftBodySet { // the soft body), with vertices in the whole-body cluster's frame (the proxy's pose holds // the rigid motion). They collide with everything, fixed and kinematic colliders included. if let Some(template) = soft_body_builder.collider_template.as_ref() { + let mut clusters = core::mem::take(&mut soft_body.clusters); + for (ci, cluster) in clusters.iter_mut().enumerate() { + for (mi, mesh) in cluster.meshes.iter_mut().enumerate() { + let Some(mesh) = mesh else { continue }; + mesh.set_id(SoftMeshId { + cluster: ci as u32, + mesh: mi as u32, + }); + if !mesh.collision_enabled() { + continue; + } + let Some(co_handle) = spawn_mesh_collider( + template, handle, mesh, &soft_body, cluster.proxy, &frame, bodies, + colliders, + ) else { + // No shape to build (no element): the mesh collides through nothing, + // and says so rather than holding an invalid collider. + mesh.collision_enabled = false; + continue; + }; + mesh.collider = co_handle; + } + } + soft_body.clusters = clusters; + } + *self.bodies.get_mut(handle.0).unwrap() = soft_body; self.island_events.push(SoftBodyIslandEvent { handle }); handle 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 17069eead..aa605aa1a 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 @@ -32,7 +32,11 @@ pub(crate) fn surface_shape( if super::soft_body_builder::element_vertices(&surface[0]).len() < DIM { let segments: Vec<[u32; 2]> = surface.iter().map(|e| [e[0], e[1]]).collect(); return Some(SharedShape::new(Polyline::with_flags( + vertices, Some(segments), + PolylineFlags::DEFORMABLE, + ))); + } SharedShape::trimesh_with_flags(vertices, surface.to_vec(), TriMeshFlags::DEFORMABLE).ok() } } @@ -71,6 +75,53 @@ pub(super) fn rebuilt_surface_shape( Some(shape) } +/// Creates the collider holding a soft body's collision `mesh`, parented to its cluster's +/// `proxy` and expressed in `frame` (the proxy's pose): the deformable shape follows the +/// particles every step. `None` when the mesh has no shape to build. +#[allow(clippy::too_many_arguments)] +pub(super) fn spawn_mesh_collider( + template: &ColliderBuilder, + handle: SoftBodyHandle, + mesh: &super::SoftCollisionMesh, + body: &SoftBody, + proxy: RigidBodyHandle, + frame: &Pose, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, +) -> Option { + let shape = surface_shape( + mesh.local_vertices(body, frame), + mesh.indices(), + mesh.is_closed(), + )?; + let mut collider = template + .clone() + .density(0.0) + .contact_skin(body.particle_radius()) + .build(); + collider.set_shape(shape); + collider.set_position(Pose::IDENTITY); + let co_handle = colliders.insert_with_parent(collider, proxy, bodies); + // `insert_with_parent` resets the internal references; the collider's world pose is the + // frame (local pose identity). + let co = colliders.index_mut_internal(co_handle); + co.deformable_mesh_ref = Some(SoftMeshRef { + body: handle, + id: mesh.id(), + }); + co.deform_pose(*frame); + Some(co_handle) +} + +/// Creates the proxy rigid body of cluster `cluster` of soft body `handle`: a +/// [`crate::dynamics::RigidBodyType::SoftFrame`] body standing for the cluster in the islands +/// and joints and holding its colliders. Cluster 0's proxy is the soft body's root body. +pub(super) fn spawn_proxy( + settings: &super::SoftBodyParticleSettings, + user_data: u128, + handle: SoftBodyHandle, + cluster: u32, + bodies: &mut RigidBodySet, ) -> RigidBodyHandle { let rb = RigidBodyBuilder::dynamic() .gravity_scale(0.0) 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 02f24d0e0..c70959f8f 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 @@ -4,6 +4,7 @@ // so a one-refresh-stale proxy pose is fine. /// Refreshes every soft body's derived state at the end of a step: the surface orientation, // Every body reads its own particles and writes only itself: updated in parallel, then + // Flagged as modified so the broad phase updates their AABBs with the new margin. /// Tears a soft body right away: removes the given edges and cells and everything spanning /// them, splits the particles the tear passes through (the velocity of the original kept, its diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs index a54a67922..486d44d20 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs @@ -224,6 +224,25 @@ const _: () = assert!(DIM + 1 == MAX_CONSTRAINT_PARTICLES); /// Barycentric weights of `p` on the surface element (segment in 2D, triangle in 3D), /// clamped to the element. pub(crate) fn barycentric_weights(x: &[Vector], p: Vector) -> [Real; DIM] { + // A segment element (2D, and a wire in 3D): the unused weights stay at zero. + #[cfg(feature = "dim3")] + if x.len() < DIM { + let mut weights = [0.0; DIM]; + if x.len() == 2 { + let e = x[1] - x[0]; + let l2 = e.length_squared(); + let t = if l2 > 0.0 { + ((p - x[0]).dot(e) / l2).clamp(0.0, 1.0) + } else { + 0.5 + }; + weights[0] = 1.0 - t; + weights[1] = t; + } else if x.len() == 1 { + weights[0] = 1.0; + } + return weights; + } #[cfg(feature = "dim2")] { let e = x[1] - x[0]; diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs index 5f814bfb0..347806110 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs @@ -40,6 +40,7 @@ impl SoftConstraintsSet { let sb = unsafe { &*awake.ptr }; for mesh in sb.meshes().filter(|mesh| mesh.collision_enabled()) { let surface_handle = mesh.collider(); + out.begin_mesh(mesh.id()); let self_frozen = awake.frozen; let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; // The other soft surfaces whose edge-vs-edge constraints this body owns: (awake @@ -82,6 +83,22 @@ impl SoftConstraintsSet { let Some(other_sb) = soft_bodies.get(other_sb_handle) else { continue; }; + // Two meshes of one body never collide within a cluster (a hull and a skin + // describe the same particles); across clusters they do, if both opted in + // and their clusters share no particle. + if other_sb_handle == awake.handle { + let Some(other_mesh) = other_sb.mesh(other_ref.id) else { + continue; + }; + let same_cluster = other_ref.id.cluster == mesh.id().cluster; + if same_cluster + || !mesh.self_contacts_enabled() + || !other_mesh.self_contacts_enabled() + || !sb.clusters_are_disjoint(mesh.id().cluster, other_ref.id.cluster) + { + continue; + } + } let frozen = match other_ai { Some(bi) => self.awake[bi].frozen, None => other_sb.particles.iter().all(|p| p.inv_mass == 0.0), @@ -116,6 +133,22 @@ impl SoftConstraintsSet { out, ); } + // The edge-vs-edge pass of a pair is owned by its lower + // (awake index, cluster, mesh): exactly one side assembles it. + let own_key = (ai, mesh.id().cluster, mesh.id().mesh); + let other_key = ( + other_ai.unwrap_or(usize::MAX), + other_ref.id.cluster, + other_ref.id.mesh, + ); + if own_key <= other_key { + edge_tasks.push(( + other_ai, + other_sb_handle, + other_handle, + detected.edges.as_ref(), + )); + } continue; } if !pair.has_any_active_contact() { @@ -211,6 +244,13 @@ impl SoftConstraintsSet { } else { (localized(sc.anchor2, &pose2), localized(sc.anchor1, &pose1)) }; + let positions: [Vector; DIM] = core::array::from_fn(|k| { + element + .get(k) + .map_or(Vector::ZERO, |v| mesh.vertex(sb, *v as usize)) + }); + let weights = + barycentric_weights(&positions[..element.len()], surface_anchor); (positions, weights, other_anchor) }; // Contacts whose point lies inside the element (vertex/edge contacts have diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs index de0cf2b9f..c71b85d3e 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs @@ -60,6 +60,7 @@ impl SoftConstraintsSet { other_co.friction_combine_rule(), ); + let current_mesh = out.current_mesh; let workspace = out; workspace.previous.clear(); for c in &mesh.edge_contacts { diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs index 35d97d8d8..05293dce3 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs @@ -57,7 +57,23 @@ pub(super) struct BodyContacts { /// vertex pass), by (vertex, element). pub(super) previous_vertex: HashMap<(u32, u32), (Real, Vector)>, } +/// The contact state one collision mesh owns across steps: what the next step warm-starts from. +#[derive(Default)] +pub(super) struct MeshContacts { + pub(super) id: SoftMeshId, + pub(super) edge_contacts: Vec, + pub(super) vertex_contacts: Vec, +} impl BodyContacts { + /// Starts assembling the mesh `id`: its contacts go to a list of its own. + pub(super) fn begin_mesh(&mut self, id: SoftMeshId) { + self.current_mesh = self.meshes.len(); + self.meshes.push(MeshContacts { + id, + ..Default::default() + }); + } + } /// The step-constant inputs of the per-body contact assembly. pub(super) struct AssemblyCtx<'a> { diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 7d304cf1f..30b92d11e 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -1,9 +1,11 @@ use crate::alloc_prelude::*; use crate::data::arena::Arena; use crate::data::{HasModifiedFlag, ModifiedObjects}; -use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet, SoftBodySet}; +use crate::dynamics::{ + IslandManager, RigidBodyHandle, RigidBodySet, SoftBindingError, SoftBodySet, SoftMeshBinding, +}; use crate::geometry::{Collider, ColliderChanges, ColliderHandle, ColliderParent}; -use crate::math::Pose; +use crate::math::{DIM, Pose, Vector}; use core::ops::{Index, IndexMut}; /// A set of modified colliders @@ -239,6 +241,65 @@ impl ColliderSet { handle } + /// Inserts a collider holding a soft body's deformable collision mesh: a polyline (2D) or + /// triangle mesh (3D) whose vertices follow the cluster proxy `parent_handle` via `binding` + /// ([`crate::dynamics::SoftBodyCluster::proxy`]); a failed call leaves both sets untouched. + pub fn insert_deformable( + &mut self, + coll: impl Into, + binding: SoftMeshBinding, + parent_handle: RigidBodyHandle, + bodies: &mut RigidBodySet, + soft_bodies: &mut SoftBodySet, + ) -> Result { + let mut coll = coll.into(); + // The parent must be a live cluster proxy: it says which body and cluster the mesh + // joins, and its pose places the collider like any attached one. + let (body_handle, cluster_id, frame) = { + let rb = bodies + .get(parent_handle) + .ok_or(SoftBindingError::NotAClusterProxy)?; + let body = rb.soft_body().ok_or(SoftBindingError::NotAClusterProxy)?; + (body, rb.soft_cluster, rb.pos.position) + }; + let sb = soft_bodies + .get_mut(body_handle) + .ok_or(SoftBindingError::NotAClusterProxy)?; + if sb + .cluster(cluster_id) + .is_none_or(|cluster| cluster.proxy() != parent_handle) + { + return Err(SoftBindingError::NotAClusterProxy); + } + + let (vertices, indices) = mesh_geometry(coll.shape())?; + // The shape is kept as given, in the collider's own frame: the binding reads its + // vertices where the collider's pose relative to the proxy puts them. + let pos_wrt_parent = coll + .parent + .as_ref() + .map_or(coll.pos.0, |parent| parent.pos_wrt_parent); + let pose = frame * pos_wrt_parent; + let vertices: Vec = vertices.iter().map(|v| pose * *v).collect(); + let mesh = sb.bind_mesh(cluster_id, &binding, vertices, indices)?; + coll.set_density(0.0); + // The contact skin is the thickness of the mesh's vertices in the soft contact + // passes: a collider left without one gets the body's particle radius. + if coll.contact_skin() <= 0.0 { + coll.set_contact_skin(sb.particle_radius()); + } + + let id = sb.push_mesh(cluster_id, mesh); + let handle = self.insert_with_parent(coll, parent_handle, bodies); + let co = self.index_mut_internal(handle); + co.deformable_mesh_ref = Some(crate::dynamics::SoftMeshRef { + body: body_handle, + id, + }); + soft_bodies[body_handle].set_mesh_collider(id, handle); + Ok(handle) + } + /// Changes which rigid body a collider is attached to, or detaches it completely. /// /// Use this to move a collider from one body to another, or to make it standalone. @@ -345,9 +406,9 @@ impl ColliderSet { let collider = self.remove_internal(handle, islands, bodies, wake_up)?; // A removed deformable collider takes its mesh with it: the soft body keeps simulating, // without that mesh's collisions. - if let Some(sb_handle) = collider.soft_body_surface() { - if let Some(sb) = soft_bodies.get_mut(sb_handle) { - sb.clear_mesh_collider(handle); + if let Some(mesh) = collider.deformable_mesh_ref() { + if let Some(sb) = soft_bodies.get_mut(mesh.body) { + sb.remove_mesh(mesh.id); } } Some(collider) @@ -497,3 +558,35 @@ impl IndexMut for ColliderSet { collider } } + +/// The vertices (world space) and elements of a deformable collider's shape. +fn mesh_geometry( + shape: &dyn crate::geometry::Shape, +) -> Result<(Vec, Vec<[u32; DIM]>), SoftBindingError> { + #[cfg(feature = "dim2")] + let (deformable, geometry) = { + use parry::shape::PolylineFlags; + let polyline = shape.as_polyline().ok_or(SoftBindingError::UnsupportedShape)?; + ( + polyline.flags().contains(PolylineFlags::DEFORMABLE), + (polyline.vertices().to_vec(), polyline.indices().to_vec()), + ) + }; + #[cfg(feature = "dim3")] + let (deformable, geometry) = { + use parry::shape::TriMeshFlags; + let trimesh = shape.as_trimesh().ok_or(SoftBindingError::UnsupportedShape)?; + ( + trimesh.flags().contains(TriMeshFlags::DEFORMABLE), + (trimesh.vertices().to_vec(), trimesh.indices().to_vec()), + ) + }; + if !deformable { + return Err(SoftBindingError::NotDeformable); + } + let (vertices, indices) = geometry; + if vertices.is_empty() || indices.is_empty() { + return Err(SoftBindingError::DegenerateMesh); + } + Ok((vertices, indices)) +} diff --git a/src/geometry/narrow_phase/pair_update.rs b/src/geometry/narrow_phase/pair_update.rs index d8fe3b996..2a6bfbd9c 100644 --- a/src/geometry/narrow_phase/pair_update.rs +++ b/src/geometry/narrow_phase/pair_update.rs @@ -198,6 +198,24 @@ pub(super) fn process_pair( let soft_surface_pair = co1.deformable_mesh_ref.is_some() || co2.deformable_mesh_ref.is_some(); let two_soft_surfaces = co1.deformable_mesh_ref.is_some() && co2.deformable_mesh_ref.is_some(); + // Two colliders of one soft body: its collision meshes decide between themselves (see + // `soft_contacts::update_pair`), but a rigid collider hung on a cluster proxy must not + // collide with the body it rides; pairs between two clusters of the same body are allowed. + let soft_body_of = |co: &crate::geometry::Collider, + rb: Option<&crate::dynamics::RigidBody>| { + co.deformable_mesh_ref + .map(|mesh| mesh.body) + .or_else(|| rb.and_then(|rb| rb.soft_body())) + }; + if let (Some(sb1), Some(sb2)) = (soft_body_of(co1, rb1), soft_body_of(co2, rb2)) { + if sb1 == sb2 && !two_soft_surfaces { + if clear_filtered_pair(pair) { + outcome = OUTCOME_CLEARED_IN_GRAPH; + } + break 'emit_events; + } + } + // Deal with contacts disabled between bodies attached by joints. if let (Some(co_parent1), Some(co_parent2)) = (&co1.parent, &co2.parent) { for (_, joint) in impulse_joints.joints_between(co_parent1.handle, co_parent2.handle) { diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs index 9cdf2aa05..63c62b48b 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs @@ -36,6 +36,14 @@ pub(crate) fn detect_edges( // In 2D two closed surfaces meet through their vertex constraints alone (point-vs-segment is // complete for first contact); in 3D two edge-leading closed bodies (slim bars crossing // corner-first) have no vertex near the other's surface and need the edge constraints. + #[cfg(feature = "dim2")] + if mesh.is_closed() && other_mesh.is_closed() { + return false; + } + #[cfg(feature = "dim3")] + if !params.soft_bodies.recovery.edge_speculation && mesh.is_closed() && other_mesh.is_closed() { + return false; + } let Some(other_bvh) = other_co.shape().as_composite_shape().map(|c| c.bvh()) else { return false; }; diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs index 5871e2e55..52a1780d7 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs @@ -41,6 +41,9 @@ bitflags::bitflags! { /// If this flag is set, the soft bodies' elements (structural edges and cell edges) and /// their soft-vs-soft contacts (vertex-vs-surface and edge-vs-edge) will be rendered. const SOFT_BODIES = 1 << 7; + /// If this flag is set, the pseudo-normals of the triangle-meshes (3D) and polylines (2D) + /// that have them will be rendered. + const PSEUDO_NORMALS = 1 << 8; } } @@ -461,6 +464,118 @@ impl DebugRenderPipeline { } } } + + if self.mode.contains(DebugRenderMode::PSEUDO_NORMALS) { + for (h, co) in colliders.iter() { + let object = DebugRenderObject::Collider(h, co); + + if backend.filter_object(object) { + self.render_shape_pseudo_normals(object, backend, co.shape(), co.position()); + } + } + } + } + + /// Renders the pseudo-normals of the polylines reachable from `shape`, if computed (polyline + /// `ORIENTED`), each starting at its vertex. + #[cfg(feature = "dim2")] + #[profiling::function] + fn render_shape_pseudo_normals( + &mut self, + object: DebugRenderObject, + backend: &mut impl DebugRenderBackend, + shape: &dyn Shape, + pos: &Pose, + ) { + let len = self.style.pseudo_normal_length; + + match shape.as_typed_shape() { + TypedShape::Polyline(s) => { + let Some(pseudo_normals) = s.pseudo_normals() else { + return; + }; + + for (vtx, n) in s.vertices().iter().zip(pseudo_normals) { + let Some(n) = n.try_normalize() else { + continue; + }; + let a = *pos * *vtx; + backend.draw_line( + object, + a, + a + pos.rotation * n * len, + self.style.vertex_pseudo_normal_color, + ); + } + } + TypedShape::Compound(s) => { + for (sub_pos, shape) in s.shapes() { + self.render_shape_pseudo_normals(object, backend, &**shape, &(pos * sub_pos)) + } + } + _ => {} + } + } + + /// Renders the pseudo-normals of the triangle meshes reachable from `shape`, if computed (mesh + /// `ORIENTED` or `FIX_INTERNAL_EDGES`), each from its feature: a vertex, or an edge's midpoint. + #[cfg(feature = "dim3")] + #[profiling::function] + fn render_shape_pseudo_normals( + &mut self, + object: DebugRenderObject, + backend: &mut impl DebugRenderBackend, + shape: &dyn Shape, + pos: &Pose, + ) { + let len = self.style.pseudo_normal_length; + + match shape.as_typed_shape() { + TypedShape::TriMesh(s) => { + let Some(pseudo_normals) = s.pseudo_normals() else { + return; + }; + let vertices = s.vertices(); + + for (vtx, n) in vertices.iter().zip(&pseudo_normals.vertices_pseudo_normal) { + let Some(n) = n.try_normalize() else { + continue; + }; + let a = *pos * *vtx; + backend.draw_line( + object, + a, + a + pos.rotation * n * len, + self.style.vertex_pseudo_normal_color, + ); + } + + // The per-triangle pseudo-normals are stored in the edge order [ab, bc, ca], as + // built by `TriMesh::compute_pseudo_normals`. + for (idx, normals) in s.indices().iter().zip(&pseudo_normals.edges_pseudo_normal) { + for (k, n) in normals.iter().enumerate() { + let Some(n) = n.try_normalize() else { + continue; + }; + let v0 = vertices[idx[k] as usize]; + let v1 = vertices[idx[(k + 1) % 3] as usize]; + let a = *pos * ((v0 + v1) * 0.5); + backend.draw_line( + object, + a, + a + pos.rotation * n * len, + self.style.edge_pseudo_normal_color, + ); + } + } + } + TypedShape::Compound(s) => { + for (sub_pos, shape) in s.shapes() { + self.render_shape_pseudo_normals(object, backend, &**shape, &(pos * sub_pos)) + } + } + _ => {} + } } #[cfg(feature = "dim2")] diff --git a/src/pipeline/debug_render_pipeline/debug_render_style.rs b/src/pipeline/debug_render_pipeline/debug_render_style.rs index 6eeeade84..572283707 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_style.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_style.rs @@ -63,6 +63,12 @@ pub struct DebugRenderStyle { pub soft_body_frame_color: DebugColor, /// The color of the colliders' [`Aabb`](crate::geometry::Aabb)s. pub collider_aabb_color: DebugColor, + /// The color of the vertex pseudo-normals of triangle-meshes and polylines. + pub vertex_pseudo_normal_color: DebugColor, + /// The color of the edge pseudo-normals of triangle-meshes (3D only). + pub edge_pseudo_normal_color: DebugColor, + /// The length of the pseudo-normals. + pub pseudo_normal_length: Real, } impl Default for DebugRenderStyle { @@ -90,6 +96,9 @@ impl Default for DebugRenderStyle { soft_body_element_color: [200.0, 0.8, 0.5, 1.0], soft_body_frame_color: [40.0, 0.9, 0.6, 1.0], collider_aabb_color: [124.0, 1.0, 0.4, 1.0], + vertex_pseudo_normal_color: [180.0, 1.0, 0.6, 1.0], + edge_pseudo_normal_color: [280.0, 1.0, 0.6, 1.0], + pseudo_normal_length: 0.2, } } } diff --git a/src/pipeline/physics_pipeline/substep.rs b/src/pipeline/physics_pipeline/substep.rs index d1501c49c..a4e6d30d3 100644 --- a/src/pipeline/physics_pipeline/substep.rs +++ b/src/pipeline/physics_pipeline/substep.rs @@ -57,6 +57,7 @@ impl PhysicsPipeline { islands: &IslandManager, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, + soft_bodies: &SoftBodySet, broad_phase: &mut BroadPhaseBvh, narrow_phase: &NarrowPhase, ccd_solver: &mut CCDSolver, @@ -71,6 +72,7 @@ impl PhysicsPipeline { islands, bodies, colliders, + soft_bodies, broad_phase, narrow_phase, hooks, @@ -528,6 +530,7 @@ impl PhysicsPipeline { islands, bodies, colliders, + soft_bodies, broad_phase, narrow_phase, ccd_solver, @@ -549,10 +552,11 @@ impl PhysicsPipeline { // re-running collision detection for the next CCD substep. self.counters.stages.collision_detection_time.resume(); self.counters.cd.final_broad_phase_time.resume(); - self.refresh_moved_collider_aabbs(&integration_parameters, broad_phase); + self.update_moved_collider_aabbs(&integration_parameters, broad_phase); self.counters.cd.final_broad_phase_time.pause(); self.counters.stages.collision_detection_time.pause(); + soft_bodies.refresh_vertex_caches(); self.detect_collisions( &integration_parameters, islands, diff --git a/src/pipeline/physics_world.rs b/src/pipeline/physics_world.rs index 5bdd583de..582df741e 100644 --- a/src/pipeline/physics_world.rs +++ b/src/pipeline/physics_world.rs @@ -395,16 +395,32 @@ impl PhysicsWorld { // ── Soft bodies ───────────────────────────────────────────────────── - /// Insert a soft body and return its handle. - /// - /// This creates the soft body's hidden root rigid body and its colliders (a deformable - /// surface, or one ball per surface particle); they are removed by - /// [`Self::remove_soft_body`]. + /// Insert a soft body and return its handle; this creates its hidden root rigid body and its + /// colliders (a deformable surface, or one ball per surface particle), which + /// [`Self::remove_soft_body`] removes. pub fn insert_soft_body(&mut self, soft_body: SoftBodyBuilder) -> SoftBodyHandle { self.soft_bodies .insert(soft_body, &mut self.bodies, &mut self.colliders) } + /// Inserts a collider holding a soft body's deformable collision mesh, bound to the cluster + /// of `parent` (a cluster proxy: see + /// [`ColliderSet::insert_deformable`](crate::geometry::ColliderSet::insert_deformable)). + pub fn insert_deformable( + &mut self, + collider: impl Into, + binding: SoftMeshBinding, + parent: RigidBodyHandle, + ) -> Result { + self.colliders.insert_deformable( + collider, + binding, + parent, + &mut self.bodies, + &mut self.soft_bodies, + ) + } + /// Remove a soft body, its root rigid body and its colliders. pub fn remove_soft_body(&mut self, handle: SoftBodyHandle) -> Option { self.soft_bodies.remove( diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs index b14ed7deb..730d485fd 100644 --- a/src_testbed/graphics.rs +++ b/src_testbed/graphics.rs @@ -140,6 +140,9 @@ pub struct IndividualNode { pub delta: rapier::math::Pose, pub color: Color, pub tmp_color: Option, + /// Whether the node's mesh shares its vertices between elements (smooth normals): how it + /// was built, and how a deformable collider's geometry is written back into it. + pub smooth: bool, } /// A render-only scene node anchored to a rigid body rather than to a @@ -265,6 +268,7 @@ impl GraphicsManager { templates: HashMap::new(), individual_nodes: Vec::new(), body_attached_nodes: Vec::new(), + soft_graphics: SoftBodyGraphics::default(), colliders_visible: true, body_render_meshes_visible: true, draw_surfaces: true, @@ -297,6 +301,7 @@ impl GraphicsManager { self.templates.clear(); self.individual_nodes.clear(); self.body_attached_nodes.clear(); + self.soft_graphics.clear(); self.c2nodes.clear(); self.b2colliders.clear(); self.c2color.clear(); @@ -411,14 +416,14 @@ impl GraphicsManager { } Some(self.scene.add_render_mesh(mesh, Vec3::ONE)) } else { - Self::create_individual_node(&mut self.scene, &**shape, color, false) + Self::create_individual_node(&mut self.scene, &**shape, color, false, false) } }; #[cfg(feature = "dim2")] let mut node = { // Custom UV/normal plumbing and PBR materials are unsupported in 2D. let _ = (uvs, normals, material); - Self::create_individual_node(&mut self.scene, &**shape, color, false) + Self::create_individual_node(&mut self.scene, &**shape, color, false, false) }; if let Some(n) = node.as_mut() { @@ -471,6 +476,26 @@ impl GraphicsManager { }); } + /// Rebuilds the node of a collider whose shape was replaced (a tear), or whose shading + /// changed. + fn rebuild_individual_node(&mut self, index: usize, collider: &rapier::geometry::Collider) { + let smooth = self.smooth_mesh_colliders; + let visible = self.draw_surfaces && self.colliders_visible; + let node = &mut self.individual_nodes[index]; + let color = node.color; + node.node.detach(); + if let Some(mut fresh) = Self::create_individual_node( + &mut self.scene, + collider.shape(), + color, + collider.is_sensor(), + smooth, + ) { + fresh.set_visible(visible); + let node = &mut self.individual_nodes[index]; + node.node = fresh; + node.smooth = smooth; + } } pub fn scene(&self) -> &SceneNode { @@ -716,7 +741,7 @@ impl GraphicsManager { } } - pub fn set_body_color(&mut self, b: RigidBodyHandle, color: Color, tmp_color: bool) { + pub fn set_body_color(&mut self, b: RigidBodyHandle, mut color: Color, tmp_color: bool) { if !tmp_color { self.b2color.insert(b, color); } @@ -726,7 +751,8 @@ impl GraphicsManager { match &self.c2nodes[co] { NodeLocation::Individual { index } => { if tmp_color { - self.individual_nodes[*index].tmp_color = Some(color); + let alpha = self.individual_nodes[*index].color.a; + self.individual_nodes[*index].tmp_color = Some(color.with_alpha(alpha)); } else { self.individual_nodes[*index].color = color; } @@ -734,7 +760,8 @@ impl GraphicsManager { NodeLocation::Instanced { template, index } => { let instances = &mut self.templates.get_mut(template).unwrap(); if tmp_color { - instances.colliders[*index].tmp_color = Some(color); + let alpha = instances.colliders[*index].color.a; + instances.colliders[*index].tmp_color = Some(color.with_alpha(alpha)); } else { instances.colliders[*index].color = color; } @@ -908,7 +935,7 @@ impl GraphicsManager { return; } - let opacity = if sensor { 0.5 } else { color.a }; + let opacity = if sensor { 0.3 } else { color.a }; // Try to use instancing for primitive shapes if let Some(template_type) = Self::shape_template_type(shape) { @@ -932,8 +959,9 @@ impl GraphicsManager { ); } else { // Create individual node for complex shapes + let smooth = self.smooth_mesh_colliders; if let Some(mut node) = - Self::create_individual_node(&mut self.scene, shape, color, sensor) + Self::create_individual_node(&mut self.scene, shape, color, sensor, smooth) { // Honor the current visibility flags immediately so the node // doesn't flash for one frame before the next `draw` (see #843). @@ -945,6 +973,7 @@ impl GraphicsManager { delta, color: color.with_alpha(opacity), tmp_color: None, + smooth, }); self.c2nodes .insert(handle, NodeLocation::Individual { index }); @@ -958,10 +987,11 @@ impl GraphicsManager { shape: &dyn Shape, color: Color, sensor: bool, + smooth: bool, ) -> Option { use kiss3d::procedural::{IndexBuffer, RenderMesh}; - fn to_render_mesh(trimesh: &rapier::geometry::TriMesh) -> RenderMesh { + fn to_render_mesh(trimesh: &rapier::geometry::TriMesh, smooth: bool) -> RenderMesh { let vtx = trimesh .vertices() .iter() @@ -969,7 +999,9 @@ impl GraphicsManager { .collect(); let idx = trimesh.indices().to_vec(); let mut mesh = RenderMesh::new(vtx, None, None, Some(IndexBuffer::Unified(idx))); - mesh.replicate_vertices(); + if !smooth { + mesh.replicate_vertices(); + } mesh.recompute_normals(); mesh } @@ -977,13 +1009,13 @@ impl GraphicsManager { let mut node = match shape.shape_type() { ShapeType::TriMesh => { let trimesh = shape.as_trimesh().unwrap(); - Some(scene.add_render_mesh(to_render_mesh(trimesh), Vec3::ONE)) + Some(scene.add_render_mesh(to_render_mesh(trimesh, smooth), Vec3::ONE)) } ShapeType::HeightField => { let heightfield = shape.as_heightfield().unwrap(); let (vertices, indices) = heightfield.to_trimesh(); let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap(); - Some(scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE)) + Some(scene.add_render_mesh(to_render_mesh(&trimesh, smooth), Vec3::ONE)) } ShapeType::ConvexPolyhedron | ShapeType::RoundConvexPolyhedron => { let poly = shape @@ -992,7 +1024,7 @@ impl GraphicsManager { poly.map(|p| { let (vertices, indices) = p.to_trimesh(); let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap(); - scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE) + scene.add_render_mesh(to_render_mesh(&trimesh, smooth), Vec3::ONE) }) } ShapeType::Capsule => { @@ -1009,7 +1041,7 @@ impl GraphicsManager { let vertices = vec![tri.a, tri.b, tri.c]; let indices = vec![[0u32, 1, 2], [0u32, 2, 1]]; let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap(); - Some(scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE)) + Some(scene.add_render_mesh(to_render_mesh(&trimesh, smooth), Vec3::ONE)) } ShapeType::HalfSpace => { let mut parent = scene.add_group(); @@ -1017,20 +1049,37 @@ impl GraphicsManager { let node = parent.add_quad(2000.0, 2000.0, 1, 1); Some(node) } + ShapeType::Polyline => { + // A wire (a soft rope) is drawn as a tube: raw lines have no thickness to + // shade, and a deforming wire needs geometry it can follow. + let (vertices, segments) = polyline_geometry(shape)?; + let (vtx, idx) = tube_polyline(&vertices, &segments); + let mut mesh = RenderMesh::new(vtx, None, None, Some(IndexBuffer::Unified(idx))); + mesh.recompute_normals(); + Some(scene.add_render_mesh(mesh, Vec3::ONE)) + } ShapeType::Voxels => { let voxels = shape.as_voxels().unwrap(); let (vertices, indices) = voxels.to_trimesh(); let trimesh = rapier::geometry::TriMesh::new(vertices, indices).unwrap(); - Some(scene.add_render_mesh(to_render_mesh(&trimesh), Vec3::ONE)) + Some(scene.add_render_mesh(to_render_mesh(&trimesh, smooth), Vec3::ONE)) } _ => None, }; if let Some(ref mut n) = node { n.set_color_recursive(color); + // Meshes are drawn from both sides: a soft body's surface is routinely seen from + // the inside (a crushed or inverted body), and an authored mesh's winding is not + // guaranteed either. + n.enable_backface_culling_recursive(false); if sensor { - n.set_surface_rendering_activation(false); - n.set_lines_width(1.0, false); + // Sensors are translucent, like their instanced counterparts: an explicit + // Blend mode routes the node to the transparency pass whatever its default + // (see `ShapeTemplate`), and its color includes the sensor opacity. + n.apply_to_objects_mut_recursive(&mut |o| { + o.set_alpha_mode(kiss3d::scene::AlphaMode::Blend) + }); } } @@ -1043,6 +1092,7 @@ impl GraphicsManager { shape: &dyn Shape, color: Color, _sensor: bool, + _smooth: bool, ) -> Option { let mut node = match shape.shape_type() { ShapeType::Triangle => { @@ -1095,32 +1145,14 @@ impl GraphicsManager { ]; Some(scene.add_polyline(vertices, None, 0.1)) } - ShapeType::Polyline => { - // Render polyline as connected thin quads - let polyline = shape.as_polyline().unwrap(); - let vertices: Vec = polyline - .vertices() - .iter() - .map(|pt| Vec2::new(pt.x as f32, pt.y as f32)) - .collect(); - Some( - scene - .add_polyline(vertices, Some(polyline.indices().to_vec()), 0.2) - .set_lines_color(Some(GROUND_COLOR)), - ) - } - ShapeType::HeightField => { - let hf = shape.as_heightfield().unwrap(); - let (polyline_verts, indices) = hf.to_polyline(); - let vertices: Vec = polyline_verts - .iter() - .map(|pt| Vec2::new(pt.x as f32, pt.y as f32)) - .collect(); - Some( - scene - .add_polyline(vertices, Some(indices), 0.2) - .set_lines_color(Some(GROUND_COLOR)), - ) + ShapeType::Polyline | ShapeType::HeightField => { + // A stroked mesh rather than raw lines: the joins are covered, the stroke takes + // the collider's color like any other surface, and a deforming polyline can be + // followed by rewriting its vertices. + let (vertices, segments) = polyline_geometry(shape)?; + let (vtx, idx) = stroked_polyline(&vertices, &segments); + let mesh = GpuMesh2d::new(vtx, idx, None, false); + Some(scene.add_mesh(Rc::new(RefCell::new(mesh)), Vec2::ONE)) } ShapeType::Voxels => { let voxels = shape.as_voxels().unwrap(); @@ -1160,11 +1192,13 @@ impl GraphicsManager { flags: TestbedStateFlags, bodies: &RigidBodySet, colliders: &ColliderSet, + soft_bodies: &SoftBodySet, ) { self.draw_surfaces = flags.contains(TestbedStateFlags::DRAW_SURFACES); self.smooth_mesh_colliders = flags.contains(TestbedStateFlags::SMOOTH_MESH_COLLIDERS); let show_colliders = self.draw_surfaces && self.colliders_visible; let show_body_meshes = self.draw_surfaces && self.body_render_meshes_visible; + self.update_soft_mesh_nodes(soft_bodies, show_colliders); // Update instance data for all templates for (template_type, template) in &mut self.templates { @@ -1285,16 +1319,31 @@ impl GraphicsManager { // mirrored by a visual-only counterpart on the same body, so // "Render visual meshes" still shows the full geometry of // those models without conflating channels. - for node in &mut self.individual_nodes { - node.node.set_visible(show_colliders); - - if let Some(co) = colliders.get(node.collider) { - let co_pos = *co.position() * node.delta; - node.node - .set_pose(co_pos.append_translation(self.gfx_shift).into()); - node.node - .set_color_recursive(node.tmp_color.take().unwrap_or(node.color)); + for index in 0..self.individual_nodes.len() { + let Some(co) = colliders.get(self.individual_nodes[index].collider) else { + self.individual_nodes[index] + .node + .set_visible(show_colliders); + continue; + }; + let hidden_by_skin = is_hidden_by_skin(co, soft_bodies); + self.individual_nodes[index] + .node + .set_visible(show_colliders && !hidden_by_skin); + // The shading choice changed under a mesh: its vertices are shared or unshared, so + // the node is built anew. + if co.shape().as_trimesh().is_some() + && self.individual_nodes[index].smooth != self.smooth_mesh_colliders + { + self.rebuild_individual_node(index, co); } + self.update_deformable_collider(index, co); + let node = &mut self.individual_nodes[index]; + let co_pos = *co.position() * node.delta; + node.node + .set_pose(co_pos.append_translation(self.gfx_shift).into()); + node.node + .set_color_recursive(node.tmp_color.take().unwrap_or(node.color)); } // Update body-attached render-only nodes (e.g. MJCF visual meshes). @@ -1316,9 +1365,11 @@ impl GraphicsManager { flags: TestbedStateFlags, _bodies: &RigidBodySet, colliders: &ColliderSet, + soft_bodies: &SoftBodySet, ) { self.draw_surfaces = flags.contains(TestbedStateFlags::DRAW_SURFACES); let show_colliders = self.draw_surfaces && self.colliders_visible; + self.update_soft_mesh_nodes(soft_bodies, show_colliders); // Update instance data for all templates for (template_type, template) in &mut self.templates { @@ -1403,19 +1454,24 @@ impl GraphicsManager { } // Update individual nodes - for node in &mut self.individual_nodes { - node.node.set_visible(show_colliders); - - if let Some(co) = colliders.get(node.collider) { - let co_pos = *co.position() * node.delta; - node.node.set_position(Vec2::new( - (co_pos.translation.x + self.gfx_shift.x) as f32, - (co_pos.translation.y + self.gfx_shift.y) as f32, - )); - node.node.set_rotation(co_pos.rotation.angle() as f32); - node.node - .set_color_recursive(node.tmp_color.take().unwrap_or(node.color)); - } + for index in 0..self.individual_nodes.len() { + self.individual_nodes[index] + .node + .set_visible(show_colliders); + + let Some(co) = colliders.get(self.individual_nodes[index].collider) else { + continue; + }; + self.update_deformable_collider(index, co); + let node = &mut self.individual_nodes[index]; + let co_pos = *co.position() * node.delta; + node.node.set_position(Vec2::new( + (co_pos.translation.x + self.gfx_shift.x) as f32, + (co_pos.translation.y + self.gfx_shift.y) as f32, + )); + node.node.set_rotation(co_pos.rotation.angle() as f32); + node.node + .set_color_recursive(node.tmp_color.take().unwrap_or(node.color)); } } } diff --git a/src_testbed/graphics/graphics_polyline.rs b/src_testbed/graphics/graphics_polyline.rs new file mode 100644 index 000000000..607e1e39f --- /dev/null +++ b/src_testbed/graphics/graphics_polyline.rs @@ -0,0 +1,181 @@ +//! Render geometry of polylines: a stroked mesh in 2D, a tube in 3D. Both layouts only depend on +//! the segment count, so a deforming polyline is followed by rewriting its vertex buffer. + +use kiss3d::prelude::*; +use rapier::geometry::Shape; +#[cfg(feature = "dim2")] +use rapier::geometry::ShapeType; + +/// Half the width, in world units, of a rendered polyline's stroke. +#[cfg(feature = "dim2")] +const POLYLINE_HALF_WIDTH: f32 = 0.05; + +/// The number of segments a round join is drawn with. +#[cfg(feature = "dim2")] +const POLYLINE_JOIN_SUBDIV: usize = 8; + +/// The filled geometry of a stroked polyline: a quad per segment, plus a disc at every vertex +/// so the corners join cleanly. The layout only depends on the segment and vertex counts, so a +/// deforming polyline's vertex buffer can be rewritten in place (see `update_deformable_node`). +#[cfg(feature = "dim2")] +pub(super) fn stroked_polyline(vertices: &[Vec2], indices: &[[u32; 2]]) -> (Vec, Vec<[u32; 3]>) { + let mut vtx = Vec::with_capacity(indices.len() * 4 + vertices.len() * (POLYLINE_JOIN_SUBDIV + 1)); + let mut idx = Vec::with_capacity(indices.len() * 2 + vertices.len() * POLYLINE_JOIN_SUBDIV); + + for segment in indices { + let (a, b) = (vertices[segment[0] as usize], vertices[segment[1] as usize]); + let dir = (b - a).normalize_or_zero(); + let normal = Vec2::new(-dir.y, dir.x) * POLYLINE_HALF_WIDTH; + let base = vtx.len() as u32; + vtx.extend_from_slice(&[a + normal, a - normal, b - normal, b + normal]); + idx.push([base, base + 1, base + 2]); + idx.push([base, base + 2, base + 3]); + } + + for vertex in vertices { + let center = vtx.len() as u32; + vtx.push(*vertex); + for k in 0..POLYLINE_JOIN_SUBDIV { + let angle = k as f32 / POLYLINE_JOIN_SUBDIV as f32 * std::f32::consts::TAU; + vtx.push(*vertex + Vec2::new(angle.cos(), angle.sin()) * POLYLINE_HALF_WIDTH); + } + for k in 0..POLYLINE_JOIN_SUBDIV { + let next = (k + 1) % POLYLINE_JOIN_SUBDIV; + idx.push([ + center, + center + 1 + k as u32, + center + 1 + next as u32, + ]); + } + } + + (vtx, idx) +} + +/// The vertices and segments of a shape drawn as a polyline (`None` for anything else). +#[cfg(feature = "dim2")] +pub(super) fn polyline_geometry(shape: &dyn Shape) -> Option<(Vec, Vec<[u32; 2]>)> { + let to_vec2 = |pts: &[rapier::math::Vector]| -> Vec { + pts.iter().map(|p| Vec2::new(p.x as f32, p.y as f32)).collect() + }; + match shape.shape_type() { + ShapeType::Polyline => { + let polyline = shape.as_polyline()?; + Some((to_vec2(polyline.vertices()), polyline.indices().to_vec())) + } + ShapeType::HeightField => { + let (vertices, indices) = shape.as_heightfield()?.to_polyline(); + Some((to_vec2(&vertices), indices)) + } + _ => None, + } +} + +/// The radius, in world units, of a rendered 3D polyline's tube. +#[cfg(feature = "dim3")] +const POLYLINE_RADIUS: f32 = 0.04; + +/// The number of sides of that tube. +#[cfg(feature = "dim3")] +const POLYLINE_SIDES: usize = 6; + +/// A 3D polyline as a closed tube: a ring of `POLYLINE_SIDES` vertices at each end of every +/// segment, its sides, and a flat cap on each end. The layout only depends on the segment count, +/// so a deforming polyline (a soft body's wire) is followed by rewriting the vertex buffer. +#[cfg(feature = "dim3")] +pub(super) fn tube_polyline(vertices: &[Vec3], indices: &[[u32; 2]]) -> (Vec, Vec<[u32; 3]>) { + let ring = POLYLINE_SIDES as u32; + let mut vtx = Vec::with_capacity(indices.len() * (2 * POLYLINE_SIDES + 2)); + let mut idx = Vec::with_capacity(indices.len() * 4 * POLYLINE_SIDES); + + for segment in indices { + let (a, b) = (vertices[segment[0] as usize], vertices[segment[1] as usize]); + let dir = (b - a).normalize_or_zero(); + // Any basis orthogonal to the segment: the tube is round, so its roll does not matter. + let up = if dir.x.abs() < 0.9 { Vec3::X } else { Vec3::Y }; + let u = dir.cross(up).normalize_or_zero() * POLYLINE_RADIUS; + let v = dir.cross(u.normalize_or_zero()) * POLYLINE_RADIUS; + + let base = vtx.len() as u32; + for center in [a, b] { + for k in 0..POLYLINE_SIDES { + let angle = k as f32 / POLYLINE_SIDES as f32 * std::f32::consts::TAU; + vtx.push(center + u * angle.cos() + v * angle.sin()); + } + } + vtx.push(a); + vtx.push(b); + + for k in 0..ring { + let next = (k + 1) % ring; + // Sides. + idx.push([base + k, base + ring + k, base + ring + next]); + idx.push([base + k, base + ring + next, base + next]); + // Caps. + idx.push([base + 2 * ring, base + next, base + k]); + idx.push([base + 2 * ring + 1, base + ring + k, base + ring + next]); + } + } + + (vtx, idx) +} + +/// The vertices and segments of a 3D shape drawn as a polyline (`None` for anything else). +#[cfg(feature = "dim3")] +pub(super) fn polyline_geometry(shape: &dyn Shape) -> Option<(Vec, Vec<[u32; 2]>)> { + let polyline = shape.as_polyline()?; + let vertices = polyline + .vertices() + .iter() + .map(|p| Vec3::new(p.x as f32, p.y as f32, p.z as f32)) + .collect(); + Some((vertices, polyline.indices().to_vec())) +} + +#[cfg(test)] +mod tests { + /// The tube built for a 3D polyline has a fixed layout per segment, which is what lets a + /// deforming wire be followed by rewriting the vertex buffer. + #[cfg(feature = "dim3")] + #[test] + fn tube_layout_is_fixed_per_segment() { + use super::{POLYLINE_SIDES, tube_polyline}; + use kiss3d::prelude::Vec3; + + let vertices: Vec = (0..5) + .map(|i| Vec3::new(i as f32 * 0.5, 0.0, 0.0)) + .collect(); + let segments: Vec<[u32; 2]> = (0..4).map(|i| [i, i + 1]).collect(); + let (vtx, idx) = tube_polyline(&vertices, &segments); + + assert_eq!(vtx.len(), segments.len() * (2 * POLYLINE_SIDES + 2)); + assert_eq!(idx.len(), segments.len() * 4 * POLYLINE_SIDES); + assert!( + vtx.iter().all(|v| v.is_finite()), + "the tube went non-finite" + ); + assert!( + idx.iter().flatten().all(|i| (*i as usize) < vtx.len()), + "the tube indexes a vertex it does not have" + ); + + // Moving the wire moves the tube with it, keeping the same layout. + let moved: Vec = vertices.iter().map(|v| *v + Vec3::Y).collect(); + let (moved_vtx, _) = tube_polyline(&moved, &segments); + assert_eq!(moved_vtx.len(), vtx.len()); + for (before, after) in vtx.iter().zip(&moved_vtx) { + assert!((*after - *before - Vec3::Y).length() < 1.0e-5); + } + } + + /// A degenerate segment (two coincident points) must not produce NaNs. + #[cfg(feature = "dim3")] + #[test] + fn a_degenerate_segment_stays_finite() { + use super::tube_polyline; + use kiss3d::prelude::Vec3; + + let (vtx, _) = tube_polyline(&[Vec3::ZERO, Vec3::ZERO], &[[0, 1]]); + assert!(vtx.iter().all(|v| v.is_finite())); + } +} diff --git a/src_testbed/graphics/graphics_soft_bodies.rs b/src_testbed/graphics/graphics_soft_bodies.rs new file mode 100644 index 000000000..d9ac54a3f --- /dev/null +++ b/src_testbed/graphics/graphics_soft_bodies.rs @@ -0,0 +1,315 @@ +//! Soft-body rendering: soft-body colors, the skins a soft body only draws, and the collision +//! meshes whose geometry deforms under their colliders. + +use super::graphics_polyline::polyline_geometry; +#[cfg(feature = "dim2")] +use super::graphics_polyline::stroked_polyline; +#[cfg(feature = "dim3")] +use super::graphics_polyline::tube_polyline; +use super::{GraphicsManager, IndividualNode, SceneNode}; +use kiss3d::prelude::*; +use rapier::dynamics::{SoftBodyHandle, SoftBodySet, SoftMeshRef}; +use rapier::geometry::{Collider, Shape, SharedShape}; +use std::collections::HashMap; + +/// A render-only node following a mesh a soft body only *draws*: the skin it wears without +/// colliding through it. A colliding mesh is a collider and is drawn as one; a drawn skin has no +/// collider, and is what the body looks like, so it replaces the collider on screen. +struct SoftMeshNode { + node: SceneNode, + mesh: SoftMeshRef, + color: Color, + /// Whether the node's mesh shares its vertices between elements (smooth normals): how it + /// was built, and how the moved vertices are written back into it. + smooth: bool, +} + +/// The soft-body render state of a [`GraphicsManager`]. +#[derive(Default)] +pub(super) struct SoftBodyGraphics { + /// Soft-body meshes that are only drawn (the skins), which no collider holds. + mesh_nodes: Vec, + /// Colors per soft body (shared by its particles and its surface). + colors: HashMap, +} + +impl SoftBodyGraphics { + pub(super) fn clear(&mut self) { + self.mesh_nodes.clear(); + self.colors.clear(); + } +} + +/// Is this mesh a skin the body only draws? A skin is what the body looks like, so a skin that +/// does not collide is still rendered, unlike a boundary the body does not collide +/// through, which stands for nothing on screen. +fn is_drawn_skin(mesh: &rapier::dynamics::SoftCollisionMesh) -> bool { + mesh.is_skinned() && !mesh.collision_enabled() +} + +/// The shape a drawn soft-body mesh is built as, from its world-space geometry: the same form +/// its colliding counterpart takes (a triangle mesh in 3D, a polyline in 2D). +fn drawn_mesh_shape( + vertices: Vec, + indices: &[[u32; rapier::math::DIM]], +) -> Option { + if vertices.is_empty() || indices.is_empty() { + return None; + } + #[cfg(feature = "dim2")] + { + use rapier::parry::shape::Polyline; + Some(SharedShape::new(Polyline::new( + vertices, + Some(indices.to_vec()), + ))) + } + #[cfg(feature = "dim3")] + { + rapier::geometry::TriMesh::new(vertices, indices.to_vec()) + .ok() + .map(SharedShape::new) + } +} + +/// Writes moved `vertices` into a triangle-mesh node's vertex buffer in the node's layout: three +/// unshared vertices per triangle under flat shading (`RenderMesh::replicate_vertices`), +/// the mesh's own under smooth shading. Returns `false` when the counts differ (topology changed). +#[cfg(feature = "dim3")] +fn write_mesh_vertices( + node: &mut SceneNode, + smooth: bool, + vertices: &[rapier::math::Vector], + indices: &[[u32; 3]], +) -> bool { + let expected = if smooth { + vertices.len() + } else { + indices.len() * 3 + }; + let mut usable = true; + node.modify_vertices(&mut |vtx: &mut Vec| { + if vtx.len() != expected { + usable = false; + return; + } + let point = |i: u32| { + let p = vertices[i as usize]; + Vec3::new(p.x as f32, p.y as f32, p.z as f32) + }; + if smooth { + for (v, i) in vtx.iter_mut().zip(0..vertices.len() as u32) { + *v = point(i); + } + } else { + for (triangle, corners) in indices.iter().enumerate() { + for (k, corner) in corners.iter().enumerate() { + vtx[triangle * 3 + k] = point(*corner); + } + } + } + }); + if usable { + node.recompute_normals(); + } + usable +} + +/// The 2D counterpart: a soft body's drawn mesh is a polyline, redrawn as a stroked mesh from +/// its moved vertices. +#[cfg(feature = "dim2")] +fn write_mesh_vertices( + node: &mut SceneNode, + _smooth: bool, + vertices: &[rapier::math::Vector], + indices: &[[u32; 2]], +) -> bool { + let points: Vec = vertices + .iter() + .map(|p| Vec2::new(p.x as f32, p.y as f32)) + .collect(); + let (stroke, _) = stroked_polyline(&points, indices); + let mut usable = true; + node.modify_vertices(&mut |vtx: &mut Vec| { + if vtx.len() != stroke.len() { + usable = false; + return; + } + vtx.copy_from_slice(&stroke); + }); + usable +} + +/// Whether a collider's node is hidden by a drawn skin: a body wearing one is seen through it, and +/// its collision meshes (the cells' boundary, which encloses the skin) would hide it. +#[cfg(feature = "dim3")] +pub(super) fn is_hidden_by_skin(collider: &Collider, soft_bodies: &SoftBodySet) -> bool { + collider + .deformable_mesh_ref() + .and_then(|mesh| soft_bodies.get(mesh.body)) + .is_some_and(|sb| sb.meshes().any(is_drawn_skin)) +} + +impl GraphicsManager { + /// The color of a soft body (allocated on first request; also applied to its particles). + pub fn soft_body_color(&mut self, handle: SoftBodyHandle) -> Color { + if let Some(c) = self.soft_graphics.colors.get(&handle) { + return *c; + } + let color = Self::gen_color(&mut self.curr_color_index); + self.soft_graphics.colors.insert(handle, color); + color + } + + /// Sets the color of a soft body (its surface and its particles). + pub fn set_initial_soft_body_color(&mut self, handle: SoftBodyHandle, color: Color) { + self.soft_graphics.colors.insert(handle, color); + } + + /// Adds a render node for each mesh `handle` only draws (a skin worn without colliding through + /// it): such a mesh has no collider for the collider path and is what the body looks like, so + /// the body's collision meshes are hidden in its favor (see [`Self::draw`]). + pub fn add_soft_body_meshes(&mut self, handle: SoftBodyHandle, soft_bodies: &SoftBodySet) { + let Some(sb) = soft_bodies.get(handle) else { + return; + }; + let color = self.soft_body_color(handle); + let smooth = self.smooth_mesh_colliders; + let visible = self.draw_surfaces && self.colliders_visible; + for mesh in sb.meshes().filter(|mesh| is_drawn_skin(mesh)) { + let Some(shape) = drawn_mesh_shape(mesh.vertex_positions(sb).collect(), mesh.indices()) + else { + continue; + }; + let Some(mut node) = + Self::create_individual_node(&mut self.scene, &*shape, color, false, smooth) + else { + continue; + }; + node.set_visible(visible); + self.soft_graphics.mesh_nodes.push(SoftMeshNode { + node, + mesh: SoftMeshRef { + body: handle, + id: mesh.id(), + }, + color, + smooth, + }); + } + } + + /// Follows the drawn soft-body meshes: their vertices move every step, so each node's vertex + /// buffer is rewritten in place. A node whose mesh is gone (a removed body) is hidden, and + /// one whose geometry no longer matches (a shading change, or a tear) is built anew. + pub(super) fn update_soft_mesh_nodes(&mut self, soft_bodies: &SoftBodySet, visible: bool) { + let smooth = self.smooth_mesh_colliders; + for index in 0..self.soft_graphics.mesh_nodes.len() { + let node = &mut self.soft_graphics.mesh_nodes[index]; + let mesh_ref = node.mesh; + let Some(sb) = soft_bodies.get(mesh_ref.body) else { + node.node.set_visible(false); + continue; + }; + let Some(mesh) = sb.mesh(mesh_ref.id).filter(|mesh| is_drawn_skin(mesh)) else { + node.node.set_visible(false); + continue; + }; + node.node.set_visible(visible); + let vertices: Vec<_> = mesh.vertex_positions(sb).collect(); + let stale = node.smooth != smooth + || !write_mesh_vertices(&mut node.node, node.smooth, &vertices, mesh.indices()); + if stale { + self.rebuild_soft_mesh_node(index, vertices, mesh.indices(), visible); + } + let node = &mut self.soft_graphics.mesh_nodes[index]; + node.node + .set_pose(rapier::math::Pose::from_translation(self.gfx_shift).into()); + node.node.set_color_recursive(node.color); + } + } + + /// Rebuilds a drawn mesh's node from the geometry it now has. + fn rebuild_soft_mesh_node( + &mut self, + index: usize, + vertices: Vec, + indices: &[[u32; rapier::math::DIM]], + visible: bool, + ) { + let smooth = self.smooth_mesh_colliders; + let color = self.soft_graphics.mesh_nodes[index].color; + let Some(shape) = drawn_mesh_shape(vertices, indices) else { + return; + }; + self.soft_graphics.mesh_nodes[index].node.detach(); + if let Some(mut fresh) = + Self::create_individual_node(&mut self.scene, &*shape, color, false, smooth) + { + fresh.set_visible(visible); + let node = &mut self.soft_graphics.mesh_nodes[index]; + node.node = fresh; + node.smooth = smooth; + } + } + + /// Follows a deformable collider's geometry: its shape's vertices move every step, so the + /// node's vertex buffer is rewritten in place rather than rebuilt. Returns `false` when the + /// shape's topology changed (a tear replaced it) and the node must be built anew. + #[cfg(feature = "dim3")] + fn update_deformable_node(node: &mut IndividualNode, shape: &dyn Shape) -> bool { + if let Some((vertices, segments)) = polyline_geometry(shape) { + let (tube, _) = tube_polyline(&vertices, &segments); + let mut usable = true; + node.node.modify_vertices(&mut |vtx: &mut Vec| { + if vtx.len() != tube.len() { + usable = false; + return; + } + vtx.copy_from_slice(&tube); + }); + if usable { + node.node.recompute_normals(); + } + return usable; + } + let Some(trimesh) = shape.as_trimesh() else { + return true; + }; + write_mesh_vertices( + &mut node.node, + node.smooth, + trimesh.vertices(), + trimesh.indices(), + ) + } + + /// See the 3D overload: in 2D a deformable collider is a polyline, drawn as a stroked mesh + /// whose geometry is rebuilt from the moved vertices. + #[cfg(feature = "dim2")] + fn update_deformable_node(node: &mut IndividualNode, shape: &dyn Shape) -> bool { + let Some((vertices, segments)) = polyline_geometry(shape) else { + return true; + }; + let (stroke, _) = stroked_polyline(&vertices, &segments); + let mut usable = true; + node.node.modify_vertices(&mut |vtx: &mut Vec| { + if vtx.len() != stroke.len() { + usable = false; + return; + } + vtx.copy_from_slice(&stroke); + }); + usable + } + + /// Follows a soft body's collision mesh: a collider like any other, except that its shape + /// deforms every step and a tear replaces it. + pub(super) fn update_deformable_collider(&mut self, index: usize, collider: &Collider) { + if collider.deformable_mesh_ref().is_some() + && !Self::update_deformable_node(&mut self.individual_nodes[index], collider.shape()) + { + self.rebuild_individual_node(index, collider); + } + } +} diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 52ce83e39..f6c5afe0a 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -330,6 +330,11 @@ fn settings_tab(ui: &mut Ui, state: &mut TestbedState, world: &mut PhysicsWorld) .flags .contains(TestbedStateFlags::SMOOTH_MESH_COLLIDERS); if ui + .checkbox(&mut smooth, "Smooth mesh colliders") + .on_hover_text( + "Shared vertices and per-vertex normals on mesh colliders, soft bodies included \ + (flat shading otherwise).", + ) .changed() { state @@ -657,6 +662,12 @@ fn debug_render_tab(ui: &mut Ui, debug_render: &mut DebugRenderPipelineResource) "The soft bodies' elements (structural and cell edges), their cluster frames, \ and their soft-vs-soft contacts.", ), + ( + DebugRenderMode::PSEUDO_NORMALS, + "Pseudo-normals", + "The pseudo-normals of the oriented triangle-meshes and polylines, at their \ + vertices and edge midpoints.", + ), ]; for (flag, label, hover) in FLAGS { @@ -699,6 +710,10 @@ fn debug_render_tab(ui: &mut Ui, debug_render: &mut DebugRenderPipelineResource) Slider::new(&mut style.contact_normal_length, 0.0..=1.0).text("Normal length"), ) .on_hover_text("Length of the contact normals."); + ui.add( + Slider::new(&mut style.pseudo_normal_length, 0.0..=1.0).text("Pseudo-normal length"), + ) + .on_hover_text("Length of the meshes' pseudo-normals."); ui.collapsing("Colors", |ui| { debug_color_picker(ui, "Dynamic colliders", &mut style.collider_dynamic_color); @@ -726,6 +741,16 @@ fn debug_render_tab(ui: &mut Ui, debug_render: &mut DebugRenderPipelineResource) debug_color_picker(ui, "Contact normals", &mut style.contact_normal_color); debug_color_picker(ui, "Soft-body elements", &mut style.soft_body_element_color); debug_color_picker(ui, "Soft-body frames", &mut style.soft_body_frame_color); + debug_color_picker( + ui, + "Vertex pseudo-normals", + &mut style.vertex_pseudo_normal_color, + ); + debug_color_picker( + ui, + "Edge pseudo-normals", + &mut style.edge_pseudo_normal_color, + ); ui.add_space(4.0); ui.label("Multipliers (per HSLA component)"); diff --git a/src_testbed/viewer.rs b/src_testbed/viewer.rs index 298d641ec..bc1f1d7bd 100644 --- a/src_testbed/viewer.rs +++ b/src_testbed/viewer.rs @@ -163,6 +163,10 @@ impl TestbedViewer { highlight_hovered_body(&mut self.graphics, &self.scene_mouse, world); self.graphics.draw( + self.state.flags, + &world.bodies, + &world.colliders, + &world.soft_bodies, ); debug_render_scene(&mut self.window, &mut self.debug_render, world); @@ -662,6 +666,12 @@ impl TestbedViewer { self.graphics.set_initial_body_color(cluster.proxy(), color); } } + // The meshes a soft body only draws (its skin) have no collider to be picked up by + // the collider pass below. + for (sb_handle, _) in world.soft_bodies.iter() { + self.graphics + .add_soft_body_meshes(sb_handle, &world.soft_bodies); + } for (handle, _) in world.bodies.iter() { self.graphics.add_body_colliders( &mut self.window, From af937d92b97f1634a2f869ae0f2664d082b0c7fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 5 Sep 2026 14:56:12 +0200 Subject: [PATCH 07/32] feat(soft-body): tangle prevention and detection: recovery settings, velocity margin, edge speculation, crossing detection --- crates/rapier2d/tests/self_intersect_probe.rs | 373 +++++++++++++ crates/rapier2d/tests/soft_stack_probe.rs | 501 ++++++++++++++++++ .../rapier3d/tests/self_intersect_probe3.rs | 357 +++++++++++++ crates/rapier3d/tests/soft_bodies.rs | 4 + crates/rapier3d/tests/soft_stack_probe3.rs | 426 +++++++++++++++ examples2d/all_examples2.rs | 2 + examples2d/debug_self_intersect2.rs | 219 ++++++++ examples3d/all_examples3.rs | 2 + examples3d/debug_self_intersect3.rs | 225 ++++++++ src/dynamics/integration_parameters.rs | 8 + .../collision_mesh/collision_mesh.rs | 8 + .../collision_mesh_accessors.rs | 21 + .../collision_mesh/collision_mesh_geometry.rs | 219 +++++++- .../collision_mesh/collision_mesh_topology.rs | 11 + src/dynamics/soft_body/mod.rs | 9 +- src/dynamics/soft_body/soft_body_cluster.rs | 7 +- .../soft_body/soft_body_crossing_tests.rs | 67 +++ src/dynamics/soft_body/soft_body_meshes.rs | 16 + .../soft_body_set/soft_body_set_step_sync.rs | 1 + .../soft_body/soft_recovery_settings.rs | 57 ++ .../soft_contact_assembly.rs | 4 + .../soft_contact_assembly_body_contacts.rs | 1 + .../soft_contact_assembly_edge_contacts.rs | 8 + .../soft_contact_assembly_workspace.rs | 65 +++ .../soft_contacts/soft_contacts_edge_pass.rs | 36 ++ .../soft_contacts_vertex_pass.rs | 66 +++ .../soft_contacts/soft_self_contacts.rs | 407 ++++++++++++++ src/pipeline/physics_pipeline/substep.rs | 7 +- src_testbed/save.rs | 3 + src_testbed/testbed/state.rs | 5 + src_testbed/ui.rs | 29 + src_testbed/viewer.rs | 5 + 32 files changed, 3165 insertions(+), 4 deletions(-) create mode 100644 crates/rapier2d/tests/self_intersect_probe.rs create mode 100644 crates/rapier2d/tests/soft_stack_probe.rs create mode 100644 crates/rapier3d/tests/self_intersect_probe3.rs create mode 100644 crates/rapier3d/tests/soft_stack_probe3.rs create mode 100644 examples2d/debug_self_intersect2.rs create mode 100644 examples3d/debug_self_intersect3.rs create mode 100644 src/dynamics/soft_body/soft_body_crossing_tests.rs create mode 100644 src/dynamics/soft_body/soft_recovery_settings.rs create mode 100644 src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs diff --git a/crates/rapier2d/tests/self_intersect_probe.rs b/crates/rapier2d/tests/self_intersect_probe.rs new file mode 100644 index 000000000..55deb7687 --- /dev/null +++ b/crates/rapier2d/tests/self_intersect_probe.rs @@ -0,0 +1,373 @@ +//! Headless probes of the `debug_self_intersect2` demo family: surfaces that start +//! self-intersecting must recover instead of being held tangled by their own self-contact +//! constraints (docs/self-intersection-recovery-plan.md); the 8-shaped blobs probe open problems. + +use rapier2d::prelude::*; + +fn segments_cross(a0: Vector, a1: Vector, b0: Vector, b1: Vector) -> bool { + let d1 = a1 - a0; + let d2 = b1 - b0; + let denom = d1.perp_dot(d2); + if denom.abs() < 1.0e-12 { + return false; + } + let t = (b0 - a0).perp_dot(d2) / denom; + let u = (b0 - a0).perp_dot(d1) / denom; + (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) +} + +fn probe(world: &PhysicsWorld, h: SoftBodyHandle) -> (usize, usize, Real) { + let sb = &world.soft_bodies[h]; + // Boundary self-crossings among non-adjacent surface segments. + let mesh = sb.meshes().next().unwrap(); + let indices = mesh.indices(); + let mut crossings = 0; + for i in 0..indices.len() { + for j in i + 1..indices.len() { + let (ei, ej) = (indices[i], indices[j]); + if ei.iter().any(|v| ej.contains(v)) { + continue; + } + let p = |v: u32| mesh.vertex(sb, v as usize); + if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { + crossings += 1; + } + } + } + // Inverted cells. + let mut inverted = 0; + let mut min_ratio: Real = Real::MAX; + for cell in sb.cells() { + let x: [Vector; 3] = core::array::from_fn(|k| sb.particle_position(cell.vertices[k] as usize)); + let vol = (x[1] - x[0]).perp_dot(x[2] - x[0]) * 0.5; + let ratio = vol / cell.rest_volume; + min_ratio = min_ratio.min(ratio); + if ratio <= 0.0 { + inverted += 1; + } + } + (crossings, inverted, min_ratio) +} + +const SELF_CONTACTS: bool = option_env!("NO_SELF_CONTACTS").is_none(); +const BLOB_HZ: Real = if option_env!("STIFF_BLOB").is_some() { 20.0 } else { 4.0 }; + +/// A closed blob (no cells: edges + boundary-volume) whose top vertex starts pushed down +/// through the bottom wall: does the cell-less capture recover? +#[test] +fn blob_vertex_capture() { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -1.0)), + ColliderBuilder::cuboid(4.0, 0.5), + ); + let h = world.insert_soft_body( + SoftBodyBuilder::disk(Vector::new(0.0, 0.0), 0.5, 16) + .softness(SpringCoefficients::new(BLOB_HZ, 1.0)) + .self_contacts(SELF_CONTACTS) + .particle_mass(0.05), + ); + // Vertex 4 sits at the top of the disk (angle 90°); park it below the bottom wall. + world.soft_bodies[h].set_particle_position(4, Vector::new(0.0, -0.8)); + + for step in 0..=600 { + if step % 60 == 0 { + let (crossings, _, _) = probe(&world, h); + let p4 = world.soft_bodies[h].particle_position(4); + println!( + "step {step:4}: crossings={crossings} p4=({:+.3},{:+.3})", + p4.x, p4.y + ); + } + world.step(); + } + let (crossings, _, _) = probe(&world, h); + println!("final: crossings={crossings}"); + assert_eq!(crossings, 0, "the blob's boundary is still self-crossed"); +} + +#[test] +fn self_intersect_recovery() { + let mut world = PhysicsWorld::new(); + let strip = SoftBodyBuilder::grid(Vector::ZERO, Vector::new(3.0, 0.15), 3, 2) + .pinned_particles([0, 1, 4, 5]) + .softness(SpringCoefficients::new(3.0, 1.0)) + .self_contacts(SELF_CONTACTS); + let h = world.insert_soft_body(strip); + world.soft_bodies[h].set_particle_position(3, Vector::new(1.0, -0.4)); + + for step in 0..=600 { + if step % 60 == 0 { + let (crossings, inverted, min_ratio) = probe(&world, h); + let p2 = world.soft_bodies[h].particle_position(2); + let p3 = world.soft_bodies[h].particle_position(3); + println!( + "step {step:4}: crossings={crossings} inverted_cells={inverted} \ + min_vol_ratio={min_ratio:+.3} p2=({:+.3},{:+.3}) p3=({:+.3},{:+.3})", + p2.x, p2.y, p3.x, p3.y + ); + } + world.step(); + } + let (crossings, inverted, _) = probe(&world, h); + println!("final: crossings={crossings} inverted_cells={inverted}"); + assert_eq!(crossings, 0, "the strip's boundary is still self-crossed"); + assert_eq!(inverted, 0, "the strip still has inverted cells"); +} + + +/// Boundary crossings between two bodies, and how many of each body's vertices lie inside +/// the other (even-odd parity along +x). +fn pair_overlap( + world: &PhysicsWorld, + a: SoftBodyHandle, + b: SoftBodyHandle, +) -> (usize, usize, usize) { + let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); + let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); + let pa = |v: u32| ma.vertex(sa, v as usize); + let pb = |v: u32| mb.vertex(sb, v as usize); + let mut crossings = 0; + for ea in ma.indices() { + for eb in mb.indices() { + if segments_cross(pa(ea[0]), pa(ea[1]), pb(eb[0]), pb(eb[1])) { + crossings += 1; + } + } + } + let inside = |point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody| { + let mut parity = false; + for e in mesh.indices() { + let (p0, p1) = ( + mesh.vertex(body, e[0] as usize), + mesh.vertex(body, e[1] as usize), + ); + if (p0.y > point.y) != (p1.y > point.y) { + let t = (point.y - p0.y) / (p1.y - p0.y); + if p0.x + t * (p1.x - p0.x) > point.x { + parity = !parity; + } + } + } + parity + }; + let a_in_b = (0..ma.vertex_count()) + .filter(|&v| inside(pa(v as u32), mb, sb)) + .count(); + let b_in_a = (0..mb.vertex_count()) + .filter(|&v| inside(pb(v as u32), ma, sa)) + .count(); + (crossings, a_in_b, b_in_a) +} + +fn eight_blob(center: Vector, radius: Real, n: usize) -> SoftBodyBuilder { + SoftBodyBuilder::disk(center, radius, n) + .softness(SpringCoefficients::new(4.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) +} + +/// Reshapes a disk blob into an asymmetric 8: a big CCW lobe and a small mirrored (CW) lobe, +/// two circles joined near `center`. The signed area stays clearly positive, but the small +/// lobe's segments are wound backward. +fn make_asym_eight( + world: &mut PhysicsWorld, + h: SoftBodyHandle, + center: Vector, + rb: Real, + rs: Real, +) { + let n = world.soft_bodies[h].particles().len(); + let n_big = (n as Real * rb / (rb + rs)) as usize; + let tau = core::f32::consts::TAU as Real; + for i in 0..n { + let p = if i < n_big { + let t = tau * i as Real / n_big as Real; + center + Vector::new(-rb + rb * t.cos(), rb * t.sin()) + } else { + let t = tau * (i - n_big) as Real / (n - n_big) as Real; + center + Vector::new(rs - rs * t.cos(), -rs * t.sin()) + }; + world.soft_bodies[h].set_particle_position(i, p); + } +} + +fn eight_floor(world: &mut PhysicsWorld) { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(6.0, 0.5), + ); +} + +/// An 8 with a clearly smaller mirrored lobe recovers to an O: the area deficit gives the +/// volume constraint a strong gradient, and the stand-down lets the lobe flip back through. +#[test] +fn asymmetric_eight_recovers() { + let mut world = PhysicsWorld::new(); + eight_floor(&mut world); + let a = world.insert_soft_body(eight_blob(Vector::new(0.0, 0.65), 0.6, 24)); + make_asym_eight(&mut world, a, Vector::new(0.1, 0.65), 0.55, 0.3); + for _ in 0..600 { + world.step(); + } + let (crossings, _, _) = probe(&world, a); + assert_eq!(crossings, 0, "the asymmetric 8 never recovered"); +} + +/// An 8 whose mirrored lobe starts swallowed inside a bigger blob still recovers, and the +/// pair de-overlaps. +#[test] +fn swallowed_lobe_recovers() { + let mut world = PhysicsWorld::new(); + eight_floor(&mut world); + let a = world.insert_soft_body(eight_blob(Vector::new(-0.5, 0.65), 0.6, 24)); + make_asym_eight(&mut world, a, Vector::new(-0.7, 0.65), 0.55, 0.3); + let b = world.insert_soft_body(eight_blob(Vector::new(0.15, 0.65), 0.55, 20)); + for _ in 0..600 { + world.step(); + } + let (sa, _, _) = probe(&world, a); + let (sb, _, _) = probe(&world, b); + let (cross, a_in_b, b_in_a) = pair_overlap(&world, a, b); + assert_eq!((sa, sb), (0, 0), "a blob is still self-crossed"); + assert_eq!((cross, a_in_b, b_in_a), (0, 0, 0), "the blobs still overlap"); +} + +/// The demo capture with a heavy ball parked on top of the tangled region: the load opposes +/// the elastic recovery, and the stand-down must still resolve the crossing (the probed +/// candidate trigger for active untangling; the elasticity won). +#[test] +fn loaded_tangle_recovers() { + let mut world = PhysicsWorld::new(); + let strip = SoftBodyBuilder::grid(Vector::ZERO, Vector::new(3.0, 0.15), 3, 2) + .pinned_particles([0, 1, 4, 5]) + .softness(SpringCoefficients::new(3.0, 1.0)) + .self_contacts(true); + let h = world.insert_soft_body(strip); + world.soft_bodies[h].set_particle_position(3, Vector::new(1.0, -0.4)); + // A ball ~20x the strip's mass resting right above the tangle. + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.3, 0.8)), + ColliderBuilder::ball(0.45).density(20.0), + ); + + for step in 0..=900 { + if step % 60 == 0 { + let (crossings, inverted, min_ratio) = probe(&world, h); + let p3 = world.soft_bodies[h].particle_position(3); + println!( + "step {step:4}: crossings={crossings} inverted={inverted} \ + min_ratio={min_ratio:+.3} p3=({:+.3},{:+.3})", + p3.x, p3.y + ); + } + world.step(); + } + let (crossings, inverted, _) = probe(&world, h); + assert_eq!(crossings, 0, "the loaded tangle never resolved"); + assert_eq!(inverted, 0, "cells stayed inverted under the load"); +} + +/// Rough step-time probe: a pile of resting self-colliding blobs (no tangles), to price the +/// per-step crossing sweep. +#[test] +#[ignore] +fn pile_timing() { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -1.0)), + ColliderBuilder::cuboid(6.0, 0.5), + ); + for j in 0..4 { + for i in 0..6 { + let x = -3.0 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.2 + j as Real * 1.1; + world.insert_soft_body( + SoftBodyBuilder::disk(Vector::new(x, y), 0.5, 24) + .softness(SpringCoefficients::new(20.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05), + ); + } + } + for _ in 0..120 { + world.step(); + } + let start = std::time::Instant::now(); + for _ in 0..2000 { + world.step(); + } + println!("pile: {:.3} ms/step", start.elapsed().as_secs_f64() * 1000.0 / 2000.0); +} + +/// A rope threaded through a pinned rope slides free under gravity instead of hanging frozen: +/// edge constraints touching the mutual crossing stand down. +#[test] +fn rope_through_rope_slides_free() { + let mut world = PhysicsWorld::new(); + let horizontal = world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(-1.5, 0.0), Vector::new(1.5, 0.0), 16) + .pinned_particles([0, 15]) + .particle_mass(0.05) + .particle_radius(0.05), + ); + let vertical = world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(0.02, -1.0), Vector::new(0.02, 1.0), 12) + .particle_mass(0.05) + .particle_radius(0.05), + ); + let crossings = |world: &PhysicsWorld| { + let (cross, _, _) = pair_overlap(world, horizontal, vertical); + cross + }; + assert!(crossings(&world) > 0, "the ropes were authored crossed"); + for _ in 0..300 { + world.step(); + } + let com = world.soft_bodies[vertical].center_of_mass(); + println!("crossings {} com_y {:+.3}", crossings(&world), com.y); + assert_eq!(crossings(&world), 0, "the ropes are still crossed"); + assert!(com.y < -1.5, "the vertical rope hangs frozen at {:+.3}", com.y); +} + +/// With the whole `soft_bodies.recovery` stack disabled a self-intersecting strip stays +/// tangled, and with the defaults it recovers: every toggle is read. +#[test] +fn recovery_toggles_are_wired() { + let build = || { + let mut world = PhysicsWorld::new(); + let strip = SoftBodyBuilder::grid(Vector::ZERO, Vector::new(3.0, 0.15), 3, 2) + .pinned_particles([0, 1, 4, 5]) + .softness(SpringCoefficients::new(3.0, 1.0)) + .self_contacts(true); + let h = world.insert_soft_body(strip); + world.soft_bodies[h].set_particle_position(3, Vector::new(1.0, -0.4)); + (world, h) + }; + + // Defaults: recovers. + let (mut world, h) = build(); + for _ in 0..600 { + world.step(); + } + assert_eq!(probe(&world, h).0, 0, "defaults did not recover the strip"); + + // Everything off: stays tangled, like the pre-recovery baseline. + let (mut world, h) = build(); + world.integration_parameters.soft_bodies.recovery = SoftRecoverySettings { + authored_velocity_margin: false, + edge_speculation: false, + inverted_cell_detection: false, + self_crossing_detection: false, + detection_motion_gating: false, + cross_body_detection: false, + }; + for _ in 0..600 { + world.step(); + } + assert!( + probe(&world, h).0 > 0, + "disabling every recovery toggle still recovered the strip: a toggle is unwired" + ); +} + diff --git a/crates/rapier2d/tests/soft_stack_probe.rs b/crates/rapier2d/tests/soft_stack_probe.rs new file mode 100644 index 000000000..a221335ff --- /dev/null +++ b/crates/rapier2d/tests/soft_stack_probe.rs @@ -0,0 +1,501 @@ +//! Stack scenes for the soft-body depenetration work (columns and a pressed pile of soft bodies) +//! with their gating metrics (boundary crossings, vertices inside another body, cell compression, +//! step time); the `SOFT_STACK_PRESET` env variable (`defaults`, `minimal`) picks the preset. + +use rapier2d::prelude::*; + +fn segments_cross(a0: Vector, a1: Vector, b0: Vector, b1: Vector) -> bool { + let d1 = a1 - a0; + let d2 = b1 - b0; + let denom = d1.perp_dot(d2); + if denom.abs() < 1.0e-12 { + return false; + } + let t = (b0 - a0).perp_dot(d2) / denom; + let u = (b0 - a0).perp_dot(d1) / denom; + (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) +} + +/// Even-odd containment of `point` in a closed polyline mesh (ray along +x). +fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { + let mut parity = false; + for e in mesh.indices() { + let (p0, p1) = ( + mesh.vertex(body, e[0] as usize), + mesh.vertex(body, e[1] as usize), + ); + if (p0.y > point.y) != (p1.y > point.y) { + let t = (point.y - p0.y) / (p1.y - p0.y); + if p0.x + t * (p1.x - p0.x) > point.x { + parity = !parity; + } + } + } + parity +} + +/// Distance from `point` to the closest element of the mesh. +fn surface_distance(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> Real { + let mut best: Real = Real::MAX; + for e in mesh.indices() { + let (p0, p1) = ( + mesh.vertex(body, e[0] as usize), + mesh.vertex(body, e[1] as usize), + ); + let d = p1 - p0; + let len2 = d.length_squared(); + let t = if len2 > 0.0 { + ((point - p0).dot(d) / len2).clamp(0.0, 1.0) + } else { + 0.0 + }; + best = best.min((point - (p0 + d * t)).length()); + } + best +} + +/// The stack metrics of one probe. +#[derive(Clone, Copy, Debug, Default)] +struct Metrics { + /// Boundary self-crossings, summed over the bodies. + self_crossings: usize, + /// Boundary crossings between distinct bodies, summed over the pairs. + cross_crossings: usize, + /// Vertices lying inside another body's closed boundary. + inside: usize, + /// The deepest such vertex (distance to that body's boundary). + max_depth: Real, + /// The most compressed cell (current over rest area), 1.0 without cells. + min_cell_ratio: Real, + /// The lowest inverted-cell count over the bodies, summed. + inverted_cells: usize, +} + +fn measure(world: &PhysicsWorld, handles: &[SoftBodyHandle]) -> Metrics { + let mut m = Metrics { + min_cell_ratio: 1.0, + ..Default::default() + }; + let bodies: Vec<(&SoftBody, &SoftCollisionMesh)> = handles + .iter() + .map(|&h| { + let sb = &world.soft_bodies[h]; + (sb, sb.meshes().next().unwrap()) + }) + .collect(); + for (sb, mesh) in &bodies { + let idx = mesh.indices(); + let p = |v: u32| mesh.vertex(sb, v as usize); + for i in 0..idx.len() { + for j in i + 1..idx.len() { + let (ei, ej) = (idx[i], idx[j]); + if ei.iter().any(|v| ej.contains(v)) { + continue; + } + if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { + m.self_crossings += 1; + } + } + } + for cell in sb.cells() { + let x: [Vector; 3] = + core::array::from_fn(|k| sb.particle_position(cell.vertices[k] as usize)); + let vol = (x[1] - x[0]).perp_dot(x[2] - x[0]) * 0.5; + let ratio = vol / cell.rest_volume; + m.min_cell_ratio = m.min_cell_ratio.min(ratio); + if ratio <= 0.0 { + m.inverted_cells += 1; + } + } + } + for a in 0..bodies.len() { + for b in 0..bodies.len() { + if a == b { + continue; + } + let (sa, ma) = bodies[a]; + let (sb, mb) = bodies[b]; + let pa = |v: u32| ma.vertex(sa, v as usize); + let pb = |v: u32| mb.vertex(sb, v as usize); + if a < b { + for ea in ma.indices() { + for eb in mb.indices() { + if segments_cross(pa(ea[0]), pa(ea[1]), pb(eb[0]), pb(eb[1])) { + m.cross_crossings += 1; + } + } + } + } + if !mb.is_closed() { + continue; + } + for v in 0..ma.vertex_count() { + let x = pa(v as u32); + if inside(x, mb, sb) { + m.inside += 1; + m.max_depth = m.max_depth.max(surface_distance(x, mb, sb)); + } + } + } + } + m +} + +/// The recovery preset under test (`SOFT_STACK_PRESET`). +fn apply_preset(world: &mut PhysicsWorld) -> String { + let preset = std::env::var("SOFT_STACK_PRESET").unwrap_or_else(|_| "defaults".into()); + let r = &mut world.integration_parameters.soft_bodies.recovery; + *r = Default::default(); + match preset.as_str() { + "defaults" => {} + // The user's reference settings (2026-09-04): prevention without edge speculation, + // detection + stand-down, the intersection-volume constraints on. + "reference" => { + r.edge_speculation = false; + } + other => panic!("unknown SOFT_STACK_PRESET `{other}`"), + } + preset +} + +fn floor(world: &mut PhysicsWorld, half_width: Real) { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(half_width, 0.5), + ); +} + +fn blob(center: Vector, radius: Real) -> SoftBodyBuilder { + SoftBodyBuilder::disk(center, radius, 20) + .softness(SpringCoefficients::new(20.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) +} + +fn jelly(center: Vector, half: Real) -> SoftBodyBuilder { + SoftBodyBuilder::grid(center, Vector::splat(half), 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 3.0e3, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .self_contacts(true) + .particle_mass(0.1) + .particle_radius(0.08) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) +} + +/// Runs `steps` steps, printing the metrics every `every` steps (and the step time at the end); +/// `hook` runs before every step (kinematic drivers). +fn run( + name: &str, + world: &mut PhysicsWorld, + handles: &[SoftBodyHandle], + steps: usize, + every: usize, + mut hook: impl FnMut(&mut PhysicsWorld, usize), +) -> Metrics { + let preset = apply_preset(world); + let mut worst = Metrics::default(); + let mut stepping = std::time::Duration::ZERO; + for step in 0..=steps { + if step % every == 0 { + let m = measure(world, handles); + println!( + "{name} [{preset}] step {step:4}: self={} cross={} inside={} depth={:.3} \ + min_cell={:+.3} inverted={}", + m.self_crossings, + m.cross_crossings, + m.inside, + m.max_depth, + m.min_cell_ratio, + m.inverted_cells + ); + worst.self_crossings = worst.self_crossings.max(m.self_crossings); + worst.cross_crossings = worst.cross_crossings.max(m.cross_crossings); + worst.inside = worst.inside.max(m.inside); + worst.max_depth = worst.max_depth.max(m.max_depth); + worst.min_cell_ratio = worst.min_cell_ratio.min(m.min_cell_ratio); + worst.inverted_cells = worst.inverted_cells.max(m.inverted_cells); + } + if step == steps { + break; + } + hook(world, step); + let start = std::time::Instant::now(); + world.step(); + stepping += start.elapsed(); + } + let ms = stepping.as_secs_f64() * 1000.0 / steps as f64; + let last = measure(world, handles); + println!( + "{name} [{preset}] final: self={} cross={} inside={} depth={:.3} min_cell={:+.3} \ + inverted={} ({ms:.3} ms/step); worst: cross={} inside={} depth={:.3} min_cell={:+.3}", + last.self_crossings, + last.cross_crossings, + last.inside, + last.max_depth, + last.min_cell_ratio, + last.inverted_cells, + worst.cross_crossings, + worst.inside, + worst.max_depth, + worst.min_cell_ratio + ); + last +} + +/// A column of twelve cell-less blobs dropped one on the other: the bottom blobs bear the +/// whole column's weight and must neither be pushed through the floor nor into each other. +#[test] +fn blob_column() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 4.0); + let handles: Vec<_> = (0..12) + .map(|i| { + let x = (i % 2) as Real * 0.05; + world.insert_soft_body(blob(Vector::new(x, 0.55 + i as Real * 1.05), 0.5)) + }) + .collect(); + // A heavy box lands on the column once it has settled (twenty times a blob's mass). + let m = run( + "blob_column", + &mut world, + &handles, + 1200, + 100, + |world, step| { + if step == 300 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 14.0)), + ColliderBuilder::cuboid(0.6, 0.4).density(10.0), + ); + } + }, + ); + assert_eq!( + m.cross_crossings, 0, + "blobs of the column penetrate each other" + ); + assert_eq!(m.self_crossings, 0, "a blob of the column is self-crossed"); +} + +/// A column of eight solid jelly squares: the cells of the bottom ones compress under the +/// load but must never invert, and the boundaries must not cross. +#[test] +fn jelly_column() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 4.0); + let handles: Vec<_> = (0..8) + .map(|i| { + let x = (i % 2) as Real * 0.05; + world.insert_soft_body(jelly(Vector::new(x, 0.55 + i as Real * 1.1), 0.5)) + }) + .collect(); + let m = run( + "jelly_column", + &mut world, + &handles, + 1200, + 100, + |world, step| { + if step == 300 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 10.0)), + ColliderBuilder::cuboid(0.6, 0.4).density(10.0), + ); + } + }, + ); + assert_eq!( + m.cross_crossings, 0, + "squares of the column penetrate each other" + ); + assert_eq!( + m.inverted_cells, 0, + "a square of the column has inverted cells" + ); +} + +/// A bin of blobs pressed from above by a kinematic plate: the plate descends slowly to two +/// thirds of the pile's height and holds there. Nothing in the pile may end up crossed or +/// inside a neighbor, whatever the load. +#[test] +fn pressed_blob_pile() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 3.5); + for x in [-3.5, 3.5] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 4.0)), + ColliderBuilder::cuboid(0.5, 5.0), + ); + } + let mut handles = Vec::new(); + for j in 0..4 { + for i in 0..5 { + let x = -2.4 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.55 + j as Real * 1.05; + handles.push(world.insert_soft_body(blob(Vector::new(x, y), 0.5))); + } + } + let plate = world.insert_body( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 5.0)), + ); + world.insert_collider(ColliderBuilder::cuboid(2.9, 0.25), Some(plate)); + // Settle, press down at 0.5 u/s to y = 3.0 (the pile starts ~4.3 high), then hold. + let m = run( + "pressed_blob_pile", + &mut world, + &handles, + 1500, + 150, + |world, step| { + if step >= 300 { + let y = (5.0 - (step - 300) as Real * 0.5 / 60.0).max(3.0); + world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y)); + } + }, + ); + assert_eq!( + m.cross_crossings, 0, + "blobs of the pressed pile penetrate each other" + ); + assert_eq!( + m.self_crossings, 0, + "a blob of the pressed pile is self-crossed" + ); +} + +/// A bin of jelly squares pressed the same way, to four fifths of the pile's height: cells +/// compress, none may invert. +#[test] +fn pressed_jelly_pile() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 3.5); + for x in [-3.5, 3.5] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 4.0)), + ColliderBuilder::cuboid(0.5, 5.0), + ); + } + let mut handles = Vec::new(); + for j in 0..3 { + for i in 0..5 { + let x = -2.4 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.55 + j as Real * 1.1; + handles.push(world.insert_soft_body(jelly(Vector::new(x, y), 0.5))); + } + } + let plate = world.insert_body( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 4.0)), + ); + world.insert_collider(ColliderBuilder::cuboid(2.9, 0.25), Some(plate)); + let m = run( + "pressed_jelly_pile", + &mut world, + &handles, + 1500, + 150, + |world, step| { + if step >= 300 { + let y = (4.0 - (step - 300) as Real * 0.5 / 60.0).max(2.7); + world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y)); + } + }, + ); + assert_eq!( + m.cross_crossings, 0, + "squares of the pressed pile penetrate each other" + ); + assert_eq!( + m.inverted_cells, 0, + "a square of the pressed pile has inverted cells" + ); +} + +/// Checks that the jelly pile crushed to two thirds of its height keeps its cells uninverted and +/// its boundaries uncrossed. Ignored until a phase passes it. +#[test] +#[ignore] +fn crushed_jelly_pile() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 3.5); + for x in [-3.5, 3.5] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 4.0)), + ColliderBuilder::cuboid(0.5, 5.0), + ); + } + let mut handles = Vec::new(); + for j in 0..3 { + for i in 0..5 { + let x = -2.4 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.55 + j as Real * 1.1; + handles.push(world.insert_soft_body(jelly(Vector::new(x, y), 0.5))); + } + } + let plate = world.insert_body( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 4.0)), + ); + world.insert_collider(ColliderBuilder::cuboid(2.9, 0.25), Some(plate)); + let m = run( + "crushed_jelly_pile", + &mut world, + &handles, + 1500, + 150, + |world, step| { + if step >= 300 { + let y = (4.0 - (step - 300) as Real * 0.5 / 60.0).max(2.4); + world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y)); + } + }, + ); + assert_eq!( + m.cross_crossings, 0, + "squares of the crushed pile penetrate each other" + ); + assert_eq!( + m.self_crossings, 0, + "a square of the crushed pile is self-crossed" + ); + assert_eq!( + m.inverted_cells, 0, + "a square of the crushed pile has inverted cells" + ); +} + +/// Step-time reference: the 24-blob pile of the recovery probes, resting. +#[test] +#[ignore] +fn pile_timing() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 6.0); + let handles: Vec<_> = (0..4) + .flat_map(|j| (0..6).map(move |i| (i, j))) + .map(|(i, j)| { + let x = -3.0 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.7 + j as Real * 1.1; + world.insert_soft_body(blob(Vector::new(x, y), 0.5)) + }) + .collect(); + apply_preset(&mut world); + for _ in 0..120 { + world.step(); + } + let start = std::time::Instant::now(); + for _ in 0..2000 { + world.step(); + } + let m = measure(&world, &handles); + println!( + "pile: {:.3} ms/step (cross={} inside={})", + start.elapsed().as_secs_f64() * 1000.0 / 2000.0, + m.cross_crossings, + m.inside + ); +} diff --git a/crates/rapier3d/tests/self_intersect_probe3.rs b/crates/rapier3d/tests/self_intersect_probe3.rs new file mode 100644 index 000000000..291dbc869 --- /dev/null +++ b/crates/rapier3d/tests/self_intersect_probe3.rs @@ -0,0 +1,357 @@ +//! Headless probes of 3D self-intersection recovery: surfaces that start in a self-crossed state +//! must recover instead of being held tangled by their own self-contact constraints (mechanism in +//! the 2D `self_intersect_probe`). Env toggle `NO_SELF_CONTACTS` runs the baseline without them. + +use rapier3d::prelude::*; + +const SELF_CONTACTS: bool = option_env!("NO_SELF_CONTACTS").is_none(); + +/// Whether the segment `[p, q]` crosses the triangle's interior transversally. +fn segment_crosses_triangle(p: Vector, q: Vector, tri: &[Vector; 3]) -> bool { + let n = (tri[1] - tri[0]).cross(tri[2] - tri[0]); + let dp = (p - tri[0]).dot(n); + let dq = (q - tri[0]).dot(n); + if dp * dq >= 0.0 { + return false; + } + let pq = q - p; + let d = [ + pq.dot((tri[0] - p).cross(tri[1] - p)), + pq.dot((tri[1] - p).cross(tri[2] - p)), + pq.dot((tri[2] - p).cross(tri[0] - p)), + ]; + d.iter().all(|x| *x >= 0.0) || d.iter().all(|x| *x <= 0.0) +} + +/// The number of edge-vs-triangle self-crossings of the body's surface, and its inverted cells. +fn probe(world: &PhysicsWorld, h: SoftBodyHandle) -> (usize, usize) { + let sb = &world.soft_bodies[h]; + let mesh = sb.meshes().next().unwrap(); + let indices = mesh.indices(); + let p = |v: u32| mesh.vertex(sb, v as usize); + let mut crossings = 0; + for i in 0..indices.len() { + for j in 0..indices.len() { + let (ei, ej) = (indices[i], indices[j]); + if i == j || ei.iter().any(|v| ej.contains(v)) { + continue; + } + let tri = [p(ej[0]), p(ej[1]), p(ej[2])]; + for k in 0..3 { + let (a, b) = (ei[k], ei[(k + 1) % 3]); + if a < b && segment_crosses_triangle(p(a), p(b), &tri) { + crossings += 1; + } + } + } + } + let mut inverted = 0; + for cell in sb.cells() { + let x: [Vector; 4] = + core::array::from_fn(|k| sb.particle_position(cell.vertices[k] as usize)); + let vol = (x[1] - x[0]).cross(x[2] - x[0]).dot(x[3] - x[0]); + if vol * cell.rest_volume <= 0.0 { + inverted += 1; + } + } + (crossings, inverted) +} + +/// Rough step-time probe: a resting self-colliding cloth (no tangles), to price the per-step +/// edge-vs-triangle crossing sweep. +#[test] +#[ignore] +fn cloth_timing() { + let mut world = PhysicsWorld::new(); + let n = 20; + let edge = |i: usize, j: usize| i == 0 || j == 0 || i == n - 1 || j == n - 1; + let pinned = (0..n * n) + .filter(|&id| edge(id / n, id % n)) + .map(|id| id as u32); + let extent = 0.12 * (n - 1) as Real / 2.0; + world.insert_soft_body( + SoftBodyBuilder::cloth( + Vector::new(-extent, 2.0, -extent), + Vector::new(0.12, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.12), + n, + n, + ) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.02) + .particle_radius(0.05) + .pinned_particles(pinned) + .self_contacts(true) + ); + for _ in 0..120 { + world.step(); + } + let start = std::time::Instant::now(); + for _ in 0..1000 { + world.step(); + } + println!( + "cloth: {:.3} ms/step", + start.elapsed().as_secs_f64() * 1000.0 / 1000.0 + ); +} + +/// A thin volumetric slab pinned at both end faces, with a top vertex parked below the bottom +/// face: cells invert and the boundary trimesh self-crosses. The stand-down (inverted cells +/// and edge-vs-triangle crossings) must let the elasticity pull it back out. +#[test] +fn slab_vertex_capture() { + let mut world = PhysicsWorld::new(); + let builder = SoftBodyBuilder::cuboid( + Vector::new(0.0, 0.15, 0.0), + Vector::new(1.5, 0.15, 0.15), + 3, + 2, + 2, + ) + .softness(SpringCoefficients::new(5.0, 1.0)); + let pinned: Vec = builder + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x.abs() > 1.4) + .map(|(i, _)| i as u32) + .collect(); + let target = Vector::new(0.0, 0.3, 0.15); + let captured = builder + .particle_positions() + .iter() + .enumerate() + .min_by(|(_, a), (_, b)| { + (**a - target) + .length() + .partial_cmp(&(**b - target).length()) + .unwrap() + }) + .unwrap() + .0; + let h = world.insert_soft_body(builder.pinned_particles(pinned).self_contacts(SELF_CONTACTS)); + world.soft_bodies[h].set_particle_position(captured, Vector::new(0.3, -0.4, 0.0)); + + for step in 0..=600 { + if step % 60 == 0 { + let (crossings, inverted) = probe(&world, h); + let p = world.soft_bodies[h].particle_position(captured); + println!( + "step {step:4}: crossings={crossings} inverted_cells={inverted} \ + captured=({:+.3},{:+.3},{:+.3})", + p.x, p.y, p.z + ); + } + world.step(); + } + let (crossings, inverted) = probe(&world, h); + println!("final: crossings={crossings} inverted_cells={inverted}"); + assert_eq!(crossings, 0, "the slab's boundary is still self-crossed"); + assert_eq!(inverted, 0, "the slab still has inverted cells"); +} + +/// A cloth strip folded into two layers (no cells at all), with one top-layer vertex parked +/// below the bottom layer: its incident triangles pierce the bottom layer. Only the crossing +/// signal can see this; the stand-down must let the elasticity pull the vertex back through. +#[test] +fn folded_cloth_vertex_capture() { + let mut world = PhysicsWorld::new(); + let (nu, nv) = (8usize, 3usize); + let builder = SoftBodyBuilder::cloth( + Vector::ZERO, + Vector::new(0.25, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.25), + nu, + nv, + ) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.02) + .particle_radius(0.05) + // Both ends of the strip pinned: the outer edge of the bottom layer at its rest place, + // the free edge of the folded-back top layer above it (positions set below). + .pinned_particles((0..nv as u32).chain((nu as u32 - 1) * nv as u32..(nu * nv) as u32)) + .self_contacts(SELF_CONTACTS); + let h = world.insert_soft_body(builder); + // Fold columns 4..8 back over the first four, one gap above them. + let idx = |i: usize, j: usize| i * nv + j; + for i in 4..nu { + for j in 0..nv { + let x = 0.25 * (7 - i) as Real; + world.soft_bodies[h] + .set_particle_position(idx(i, j), Vector::new(x, 0.15, 0.25 * j as Real)); + } + } + // Park a middle vertex of the top layer below the bottom layer. + let captured = idx(5, 1); + world.soft_bodies[h].set_particle_position(captured, Vector::new(0.5, -0.15, 0.25)); + + for step in 0..=600 { + if step % 60 == 0 { + let (crossings, _) = probe(&world, h); + let p = world.soft_bodies[h].particle_position(captured); + println!( + "step {step:4}: crossings={crossings} captured=({:+.3},{:+.3},{:+.3})", + p.x, p.y, p.z + ); + } + world.step(); + } + let (crossings, _) = probe(&world, h); + println!("final: crossings={crossings}"); + assert_eq!(crossings, 0, "the cloth is still self-crossed"); +} + +/// The number of edge-vs-triangle crossings between two bodies' surfaces (both directions). +fn cross_crossings(world: &PhysicsWorld, a: SoftBodyHandle, b: SoftBodyHandle) -> usize { + let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); + let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); + let pa = |v: u32| ma.vertex(sa, v as usize); + let pb = |v: u32| mb.vertex(sb, v as usize); + let mut crossings = 0; + let mut count = |ea: &[u32; 3], tri: &[Vector; 3], p: &dyn Fn(u32) -> Vector| { + for k in 0..3 { + let (u, v) = (ea[k], ea[(k + 1) % 3]); + if u < v && segment_crosses_triangle(p(u), p(v), tri) { + crossings += 1; + } + } + }; + for ea in ma.indices() { + for eb in mb.indices() { + let tb = [pb(eb[0]), pb(eb[1]), pb(eb[2])]; + count(ea, &tb, &pa); + let ta = [pa(ea[0]), pa(ea[1]), pa(ea[2])]; + count(eb, &ta, &pb); + } + } + crossings +} + +/// A cloth strip threaded through a pinned strip slides free under gravity instead of +/// hanging frozen on it: edge constraints touching the mutual crossing stand down (thin open +/// bodies meet through their edge constraints, so the vertex-pass gate alone cannot free them). +#[test] +fn ribbon_through_ribbon_slides_free() { + let mut world = PhysicsWorld::new(); + let strip = |origin: Vector, du: Vector, dv: Vector| { + SoftBodyBuilder::cloth(origin, du, dv, 12, 2) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.02) + .particle_radius(0.04) + }; + let horizontal = world.insert_soft_body( + strip( + Vector::new(-1.1, 0.0, -0.1), + Vector::new(0.2, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.2), + ) + .pinned_particles([0, 1, 22, 23]), + ); + // Vertical, in the xz-plane of the pinned strip's middle, crossing its surface. + let vertical = world.insert_soft_body(strip( + Vector::new(0.03, -1.1, -0.1), + Vector::new(0.0, 0.2, 0.0), + Vector::new(0.0, 0.0, 0.2), + )); + let crossings = |world: &PhysicsWorld| cross_crossings(world, horizontal, vertical); + assert!(crossings(&world) > 0, "the strips were authored crossed"); + for _ in 0..300 { + world.step(); + } + let com = world.soft_bodies[vertical].center_of_mass(); + println!("crossings {} com_y {:+.3}", crossings(&world), com.y); + assert_eq!(crossings(&world), 0, "the strips are still crossed"); + assert!(com.y < -1.5, "the vertical strip hangs frozen at {:+.3}", com.y); +} + +/// Two closed slim bars crossing edge-first collide instead of passing through: closed-vs-closed +/// pairs emit edge-vs-edge constraints in 3D with the vertex pass's speculative reach. +#[test] +fn edge_leading_bars_collide() { + for speed in [5.0, 20.0] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + // The edge constraints under test are behind the (default-off) edge speculation. + world.integration_parameters.soft_bodies.recovery.edge_speculation = true; + let bar = |center: Vector, along_x: bool| { + let he = if along_x { + Vector::new(1.5, 0.15, 0.15) + } else { + Vector::new(0.15, 0.15, 1.5) + }; + SoftBodyBuilder::cuboid( + center, + he, + if along_x { 6 } else { 2 }, + 2, + if along_x { 2 } else { 6 }, + ) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.1) + .particle_radius(0.02) + }; + let c = core::f32::consts::FRAC_1_SQRT_2; + let a = world.insert_soft_body(bar(Vector::new(0.0, 0.0, 0.0), true)); + for i in 0..world.soft_bodies[a].particles().len() { + let p = world.soft_bodies[a].particle_position(i); + let (y, z) = (p.y, p.z); + world.soft_bodies[a] + .set_particle_position(i, Vector::new(p.x, c * (y - z), c * (y + z))); + } + let b = world.insert_soft_body(bar(Vector::new(0.0, 1.2, 0.0), false)); + for i in 0..world.soft_bodies[b].particles().len() { + let p = world.soft_bodies[b].particle_position(i); + let (x, y) = (p.x, p.y - 1.2); + world.soft_bodies[b].set_particle_position( + i, + Vector::new(c * (x + y), 1.2 + c * (y - x), p.z), + ); + } + for i in 0..world.soft_bodies[b].particles().len() { + world.soft_bodies[b].set_particle_velocity(i, Vector::new(0.0, -speed, 0.0)); + } + let mut max_cross = 0; + for _ in 0..90 { + world.step(); + max_cross = max_cross.max(cross_crossings(&world, a, b)); + } + let com = world.soft_bodies[b].center_of_mass(); + println!("speed {speed:5}: max_cross {max_cross} b_com_y {:+.3}", com.y); + assert_eq!(max_cross, 0, "speed {speed}: the bars crossed"); + } +} + +/// A frictionless rope threaded through a closed soft ball slides out under gravity: the crossing +/// stand-down frees the piercing, so nothing wrong-sided holds the thread. +#[test] +fn rope_through_ball_slides_out() { + let mut world = PhysicsWorld::new(); + world.insert_soft_body( + SoftBodyBuilder::sphere(Vector::new(0.0, 0.0, 0.0), 0.4, 1) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.1) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.0)) + .pinned_particles([0, 3, 11]), + ); + let rope = world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(0.0, -1.0, 0.02), Vector::new(0.0, 1.0, 0.02), 16) + .particle_mass(0.02) + .particle_radius(0.03) + .surface_collider(ColliderBuilder::ball(0.03).friction(0.0)), + ); + for _ in 0..400 { + world.step(); + } + let com = world.soft_bodies[rope].center_of_mass(); + let inside = (0..16) + .filter(|&i| world.soft_bodies[rope].particle_position(i).length() < 0.4) + .count(); + println!("rope com_y {:+.3}, particles inside ball: {inside}", com.y); + assert_eq!(inside, 0, "the rope still threads the ball"); + assert!(com.y < -1.0, "the rope hangs frozen at {:+.3}", com.y); +} + diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index 57357d487..e6968967f 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -1508,6 +1508,10 @@ fn creeping_body_stays_awake() { let mut world = world_with_ground(); let mut handles = vec![]; for (i, plastic_yield) in [0.0, 0.05].into_iter().enumerate() { + // Resting on the ground rather than authored into it, and the elastic twin squat enough + // not to buckle: overlap recovery is paced (see SoftPairBudget) and this test is about + // sleep gating. The creeping twin stays slender (it collapses either way). + let width = if plastic_yield > 0.0 { 0.3 } else { 0.45 }; let column = SoftBodyBuilder::cuboid( Vector::new(i as Real * 3.0, 1.3, 0.0), Vector::new(width, 1.2, width), diff --git a/crates/rapier3d/tests/soft_stack_probe3.rs b/crates/rapier3d/tests/soft_stack_probe3.rs new file mode 100644 index 000000000..b25bf1d9e --- /dev/null +++ b/crates/rapier3d/tests/soft_stack_probe3.rs @@ -0,0 +1,426 @@ +//! 3D stack scenes for the soft-body depenetration work (metrics in the 2D `soft_stack_probe`): +//! balloon and jelly-cube columns under a heavy weight, and a cube pile pressed by a kinematic +//! plate. `SOFT_STACK_PRESET` selects the recovery preset (`defaults`, `minimal`). + +use rapier3d::parry::query::PointQuery; +use rapier3d::prelude::*; + +/// Whether the segment `[p, q]` crosses the triangle's interior transversally. +fn segment_crosses_triangle(p: Vector, q: Vector, tri: &[Vector; 3]) -> bool { + let n = (tri[1] - tri[0]).cross(tri[2] - tri[0]); + let dp = (p - tri[0]).dot(n); + let dq = (q - tri[0]).dot(n); + if dp * dq >= 0.0 { + return false; + } + let pq = q - p; + let d = [ + pq.dot((tri[0] - p).cross(tri[1] - p)), + pq.dot((tri[1] - p).cross(tri[2] - p)), + pq.dot((tri[2] - p).cross(tri[0] - p)), + ]; + d.iter().all(|x| *x >= 0.0) || d.iter().all(|x| *x <= 0.0) +} + +/// Whether two triangles cross (an edge of one pierces the other). +fn triangles_cross(a: &[Vector; 3], b: &[Vector; 3]) -> bool { + (0..3).any(|k| segment_crosses_triangle(a[k], a[(k + 1) % 3], b)) + || (0..3).any(|k| segment_crosses_triangle(b[k], b[(k + 1) % 3], a)) +} + +/// Ray-parity containment of `point` in a closed triangle mesh. +fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { + let q = point + Vector::new(0.5341, 0.6432, 0.5487) * 100.0; + let mut crossings = 0; + for e in mesh.indices() { + let tri: [Vector; 3] = core::array::from_fn(|k| mesh.vertex(body, e[k] as usize)); + if segment_crosses_triangle(point, q, &tri) { + crossings += 1; + } + } + crossings % 2 == 1 +} + +/// Distance from `point` to the closest triangle of the mesh. +fn surface_distance(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> Real { + let mut best: Real = Real::MAX; + for e in mesh.indices() { + let tri = rapier3d::parry::shape::Triangle::new( + mesh.vertex(body, e[0] as usize), + mesh.vertex(body, e[1] as usize), + mesh.vertex(body, e[2] as usize), + ); + best = best.min(tri.distance_to_local_point(point, true)); + } + best +} + +#[derive(Clone, Copy, Debug, Default)] +struct Metrics { + self_crossings: usize, + cross_crossings: usize, + inside: usize, + max_depth: Real, + min_cell_ratio: Real, + inverted_cells: usize, +} + +fn measure(world: &PhysicsWorld, handles: &[SoftBodyHandle]) -> Metrics { + let mut m = Metrics { + min_cell_ratio: 1.0, + ..Default::default() + }; + let bodies: Vec<(&SoftBody, &SoftCollisionMesh)> = handles + .iter() + .map(|&h| { + let sb = &world.soft_bodies[h]; + (sb, sb.meshes().next().unwrap()) + }) + .collect(); + let tris = |sb: &SoftBody, mesh: &SoftCollisionMesh| -> Vec<[Vector; 3]> { + mesh.indices() + .iter() + .map(|e| core::array::from_fn(|k| mesh.vertex(sb, e[k] as usize))) + .collect() + }; + let all: Vec> = bodies.iter().map(|(sb, mesh)| tris(sb, mesh)).collect(); + for (bi, (sb, mesh)) in bodies.iter().enumerate() { + let idx = mesh.indices(); + let t = &all[bi]; + for i in 0..idx.len() { + for j in i + 1..idx.len() { + if idx[i].iter().any(|v| idx[j].contains(v)) { + continue; + } + if triangles_cross(&t[i], &t[j]) { + m.self_crossings += 1; + } + } + } + for cell in sb.cells() { + let x: [Vector; 4] = + core::array::from_fn(|k| sb.particle_position(cell.vertices[k] as usize)); + let vol = (x[1] - x[0]).cross(x[2] - x[0]).dot(x[3] - x[0]) / 6.0; + let ratio = vol / cell.rest_volume; + m.min_cell_ratio = m.min_cell_ratio.min(ratio); + if ratio <= 0.0 { + m.inverted_cells += 1; + } + } + } + for a in 0..bodies.len() { + for b in 0..bodies.len() { + if a == b { + continue; + } + let (sa, ma) = bodies[a]; + let (sb, mb) = bodies[b]; + if a < b { + for ta in &all[a] { + for tb in &all[b] { + if triangles_cross(ta, tb) { + m.cross_crossings += 1; + } + } + } + } + if !mb.is_closed() { + continue; + } + for v in 0..ma.vertex_count() { + let x = ma.vertex(sa, v); + if inside(x, mb, sb) { + m.inside += 1; + m.max_depth = m.max_depth.max(surface_distance(x, mb, sb)); + } + } + } + } + m +} + +fn apply_preset(world: &mut PhysicsWorld) -> String { + let preset = std::env::var("SOFT_STACK_PRESET").unwrap_or_else(|_| "defaults".into()); + let r = &mut world.integration_parameters.soft_bodies.recovery; + *r = Default::default(); + match preset.as_str() { + "defaults" => {} + // The user's reference settings (2026-09-04): prevention without edge speculation, + // detection + stand-down, the intersection-volume constraints on. + "reference" => { + r.edge_speculation = false; + } + other => panic!("unknown SOFT_STACK_PRESET `{other}`"), + } + preset +} + +fn floor(world: &mut PhysicsWorld, half: Real) { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), + ColliderBuilder::cuboid(half, 0.5, half), + ); +} + +fn balloon(center: Vector, radius: Real) -> SoftBodyBuilder { + SoftBodyBuilder::sphere(center, radius, 1) + .softness(SpringCoefficients::new(20.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) +} + +fn jelly(center: Vector, half: Real) -> SoftBodyBuilder { + SoftBodyBuilder::cuboid(center, Vector::splat(half), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 3.0e3, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .self_contacts(true) + .particle_mass(0.1) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) +} + +fn run( + name: &str, + world: &mut PhysicsWorld, + handles: &[SoftBodyHandle], + steps: usize, + every: usize, + mut hook: impl FnMut(&mut PhysicsWorld, usize), +) -> Metrics { + let preset = apply_preset(world); + let mut worst = Metrics::default(); + let mut stepping = std::time::Duration::ZERO; + for step in 0..=steps { + if step % every == 0 { + let m = measure(world, handles); + println!( + "{name} [{preset}] step {step:4}: self={} cross={} inside={} depth={:.3} \ + min_cell={:+.3} inverted={}", + m.self_crossings, + m.cross_crossings, + m.inside, + m.max_depth, + m.min_cell_ratio, + m.inverted_cells + ); + worst.cross_crossings = worst.cross_crossings.max(m.cross_crossings); + worst.inside = worst.inside.max(m.inside); + worst.max_depth = worst.max_depth.max(m.max_depth); + worst.min_cell_ratio = worst.min_cell_ratio.min(m.min_cell_ratio); + } + if step == steps { + break; + } + hook(world, step); + let start = std::time::Instant::now(); + world.step(); + stepping += start.elapsed(); + } + let ms = stepping.as_secs_f64() * 1000.0 / steps as f64; + let last = measure(world, handles); + println!( + "{name} [{preset}] final: self={} cross={} inside={} depth={:.3} min_cell={:+.3} \ + inverted={} ({ms:.3} ms/step); worst: cross={} inside={} depth={:.3} min_cell={:+.3}", + last.self_crossings, + last.cross_crossings, + last.inside, + last.max_depth, + last.min_cell_ratio, + last.inverted_cells, + worst.cross_crossings, + worst.inside, + worst.max_depth, + worst.min_cell_ratio + ); + last +} + +/// A column of six balloons with a heavy box landing on top once it has settled. +#[test] +fn balloon_column() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 4.0); + let handles: Vec<_> = (0..6) + .map(|i| { + let x = (i % 2) as Real * 0.05; + world.insert_soft_body(balloon(Vector::new(x, 0.55 + i as Real * 1.05, 0.0), 0.5)) + }) + .collect(); + let m = run( + "balloon_column", + &mut world, + &handles, + 900, + 100, + |world, step| { + if step == 300 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 8.0, 0.0)), + ColliderBuilder::cuboid(0.5, 0.3, 0.5).density(10.0), + ); + } + }, + ); + assert_eq!( + m.cross_crossings, 0, + "balloons of the column penetrate each other" + ); + assert_eq!( + m.self_crossings, 0, + "a balloon of the column is self-crossed" + ); +} + +/// A column of five jelly cubes under the same weight: cells compress, none may invert. +#[test] +fn jelly_column() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 4.0); + let handles: Vec<_> = (0..5) + .map(|i| { + let x = (i % 2) as Real * 0.05; + world.insert_soft_body(jelly(Vector::new(x, 0.55 + i as Real * 1.1, 0.0), 0.5)) + }) + .collect(); + let m = run( + "jelly_column", + &mut world, + &handles, + 900, + 100, + |world, step| { + if step == 300 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 7.0, 0.0)), + ColliderBuilder::cuboid(0.5, 0.3, 0.5).density(10.0), + ); + } + }, + ); + assert_eq!( + m.cross_crossings, 0, + "cubes of the column penetrate each other" + ); + assert_eq!( + m.inverted_cells, 0, + "a cube of the column has inverted cells" + ); +} + +/// Two layers of jelly cubes in a bin, pressed by a kinematic plate to four fifths of +/// their height and held there. +#[test] +fn pressed_jelly_pile() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 3.0); + for (x, z) in [(-2.6, 0.0), (2.6, 0.0), (0.0, -2.6), (0.0, 2.6)] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 3.0, z)), + ColliderBuilder::cuboid( + if z == 0.0 { 0.3 } else { 2.9 }, + 3.5, + if z == 0.0 { 2.9 } else { 0.3 }, + ), + ); + } + let mut handles = Vec::new(); + for j in 0..2 { + for i in 0..2 { + for k in 0..2 { + let x = -0.6 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let z = -0.6 + k as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.55 + j as Real * 1.1; + handles.push(world.insert_soft_body(jelly(Vector::new(x, y, z), 0.5))); + } + } + } + let plate = world.insert_body( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 3.0, 0.0)), + ); + world.insert_collider(ColliderBuilder::cuboid(2.2, 0.25, 2.2), Some(plate)); + let m = run( + "pressed_jelly_pile", + &mut world, + &handles, + 1200, + 150, + |world, step| { + if step >= 300 { + let y = (3.0 - (step - 300) as Real * 0.5 / 60.0).max(1.95); + world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y, 0.0)); + } + }, + ); + assert_eq!( + m.cross_crossings, 0, + "cubes of the pressed pile penetrate each other" + ); + assert_eq!( + m.inverted_cells, 0, + "a cube of the pressed pile has inverted cells" + ); +} + +/// The 3D trigger scene: the same pile crushed to two thirds of its height. Ignored until a +/// phase passes it; see the 2D `crushed_jelly_pile`. +#[test] +#[ignore] +fn crushed_jelly_pile() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 3.0); + for (x, z) in [(-2.6, 0.0), (2.6, 0.0), (0.0, -2.6), (0.0, 2.6)] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 3.0, z)), + ColliderBuilder::cuboid( + if z == 0.0 { 0.3 } else { 2.9 }, + 3.5, + if z == 0.0 { 2.9 } else { 0.3 }, + ), + ); + } + let mut handles = Vec::new(); + for j in 0..2 { + for i in 0..2 { + for k in 0..2 { + let x = -0.6 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let z = -0.6 + k as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.55 + j as Real * 1.1; + handles.push(world.insert_soft_body(jelly(Vector::new(x, y, z), 0.5))); + } + } + } + let plate = world.insert_body( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 3.0, 0.0)), + ); + world.insert_collider(ColliderBuilder::cuboid(2.2, 0.25, 2.2), Some(plate)); + let m = run( + "crushed_jelly_pile", + &mut world, + &handles, + 1200, + 150, + |world, step| { + if step >= 300 { + let y = (3.0 - (step - 300) as Real * 0.5 / 60.0).max(1.6); + world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y, 0.0)); + } + }, + ); + assert_eq!( + m.cross_crossings, 0, + "cubes of the crushed pile penetrate each other" + ); + assert_eq!( + m.self_crossings, 0, + "a cube of the crushed pile is self-crossed" + ); + assert_eq!( + m.inverted_cells, 0, + "a cube of the crushed pile has inverted cells" + ); +} diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 6a89a92b8..2accc00c1 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -42,6 +42,7 @@ mod platform2; mod polyline2; mod pyramid2; mod restitution2; +mod debug_self_intersect2; mod rope_joints2; mod s2d_arch; mod s2d_ball_and_chain; @@ -159,6 +160,7 @@ pub async fn main() { DEBUG, "Many colliders", debug_many_colliders2::run; DEBUG, "Total overlap", debug_total_overlap2::run; DEBUG, "Vertical column", debug_vertical_column2::run; + DEBUG, "Self intersect", debug_self_intersect2::run; // ── Inspired by Solver2D ──────────────────────────────────────────── S2D, "High mass ratio 1", s2d_high_mass_ratio_1::run; S2D, "High mass ratio 2", s2d_high_mass_ratio_2::run; diff --git a/examples2d/debug_self_intersect2.rs b/examples2d/debug_self_intersect2.rs new file mode 100644 index 000000000..4b43cb4a5 --- /dev/null +++ b/examples2d/debug_self_intersect2.rs @@ -0,0 +1,219 @@ +//! The self-intersection recovery scenarios of `crates/rapier2d/tests/self_intersect_probe.rs` +//! side by side (strip tangles, a fast blob stopped by the crossing guard, the 8-shaped blob +//! family): without recovery the self-contacts hold each tangle; the stand-down pulls them out. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * A captured strip: ends pinned, middle-top particle parked below the bottom boundary. + */ + let strip = |center: Vector| { + SoftBodyBuilder::grid(center, Vector::new(3.0, 0.15), 3, 2) + .pinned_particles([0, 1, 4, 5]) + .softness(SpringCoefficients::new(3.0, 1.0)) + .self_contacts(true) + }; + let captured = world.insert_soft_body(strip(Vector::new(-16.0, 0.0))); + world.soft_bodies[captured].set_particle_position(3, Vector::new(-15.0, -0.4)); + + /* + * The same capture loaded against its recovery by a heavy resting ball. + */ + let loaded = world.insert_soft_body(strip(Vector::new(-6.0, 0.0))); + world.soft_bodies[loaded].set_particle_position(3, Vector::new(-5.0, -0.4)); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(-5.7, 0.8)), + ColliderBuilder::ball(0.45).density(20.0), + ); + + /* + * A cell-less blob (edges + boundary volume) with its top vertex below the bottom wall, over + * a floor; too soft to escape by tunneling, only the self-crossing stand-down frees it. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(2.0, -1.0)), + ColliderBuilder::cuboid(2.5, 0.5), + ); + let blob = world.insert_soft_body( + SoftBodyBuilder::disk(Vector::new(2.0, 0.0), 0.5, 16) + .softness(SpringCoefficients::new(4.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05), + ); + // Vertex 4 sits at the top of the disk (angle 90°); park it below the bottom wall. + world.soft_bodies[blob].set_particle_position(4, Vector::new(2.0, -0.8)); + + /* + * A strip whose top boundary is grabbed (like the testbed mouse) and ground into the interior + * past the middle row: the cells invert while held, and the boundary must never cross itself. + */ + let grab_origin = Vector::new(12.0, 0.0); + world.insert( + RigidBodyBuilder::fixed().translation(grab_origin + Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(2.5, 0.5), + ); + let bottom_row = (0..7).map(|i| i * 3); + let ground_strip = world.insert_soft_body( + SoftBodyBuilder::grid(grab_origin + Vector::new(0.0, 0.15), Vector::new(1.5, 0.15), 7, 3) + .pinned_particles(bottom_row) + .softness(SpringCoefficients::new(5.0, 1.0)) + .self_contacts(true), + ); + let grabbed = 11u32; + let anchor = world.soft_bodies[ground_strip].particle_position(grabbed as usize); + let cluster = world.add_soft_body_cluster(ground_strip, &[grabbed]).unwrap(); + let proxy = world.soft_bodies[ground_strip].cluster_proxy(cluster).unwrap(); + let mouse = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); + let joint: GenericJoint = GenericJointBuilder::new(JointAxesMask::empty()) + .motor_position(JointAxis::LinX, 0.0, 1000.0, 50.0) + .motor_position(JointAxis::LinY, 0.0, 1000.0, 50.0) + .into(); + world.insert_impulse_joint(mouse, proxy, joint); + + /* + * The crossing-guard cannon: a blob shot at a thin pinned strip faster than the contact + * reach, re-teleported and re-shot every few seconds. + */ + let cannon_origin = Vector::new(20.5, 0.0); + let cannon_strip = SoftBodyBuilder::grid(cannon_origin, Vector::new(2.0, 0.1), 9, 2) + .pinned_particles([0, 1, 16, 17]) + .softness(SpringCoefficients::new(10.0, 1.0)) + .self_contacts(true); + world.insert_soft_body(cannon_strip); + let bullet = world.insert_soft_body( + SoftBodyBuilder::disk(cannon_origin + Vector::new(0.0, 2.5), 0.3, 12) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.2) + .self_contacts(true), + ); + let reload = |world: &mut PhysicsWorld| { + let n = world.soft_bodies[bullet].particles().len(); + for i in 0..n { + let angle = core::f32::consts::TAU as Real * i as Real / n as Real; + let p = cannon_origin + + Vector::new(0.0, 2.5) + + Vector::new(angle.cos(), angle.sin()) * 0.3; + world.soft_bodies[bullet].set_particle_position(i, p); + world.soft_bodies[bullet].set_particle_velocity(i, Vector::new(0.0, -150.0)); + } + }; + reload(&mut world); + + /* + * The 8-shaped blob family (bottom row): a disk blob reshaped into a figure-eight, one lobe + * keeping the rest winding, the other wound backward. + */ + let eight_blob = |world: &mut PhysicsWorld, center: Vector, radius: Real, n: usize| { + world.insert_soft_body( + SoftBodyBuilder::disk(center, radius, n) + .softness(SpringCoefficients::new(4.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05), + ) + }; + let gerono_eight = |world: &mut PhysicsWorld, h: SoftBodyHandle, center: Vector, r: Real| { + let n = world.soft_bodies[h].particles().len(); + for i in 0..n { + let t = core::f32::consts::TAU as Real * i as Real / n as Real; + let p = center + Vector::new(r * t.cos(), r * t.sin() * t.cos()); + world.soft_bodies[h].set_particle_position(i, p); + } + }; + let asym_eight = + |world: &mut PhysicsWorld, h: SoftBodyHandle, center: Vector, rb: Real, rs: Real| { + let n = world.soft_bodies[h].particles().len(); + let n_big = (n as Real * rb / (rb + rs)) as usize; + let tau = core::f32::consts::TAU as Real; + for i in 0..n { + let p = if i < n_big { + let t = tau * i as Real / n_big as Real; + center + Vector::new(-rb + rb * t.cos(), rb * t.sin()) + } else { + let t = tau * (i - n_big) as Real / (n - n_big) as Real; + center + Vector::new(rs - rs * t.cos(), -rs * t.sin()) + }; + world.soft_bodies[h].set_particle_position(i, p); + } + }; + // Wide enough for a de-overlapping pair to drift on (a 2.0 half-width let the upper + // 8 of the penetrating pair slide off the edge). + let eight_floor = |world: &mut PhysicsWorld, x: Real| { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, -6.5)), + ColliderBuilder::cuboid(3.0, 0.5), + ); + }; + + // An asymmetric 8 (small mirrored lobe): recovers to an O. + eight_floor(&mut world, -14.0); + let asym = eight_blob(&mut world, Vector::new(-14.0, -5.35), 0.6, 24); + asym_eight(&mut world, asym, Vector::new(-13.9, -5.35), 0.55, 0.3); + + // A balanced 8: a second stable elastic equilibrium, resolved by the active untangler. + eight_floor(&mut world, -7.0); + let sym = eight_blob(&mut world, Vector::new(-7.0, -5.35), 0.6, 24); + gerono_eight(&mut world, sym, Vector::new(-7.0, -5.35), 0.6); + + // Two 8s penetrating each other: unknots and de-overlaps. + eight_floor(&mut world, 1.0); + let e1 = eight_blob(&mut world, Vector::new(0.65, -5.35), 0.6, 24); + gerono_eight(&mut world, e1, Vector::new(0.65, -5.35), 0.6); + let e2 = eight_blob(&mut world, Vector::new(1.35, -5.05), 0.6, 24); + gerono_eight(&mut world, e2, Vector::new(1.35, -5.05), 0.6); + + // An 8 whose mirrored lobe starts swallowed inside a bigger blob: recovers fully. + eight_floor(&mut world, 8.0); + let swallowed = eight_blob(&mut world, Vector::new(8.0, -5.35), 0.6, 24); + asym_eight(&mut world, swallowed, Vector::new(7.8, -5.35), 0.55, 0.3); + eight_blob(&mut world, Vector::new(8.65, -5.35), 0.55, 20); + + // An asymmetric 8 mutually penetrating a plain blob: unknots and de-overlaps. + eight_floor(&mut world, 15.0); + let overlapped = eight_blob(&mut world, Vector::new(15.0, -5.35), 0.6, 24); + asym_eight(&mut world, overlapped, Vector::new(15.1, -5.35), 0.55, 0.3); + eight_blob(&mut world, Vector::new(14.15, -5.35), 0.5, 20); + + // A small blob threaded through the 8's mirrored lobe: unknots and de-overlaps. + eight_floor(&mut world, 22.0); + let threaded = eight_blob(&mut world, Vector::new(22.0, -5.35), 0.6, 24); + asym_eight(&mut world, threaded, Vector::new(21.8, -5.35), 0.55, 0.3); + eight_blob(&mut world, Vector::new(22.1, -5.35), 0.22, 14); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, -2.5), 38.0); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + // The grab loops: pull the vertex down just above the bottom boundary, grind it + // around while held, lift it back out, rest, repeat. + let cycle = t % 6.0; + let target = if cycle < 1.0 { + Vector::new(0.0, 0.5 - 0.48 * cycle) + } else if cycle < 4.0 { + let w = core::f32::consts::TAU as Real * (cycle - 1.0); + Vector::new(0.3 * w.sin(), 0.02 + 0.03 * (1.0 - (2.0 * w).cos())) + } else if cycle < 5.0 { + Vector::new(0.0, 0.02 + 0.48 * (cycle - 4.0)) + } else { + Vector::new(0.0, 0.5) + }; + world.bodies[mouse].set_next_kinematic_translation(grab_origin + target); + world.bodies[proxy].wake_up(true); + // Re-shoot the cannon blob every few seconds (a teleport resets the crossing + // guard's trajectory baseline, so only the flight itself is guarded). + let dt = world.integration_parameters.dt; + if (t % 4.0) < dt && t > 0.0 { + reload(&mut world); + } + world.step(); + t += dt; + } + } + Ok(()) +} diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 309fc59ac..2d4a6bd55 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -46,6 +46,7 @@ mod debug_long_chain3; mod debug_multi_collider_body3; mod debug_multibody_ang_motor_pos3; mod debug_pop3; +mod debug_self_intersect3; mod debug_prismatic3; mod debug_rollback3; mod debug_shape_modification3; @@ -206,6 +207,7 @@ pub async fn main() { DEBUG, "Dyn. collider add", debug_dynamic_collider_add3::run; DEBUG, "Friction", debug_friction3::run; DEBUG, "Internal edges", debug_internal_edges3::run; + DEBUG, "Self intersect", debug_self_intersect3::run; DEBUG, "Long chain", debug_long_chain3::run; DEBUG, "High mass ratio: chain", debug_chain_high_mass_ratio3::run; DEBUG, "High mass ratio: cube", debug_cube_high_mass_ratio3::run; diff --git a/examples3d/debug_self_intersect3.rs b/examples3d/debug_self_intersect3.rs new file mode 100644 index 000000000..b8c003270 --- /dev/null +++ b/examples3d/debug_self_intersect3.rs @@ -0,0 +1,225 @@ +//! The 3D self-intersection recovery scenarios of `crates/rapier3d/tests/self_intersect_probe3.rs` +//! (a pinned slab and a folded cloth with a vertex parked below the bottom face or layer) and the +//! re-firing crossing-guard scenes of `crates/rapier3d/tests/crossing_guard3.rs`, side by side. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * A ground plane for orientation (the tangles hang above it). + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -1.5, 0.0)), + ColliderBuilder::cuboid(8.0, 0.5, 4.0), + ); + + /* + * A volumetric slab pinned at both end faces, with its top-middle vertex parked below + * the bottom face. + */ + let slab_origin = Vector::new(-3.0, 0.0, 0.0); + let builder = SoftBodyBuilder::cuboid( + slab_origin + Vector::new(0.0, 0.15, 0.0), + Vector::new(1.5, 0.15, 0.15), + 3, + 2, + 2, + ) + .softness(SpringCoefficients::new(5.0, 1.0)); + let pinned: Vec = builder + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| (p.x - slab_origin.x).abs() > 1.4) + .map(|(i, _)| i as u32) + .collect(); + let target = slab_origin + Vector::new(0.0, 0.3, 0.15); + let captured = builder + .particle_positions() + .iter() + .enumerate() + .min_by(|(_, a), (_, b)| { + (**a - target) + .length() + .partial_cmp(&(**b - target).length()) + .unwrap() + }) + .unwrap() + .0; + let slab = + world.insert_soft_body(builder.pinned_particles(pinned).self_contacts(true)); + world.soft_bodies[slab] + .set_particle_position(captured, slab_origin + Vector::new(0.3, -0.4, 0.0)); + + /* + * A cloth strip folded into two layers (cell-less), both end columns pinned, with one + * top-layer vertex parked below the bottom layer so its triangles pierce it. + */ + let cloth_origin = Vector::new(1.5, 0.0, -0.25); + let (nu, nv) = (8usize, 3usize); + let cloth = world.insert_soft_body( + SoftBodyBuilder::cloth( + cloth_origin, + Vector::new(0.25, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.25), + nu, + nv, + ) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.02) + .particle_radius(0.05) + // Both ends of the strip pinned: the outer edge of the bottom layer at its rest + // place, the free edge of the folded-back top layer above it (positions set below). + .pinned_particles((0..nv as u32).chain((nu as u32 - 1) * nv as u32..(nu * nv) as u32)) + .self_contacts(true), + ); + // Fold columns 4..8 back over the first four, one gap above them. + let idx = |i: usize, j: usize| i * nv + j; + for i in 4..nu { + for j in 0..nv { + let x = 0.25 * (7 - i) as Real; + world.soft_bodies[cloth].set_particle_position( + idx(i, j), + cloth_origin + Vector::new(x, 0.15, 0.25 * j as Real), + ); + } + } + // Park a middle vertex of the top layer below the bottom layer. + world.soft_bodies[cloth] + .set_particle_position(idx(5, 1), cloth_origin + Vector::new(0.5, -0.15, 0.25)); + + /* + * Guard scene: a folded cloth (clean start) whose top-layer middle vertex is flicked + * straight down through the bottom layer. + */ + let flick_origin = Vector::new(-6.5, 0.0, -0.25); + let flick = world.insert_soft_body( + SoftBodyBuilder::cloth( + flick_origin, + Vector::new(0.25, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.25), + nu, + nv, + ) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.02) + .particle_radius(0.05) + .pinned_particles((0..nv as u32).chain((nu as u32 - 1) * nv as u32..(nu * nv) as u32)) + .self_contacts(true), + ); + for i in 4..nu { + for j in 0..nv { + let x = 0.25 * (7 - i) as Real; + world.soft_bodies[flick].set_particle_position( + idx(i, j), + flick_origin + Vector::new(x, 0.2, 0.25 * j as Real), + ); + } + } + let flick_rest: Vec = world.soft_bodies[flick].particle_positions().collect(); + + /* + * Guard scene: a soft ball shot straight down at a pinned cloth faster than the + * contact reach. + */ + let cannon_origin = Vector::new(5.0, 1.2, 0.0); + let n = 12; + let extent = 0.12 * (n - 1) as Real / 2.0; + let pinned_edge = |k: usize| { + let (i, j) = (k / n, k % n); + i == 0 || j == 0 || i == n - 1 || j == n - 1 + }; + world.insert_soft_body( + SoftBodyBuilder::cloth( + cannon_origin + Vector::new(-extent, 0.0, -extent), + Vector::new(0.12, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.12), + n, + n, + ) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.02) + .particle_radius(0.05) + .pinned_particles((0..n * n).filter(|&k| pinned_edge(k)).map(|k| k as u32)) + .self_contacts(true), + ); + let bullet = world.insert_soft_body( + SoftBodyBuilder::sphere(cannon_origin + Vector::new(0.0, 1.4, 0.0), 0.22, 1) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.05), + ); + let bullet_rest: Vec = world.soft_bodies[bullet].particle_positions().collect(); + + /* + * Guard scene: two perpendicular strips, the top one shot down across the bottom one + * (the first touch is edge on edge). + */ + let strips_origin = Vector::new(8.5, 0.0, 0.0); + let strip = |origin: Vector, du: Vector, dv: Vector| { + SoftBodyBuilder::cloth(origin, du, dv, 12, 2) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.02) + .particle_radius(0.05) + .self_contacts(true) + }; + world.insert_soft_body( + strip( + strips_origin + Vector::new(-1.1, 0.3, -0.1), + Vector::new(0.2, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.2), + ) + .pinned_particles([0, 1, 22, 23]), + ); + let top_strip = world.insert_soft_body(strip( + strips_origin + Vector::new(-0.1, 1.3, -1.1), + Vector::new(0.0, 0.0, 0.2), + Vector::new(0.2, 0.0, 0.0), + )); + let top_rest: Vec = world.soft_bodies[top_strip].particle_positions().collect(); + + // Teleports every re-fire reset the guard's trajectory baseline by design. + let reset = |world: &mut PhysicsWorld, + h: SoftBodyHandle, + rest: &[Vector], + vel: &dyn Fn(usize) -> Vector| { + for (i, p) in rest.iter().enumerate() { + world.soft_bodies[h].set_particle_position(i, *p); + world.soft_bodies[h].set_particle_velocity(i, vel(i)); + } + }; + let refire = |world: &mut PhysicsWorld| { + reset(world, flick, &flick_rest, &|i| { + if i == idx(5, 1) { + Vector::new(0.0, -40.0, 0.0) + } else { + Vector::ZERO + } + }); + reset(world, bullet, &bullet_rest, &|_| Vector::new(0.0, -60.0, 0.0)); + reset(world, top_strip, &top_rest, &|_| Vector::new(0.0, -30.0, 0.0)); + }; + refire(&mut world); + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(1.0, 3.5, 11.0), Vec3::new(1.0, 0.5, 0.0)); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + let dt = world.integration_parameters.dt; + t += dt; + if (t % 4.0) < dt && t > 0.0 { + refire(&mut world); + } + world.step(); + } + } + Ok(()) +} diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 683138b10..7133bbbe3 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -244,6 +244,13 @@ pub struct IntegrationParameters { /// with your chosen units. pub length_unit: Real, + /// Settings shared by every soft body: tangle recovery, strained-constraint re-sweep, impact + /// substeps, contact stiffening and FEM solver tuning (see [`SoftBodiesSettings`]). + /// + /// [`SoftBodiesSettings`]: crate::dynamics::SoftBodiesSettings + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub soft_bodies: crate::dynamics::SoftBodiesSettings, + /// Geometric slop distance (default: `0.005`), e.g. the standoff kept /// by the CCD clamp. NOT a deadzone on the position-correction bias: penetrations are corrected /// all the way to zero; a deadzone would keep loaded piles wedging and creeping. @@ -414,6 +421,7 @@ impl Default for IntegrationParameters { friction_in_bias_pass: false, warmstart_joints: false, length_unit: 1.0, + soft_bodies: Default::default(), #[cfg(feature = "dim3")] friction_model: FrictionModel::default(), } diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh.rs index de0c611c3..f69ec6406 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh.rs @@ -128,6 +128,14 @@ pub struct SoftCollisionMesh { /// Vertex-vs-surface contacts of the last step against this mesh (self contacts and the /// other soft bodies' vertices): warm-start state. pub(crate) vertex_contacts: Vec, + /// Surface travel accumulated since the last self-crossing sweep (the sweep is skipped + /// while this cannot have bridged half a skin). + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) crossing_sweep_travel: Real, + /// The surface colliders whose pair with this mesh crosses this step (its own collider + /// for a self tangle, a partner it is tangled with): the pairs the recovery owns. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) crossed_partners: Vec, /// Bumped whenever this mesh's elements change (a tear): renderers keying their mesh on it /// rebuild it on change. #[cfg_attr(feature = "serde-serialize", serde(default))] diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs index e87502c99..5deabbf1a 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs @@ -195,6 +195,7 @@ impl SoftCollisionMesh { /// The particles a contact at the `i`-th vertex acts through, and their weights: the /// vertex's own particle for a direct mesh, the particles of the cell holding it for a /// skinned one. + pub fn vertex_anchors( &self, body: &SoftBody, i: usize, @@ -282,4 +283,24 @@ impl SoftCollisionMesh { Some((cell.vertices, binding.weights, self.vertices[vertex])) } + /// The cell id backing each element, `u32::MAX` for none (empty when the elements are not + /// cell-backed at all: wires, skinned meshes). + pub(crate) fn element_cell_ids<'a>(&'a self, body: &'a SoftBody) -> &'a [u32] { + if self.follows_boundary { + &body.boundary_element_cells + } else { + &self.element_cells + } + } + + /// The surface elements incident to `vertex`. + pub(crate) fn vertex_element_ids(&self, vertex: u32) -> &[u32] { + match ( + self.vertex_elements_offsets.get(vertex as usize), + self.vertex_elements_offsets.get(vertex as usize + 1), + ) { + (Some(&start), Some(&end)) => &self.vertex_elements[start as usize..end as usize], + _ => &[], + } + } } diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs index 96266b2f2..f9ef14e6c 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs @@ -1 +1,218 @@ - /// carries a spurious normal. +//! Geometric queries on a collision mesh: inside tests, self-contact exclusion, ghost contacts, +//! element inversion and outward normals. + +use crate::math::{DIM, Real, Vector}; +use parry::query::PointQuery; +use parry::shape::Segment; +#[cfg(feature = "dim3")] +use parry::shape::Triangle; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::{SoftBody, SoftCollisionMesh}; + +impl SoftCollisionMesh { + /// Whether the point lies inside this closed surface, by ray-crossing parity: a brute + /// scan over the elements (callers use it rarely, on a handful of vertices), robust to + /// inverted windings since parity ignores orientation. + pub(crate) fn contains_point_parity(&self, body: &SoftBody, p: Vector) -> bool { + #[cfg(feature = "dim2")] + let dir = Vector::new(0.9063, 0.4226); + #[cfg(feature = "dim3")] + let dir = Vector::new(0.5341, 0.6432, 0.5487); + let mut reach: Real = 1.0; + for v in 0..self.vertex_count() { + let x = self.vertex(body, v); + if x.is_finite() { + reach = reach.max((x - p).length()); + } + } + let q = p + dir * (2.0 * reach + 1.0); + let mut crossings = 0usize; + for e in 0..self.indices().len() { + let ids = self.element(e); + #[cfg(feature = "dim2")] + { + let (a, b) = ( + self.vertex(body, ids[0] as usize), + self.vertex(body, ids[1] as usize), + ); + if a.is_finite() + && b.is_finite() + && crate::dynamics::soft_body::soft_body_crossing_tests::segments_cross([p, q], [a, b]) + { + crossings += 1; + } + } + #[cfg(feature = "dim3")] + { + let (a, b, c) = ( + self.vertex(body, ids[0] as usize), + self.vertex(body, ids[1] as usize), + self.vertex(body, ids[2] as usize), + ); + if a.is_finite() + && b.is_finite() + && c.is_finite() + && crate::dynamics::soft_body::soft_body_crossing_tests::segment_crosses_triangle(p, q, a, b, c) + { + crossings += 1; + } + } + } + crossings % 2 == 1 + } + + /// Whether a self contact between `vertex` and `element` is ignored: the element holds the + /// vertex or a surface neighbor, or (FEM bodies) sits within the skins' clearance of it at + /// rest, except across a crack (see `across_crack`) where the material between is gone. + pub(crate) fn self_contact_excluded( + &self, + body: &SoftBody, + vertex: u32, + element: &[u32], + skin: Real, + ) -> bool { + if element.contains(&vertex) { + return true; + } + } + /// Whether a contact on `element_id` at barycentric `weights`, with the other shape's point at + /// `point`, is a ghost of an internal vertex/edge: the point projects inside a neighboring + /// element, which reports the real contact, so this one only brings a spurious normal. + pub(crate) fn contact_is_ghost( + &self, + body: &SoftBody, + element_id: usize, + weights: &[Real; DIM], + point: Vector, + ) -> bool { + const EPS: Real = 1.0e-3; + let element = self.element(element_id); + let mut touched = [u32::MAX; DIM]; + let mut num_touched = 0; + for (k, v) in element.iter().enumerate() { + if weights[k] > EPS { + touched[num_touched] = *v; + num_touched += 1; + } + } + if num_touched == element.len() { + // Interior contact. + return false; + } + let touched = &touched[..num_touched]; + let pos = |v: u32| self.vertex(body, v as usize); + // Candidates: the elements incident to every touched vertex, other than this one. + let Some(&first) = touched.first() else { + return false; + }; + let (start, end) = match ( + self.vertex_elements_offsets.get(first as usize), + self.vertex_elements_offsets.get(first as usize + 1), + ) { + (Some(&s), Some(&e)) => (s as usize, e as usize), + _ => return false, + }; + for &candidate in &self.vertex_elements[start..end] { + if candidate as usize == element_id { + continue; + } + let other = self.element(candidate as usize); + if !touched.iter().all(|v| other.contains(v)) { + continue; + } + // A segment neighbor is tested as a segment, whatever the dimension. + if other.len() < DIM { + let (a, b) = ( + self.vertex(body, other[0] as usize), + self.vertex(body, other[1] as usize), + ); + let ab = b - a; + let t = (point - a).dot(ab) / ab.length_squared().max(1.0e-12); + if t > EPS && t < 1.0 - EPS { + return true; + } + continue; + } + #[cfg(feature = "dim2")] + { + let (a, b) = (pos(other[0]), pos(other[1])); + let ab = b - a; + let t = (point - a).dot(ab) / ab.length_squared().max(1.0e-12); + if t > EPS && t < 1.0 - EPS { + return true; + } + } + #[cfg(feature = "dim3")] + { + let (a, b, c) = (pos(other[0]), pos(other[1]), pos(other[2])); + let n = (b - a).cross(c - a); + let nn = n.length_squared().max(1.0e-12); + let inside = |p: Vector, q: Vector| n.dot((q - p).cross(point - p)) / nn; + let (u, v, w) = (inside(a, b), inside(b, c), inside(c, a)); + if u > EPS && v > EPS && w > EPS { + return true; + } + } + } + false + } + + /// Whether the cell owning `element_id` is currently inverted (negative volume relative to + /// rest): its material is locally inside out, so contacts read from its geometry have a + /// meaningless side. `false` when no cell backs the element (wires, skinned meshes). + pub(crate) fn element_cell_inverted(&self, body: &SoftBody, element_id: usize) -> bool { + if let Some(&cell) = self.element_cell_ids(body).get(element_id) { + if let Some(cell) = body.cells.get(cell as usize) { + let x: [Vector; DIM + 1] = + core::array::from_fn(|k| body.particles[cell.vertices[k] as usize].position); + return SoftBody::cell_volume(x) * cell.rest_volume <= 0.0; + } + } + false + } + + /// Outward normal (not normalized) of an element from the cached vertex positions, only + /// meaningful for a closed mesh (`None` otherwise, and for an element whose cell is + /// currently inverted: its winding is mirrored, so it cannot orient its contacts). + pub(crate) fn element_outward_normal( + &self, + body: &SoftBody, + element_id: usize, + ) -> Option { + if !self.closed { + return None; + } + // A likely self-crossed loop (an O squeezed into an 8) has part of its winding mirrored; + // cells still orient each element, but without them contacts stand down to two-sided (a + // one-sided "expel" read from a mirrored winding drags bodies through the surface). + if self.orientation_unreliable && (body.cells.is_empty() || self.is_skinned()) { + return None; + } + let element = self.element(element_id); + // A skinned mesh has no cell of its own to check for inversion; the cells' boundary does. + if self.element_cell_inverted(body, element_id) { + return None; + } + // Only a closed mesh gets here, and a wire is never closed: the element is a facet. + debug_assert_eq!(element.len(), DIM); + let x: [Vector; DIM] = core::array::from_fn(|k| self.vertex(body, element[k] as usize)); + #[cfg(feature = "dim2")] + let n = { + let d = x[1] - x[0]; + Vector::new(d.y, -d.x) + }; + #[cfg(feature = "dim3")] + let n = (x[1] - x[0]).cross(x[2] - x[0]); + // A negative rest volume means the mesh is oriented inward; a mesh turned inside out is + // mirrored as a whole. + Some(if (self.rest_signed_volume < 0.0) != self.inverted { + -n + } else { + n + }) + } + +} diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs index 36e8aecfd..4ed76d1a3 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs @@ -62,6 +62,9 @@ impl SoftCollisionMesh { self_contacts, edge_contacts: Vec::new(), vertex_contacts: Vec::new(), + crossing_sweep_travel: Real::MAX, + crossed_partners: Vec::new(), + vertex_cache: Vec::new(), topology_version: 0, } } @@ -124,6 +127,9 @@ impl SoftCollisionMesh { self_contacts, edge_contacts: Vec::new(), vertex_contacts: Vec::new(), + crossing_sweep_travel: Real::MAX, + crossed_partners: Vec::new(), + vertex_cache: Vec::new(), topology_version: 0, } } @@ -199,6 +205,9 @@ impl SoftCollisionMesh { self_contacts, edge_contacts: Vec::new(), vertex_contacts: Vec::new(), + crossing_sweep_travel: Real::MAX, + crossed_partners: Vec::new(), + vertex_cache: Vec::new(), topology_version: 0, }) } @@ -371,5 +380,7 @@ impl SoftCollisionMesh { pub(crate) fn clear_contacts(&mut self) { self.edge_contacts.clear(); self.vertex_contacts.clear(); + self.crossing_sweep_travel = Real::MAX; + self.crossed_partners.clear(); } } diff --git a/src/dynamics/soft_body/mod.rs b/src/dynamics/soft_body/mod.rs index 2f9ac337a..6fee5b4e0 100644 --- a/src/dynamics/soft_body/mod.rs +++ b/src/dynamics/soft_body/mod.rs @@ -24,11 +24,17 @@ pub use self::soft_body_plasticity::SoftEdgePlasticFlow; pub(crate) use self::soft_body::SOFT_BODY_OVERFLOW_COLOR; pub(crate) use self::soft_body_contacts::{SoftEdgeContact, SoftVertexContact}; - #[allow(unused_imports)] use self::soft_body_elements::{CELL_IMPULSES}; #[allow(unused_imports)] use self::soft_body_material::{default_true, one}; +mod soft_recovery_settings; +pub use soft_recovery_settings::{SoftPatchConstraints, SoftRecoverySettings}; +mod soft_body_settings; +#[cfg(feature = "fem")] +pub use soft_body_settings::SoftFemParameters; +pub use soft_body_settings::SoftBodiesSettings; + pub(crate) mod collision_mesh; mod soft_body; mod soft_body_accessors; @@ -36,6 +42,7 @@ mod soft_body_builder; mod soft_body_cluster; mod soft_body_coloring; mod soft_body_contacts; +pub(crate) mod soft_body_crossing_tests; mod soft_body_elements; mod soft_body_geometry; mod soft_body_handle; diff --git a/src/dynamics/soft_body/soft_body_cluster.rs b/src/dynamics/soft_body/soft_body_cluster.rs index 11998b004..8b1b9a73f 100644 --- a/src/dynamics/soft_body/soft_body_cluster.rs +++ b/src/dynamics/soft_body/soft_body_cluster.rs @@ -87,7 +87,12 @@ impl SoftBodyCluster { self.proxy } - /// The collision meshes this cluster carries (dead slots skipped). + /// The collision meshes this cluster owns mutably (dead slots skipped). + pub(crate) fn meshes_mut(&mut self) -> impl Iterator { + self.meshes.iter_mut().flatten() + } + + /// The collision meshes this cluster owns (dead slots skipped). pub fn meshes(&self) -> impl Iterator { self.meshes.iter().flatten() } diff --git a/src/dynamics/soft_body/soft_body_crossing_tests.rs b/src/dynamics/soft_body/soft_body_crossing_tests.rs new file mode 100644 index 000000000..2fa8ff295 --- /dev/null +++ b/src/dynamics/soft_body/soft_body_crossing_tests.rs @@ -0,0 +1,67 @@ +//! Exact crossing predicates shared by the tangle detection and the volume constraints. + +use crate::math::Vector; +#[cfg(feature = "dim3")] +use crate::utils::DotProduct; + +/// Whether two segments intersect (sign tests; parallel or degenerate pairs never cross). +#[cfg(feature = "dim2")] +pub(crate) fn segments_cross(a: [Vector; 2], b: [Vector; 2]) -> bool { + let (d1, d2) = (a[1] - a[0], b[1] - b[0]); + let denom = d1.perp_dot(d2); + if denom.abs() < 1.0e-12 { + return false; + } + let t = (b[0] - a[0]).perp_dot(d2) / denom; + let u = (b[0] - a[0]).perp_dot(d1) / denom; + (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) +} + +/// Whether two triangles cross: any edge of one pierces the other. For a self pair, an +/// edge touching the other triangle's vertices is skipped (the fan around a shared vertex +/// is contact, not crossing), matching the recovery's self-crossing predicate. +#[cfg(feature = "dim3")] +pub(crate) fn tri_pair_crosses( + ids_a: &[u32], + pa: &[Vector; 3], + ids_b: &[u32], + qb: &[Vector; 3], + is_self: bool, +) -> bool { + let edge_hits = |ids_e: &[u32], pe: &[Vector; 3], ids_t: &[u32], pt: &[Vector; 3]| { + for k in 0..3 { + let (i, j) = (k, (k + 1) % 3); + if is_self && (ids_t.contains(&ids_e[i]) || ids_t.contains(&ids_e[j])) { + continue; + } + if segment_crosses_triangle(pe[i], pe[j], pt[0], pt[1], pt[2]) { + return true; + } + } + false + }; + edge_hits(ids_a, pa, ids_b, qb) || edge_hits(ids_b, qb, ids_a, pa) +} + +/// Whether the open segment strictly pierces the triangle (sign tests; touching or +/// degenerate configurations never cross). +#[cfg(feature = "dim3")] +pub(crate) fn segment_crosses_triangle( + p: Vector, + q: Vector, + a: Vector, + b: Vector, + c: Vector, +) -> bool { + let n = (b - a).cross(c - a); + let dp = (p - a).gdot(n); + let dq = (q - a).gdot(n); + if dp == 0.0 || dq == 0.0 || dp.signum() == dq.signum() { + return false; + } + let d = q - p; + let s1 = (a - p).cross(b - p).gdot(d); + let s2 = (b - p).cross(c - p).gdot(d); + let s3 = (c - p).cross(a - p).gdot(d); + (s1 > 0.0 && s2 > 0.0 && s3 > 0.0) || (s1 < 0.0 && s2 < 0.0 && s3 < 0.0) +} diff --git a/src/dynamics/soft_body/soft_body_meshes.rs b/src/dynamics/soft_body/soft_body_meshes.rs index 7ef111afc..1c0a41dd7 100644 --- a/src/dynamics/soft_body/soft_body_meshes.rs +++ b/src/dynamics/soft_body/soft_body_meshes.rs @@ -32,6 +32,22 @@ impl SoftBody { } /// The mesh with the given id, mutably. + /// Iterates over all collision meshes mutably. + pub(crate) fn meshes_mut(&mut self) -> impl Iterator { + self.clusters.iter_mut().flat_map(|c| c.meshes_mut()) + } + + /// Refreshes every mesh's vertex cache from the particles (see + /// `SoftCollisionMesh::cached_vertex`). + pub(crate) fn refresh_vertex_caches(&mut self) { + let particles = &self.particles; + for cluster in &mut self.clusters { + for mesh in cluster.meshes_mut() { + mesh.refresh_vertex_cache(particles); + } + } + } + pub(crate) fn mesh_mut(&mut self, id: SoftMeshId) -> Option<&mut SoftCollisionMesh> { self.clusters .get_mut(id.cluster as usize)? 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 c70959f8f..399ba0e0e 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 @@ -1,4 +1,5 @@ /// refreshes the attachment index and island links of the bodies whose attachments changed. + // Flagged as modified so the broad phase updates their AABBs with the raised margin. /// Refreshes every live cluster's proxy rigid body from the current particles: its pose is // Dead zone: the frame fit carries a little noise even at rest (the least-squares // so a one-refresh-stale proxy pose is fine. diff --git a/src/dynamics/soft_body/soft_recovery_settings.rs b/src/dynamics/soft_body/soft_recovery_settings.rs new file mode 100644 index 000000000..676c1dd6c --- /dev/null +++ b/src/dynamics/soft_body/soft_recovery_settings.rs @@ -0,0 +1,57 @@ +//! Runtime toggles and tuning for the soft-body tangle detection and recovery machinery: +//! prevention, detection, passive stand-down, and the intersection-volume rows. Every +//! mechanism can be switched off individually and the empirical constants are exposed, so +//! the minimal subset that a scene needs can be found experimentally. + +/// Runtime configuration of the soft-body tangle detection and recovery stack. +/// +/// Lives on [`IntegrationParameters::soft_recovery`], so it can be changed between steps. +/// +/// [`IntegrationParameters::soft_recovery`]: crate::dynamics::IntegrationParameters +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "serde-serialize", serde(default))] +pub struct SoftRecoverySettings { + // --- Prevention --- + /// Raise the speculative contact margin from velocities authored between steps (at + /// insertion, or through `set_particle_velocity`), so the coming step's reach already + /// covers them instead of lagging one step behind and letting a fast fresh body + /// tunnel. (default: `true`) + pub authored_velocity_margin: bool, + /// Give the edge-vs-edge pass a speculative reach (motion margin plus the closing-speed + /// rule) and, in 3D, emit edge rows for closed-vs-closed pairs, so two edge-leading + /// bodies crossing corner-first (no vertex near the other's surface) collide instead of + /// passing straight through. Off by default: the closed-vs-closed edge rows leave the + /// pressed 3D piles crossed. (default: `false`) + pub edge_speculation: bool, + + // --- Detection --- + /// Detect inverted cells (material locally inside out) each step. Feeds the self + /// stand-down. (default: `true`) + pub inverted_cell_detection: bool, + /// Detect surface self-crossings each step (the BVTT sweep). Feeds the self + /// stand-down and the self untangler. (default: `true`) + pub self_crossing_detection: bool, + /// Skip the self-crossing sweep while the surface's accumulated travel could not have + /// bridged half a skin (crossings only form through motion). Off: sweep every step. + /// (default: `true`) + pub detection_motion_gating: bool, + /// Detect boundary crossings between pairs of soft surfaces. Feeds the cross-body + /// expel-only gate, the edge-pass stand-down, and the intersection-volume rows. + /// (default: `true`) + pub cross_body_detection: bool, + +} + +impl Default for SoftRecoverySettings { + fn default() -> Self { + Self { + authored_velocity_margin: true, + edge_speculation: false, + inverted_cell_detection: true, + self_crossing_detection: true, + detection_motion_gating: true, + cross_body_detection: true, + } + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs index d97553675..16037f722 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs @@ -142,6 +142,10 @@ impl SoftConstraintsSet { }; core::mem::swap(&mut mesh.edge_contacts, &mut assembled.edge_contacts); core::mem::swap(&mut mesh.vertex_contacts, &mut assembled.vertex_contacts); + mesh.crossing_sweep_travel = assembled.crossing_sweep_travel; + core::mem::swap(&mut mesh.crossed_partners, &mut assembled.crossed_partners); + } + } self.edge_workspace.per_body = per_body; self.chunk_contacts(); } diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs index 347806110..ed1a9bf32 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs @@ -428,6 +428,7 @@ impl SoftConstraintsSet { out.tangled_elements.clone_from(&detected.tangled_elements); out.tangled_vertices.clone_from(&detected.tangled_vertices); out.crossings.clone_from(&detected.crossings); + out.exempt_self_tangles(surface_handle); self.assemble_vertex_contacts( ctx, ai, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs index c71b85d3e..960f26979 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs @@ -44,9 +44,17 @@ impl SoftConstraintsSet { return; }; let is_self = other_ai == Some(ai); + // In 2D vertex constraints alone complete a first contact between closed surfaces, and + // crossing edges at dented corners would only add oblique constraints fighting them; in 3D + // edge-leading bodies (bars crossing corner-first) need them, filtered below. + #[cfg(feature = "dim2")] if mesh.is_closed() && other_mesh.is_closed() { return; } + #[cfg(feature = "dim3")] + if !params.soft_bodies.recovery.edge_speculation && mesh.is_closed() && other_mesh.is_closed() { + return; + } let other_handle = mesh_ref(other_co); let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; let other_slots = other_ai.map(|bi| { diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs index 05293dce3..ab58a0a15 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs @@ -56,6 +56,24 @@ pub(super) struct BodyContacts { /// The owner's vertex contacts of the last step with the current other body (and /// vertex pass), by (vertex, element). pub(super) previous_vertex: HashMap<(u32, u32), (Real, Vector)>, + /// Elements (and vertices) of the mesh being assembled whose self contacts stand down + /// this step: carried by inverted cells, or part of a self-crossing of the surface (empty + /// while the surface is healthy). Copied from the narrow phase's self detection. + pub(super) tangled_elements: Vec, + pub(super) tangled_vertices: Vec, + /// The crossing element pairs the narrow phase's sweep found on the mesh being assembled. + pub(super) crossings: Vec<(u32, u32)>, + /// Elements of the current pair's surface side, and vertices of its vertex side, taking + /// part in a crossing *between* the two surfaces (empty while the surfaces do not + /// cross). Rows touching them may only expel, never hold at skin distance: the + /// closed-surface expulsion still pushes a lens out, while nothing freezes the + /// crossing. + pub(super) cross_tangled_elements: Vec, + pub(super) cross_tangled_vertices: Vec, + /// Element-granularity crossing flags on the vertex-side mesh, and the recorded + /// (element-side, vertex-side) crossing element pairs. + pub(super) cross_tangled_vb_elements: Vec, + pub(super) cross_pairs: Vec<(u32, u32)>, } /// The contact state one collision mesh owns across steps: what the next step warm-starts from. #[derive(Default)] @@ -63,7 +81,14 @@ pub(super) struct MeshContacts { pub(super) id: SoftMeshId, pub(super) edge_contacts: Vec, pub(super) vertex_contacts: Vec, + /// Surface travel since the last self-crossing sweep (the owner's `crossing_sweep_travel` of the + /// next step). + pub(super) crossing_sweep_travel: Real, + /// The surfaces whose pair with this mesh crosses this step (the owner's + /// `crossed_partners`). + pub(super) crossed_partners: Vec, } + impl BodyContacts { /// Starts assembling the mesh `id`: its contacts go to a list of its own. pub(super) fn begin_mesh(&mut self, id: SoftMeshId) { @@ -74,6 +99,46 @@ impl BodyContacts { }); } + /// The contact state of the mesh being assembled. + pub(super) fn mesh(&mut self) -> &mut MeshContacts { + &mut self.meshes[self.current_mesh] + } + + /// Records the self pair as recovery-owned when this mesh has self tangles. + pub(super) fn exempt_self_tangles(&mut self, own_surface: crate::geometry::ColliderHandle) { + let mc = &mut self.meshes[self.current_mesh]; + if (!self.tangled_elements.is_empty() || !self.tangled_vertices.is_empty()) + && !mc.crossed_partners.contains(&own_surface) + { + mc.crossed_partners.push(own_surface); + } + } + + /// Records the current pair as recovery-owned when the two boundaries cross. + pub(super) fn exempt_cross_tangles(&mut self, other_surface: crate::geometry::ColliderHandle) { + if self.cross_tangled_elements.is_empty() { + return; + } + let mc = &mut self.meshes[self.current_mesh]; + if !mc.crossed_partners.contains(&other_surface) { + mc.crossed_partners.push(other_surface); + } + } + + pub(super) fn clear(&mut self) { + self.contacts.clear(); + self.meshes.clear(); + self.current_mesh = 0; + self.max_approach_speed = 0.0; + self.previous.clear(); + self.previous_vertex.clear(); + self.tangled_elements.clear(); + self.tangled_vertices.clear(); + self.crossings.clear(); + self.cross_tangled_elements.clear(); + self.cross_tangled_vertices.clear(); + self.cross_tangled_vb_elements.clear(); + self.cross_pairs.clear(); } /// The step-constant inputs of the per-body contact assembly. pub(super) struct AssemblyCtx<'a> { diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs index 63c62b48b..5ce505119 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs @@ -44,11 +44,27 @@ pub(crate) fn detect_edges( if !params.soft_bodies.recovery.edge_speculation && mesh.is_closed() && other_mesh.is_closed() { return false; } + let speculation = params.soft_bodies.recovery.edge_speculation; let Some(other_bvh) = other_co.shape().as_composite_shape().map(|c| c.bvh()) else { return false; }; let skins = surface_co.contact_skin() + other_co.contact_skin(); + // The motion margin only matters when the pair can skip the skin band in + // a step; a resting pair keeps the tight reach (candidate enumeration is the edge + // pass's cost). let motion = ctx.motion_margin(surface_co) + ctx.motion_margin(other_co); + let reach = params.prediction_distance() + + skins + + if speculation && motion > 0.25 * skins { + motion + } else { + 0.0 + }; + let tangled_elements: &[bool] = tangles.as_ref().map_or(&[], |t| t.tangled_elements); + + let other_inv_pose = other_co.position().inverse(); + if !is_self && params.soft_bodies.recovery.cross_body_detection && params.soft_bodies.recovery.edge_stand_down + { let scan = EdgeScan { }; let n_e = mesh.indices().len(); @@ -147,6 +163,26 @@ impl EdgeScan<'_> { let sep = point_b - point_a; let len = sep.length(); if len >= self.reach || len < 1.0e-6 { + let dist = len - skins; + continue; + } + // Beyond the prediction distance, only a contact closing fast enough + // to happen within the whole step is kept (the vertex pass's rule): + // the rest of the self.reach is the bodies' motion margin. + if dist >= self.params.prediction_distance() { + if !self.speculation { + continue; + } + let vel_a = self.mesh.vertex_velocity(self.sb, va[0] as usize) * ba[0] + + self.mesh.vertex_velocity(self.sb, va[1] as usize) * ba[1]; + let (other, other_mesh) = (self.other, self.other_mesh); + let vel_b = other_mesh.vertex_velocity(other, vb[0] as usize) * bb[0] + + other_mesh.vertex_velocity(other, vb[1] as usize) * bb[1]; + let closing = (vel_a - vel_b).gdot(sep / len); + if dist - closing * self.step_dt >= self.params.prediction_distance() { + continue; + } + } // Force direction on this body's edge (away from the self.other edge). let dir = -sep / len; out.push(SoftEdgeCandidate { diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs index 3fda74352..7d4be0c6b 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs @@ -1,3 +1,66 @@ +//! The vertex-vs-surface pass of a pair of soft surfaces (or of a mesh against itself), with the crossing tests it relies on. + +use crate::alloc_prelude::*; + +#[cfg(feature = "dim2")] +use crate::dynamics::soft_body_crossing_tests::segments_cross; +use crate::dynamics::{SoftBody, SoftCollisionMesh}; +use crate::math::{DIM, Real, Vector}; +use crate::utils::DotProduct; +use parry::bounding_volume::BoundingVolume; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::soft_contacts_classify::{classify_inside, classify_inside_self, project_on_element, ring_depths}; +use super::soft_contacts_volume::{VolumeSide, fill_depths, patch_vertices, volume_bins, volume_split}; +use super::{SelfTangles, Side, SoftDetectionCtx, SoftVertexPass, SoftVertexCandidate, SoftVertexHits}; + +/// Whether two elements' boundaries cross transversally, whatever their arities (a +/// segment through a triangle covers a wire through a surface; two wires cross with +/// measure zero). +pub(crate) fn elements_cross(pa: &[Vector], pb: &[Vector]) -> bool { + #[cfg(feature = "dim2")] + { + segments_cross([pa[0], pa[1]], [pb[0], pb[1]]) + } + #[cfg(feature = "dim3")] + match (pa.len(), pb.len()) { + (3, 3) => { + let (ta, tb) = ([pa[0], pa[1], pa[2]], [pb[0], pb[1], pb[2]]); + crate::dynamics::soft_body_crossing_tests::tri_pair_crosses( + &[0, 1, 2], + &ta, + &[3, 4, 5], + &tb, + false, + ) + } + (2, 3) => crate::dynamics::soft_body_crossing_tests::segment_crosses_triangle( + pa[0], pa[1], pb[0], pb[1], pb[2], + ), + (3, 2) => crate::dynamics::soft_body_crossing_tests::segment_crosses_triangle( + pb[0], pb[1], pa[0], pa[1], pa[2], + ), + _ => false, + } +} + +/// The positions of an element's vertices (an element has at most `DIM` vertices; the unused +/// entries stay zero). +#[inline] +pub(crate) fn element_points(mesh: &SoftCollisionMesh, element: &[u32]) -> [Vector; DIM] { + let mut points = [Vector::ZERO; DIM]; + for (k, &v) in element.iter().enumerate().take(DIM) { + points[k] = mesh.cached_vertex(v as usize); + } + points +} + + // each other, the pair's keep-apart rows are wrong-sided by construction and freeze + // opened), so they stand down around it, like the self rows do around a + // The two contact skins (a soft surface's is its vertices' thickness; between two pieces of + // one torn body, capped per pair by the gap at rest, see `rest_gap_skins`); the // Crossing repulsion (see `repel_row`): the detected crossing pairs also get rows if len >= reach || len < 1.0e-6 { } @@ -8,3 +71,6 @@ // a ghost of that internal feature (the neighbor carries the actual contact); // Deterministic order, and one constraint per surface vertex touched: the elements sharing + // A foreign vertex that crossed a closed surface: seen from behind (reversed + // fast incoming vertex also sees the far side of a closed surface as reversed + // through the speculative reach (its near-side sightings often land next to diff --git a/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs b/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs new file mode 100644 index 000000000..f17d0f7c1 --- /dev/null +++ b/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs @@ -0,0 +1,407 @@ +//! The self contact detection of one soft collision mesh: tangle signal, self candidates and self-overlap regions. + +use crate::alloc_prelude::*; + +#[cfg(feature = "dim2")] +use crate::dynamics::soft_body_crossing_tests::segments_cross; +use crate::dynamics::{SoftBody, SoftCollisionMesh}; +use crate::geometry::{Collider, ColliderHandle}; +use crate::math::{DIM, Real, Vector}; +use crate::utils::DotProduct; +use parry::partitioning::BvhWorkspace; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::soft_contacts_classify::{classify_inside_self, project_on_element}; +use super::soft_contacts_edge_pass::detect_edges; +use super::soft_contacts_vertex_pass::detect_vertex_pass; +use super::soft_contacts_volume::{VolumeBin, VolumeSide, patch_vertices, volume_bins, volume_split}; +use super::{SelfTangles, SoftDetectionCtx, SoftVertexPass, SoftEdgePass}; + +/// The self contact detection of one soft collision mesh (see +/// [`NarrowPhase::soft_self_contacts`]): its tangle signal, and its self candidates. +#[derive(Clone, Default)] +pub(crate) struct SoftSelfContacts { + /// The body's currently inverted cells and the particles they touch (empty while every + /// cell is healthy, and for cell-less bodies): the material-side tangle signal. + pub inverted_cells: Vec, + pub inverted_particles: Vec, + /// Elements (and vertices) whose self contacts stand down this step: backed by inverted + /// cells, or part of a self-crossing of the surface (empty while the surface is healthy). + /// See [`detect_self_tangles`]. + pub tangled_elements: Vec, + pub tangled_vertices: Vec, + /// The crossing element pairs the sweep found (capped, sorted). + pub crossings: Vec<(u32, u32)>, + /// The mesh's `crossing_sweep_travel` of the next step: reset by a sweep, accumulated otherwise. + pub crossing_sweep_travel_next: Real, + /// The mesh's vertices against its own surface. + pub vertex_pass: SoftVertexPass, + /// The mesh's edges against its own edges (`None`: no self edge constraints for this mesh). + pub edges: Option, + /// The bins of the self-overlaps between distinct regions of the closed surface (see + /// `overlap_self_regions`), one entry per region constraint, in emission order. + pub region_bins: Vec>, + /// Workspace of the self-crossing sweep's tree traversal. + bvh_workspace: BvhWorkspace, +} + +impl SoftSelfContacts { + /// The mesh's tangle signal, as the self passes read it. + pub fn tangles(&self) -> SelfTangles<'_> { + SelfTangles { + tangled_elements: &self.tangled_elements, + tangled_vertices: &self.tangled_vertices, + crossings: &self.crossings, + } + } +} + +/// Marks the elements (and vertices) of `mesh` whose self contacts stand down this step: features +/// backed by inverted cells or taking part in a self-crossing of the surface, where proximity +/// constraints would freeze the tangle that elasticity resolves; both signals are memoryless. +fn detect_self_tangles( + sb: &SoftBody, + mesh: &SoftCollisionMesh, + surface_co: &Collider, + run_crossing_sweep: bool, + out: &mut SoftSelfContacts, +) { + out.tangled_elements.clear(); + out.tangled_vertices.clear(); + out.crossings.clear(); + let num_elements = mesh.indices().len(); + let num_vertices = mesh.vertex_count(); + // The tables are allocated on the first flag: the healthy path stays free. + fn mark_element( + tangled: (&mut Vec, &mut Vec), + sizes: (usize, usize), + element: &[u32], + e: usize, + ) { + if tangled.0.is_empty() { + tangled.0.resize(sizes.0, false); + tangled.1.resize(sizes.1, false); + } + tangled.0[e] = true; + for &v in element { + tangled.1[v as usize] = true; + } + } + + // Inverted material: elements whose backing cell is inverted, and vertices held by + // any inverted cell (a boundary vertex pushed through its own surface sits in one). + if !out.inverted_particles.is_empty() { + for (e, &cell) in mesh.element_cell_ids(sb).iter().enumerate() { + if out + .inverted_cells + .get(cell as usize) + .copied() + .unwrap_or(false) + { + mark_element( + (&mut out.tangled_elements, &mut out.tangled_vertices), + (num_elements, num_vertices), + mesh.element(e), + e, + ); + } + } + for v in 0..num_vertices { + let (anchors, weights) = mesh.vertex_anchors(sb, v); + if anchors + .iter() + .zip(&weights) + .any(|(&p, &w)| p != u32::MAX && w > 0.01 && out.inverted_particles[p as usize]) + { + if out.tangled_vertices.is_empty() { + out.tangled_elements.resize(num_elements, false); + out.tangled_vertices.resize(num_vertices, false); + } + out.tangled_vertices[v] = true; + } + } + } + + if !run_crossing_sweep { + return; + } + // Surface self-crossings, from one traversal of the surface's tree against itself + // (the leaves are element ids, in the shape's local space on both sides, so no pose + // is involved). + #[cfg(feature = "dim3")] + if mesh.is_wire() { + // Two wire curves cross with measure zero: no instantaneous predicate can see + // their tangles (left to CCD prevention). + return; + } + let Some(bvh) = surface_co.shape().as_composite_shape().map(|c| c.bvh()) else { + return; + }; + // The predicates read the shape's own vertex buffer: the exact positions the tree's + // leaves were refit on, contiguous, and all in one frame (crossings are + // frame-invariant, so the shape's local space is fine). + #[cfg(feature = "dim2")] + let shape_vertices = surface_co.shape().as_polyline().map(|p| p.vertices()); + #[cfg(feature = "dim3")] + let shape_vertices = surface_co.shape().as_trimesh().map(|t| t.vertices()); + let Some(shape_vertices) = shape_vertices else { + return; + }; + if shape_vertices.len() < num_vertices { + return; + } + // Every leaf pair of the tree against itself is tested for a crossing (see `test_crossing`) + // into a sink, applied afterwards. A large surface runs in parallel: pairs and chunk sinks + // keep the sequential order, so the records (and the cap) are the same. + let mut sink = TangleSink::default(); + #[cfg(feature = "parallel")] + let parallel = num_elements >= 4096 && rayon::current_num_threads() > 1; + #[cfg(not(feature = "parallel"))] + let parallel = false; + if parallel { + #[cfg(feature = "parallel")] + { + use rayon::prelude::*; + let pairs = bvh.traverse_bvtt_single_tree_parallel::(); + let sinks: Vec = pairs + .par_chunks(2048) + .map(|chunk| { + let mut sink = TangleSink::default(); + for &(i, j) in chunk { + test_crossing(mesh, shape_vertices, i as usize, j as usize, &mut sink); + } + sink + }) + .collect(); + for chunk in sinks { + sink.merge(chunk); + } + } + } else { + let SoftSelfContacts { bvh_workspace, .. } = &mut *out; + bvh.traverse_bvtt_single_tree::(bvh_workspace, &mut |i, j| { + test_crossing(mesh, shape_vertices, i as usize, j as usize, &mut sink); + }); + } + let SoftSelfContacts { + tangled_elements, + tangled_vertices, + crossings, + .. + } = out; + for &e in &sink.marks { + mark_element( + (tangled_elements, tangled_vertices), + (num_elements, num_vertices), + mesh.element(e as usize), + e as usize, + ); + } + *crossings = sink.crossings; + // Deterministic untangler input whatever order the traversal visited pairs in. + crossings.sort_unstable(); + crossings.dedup(); +} + +/// What the self-crossing sweep found: the elements to flag as tangled, and the crossing +/// element pairs, deduplicated in visitation order and capped. +#[derive(Default)] +struct TangleSink { + marks: Vec, + crossings: Vec<(u32, u32)>, +} + +impl TangleSink { + /// The cap on recorded crossings: room for several simultaneous tangles (a tight cap + /// starves later tangles of any recorded crossings, collapsing the cluster analysis). + #[cfg(feature = "dim3")] + const MAX_CROSSINGS: usize = 256; + #[cfg(feature = "dim2")] + const MAX_CROSSINGS: usize = 64; + + #[inline] + fn record(&mut self, pair: (u32, u32)) { + if self.crossings.len() < Self::MAX_CROSSINGS && !self.crossings.contains(&pair) { + self.crossings.push(pair); + } + } + + /// Appends a later chunk's findings (its records after this sink's, same rule). + fn merge(&mut self, other: TangleSink) { + self.marks.extend(other.marks); + for pair in other.crossings { + self.record(pair); + } + } +} + +/// The crossing test of one leaf pair of the sweep (see `detect_self_tangles`). +#[inline] +fn test_crossing( + mesh: &SoftCollisionMesh, + shape_vertices: &[Vector], + i: usize, + j: usize, + sink: &mut TangleSink, +) { + if i == j { + return; + } + let (ei, ej) = (mesh.element(i), mesh.element(j)); + let p = |v: u32| shape_vertices[v as usize]; + #[cfg(feature = "dim2")] + { + // Adjacent segments only meet at their shared vertex: never a crossing. + if ei.iter().any(|v| ej.contains(v)) { + return; + } + if segments_cross([p(ei[0]), p(ei[1])], [p(ej[0]), p(ej[1])]) { + sink.marks.push(i as u32); + sink.marks.push(j as u32); + sink.record((i.min(j) as u32, i.max(j) as u32)); + } + } + #[cfg(feature = "dim3")] + { + // Triangles sharing an edge have no testable edge (a touching edge is adjacent, never a + // transversal pierce): most visited pairs die on this index-only test before reading any + // position. + if ei.iter().filter(|v| ej.contains(v)).count() >= 2 { + return; + } + // Each direction: the edges of one triangle against the other, skipping those touching + // it; what is left (a resting fold, a ring neighbor) mostly dies on one plane test. A hit + // flags the pierced triangle and those sharing the piercing edge, and records the pair. + let mut pierce = |ea: &[u32], eb: &[u32], a: usize, b: usize| { + let tb = [p(eb[0]), p(eb[1]), p(eb[2])]; + let n = (tb[1] - tb[0]).cross(tb[2] - tb[0]); + let nn = n.length_squared(); + if nn < 1.0e-20 { + return; + } + // Plane offsets scale as `|n| * distance`; the band is a distance of + // 1.0e-4 triangle scales (the scale is about `sqrt(|n|)`), so an edge + // grazing the plane (a resting fold) never reads as a pierce. + let len_n = nn.sqrt(); + let band = 1.0e-4 * len_n * len_n.sqrt(); + let pa = [p(ea[0]), p(ea[1]), p(ea[2])]; + let dp = [ + (pa[0] - tb[0]).dot(n), + (pa[1] - tb[0]).dot(n), + (pa[2] - tb[0]).dot(n), + ]; + if dp.iter().all(|d| *d >= -band) || dp.iter().all(|d| *d <= band) { + return; + } + for k in 0..3 { + let k1 = (k + 1) % 3; + if dp[k] * dp[k1] >= 0.0 { + continue; + } + let (v0, v1) = (ea[k], ea[k1]); + if eb.contains(&v0) || eb.contains(&v1) { + continue; + } + let (q0, q1) = (pa[k], pa[k1]); + let pq = q1 - q0; + let d = [ + pq.dot((tb[0] - q0).cross(tb[1] - q0)), + pq.dot((tb[1] - q0).cross(tb[2] - q0)), + pq.dot((tb[2] - q0).cross(tb[0] - q0)), + ]; + if !d.iter().all(|x| *x >= 0.0) && !d.iter().all(|x| *x <= 0.0) { + continue; + } + sink.marks.push(b as u32); + // The pierce fires once per piercing edge and the traversal revisits + // pairs: recorded deduplicated. + sink.record((a.min(b) as u32, a.max(b) as u32)); + for &incident in mesh.vertex_element_ids(v0) { + if mesh.element(incident as usize).contains(&v1) { + sink.marks.push(incident); + } + } + } + }; + pierce(ei, ej, i, j); + pierce(ej, ei, j, i); + } +} + +/// The self contact detection of one mesh with self contacts enabled, on a body with free +/// particles: the tangle signal, then the vertex and edge candidates against itself. +pub(super) fn update_self( + out: &mut SoftSelfContacts, + sb: &SoftBody, + mesh: &SoftCollisionMesh, + handle: ColliderHandle, + co: &Collider, + ctx: &SoftDetectionCtx, +) { + let recovery = &ctx.params.soft_bodies.recovery; + // The material-side tangle signal: the cells currently inverted (negative volume + // relative to rest) and the particles they touch. Left empty while every cell is + // healthy, so the healthy path allocates and scans nothing downstream. + out.inverted_cells.clear(); + out.inverted_particles.clear(); + if recovery.inverted_cell_detection && !sb.cells.is_empty() { + for (ci, cell) in sb.cells.iter().enumerate() { + let x: [Vector; DIM + 1] = + core::array::from_fn(|k| sb.particles[cell.vertices[k] as usize].position); + if SoftBody::cell_volume(x) * cell.rest_volume <= 0.0 { + if out.inverted_cells.is_empty() { + out.inverted_cells.resize(sb.cells.len(), false); + out.inverted_particles.resize(sb.particles.len(), false); + } + out.inverted_cells[ci] = true; + for &p in &cell.vertices { + out.inverted_particles[p as usize] = true; + } + } + } + } + // The self-crossing sweep (the expensive half) runs when the surface's accumulated travel + // since the last sweep could have bridged half a skin, and every step while anything is + // flagged; the cheap inverted-cell scan always runs. + let was_tangled = mesh.crossed_partners.contains(&handle); + let travel = mesh.crossing_sweep_travel + ctx.motion_margin(co); + let run_crossing_sweep = recovery.self_crossing_detection + && (was_tangled || !recovery.detection_motion_gating || travel > 0.5 * co.contact_skin()); + out.crossing_sweep_travel_next = if run_crossing_sweep { 0.0 } else { travel }; + detect_self_tangles(sb, mesh, co, run_crossing_sweep, out); + let side = (sb, mesh, handle, co); + let mut vertex_pass = core::mem::take(&mut out.vertex_pass); + detect_vertex_pass(&mut vertex_pass, side, side, Some(out.tangles()), ctx); + out.vertex_pass = vertex_pass; + let mut edges = out.edges.take().unwrap_or_default(); + let has_edges = detect_edges(&mut edges, side, side, Some(out.tangles()), ctx); + out.edges = has_edges.then_some(edges); + detect_self_regions(out, sb, mesh, co, ctx); +} + +/// Self-overlaps between distinct regions of one closed surface (see `overlap_self_regions`): +/// vertices behind another part of their own surface, grouped into connected regions, each binned +/// with the vertices of the elements it faces, like a pair of bodies would. +fn detect_self_regions( + out: &mut SoftSelfContacts, + sb: &SoftBody, + mesh: &SoftCollisionMesh, + co: &Collider, + ctx: &SoftDetectionCtx, +) { + let params = ctx.params; + let recovery = ¶ms.soft_bodies.recovery; + out.region_bins.clear(); + if !recovery.overlap_constraints + || !recovery.overlap_self_regions + || !mesh.is_closed() + || mesh.orientation_unreliable + || out.crossings.is_empty() + { + return; + } + let n_v = mesh.vertex_count(); + let n_e = mesh.indices().len(); +} diff --git a/src/pipeline/physics_pipeline/substep.rs b/src/pipeline/physics_pipeline/substep.rs index a4e6d30d3..504752c9e 100644 --- a/src/pipeline/physics_pipeline/substep.rs +++ b/src/pipeline/physics_pipeline/substep.rs @@ -306,7 +306,12 @@ impl PhysicsPipeline { // Soft bodies whose settings changed must wake up for the change to take effect; the // colliders of the ones whose particles were moved follow them (picked up as modified // colliders below). - soft_bodies.apply_user_changes(bodies, colliders, &mut self.quarantine.soft_bodies); + soft_bodies.apply_user_changes( + bodies, + colliders, + &integration_parameters, + &mut self.quarantine.soft_bodies, + ); // Apply modifications. let mut modified_colliders = colliders.take_modified(); diff --git a/src_testbed/save.rs b/src_testbed/save.rs index 5eb085bbc..0688ddaef 100644 --- a/src_testbed/save.rs +++ b/src_testbed/save.rs @@ -9,6 +9,7 @@ pub struct SerializableTestbedState { pub flags: TestbedStateFlags, pub selected_example: usize, pub example_settings: ExampleSettings, + pub soft_recovery: rapier::dynamics::SoftRecoverySettings, pub camera: Camera, } @@ -19,6 +20,7 @@ impl TestbedState { flags: self.flags, selected_example: self.selected_display_index, example_settings: self.example_settings.clone(), + soft_recovery: self.soft_recovery, camera, } } @@ -29,6 +31,7 @@ impl TestbedState { self.flags = state.flags; self.selected_display_index = state.selected_example; self.example_settings = state.example_settings; + self.soft_recovery = state.soft_recovery; *camera = state.camera; } } diff --git a/src_testbed/testbed/state.rs b/src_testbed/testbed/state.rs index e19278af5..44ce9753c 100644 --- a/src_testbed/testbed/state.rs +++ b/src_testbed/testbed/state.rs @@ -115,6 +115,10 @@ pub struct TestbedState { /// Currently selected position in the display order pub selected_display_index: usize, pub example_settings: ExampleSettings, + /// The soft-body recovery toggles (see `SoftRecoverySettings`): owned by the testbed + /// and stamped onto the running world every frame, so the panel's choices survive + /// demo restarts and switches. + pub soft_recovery: rapier::dynamics::SoftRecoverySettings, pub broad_phase_type: RapierBroadPhaseType, pub snapshot: Option, /// Number of physics steps run since the example was (re)started. Bumped by @@ -153,6 +157,7 @@ impl Default for TestbedState { examples: Vec::new(), example_groups: Vec::new(), example_settings: ExampleSettings::default(), + soft_recovery: Default::default(), selected_display_index: 0, broad_phase_type: RapierBroadPhaseType::default(), camera_locked: false, diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index f6c5afe0a..a4c7c3016 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -546,6 +546,13 @@ fn settings_tab(ui: &mut Ui, state: &mut TestbedState, world: &mut PhysicsWorld) ) .on_hover_text("Continuous collision detection substeps."); + if world.soft_bodies.iter().next().is_some() { + // Edits the testbed-owned copy, stamped onto the world every frame: the + // choices survive demo restarts and switches (and app relaunches, through the + // saved testbed state). + soft_recovery_section(ui, &mut state.soft_recovery); + } + #[cfg(feature = "parallel")] { let max_threads = num_cpus::get(); @@ -576,6 +583,28 @@ fn settings_tab(ui: &mut Ui, state: &mut TestbedState, world: &mut PhysicsWorld) } } +/// The soft-body tangle detection and recovery toggles and knobs (see `SoftRecoverySettings`), +/// edited live on the running world; shown whenever the world contains soft bodies. Re-enabling +/// a mechanism does not wake bodies that fell asleep while it was off. +fn soft_recovery_section(ui: &mut Ui, r: &mut rapier::dynamics::SoftRecoverySettings) { + egui::CollapsingHeader::new("Soft recovery") + .default_open(false) + .show(ui, |ui| { + ui.label("Prevention"); + ui.checkbox(&mut r.authored_velocity_margin, "Authored-velocity margin"); + ui.checkbox(&mut r.edge_speculation, "Edge speculation (3D closed pairs)"); + ui.separator(); + ui.label("Detection"); + ui.checkbox(&mut r.inverted_cell_detection, "Inverted cells"); + ui.checkbox(&mut r.self_crossing_detection, "Self-crossings"); + ui.checkbox(&mut r.detection_motion_gating, "Self-crossing sweep motion gating"); + ui.checkbox(&mut r.cross_body_detection, "Cross-body crossings"); + if ui.button("Reset to defaults").clicked() { + *r = Default::default(); + } + }); +} + /// One of the debug renderer's colors, as a color picker. The style stores HSLA; the picker /// works in sRGB, so the value round-trips through [`crate::debug_render::rgb_to_hsla`]. fn debug_color_picker(ui: &mut Ui, label: &str, color: &mut rapier::pipeline::DebugColor) { diff --git a/src_testbed/viewer.rs b/src_testbed/viewer.rs index bc1f1d7bd..3624fd2f1 100644 --- a/src_testbed/viewer.rs +++ b/src_testbed/viewer.rs @@ -150,6 +150,11 @@ impl TestbedViewer { return false; } + // The testbed owns the soft-recovery settings: stamped onto the (possibly fresh) + // world every frame, so the panel's choices survive demo restarts and switches. + world.integration_parameters.soft_bodies.recovery = self.state.soft_recovery; + + // Update the world-space cursor before the event pass: a mouse press picks with it. let cursor_pos = self.window.cursor_pos(); self.scene_mouse .update_from_window(cursor_pos, self.window.size().into(), &self.camera); From 949f89e911f15ff0b55ab536adc799b86cee6814 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 5 Sep 2026 15:53:10 +0200 Subject: [PATCH 08/32] feat(soft-body): tangle recovery: stand-down, expel-only gate and crossing repulsion on crossed pairs --- crates/rapier2d/tests/self_intersect_probe.rs | 38 +++++ crates/rapier2d/tests/soft_stack_probe.rs | 6 + .../rapier3d/tests/self_intersect_probe3.rs | 49 ++++++ crates/rapier3d/tests/soft_stack_probe3.rs | 6 + .../soft_body/soft_recovery_settings.rs | 25 +++ .../soft_contact_assembly_edge_contacts.rs | 6 + .../soft_contact_assembly_vertex_contacts.rs | 153 +++++++++++++++++- .../soft_contacts/soft_contacts_edge_pass.rs | 85 ++++++++++ .../soft_contacts_vertex_pass.rs | 141 +++++++++++++++- src_testbed/ui.rs | 6 + 10 files changed, 507 insertions(+), 8 deletions(-) diff --git a/crates/rapier2d/tests/self_intersect_probe.rs b/crates/rapier2d/tests/self_intersect_probe.rs index 55deb7687..e2240adcf 100644 --- a/crates/rapier2d/tests/self_intersect_probe.rs +++ b/crates/rapier2d/tests/self_intersect_probe.rs @@ -361,6 +361,10 @@ fn recovery_toggles_are_wired() { self_crossing_detection: false, detection_motion_gating: false, cross_body_detection: false, + self_stand_down: false, + cross_body_expel_gate: false, + edge_stand_down: false, + crossing_repulsion: false, }; for _ in 0..600 { world.step(); @@ -371,3 +375,37 @@ fn recovery_toggles_are_wired() { ); } +/// Crossing repulsion versus the freeze: with the self stand-down off, the self contacts +/// of a strip vertex flicked through its bottom edge hold the crossing forever; with +/// repulsion the same constraints push the vertex back to the side its neighbors are on. +#[test] +fn crossing_repulsion_recovers_flick() { + for repulsion in [true, false] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let r = &mut world.integration_parameters.soft_bodies.recovery; + r.authored_velocity_margin = false; + r.self_stand_down = false; + r.crossing_repulsion = repulsion; + let strip = SoftBodyBuilder::grid(Vector::ZERO, Vector::new(3.0, 0.15), 3, 2) + .pinned_particles([0, 1, 4, 5]) + .softness(SpringCoefficients::new(3.0, 1.0)) + .particle_radius(0.02) + .self_contacts(true); + let h = world.insert_soft_body(strip); + for _ in 0..5 { + world.step(); + } + world.soft_bodies[h].set_particle_velocity(3, Vector::new(0.0, -600.0)); + for _ in 0..120 { + world.step(); + } + let end = probe(&world, h).0; + println!("repulsion={repulsion}: end={end}"); + if repulsion { + assert_eq!(end, 0, "crossing repulsion did not push the vertex back"); + } else { + assert!(end > 0, "the frozen constraints did not keep the strip crossed"); + } + } +} diff --git a/crates/rapier2d/tests/soft_stack_probe.rs b/crates/rapier2d/tests/soft_stack_probe.rs index a221335ff..2193c6b0c 100644 --- a/crates/rapier2d/tests/soft_stack_probe.rs +++ b/crates/rapier2d/tests/soft_stack_probe.rs @@ -153,6 +153,12 @@ fn apply_preset(world: &mut PhysicsWorld) -> String { "reference" => { r.edge_speculation = false; } + // The reference settings with the crossing repulsion, unguided or guided by the + // pair's (and the fold's) volume normal (see `crossing_repulsion_guide`). + "reference+repel" | "reference+repel-guide" => { + r.edge_speculation = false; + r.crossing_repulsion = true; + } other => panic!("unknown SOFT_STACK_PRESET `{other}`"), } preset diff --git a/crates/rapier3d/tests/self_intersect_probe3.rs b/crates/rapier3d/tests/self_intersect_probe3.rs index 291dbc869..6ea155a6d 100644 --- a/crates/rapier3d/tests/self_intersect_probe3.rs +++ b/crates/rapier3d/tests/self_intersect_probe3.rs @@ -355,3 +355,52 @@ fn rope_through_ball_slides_out() { assert!(com.y < -1.0, "the rope hangs frozen at {:+.3}", com.y); } +/// Crossing repulsion clears a static crossing nothing else resolves: an unstrained horizontal +/// strip whose free end pokes through a vertical strip. With the crossing constraints dropped +/// nothing moves; with repulsion the end vertices are pushed back to their neighbors' side. +#[test] +fn crossing_repulsion_clears_static_poke() { + for repulsion in [true, false] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let r = &mut world.integration_parameters.soft_bodies.recovery; + r.crossing_repulsion = repulsion; + let strip = |origin: Vector, du: Vector, dv: Vector| { + SoftBodyBuilder::cloth(origin, du, dv, 12, 2) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.02) + .particle_radius(0.02) + }; + // Horizontal, pinned at its left end, its right end (x = 1.2) 0.05 past the vertical + // strip's plane (x = 1.15). + let horizontal = world.insert_soft_body( + strip( + Vector::new(-1.0, 0.0, -0.1), + Vector::new(0.2, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.2), + ) + .pinned_particles([0, 1]), + ); + let vertical = world.insert_soft_body( + strip( + Vector::new(1.15, -1.1, -0.1), + Vector::new(0.0, 0.2, 0.0), + Vector::new(0.0, 0.0, 0.2), + ) + .pinned_particles([0, 1, 22, 23]), + ); + let start = cross_crossings(&world, horizontal, vertical); + assert!(start > 0, "the strips were not authored crossed"); + for _ in 0..120 { + world.step(); + } + let end = cross_crossings(&world, horizontal, vertical); + println!("repulsion={repulsion}: {start} -> {end}"); + if repulsion { + assert_eq!(end, 0, "crossing repulsion did not clear the poke"); + } else { + assert!(end > 0, "the dropped constraints resolved the poke (the scene proves nothing)"); + } + } +} diff --git a/crates/rapier3d/tests/soft_stack_probe3.rs b/crates/rapier3d/tests/soft_stack_probe3.rs index b25bf1d9e..451613312 100644 --- a/crates/rapier3d/tests/soft_stack_probe3.rs +++ b/crates/rapier3d/tests/soft_stack_probe3.rs @@ -150,6 +150,12 @@ fn apply_preset(world: &mut PhysicsWorld) -> String { "reference" => { r.edge_speculation = false; } + // The reference settings with the crossing repulsion, unguided or guided by the + // pair's (and the fold's) volume normal (see `crossing_repulsion_guide`). + "reference+repel" | "reference+repel-guide" => { + r.edge_speculation = false; + r.crossing_repulsion = true; + } other => panic!("unknown SOFT_STACK_PRESET `{other}`"), } preset diff --git a/src/dynamics/soft_body/soft_recovery_settings.rs b/src/dynamics/soft_body/soft_recovery_settings.rs index 676c1dd6c..2371ccb36 100644 --- a/src/dynamics/soft_body/soft_recovery_settings.rs +++ b/src/dynamics/soft_body/soft_recovery_settings.rs @@ -41,6 +41,27 @@ pub struct SoftRecoverySettings { /// (default: `true`) pub cross_body_detection: bool, + // --- Passive recovery (stand-down) --- + /// Self contacts of tangled features (inverted cells, self-crossings) stand down so + /// the elasticity can resolve the tangle instead of freezing it at skin distance. + /// (default: `true`) + pub self_stand_down: bool, + /// A vertex row touching a boundary crossing between two surfaces may only expel, + /// never hold (a keep-apart row there is wrong-sided by construction). + /// (default: `true`) + pub cross_body_expel_gate: bool, + /// Edge rows touching a cross-body boundary crossing stand down (thin open bodies + /// meet through their edge rows, so the vertex gate alone cannot free them). + /// (default: `true`) + pub edge_stand_down: bool, + /// Rows on crossing-flagged features repel instead of standing down: the row pushes + /// the vertex to the side of the element where its own neighbors lie (its one-ring + /// centroid), through to skin distance there. A shallow crossing returns where it came + /// from, a body already past halfway completes its passage, and adjacent vertices + /// agree; a neighborhood straddling the element (a genuine tangle) still stands down. + /// (default: `false`) + pub crossing_repulsion: bool, + } impl Default for SoftRecoverySettings { @@ -52,6 +73,10 @@ impl Default for SoftRecoverySettings { self_crossing_detection: true, detection_motion_gating: true, cross_body_detection: true, + self_stand_down: true, + cross_body_expel_gate: true, + edge_stand_down: true, + crossing_repulsion: false, } } } diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs index 960f26979..5254bd0fe 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs @@ -81,6 +81,12 @@ impl SoftConstraintsSet { } if detected.crossed { // The pair is recovery-owned: the crossing guard leaves it alone. + let mc = &mut workspace.meshes[current_mesh]; + if !mc.crossed_partners.contains(&other_surface_handle) { + mc.crossed_partners.push(other_surface_handle); + } + } + let inv_mass_of = |body: &SoftBody, slots: Option<&[u32]>, v: u32| -> (u32, Real) { match slots { Some(slots) if slots[v as usize] != u32::MAX => { diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs index b83abb550..ab6e4ff37 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs @@ -1,8 +1,151 @@ - /// Vertex-vs-surface contact rows: the surface vertices of `other` (itself for self - /// simulated) against the surface of awake body `ai`, one row per (vertex, element) the - /// the other's surface, for an other body that is not in the awake list). A row holds the - // already pass through each other, the pair's keep-apart rows are wrong-sided by - // the self stand-down opened), so they stand down around it, like the self rows do +//! Vertex-vs-surface contact constraints (self contacts and surface-vs-surface pairs) and the crossing-repulsion constraint. + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::alloc_prelude::*; + +use super::soft_contact_assembly_volume_constraints::{emit_volume_bins, mirrored_bins}; +use super::{AssemblyCtx, BodyContacts, SOURCE_VERTEX_CONTACT, mesh_of, mesh_ref}; +use super::super::soft_constraints_set::SoftConstraintsSet; +use super::super::soft_contact::CONTACT_ANCHORS; +use super::super::soft_contact::{SoftContact, SoftContactElement, SoftContactSource}; +use crate::dynamics::soft_body::SoftPatchConstraints; +use crate::dynamics::soft_body::{SoftCollisionMesh, SoftVertexContact}; +use crate::dynamics::SoftBody; +use crate::geometry::soft_contacts::{SoftVertexPass, SoftVolumePatch, VolumeBin, element_plane}; +use crate::geometry::{Collider, ColliderHandle}; +use crate::math::{DIM, Real, Vector}; +use crate::utils::DotProduct; + +/// The crossing-repulsion constraint of a vertex against an element: pushes along the element's +/// plane normal, signed toward the vertex's one-ring centroid, to skin distance on that side +/// (inactive once there). `None` when the one-ring straddles the plane or the element is a wire. +fn repel_constraint( + (eb, eb_mesh, element): (&SoftBody, &SoftCollisionMesh, usize), + (vb, vb_mesh, vertex_pos, neighbors): (&SoftBody, &SoftCollisionMesh, Vector, &[u32]), + skins: Real, +) -> Option<(Vector, Real)> { + let (p0, n) = element_plane(eb_mesh, element, |v| eb_mesh.vertex(eb, v))?; + if neighbors.is_empty() { + return None; + } + let mut centroid = Vector::ZERO; + for &u in neighbors { + centroid += vb_mesh.vertex(vb, u as usize); + } + centroid /= neighbors.len() as Real; + let side = (centroid - p0).dot(n); + if side.abs() < 0.25 * skins { + return None; + } + // `dir` is the force on the surface; the vertex is pushed along its opposite. + let push = n * side.signum(); + Some((-push, (vertex_pos - p0).dot(push) - skins)) +} + +impl SoftConstraintsSet { + /// Vertex-vs-surface contact constraints: the surface vertices of `other` (`other_ai` its awake + /// index, `None` when fully pinned; itself for self contacts) against awake body `ai`, one per + /// (vertex, element) in `detected` (`flipped`: this body's vertices against a non-awake other). + #[allow(clippy::too_many_arguments)] + pub(super) fn assemble_vertex_contacts( + &self, + ctx: &AssemblyCtx, + ai: usize, + other_ai: Option, + other: &SoftBody, + (surface_handle, surface_co): (ColliderHandle, &Collider), + (other_surface_handle, other_co): (ColliderHandle, &Collider), + flipped: bool, + detected: &SoftVertexPass, + volume: Option<&SoftVolumePatch>, + regions: &[Vec], + out: &mut BodyContacts, + ) { + let AssemblyCtx { + params, + dyn_soft: (dyn_erp, dyn_cfm), + static_soft: (static_erp, static_cfm), + .. + } = *ctx; + let awake = &self.awake[ai]; + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + let other_handle = mesh_ref(other_co); + let current_mesh = out.current_mesh; + let is_self = other_ai == Some(ai); + let self_frozen = awake.frozen; + let own_slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let other_slots: Option<&[u32]> = other_ai.map(|bi| { + let a = &self.awake[bi]; + &self.slots[a.slot_start..a.slot_start + a.num_particles] + }); + // The meshes the two sides collide through. + let (Some(mesh), Some(other_mesh)) = ( + mesh_of(sb, surface_handle), + mesh_of(other, other_surface_handle), + ) else { + return; + }; + // The vertex side and the element side. + let (vb, vb_mesh, vb_slots, vb_co, eb, eb_mesh, eb_slots, eb_co) = if flipped { + (sb, mesh, Some(own_slots), surface_co, other, other_mesh, other_slots, other_co) + } else { + (other, other_mesh, other_slots, other_co, sb, mesh, Some(own_slots), surface_co) + }; + // Crossings between the two surfaces (see `soft_contacts`): the pair's keep-apart + // constraints are wrong-sided there and would freeze the crossing (or push material through + // the gates the self stand-down opened), so they stand down around it like self ones. + out.cross_tangled_elements + .clone_from(&detected.cross_tangled_elements); + out.cross_tangled_vertices + .clone_from(&detected.cross_tangled_vertices); + out.cross_tangled_vb_elements + .clone_from(&detected.cross_tangled_vb_elements); + out.cross_pairs.clone_from(&detected.cross_pairs); + if !is_self && !flipped { + out.exempt_cross_tangles(other_surface_handle); + } + let friction = crate::dynamics::CoefficientCombineRule::combine( + surface_co.friction(), + other_co.friction(), + surface_co.friction_combine_rule(), + other_co.friction_combine_rule(), + ); + let hard = recovery.overlap_skin_volume + || recovery.overlap_patch_constraints != SoftPatchConstraints::Keep; + if let Some(volume) = volume.filter(|_| { + !flipped + && (!recovery.overlap_skip_self_tangled || out.tangled_elements.is_empty()) + out.previous_vertex.clear(); + for c in &mesh.vertex_contacts { + if c.other == other_handle && c.flipped == flipped { + out.previous_vertex + .insert((c.vertex, c.element), (c.impulse, c.tangent_impulse)); + } + } + // Crossing repulsion (see `repel_constraint`): each crossing pair also gets constraints of + // its own (piercing element's vertices vs the pierced element), so an edge-first crossing + // with no vertex within reach is repelled too. `targets[f]` lists the elements `f` crosses. + let repel = params.soft_bodies.recovery.crossing_repulsion + && !vb_mesh.is_wire() + && !eb_mesh.is_wire() + && if is_self { + !out.tangled_vertices.is_empty() || !out.tangled_elements.is_empty() + } else { + !out.cross_tangled_vertices.is_empty() || !out.cross_tangled_elements.is_empty() + }; + // Narrow-phase results: the guiding volume normals (see `crossing_repulsion_guide`), the + // vertices inside the other side's volume patch (see `overlap_patch_constraints`) and, for + // a self pair, the vertices in their own body's self-intersection region. // boundary, the particles of the cell carrying it when it collides through a skin. // A candidate the contact-modification hook disabled gets no constraint. + // Crossing repulsion (see `repel_constraint`): a flagged feature's constraint repels toward // would pin the intruder inside; the volume rows and the elasticity + // A row touching a boundary crossing between the two surfaces may only + // expel, never hold: a keep-apart row there is wrong-sided by construction + } + } +} diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs index 5ce505119..14c6c7ea2 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs @@ -62,14 +62,65 @@ pub(crate) fn detect_edges( }; let tangled_elements: &[bool] = tangles.as_ref().map_or(&[], |t| t.tangled_elements); + // Crossings between the two surfaces: where they already pass through each other, edge + // constraints are wrong-sided and would freeze the crossing, so element pairs touching a + // crossing stand down. Mixed arities cover a wire through a surface. + let mut crossed_own: Vec = Vec::new(); + let mut crossed_other: Vec = Vec::new(); let other_inv_pose = other_co.position().inverse(); if !is_self && params.soft_bodies.recovery.cross_body_detection && params.soft_bodies.recovery.edge_stand_down { + for i in 0..mesh.indices().len() { + let element = mesh.element(i); + let pa = element_points(mesh, element); + let mut aabb = crate::geometry::Aabb::new_invalid(); + for p in &pa[..element.len()] { + aabb.take_point(*p); + } + let aabb = aabb.transform_by(&other_inv_pose); + for j in other_bvh.intersect_aabb(&aabb) { + let ej = other_mesh.element(j as usize); + let pb = element_points(other_mesh, ej); + if !elements_cross(&pa[..element.len()], &pb[..ej.len()]) { + continue; + } + if crossed_own.is_empty() { + crossed_own.resize(mesh.indices().len(), false); + crossed_other.resize(other_mesh.indices().len(), false); + } + crossed_own[i] = true; + crossed_other[j as usize] = true; + } + } + // The pair is recovery-owned: the crossing guard leaves it alone. + out.crossed = !crossed_own.is_empty(); + } + + // The candidate element pairs: every element of this surface against the other's BVH (in its + // cluster frame, so the world box is localized first), AABB grown by the contact reach; then + // their edge pairs. Chunked across threads for a large surface, candidates appended in order. let scan = EdgeScan { + sb, + mesh, + other, + other_mesh, + other_bvh, + other_inv_pose, + reach, + is_self, + tangled_elements, + crossed_own: &crossed_own, + crossed_other: &crossed_other, + speculation, + rest_gaps, + skins, + params, + step_dt, }; let n_e = mesh.indices().len(); #[cfg(feature = "parallel")] if n_e >= 2 * PARALLEL_CHUNK && rayon::current_num_threads() > 1 { + use rayon::prelude::*; let chunks: Vec> = (0..n_e.div_ceil(PARALLEL_CHUNK)) .into_par_iter() .map(|c| { @@ -77,22 +128,42 @@ pub(crate) fn detect_edges( for i in c * PARALLEL_CHUNK..((c + 1) * PARALLEL_CHUNK).min(n_e) { scan.scan_element(i, &mut candidates); } + candidates }) .collect(); for chunk in chunks { out.candidates.extend(chunk); } + return true; } for i in 0..n_e { scan.scan_element(i, &mut out.candidates); } + true } + +/// The read-only state of an edge pass's per-element enumeration (see [`EdgeScan::scan_element`]). struct EdgeScan<'a> { + sb: &'a SoftBody, + mesh: &'a SoftCollisionMesh, + other: &'a SoftBody, + other_mesh: &'a SoftCollisionMesh, + other_bvh: &'a parry::partitioning::Bvh, + other_inv_pose: crate::math::Pose, + reach: Real, + is_self: bool, tangled_elements: &'a [bool], crossed_own: &'a [bool], crossed_other: &'a [bool], + speculation: bool, + rest_gaps: bool, + skins: Real, + params: &'a crate::dynamics::IntegrationParameters, + step_dt: Real, } + impl EdgeScan<'_> { + /// The edge candidates of the element `i` against the other surface, appended to `out`. fn scan_element(&self, i: usize, out: &mut Vec) { let (mesh, other_mesh) = (self.mesh, self.other_mesh); let element = mesh.element(i); @@ -111,6 +182,20 @@ impl EdgeScan<'_> { if ei.iter().any(|v| ej.contains(v)) { continue; } + // Tangled elements (inverted cells, or part of a surface self-crossing): + // their self contacts stand down (see `detect_self_tangles`). + if self.tangled_elements.get(i as usize).copied().unwrap_or(false) + || self.tangled_elements.get(j as usize).copied().unwrap_or(false) + { + continue; + } + } else if self.crossed_own.get(i as usize).copied().unwrap_or(false) + || self.crossed_other.get(j as usize).copied().unwrap_or(false) + { + continue; + } + // The edges owned by each element (a segment is its own edge, in 2D and for a + // wire in 3D): a pair of edges is tested by exactly one element pair. for ea in self.mesh.element_edge_ids(i as usize) { if self.mesh.edge_owner(ea) != i { continue; diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs index 7d4be0c6b..3b657bfd5 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs @@ -6,6 +6,7 @@ use crate::alloc_prelude::*; use crate::dynamics::soft_body_crossing_tests::segments_cross; use crate::dynamics::{SoftBody, SoftCollisionMesh}; use crate::math::{DIM, Real, Vector}; +use crate::geometry::PointQueryWithLocation; use crate::utils::DotProduct; use parry::bounding_volume::BoundingVolume; #[cfg(not(feature = "std"))] @@ -57,11 +58,143 @@ pub(crate) fn element_points(mesh: &SoftCollisionMesh, element: &[u32]) -> [Vect points } - // each other, the pair's keep-apart rows are wrong-sided by construction and freeze - // opened), so they stand down around it, like the self rows do around a +/// An element's supporting plane: a point on it and its unit normal (`None` for a wire's +/// element or a degenerate one), from the vertex positions `vertex` reads. +pub(crate) fn element_plane( + eb_mesh: &SoftCollisionMesh, + element: usize, + vertex: impl Fn(usize) -> Vector, +) -> Option<(Vector, Vector)> { + let el = eb_mesh.element(element); + let p0 = vertex(el[0] as usize); + #[cfg(feature = "dim2")] + let n = { + let d = vertex(el[1] as usize) - p0; + Vector::new(-d.y, d.x) + }; + #[cfg(feature = "dim3")] + let n = { + if el.len() < 3 { + return None; + } + (vertex(el[1] as usize) - p0).cross(vertex(el[2] as usize) - p0) + }; + Some((p0, n.try_normalize()?)) +} + +/// The vertex-vs-surface candidates of the vertices of `vb` against the surface of `eb` +/// (`tangles`: the mesh's self-tangle signal for a self pass, `None` for a pair), with the +/// crossings between the two surfaces and the per-vertex crossing classification. +pub(crate) fn detect_vertex_pass( + out: &mut SoftVertexPass, + (eb, eb_mesh, surface_handle, eb_co): Side<'_>, + (vb, vb_mesh, vertices_handle, vb_co): Side<'_>, + tangles: Option>, + ctx: &SoftDetectionCtx, +) { + let params = ctx.params; + let step_dt = ctx.dt; + let is_self = tangles.is_some(); + out.clear(); + out.surface = surface_handle; + out.vertices_of = vertices_handle; + let Some(eb_bvh) = eb_co.shape().as_composite_shape().map(|c| c.bvh()) else { + return; + }; + // Crossings between the two surfaces: where the boundaries already pass through each other, + // the pair's keep-apart constraints are wrong-sided, so they stand down around the crossing, + // like the self constraints do. 3D wires sit out (curve-curve crossings have measure zero). + if !is_self && params.soft_bodies.recovery.cross_body_detection { + if let Some(vb_bvh) = vb_co.shape().as_composite_shape().map(|c| c.bvh()) { + let vb_inv_pose = vb_co.position().inverse(); + for e in 0..eb_mesh.indices().len() { + let ee = eb_mesh.element(e); + let pe = element_points(eb_mesh, ee); + let mut aabb = crate::geometry::Aabb::new_invalid(); + for p in &pe[..ee.len()] { + aabb.take_point(*p); + } + let aabb = aabb.transform_by(&vb_inv_pose); + for f in vb_bvh.intersect_aabb(&aabb) { + let ef = vb_mesh.element(f as usize); + let qf = element_points(vb_mesh, ef); + if !elements_cross(&pe[..ee.len()], &qf[..ef.len()]) { + continue; + } + if out.cross_tangled_elements.is_empty() { + out.cross_tangled_elements + .resize(eb_mesh.indices().len(), false); + out.cross_tangled_vertices + .resize(vb_mesh.vertex_count(), false); + } + out.cross_tangled_elements[e] = true; + for &v in ef { + out.cross_tangled_vertices[v as usize] = true; + } + { + if out.cross_tangled_vb_elements.is_empty() { + out.cross_tangled_vb_elements + .resize(vb_mesh.indices().len(), false); + } + out.cross_tangled_vb_elements[f as usize] = true; + let pair = (e as u32, f); + if out.cross_pairs.len() < 256 && !out.cross_pairs.contains(&pair) { + out.cross_pairs.push(pair); + } + } + } + } + } + } // The two contact skins (a soft surface's is its vertices' thickness; between two pieces of // one torn body, capped per pair by the gap at rest, see `rest_gap_skins`); the - // Crossing repulsion (see `repel_row`): the detected crossing pairs also get rows + // speculative reach grows with both bodies' motion over the step (see the narrow phase). + let skins = vb_co.contact_skin() + eb_co.contact_skin(); + let prediction = params.prediction_distance(); + let reach = prediction + skins + ctx.motion_margin(eb_co) + ctx.motion_margin(vb_co); + // A closed surface's reversed sightings mark a foreign vertex inside it. + let closed = eb_mesh.is_closed() && !is_self; + + // Crossing repulsion (see `repel_constraint`): the detected crossing pairs also get their own + // constraints (the piercing element's vertices against the pierced element), so an edge-first + // crossing with no vertex within reach is repelled too. `targets[f]`: elements crossed by `f`. + let (tangled_vertices, tangled_elements, crossings): (&[bool], &[bool], &[(u32, u32)]) = + match &tangles { + Some(t) => (t.tangled_vertices, t.tangled_elements, t.crossings), + None => (&[], &[], &[]), + }; + let repel = params.soft_bodies.recovery.crossing_repulsion + && !vb_mesh.is_wire() + && !eb_mesh.is_wire() + && if is_self { + !tangled_vertices.is_empty() || !tangled_elements.is_empty() + } else { + !out.cross_tangled_vertices.is_empty() || !out.cross_tangled_elements.is_empty() + }; + let (targets, vertex_elements): (Vec>, Vec>) = if repel { + let mut targets = vec![Vec::new(); vb_mesh.indices().len()]; + if is_self { + for &(i, j) in crossings { + targets[i as usize].push(j); + targets[j as usize].push(i); + } + } else { + for &(e, f) in &out.cross_pairs { + targets[f as usize].push(e); + } + } + let mut vertex_elements = vec![Vec::new(); vb_mesh.vertex_count()]; + for f in 0..vb_mesh.indices().len() { + for &v in vb_mesh.element(f) { + vertex_elements[v as usize].push(f as u32); + } + } + (targets, vertex_elements) + } else { + (Vec::new(), Vec::new()) + }; +} + // A tangled vertex (backed by inverted material, or part of a surface if len >= reach || len < 1.0e-6 { } let dist = len - skins; @@ -69,6 +202,8 @@ pub(crate) fn element_points(mesh: &SoftCollisionMesh, element: &[u32]) -> [Vect // `params.dt` is the substep): the rest of the reach is the bodies' motion // margin, and a resting vertex must not carry rows (warm-started) against every // a ghost of that internal feature (the neighbor carries the actual contact); + // Crossing repulsion: the crossing pairs' constraints come before the self.reach test + // of a piercing edge): a constraint on a far vertex is a long-range hold that // Deterministic order, and one constraint per surface vertex touched: the elements sharing // A foreign vertex that crossed a closed surface: seen from behind (reversed diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index a4c7c3016..1cb0fd0ca 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -599,6 +599,12 @@ fn soft_recovery_section(ui: &mut Ui, r: &mut rapier::dynamics::SoftRecoverySett ui.checkbox(&mut r.self_crossing_detection, "Self-crossings"); ui.checkbox(&mut r.detection_motion_gating, "Self-crossing sweep motion gating"); ui.checkbox(&mut r.cross_body_detection, "Cross-body crossings"); + ui.separator(); + ui.label("Passive stand-down"); + ui.checkbox(&mut r.self_stand_down, "Self stand-down"); + ui.checkbox(&mut r.cross_body_expel_gate, "Cross expel-only gate"); + ui.checkbox(&mut r.edge_stand_down, "Edge-pass stand-down"); + ui.checkbox(&mut r.crossing_repulsion, "Crossing repulsion (repel, not drop)"); if ui.button("Reset to defaults").clicked() { *r = Default::default(); } From a361f4cf688ed7a521407bc4f9b0b0b563c195bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 6 Sep 2026 18:12:20 +0200 Subject: [PATCH 09/32] feat(soft-body): intersection-volume constraints for crossed soft and rigid pairs, self-overlaps and the skin-volume contact model --- crates/rapier2d/tests/overlap_constraints.rs | 255 ++++++++ crates/rapier2d/tests/self_intersect_probe.rs | 16 + crates/rapier2d/tests/soft_stack_probe.rs | 12 + crates/rapier2d/tests/volume_contact.rs | 279 +++++++++ crates/rapier3d/tests/overlap_constraints3.rs | 108 ++++ crates/rapier3d/tests/soft_stack_probe3.rs | 12 + .../collision_mesh/collision_mesh.rs | 9 + .../collision_mesh/collision_mesh_geometry.rs | 9 + .../collision_mesh/collision_mesh_topology.rs | 9 + src/dynamics/soft_body/mod.rs | 10 +- src/dynamics/soft_body/soft_body_contacts.rs | 130 ++++ .../soft_body/soft_recovery_settings.rs | 120 +++- .../soft_constraint_solve.rs | 139 +++++ .../soft_constraints_set/soft_constraints.rs | 44 ++ .../soft_constraints_set.rs | 10 + .../soft_contact_assembly.rs | 42 ++ .../soft_contact_assembly_body_contacts.rs | 312 +++++++++- .../soft_contact_assembly_edge_contacts.rs | 17 + .../soft_contact_assembly_vertex_contacts.rs | 341 ++++++++++- ...oft_contact_assembly_volume_constraints.rs | 363 ++++++++++++ .../soft_contact_assembly_workspace.rs | 52 ++ .../soft_contact_assembly_writeback.rs | 68 +++ .../soft_element_constraint_assembly.rs | 4 + .../solver/staged_island_solver/solve.rs | 33 +- .../soft_contacts/soft_contacts_classify.rs | 364 ++++++++++++ .../soft_contacts/soft_contacts_pairs.rs | 155 +++++ .../soft_contacts_vertex_pass.rs | 58 ++ .../soft_contacts/soft_contacts_volume.rs | 559 ++++++++++++++++++ .../soft_contacts/soft_self_contacts.rs | 136 +++++ .../debug_render_pipeline.rs | 43 +- .../debug_render_style.rs | 6 + src_testbed/ui.rs | 48 ++ 32 files changed, 3737 insertions(+), 26 deletions(-) create mode 100644 crates/rapier2d/tests/overlap_constraints.rs create mode 100644 crates/rapier2d/tests/volume_contact.rs create mode 100644 crates/rapier3d/tests/overlap_constraints3.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs create mode 100644 src/geometry/narrow_phase/soft_contacts/soft_contacts_classify.rs create mode 100644 src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs create mode 100644 src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs diff --git a/crates/rapier2d/tests/overlap_constraints.rs b/crates/rapier2d/tests/overlap_constraints.rs new file mode 100644 index 000000000..b90470427 --- /dev/null +++ b/crates/rapier2d/tests/overlap_constraints.rs @@ -0,0 +1,255 @@ +//! Intersection-volume constraints (see docs/soft-depenetration-literature-plan.md, phase 3): +//! the part of a closed boundary inside another, or a self-crossed loop's mirrored lobe, +//! retracts along its own area gradient. Every other active recovery is off in these scenes. + +use rapier2d::prelude::*; + +fn segments_cross(a0: Vector, a1: Vector, b0: Vector, b1: Vector) -> bool { + let d1 = a1 - a0; + let d2 = b1 - b0; + let denom = d1.perp_dot(d2); + if denom.abs() < 1.0e-12 { + return false; + } + let t = (b0 - a0).perp_dot(d2) / denom; + let u = (b0 - a0).perp_dot(d1) / denom; + (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) +} + +fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { + let mut parity = false; + for e in mesh.indices() { + let (p0, p1) = ( + mesh.vertex(body, e[0] as usize), + mesh.vertex(body, e[1] as usize), + ); + if (p0.y > point.y) != (p1.y > point.y) { + let t = (point.y - p0.y) / (p1.y - p0.y); + if p0.x + t * (p1.x - p0.x) > point.x { + parity = !parity; + } + } + } + parity +} + +fn overlap(world: &PhysicsWorld, a: SoftBodyHandle, b: SoftBodyHandle) -> (usize, usize, usize) { + let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); + let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); + let pa = |v: u32| ma.vertex(sa, v as usize); + let pb = |v: u32| mb.vertex(sb, v as usize); + let mut crossings = 0; + for ea in ma.indices() { + for eb in mb.indices() { + if segments_cross(pa(ea[0]), pa(ea[1]), pb(eb[0]), pb(eb[1])) { + crossings += 1; + } + } + } + let a_in_b = (0..ma.vertex_count()) + .filter(|&v| inside(pa(v as u32), mb, sb)) + .count(); + let b_in_a = (0..mb.vertex_count()) + .filter(|&v| inside(pb(v as u32), ma, sa)) + .count(); + (crossings, a_in_b, b_in_a) +} + +fn self_crossings(world: &PhysicsWorld, h: SoftBodyHandle) -> usize { + let sb = &world.soft_bodies[h]; + let mesh = sb.meshes().next().unwrap(); + let idx = mesh.indices(); + let p = |v: u32| mesh.vertex(sb, v as usize); + let mut crossings = 0; + for i in 0..idx.len() { + for j in i + 1..idx.len() { + let (ei, ej) = (idx[i], idx[j]); + if ei.iter().any(|v| ej.contains(v)) { + continue; + } + if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { + crossings += 1; + } + } + } + crossings +} + +/// Overlap constraints on, every other active recovery off (`SOFT_DEPEN_PRESET=defaults` keeps +/// the defaults for A/B runs). +fn overlap_only(world: &mut PhysicsWorld) { + if std::env::var("SOFT_DEPEN_PRESET").as_deref() == Ok("defaults") { + return; + } + // The reference settings: prevention without edge speculation, detection and the + // stand-down, the constraints on. + let r = &mut world.integration_parameters.soft_bodies.recovery; + r.overlap_constraints = true; + r.edge_speculation = false; + // Diagnostic: the closed-closed edge constraints off, or the overlap constraints off (the reference + // settings minus the constraints). + if std::env::var("SOFT_DEPEN_NO_EDGE").is_ok() { + r.edge_speculation = false; + } + if std::env::var("SOFT_OVERLAP_OFF").is_ok() { + r.overlap_constraints = false; + } + if let Some(pace) = std::env::var("SOFT_OVERLAP_CONSTRAINT_PACE").ok().and_then(|v| v.parse().ok()) { + r.overlap_constraint_pace = pace; + } +} + +fn floor(world: &mut PhysicsWorld) { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(6.0, 0.5), + ); +} + +fn blob(center: Vector, radius: Real, n: usize) -> SoftBodyBuilder { + SoftBodyBuilder::disk(center, radius, n) + .softness(SpringCoefficients::new(4.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) +} + +fn make_gerono_eight(world: &mut PhysicsWorld, h: SoftBodyHandle, center: Vector, r: Real) { + let n = world.soft_bodies[h].particles().len(); + for i in 0..n { + let t = core::f32::consts::TAU as Real * i as Real / n as Real; + let p = center + Vector::new(r * t.cos(), r * t.sin() * t.cos()); + world.soft_bodies[h].set_particle_position(i, p); + } +} + +/// Two cell-less blobs inserted overlapping by a third of a radius: each one's intruding +/// arc retracts into its own body, and the pair de-overlaps with no expulsion. +#[test] +fn overlapping_blobs_deoverlap() { + let mut world = PhysicsWorld::new(); + floor(&mut world); + let a = world.insert_soft_body(blob(Vector::new(-0.42, 0.6), 0.6, 24)); + let b = world.insert_soft_body(blob(Vector::new(0.42, 0.6), 0.6, 24)); + overlap_only(&mut world); + for step in 0..=900 { + if step % 100 == 0 { + let (c, ab, ba) = overlap(&world, a, b); + println!("step {step:4}: crossings={c} a_in_b={ab} b_in_a={ba}"); + } + world.step(); + } + let (c, ab, ba) = overlap(&world, a, b); + assert_eq!((c, ab, ba), (0, 0, 0), "the blobs still overlap"); +} + +/// Checks that a balanced figure-eight's mirrored lobe deflates along its area gradient until +/// the crossing annihilates and the loop re-inflates. Ignored: without the active untangler's +/// endgame pull the balanced eight stalls as a sliver crossing. +#[test] +#[ignore] +fn balanced_eight_recovers() { + let mut world = PhysicsWorld::new(); + floor(&mut world); + let a = world.insert_soft_body(blob(Vector::new(0.0, 0.65), 0.6, 24)); + make_gerono_eight(&mut world, a, Vector::new(0.0, 0.65), 0.6); + overlap_only(&mut world); + for step in 0..=1200 { + if step % 100 == 0 { + println!( + "step {step:4}: crossings={} area={:.3}", + self_crossings(&world, a), + world.soft_bodies[a].volume() + ); + } + world.step(); + } + assert_eq!( + self_crossings(&world, a), + 0, + "the balanced eight never recovered" + ); + let area = world.soft_bodies[a].volume(); + let rest = world.soft_bodies[a].rest_volume(); + assert!( + area > 0.8 * rest, + "the loop did not re-inflate: {area:.3} of {rest:.3}" + ); +} + +/// A dynamic rigid box inserted inside a hollow blob, with expulsion off: the blob's patch +/// inside the box and the box itself share one constraint, and the pair separates without the box +/// being thrown (a box, not a ball: a ball keeps rolling with whatever it was handed). +#[test] +fn rigid_ball_inside_blob_separates() { + let mut world = PhysicsWorld::new(); + floor(&mut world); + let a = world.insert_soft_body(blob(Vector::new(0.0, 0.65), 0.6, 24)); + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.3, 0.65)), + ColliderBuilder::cuboid(0.3, 0.3).density(1.0), + ); + overlap_only(&mut world); + let inside_count = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[a]; + let c = world.bodies[ball].center_of_mass(); + sb.particle_positions() + .filter(|p| (p.x - c.x).abs() < 0.3 && (p.y - c.y).abs() < 0.3) + .count() + }; + for step in 0..=600 { + if step % 100 == 0 { + println!( + "step {step:4}: vertices inside ball={} ball=({:+.2},{:+.2}) blob=({:+.2},{:+.2})", + inside_count(&world), + world.bodies[ball].center_of_mass().x, + world.bodies[ball].center_of_mass().y, + world.soft_bodies[a].center_of_mass().x, + world.soft_bodies[a].center_of_mass().y, + ); + } + world.step(); + } + assert_eq!( + inside_count(&world), + 0, + "the blob still has vertices inside the ball" + ); + let speed = world.bodies[ball].linvel().length(); + assert!(speed < 1.0, "the ball was thrown: {speed} u/s"); +} + +/// A hollow blob dropped onto a thin fixed pin that ends up inside it (the pile's impaled +/// blobs): the rigid overlap constraint lifts the blob off the pin, expulsion off. +#[test] +fn impaled_blob_recovers() { + let mut world = PhysicsWorld::new(); + floor(&mut world); + let pin_friction: Real = std::env::var("SOFT_PIN_FRICTION") + .ok() + .and_then(|v| v.parse().ok()) + .unwrap_or(0.5); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, 1.5)), + ColliderBuilder::capsule_x(0.6, 0.1).friction(pin_friction), + ); + let a = world.insert_soft_body(blob(Vector::new(0.0, 1.5), 0.6, 24)); + overlap_only(&mut world); + let pin_inside = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[a]; + let mesh = sb.meshes().next().unwrap(); + inside(Vector::new(0.0, 1.5), mesh, sb) + }; + println!("start: pin inside={}", pin_inside(&world)); + for step in 0..=600 { + if step % 100 == 0 { + println!( + "step {step:4}: pin inside={} blob=({:+.2},{:+.2})", + pin_inside(&world), + world.soft_bodies[a].center_of_mass().x, + world.soft_bodies[a].center_of_mass().y, + ); + } + world.step(); + } + assert!(!pin_inside(&world), "the pin is still inside the blob"); +} diff --git a/crates/rapier2d/tests/self_intersect_probe.rs b/crates/rapier2d/tests/self_intersect_probe.rs index e2240adcf..06503f846 100644 --- a/crates/rapier2d/tests/self_intersect_probe.rs +++ b/crates/rapier2d/tests/self_intersect_probe.rs @@ -365,6 +365,22 @@ fn recovery_toggles_are_wired() { cross_body_expel_gate: false, edge_stand_down: false, crossing_repulsion: false, + crossing_repulsion_guide: false, + recovery_pace: 0.5, + overlap_constraints: false, + overlap_rigid: true, + overlap_skip_self_tangled: true, + overlap_edge_stand_down: true, + overlap_constraint_pace: 1.0, + overlap_patch_constraints: SoftPatchConstraints::Keep, + overlap_skin_volume: false, + overlap_kept_depth: 0.0, + overlap_normal_push: false, + overlap_self_regions: false, + overlap_multi_volume: false, + overlap_split: 3, + overlap_patience: 240, + overlap_progress_margin: 0.02, }; for _ in 0..600 { world.step(); diff --git a/crates/rapier2d/tests/soft_stack_probe.rs b/crates/rapier2d/tests/soft_stack_probe.rs index 2193c6b0c..69a0b0c0f 100644 --- a/crates/rapier2d/tests/soft_stack_probe.rs +++ b/crates/rapier2d/tests/soft_stack_probe.rs @@ -146,19 +146,31 @@ fn apply_preset(world: &mut PhysicsWorld) -> String { let preset = std::env::var("SOFT_STACK_PRESET").unwrap_or_else(|_| "defaults".into()); let r = &mut world.integration_parameters.soft_bodies.recovery; *r = Default::default(); + // Per-feature policy of the volume-patch constraints (`SoftPatchConstraints`) on any preset: + // `SOFT_PATCH_CONSTRAINTS=stand|normal`. + match std::env::var("SOFT_PATCH_CONSTRAINTS").as_deref() { + Ok("stand") => r.overlap_patch_constraints = SoftPatchConstraints::StandDown, + Ok("normal") => r.overlap_patch_constraints = SoftPatchConstraints::AlongNormal, + _ => {} + } match preset.as_str() { "defaults" => {} // The user's reference settings (2026-09-04): prevention without edge speculation, // detection + stand-down, the intersection-volume constraints on. "reference" => { r.edge_speculation = false; + r.overlap_constraints = true; } // The reference settings with the crossing repulsion, unguided or guided by the // pair's (and the fold's) volume normal (see `crossing_repulsion_guide`). "reference+repel" | "reference+repel-guide" => { r.edge_speculation = false; + r.overlap_constraints = true; r.crossing_repulsion = true; + r.crossing_repulsion_guide = preset.ends_with("-guide"); } + // The intersection-volume constraints on the defaults. + "overlap" => r.overlap_constraints = true, other => panic!("unknown SOFT_STACK_PRESET `{other}`"), } preset diff --git a/crates/rapier2d/tests/volume_contact.rs b/crates/rapier2d/tests/volume_contact.rs new file mode 100644 index 000000000..f86f7eeed --- /dev/null +++ b/crates/rapier2d/tests/volume_contact.rs @@ -0,0 +1,279 @@ +//! The intersection-volume constraints as contacts (`SoftRecoverySettings::overlap_constraints`) +//! under the reference settings (`SOFT_VOLUME_MONO`: no multi-volume grid, `SOFT_NORMAL_PUSH`: +//! push along the normal): patch policies, self-guided repulsion, self-region overlaps. + +use rapier2d::prelude::*; + +fn segments_cross(a0: Vector, a1: Vector, b0: Vector, b1: Vector) -> bool { + let d1 = a1 - a0; + let d2 = b1 - b0; + let denom = d1.perp_dot(d2); + if denom.abs() < 1.0e-12 { + return false; + } + let t = (b0 - a0).perp_dot(d2) / denom; + let u = (b0 - a0).perp_dot(d1) / denom; + (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) +} + +fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { + let mut parity = false; + for e in mesh.indices() { + let (p0, p1) = ( + mesh.vertex(body, e[0] as usize), + mesh.vertex(body, e[1] as usize), + ); + if (p0.y > point.y) != (p1.y > point.y) { + let t = (point.y - p0.y) / (p1.y - p0.y); + if p0.x + t * (p1.x - p0.x) > point.x { + parity = !parity; + } + } + } + parity +} + +fn overlap(world: &PhysicsWorld, a: SoftBodyHandle, b: SoftBodyHandle) -> (usize, usize, usize) { + let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); + let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); + let pa = |v: u32| ma.vertex(sa, v as usize); + let pb = |v: u32| mb.vertex(sb, v as usize); + let mut crossings = 0; + for ea in ma.indices() { + for eb in mb.indices() { + if segments_cross(pa(ea[0]), pa(ea[1]), pb(eb[0]), pb(eb[1])) { + crossings += 1; + } + } + } + let a_in_b = (0..ma.vertex_count()) + .filter(|&v| inside(pa(v as u32), mb, sb)) + .count(); + let b_in_a = (0..mb.vertex_count()) + .filter(|&v| inside(pb(v as u32), ma, sa)) + .count(); + (crossings, a_in_b, b_in_a) +} + +fn max_speed(world: &PhysicsWorld, h: SoftBodyHandle) -> Real { + world.soft_bodies[h] + .particle_velocities() + .map(|v| v.length()) + .fold(0.0, Real::max) +} + +/// The reference settings with the crossing repulsion guides and the volume constraints on (see +/// the module docs). +fn configure(world: &mut PhysicsWorld) { + let r = &mut world.integration_parameters.soft_bodies.recovery; + r.overlap_constraints = true; + r.edge_speculation = false; + // The mono-volume variant (a single multiplier per pair: blocks tip on slopes). + if std::env::var("SOFT_VOLUME_MONO").is_ok() { + r.overlap_multi_volume = false; + } + if std::env::var("SOFT_NORMAL_PUSH").is_ok() { + r.overlap_normal_push = true; + } +} + +fn floor(world: &mut PhysicsWorld) { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(6.0, 0.5).friction(0.6), + ); +} + +/// Steps to run (`SOFT_STEPS` shortens a scene for tracing). +fn steps() -> usize { + std::env::var("SOFT_STEPS") + .ok() + .and_then(|v| v.parse().ok()) + .unwrap_or(900) +} + +/// The stack suite's blob: a cell-less loop stiff enough to support another. +fn blob(center: Vector, radius: Real, n: usize) -> SoftBodyBuilder { + SoftBodyBuilder::disk(center, radius, n) + .softness(SpringCoefficients::new(20.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) +} + +/// The stack suite's jelly square: a solid corotational grid (its flat faces neither roll +/// nor tip, unlike a blob). +fn jelly(center: Vector, half: Real) -> SoftBodyBuilder { + SoftBodyBuilder::grid(center, Vector::splat(half), 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 3.0e3, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .self_contacts(true) + .particle_mass(0.1) + .particle_radius(0.08) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) +} + +/// A blob on the floor with two bottom vertices tunneled through it, manifold constraints kept: +/// the volume constraint pulls the vertices back while the manifold constraints of the same +/// features may fight it. Every `overlap_patch_constraints` policy must recover the vertices. +#[test] +fn patch_constraints_tunneled_vertices_recover() { + let run = |policy: SoftPatchConstraints| -> (usize, Real) { + let mut world = PhysicsWorld::new(); + floor(&mut world); + let a = world.insert_soft_body(blob(Vector::new(0.0, 0.56), 0.5, 24)); + // Vertices 17, 18, 19 are the bottom of the loop (angles around 270 degrees). + for v in [17usize, 18, 19] { + let p = world.soft_bodies[a].particle_position(v); + world.soft_bodies[a].set_particle_position(v, Vector::new(p.x, p.y - 0.2)); + } + configure(&mut world); + world + .integration_parameters + .soft_bodies + .recovery + .overlap_patch_constraints = policy; + let below = |world: &PhysicsWorld| { + world.soft_bodies[a] + .particle_positions() + .filter(|p| p.y < 0.0) + .count() + }; + for step in 0..=steps() { + if step % 100 == 0 { + println!( + "{policy:?} step {step:4}: below floor={} speed={:.4}", + below(&world), + max_speed(&world, a) + ); + } + world.step(); + } + (below(&world), max_speed(&world, a)) + }; + let mut failed = Vec::new(); + for policy in [ + SoftPatchConstraints::Keep, + SoftPatchConstraints::StandDown, + SoftPatchConstraints::AlongNormal, + ] { + let (below, speed) = run(policy); + println!("{policy:?}: below floor {below}, speed {speed:.4}"); + if below != 0 { + failed.push(policy); + } + } + assert!( + failed.is_empty(), + "{failed:?} left vertices under the floor" + ); +} + +/// A concave "U" whose legs are pressed into each other (a self-contact between distinct regions +/// of one surface): the self-region constraints de-overlap the legs like a pair of bodies. +#[test] +fn self_regions_deoverlap_legs() { + let run = |regions: bool| -> usize { + let mut world = PhysicsWorld::new(); + floor(&mut world); + // A U, counter-clockwise, densified to 0.2 segments. + let corners = [ + Vector::new(0.0, 0.0), + Vector::new(1.4, 0.0), + Vector::new(1.4, 1.2), + Vector::new(1.0, 1.2), + Vector::new(1.0, 0.4), + Vector::new(0.4, 0.4), + Vector::new(0.4, 1.2), + Vector::new(0.0, 1.2), + ]; + let mut points = Vec::new(); + for i in 0..corners.len() { + let (a, b) = (corners[i], corners[(i + 1) % corners.len()]); + let n = ((b - a).length() / 0.2).round().max(1.0) as usize; + for k in 0..n { + points.push(a + (b - a) * (k as Real / n as Real)); + } + } + let lift = Vector::new(-0.7, 0.1); + let points: Vec = points.iter().map(|p| *p + lift).collect(); + let h = world.insert_soft_body( + SoftBodyBuilder::polygon(points.clone()) + .softness(SpringCoefficients::new(20.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)), + ); + // The right leg pushed into the left one (their gap is 0.6): by 0.15 by default, + // `SOFT_LEG_PUSH` sets the shift. + let shift: Real = std::env::var("SOFT_LEG_PUSH") + .ok() + .and_then(|v| v.parse().ok()) + .unwrap_or(0.75); + for (i, p) in points.iter().enumerate() { + if p.x - lift.x > 0.9 && p.y - lift.y > 0.4 { + world.soft_bodies[h].set_particle_position(i, *p - Vector::new(shift, 0.0)); + } + } + configure(&mut world); + world + .integration_parameters + .soft_bodies + .recovery + .overlap_self_regions = regions; + world.step(); + let constraints = world.soft_bodies[h].volume_contacts().count(); + println!("regions {regions}: {constraints} volume constraints after the first step"); + assert_eq!( + constraints > 0, + regions, + "the self-region constraints are {}", + if regions { "missing" } else { "unexpected" } + ); + let self_crossings = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[h]; + let mesh = sb.meshes().next().unwrap(); + let idx = mesh.indices(); + let p = |v: u32| mesh.vertex(sb, v as usize); + let mut crossings = 0; + for i in 0..idx.len() { + for j in i + 1..idx.len() { + let (ei, ej) = (idx[i], idx[j]); + if ei.iter().any(|v| ej.contains(v)) { + continue; + } + if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { + crossings += 1; + } + } + } + crossings + }; + for step in 0..=steps() { + if step % 100 == 0 { + println!( + "regions {regions} step {step:4}: crossings={} area={:+.3} speed={:.3}", + self_crossings(&world), + world.soft_bodies[h].volume(), + max_speed(&world, h) + ); + } + world.step(); + } + self_crossings(&world) + }; + let plain = run(false); + let regions = run(true); + println!("end crossings: plain {plain}, regions {regions}"); + assert_eq!( + regions, 0, + "the self-region constraints never de-overlapped the legs" + ); +} diff --git a/crates/rapier3d/tests/overlap_constraints3.rs b/crates/rapier3d/tests/overlap_constraints3.rs new file mode 100644 index 000000000..296113d9b --- /dev/null +++ b/crates/rapier3d/tests/overlap_constraints3.rs @@ -0,0 +1,108 @@ +//! 3D intersection-volume constraints (see the 2D `overlap_constraints`): two closed cell-less balloons +//! inserted overlapping de-overlap, each retracting its own intruding patch. + +use rapier3d::prelude::*; + +fn segment_crosses_triangle(p: Vector, q: Vector, tri: &[Vector; 3]) -> bool { + let n = (tri[1] - tri[0]).cross(tri[2] - tri[0]); + let dp = (p - tri[0]).dot(n); + let dq = (q - tri[0]).dot(n); + if dp * dq >= 0.0 { + return false; + } + let pq = q - p; + let d = [ + pq.dot((tri[0] - p).cross(tri[1] - p)), + pq.dot((tri[1] - p).cross(tri[2] - p)), + pq.dot((tri[2] - p).cross(tri[0] - p)), + ]; + d.iter().all(|x| *x >= 0.0) || d.iter().all(|x| *x <= 0.0) +} + +fn triangles_cross(a: &[Vector; 3], b: &[Vector; 3]) -> bool { + (0..3).any(|k| segment_crosses_triangle(a[k], a[(k + 1) % 3], b)) + || (0..3).any(|k| segment_crosses_triangle(b[k], b[(k + 1) % 3], a)) +} + +fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { + let q = point + Vector::new(0.5341, 0.6432, 0.5487) * 100.0; + let mut crossings = 0; + for e in mesh.indices() { + let tri: [Vector; 3] = core::array::from_fn(|k| mesh.vertex(body, e[k] as usize)); + if segment_crosses_triangle(point, q, &tri) { + crossings += 1; + } + } + crossings % 2 == 1 +} + +fn overlap(world: &PhysicsWorld, a: SoftBodyHandle, b: SoftBodyHandle) -> (usize, usize, usize) { + let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); + let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); + let tris = |sb: &SoftBody, mesh: &SoftCollisionMesh| -> Vec<[Vector; 3]> { + mesh.indices() + .iter() + .map(|e| core::array::from_fn(|k| mesh.vertex(sb, e[k] as usize))) + .collect() + }; + let (ta, tb) = (tris(sa, ma), tris(sb, mb)); + let mut crossings = 0; + for x in &ta { + for y in &tb { + if triangles_cross(x, y) { + crossings += 1; + } + } + } + let a_in_b = (0..ma.vertex_count()) + .filter(|&v| inside(ma.vertex(sa, v), mb, sb)) + .count(); + let b_in_a = (0..mb.vertex_count()) + .filter(|&v| inside(mb.vertex(sb, v), ma, sa)) + .count(); + (crossings, a_in_b, b_in_a) +} + +fn overlap_only(world: &mut PhysicsWorld) { + if std::env::var("SOFT_DEPEN_PRESET").as_deref() == Ok("defaults") { + return; + } + // The reference settings: prevention without edge speculation, detection and the + // stand-down, the constraints on. + let r = &mut world.integration_parameters.soft_bodies.recovery; + r.overlap_constraints = true; + r.edge_speculation = false; + // Diagnostic: the closed-closed edge constraints off. + if std::env::var("SOFT_DEPEN_NO_EDGE").is_ok() { + r.edge_speculation = false; + } +} + +fn balloon(center: Vector, radius: Real) -> SoftBodyBuilder { + SoftBodyBuilder::sphere(center, radius, 1) + .softness(SpringCoefficients::new(20.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) +} + +#[test] +fn overlapping_balloons_deoverlap() { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), + ColliderBuilder::cuboid(6.0, 0.5, 6.0), + ); + let a = world.insert_soft_body(balloon(Vector::new(-0.35, 0.55, 0.0), 0.5)); + let b = world.insert_soft_body(balloon(Vector::new(0.35, 0.55, 0.0), 0.5)); + overlap_only(&mut world); + for step in 0..=900 { + if step % 100 == 0 { + let (c, ab, ba) = overlap(&world, a, b); + println!("step {step:4}: crossings={c} a_in_b={ab} b_in_a={ba}"); + } + world.step(); + } + let (c, ab, ba) = overlap(&world, a, b); + assert_eq!((c, ab, ba), (0, 0, 0), "the balloons still overlap"); +} diff --git a/crates/rapier3d/tests/soft_stack_probe3.rs b/crates/rapier3d/tests/soft_stack_probe3.rs index 451613312..85016a152 100644 --- a/crates/rapier3d/tests/soft_stack_probe3.rs +++ b/crates/rapier3d/tests/soft_stack_probe3.rs @@ -143,19 +143,31 @@ fn apply_preset(world: &mut PhysicsWorld) -> String { let preset = std::env::var("SOFT_STACK_PRESET").unwrap_or_else(|_| "defaults".into()); let r = &mut world.integration_parameters.soft_bodies.recovery; *r = Default::default(); + // Per-feature policy of the volume-patch constraints (`SoftPatchConstraints`), on any preset: + // `SOFT_PATCH_CONSTRAINTS=stand|normal`. + match std::env::var("SOFT_PATCH_CONSTRAINTS").as_deref() { + Ok("stand") => r.overlap_patch_constraints = SoftPatchConstraints::StandDown, + Ok("normal") => r.overlap_patch_constraints = SoftPatchConstraints::AlongNormal, + _ => {} + } match preset.as_str() { "defaults" => {} // The user's reference settings (2026-09-04): prevention without edge speculation, // detection + stand-down, the intersection-volume constraints on. "reference" => { r.edge_speculation = false; + r.overlap_constraints = true; } // The reference settings with the crossing repulsion, unguided or guided by the // pair's (and the fold's) volume normal (see `crossing_repulsion_guide`). "reference+repel" | "reference+repel-guide" => { r.edge_speculation = false; + r.overlap_constraints = true; r.crossing_repulsion = true; + r.crossing_repulsion_guide = preset.ends_with("-guide"); } + // The intersection-volume constraints on the defaults. + "overlap" => r.overlap_constraints = true, other => panic!("unknown SOFT_STACK_PRESET `{other}`"), } preset diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh.rs index f69ec6406..09a36d9d4 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh.rs @@ -128,6 +128,15 @@ pub struct SoftCollisionMesh { /// Vertex-vs-surface contacts of the last step against this mesh (self contacts and the /// other soft bodies' vertices): warm-start state. pub(crate) vertex_contacts: Vec, + /// The overlap corrections' progress per pair (see `SoftOverlapState`). + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) overlap_states: Vec, + /// The volume contacts' warm impulses (see `SoftOverlapWarm`). + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) overlap_warm: Vec, + /// The volume constraints assembled at the last step (see `SoftVolumeContact`). + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) volume_contacts: Vec, /// Surface travel accumulated since the last self-crossing sweep (the sweep is skipped /// while this cannot have bridged half a skin). #[cfg_attr(feature = "serde-serialize", serde(skip))] diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs index f9ef14e6c..464518e15 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs @@ -215,4 +215,13 @@ impl SoftCollisionMesh { }) } + /// The sign turning this mesh's raw winding into its geometric orientation: `-1.0` for + /// a mesh oriented inward at rest or turned inside out (see `element_outward_normal`). + pub(crate) fn winding_sign(&self) -> Real { + if (self.rest_signed_volume < 0.0) != self.inverted { + -1.0 + } else { + 1.0 + } + } } diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs index 4ed76d1a3..f021bd38f 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs @@ -62,6 +62,9 @@ impl SoftCollisionMesh { self_contacts, edge_contacts: Vec::new(), vertex_contacts: Vec::new(), + overlap_states: Vec::new(), + overlap_warm: Vec::new(), + volume_contacts: Vec::new(), crossing_sweep_travel: Real::MAX, crossed_partners: Vec::new(), vertex_cache: Vec::new(), @@ -127,6 +130,9 @@ impl SoftCollisionMesh { self_contacts, edge_contacts: Vec::new(), vertex_contacts: Vec::new(), + overlap_states: Vec::new(), + overlap_warm: Vec::new(), + volume_contacts: Vec::new(), crossing_sweep_travel: Real::MAX, crossed_partners: Vec::new(), vertex_cache: Vec::new(), @@ -205,6 +211,9 @@ impl SoftCollisionMesh { self_contacts, edge_contacts: Vec::new(), vertex_contacts: Vec::new(), + overlap_states: Vec::new(), + overlap_warm: Vec::new(), + volume_contacts: Vec::new(), crossing_sweep_travel: Real::MAX, crossed_partners: Vec::new(), vertex_cache: Vec::new(), diff --git a/src/dynamics/soft_body/mod.rs b/src/dynamics/soft_body/mod.rs index 6fee5b4e0..e05965c16 100644 --- a/src/dynamics/soft_body/mod.rs +++ b/src/dynamics/soft_body/mod.rs @@ -12,6 +12,7 @@ pub use self::soft_body_elements::SoftBodyDihedral; #[cfg(feature = "fem")] pub use self::soft_body_elements::SoftBodySolver; pub use self::soft_body::{SOFT_BODY_MAX_CONSTRAINT_PARTICLES, SoftBody}; +pub use self::soft_body_contacts::SoftVolumeContact; pub use self::soft_body_elements::{ SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyEdgeKind, SoftBodyParticle, }; @@ -23,11 +24,10 @@ pub use self::soft_body_set::SoftBodySet; pub use self::soft_body_plasticity::SoftEdgePlasticFlow; pub(crate) use self::soft_body::SOFT_BODY_OVERFLOW_COLOR; -pub(crate) use self::soft_body_contacts::{SoftEdgeContact, SoftVertexContact}; -#[allow(unused_imports)] -use self::soft_body_elements::{CELL_IMPULSES}; -#[allow(unused_imports)] -use self::soft_body_material::{default_true, one}; +pub(crate) use self::soft_body_contacts::{ + SoftEdgeContact, SoftOverlapState, SoftOverlapWarm, SoftVertexContact, +}; +use self::soft_body_elements::CELL_IMPULSES; mod soft_recovery_settings; pub use soft_recovery_settings::{SoftPatchConstraints, SoftRecoverySettings}; mod soft_body_settings; diff --git a/src/dynamics/soft_body/soft_body_contacts.rs b/src/dynamics/soft_body/soft_body_contacts.rs index f3a137366..e7947161a 100644 --- a/src/dynamics/soft_body/soft_body_contacts.rs +++ b/src/dynamics/soft_body/soft_body_contacts.rs @@ -37,6 +37,136 @@ pub(crate) struct SoftVertexContact { pub tangent_impulse: Vector, } +/// The progress of one pair's overlap correction across steps: a pair whose volume estimate stops +/// improving stands its positional correction down after `SoftRecoverySettings::overlap_patience` +/// steps (a static compression) and re-arms when the estimate drops by `overlap_progress_margin`. +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) struct SoftOverlapState { + /// The other collider of the pair. + pub other: crate::geometry::ColliderHandle, + /// Lowest volume estimate reached so far. + pub best_error: Real, + /// Steps since the estimate last improved by the margin. + pub steps_stuck: u16, + /// Whether the correction has stood down (the patience ran out): re-arming then takes + /// a clearly better estimate, not the noise of a pair squeezed against what holds it. + pub stalled: bool, +} + +/// The impulses a volume contact accumulated in the last step, per pair and per particle (see +/// `SoftOverlapConstraint`), plus the rigid side's: the hard constraints warm start from them +/// before the elastic constraints of the first substep, so resting springs see the contact force. +#[derive(Clone, Debug)] +pub(crate) struct SoftOverlapWarm { + pub other: crate::geometry::ColliderHandle, + /// The owner body's particles and their impulses. + pub own: Vec<(u32, Vector)>, + /// The other soft body's particles and their impulses. + pub other_soft: Vec<(u32, Vector)>, + /// The rigid side's linear and angular impulse. + pub rigid: (Vector, AngVector), +} + +/// One volume constraint assembled at the last step (a bin of a pair's volume contact, see +/// `SoftOverlapConstraint`), kept for debug rendering: center, normal (from the owner body into the +/// other side), signed volume estimate, and every entry's position and volume gradient. +#[derive(Clone, Debug)] +pub struct SoftVolumeContact { + /// The area-weighted center of the constraint's patch. + pub center: Vector, + /// The contact normal, from the owner body into the other side. + pub normal: Vector, + /// The signed volume estimate (negative: speculative slack). + pub volume: Real, + /// The entries' positions and volume gradients. + pub gradients: Vec<(Vector, Vector)>, +} + +impl SoftBody { + /// The world position of the `i`-th vertex of `mesh` (which must belong to `sb`), or `None` + /// if the mesh does not have that vertex. + fn mesh_vertex(sb: &SoftBody, mesh: &SoftCollisionMesh, i: u32) -> Option { + ((i as usize) < mesh.vertex_count()).then(|| mesh.vertex(sb, i as usize)) + } + + /// The `i`-th edge of `mesh` (which must belong to `sb`) as a world segment. + fn mesh_edge(sb: &SoftBody, mesh: &SoftCollisionMesh, i: u32) -> Option { + #[cfg(feature = "dim2")] + let vertices = *mesh.indices.get(i as usize)?; + #[cfg(feature = "dim3")] + let vertices = *mesh.edges.get(i as usize)?; + Some(parry::shape::Segment::new( + Self::mesh_vertex(sb, mesh, vertices[0])?, + Self::mesh_vertex(sb, mesh, vertices[1])?, + )) + } + + /// The edge-vs-edge contacts this soft body's collision meshes owned at the last step (self + /// contacts and contacts with other soft bodies' meshes), as world segments joining the two + /// edges' closest points, i.e. the contact's witness points (for debug rendering). + pub fn edge_contact_segments<'a>( + &'a self, + soft_bodies: &'a super::SoftBodySet, + ) -> impl Iterator + 'a { + self.meshes() + .flat_map(|mesh| mesh.edge_contacts.iter().map(move |c| (mesh, c))) + .filter_map(move |(mesh, c)| { + // `edge` indexes the mesh owning the contact, `other_edge` the mesh it hit. + let other = soft_bodies.get(c.other.body)?; + let seg1 = Self::mesh_edge(self, mesh, c.edge)?; + let seg2 = Self::mesh_edge(other, other.mesh(c.other.id)?, c.other_edge)?; + let (loc1, loc2) = + parry::query::details::closest_points_segment_segment_with_locations( + &Pose::IDENTITY, + &seg1, + &seg2, + ); + Some((seg1.point_at(&loc1), seg2.point_at(&loc2))) + }) + } + + /// The volume constraints this soft body's collision meshes assembled at the last step (see + /// `SoftVolumeContact`; the pair's owner mesh keeps them), for debug rendering. + pub fn volume_contacts(&self) -> impl Iterator + '_ { + self.meshes().flat_map(|mesh| mesh.volume_contacts.iter()) + } + + /// The vertex-vs-surface contacts against this soft body's collision meshes at the last step + /// (self contacts and other soft bodies' vertices), as world segments joining the vertex to + /// its projection on the element, i.e. the contact's witness points (for debug rendering). + pub fn vertex_contact_segments<'a>( + &'a self, + soft_bodies: &'a super::SoftBodySet, + ) -> impl Iterator + 'a { + use parry::query::PointQuery; + + self.meshes() + .flat_map(|mesh| mesh.vertex_contacts.iter().map(move |c| (mesh, c))) + .filter_map(move |(mesh, c)| { + let other = soft_bodies.get(c.other.body)?; + let other_mesh = other.mesh(c.other.id)?; + // `vertex` indexes the other mesh and `element` the mesh owning the contact, + // unless the contact is flipped (see `SoftVertexContact`). + let ((vertex_body, vertex_mesh), (element_body, element_mesh)) = if c.flipped { + ((self, mesh), (other, other_mesh)) + } else { + ((other, other_mesh), (self, mesh)) + }; + let point = Self::mesh_vertex(vertex_body, vertex_mesh, c.vertex)?; + let element = *element_mesh.indices.get(c.element as usize)?; + let mut vtx = [Vector::ZERO; DIM]; + for (dest, v) in vtx.iter_mut().zip(&element) { + *dest = Self::mesh_vertex(element_body, element_mesh, *v)?; + } + #[cfg(feature = "dim2")] + let projection = parry::shape::Segment::new(vtx[0], vtx[1]); + #[cfg(feature = "dim3")] + let projection = parry::shape::Triangle::new(vtx[0], vtx[1], vtx[2]); + Some((point, projection.project_local_point(point, false).point)) + }) + } +} #[cfg(all(test, feature = "dim2"))] mod test { diff --git a/src/dynamics/soft_body/soft_recovery_settings.rs b/src/dynamics/soft_body/soft_recovery_settings.rs index 2371ccb36..0e12c16ab 100644 --- a/src/dynamics/soft_body/soft_recovery_settings.rs +++ b/src/dynamics/soft_body/soft_recovery_settings.rs @@ -1,7 +1,22 @@ -//! Runtime toggles and tuning for the soft-body tangle detection and recovery machinery: -//! prevention, detection, passive stand-down, and the intersection-volume rows. Every -//! mechanism can be switched off individually and the empirical constants are exposed, so -//! the minimal subset that a scene needs can be found experimentally. +//! Runtime toggles and tuning for soft-body tangle prevention, detection, passive stand-down and +//! intersection-volume constraints. Each mechanism switches off individually and the empirical +//! constants are exposed. + +use crate::math::Real; + +/// What the per-point constraints of the features a volume constraint acts on do (see +/// [`SoftRecoverySettings::overlap_patch_constraints`]). +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum SoftPatchConstraints { + /// Keep them as they are (they may fight the volume constraint). + Keep, + /// Stand them down: the volume constraint alone acts on those features. + StandDown, + /// Keep them but along the volume constraint's normal (the nearest grid cell's), with + /// their separation measured along it, so they push the way the constraint does. + AlongNormal, +} /// Runtime configuration of the soft-body tangle detection and recovery stack. /// @@ -61,6 +76,87 @@ pub struct SoftRecoverySettings { /// agree; a neighborhood straddling the element (a genuine tangle) still stands down. /// (default: `false`) pub crossing_repulsion: bool, + /// Guide the crossing repulsion by the pair's volume normal: the direction the + /// intersection-volume gradients of the two intruding patches separate along (per + /// grid cell, the nearest one to the repelled vertex), instead of the pierced element's + /// plane normal signed by the vertex's neighborhood (which cannot be validated on a + /// crossed surface). Closed pairs only; needs no volume row. (default: `false`) + pub crossing_repulsion_guide: bool, + + // --- Pacing --- + /// The material recovery pace, in length units per second (scaled by + /// `IntegrationParameters::length_unit`): the corrective rate deep recovery, demoted + /// intruders, and untangling pulls are allowed. (default: `0.5`) + pub recovery_pace: Real, + + // --- Intersection-volume rows --- + /// Intersection-volume contact (Allard et al. 2010) for the closed surfaces: one coupled + /// row per overlapping pair (soft-soft, or soft-rigid) over both sides' intruding + /// patches, carrying the intersection volume as its correction. A smooth pressure with + /// no side to decide. Master switch of the knobs below. (default: `true`) + pub overlap_constraints: bool, + /// Overlap rows against rigid colliders too (the surface vertices inside the shape; + /// a dynamic rigid body takes the reaction, a fixed or kinematic one is a wall). + /// (default: `true`) + pub overlap_rigid: bool, + /// A self-crossed mesh takes no pair row: part of its winding is mirrored, so its volume + /// gradient points the wrong way there (two crossed eights exploded without this). + /// (default: `true`) + pub overlap_skip_self_tangled: bool, + /// The 3D closed-closed edge rows stand down on a pair an overlap row owns (inside the + /// overlap they are wrong-sided and freeze it). (default: `true`) + pub overlap_edge_stand_down: bool, + /// Bound on the velocity change the coupled row may hand any side per step, in + /// multiples of [`Self::recovery_pace`] (a light box inside a blob was otherwise shot out + /// with the blob's stored elastic energy); `Real::MAX`: a hard row. With + /// [`Self::overlap_velocity_correction`] it bounds the closing rate instead. + /// (default: `1.0`) + pub overlap_constraint_pace: Real, + /// What the per-point constraints (manifold constraints against rigid colliders, vertex + /// constraints against soft surfaces) of the features inside a volume constraint's patch do, + /// feature by feature rather than pair by pair (see [`SoftPatchConstraints`]): a couple of + /// vertices tunneled through a floor otherwise keep contact constraints fighting the volume + /// constraint that pulls them back. (default: `Keep`) + pub overlap_patch_constraints: SoftPatchConstraints, + /// Measure the intersection volume on the contact skins (the surfaces dilated by their + /// skin) instead of the geometric surfaces: a resting pair then has a volume row, with + /// a speculative slack before the skins meet, and the rows are hard (a barrier), like + /// contacts. Implied by [`SoftContactModel::VolumeOnly`]. (default: `false`) + pub overlap_skin_volume: bool, + /// The skin overlap kept at rest, as a fraction of the pair's skins (the paper's + /// maintained interpenetration layer): the correction only removes the volume beyond + /// it. The volume constraint holds the patch's average depth at it, so a curved body + /// sinks deeper than a flat one; `0.0` rests the surfaces at their skins like the + /// per-point rows. (default: `0.0`) + pub overlap_kept_depth: Real, + /// Volume constraints on a body's self-overlaps between distinct regions of its surface + /// (the two legs of a concave body pressed into each other): the vertices lying behind + /// another part of their own surface form patches, one per connected region, and each + /// region gets a coupled row with the surface it faces, like a pair of bodies. A + /// mirrored lobe (a vertex in front of the surface it faces) is left to the lobe rows. + /// Closed meshes only. (default: `false`) + pub overlap_self_regions: bool, + /// Push along each constraint's normal instead of along the volume gradients: every + /// entry keeps its gradient's magnitude (its share of the patch area) but takes the + /// constraint's mean direction (the owner side toward the other side, the other side + /// the opposite way), so a corner's sideways gradient no longer squeezes it and the + /// whole patch separates along one axis. (default: `false`) + pub overlap_normal_push: bool, + /// Multi-volume grid (the paper's section 5): each pair's patch is split into a regular + /// grid of cells along its tangent axes, each with its own row and multiplier, so the + /// pressure (and the friction state) can vary across the patch: a single multiplier + /// cannot keep a body from tipping, and its whole patch sticks or slides at once. + /// (default: `false`) + pub overlap_multi_volume: bool, + /// Cells per tangent axis of the multi-volume grid. (default: `3`) + pub overlap_split: u32, + /// Steps without progress of a pair's volume estimate before its positional correction + /// stands down (a wedged pair jiggled forever otherwise); it re-arms when the estimate + /// drops again. (default: `240`) + pub overlap_patience: u32, + /// Relative drop of the estimate that counts as progress for the patience. + /// (default: `0.02`) + pub overlap_progress_margin: Real, } @@ -77,6 +173,22 @@ impl Default for SoftRecoverySettings { cross_body_expel_gate: true, edge_stand_down: true, crossing_repulsion: false, + crossing_repulsion_guide: false, + recovery_pace: 0.5, + overlap_constraints: true, + overlap_rigid: true, + overlap_skip_self_tangled: true, + overlap_edge_stand_down: true, + overlap_constraint_pace: 1.0, + overlap_patch_constraints: SoftPatchConstraints::Keep, + overlap_skin_volume: false, + overlap_kept_depth: 0.0, + overlap_normal_push: false, + overlap_self_regions: false, + overlap_multi_volume: true, + overlap_split: 3, + overlap_patience: 240, + overlap_progress_margin: 0.02, } } } diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs index db75d6547..9d294d87e 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs @@ -54,6 +54,145 @@ impl SoftConstraintsSet { apply(bodies, delta); } + /// Applies an overlap constraint's warm impulses (see `SoftOverlapConstraint::warm`) at the + /// start of every substep, before the elastic constraints: the per-particle impulses of the + /// last step on the first substep, the constraint's own (fitted, then solved) on later ones. + pub fn warmstart_overlap_constraint(&mut self, oi: usize, bodies: &mut SolverBodies) { + let constraint = &mut self.overlap_constraints[oi]; + if constraint.warm.is_none() { + return; + } + let num_bodies = bodies.len(); + let live = |slot: u32| (slot as usize) < num_bodies; + let grads = &self.overlap_grads[constraint.grads.clone()]; + let fem_sides = &self.overlap_fem_sides + [constraint.fem_sides.start as usize..constraint.fem_sides.end as usize]; + if constraint.warm_pending { + constraint.warm_pending = false; + let warm_impulses = &self.overlap_warm_impulses[constraint.grads.clone()]; + for (&(slot, im, _, _), &p) in grads.iter().zip(warm_impulses) { + if live(slot) { + bodies.vels[slot as usize].linear += p * im; + } + } + if let Some((slot, ..)) = constraint.rigid.filter(|r| live(r.0)) { + let pose = bodies.get_pose(slot); + let (lin, ang) = constraint.warm_rigid; + let v = &mut bodies.vels[slot as usize]; + v.linear += pose.im.component_mul(&lin); + v.angular += pose.ii.transform_vector(ang); + } + return; + } + let constraint = &self.overlap_constraints[oi]; + for &(slot, im, g, _) in grads { + if live(slot) { + bodies.vels[slot as usize].linear -= g * (im * constraint.impulse); + } + } + if let Some((slot, g_lin, g_ang)) = constraint.rigid.filter(|r| live(r.0)) { + let pose = bodies.get_pose(slot); + let ii_g = pose.ii.transform_vector(g_ang); + let v = &mut bodies.vels[slot as usize]; + v.linear -= pose.im.component_mul(&g_lin) * constraint.impulse; + v.angular -= ii_g * constraint.impulse; + } + } + + pub fn solve_overlap_constraint(&mut self, oi: usize, bodies: &mut SolverBodies) { + let constraint = &mut self.overlap_constraints[oi]; + let num_bodies = bodies.len(); + let live = |slot: u32| (slot as usize) < num_bodies; + let grads: &[(u32, Real, Vector, Vector)] = &self.overlap_grads[constraint.grads.clone()]; + let fem_sides = &self.overlap_fem_sides + [constraint.fem_sides.start as usize..constraint.fem_sides.end as usize]; + let mut w = 0.0; + let mut g_max: Real = 0.0; + for &(slot, im, g, _) in grads { + if live(slot) { + w += im * g.length_squared(); + g_max = g_max.max(g.length() * im); + } + } + // The rigid side's effective mass (its mass properties are step-constant, read from + // the solver body). + let rigid = constraint.rigid.filter(|r| live(r.0)).map(|(slot, g_lin, g_ang)| { + let pose = bodies.get_pose(slot); + let ii_g = pose.ii.transform_vector(g_ang); + (slot, g_lin, g_ang, pose.im, ii_g) + }); + if let Some((_, g_lin, g_ang, im, ii_g)) = rigid { + w += g_lin.gdot(im.component_mul(&g_lin)) + ii_g.gdot(g_ang); + g_max = g_max.max(im.component_mul(&g_lin).length()); + } + if w <= 0.0 { + return; + } + // A hard constraint's volume, updated from the substep's poses (see `hard`). + if constraint.hard { + let mut rhs = constraint.rhs0; + for &(slot, _, g, x0) in grads { + if live(slot) { + rhs += g.gdot(bodies.get_pose(slot).translation - x0); + } + } + if let Some((slot, g_lin, g_ang, _, _)) = rigid { + let pose = bodies.get_pose(slot); + let (com0, rot0) = constraint.rigid_pose0; + let drot = pose.rotation * rot0.inverse(); + #[cfg(feature = "dim2")] + let dtheta = drot.angle(); + #[cfg(feature = "dim3")] + let dtheta = drot.to_scaled_axis(); + rhs += g_lin.gdot(pose.translation - com0) + g_ang.gdot(dtheta); + } + constraint.rhs = rhs; + } + let cfm_gain = w * constraint.cfm_coeff; + let inv_lhs = crate::utils::inv(w + cfm_gain); + // Capped like the volume-piece constraints: no particle faster than the pace. + let max_bias = if g_max * inv_lhs > 0.0 { + constraint.max_bias_velocity / (g_max * inv_lhs) + } else { + Real::MAX + }; + let rhs_bias = if constraint.rhs < 0.0 { + constraint.rhs * constraint.speculative_inv_dt + } else { + (constraint.rhs * constraint.erp_inv_dt).clamp(0.0, max_bias) + }; + let mut dc = 0.0; + for &(slot, _, g, _) in grads { + if live(slot) { + dc += g.gdot(bodies.get_vel(slot).linear); + } + } + if let Some((slot, g_lin, g_ang, _, _)) = rigid { + let v = bodies.get_vel(slot); + dc += g_lin.gdot(v.linear) + g_ang.gdot(v.angular); + } + let mut total = + (constraint.impulse + inv_lhs * (dc + rhs_bias - cfm_gain * constraint.impulse)).max(0.0); + // A soft overlap constraint's accumulated impulse is bounded by the pace (released elastic + // energy is metered, not shot into the lighter side); a hard constraint keeps its velocity + // part, and one applying the correction itself (`rhs > 0`) bounds the bias instead. + if !constraint.hard && constraint.rhs <= 0.0 && g_max > 0.0 { + total = total.min(constraint.max_bias_velocity / g_max); + } + let delta = total - constraint.impulse; + constraint.impulse = total; + for &(slot, im, g, _) in grads { + if live(slot) { + bodies.vels[slot as usize].linear -= g * (im * delta); + } + } + if let Some((slot, g_lin, _, im, ii_g)) = rigid { + let v = &mut bodies.vels[slot as usize]; + v.linear -= im.component_mul(&g_lin) * delta; + v.angular -= ii_g * delta; + } + } + /// Total number of element constraints (scalar + elastic-cell blocks), the writeback stage's domain. #[inline] pub fn num_element_constraints(&self) -> usize { diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs index 10b4d0c3f..b4753298c 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs @@ -6,6 +6,8 @@ use core::ops::Range; use simba::scalar::{ComplexField as _, RealField as _}; use crate::dynamics::solver::solver_body::SolverBodies; +use crate::dynamics::soft_body::SoftMeshId; +use crate::geometry::ColliderHandle; use crate::math::{AngVector, DIM, Matrix, Real, Rotation, Vector}; use crate::utils::ComponentMul; @@ -63,3 +65,45 @@ pub(crate) struct SoftVolumeConstraint { pub rhs: Real, pub impulse: Real, } +/// An intersection-volume constraint (see `SoftRecoverySettings::overlap_constraints`): a scalar +/// unilateral constraint retracting the part of a closed boundary inside another (or its own +/// mirrored lobe) along the gradient of the enclosed area/volume. Solved serially, no warm start. +#[derive(Clone, Debug)] +pub(crate) struct SoftOverlapConstraint { + /// Range of this constraint's `(solver slot, inverse mass, gradient)` triples in `overlap_grads`. + pub grads: Range, + /// The overlap measure the constraint corrects (positive while overlapping). + pub rhs: Real, + pub erp_inv_dt: Real, + pub cfm_coeff: Real, + /// Cap on the velocity the bias may give any particle in one solve. + pub max_bias_velocity: Real, + pub impulse: Real, + /// A negative `rhs` is slack, allowed to close at this rate (the skin band of a hard + /// row may be consumed within the substep, never past it); `0.0` for the rows whose + /// `rhs` is an error. + pub speculative_inv_dt: Real, + /// The rigid side of an overlap row, when the other body is a simulated rigid body: + /// its solver slot, and the volume gradient with respect to its translation and its + /// rotation about its center of mass (the reaction to the soft patch, applied at the + /// patch's vertices). + pub rigid: Option<(u32, Vector, AngVector)>, + /// A hard row (the skin-volume rows, contacts in their own right): its impulse is not + /// bounded by the pace, and its right-hand side is refreshed every substep from the + /// current poses (the volume linearized with the frozen gradients from `rhs0`), so + /// its slack is consumed exactly, never several times over. + pub hard: bool, + /// The assembled right-hand side (see `hard`). + pub rhs0: Real, + /// The rigid side's center of mass and rotation at assembly (see `hard`). + pub rigid_pose0: (Vector, Rotation), + /// Where the row's impulses are carried to for the next step's warm start (see + /// `SoftOverlapWarm`): the awake body, its mesh and the other collider. The carried + /// impulse of every entry lives in `overlap_warm_impulses`, aligned with `grads`, and + /// the entries' particles in `overlap_particles`. + pub warm: Option<(u32, SoftMeshId, ColliderHandle)>, + /// The carried per-particle impulses are still to be applied (the first substep). + pub warm_pending: bool, + /// The rigid side's carried linear and angular impulse. + pub warm_rigid: (Vector, AngVector), +} 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 ad7ef0aa3..2715342af 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 @@ -107,6 +107,8 @@ pub(crate) struct SoftGroupLayout { pub shape_constraints: Range, /// Range into `volume_constraints`. pub volume_constraints: Range, + /// Range into `overlap_constraints`. + pub overlap_constraints: Range, /// Range into `contacts`. pub contacts: Range, /// Range into `contact_colors`: the parallel contact stages. @@ -157,6 +159,14 @@ pub(crate) struct SoftConstraintsSet { pub shape_constraints: Vec, pub volume_constraints: Vec, pub volume_grads: Vec, + /// The intersection-volume rows (see `SoftOverlapRow`), group-major, and their gradients. + pub overlap_constraints: Vec, + pub overlap_grads: Vec<(u32, Real, Vector, Vector)>, + /// The carried impulse of every overlap gradient entry (see `SoftOverlapRow::warm`). + pub overlap_warm_impulses: Vec, + /// The `(side, particle)` of every overlap gradient entry (`0`: the owner body, `1`: + /// the other soft body), for the write-back of the carried impulses. + pub overlap_particles: Vec<(u8, u32)>, /// Contacts against the awake soft bodies' surface colliders, group-major. pub contacts: Vec, /// The particle attachments of the awake soft bodies, group-major. diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs index 16037f722..c52522111 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs @@ -131,6 +131,45 @@ impl SoftConstraintsSet { (c.erp_inv_dt, c.cfm_factor) = if world_fixed { static_soft } else { dyn_soft }; } self.groups[gi].contacts = start..self.contacts.len(); + // The intersection-volume constraints of the group's bodies, with the contact softness. + let ostart = self.overlap_constraints.len(); + for ai in self.groups[gi].awake.clone() { + let mesh_id = per_body[ai].meshes.get(per_body[ai].current_mesh).map(|m| m.id); + for constraint in per_body[ai].overlap_constraints.drain(..) { + let gs = self.overlap_grads.len(); + let n = constraint.grads.len(); + self.overlap_grads.extend(constraint.grads); + self.overlap_particles.extend(constraint.particles); + if constraint.warm_impulses.len() == n { + self.overlap_warm_impulses.extend(constraint.warm_impulses); + } else { + self.overlap_warm_impulses + .extend(core::iter::repeat_n(Vector::ZERO, n)); + } + let (impulse, warm) = match (constraint.warm, mesh_id) { + (Some((other, impulse)), Some(mesh_id)) => { + (impulse, Some((ai as u32, mesh_id, other))) + } + _ => (0.0, None), + }; + self.overlap_constraints.push(SoftOverlapConstraint { + grads: gs..self.overlap_grads.len(), + rhs: constraint.rhs, + rhs0: constraint.rhs, + rigid_pose0: constraint.rigid_pose0, + warm, + warm_pending: warm.is_some(), + warm_rigid: constraint.warm_rigid, + erp_inv_dt: stiffen(¶ms.contact_softness).erp_inv_dt(group_dt(gi)), + cfm_coeff: stiffen(¶ms.contact_softness).cfm_coeff(group_dt(gi)), + max_bias_velocity: constraint.max_bias_velocity, + impulse, + // A hard constraint's slack is speculative: consumed within the substep, + // never past it. + speculative_inv_dt: if constraint.hard { 1.0 / group_dt(gi) } else { 0.0 }, + rigid: constraint.rigid, + hard: constraint.hard, + self.groups[gi].overlap_constraints = ostart..self.overlap_constraints.len(); } // Hand the new edge and vertex contacts to their owner bodies. // SAFETY: the assembly holds no other reference into the soft bodies any more. @@ -142,6 +181,9 @@ impl SoftConstraintsSet { }; core::mem::swap(&mut mesh.edge_contacts, &mut assembled.edge_contacts); core::mem::swap(&mut mesh.vertex_contacts, &mut assembled.vertex_contacts); + core::mem::swap(&mut mesh.overlap_states, &mut assembled.overlap_states); + core::mem::swap(&mut mesh.volume_contacts, &mut assembled.volume_contacts); + mesh.overlap_warm.clear(); mesh.crossing_sweep_travel = assembled.crossing_sweep_travel; core::mem::swap(&mut mesh.crossed_partners, &mut assembled.crossed_partners); } diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs index ed1a9bf32..418db5c97 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs @@ -53,6 +53,12 @@ impl SoftConstraintsSet { Option<&SoftEdgePass>, )> = Vec::new(); + if params.soft_bodies.recovery.overlap_constraints + && params.soft_bodies.recovery.overlap_rigid + && !self_frozen + { + self.assemble_rigid_overlap(ai, ctx, mesh, surface_handle, out); + } for pair in narrow_phase.contact_pairs_with(surface_handle) { let surface_first = pair.collider1 == surface_handle; let other_handle = if surface_first { @@ -277,16 +283,310 @@ impl SoftConstraintsSet { (v_other - v_surface).gdot(dir).max(0.0) }; - // A closed surface's reversed interior contacts (a body that crossed it - // The rule reads the cells' boundary and its winding, which a skin is - // not: a skinned body collides plainly. + // A closed surface's reversed interior contacts (an intruder seen from inside, + // a fold's far layer) are dropped: the volume constraints and the elasticity + // resolve them. The rule reads the cells' winding, so a skin collides plainly. let skinned = mesh.is_skinned(); let closed = mesh.is_closed() && self_particle.is_none() && !skinned; - // The row tracks the barycentric point of the element: its reference + // The features a volume constraint acts on (see `overlap_patch_constraints`). + let patch_policy = params.soft_bodies.recovery.overlap_patch_constraints; + let rigid_patch: Option<(Vec, Vec<(Vector, Vector)>)> = + (patch_policy != SoftPatchConstraints::Keep) + .then(|| { + out.rigid_patches + .iter() + .find(|p| p.0 == other_handle) + .map(|p| (p.1.clone(), p.2.clone())) + }) + .flatten(); + for (mi, manifold) in pair.manifolds().iter().enumerate() { + let Some((element_id, element)) = manifold_element(manifold) else { + continue; + }; + if let Some(p) = self_particle { + if mesh.self_contact_excluded(sb, p, element, surface_co.contact_skin()) { + continue; + } + } + let in_patch = rigid_patch.as_ref().is_some_and(|(depths, _)| { + element.iter().any(|&v| { + depths.get(v as usize).copied().unwrap_or(Real::NEG_INFINITY) + > Real::NEG_INFINITY + }) + }); + // Force direction on the surface (the normal points from collider 1 to 2). + let normal = manifold.data.normal; + let dir = if surface_first { -normal } else { normal }; + // Reversed interior contacts of a closed surface: expelled through the + // far side, or dropped (see above). + let reversed = if closed { + mesh.element_outward_normal(sb, element_id) + .and_then(|n| n.try_normalize()) + .filter(|n| dir.gdot(*n) > 0.5) + } else { + None + }; + // An element contact's point: weights, point, rigid anchor, separation, + // warm-start share. A segment's interior contact on a smooth rigid surface + // splits into one per endpoint, in the tangent plane, so it cannot rock. + let contact_points = |sc: &crate::geometry::SolverContact| { + let (positions, weights, other_anchor) = + contact_geometry(element, manifold, sc); + // The constraint tracks the barycentric point of the element: its reference + // must be that point too (the anchor is offset from the element by + // the contact skins when the surface is the pair's first collider). + let mut surface_point0 = Vector::ZERO; + for k in 0..DIM { + surface_point0 += positions[k] * weights[k]; + } + let split = element.len() == 2 + && weights[0] > 0.02 + && weights[1] > 0.02 + && (positions[1] - positions[0]).gdot(dir).abs() + < 0.5 * (positions[1] - positions[0]).length(); + let mut points = [( + positions, + weights, + surface_point0, + other_anchor, + sc.dist, + 1.0, + ); 2]; + if split { + for (k, point) in points.iter_mut().enumerate() { + let mut w = [0.0; DIM]; + w[k] = 1.0; + let offset = positions[k] - surface_point0; + let along = offset.gdot(dir); + *point = ( + positions, + w, + positions[k], + other_anchor + offset - dir * along, + sc.dist + along, + 0.5, + ); + } + } + points + .into_iter() + .take(if split { 2 } else { 1 }) + .map(move |p| (p, split)) + }; + for (sc, (point, split_endpoints)) in manifold + .data + .solver_contacts + .iter() + .flat_map(|sc| contact_points(sc).map(move |p| (sc, p))) + { + let point_id = (sc.contact_id[0] & !NEW_CONTACT_BIT) as usize; + let (positions, weights, surface_point0, other_anchor, sc_dist, warm_share) = point; + // A constraint disagreeing with the volume constraint (see + // `overlap_patch_constraints`) pushes the surface into the rigid body + // along the nearest cell's normal: stood down, or bent along it. + let disagreeing = if in_patch { + rigid_patch.as_ref().and_then(|(depths, cells)| { + let n = cells + .iter() + .min_by(|a, b| { + let da = (a.0 - surface_point0).length_squared(); + let db = (b.0 - surface_point0).length_squared(); + da.partial_cmp(&db) + .unwrap_or(core::cmp::Ordering::Equal) + }) + .map(|c| c.1)?; + if dir.gdot(n) <= 0.0 { + return None; + } + let mut depth = 0.0; + for k in 0..DIM { + if let Some(&v) = element.get(k) { + depth += weights[k] + * depths[v as usize].max(0.0); + } + } + Some((-n, -depth, false)) + }) + } else { + None + }; + if disagreeing.is_some() && patch_policy == SoftPatchConstraints::StandDown + { + continue; + } + let along_normal = disagreeing + .filter(|_| patch_policy == SoftPatchConstraints::AlongNormal); + let (dir, dist, _expelling) = match (along_normal, reversed) { + (Some(constraint), _) => constraint, + (None, Some(_)) if is_interior(element, &weights) => continue, + _ => (dir, sc.dist, false), + }; // The point the constraint tracks, expressed in the particles it acts through. - // ghost of that internal vertex/edge (the neighbor carries the - // once, their vertex contacts are real support. + let (anchors, anchor_weights, tracked) = + mesh.contact_anchors(sb, element, &weights, surface_point0); + let surface_point0 = tracked; + let particles: [u32; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if anchors[k] == u32::MAX { + u32::MAX + } else { + slots[anchors[k] as usize] + } + }); + let im_particles: [Real; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if particles[k] == u32::MAX { + 0.0 + } else { + sb.particles[anchors[k] as usize].inv_mass + } + }); + let anchor_positions: [Vector; CONTACT_ANCHORS] = + core::array::from_fn(|k| { + if anchors[k] == u32::MAX { + Vector::ZERO + } else { + sb.particles[anchors[k] as usize].position + } + }); + let (body_local_point, body_arm) = match &other_com_pose { + Some(pose) => ( + pose.inverse_transform_point(other_anchor), + other_anchor - pose.translation, + ), + None => (other_anchor, Vector::ZERO), + }; + // A small ball (a soft particle) touching a surface vertex/edge while + // its center projects inside a neighboring element is a ghost contact + // (spurious normal): skipped; large balls and split endpoints are real. + if let Some(ball) = other_co + .shape() + .as_ball() + .filter(|_| !split_endpoints) + { + let size = (positions[1] - positions[0]).length(); + if ball.radius < size { + let center = other_anchor - dir * ball.radius; + if !skinned + && mesh.contact_is_ghost(sb, element_id, &weights, center) + { + continue; + } + } + } + // Vertex contact shared with a neighboring element? + let vertex = (0..element.len()).find(|&k| weights[k] > 0.999); + let vertex_key = vertex.map(|k| element[k]); + let mut replace_constraint = None; + if let Some(key) = vertex_key { + if let Some(&existing) = vertex_constraints.get(&key) { + if out.contacts[existing].dist0 <= dist { + continue; + } + replace_constraint = Some(existing); + } + } + let feature_key = if vertex_key.is_none() && rigid_other.is_some() { + let point = &manifold.points[point_id]; + let fid = if surface_first { + point.fid2 + } else { + point.fid1 + }; + let dominant = (0..element.len()) + .max_by(|&a, &b| weights[a].partial_cmp(&weights[b]).unwrap()) + .map(|k| element[k]) + .unwrap_or(0); + Some((fid.0, dominant)) + } else { + None + }; + if let Some(key) = feature_key { + if let Some(&existing) = feature_constraints.get(&key) { + if out.contacts[existing].dist0 <= dist { + continue; + } + replace_constraint = Some(existing); + } + } + let tangents = SoftContact::tangent_basis(dir); + // Warm start from the manifold point (world-space friction impulse + // projected on the fresh tangent basis). + let data = &manifold.points[point_id].data; + let warm_normal = data.warmstart_impulse * warm_share; + #[cfg(feature = "dim2")] + let warm_tangent = [data.warmstart_tangent_impulse[0] * warm_share]; + #[cfg(feature = "dim3")] + let warm_tangent = [ + data.warmstart_tangent_world.dot(tangents[0]) * warm_share, + data.warmstart_tangent_world.dot(tangents[1]) * warm_share, + ]; + let contact = SoftContact { + source: SoftContactSource { + collider1: pair.collider1, + collider2: pair.collider2, + manifold: mi as u32, + point: point_id as u32, + slot: u32::MAX, + }, + support_body: ai as u32, + support_particle: anchors, + particles, + weights: anchor_weights, + im_particles, + frozen_pos: anchor_positions, + body: other_slot, + element: None, + other_body, + body_im: Vector::ZERO, + body_ii: Default::default(), + body_local_point, + body_arm, + surface_point0, + body_point0: other_anchor, + dir, + tangents, + dist0: dist, + friction: manifold.data.friction, + soft_other: false, + erp_inv_dt, + cfm_factor, + max_bias: Real::MAX, + torque_dir: Default::default(), + ii_torque_dir: Default::default(), + r_normal: 0.0, + rhs_normal: 0.0, + cfm_normal: 1.0, + impulse_normal: warm_normal, + impulse_normal_acc: -warm_normal, + torque_tangent: [Default::default(); DIM - 1], + ii_torque_tangent: [Default::default(); DIM - 1], + r_tangent: [0.0; DIM - 1], + rhs_tangent: [0.0; DIM - 1], + impulse_tangent: warm_tangent, + impulse_tangent_acc: core::array::from_fn(|j| -warm_tangent[j]), + }; + out.max_approach_speed = out.max_approach_speed.max(approach_speed( + element, + &weights, + other_anchor, + dir, + )); + match replace_constraint { + Some(existing) => out.contacts[existing] = contact, + None => { + if let Some(key) = vertex_key { + vertex_constraints.insert(key, out.contacts.len()); + } + if let Some(key) = feature_key { + feature_constraints.insert(key, out.contacts.len()); + } + out.contacts.push(contact); + } + } + if let Some(k) = (0..element.len()).find(|&k| weights[k] > 0.9) { + held_vertices.push(element[k]); + } + } } // The pair's predictive vertex contacts (see `SoftRigidVertexContact`): the diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs index 5254bd0fe..eec3e21d5 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs @@ -55,6 +55,23 @@ impl SoftConstraintsSet { if !params.soft_bodies.recovery.edge_speculation && mesh.is_closed() && other_mesh.is_closed() { return; } + // While a volume constraint owns a crossed closed-closed pair, its edge constraints stand down + // entirely: inside the overlap they are wrong-sided and freeze it (two overlapping + // balloons never de-overlapped through them). + #[cfg(feature = "dim3")] + if !is_self && mesh.is_closed() && other_mesh.is_closed() { + let crossed = out + .meshes + .get(out.current_mesh) + .is_some_and(|m| m.crossed_partners.contains(&other_surface_handle)); + let recovery = ¶ms.soft_bodies.recovery; + if recovery.overlap_constraints && recovery.overlap_edge_stand_down && crossed { + return; + } + } + let Some(detected) = detected else { + return; + }; let other_handle = mesh_ref(other_co); let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; let other_slots = other_ai.map(|bi| { diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs index ab6e4ff37..d4500fccc 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs @@ -26,8 +26,14 @@ fn repel_constraint( (eb, eb_mesh, element): (&SoftBody, &SoftCollisionMesh, usize), (vb, vb_mesh, vertex_pos, neighbors): (&SoftBody, &SoftCollisionMesh, Vector, &[u32]), skins: Real, + guide: Option, ) -> Option<(Vector, Real)> { let (p0, n) = element_plane(eb_mesh, element, |v| eb_mesh.vertex(eb, v))?; + // Guided (see `crossing_repulsion_guide`): the vertex is pushed along the pair's volume + // normal, its separation measured along it from the element's plane point. + if let Some(push) = guide { + return Some((-push, (vertex_pos - p0).dot(push) - skins)); + } if neighbors.is_empty() { return None; } @@ -108,6 +114,10 @@ impl SoftConstraintsSet { if !is_self && !flipped { out.exempt_cross_tangles(other_surface_handle); } + // Intersection-volume contact (`SoftOverlapConstraint`, Allard et al. 2010): one coupled + // constraint per crossed closed pair over both intruding patches (none if self-tangled), + // built once per pair (lower surface handle, or this body if the other is not simulated). + let recovery = ¶ms.soft_bodies.recovery; let friction = crate::dynamics::CoefficientCombineRule::combine( surface_co.friction(), other_co.friction(), @@ -119,6 +129,48 @@ impl SoftConstraintsSet { if let Some(volume) = volume.filter(|_| { !flipped && (!recovery.overlap_skip_self_tangled || out.tangled_elements.is_empty()) + && (other_ai.is_none() + || surface_handle.into_raw_parts() < other_surface_handle.into_raw_parts()) + }) { + // The patches are binned from the pair's lower surface; consumed from the other + // side (an owner that is not simulated), their bins are mirrored. + let mirrored = (volume.own != surface_handle).then(|| mirrored_bins(&volume.bins)); + let bins = mirrored.as_deref().unwrap_or(&volume.bins); + emit_volume_bins( + params, + out, + mesh, + other_surface_handle, + (eb, eb_slots), + Some((vb, vb_slots)), + None, + hard, + bins, + volume.slot_offset, + ); + } + // Self-overlaps between distinct regions of one closed surface (see + // `overlap_self_regions`, detected by the narrow phase): each region gets a coupled + // constraint with the vertices of the elements it faces, like a pair of bodies would. + if is_self { + for bins in regions { + emit_volume_bins( + params, + out, + mesh, + surface_handle, + (vb, vb_slots), + Some((vb, vb_slots)), + None, + hard, + bins, + u32::MAX, + ); + } + } + // The two contact skins (a soft surface's is its vertices' thickness). + let skins = vb_co.contact_skin() + eb_co.contact_skin(); + out.previous_vertex.clear(); for c in &mesh.vertex_contacts { if c.other == other_handle && c.flipped == flipped { @@ -140,12 +192,293 @@ impl SoftConstraintsSet { // Narrow-phase results: the guiding volume normals (see `crossing_repulsion_guide`), the // vertices inside the other side's volume patch (see `overlap_patch_constraints`) and, for // a self pair, the vertices in their own body's self-intersection region. - // boundary, the particles of the cell carrying it when it collides through a skin. + let patch_policy = params.soft_bodies.recovery.overlap_patch_constraints; + out.repel_guides.clone_from(&detected.repel_guides); + out.patch_inside_vb.clone_from(&detected.patch_inside_vb); + out.repel_inside.clone_from(&detected.repel_inside); + // The surface vertices with an element within reach (see `soft_contacts`). + for hit in &detected.hits { + let v = hit.vertex as usize; + let candidates = detected.candidates_of(hit); + // A vertex inside the other side's volume patch (see `overlap_patch_constraints`): its + // constraints disagreeing with the constraint are stood down or bent along its normal. + let in_patch = out.patch_inside_vb.get(v).copied().unwrap_or(false) + && patch_policy != SoftPatchConstraints::Keep; + let vertex_pos = vb_mesh.vertex(vb, v); + // The vertex side: its own solver body when the body collides through its cells' + // boundary, the particles of the cell holding it when it collides through a skin. + let (vertex_anchors, vertex_weights) = vb_mesh.vertex_anchors(vb, v); + let vertex_element = vb_mesh.is_skinned().then(|| { + let particles: [u32; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if vertex_anchors[k] == u32::MAX { + u32::MAX + } else { + vb_slots + .map(|s| s[vertex_anchors[k] as usize]) + .unwrap_or(u32::MAX) + } + }); + SoftContactElement { + particles, + weights: vertex_weights, + im_particles: core::array::from_fn(|k| { + if particles[k] == u32::MAX { + 0.0 + } else { + vb.particles[vertex_anchors[k] as usize].inv_mass + } + }), + frozen_pos: core::array::from_fn(|k| { + if vertex_anchors[k] == u32::MAX { + Vector::ZERO + } else { + vb.particles[vertex_anchors[k] as usize].position + } + }), + } + }); + let vslot = match &vertex_element { + Some(_) => u32::MAX, + None => vb_slots.map(|s| s[v]).unwrap_or(u32::MAX), + }; + // Whether the vertex side can move at all: its slot, or any of its cell's particles. + let vertex_free = match &vertex_element { + Some(e) => e.im_particles.iter().any(|im| *im > 0.0), + None => vslot != u32::MAX, + }; + // A frozen surface against a vertex that is not simulated: no DOF on either side. + if self_frozen && !vertex_free { + continue; + } + + // A foreign vertex that crossed a closed surface (see `soft_contacts`) hands its + // pair to the recovery (the pair is crossed). + if hit.crossed { + let mc = &mut out.meshes[current_mesh]; + if !mc.crossed_partners.contains(&other_surface_handle) { + mc.crossed_partners.push(other_surface_handle); + } + } + // The vertex's solver body (its contact point is tracked in that body's CoM frame). + // The vertex's solver body sits at the particle: the contact point is the origin of + // its frame. + let (body_local_point, body_arm) = ( + if vslot != u32::MAX { + Vector::ZERO + } else { + vertex_pos + }, + Vector::ZERO, + ); + let (erp_inv_dt, cfm_factor) = if !vertex_free { + (static_erp, static_cfm) + } else { + (dyn_erp, dyn_cfm) + }; + + for (ci, c) in candidates.iter().enumerate() { // A candidate the contact-modification hook disabled gets no constraint. + if !c.enabled { + continue; + } + // The pair's contact slot of this candidate (none for a self contact). + let slot = if detected.slot_offset == u32::MAX { + u32::MAX + } else { + detected.slot_offset + hit.candidates.start + ci as u32 + }; // Crossing repulsion (see `repel_constraint`): a flagged feature's constraint repels toward - // would pin the intruder inside; the volume rows and the elasticity - // A row touching a boundary crossing between the two surfaces may only - // expel, never hold: a keep-apart row there is wrong-sided by construction + // the vertex's neighborhood side instead of standing down or being dropped, + // and drops when the neighborhood straddles the element. + let flagged = if is_self { + out.tangled_vertices.get(v).copied().unwrap_or(false) + || out.tangled_elements.get(c.element as usize).copied().unwrap_or(false) + } else { + out.cross_tangled_vertices.get(v).copied().unwrap_or(false) + || out + .cross_tangled_elements + .get(c.element as usize) + .copied() + .unwrap_or(false) + }; + // The nearest guiding cell's normal (the vertex's push direction), if any. + let patch_guide = if in_patch { + out.repel_guides + .iter() + .min_by(|a, b| { + let da = (a.0 - vertex_pos).length_squared(); + let db = (b.0 - vertex_pos).length_squared(); + da.partial_cmp(&db).unwrap_or(core::cmp::Ordering::Equal) + }) + .map(|g| g.1) + // Disagreeing: the constraint pushes the vertex (along minus its `dir`) + // against the normal. + .filter(|g| c.dir.gdot(*g) > 0.0) + } else { + None + }; + if patch_guide.is_some() && patch_policy == SoftPatchConstraints::StandDown { + continue; + } + let along_patch_normal = + patch_guide.is_some() && patch_policy == SoftPatchConstraints::AlongNormal; + let repelled = if repel && flagged || along_patch_normal { + // The nearest guiding cell's normal, if the guide is on. + let guide = out + .repel_guides + .iter() + .min_by(|a, b| { + let da = (a.0 - vertex_pos).length_squared(); + let db = (b.0 - vertex_pos).length_squared(); + da.partial_cmp(&db).unwrap_or(core::cmp::Ordering::Equal) + }) + .map(|g| { + // A self pair: the fold's vertices go one way, the facing + // surface's the other. + if is_self && !out.repel_inside.get(v).copied().unwrap_or(false) { + -g.1 + } else { + g.1 + } + }); + if guide.is_none() && !(repel && flagged) { + // A patch vertex with no cell to follow keeps its constraint as is. + None + } else { + match repel_constraint( + (eb, eb_mesh, c.element as usize), + ( + vb, + vb_mesh, + vertex_pos, + &vb_mesh.ring[vb_mesh.ring_offsets[v] as usize + ..vb_mesh.ring_offsets[v + 1] as usize], + ), + skins, + guide, + ) { + Some((d, dd)) => Some((d, dd, true)), + None => continue, + } + } + } else { + None + }; + // Reversed interior contacts of a closed surface are dropped: holding them + // would pin the intruder inside; the volume constraints and the elasticity + // resolve it. + let (dir, dist, expelling) = match c.outward { + _ if repelled.is_some() => repelled.unwrap(), + Some(outward) if c.interior && c.dir.gdot(outward) > 0.5 => continue, + _ => (c.dir, c.dist, false), + }; + // A constraint touching a boundary crossing between the surfaces may only expel, + // never hold: a keep-apart constraint there is wrong-sided and freezes it. + if !expelling + && !is_self + && params.soft_bodies.recovery.cross_body_expel_gate + && (out.cross_tangled_vertices.get(v).copied().unwrap_or(false) + || out + .cross_tangled_elements + .get(c.element as usize) + .copied() + .unwrap_or(false)) + { + continue; + } + let element = eb_mesh.element(c.element as usize); + let mut surface_point0 = Vector::ZERO; + for (k, v) in element.iter().enumerate() { + surface_point0 += eb_mesh.vertex(eb, *v as usize) * c.weights[k]; + } + let (anchors, anchor_weights, tracked) = + eb_mesh.contact_anchors(eb, element, &c.weights, surface_point0); + let surface_point0 = tracked; + let particles: [u32; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if anchors[k] == u32::MAX { + u32::MAX + } else { + eb_slots.map(|s| s[anchors[k] as usize]).unwrap_or(u32::MAX) + } + }); + let im_particles: [Real; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if particles[k] == u32::MAX { + 0.0 + } else { + eb.particles[anchors[k] as usize].inv_mass + } + }); + let frozen_pos: [Vector; CONTACT_ANCHORS] = core::array::from_fn(|k| { + if anchors[k] == u32::MAX { + Vector::ZERO + } else { + eb.particles[anchors[k] as usize].position + } + }); + let tangents = SoftContact::tangent_basis(dir); + let (warm_normal, warm_tangent_world) = out + .previous_vertex + .get(&(v as u32, c.element)) + .copied() + .unwrap_or((0.0, Vector::ZERO)); + let warm_tangent: [Real; DIM - 1] = + core::array::from_fn(|k| warm_tangent_world.gdot(tangents[k])); + let assembled = &mut out.meshes[current_mesh]; + let point = assembled.vertex_contacts.len() as u32; + assembled.vertex_contacts.push(SoftVertexContact { + other: other_handle, + vertex: v as u32, + element: c.element, + flipped, + impulse: warm_normal, + tangent_impulse: warm_tangent_world, + }); + out.contacts.push(SoftContact { + source: SoftContactSource { + collider1: surface_handle, + collider2: other_surface_handle, + manifold: SOURCE_VERTEX_CONTACT, + point, + slot, + }, + support_body: ai as u32, + support_particle: anchors, + particles, + weights: anchor_weights, + im_particles, + frozen_pos, + body: vslot, + other_body: other_ai.map_or(u32::MAX, |bi| bi as u32), + element: vertex_element, + body_im: Vector::ZERO, + body_ii: Default::default(), + body_local_point, + body_arm, + surface_point0, + body_point0: vertex_pos, + dir, + tangents, + dist0: dist, + friction, + soft_other: true, + erp_inv_dt, + cfm_factor, + max_bias: Real::MAX, + torque_dir: Default::default(), + ii_torque_dir: Default::default(), + r_normal: 0.0, + rhs_normal: 0.0, + cfm_normal: 1.0, + impulse_normal: warm_normal, + impulse_normal_acc: -warm_normal, + torque_tangent: [Default::default(); DIM - 1], + ii_torque_tangent: [Default::default(); DIM - 1], + r_tangent: [0.0; DIM - 1], + rhs_tangent: [0.0; DIM - 1], + impulse_tangent: warm_tangent, + impulse_tangent_acc: core::array::from_fn(|k| -warm_tangent[k]), + }); + } } } } diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs new file mode 100644 index 000000000..d026aceaf --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs @@ -0,0 +1,363 @@ +//! Intersection-volume constraints of a pair's patch bins: the bins' emission, the overlap arming across steps, the patch constraint entries and the rigid-side volume contacts. + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::alloc_prelude::*; + +use super::{AssemblyCtx, BodyContacts, MeshContacts, OverlapConstraintWorkspace, material_pace}; +use super::super::soft_constraints_set::SoftConstraintsSet; +use crate::dynamics::soft_body::SoftPatchConstraints; +use crate::dynamics::soft_body::{SoftCollisionMesh, SoftOverlapState, SoftVolumeContact}; +use crate::dynamics::{IntegrationParameters, SoftBody}; +use crate::geometry::ColliderHandle; +use crate::geometry::soft_contacts::VolumeBin; +use crate::math::{AngVector, Real, Rotation, Vector}; +use crate::utils::{CrossProduct, DotProduct}; + +/// The bins of a pair's volume patch seen from its other side: the sides swapped, the normal +/// reversed. +pub(super) fn mirrored_bins(bins: &[VolumeBin]) -> Vec { + bins.iter() + .map(|b| VolumeBin { + own: b.other.clone(), + other: b.own.clone(), + volume: b.volume, + normal: -b.normal, + center: b.center, + enabled: b.enabled, + impulse: b.impulse, + }) + .collect() +} + +/// The rigid side of a volume contact. +pub(super) struct RigidSide { + /// Its solver slot (`u32::MAX`: not simulated this step). + slot: u32, + com: Vector, + rot: Rotation, +} + +/// Emits one volume constraint per bin of a pair (see `SoftOverlapConstraint`), the signed volume +/// as correction; `None` slots: side not simulated (fixed anchors); `hard`: no stall fade; +/// `slot_offset`: first bin's contact slot (`u32::MAX`: none). Returns the bins' (center, normal). +#[allow(clippy::too_many_arguments)] +pub(super) fn emit_volume_bins( + params: &IntegrationParameters, + out: &mut BodyContacts, + mesh: &SoftCollisionMesh, + other_handle: ColliderHandle, + own: (&SoftBody, Option<&[u32]>), + other: Option<(&SoftBody, Option<&[u32]>)>, + rigid: Option, + hard: bool, + bins: &[VolumeBin], + slot_offset: u32, +) -> Vec<(Vector, Vector)> { + let recovery = ¶ms.soft_bodies.recovery; + let mut cells = Vec::with_capacity(bins.len()); + if bins.is_empty() { + return cells; + } + let error: Real = bins.iter().map(|b| b.volume.max(0.0)).sum(); + let scale = if hard { + 1.0 + } else { + overlap_arming(params, mesh, out.mesh(), other_handle, error).scale + }; + let warm_coeff = params.warmstart_coefficient; + // The impulses the pair's particles accumulated in the last step (a hard constraint only). + let prev_warm = (hard && warm_coeff != 0.0) + .then(|| mesh.overlap_warm.iter().find(|w| w.other == other_handle)) + .flatten(); + let prev_warm_of = |side: u8, p: u32| -> Vector { + prev_warm.map_or(Vector::ZERO, |w| { + let list = if side == 0 { &w.own } else { &w.other_soft }; + list.iter() + .find(|e| e.0 == p) + .map_or(Vector::ZERO, |e| e.1 * warm_coeff) + }) + }; + for (bi, bin) in bins.iter().enumerate() { + if !bin.enabled { + continue; + } + let mut bin = bin.clone(); + // Along the normal instead of the gradients (see `overlap_normal_push`): each entry + // keeps its magnitude, the sides take opposite directions. + if recovery.overlap_normal_push && bin.normal != Vector::ZERO { + let n = bin.normal; + for (_, g) in &mut bin.own { + *g = n * g.length(); + } + for (_, g) in &mut bin.other { + *g = -n * g.length(); + } + } + // The constraint as drawn by the debug renderer. + let mut gradients: Vec<(Vector, Vector)> = bin + .own + .iter() + .map(|&(p, g)| (own.0.particles[p as usize].position, g)) + .collect(); + if let Some((o, _)) = other { + gradients.extend( + bin.other + .iter() + .map(|&(p, g)| (o.particles[p as usize].position, g)), + ); + } + out.mesh().volume_contacts.push(SoftVolumeContact { + center: bin.center, + normal: bin.normal, + volume: bin.volume, + gradients, + }); + if bin.normal != Vector::ZERO { + cells.push((bin.center, bin.normal)); + } + // The rigid side takes the reaction: the negated gradient sum (the two surfaces + // bound one volume) with its torque about the center of mass. + let mut g_lin = Vector::ZERO; + let mut g_ang = AngVector::default(); + if let Some(rs) = &rigid { + for &(p, g) in &bin.own { + g_lin -= g; + g_ang -= (own.0.particles[p as usize].position - rs.com).gcross(g); + } + } + let mut grads = Vec::new(); + let mut particles = Vec::new(); + patch_constraint_entries(own.0, 0, own.1, &bin.own, &mut grads, &mut particles); + if let Some((o, o_slots)) = other { + patch_constraint_entries(o, 1, o_slots, &bin.other, &mut grads, &mut particles); + } + let rigid_part = rigid + .as_ref() + .filter(|rs| rs.slot != u32::MAX) + .map(|rs| (rs.slot, g_lin, g_ang)); + // The warm impulse of every entry, and the multiplier fitted to them (least + // squares along the new gradients): what the solve starts from. + let (warm, warm_impulses, warm_rigid) = if hard { + let warm_impulses: Vec = + particles.iter().map(|&(side, p)| prev_warm_of(side, p)).collect(); + let mut num = 0.0; + let mut den = 0.0; + for (&(_, _, g, _), &p) in grads.iter().zip(&warm_impulses) { + num -= p.gdot(g); + den += g.length_squared(); + } + let impulse = if den > 0.0 { (num / den).max(0.0) } else { 0.0 }; + let warm_rigid = prev_warm.map_or((Vector::ZERO, AngVector::default()), |w| { + (w.rigid.0 * warm_coeff, w.rigid.1 * warm_coeff) + }); + (Some((other_handle, impulse)), warm_impulses, warm_rigid) + } else { + (None, Vec::new(), (Vector::ZERO, AngVector::default())) + }; + // A hard constraint applies the signed volume (its slack is speculative); a recovery constraint + // applies the volume, metered by the stall fade. + let rhs = if hard { + bin.volume + } else { + bin.volume.max(0.0) * scale + }; + if !grads.is_empty() || rigid_part.is_some() { + out.overlap_constraints.push(OverlapConstraintWorkspace { + grads, + particles, + rigid: rigid_part, + rigid_pose0: rigid + .as_ref() + .map_or((Vector::ZERO, Rotation::IDENTITY), |rs| (rs.com, rs.rot)), + rhs, + hard, + warm, + warm_impulses, + warm_rigid, + max_bias_velocity: if hard { + params.max_corrective_velocity() + } else { + recovery.overlap_constraint_pace * material_pace(params) + }, + report: ( + mesh.collider(), + other_handle, + if slot_offset == u32::MAX { + u32::MAX + } else { + slot_offset + bi as u32 + }, + ), + }); + } + } + cells +} + +/// How much of a pair's overlap correction runs this step (see `SoftOverlapState`): full while the +/// volume estimate improves, fading over the second half of the patience once it stalls, stood +/// down after it; re-arms only when the estimate drops five margins below its best. Writes `out`. +struct OverlapArming { + /// Scale of the constraint's closing rate (`0.0`: stood down). + scale: Real, +} + +fn overlap_arming( + params: &IntegrationParameters, + mesh: &SoftCollisionMesh, + out: &mut MeshContacts, + other: ColliderHandle, + error: Real, +) -> OverlapArming { + let margin = params.soft_bodies.recovery.overlap_progress_margin; + let patience = params.soft_bodies.recovery.overlap_patience.min(u16::MAX as u32) as u16; + let previous = mesh.overlap_states.iter().find(|s| s.other == other).copied(); + let state = match previous { + Some(prev) => { + let needed = if prev.stalled { 5.0 * margin } else { margin }; + if error < prev.best_error * (1.0 - needed) { + SoftOverlapState { + other, + best_error: error, + steps_stuck: 0, + stalled: false, + } + } else { + let steps_stuck = prev.steps_stuck.saturating_add(1); + SoftOverlapState { + steps_stuck, + stalled: prev.stalled || steps_stuck > patience, + ..prev + } + } + } + None => SoftOverlapState { + other, + best_error: error, + steps_stuck: 0, + stalled: false, + }, + }; + out.overlap_states.push(state); + let scale = if state.stalled || patience == 0 { + 0.0 + } else { + let half = patience / 2; + if state.steps_stuck <= half { + 1.0 + } else { + 1.0 - (state.steps_stuck - half) as Real / (patience - half).max(1) as Real + } + }; + OverlapArming { + scale: scale.clamp(0.0, 1.0), + } +} + +/// The `(slot, inverse mass, gradient, position)` constraint entries of a body's patch entries, +/// with their `(side, particle)`; `None` slots (the body is not simulated this step) make +/// the entries fixed anchors. +fn patch_constraint_entries( + body: &SoftBody, + side: u8, + slots: Option<&[u32]>, + entries: &[(u32, Vector)], + out: &mut Vec<(u32, Real, Vector, Vector)>, + particles: &mut Vec<(u8, u32)>, +) { + let Some(slots) = slots else { + return; + }; + for &(p, g) in entries { + let slot = slots[p as usize]; + let part = &body.particles[p as usize]; + if slot != u32::MAX && part.inv_mass > 0.0 { + out.push((slot, part.inv_mass, g, part.position)); + particles.push((side, p)); + } + } +} + +impl SoftConstraintsSet { + /// The volume contacts of awake body `ai`'s closed `mesh` against the rigid colliders it + /// has a narrow-phase pair with (see `SoftOverlapConstraint`), from the patches the narrow phase + /// detected on the pairs: the rigid side takes the reaction. + pub(super) fn assemble_rigid_overlap( + &self, + ai: usize, + ctx: &AssemblyCtx, + mesh: &SoftCollisionMesh, + surface_handle: ColliderHandle, + out: &mut BodyContacts, + ) { + let AssemblyCtx { + island_id, + params, + narrow_phase, + colliders, + bodies, + .. + } = *ctx; + let recovery = ¶ms.soft_bodies.recovery; + let awake = &self.awake[ai]; + // SAFETY: read-only access during assembly. + let sb = unsafe { &*awake.ptr }; + // Standing the patch's per-point constraints down (or bending them) leaves the volume + // constraint as the support of those features: a hard constraint, not a paced recovery. + let hard = recovery.overlap_skin_volume + || recovery.overlap_patch_constraints != SoftPatchConstraints::Keep; + let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + for pair in narrow_phase.contact_pairs_with(surface_handle) { + let other_handle = if pair.collider1 == surface_handle { + pair.collider2 + } else { + pair.collider1 + }; + let Some(other_co) = colliders.get(other_handle) else { + continue; + }; + if other_co.deformable_mesh_ref.is_some() || other_co.is_sensor() { + continue; + } + let Some(patch) = pair.rigid().and_then(|r| r.soft_patch.as_deref()) else { + }; + let other_rb = other_co.parent().and_then(|h| bodies.get(h)); + let dynamic = other_rb.is_some_and(|rb| rb.is_dynamic()); + let slot = match other_rb { + Some(rb) + if dynamic + && rb.ids.active_island_id == island_id as u32 + && !rb.is_sleeping() + && rb.is_enabled() => + { + rb.ids.active_set_id + } + _ => u32::MAX, + }; + let rigid = RigidSide { + slot, + com: patch.com, + rot: patch.rot, + }; + let cells = emit_volume_bins( + params, + out, + mesh, + other_handle, + (sb, Some(slots)), + None, + Some(rigid), + hard, + &patch.bins, + u32::MAX, + ); + if recovery.overlap_patch_constraints != SoftPatchConstraints::Keep { + out.rigid_patches + .push((other_handle, patch.depth.clone(), cells)); + } + } + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs index ab58a0a15..eaf0cfc25 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs @@ -39,6 +39,12 @@ pub(crate) struct EdgeContactWorkspace { /// warm start lives in the owner's `edge_contacts`) or a vertex-vs-surface row (`vertex_contacts`). pub(super) const SOURCE_EDGE_CONTACT: u32 = u32::MAX; pub(super) const SOURCE_VERTEX_CONTACT: u32 = u32::MAX - 1; +/// The corrective pace deep recovery is allowed (demoted intruders, the volume rows): +/// fast enough to visibly heal, slow enough that a correction cannot pump the material. +pub(super) fn material_pace(params: &IntegrationParameters) -> Real { + params.soft_bodies.recovery.recovery_pace * params.length_unit +} + /// The contact rows of one awake soft body, and its new edge-vs-edge and vertex-vs-surface /// contacts (the rows' `point` indices are positions in those lists). #[derive(Default)] @@ -74,13 +80,55 @@ pub(super) struct BodyContacts { /// (element-side, vertex-side) crossing element pairs. pub(super) cross_tangled_vb_elements: Vec, pub(super) cross_pairs: Vec<(u32, u32)>, + /// The intersection-volume rows this body assembled (see `SoftOverlapRow`). + pub(super) overlap_constraints: Vec, + /// The current pair's volume normals guiding its crossing repulsion (see + /// `crossing_repulsion_guide`): per grid cell, its center and its normal from the + /// element side into the vertex side (for a self pair: the push direction of the fold's + /// vertices, the facing surface's vertices take the opposite). + pub(super) repel_guides: Vec<(Vector, Vector)>, + /// For the current pair, whether each vertex of the vertex side lies in the other + /// side's volume patch (see `overlap_patch_rows`). + pub(super) patch_inside_vb: Vec, + /// The rigid pairs of the current mesh with a volume constraint (see + /// `overlap_patch_rows`): the collider, the surface vertices' depths in its patch + /// (`NEG_INFINITY` outside), and the constraint's cells (center, normal). + pub(super) rigid_patches: Vec<(ColliderHandle, Vec, Vec<(Vector, Vector)>)>, +} + +/// An intersection-volume row assembled by one body (see `SoftOverlapRow`): the gradient +/// triples of every simulated particle of the pair, the rigid side if any, and the row's +/// right-hand side (the signed volume estimate when the correction runs on the row, the +/// slack alone otherwise). +pub(super) struct OverlapConstraintWorkspace { + pub(super) grads: Vec<(u32, Real, Vector, Vector)>, + /// The `(side, particle)` of every gradient entry (see `SoftOverlapRow::warm`). + pub(super) particles: Vec<(u8, u32)>, + pub(super) rigid: Option<(u32, Vector, AngVector)>, + pub(super) rigid_pose0: (Vector, Rotation), + pub(super) rhs: Real, + /// A contact in its own right (see `SoftOverlapRow::hard`). + pub(super) hard: bool, + /// The carried impulses of a hard row (see `SoftOverlapWarm`): the other collider, the + /// fitted multiplier, the carried impulse per gradient entry, and the rigid side's. + pub(super) warm: Option<(ColliderHandle, Real)>, + pub(super) warm_impulses: Vec, + pub(super) warm_rigid: (Vector, AngVector), + pub(super) max_bias_velocity: Real, + /// The pair's contact slot of the row's bin (see `SoftOverlapRow::report`). + pub(super) report: (ColliderHandle, ColliderHandle, u32), } + /// The contact state one collision mesh owns across steps: what the next step warm-starts from. #[derive(Default)] pub(super) struct MeshContacts { pub(super) id: SoftMeshId, pub(super) edge_contacts: Vec, pub(super) vertex_contacts: Vec, + /// The pairs' overlap progress states (the owner's `overlap_states` of the next step). + pub(super) overlap_states: Vec, + /// The volume constraints assembled this step (the owner's `volume_contacts`). + pub(super) volume_contacts: Vec, /// Surface travel since the last self-crossing sweep (the owner's `crossing_sweep_travel` of the /// next step). pub(super) crossing_sweep_travel: Real, @@ -93,6 +141,7 @@ impl BodyContacts { /// Starts assembling the mesh `id`: its contacts go to a list of its own. pub(super) fn begin_mesh(&mut self, id: SoftMeshId) { self.current_mesh = self.meshes.len(); + self.rigid_patches.clear(); self.meshes.push(MeshContacts { id, ..Default::default() @@ -139,7 +188,10 @@ impl BodyContacts { self.cross_tangled_vertices.clear(); self.cross_tangled_vb_elements.clear(); self.cross_pairs.clear(); + self.overlap_constraints.clear(); + } } + /// The step-constant inputs of the per-body contact assembly. pub(super) struct AssemblyCtx<'a> { pub(super) island_id: usize, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs index 87de53205..740e81618 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs @@ -47,6 +47,74 @@ impl SoftConstraintsSet { /// Writes the soft contacts' impulses back to their narrow-phase manifold points (total /// impulse for the contact events, warm-start impulses for the next step). pub fn writeback_contacts(&self, narrow_phase: &mut NarrowPhase) { + // The volume contacts' warm impulses (see `SoftOverlapWarm`): each entry's last-substep + // normal and friction impulse, accumulated per pair; a bin's contact slot reports the + // impulse as the force put on the own side (the multiplier along the own gradients). + for constraint in &self.overlap_constraints { + let (c1, c2, slot) = constraint.report; + if slot != u32::MAX { + let mut own_grad = Vector::ZERO; + for (&(_, _, g, _), &(side, _)) in self.overlap_grads[constraint.grads.clone()] + .iter() + .zip(&self.overlap_particles[constraint.grads.clone()]) + { + if side == 0 { + own_grad += g; + } + } + if let Some(soft) = narrow_phase + .contact_pair_mut(c1, c2) + .and_then(|pair| pair.soft_mut()) + { + soft.report_impulse( + slot, + crate::utils::canonicalize_zero(constraint.impulse * own_grad.length()), + Vector::ZERO, + ); + } + } + let Some((ai, mesh_id, other)) = constraint.warm else { + continue; + }; + // SAFETY: the writeback stage is serial and owns the soft bodies. + let ptr = self.awake[ai as usize].ptr; + let sb = unsafe { &mut *ptr }; + let Some(mesh) = sb.mesh_mut(mesh_id) else { + continue; + }; + let idx = match mesh.overlap_warm.iter().position(|w| w.other == other) { + Some(i) => i, + None => { + mesh.overlap_warm.push(SoftOverlapWarm { + other, + own: Vec::new(), + other_soft: Vec::new(), + rigid: (Vector::ZERO, AngVector::default()), + }); + mesh.overlap_warm.len() - 1 + } + }; + let record = &mut mesh.overlap_warm[idx]; + for (&(_, _, g, _), &(side, p)) in self.overlap_grads[constraint.grads.clone()] + .iter() + .zip(&self.overlap_particles[constraint.grads.clone()]) + { + let impulse = -g * constraint.impulse; + let list = if side == 0 { + &mut record.own + } else { + &mut record.other_soft + }; + match list.iter_mut().find(|e| e.0 == p) { + Some(e) => e.1 += impulse, + None => list.push((p, impulse)), + } + } + if let Some((_, g_lin, g_ang)) = constraint.rigid { + record.rigid.0 += -g_lin * constraint.impulse; + record.rigid.1 += -g_ang * constraint.impulse; + } + } // The constraints of a pair are contiguous: on entering a new pair, clear the reported impulses // of all its points first (points without a constraint this step, e.g. deduplicated vertex // contacts, must not keep reporting a stale force). diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs index 7c8bc431b..70bb25e20 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs @@ -118,6 +118,10 @@ impl SoftConstraintsSet { self.shape_constraints.clear(); self.volume_constraints.clear(); self.volume_grads.clear(); + self.overlap_constraints.clear(); + self.overlap_grads.clear(); + self.overlap_warm_impulses.clear(); + self.overlap_particles.clear(); self.contacts.clear(); self.attachments.clear(); self.groups.clear(); diff --git a/src/dynamics/solver/staged_island_solver/solve.rs b/src/dynamics/solver/staged_island_solver/solve.rs index 108f0583e..10f1cef68 100644 --- a/src/dynamics/solver/staged_island_solver/solve.rs +++ b/src/dynamics/solver/staged_island_solver/solve.rs @@ -25,16 +25,34 @@ pub(super) unsafe fn solve_pass( ) -> usize { let group = &ctx.groups[group_index]; let sync = ctx.sync; - // Soft-body work of this group. The set is shared and immutable in shape during the solve, so - // every worker takes the same branches (identical stage sequences). - // Soft rows are springs solved once per substep, in the biased pass only: solving them - // again in the relax pass (with or without their bias) leaves the two passes fighting over - // the same impulse, and the velocity read out after the relax pass carries the difference. + // Soft-body work of this group: the set is shared and immutable in shape during the solve, so + // every worker takes the same branches. Soft constraints are springs solved once per substep, + // in the biased pass only; a relax-pass re-solve leaves both passes fighting over one impulse. let soft_ref = unsafe { &*ctx.soft_constraints }; let has_soft = !soft_ref.is_empty() && !wo_bias; // Particle attachments are joints: solved in both passes, right after the joints. let has_attachments = !soft_ref.is_empty() && !soft_ref.groups[group_index].attachments.is_empty(); + /* + * Stage: warm start the intersection-volume constraints (worker 0, serial): warm impulses go + * before the elastic constraints, so a resting patch's springs see the contact force instead of + * pushing the patch through it first (see `SoftOverlapWarm`). + */ + if has_soft + && soft_warmstart.is_some() + && !soft_ref.groups[group_index].overlap_constraints.is_empty() + { + if worker_id == 0 { + let soft = unsafe { &mut *ctx.soft_constraints }; + let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; + for oi in soft.groups[group_index].overlap_constraints.clone() { + soft.warmstart_overlap_constraint(oi, solver_bodies); + } + sync.complete(stage, 1, 1); + } + stage = sync.sync(stage, 1); + } + /* * Stage: soft-body prepare (parallel over the group's awake soft bodies): shape-matching * goals and global-volume gradients from the current poses. @@ -446,7 +464,7 @@ unsafe fn solve_soft_constraints( stage = sync.sync(stage, 1); } - if sg.serial.len() > 0 || !sg.volume_rows.is_empty() { + if sg.serial.len() > 0 || !sg.volume_constraints.is_empty() || !sg.overlap_constraints.is_empty() { if worker_id == 0 { let soft = unsafe { &mut *ctx.soft_constraints }; let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; @@ -463,6 +481,9 @@ unsafe fn solve_soft_constraints( for vi in sg.volume_constraints.clone() { soft.solve_volume_constraint(vi, solver_bodies, soft_warmstart); } + for oi in sg.overlap_constraints.clone() { + soft.solve_overlap_constraint(oi, solver_bodies); + } sync.complete(stage, 1, 1); } stage = sync.sync(stage, 1); diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_classify.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_classify.rs new file mode 100644 index 000000000..a97a1ba90 --- /dev/null +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_classify.rs @@ -0,0 +1,364 @@ +//! Membership classification of surface vertices against another closed surface or a rigid collider (patch depths, inside tests). + +use crate::alloc_prelude::*; + +use crate::dynamics::{SoftBody, SoftCollisionMesh}; +use crate::geometry::Collider; +use crate::math::{DIM, Real, Vector}; +use crate::utils::DotProduct; +use parry::bounding_volume::BoundingVolume; +use parry::utils::hashmap::HashMap; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// Projects `point` on element `e` of `mesh`: the closest point, and whether it lies in +/// the element's interior (away from its boundary). +pub(super) fn project_on_element( + mesh: &SoftCollisionMesh, + body: &SoftBody, + e: usize, + point: Vector, +) -> Option<(Vector, bool)> { + use crate::geometry::PointQueryWithLocation; + let element = mesh.element(e); + if element.len() < DIM { + return None; + } + let positions: [Vector; DIM] = core::array::from_fn(|k| mesh.vertex(body, element[k] as usize)); + #[cfg(feature = "dim2")] + let (proj, weights) = { + let (proj, loc) = parry::shape::Segment::new(positions[0], positions[1]) + .project_local_point_and_get_location(point, false); + (proj, loc.barycentric_coordinates()) + }; + #[cfg(feature = "dim3")] + let (proj, weights) = { + let (proj, loc) = parry::shape::Triangle::new(positions[0], positions[1], positions[2]) + .project_local_point_and_get_location(point, false); + (proj, loc.barycentric_coordinates()?) + }; + let interior = weights.iter().all(|&w| w > 1.0e-3); + Some((proj.point, interior)) +} + +/// Classifies the surface vertices of `mesh` against the closed `other` mesh by signed distance: +/// `depth[v] = rest - d` (`d` negative inside) within `reach` or inside, `NEG_INFINITY` otherwise. +/// Near vertices project on the BVH, others flood per component; returns (any member, any inside). +#[allow(clippy::too_many_arguments)] +pub(super) fn classify_skin( + mesh: &SoftCollisionMesh, + body: &SoftBody, + crossing: &[bool], + (other_mesh, other, other_co): (&SoftCollisionMesh, &SoftBody, &Collider), + reach: Real, + rest: Real, + depth: &mut Vec, +) -> (bool, bool) { + let n_v = mesh.vertex_count(); + depth.clear(); + depth.resize(n_v, Real::NEG_INFINITY); + let Some(other_bvh) = other_co.shape().as_composite_shape().map(|c| c.bvh()) else { + return (false, false); + }; + let other_inv_pose = other_co.position().inverse(); + let other_aabb = other_co.compute_aabb().loosened(reach); + let mut any = false; + let mut deep = false; + let mut near = vec![false; n_v]; + for v in 0..n_v { + if !mesh.vertex_on_surface(v) { + continue; + } + let p = mesh.vertex(body, v); + if !other_aabb.contains_local_point(p) { + continue; + } + let aabb = crate::geometry::Aabb::from_half_extents(p, Vector::splat(reach)) + .transform_by(&other_inv_pose); + let mut best: Option<(Real, u32, Vector, bool)> = None; + for e in other_bvh.intersect_aabb(&aabb) { + let Some((q, interior)) = project_on_element(other_mesh, other, e as usize, p) else { + continue; + }; + let d2 = (p - q).length_squared(); + if best.is_none_or(|(bd, ..)| d2 < bd) { + best = Some((d2, e, q, interior)); + } + } + let Some((d2, e, q, interior)) = best.filter(|b| b.0 <= reach * reach) else { + continue; + }; + near[v] = true; + let d = d2.sqrt(); + let inside = if d <= 1.0e-9 * (1.0 + reach) { + false + } else { + match other_mesh.element_outward_normal(other, e as usize) { + Some(n) => { + let along = (p - q).gdot(n); + let n_len = n.length(); + if interior || along.abs() > 0.7 * d * n_len { + along < 0.0 + } else { + other_mesh.contains_point_parity(other, p) + } + } + None => other_mesh.contains_point_parity(other, p), + } + }; + depth[v] = if inside { rest + d } else { rest - d }; + any = true; + deep |= inside; + } + // The vertices beyond reach: components over the non-crossing elements made of such + // vertices only, one parity test each; an inside component's vertices get their + // distance from the BVH. + let mut parent: Vec = (0..n_v as u32).collect(); + fn find(parent: &mut [u32], mut i: u32) -> u32 { + while parent[i as usize] != i { + parent[i as usize] = parent[parent[i as usize] as usize]; + i = parent[i as usize]; + } + i + } + let mut touched = vec![false; n_v]; + for (f, el) in mesh.indices().iter().enumerate() { + if crossing.get(f).copied().unwrap_or(false) || el.iter().any(|&v| near[v as usize]) { + continue; + } + for &v in el { + touched[v as usize] = true; + } + for k in 1..el.len() { + let (a, b) = (find(&mut parent, el[0]), find(&mut parent, el[k])); + if a != b { + parent[a as usize] = b; + } + } + } + let mut root_inside: HashMap = HashMap::default(); + for v in 0..n_v { + if near[v] || !mesh.vertex_on_surface(v) { + continue; + } + let p = mesh.vertex(body, v); + if !other_aabb.contains_local_point(p) { + continue; + } + let inside = if touched[v] { + let root = find(&mut parent, v as u32); + *root_inside + .entry(root) + .or_insert_with(|| other_mesh.contains_point_parity(other, p)) + } else { + other_mesh.contains_point_parity(other, p) + }; + if inside { + let proj = other_co + .shape() + .project_point(other_co.position(), p, false); + depth[v] = rest + (proj.point - p).length(); + any = true; + deep = true; + } + } + if any { + ring_depths(mesh, body, other_co, rest, depth); + } + (any, deep) +} + +/// The depth of the outside vertices sharing an element with a member (see +/// `patch_vertices`): their distance to the other collider, so the elements crossing the +/// reach are clipped where they do, not halfway. +pub(super) fn ring_depths( + mesh: &SoftCollisionMesh, + body: &SoftBody, + other_co: &Collider, + rest: Real, + depth: &mut [Real], +) { + let member = |v: u32, depth: &[Real]| depth[v as usize] > Real::NEG_INFINITY; + let mut ring: Vec = Vec::new(); + for el in mesh.indices() { + if el.iter().any(|&v| member(v, depth)) { + for &v in el { + if !member(v, depth) && !ring.contains(&v) { + ring.push(v); + } + } + } + } + for v in ring { + let p = mesh.vertex(body, v as usize); + let proj = other_co + .shape() + .project_point(other_co.position(), p, false); + depth[v as usize] = rest - (proj.point - p).length(); + } +} + +/// Whether the surface vertices of the closed `mesh` lie in their own body's self-intersection +/// region (a mirrored lobe, a fold pushed through): non-crossing patches flood and parity-test as +/// one, crossing vertices individually, each point nudged outward by half the contact `skin`. +pub(super) fn classify_inside_self( + mesh: &SoftCollisionMesh, + body: &SoftBody, + skin: Real, + crossing: &[bool], + vertex_elements: &[Vec], + inside: &mut Vec, +) -> bool { + let n_v = mesh.vertex_count(); + inside.clear(); + inside.resize(n_v, false); + let nudge = (0.5 * skin).max(1.0e-4); + let probe = |v: usize| -> Option { + let mut n = Vector::ZERO; + for &e in vertex_elements.get(v)? { + n += mesh.element_outward_normal(body, e as usize)?; + } + Some(mesh.vertex(body, v) + n.try_normalize()? * nudge) + }; + let mut parent: Vec = (0..n_v as u32).collect(); + fn find(parent: &mut [u32], mut i: u32) -> u32 { + while parent[i as usize] != i { + parent[i as usize] = parent[parent[i as usize] as usize]; + i = parent[i as usize]; + } + i + } + let mut touched = vec![false; n_v]; + for (f, el) in mesh.indices().iter().enumerate() { + if crossing.get(f).copied().unwrap_or(false) { + continue; + } + for &v in el { + touched[v as usize] = true; + } + for k in 1..el.len() { + let (a, b) = (find(&mut parent, el[0]), find(&mut parent, el[k])); + if a != b { + parent[a as usize] = b; + } + } + } + let mut root_inside: HashMap = HashMap::default(); + let mut any = false; + for v in 0..n_v { + if !mesh.vertex_on_surface(v) { + continue; + } + let test = |v: usize| probe(v).is_some_and(|p| mesh.contains_point_parity(body, p)); + inside[v] = if touched[v] { + let root = find(&mut parent, v as u32); + *root_inside + .entry(root) + .or_insert_with(|| test(root as usize)) + } else { + test(v) + }; + any |= inside[v]; + } + any +} + +/// Classifies the surface vertices of `mesh` against a rigid collider, like +/// `classify_skin`: by containment alone when `reach` is `None` (the intruding patch), by +/// signed distance within the reach otherwise. +pub(super) fn classify_skin_rigid( + mesh: &SoftCollisionMesh, + body: &SoftBody, + other_co: &Collider, + reach: Option, + rest: Real, + depth: &mut Vec, +) -> (bool, bool) { + let n_v = mesh.vertex_count(); + depth.clear(); + depth.resize(n_v, Real::NEG_INFINITY); + let aabb = other_co.compute_aabb().loosened(reach.unwrap_or(0.0)); + let mut any = false; + let mut deep = false; + for v in 0..n_v { + if !mesh.vertex_on_surface(v) { + continue; + } + let p = mesh.vertex(body, v); + if !aabb.contains_local_point(p) { + continue; + } + let inside = other_co.shape().contains_point(other_co.position(), p); + if !inside && reach.is_none() { + continue; + } + let proj = other_co + .shape() + .project_point(other_co.position(), p, false); + let d = (proj.point - p).length(); + if !inside && d > reach.unwrap_or(0.0) { + continue; + } + depth[v] = if inside { rest + d } else { rest - d }; + any = true; + deep |= inside; + } + if any { + ring_depths(mesh, body, other_co, rest, depth); + } + (any, deep) +} + +/// Whether the vertices of `mesh` lie inside the closed `other` mesh: the patches of +/// non-crossing elements are flooded (union-find through shared vertices) and classified by +/// one parity test each, the crossing elements' vertices individually. +pub(super) fn classify_inside( + mesh: &SoftCollisionMesh, + body: &SoftBody, + crossing: &[bool], + (other_mesh, other): (&SoftCollisionMesh, &SoftBody), + inside: &mut Vec, +) { + let n_v = mesh.vertex_count(); + inside.clear(); + inside.resize(n_v, false); + let mut parent: Vec = (0..n_v as u32).collect(); + fn find(parent: &mut [u32], mut i: u32) -> u32 { + while parent[i as usize] != i { + parent[i as usize] = parent[parent[i as usize] as usize]; + i = parent[i as usize]; + } + i + } + let mut touched = vec![false; n_v]; + for (f, el) in mesh.indices().iter().enumerate() { + if crossing.get(f).copied().unwrap_or(false) { + continue; + } + for &v in el { + touched[v as usize] = true; + } + for k in 1..el.len() { + let (a, b) = (find(&mut parent, el[0]), find(&mut parent, el[k])); + if a != b { + parent[a as usize] = b; + } + } + } + let mut root_inside: HashMap = HashMap::default(); + for v in 0..n_v { + if !mesh.vertex_on_surface(v) { + continue; + } + let p = mesh.vertex(body, v); + inside[v] = if touched[v] { + let root = find(&mut parent, v as u32); + *root_inside + .entry(root) + .or_insert_with(|| other_mesh.contains_point_parity(other, p)) + } else { + other_mesh.contains_point_parity(other, p) + }; + } +} diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs new file mode 100644 index 000000000..b377d13fa --- /dev/null +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs @@ -0,0 +1,155 @@ +//! The pair entry points of the soft contact detection: the update of a soft-soft pair (both vertex passes, the edge pass, the volume patches) and of a soft-rigid pair (the predictive vertex contacts, the rigid patch). + +use crate::alloc_prelude::*; + +use crate::dynamics::{RigidBodySet, SoftMeshRef}; +use crate::geometry::{Collider, ColliderHandle, ContactPair, PairContacts, RigidPairContacts}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::soft_contacts_volume::{detect_rigid_patch, detect_volume_patch}; +use super::{ + SoftDetectionCtx, SoftVertexPass, SoftEdgePass, SoftPairContacts, body_frozen, detect_vertex_pass, + detect_edges, +}; + +/// The edge-pass owner of a pair of meshes: the lower `(body handle, cluster, mesh)`, which is +/// the solver's awake order for two touching bodies (they share a substep group). +fn owns_edge_pass(a: SoftMeshRef, b: SoftMeshRef) -> bool { + (a.body.0.into_raw_parts(), a.id.cluster, a.id.mesh) + <= (b.body.0.into_raw_parts(), b.id.cluster, b.id.mesh) +} + + +pub(crate) fn update_pair_soft_soft( + pair: &mut ContactPair, + co1: &Collider, + co2: &Collider, + ctx: &SoftDetectionCtx, +) { + let (h1, h2) = (pair.collider1, pair.collider2); + if let (Some(r1), Some(r2)) = (co1.deformable_mesh_ref, co2.deformable_mesh_ref) { + let PairContacts::Soft { + touching, + candidates, + } = &mut pair.contacts + else { + return; + }; + candidates.clear(); + detect_soft_pair(candidates, (h1, co1, r1), (h2, co2, r2), ctx); + candidates.number_slots(); + *touching = candidates.is_touching(); + } +} + +pub(crate) fn update_pair_soft_rigid( + rigid: &mut RigidPairContacts, + h1: ColliderHandle, + h2: ColliderHandle, + co1: &Collider, + co2: &Collider, + bodies: &RigidBodySet, + ctx: &SoftDetectionCtx, +) { +} + match (co1.deformable_mesh_ref, co2.deformable_mesh_ref) { + (Some(r), None) | (None, Some(r)) => { + let (soft_co, other_co, handle) = if co1.deformable_mesh_ref.is_some() { + (co1, co2, h1) + } else { + (co2, co1, h2) + }; + rigid.soft_patch = ctx + .soft_bodies + .get(r.body) + .and_then(|sb| sb.mesh(r.id).map(|mesh| (sb, mesh))) + .and_then(|(sb, mesh)| { + detect_rigid_patch((sb, mesh, handle, soft_co), other_co, bodies, ctx) + }) + .map(Box::new); + } + _ => {} +} + +/// The detection of a pair of two soft surfaces (see [`update_pair`]), into the cleared +/// candidates. +fn detect_soft_pair( + soft: &mut SoftPairContacts, + (h1, co1, r1): (ColliderHandle, &Collider, SoftMeshRef), + (h2, co2, r2): (ColliderHandle, &Collider, SoftMeshRef), + ctx: &SoftDetectionCtx, +) { + let (Some(sb1), Some(sb2)) = (ctx.soft_bodies.get(r1.body), ctx.soft_bodies.get(r2.body)) + else { + return; + }; + let (Some(mesh1), Some(mesh2)) = (sb1.mesh(r1.id), sb2.mesh(r2.id)) else { + return; + }; + // Two meshes of one body: alternative descriptions of the same particles when they share a + // cluster (a hull and a skin), so they never collide; across clusters they do, but only if + // both opted in and their clusters share no particle. + if r1.body == r2.body { + let same_cluster = r1.id.cluster == r2.id.cluster; + if same_cluster + || !mesh1.self_contacts_enabled() + || !mesh2.self_contacts_enabled() + || !sb1.clusters_are_disjoint(r1.id.cluster, r2.id.cluster) + { + return; + } + } + // Two frozen surfaces have no DOF between them. + let (frozen1, frozen2) = (body_frozen(sb1), body_frozen(sb2)); + if frozen1 && frozen2 { + return; + } + // Both vertex passes: each surface's owner consumes the other's vertices against it (and + // the reverse pass too when the other body is not simulated). + let mut pass1 = SoftVertexPass::default(); + let mut pass2 = SoftVertexPass::default(); + detect_vertex_pass( + &mut pass1, + (sb1, mesh1, h1, co1), + (sb2, mesh2, h2, co2), + None, + ctx, + ); + detect_vertex_pass( + &mut pass2, + (sb2, mesh2, h2, co2), + (sb1, mesh1, h1, co1), + None, + ctx, + ); + // The edge pass, oriented from its owner: a crossed closed-closed pair whose volume constraint + // stands its edges down (see `overlap_edge_stand_down`) skips it once the owner's vertex pass + // found the crossing; the solver applies the per-vertex part of that rule itself. + let (own, other) = if owns_edge_pass(r1, r2) { + ((sb1, mesh1, h1, co1), (sb2, mesh2, h2, co2)) + } else { + ((sb2, mesh2, h2, co2), (sb1, mesh1, h1, co1)) + }; + let owner_pass = if own.2 == h1 { &pass1 } else { &pass2 }; + let recovery = &ctx.params.soft_bodies.recovery; + let stood_down = cfg!(feature = "dim3") + && mesh1.is_closed() + && mesh2.is_closed() + && recovery.overlap_constraints + && recovery.overlap_edge_stand_down + && !owner_pass.cross_tangled_elements.is_empty(); + let mut edges = SoftEdgePass::default(); + let has_edges = !stood_down && detect_edges(&mut edges, own, other, None, ctx); + // The volume patches, from the vertex pass of the pair's lower surface. + let (lower_pass, lower, higher) = if h1.into_raw_parts() < h2.into_raw_parts() { + (&pass1, (sb1, mesh1, h1, co1), (sb2, mesh2, h2, co2)) + } else { + (&pass2, (sb2, mesh2, h2, co2), (sb1, mesh1, h1, co1)) + }; + soft.volume = detect_volume_patch(lower_pass, lower, higher, ctx); + soft.vertex_passes.push(pass1); + soft.vertex_passes.push(pass2); + soft.edges = has_edges.then_some(edges); +} diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs index 3b657bfd5..faa2abf04 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs @@ -193,6 +193,64 @@ pub(crate) fn detect_vertex_pass( } else { (Vec::new(), Vec::new()) }; + // The guiding volume normals (see `crossing_repulsion_guide`): the intersection patches of the + // two closed surfaces, binned like the volume contact, computed in every vertex pass and + // without any volume constraint; the vertices inside the surface side's patch come with them. + let patch_policy = params.soft_bodies.recovery.overlap_patch_constraints; + if ((repel && params.soft_bodies.recovery.crossing_repulsion_guide) + || patch_policy != crate::dynamics::SoftPatchConstraints::Keep) + && !is_self + && vb_mesh.is_closed() + && eb_mesh.is_closed() + && !out.cross_pairs.is_empty() + { + let mut inside_vb = Vec::new(); + let mut inside_eb = Vec::new(); + let mut depth_vb = Vec::new(); + let mut depth_eb = Vec::new(); + classify_inside( + vb_mesh, + vb, + &out.cross_tangled_vb_elements, + (eb_mesh, eb), + &mut inside_vb, + ); + classify_inside( + eb_mesh, + eb, + &out.cross_tangled_elements, + (vb_mesh, vb), + &mut inside_eb, + ); + let a1 = fill_depths(vb_mesh, vb, &inside_vb, eb_co, &mut depth_vb); + let a2 = fill_depths(eb_mesh, eb, &inside_eb, vb_co, &mut depth_eb); + if patch_policy != crate::dynamics::SoftPatchConstraints::Keep { + out.patch_inside_vb = inside_vb; + } + if a1 || a2 { + if a1 { + ring_depths(vb_mesh, vb, eb_co, 0.0, &mut depth_vb); + } + if a2 { + ring_depths(eb_mesh, eb, vb_co, 0.0, &mut depth_eb); + } + let own = VolumeSide { + body: eb, + mesh: eb_mesh, + vertices: patch_vertices(eb_mesh, eb, &depth_eb, 0.0), + }; + let other = VolumeSide { + body: vb, + mesh: vb_mesh, + vertices: patch_vertices(vb_mesh, vb, &depth_vb, 0.0), + }; + for bin in volume_bins(&own, Some(&other), volume_split(params)) { + if bin.normal != Vector::ZERO { + out.repel_guides.push((bin.center, bin.normal)); + } + } + } + } } // A tangled vertex (backed by inverted material, or part of a surface if len >= reach || len < 1.0e-6 { diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs new file mode 100644 index 000000000..559254ec4 --- /dev/null +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs @@ -0,0 +1,559 @@ +//! The volume patches of crossed closed surfaces (soft-soft and soft-rigid) and their binning into volume constraints. + +use crate::alloc_prelude::*; + +use crate::dynamics::{IntegrationParameters, RigidBodySet, SoftBody, SoftCollisionMesh}; +use crate::geometry::Collider; +use crate::math::{DIM, Real, Vector}; +use crate::utils::{DotProduct, OrthonormalBasis}; +use parry::utils::hashmap::HashMap; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::soft_contacts_classify::{classify_inside, classify_skin, classify_skin_rigid, ring_depths}; +use super::{Side, SoftDetectionCtx, SoftVertexPass, SoftRigidPatch, SoftVolumePatch, body_frozen}; + +/// The multi-volume grid's cells per tangent axis (see `overlap_multi_volume`). +pub(super) fn volume_split(params: &IntegrationParameters) -> u32 { + if params.soft_bodies.recovery.overlap_multi_volume { + params.soft_bodies.recovery.overlap_split.max(1) + } else { + 1 + } +} + +/// The depth of the vertices `inside` the other collider: their distance to its surface +/// (`NEG_INFINITY` for the others). Returns whether any vertex is inside. +pub(super) fn fill_depths( + mesh: &SoftCollisionMesh, + body: &SoftBody, + inside: &[bool], + co: &Collider, + depth: &mut Vec, +) -> bool { + depth.clear(); + depth.resize(mesh.vertex_count(), Real::NEG_INFINITY); + let mut any = false; + for v in 0..mesh.vertex_count() { + if inside[v] { + let p = mesh.vertex(body, v); + let proj = co.shape().project_point(co.position(), p, false); + depth[v] = (proj.point - p).length(); + any = true; + } + } + any +} + +/// Whether a closed mesh is self-crossed this step (part of its winding is mirrored, so its +/// volume gradient points the wrong way there): its volume constraints sit out. +fn self_tangled(m: &SoftCollisionMesh) -> bool { + m.orientation_unreliable || m.crossed_partners.contains(&m.collider()) +} + +/// The volume patches of a crossed pair of closed surfaces (see `SoftOverlapConstraint`, Allard et +/// al. 2010): both intruding patches, on the skin-dilated surfaces under `overlap_skin_volume`, +/// binned from `pass` (the vertices of `vb` against the surface of `eb`, the bins' own side). +pub(super) fn detect_volume_patch( + pass: &SoftVertexPass, + (eb, eb_mesh, eb_handle, eb_co): Side<'_>, + (vb, vb_mesh, _, vb_co): Side<'_>, + ctx: &SoftDetectionCtx, +) -> Option { + let params = ctx.params; + let recovery = ¶ms.soft_bodies.recovery; + let closed_pair = vb_mesh.is_closed() && eb_mesh.is_closed(); + let crossed = !pass.cross_pairs.is_empty() && !pass.cross_tangled_vb_elements.is_empty(); + if !recovery.overlap_constraints + || !closed_pair + || !crossed + || (recovery.overlap_skip_self_tangled && (self_tangled(vb_mesh) || self_tangled(eb_mesh))) + { + return None; + } + let skins = vb_co.contact_skin() + eb_co.contact_skin(); + let mut depth_vb = Vec::new(); + let mut depth_eb = Vec::new(); + let band = params.prediction_distance() + ctx.motion_margin(eb_co) + ctx.motion_margin(vb_co); + let any = if recovery.overlap_skin_volume { + let rest = (1.0 - recovery.overlap_kept_depth.clamp(0.0, 1.0)) * skins; + let reach = skins + band; + let (a1, _) = classify_skin( + vb_mesh, + vb, + &pass.cross_tangled_vb_elements, + (eb_mesh, eb, eb_co), + reach, + rest, + &mut depth_vb, + ); + let (a2, _) = classify_skin( + eb_mesh, + eb, + &pass.cross_tangled_elements, + (vb_mesh, vb, vb_co), + reach, + rest, + &mut depth_eb, + ); + a1 || a2 + } else { + // The geometric intersection alone: the inside vertices, at their distance to the + // other surface. + let mut inside_vb = Vec::new(); + let mut inside_eb = Vec::new(); + classify_inside( + vb_mesh, + vb, + &pass.cross_tangled_vb_elements, + (eb_mesh, eb), + &mut inside_vb, + ); + classify_inside( + eb_mesh, + eb, + &pass.cross_tangled_elements, + (vb_mesh, vb), + &mut inside_eb, + ); + let a1 = fill_depths(vb_mesh, vb, &inside_vb, eb_co, &mut depth_vb); + let a2 = fill_depths(eb_mesh, eb, &inside_eb, vb_co, &mut depth_eb); + if a1 { + ring_depths(vb_mesh, vb, eb_co, 0.0, &mut depth_vb); + } + if a2 { + ring_depths(eb_mesh, eb, vb_co, 0.0, &mut depth_eb); + } + a1 || a2 + }; + if !any { + return None; + } + let band = if recovery.overlap_skin_volume { + band + } else { + 0.0 + }; + let own = VolumeSide { + body: eb, + mesh: eb_mesh, + vertices: patch_vertices(eb_mesh, eb, &depth_eb, band), + }; + let other = VolumeSide { + body: vb, + mesh: vb_mesh, + vertices: patch_vertices(vb_mesh, vb, &depth_vb, band), + }; + Some(SoftVolumePatch { + own: eb_handle, + bins: volume_bins(&own, Some(&other), volume_split(params)), + slot_offset: u32::MAX, + }) +} + +/// The volume patch of a closed surface against a rigid collider (see `SoftOverlapConstraint`): +/// the surface vertices inside the rigid shape (plus its skin layer under the skin models), found +/// independently of the pair's manifolds (a shape fully inside the surface has no contact). +pub(super) fn detect_rigid_patch( + (sb, mesh, handle, surface_co): Side<'_>, + other_co: &Collider, + bodies: &RigidBodySet, + ctx: &SoftDetectionCtx, +) -> Option { + let params = ctx.params; + let recovery = ¶ms.soft_bodies.recovery; + if !recovery.overlap_constraints + || !recovery.overlap_rigid + || body_frozen(sb) + || !mesh.collision_enabled() + || other_co.is_sensor() + { + return None; + } + // A self-crossed mesh sits out: part of its winding is mirrored. + if !mesh.is_closed() + || (recovery.overlap_skip_self_tangled + && (mesh.orientation_unreliable || mesh.crossed_partners.contains(&handle))) + { + return None; + } + let skin = recovery.overlap_skin_volume; + let other_rb = other_co.parent().and_then(|h| bodies.get(h)); + let skins = surface_co.contact_skin() + other_co.contact_skin(); + let rest = if skin { + (1.0 - recovery.overlap_kept_depth.clamp(0.0, 1.0)) * skins + } else { + 0.0 + }; + let band = params.prediction_distance() + ctx.motion_margin(surface_co); + let reach = skin.then_some(skins + band); + let mut depth = Vec::new(); + let (any, _) = classify_skin_rigid(mesh, sb, other_co, reach, rest, &mut depth); + if !any { + return None; + } + let own = VolumeSide { + body: sb, + mesh, + vertices: patch_vertices(mesh, sb, &depth, if skin { band } else { 0.0 }), + }; + let bins = volume_bins(&own, None, volume_split(params)); + Some(SoftRigidPatch { + bins, + depth: if recovery.overlap_patch_constraints != crate::dynamics::SoftPatchConstraints::Keep { + depth + } else { + Vec::new() + }, + com: other_rb.map_or(other_co.translation(), |rb| rb.center_of_mass()), + rot: other_co.position().rotation, + }) +} + +/// Accumulates, per vertex of `mesh`, the gradient of the enclosed area/volume over the elements +/// `keep` accepts (weighted as it returns); translation-invariant per element, so a subset of a +/// closed boundary is fine. `sign` flips the gradient (a mirrored lobe raises its negative area). +fn partial_volume_gradients( + mesh: &SoftCollisionMesh, + body: &SoftBody, + sign: Real, + mut keep: impl FnMut(usize) -> Option, + grads: &mut [Vector], +) { + for (f, el) in mesh.indices().iter().enumerate() { + let Some(w) = keep(f) else { + continue; + }; + #[cfg(feature = "dim2")] + { + let (a, b) = ( + mesh.vertex(body, el[0] as usize), + mesh.vertex(body, el[1] as usize), + ); + let d = b - a; + let g = Vector::new(d.y, -d.x) * (0.5 * w * sign); + grads[el[0] as usize] += g; + grads[el[1] as usize] += g; + } + #[cfg(feature = "dim3")] + { + let (a, b, c) = ( + mesh.vertex(body, el[0] as usize), + mesh.vertex(body, el[1] as usize), + mesh.vertex(body, el[2] as usize), + ); + let n = (b - a).cross(c - a); + let g = n * (w * sign / 6.0); + for &v in el { + grads[v as usize] += g; + } + } + } +} + +/// A vertex of a volume contact's patch: its share of the (partial) volume gradient and its +/// depth in the other side's volume (the skin-dilated one under the skin models, where it +/// is negative in the speculative band and above the kept layer). +#[derive(Copy, Clone)] +pub(super) struct PatchVertex { + vertex: u32, + grad: Vector, + depth: Real, +} + +/// One side of a volume contact: its patch vertices. +pub(super) struct VolumeSide<'a> { + pub(super) body: &'a SoftBody, + pub(super) mesh: &'a SoftCollisionMesh, + pub(super) vertices: Vec, +} + +/// One volume constraint of a pair (see `SoftOverlapConstraint`): the whole patch, or a cell of +/// the multi-volume grid (Allard et al. 2010, section 5), as particle entries (particle, +/// gradient) per side. +#[derive(Clone, Debug)] +pub struct VolumeBin { + /// The own side's entries: (particle, volume gradient). + pub own: Vec<(u32, Vector)>, + /// The other side's entries: (particle, volume gradient); empty against a rigid collider. + pub other: Vec<(u32, Vector)>, + /// The signed volume estimate, `sum depth |g|` over both sides (negative: slack). + pub volume: Real, + /// The contact normal, from the own side into the other (zero: degenerate). + pub normal: Vector, + /// The area-weighted center of the bin's vertices. + pub center: Vector, + /// Whether the solver builds a constraint for it (the hook clears it to drop the constraint). + pub enabled: bool, + /// The impulse the solver applied through it at the last step (its multiplier along + /// the own side's gradients). + pub impulse: Real, +} + +/// The fraction of an element lying in the volume where the depth (offset by the band, +/// see `patch_vertices`) is positive, the zero crossing interpolated linearly along its +/// edges: exact for a segment, the standard triangle clip in 3D. +fn clipped_fraction(s: &[Real]) -> Real { + let inside: Vec = (0..s.len()).filter(|&i| s[i] >= 0.0).collect(); + if inside.len() == s.len() { + return 1.0; + } + if inside.is_empty() { + return 0.0; + } + // The inside vertex's share along an edge to an outside vertex (half the edge when + // that vertex has no depth: the member-fraction rule). + let share = |a: usize, b: usize| -> Real { + if s[b] == Real::NEG_INFINITY { + 0.5 + } else { + (s[a] / (s[a] - s[b])).clamp(0.0, 1.0) + } + }; + #[cfg(feature = "dim2")] + { + let a = inside[0]; + share(a, 1 - a) + } + #[cfg(feature = "dim3")] + { + if inside.len() == 1 { + let a = inside[0]; + let (b, c) = ((a + 1) % 3, (a + 2) % 3); + share(a, b) * share(a, c) + } else { + let o = (0..3).find(|i| !inside.contains(i)).unwrap(); + let (b, c) = ((o + 1) % 3, (o + 2) % 3); + let out_share = |i: usize| { + if s[o] == Real::NEG_INFINITY { + 0.5 + } else { + (s[i] / (s[i] - s[o])).clamp(0.0, 1.0) + } + }; + 1.0 - (1.0 - out_share(b)) * (1.0 - out_share(c)) + } + } +} + +/// The patch of `mesh` made of its member vertices (`depth[v]` finite): every element weighs the +/// fraction of it within reach (depth offset by the speculative slack `band`, interpolated along +/// its edges), and each of its vertices gets its share of the volume gradient (none if depthless). +pub(super) fn patch_vertices( + mesh: &SoftCollisionMesh, + body: &SoftBody, + depth: &[Real], + band: Real, +) -> Vec { + let member = |v: usize| depth[v] > Real::NEG_INFINITY; + let mut grads = vec![Vector::ZERO; mesh.vertex_count()]; + // The gradient follows the geometric orientation, not the raw winding: a loop turned + // inside out (crushed against a wall, no self-crossing) would otherwise be inflated + // out of the wall instead of shrunk. + partial_volume_gradients( + mesh, + body, + mesh.winding_sign(), + |f| { + let el = mesh.element(f); + let s: [Real; DIM] = core::array::from_fn(|k| { + el.get(k).map_or(Real::NEG_INFINITY, |&v| { + let d = depth[v as usize]; + if d == Real::NEG_INFINITY { d } else { d + band } + }) + }); + let w = clipped_fraction(&s[..el.len().min(DIM)]); + (w > 0.0).then_some(w) + }, + &mut grads, + ); + // A vertex beyond the reach (a ring vertex, whose depth only places the clip) bounds + // the patch without adding volume. + grads + .iter() + .enumerate() + .filter(|(_, g)| **g != Vector::ZERO) + .map(|(v, g)| PatchVertex { + vertex: v as u32, + grad: *g, + depth: if member(v) && depth[v] >= -band { + depth[v] + } else { + 0.0 + }, + }) + .collect() +} + +/// The particle-level entries of a side's patch vertices (a skinned vertex spread over its +/// cell), sorted by particle. +fn side_entries(side: &VolumeSide, picked: &[usize]) -> Vec<(u32, Vector)> { + let mut by_particle: HashMap = HashMap::default(); + for &i in picked { + let pv = side.vertices[i]; + let (anchors, weights) = side.mesh.vertex_anchors(side.body, pv.vertex as usize); + for (p, w) in anchors.iter().zip(&weights) { + if *p != u32::MAX && *w != 0.0 { + *by_particle.entry(*p).or_insert(Vector::ZERO) += pv.grad * *w; + } + } + } + let mut entries: Vec<(u32, Vector)> = by_particle.into_iter().collect(); + entries.sort_by_key(|e| e.0); + entries +} + +/// Splits a pair's patches into the volume constraints: one bin (mono-volume), or `split` cells +/// per tangent axis of a regular grid over the patches, aligned with the pair's mean normal +/// (multi-volume). A cell missing one side joins the nearest two-sided cell; with none, one bin. +pub(super) fn volume_bins(own: &VolumeSide, other: Option<&VolumeSide>, split: u32) -> Vec { + let n_own = own.vertices.len(); + let n_other = other.map_or(0, |o| o.vertices.len()); + if n_own + n_other == 0 { + return Vec::new(); + } + // Every vertex of both sides: position, gradient (the other side's negated, so the + // sum points from the own side into the other), and its side index. + let pos = |i: usize| -> Vector { + if i < n_own { + own.mesh.vertex(own.body, own.vertices[i].vertex as usize) + } else { + let o = other.unwrap(); + o.mesh.vertex(o.body, o.vertices[i - n_own].vertex as usize) + } + }; + let pv = |i: usize| -> PatchVertex { + if i < n_own { + own.vertices[i] + } else { + other.unwrap().vertices[i - n_own] + } + }; + let total = n_own + n_other; + let mut normal_sum = Vector::ZERO; + for i in 0..total { + normal_sum += if i < n_own { pv(i).grad } else { -pv(i).grad }; + } + let normal = normal_sum.try_normalize().unwrap_or(Vector::ZERO); + // The cells: each vertex's grid coordinates along the tangent axes. + let mut cells: Vec> = Vec::new(); + let split = split.max(1) as usize; + if split > 1 && normal != Vector::ZERO && total >= 2 { + let tangents = normal.orthonormal_basis(); + let mut center = Vector::ZERO; + for i in 0..total { + center += pos(i); + } + center /= total as Real; + let coords: Vec<[Real; DIM - 1]> = (0..total) + .map(|i| core::array::from_fn(|j| (pos(i) - center).gdot(tangents[j]))) + .collect(); + let mut lo = [Real::MAX; DIM - 1]; + let mut hi = [-Real::MAX; DIM - 1]; + for c in &coords { + for j in 0..DIM - 1 { + lo[j] = lo[j].min(c[j]); + hi[j] = hi[j].max(c[j]); + } + } + let cell_of = |c: &[Real; DIM - 1]| -> usize { + let mut idx = 0; + for j in 0..DIM - 1 { + let span = hi[j] - lo[j]; + let k = if span > 0.0 { + (((c[j] - lo[j]) / span) * split as Real) as usize + } else { + 0 + }; + idx = idx * split + k.min(split - 1); + } + idx + }; + let n_cells = split.pow((DIM - 1) as u32); + cells.resize(n_cells, Vec::new()); + for (i, c) in coords.iter().enumerate() { + cells[cell_of(c)].push(i); + } + let two_sided = |cell: &Vec| { + other.is_none() || (cell.iter().any(|&i| i < n_own) && cell.iter().any(|&i| i >= n_own)) + }; + let cell_center = |k: usize| -> [Real; DIM - 1] { + let mut c = [0.0; DIM - 1]; + let mut idx = k; + for j in (0..DIM - 1).rev() { + c[j] = (idx % split) as Real; + idx /= split; + } + c + }; + let keep: Vec = cells + .iter() + .map(|c| !c.is_empty() && two_sided(c)) + .collect(); + if keep.iter().any(|&k| k) { + for k in 0..n_cells { + if keep[k] || cells[k].is_empty() { + continue; + } + let ck = cell_center(k); + let mut best = (Real::MAX, usize::MAX); + for t in 0..n_cells { + if !keep[t] { + continue; + } + let ct = cell_center(t); + let d2: Real = (0..DIM - 1).map(|j| (ck[j] - ct[j]).powi(2)).sum(); + if d2 < best.0 { + best = (d2, t); + } + } + let moved = core::mem::take(&mut cells[k]); + cells[best.1].extend(moved); + } + cells.retain(|c| !c.is_empty()); + } else { + cells.clear(); + } + } + if cells.is_empty() { + cells.push((0..total).collect()); + } + cells + .iter() + .map(|cell| { + let own_idx: Vec = cell.iter().copied().filter(|&i| i < n_own).collect(); + let other_idx: Vec = cell + .iter() + .filter(|&&i| i >= n_own) + .map(|&i| i - n_own) + .collect(); + let mut volume = 0.0; + let mut normal_sum = Vector::ZERO; + let mut center = Vector::ZERO; + let mut weight = 0.0; + for &i in cell { + let v = pv(i); + let a = v.grad.length(); + volume += v.depth * a; + normal_sum += if i < n_own { v.grad } else { -v.grad }; + center += pos(i) * a; + weight += a; + } + VolumeBin { + own: side_entries(own, &own_idx), + other: other.map_or_else(Vec::new, |o| side_entries(o, &other_idx)), + volume, + normal: normal_sum.try_normalize().unwrap_or(Vector::ZERO), + center: if weight > 0.0 { + center / weight + } else { + Vector::ZERO + }, + enabled: true, + impulse: 0.0, + } + }) + .collect() +} + diff --git a/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs b/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs index f17d0f7c1..3222730c5 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs @@ -404,4 +404,140 @@ fn detect_self_regions( } let n_v = mesh.vertex_count(); let n_e = mesh.indices().len(); + let mut crossing = vec![false; n_e]; + for &(i, j) in &out.crossings { + crossing[i as usize] = true; + crossing[j as usize] = true; + } + let mut vertex_elements: Vec> = vec![Vec::new(); n_v]; + for (e, el) in mesh.indices().iter().enumerate() { + for &v in el { + vertex_elements[v as usize].push(e as u32); + } + } + let mut inside = Vec::new(); + if !classify_inside_self(mesh, sb, co.contact_skin(), &crossing, &vertex_elements, &mut inside) { + return; + } + // Each inside vertex's facing element (the nearest one outside its neighborhood) and + // distance; a vertex in front of the surface it faces is a mirrored lobe's, not a + // region's. + let mut facing: Vec> = vec![None; n_v]; + for v in 0..n_v { + if !inside[v] { + continue; + } + let p = mesh.vertex(sb, v); + let ring = &mesh.ring[mesh.ring_offsets[v] as usize..mesh.ring_offsets[v + 1] as usize]; + let mut best: Option<(Real, usize, Vector)> = None; + for e in 0..n_e { + let el = mesh.element(e); + if el.iter().any(|&u| u as usize == v || ring.contains(&u)) { + continue; + } + let Some((q, _)) = project_on_element(mesh, sb, e, p) else { + continue; + }; + let d2 = (q - p).length_squared(); + if best.is_none_or(|b| d2 < b.0) { + best = Some((d2, e, q)); + } + } + match best { + Some((d2, e, q)) + if mesh + .element_outward_normal(sb, e) + .is_some_and(|n| (p - q).gdot(n) < 0.0) => + { + facing[v] = Some((e, d2.sqrt())); + } + _ => inside[v] = false, + } + } + // The regions: the inside vertices flooded through their rings. + let mut parent: Vec = (0..n_v as u32).collect(); + fn find(parent: &mut [u32], mut i: u32) -> u32 { + while parent[i as usize] != i { + parent[i as usize] = parent[parent[i as usize] as usize]; + i = parent[i as usize]; + } + i + } + for v in 0..n_v { + if !inside[v] { + continue; + } + for &u in &mesh.ring[mesh.ring_offsets[v] as usize..mesh.ring_offsets[v + 1] as usize] { + if inside[u as usize] { + let (a, b) = (find(&mut parent, v as u32), find(&mut parent, u)); + if a != b { + parent[a as usize] = b; + } + } + } + } + let region: Vec = (0..n_v as u32) + .map(|v| { + if inside[v as usize] { + find(&mut parent, v) + } else { + u32::MAX + } + }) + .collect(); + let mut roots: Vec = region.iter().copied().filter(|&r| r != u32::MAX).collect(); + roots.sort_unstable(); + roots.dedup(); + // One constraint per region, against the vertices of the elements it faces; a pair of + // regions facing each other is emitted once, by the lower root. + let mut emitted: Vec<(u32, u32)> = Vec::new(); + for &root in &roots { + let mut depth_own = vec![Real::NEG_INFINITY; n_v]; + let mut depth_other = vec![Real::NEG_INFINITY; n_v]; + let mut faced_roots: Vec = Vec::new(); + for v in 0..n_v { + if region[v] != root { + continue; + } + let Some((e, d)) = facing[v] else { + continue; + }; + depth_own[v] = d; + for &u in mesh.element(e) { + let u = u as usize; + if region[u] == root { + continue; + } + depth_other[u] = facing[u].map_or(0.0, |f| f.1); + if region[u] != u32::MAX && !faced_roots.contains(®ion[u]) { + faced_roots.push(region[u]); + } + } + } + if faced_roots + .iter() + .any(|&r| emitted.contains(&(r.min(root), r.max(root)))) + { + continue; + } + for &r in &faced_roots { + emitted.push((r.min(root), r.max(root))); + } + let own = VolumeSide { + body: sb, + mesh, + vertices: patch_vertices(mesh, sb, &depth_own, 0.0), + }; + let other = VolumeSide { + body: sb, + mesh, + vertices: patch_vertices(mesh, sb, &depth_other, 0.0), + }; + if own.vertices.is_empty() { + continue; + } + out.region_bins + .push(volume_bins(&own, Some(&other), volume_split(params))); + } } + diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs index 52a1780d7..b512ed229 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs @@ -44,6 +44,10 @@ bitflags::bitflags! { /// If this flag is set, the pseudo-normals of the triangle-meshes (3D) and polylines (2D) /// that have them will be rendered. const PSEUDO_NORMALS = 1 << 8; + /// If this flag is set, the soft bodies' volume constraints (the intersection-volume + /// rows) will be rendered: each constraint's normal at its patch center, and the + /// volume gradient at every particle it acts on. + const SOFT_VOLUME_CONTACTS = 1 << 9; } } @@ -145,6 +149,9 @@ impl DebugRenderPipeline { backend: &mut B, soft_bodies: &SoftBodySet, ) { + if self.mode.contains(DebugRenderMode::SOFT_VOLUME_CONTACTS) { + self.render_soft_volume_contacts(backend, soft_bodies); + } if !self.mode.contains(DebugRenderMode::SOFT_BODIES) { return; } @@ -203,6 +210,40 @@ impl DebugRenderPipeline { } } + /// Renders the soft bodies' volume constraints (`SoftVolumeContact`): each one's normal at its + /// patch center (owner body toward the other side) and the volume gradient at every particle it + /// acts on, scaled so the largest has the normal's length (the push goes the opposite way). + #[profiling::function] + pub fn render_soft_volume_contacts( + &mut self, + backend: &mut B, + soft_bodies: &SoftBodySet, + ) { + let normal_color = self.style.volume_contact_normal_color; + let gradient_color = self.style.volume_gradient_color; + let length = self.style.contact_normal_length; + for (handle, sb) in soft_bodies.iter() { + let object = DebugRenderObject::SoftBody(handle, sb); + if !backend.filter_object(object) { + continue; + } + for c in sb.volume_contacts() { + backend.draw_line(object, c.center, c.center + c.normal * length, normal_color); + let g_max = c + .gradients + .iter() + .map(|(_, g)| g.length()) + .fold(0.0, crate::math::Real::max); + if g_max <= 0.0 { + continue; + } + for (p, g) in &c.gradients { + backend.draw_line(object, *p, *p + *g * (length / g_max), gradient_color); + } + } + } + } + /// Render contact. #[profiling::function] pub fn render_contacts( @@ -219,7 +260,7 @@ impl DebugRenderPipeline { let object = DebugRenderObject::ContactPair(pair, co1, co2); if backend.filter_object(object) { - for manifold in &pair.manifolds { + for manifold in pair.manifolds() { for contact in manifold.contacts() { let world_subshape_pos1 = manifold.subshape_pos1().prepend_to(co1.position()); diff --git a/src/pipeline/debug_render_pipeline/debug_render_style.rs b/src/pipeline/debug_render_pipeline/debug_render_style.rs index 572283707..b93dbfe50 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_style.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_style.rs @@ -69,6 +69,10 @@ pub struct DebugRenderStyle { pub edge_pseudo_normal_color: DebugColor, /// The length of the pseudo-normals. pub pseudo_normal_length: Real, + /// Color of the soft bodies' volume constraints' normals. + pub volume_contact_normal_color: DebugColor, + /// Color of the volume gradients drawn at the particles of a volume constraint. + pub volume_gradient_color: DebugColor, } impl Default for DebugRenderStyle { @@ -99,6 +103,8 @@ impl Default for DebugRenderStyle { vertex_pseudo_normal_color: [180.0, 1.0, 0.6, 1.0], edge_pseudo_normal_color: [280.0, 1.0, 0.6, 1.0], pseudo_normal_length: 0.2, + volume_contact_normal_color: [0.0, 0.9, 0.35, 1.0], + volume_gradient_color: [270.0, 0.8, 0.3, 1.0], } } } diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 1cb0fd0ca..ac65d76c7 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -587,6 +587,7 @@ fn settings_tab(ui: &mut Ui, state: &mut TestbedState, world: &mut PhysicsWorld) /// edited live on the running world; shown whenever the world contains soft bodies. Re-enabling /// a mechanism does not wake bodies that fell asleep while it was off. fn soft_recovery_section(ui: &mut Ui, r: &mut rapier::dynamics::SoftRecoverySettings) { + use rapier::dynamics::SoftPatchConstraints; egui::CollapsingHeader::new("Soft recovery") .default_open(false) .show(ui, |ui| { @@ -605,6 +606,47 @@ fn soft_recovery_section(ui: &mut Ui, r: &mut rapier::dynamics::SoftRecoverySett ui.checkbox(&mut r.cross_body_expel_gate, "Cross expel-only gate"); ui.checkbox(&mut r.edge_stand_down, "Edge-pass stand-down"); ui.checkbox(&mut r.crossing_repulsion, "Crossing repulsion (repel, not drop)"); + ui.checkbox(&mut r.crossing_repulsion_guide, "Repulsion guided by the volume normal"); + ui.separator(); + ui.label("Intersection-volume constraints (closed surfaces)"); + ui.checkbox(&mut r.overlap_constraints, "Overlap constraints (master)"); + ui.checkbox(&mut r.overlap_skin_volume, "Skin volume"); + ui.add( + Slider::new(&mut r.overlap_kept_depth, 0.0..=1.0) + .text("Kept skin overlap (fraction of skins)"), + ); + ui.checkbox(&mut r.overlap_normal_push, "Push along the normal instead of the gradients"); + ui.checkbox(&mut r.overlap_self_regions, "Self-overlaps between distinct regions"); + ui.horizontal(|ui| { + ui.label("Per-point constraints on the patch's features"); + for (policy, name) in [ + (SoftPatchConstraints::Keep, "Keep"), + (SoftPatchConstraints::StandDown, "Stand down"), + (SoftPatchConstraints::AlongNormal, "Along the normal"), + ] { + ui.selectable_value(&mut r.overlap_patch_constraints, policy, name); + } + }); + ui.checkbox(&mut r.overlap_multi_volume, "Multi-volume grid (section 5)"); + ui.add(Slider::new(&mut r.overlap_split, 1..=8).text("Grid cells per tangent axis")); + ui.checkbox(&mut r.overlap_rigid, "Against rigid colliders"); + ui.checkbox(&mut r.overlap_skip_self_tangled, "Skip self-crossed meshes"); + ui.checkbox(&mut r.overlap_edge_stand_down, "Edge constraints stand down on owned pairs (3D)"); + ui.add( + Slider::new(&mut r.recovery_pace, 0.05..=8.0) + .logarithmic(true) + .text("Recovery pace (length units / s)"), + ); + ui.add( + Slider::new(&mut r.overlap_constraint_pace, 0.05..=64.0) + .logarithmic(true) + .text("Constraint impulse bound (x recovery pace)"), + ); + ui.add(Slider::new(&mut r.overlap_patience, 10..=2000).text("Stall patience (steps)")); + ui.add( + Slider::new(&mut r.overlap_progress_margin, 0.0..=0.5) + .text("Progress margin (fraction)"), + ); if ui.button("Reset to defaults").clicked() { *r = Default::default(); } @@ -703,6 +745,12 @@ fn debug_render_tab(ui: &mut Ui, debug_render: &mut DebugRenderPipelineResource) "The pseudo-normals of the oriented triangle-meshes and polylines, at their \ vertices and edge midpoints.", ), + ( + DebugRenderMode::SOFT_VOLUME_CONTACTS, + "Soft volume constraints", + "The soft bodies' intersection-volume constraints: each one's normal at its \ + patch center, and the volume gradient at every particle it acts on.", + ), ]; for (flag, label, hover) in FLAGS { From 89f2f0b400350c272a2aee9b51e491c0c0b8de23 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 10 Sep 2026 09:35:11 +0200 Subject: [PATCH 10/32] feat(soft-body): self-repulsion guided by the fold's volume normal --- crates/rapier2d/tests/self_intersect_probe.rs | 1 + crates/rapier2d/tests/soft_stack_probe.rs | 1 + crates/rapier2d/tests/volume_contact.rs | 53 +++ crates/rapier3d/tests/soft_stack_probe3.rs | 1 + .../soft_body/soft_recovery_settings.rs | 149 +++---- .../soft_contact_assembly_workspace.rs | 43 +- .../soft_contacts_vertex_pass.rs | 421 +++++++++++++++++- src_testbed/ui.rs | 4 + 8 files changed, 558 insertions(+), 115 deletions(-) diff --git a/crates/rapier2d/tests/self_intersect_probe.rs b/crates/rapier2d/tests/self_intersect_probe.rs index 06503f846..e5be86564 100644 --- a/crates/rapier2d/tests/self_intersect_probe.rs +++ b/crates/rapier2d/tests/self_intersect_probe.rs @@ -366,6 +366,7 @@ fn recovery_toggles_are_wired() { edge_stand_down: false, crossing_repulsion: false, crossing_repulsion_guide: false, + crossing_repulsion_self_guide: false, recovery_pace: 0.5, overlap_constraints: false, overlap_rigid: true, diff --git a/crates/rapier2d/tests/soft_stack_probe.rs b/crates/rapier2d/tests/soft_stack_probe.rs index 69a0b0c0f..bfb56bf85 100644 --- a/crates/rapier2d/tests/soft_stack_probe.rs +++ b/crates/rapier2d/tests/soft_stack_probe.rs @@ -168,6 +168,7 @@ fn apply_preset(world: &mut PhysicsWorld) -> String { r.overlap_constraints = true; r.crossing_repulsion = true; r.crossing_repulsion_guide = preset.ends_with("-guide"); + r.crossing_repulsion_self_guide = preset.ends_with("-guide"); } // The intersection-volume constraints on the defaults. "overlap" => r.overlap_constraints = true, diff --git a/crates/rapier2d/tests/volume_contact.rs b/crates/rapier2d/tests/volume_contact.rs index f86f7eeed..49322c684 100644 --- a/crates/rapier2d/tests/volume_contact.rs +++ b/crates/rapier2d/tests/volume_contact.rs @@ -119,6 +119,59 @@ fn jelly(center: Vector, half: Real) -> SoftBodyBuilder { .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) } +/// A blob whose top vertex was pushed through its own bottom (two crossings and a mirrored tip), +/// with the crossing repulsion and no volume constraint: guided by the fold's volume normal, the +/// tip is pulled back onto the pierced surface until the crossings annihilate. +#[test] +fn self_guided_repulsion_recovers_pierced_tip() { + let run = |guided: bool| -> usize { + let mut world = PhysicsWorld::new(); + floor(&mut world); + let a = world.insert_soft_body(blob(Vector::new(0.0, 0.56), 0.5, 24)); + // Vertex 6 is the top of the loop (angle 90 degrees): pushed below the bottom. + world.soft_bodies[a].set_particle_position(6, Vector::new(0.0, 0.56 - 0.5 - 0.15)); + configure(&mut world); + let r = &mut world.integration_parameters.soft_bodies.recovery; + r.overlap_constraints = false; + r.crossing_repulsion = true; + r.crossing_repulsion_self_guide = guided; + let self_crossings = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[a]; + let mesh = sb.meshes().next().unwrap(); + let idx = mesh.indices(); + let p = |v: u32| mesh.vertex(sb, v as usize); + let mut crossings = 0; + for i in 0..idx.len() { + for j in i + 1..idx.len() { + let (ei, ej) = (idx[i], idx[j]); + if ei.iter().any(|v| ej.contains(v)) { + continue; + } + if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { + crossings += 1; + } + } + } + crossings + }; + for step in 0..=steps() { + if step % 100 == 0 { + println!( + "guided {guided} step {step:4}: crossings={} area={:+.3}", + self_crossings(&world), + world.soft_bodies[a].volume() + ); + } + world.step(); + } + self_crossings(&world) + }; + let unguided = run(false); + let guided = run(true); + println!("end crossings: unguided {unguided}, guided {guided}"); + assert_eq!(guided, 0, "the guided repulsion never pulled the tip back"); +} + /// A blob on the floor with two bottom vertices tunneled through it, manifold constraints kept: /// the volume constraint pulls the vertices back while the manifold constraints of the same /// features may fight it. Every `overlap_patch_constraints` policy must recover the vertices. diff --git a/crates/rapier3d/tests/soft_stack_probe3.rs b/crates/rapier3d/tests/soft_stack_probe3.rs index 85016a152..a77a06d00 100644 --- a/crates/rapier3d/tests/soft_stack_probe3.rs +++ b/crates/rapier3d/tests/soft_stack_probe3.rs @@ -165,6 +165,7 @@ fn apply_preset(world: &mut PhysicsWorld) -> String { r.overlap_constraints = true; r.crossing_repulsion = true; r.crossing_repulsion_guide = preset.ends_with("-guide"); + r.crossing_repulsion_self_guide = preset.ends_with("-guide"); } // The intersection-volume constraints on the defaults. "overlap" => r.overlap_constraints = true, diff --git a/src/dynamics/soft_body/soft_recovery_settings.rs b/src/dynamics/soft_body/soft_recovery_settings.rs index 0e12c16ab..223011097 100644 --- a/src/dynamics/soft_body/soft_recovery_settings.rs +++ b/src/dynamics/soft_body/soft_recovery_settings.rs @@ -18,135 +18,111 @@ pub enum SoftPatchConstraints { AlongNormal, } -/// Runtime configuration of the soft-body tangle detection and recovery stack. -/// -/// Lives on [`IntegrationParameters::soft_recovery`], so it can be changed between steps. -/// -/// [`IntegrationParameters::soft_recovery`]: crate::dynamics::IntegrationParameters +/// Runtime configuration of the soft-body tangle detection and recovery stack. It lives on +/// [`crate::dynamics::SoftBodiesSettings::recovery`] (itself on +/// `IntegrationParameters::soft_bodies`), so it can be changed between steps. #[derive(Copy, Clone, Debug, PartialEq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", serde(default))] pub struct SoftRecoverySettings { // --- Prevention --- - /// Raise the speculative contact margin from velocities authored between steps (at - /// insertion, or through `set_particle_velocity`), so the coming step's reach already - /// covers them instead of lagging one step behind and letting a fast fresh body - /// tunnel. (default: `true`) + /// Raise the speculative contact margin from velocities authored between steps (insertion, + /// `set_particle_velocity`), so the coming step's reach covers them and a fast fresh body + /// does not tunnel. (default: `true`) pub authored_velocity_margin: bool, - /// Give the edge-vs-edge pass a speculative reach (motion margin plus the closing-speed - /// rule) and, in 3D, emit edge rows for closed-vs-closed pairs, so two edge-leading - /// bodies crossing corner-first (no vertex near the other's surface) collide instead of - /// passing straight through. Off by default: the closed-vs-closed edge rows leave the - /// pressed 3D piles crossed. (default: `false`) + /// Give the edge-vs-edge pass a speculative reach (motion margin plus the closing-speed rule) + /// and, in 3D, edge constraints for closed-vs-closed pairs, so bodies crossing corner-first + /// collide; on, those constraints leave pressed 3D piles crossed. (default: `false`) pub edge_speculation: bool, // --- Detection --- - /// Detect inverted cells (material locally inside out) each step. Feeds the self - /// stand-down. (default: `true`) + /// Detect inverted cells (material locally inside out) each step; feeds the self stand-down. + /// (default: `true`) pub inverted_cell_detection: bool, - /// Detect surface self-crossings each step (the BVTT sweep). Feeds the self - /// stand-down and the self untangler. (default: `true`) + /// Detect surface self-crossings each step (the BVTT sweep); feeds the self stand-down and the + /// self untangler. (default: `true`) pub self_crossing_detection: bool, - /// Skip the self-crossing sweep while the surface's accumulated travel could not have - /// bridged half a skin (crossings only form through motion). Off: sweep every step. - /// (default: `true`) + /// Skip the self-crossing sweep while the surface's accumulated travel could not have bridged + /// half a skin (crossings only form through motion). Off: sweep every step. (default: `true`) pub detection_motion_gating: bool, - /// Detect boundary crossings between pairs of soft surfaces. Feeds the cross-body - /// expel-only gate, the edge-pass stand-down, and the intersection-volume rows. - /// (default: `true`) + /// Detect boundary crossings between pairs of soft surfaces; feeds the cross-body expel-only + /// gate, the edge-pass stand-down and the intersection-volume constraints. (default: `true`) pub cross_body_detection: bool, // --- Passive recovery (stand-down) --- - /// Self contacts of tangled features (inverted cells, self-crossings) stand down so - /// the elasticity can resolve the tangle instead of freezing it at skin distance. - /// (default: `true`) + /// Self contacts of tangled features (inverted cells, self-crossings) stand down so the + /// elasticity resolves the tangle instead of freezing it at skin distance. (default: `true`) pub self_stand_down: bool, - /// A vertex row touching a boundary crossing between two surfaces may only expel, - /// never hold (a keep-apart row there is wrong-sided by construction). + /// A vertex constraint touching a boundary crossing between two surfaces may only expel, + /// never hold (a keep-apart constraint there is wrong-sided by construction). /// (default: `true`) pub cross_body_expel_gate: bool, - /// Edge rows touching a cross-body boundary crossing stand down (thin open bodies - /// meet through their edge rows, so the vertex gate alone cannot free them). + /// Edge constraints touching a cross-body boundary crossing stand down (thin open bodies + /// meet through their edge constraints, so the vertex gate alone cannot free them). /// (default: `true`) pub edge_stand_down: bool, - /// Rows on crossing-flagged features repel instead of standing down: the row pushes - /// the vertex to the side of the element where its own neighbors lie (its one-ring - /// centroid), through to skin distance there. A shallow crossing returns where it came - /// from, a body already past halfway completes its passage, and adjacent vertices - /// agree; a neighborhood straddling the element (a genuine tangle) still stands down. - /// (default: `false`) + /// Constraints on crossing-flagged features repel instead of standing down: the vertex is + /// pushed to the side of the element where its one-ring centroid lies, through to skin + /// distance; a neighborhood straddling the element still stands down. (default: `false`) pub crossing_repulsion: bool, - /// Guide the crossing repulsion by the pair's volume normal: the direction the - /// intersection-volume gradients of the two intruding patches separate along (per - /// grid cell, the nearest one to the repelled vertex), instead of the pierced element's - /// plane normal signed by the vertex's neighborhood (which cannot be validated on a - /// crossed surface). Closed pairs only; needs no volume row. (default: `false`) + /// Guide the crossing repulsion by the pair's volume normal (the separating direction of the + /// two intruding patches' intersection-volume gradients, per grid cell) instead of the pierced + /// element's plane normal. Closed pairs only; needs no volume constraint. (default: `false`) pub crossing_repulsion_guide: bool, + /// Guide the self-crossing repulsion by the fold's volume normal: the vertices inside their own + /// body's self-intersection region are pulled toward the surface they face along the region's + /// mean normal. Closed meshes only; needs no volume constraint. (default: `false`) + pub crossing_repulsion_self_guide: bool, // --- Pacing --- - /// The material recovery pace, in length units per second (scaled by - /// `IntegrationParameters::length_unit`): the corrective rate deep recovery, demoted - /// intruders, and untangling pulls are allowed. (default: `0.5`) + /// Material recovery pace, in length units per second (scaled by + /// `IntegrationParameters::length_unit`): the corrective rate allowed to deep recovery, + /// demoted intruders and untangling pulls. (default: `0.5`) pub recovery_pace: Real, - // --- Intersection-volume rows --- - /// Intersection-volume contact (Allard et al. 2010) for the closed surfaces: one coupled - /// row per overlapping pair (soft-soft, or soft-rigid) over both sides' intruding - /// patches, carrying the intersection volume as its correction. A smooth pressure with - /// no side to decide. Master switch of the knobs below. (default: `true`) + // --- Intersection-volume constraints --- + /// Intersection-volume contact (Allard et al. 2010) for closed surfaces: one coupled constraint + /// per overlapping pair (soft-soft or soft-rigid) over both sides' intruding patches, corrected + /// by the intersection volume; master switch of the knobs below. (default: `true`) pub overlap_constraints: bool, - /// Overlap rows against rigid colliders too (the surface vertices inside the shape; + /// Overlap constraints against rigid colliders too (the surface vertices inside the shape; /// a dynamic rigid body takes the reaction, a fixed or kinematic one is a wall). /// (default: `true`) pub overlap_rigid: bool, - /// A self-crossed mesh takes no pair row: part of its winding is mirrored, so its volume + /// A self-crossed mesh takes no pair constraint: part of its winding is mirrored, so its volume /// gradient points the wrong way there (two crossed eights exploded without this). /// (default: `true`) pub overlap_skip_self_tangled: bool, - /// The 3D closed-closed edge rows stand down on a pair an overlap row owns (inside the + /// The 3D closed-closed edge constraints stand down on a pair an overlap constraint owns (inside the /// overlap they are wrong-sided and freeze it). (default: `true`) pub overlap_edge_stand_down: bool, - /// Bound on the velocity change the coupled row may hand any side per step, in - /// multiples of [`Self::recovery_pace`] (a light box inside a blob was otherwise shot out - /// with the blob's stored elastic energy); `Real::MAX`: a hard row. With - /// [`Self::overlap_velocity_correction`] it bounds the closing rate instead. - /// (default: `1.0`) + /// Bound on the velocity change the coupled constraint may hand any side per step, in multiples + /// of [`Self::recovery_pace`]; `Real::MAX` makes it hard. With + /// [`Self::overlap_velocity_correction`] it bounds the closing rate instead. (default: `1.0`) pub overlap_constraint_pace: Real, /// What the per-point constraints (manifold constraints against rigid colliders, vertex /// constraints against soft surfaces) of the features inside a volume constraint's patch do, - /// feature by feature rather than pair by pair (see [`SoftPatchConstraints`]): a couple of - /// vertices tunneled through a floor otherwise keep contact constraints fighting the volume - /// constraint that pulls them back. (default: `Keep`) + /// feature by feature (see [`SoftPatchConstraints`]). (default: `Keep`) pub overlap_patch_constraints: SoftPatchConstraints, - /// Measure the intersection volume on the contact skins (the surfaces dilated by their - /// skin) instead of the geometric surfaces: a resting pair then has a volume row, with - /// a speculative slack before the skins meet, and the rows are hard (a barrier), like - /// contacts. Implied by [`SoftContactModel::VolumeOnly`]. (default: `false`) + /// Measure the intersection volume on the contact skins (surfaces dilated by their skin), not + /// the geometric surfaces: a resting pair then has a hard volume constraint with speculative + /// slack before the skins meet. Implied by [`SoftContactModel::VolumeOnly`]. (default: `false`) pub overlap_skin_volume: bool, - /// The skin overlap kept at rest, as a fraction of the pair's skins (the paper's - /// maintained interpenetration layer): the correction only removes the volume beyond - /// it. The volume constraint holds the patch's average depth at it, so a curved body - /// sinks deeper than a flat one; `0.0` rests the surfaces at their skins like the - /// per-point rows. (default: `0.0`) + /// The skin overlap kept at rest, as a fraction of the pair's skins (the paper's maintained + /// interpenetration layer): the correction removes only the volume beyond it, holding the + /// patch's average depth there; `0.0` rests the surfaces at their skins. (default: `0.0`) pub overlap_kept_depth: Real, - /// Volume constraints on a body's self-overlaps between distinct regions of its surface - /// (the two legs of a concave body pressed into each other): the vertices lying behind - /// another part of their own surface form patches, one per connected region, and each - /// region gets a coupled row with the surface it faces, like a pair of bodies. A - /// mirrored lobe (a vertex in front of the surface it faces) is left to the lobe rows. - /// Closed meshes only. (default: `false`) + /// Volume constraints on a body's self-overlaps between distinct surface regions: the vertices + /// behind another part of their own surface form one patch per connected region, each coupled + /// with the surface it faces like a pair of bodies. Closed meshes only. (default: `false`) pub overlap_self_regions: bool, - /// Push along each constraint's normal instead of along the volume gradients: every - /// entry keeps its gradient's magnitude (its share of the patch area) but takes the - /// constraint's mean direction (the owner side toward the other side, the other side - /// the opposite way), so a corner's sideways gradient no longer squeezes it and the - /// whole patch separates along one axis. (default: `false`) + /// Push along each constraint's normal instead of the volume gradients: every entry keeps its + /// gradient's magnitude but takes the constraint's mean direction, so the whole patch + /// separates along one axis. (default: `false`) pub overlap_normal_push: bool, - /// Multi-volume grid (the paper's section 5): each pair's patch is split into a regular - /// grid of cells along its tangent axes, each with its own row and multiplier, so the - /// pressure (and the friction state) can vary across the patch: a single multiplier - /// cannot keep a body from tipping, and its whole patch sticks or slides at once. - /// (default: `false`) + /// Multi-volume grid (the paper's section 5): each pair's patch is split into a regular grid of + /// cells along its tangent axes, each with its own constraint and multiplier, so the pressure + /// and friction state vary across the patch. (default: `false`) pub overlap_multi_volume: bool, /// Cells per tangent axis of the multi-volume grid. (default: `3`) pub overlap_split: u32, @@ -174,6 +150,7 @@ impl Default for SoftRecoverySettings { edge_stand_down: true, crossing_repulsion: false, crossing_repulsion_guide: false, + crossing_repulsion_self_guide: false, recovery_pace: 0.5, overlap_constraints: true, overlap_rigid: true, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs index eaf0cfc25..59fa1b268 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs @@ -63,59 +63,58 @@ pub(super) struct BodyContacts { /// vertex pass), by (vertex, element). pub(super) previous_vertex: HashMap<(u32, u32), (Real, Vector)>, /// Elements (and vertices) of the mesh being assembled whose self contacts stand down - /// this step: carried by inverted cells, or part of a self-crossing of the surface (empty + /// this step: backed by inverted cells, or part of a self-crossing of the surface (empty /// while the surface is healthy). Copied from the narrow phase's self detection. pub(super) tangled_elements: Vec, pub(super) tangled_vertices: Vec, /// The crossing element pairs the narrow phase's sweep found on the mesh being assembled. pub(super) crossings: Vec<(u32, u32)>, - /// Elements of the current pair's surface side, and vertices of its vertex side, taking - /// part in a crossing *between* the two surfaces (empty while the surfaces do not - /// cross). Rows touching them may only expel, never hold at skin distance: the - /// closed-surface expulsion still pushes a lens out, while nothing freezes the - /// crossing. + /// Elements of the current pair's surface side, and vertices of its vertex side, in a crossing + /// between the two surfaces (empty while they do not cross); constraints touching them may + /// only expel, never hold at skin distance, so nothing freezes the crossing. pub(super) cross_tangled_elements: Vec, pub(super) cross_tangled_vertices: Vec, /// Element-granularity crossing flags on the vertex-side mesh, and the recorded /// (element-side, vertex-side) crossing element pairs. pub(super) cross_tangled_vb_elements: Vec, pub(super) cross_pairs: Vec<(u32, u32)>, - /// The intersection-volume rows this body assembled (see `SoftOverlapRow`). + /// The intersection-volume constraints this body assembled (see `SoftOverlapConstraint`). pub(super) overlap_constraints: Vec, - /// The current pair's volume normals guiding its crossing repulsion (see - /// `crossing_repulsion_guide`): per grid cell, its center and its normal from the - /// element side into the vertex side (for a self pair: the push direction of the fold's - /// vertices, the facing surface's vertices take the opposite). + /// Per grid cell, the center and normal (element side into vertex side) guiding the current + /// pair's crossing repulsion (see `crossing_repulsion_guide`); for a self pair, the push + /// direction of the fold's vertices, the facing surface's vertices taking the opposite. pub(super) repel_guides: Vec<(Vector, Vector)>, + /// For a self pair, whether each vertex lies in its own body's self-intersection region + /// (see `classify_inside_self`). + pub(super) repel_inside: Vec, /// For the current pair, whether each vertex of the vertex side lies in the other - /// side's volume patch (see `overlap_patch_rows`). + /// side's volume patch (see `overlap_patch_constraints`). pub(super) patch_inside_vb: Vec, /// The rigid pairs of the current mesh with a volume constraint (see - /// `overlap_patch_rows`): the collider, the surface vertices' depths in its patch + /// `overlap_patch_constraints`): the collider, the surface vertices' depths in its patch /// (`NEG_INFINITY` outside), and the constraint's cells (center, normal). pub(super) rigid_patches: Vec<(ColliderHandle, Vec, Vec<(Vector, Vector)>)>, } -/// An intersection-volume row assembled by one body (see `SoftOverlapRow`): the gradient -/// triples of every simulated particle of the pair, the rigid side if any, and the row's -/// right-hand side (the signed volume estimate when the correction runs on the row, the -/// slack alone otherwise). +/// An intersection-volume constraint assembled by one body (see `SoftOverlapConstraint`): the +/// gradient triples of every simulated particle of the pair, the rigid side if any, and the +/// right-hand side (the signed volume estimate when the correction runs on it, else the slack). pub(super) struct OverlapConstraintWorkspace { pub(super) grads: Vec<(u32, Real, Vector, Vector)>, - /// The `(side, particle)` of every gradient entry (see `SoftOverlapRow::warm`). + /// The `(side, particle)` of every gradient entry (see `SoftOverlapConstraint::warm`). pub(super) particles: Vec<(u8, u32)>, pub(super) rigid: Option<(u32, Vector, AngVector)>, pub(super) rigid_pose0: (Vector, Rotation), pub(super) rhs: Real, - /// A contact in its own right (see `SoftOverlapRow::hard`). + /// A contact in its own right (see `SoftOverlapConstraint::hard`). pub(super) hard: bool, - /// The carried impulses of a hard row (see `SoftOverlapWarm`): the other collider, the - /// fitted multiplier, the carried impulse per gradient entry, and the rigid side's. + /// The warm impulses of a hard constraint (see `SoftOverlapWarm`): the other collider, the + /// fitted multiplier, the warm impulse per gradient entry, and the rigid side's. pub(super) warm: Option<(ColliderHandle, Real)>, pub(super) warm_impulses: Vec, pub(super) warm_rigid: (Vector, AngVector), pub(super) max_bias_velocity: Real, - /// The pair's contact slot of the row's bin (see `SoftOverlapRow::report`). + /// The pair's contact slot of the constraint's bin (see `SoftOverlapConstraint::report`). pub(super) report: (ColliderHandle, ColliderHandle, u32), } diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs index faa2abf04..0f0d1c29f 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs @@ -251,19 +251,426 @@ pub(crate) fn detect_vertex_pass( } } } + // The self guide (see `crossing_repulsion_self_guide`): the fold's vertices (in their body's + // self-intersection region) form the own side, the nearest elements outside their neighborhood + // the other side; each cell's normal, from the fold toward the facing surface, is the push. + let self_guided = repel + && params.soft_bodies.recovery.crossing_repulsion_self_guide + && is_self + && vb_mesh.is_closed() + && !crossings.is_empty(); + if self_guided { + let n_v = vb_mesh.vertex_count(); + let mut crossing = vec![false; vb_mesh.indices().len()]; + for &(i, j) in crossings { + crossing[i as usize] = true; + crossing[j as usize] = true; + } + let mut inside = Vec::new(); + if classify_inside_self(vb_mesh, vb, vb_co.contact_skin(), &crossing, &vertex_elements, &mut inside) { + let mut depth_fold = vec![Real::NEG_INFINITY; n_v]; + let mut depth_face = vec![Real::NEG_INFINITY; n_v]; + for v in 0..n_v { + if !inside[v] { + continue; + } + let p = vb_mesh.cached_vertex(v); + let ring = &vb_mesh.ring + [vb_mesh.ring_offsets[v] as usize..vb_mesh.ring_offsets[v + 1] as usize]; + let mut best: Option<(Real, usize)> = None; + for e in 0..vb_mesh.indices().len() { + let el = vb_mesh.element(e); + if el.iter().any(|&u| u as usize == v || ring.contains(&u)) { + continue; + } + let Some((q, _)) = project_on_element(vb_mesh, vb, e, p) else { + continue; + }; + let d2 = (q - p).length_squared(); + if best.is_none_or(|b| d2 < b.0) { + best = Some((d2, e)); + } + } + let Some((d2, e)) = best else { + continue; + }; + depth_fold[v] = d2.sqrt(); + for &u in vb_mesh.element(e) { + depth_face[u as usize] = 0.0; + } + } + let own = VolumeSide { + body: vb, + mesh: vb_mesh, + vertices: patch_vertices(vb_mesh, vb, &depth_fold, 0.0), + }; + let other = VolumeSide { + body: vb, + mesh: vb_mesh, + vertices: patch_vertices(vb_mesh, vb, &depth_face, 0.0), + }; + for bin in volume_bins(&own, Some(&other), volume_split(params)) { + if bin.normal != Vector::ZERO { + out.repel_guides.push((bin.center, bin.normal)); + } + } + } + out.repel_inside = inside; + } + // The per-vertex enumeration, chunked across threads for a large vertex side (the chunks' + // outputs are appended in order: the same candidates as the serial loop). + let SoftVertexPass { + cross_tangled_vertices, + hits, + candidates, + .. + } = out; + let scan = VertexScan { + eb, + eb_mesh, + eb_co, + eb_bvh, + eb_inv_pose: eb_co.position().inverse(), + bounds: eb_co.compute_aabb().loosened(reach), + vb, + vb_mesh, + vb_co, + is_self, + params, + step_dt, + reach, + prediction, + skins, + closed, + rest_gaps, + tangled_vertices, + tangled_elements, + repel, + targets: &targets, + vertex_elements: &vertex_elements, + cross_tangled_vertices, + }; + let n_v = vb_mesh.vertex_count(); + #[cfg(feature = "parallel")] + if n_v >= 2 * PARALLEL_CHUNK && rayon::current_num_threads() > 1 { + use rayon::prelude::*; + let chunks: Vec<(Vec, Vec)> = (0..n_v + .div_ceil(PARALLEL_CHUNK)) + .into_par_iter() + .map(|c| { + let (mut hits, mut candidates) = (Vec::new(), Vec::new()); + let mut scratch = VertexScratch::default(); + for v in c * PARALLEL_CHUNK..((c + 1) * PARALLEL_CHUNK).min(n_v) { + scan.scan_vertex(v, &mut scratch, &mut hits, &mut candidates); + } + (hits, candidates) + }) + .collect(); + for (chunk_hits, chunk_candidates) in chunks { + let offset = candidates.len() as u32; + hits.extend(chunk_hits.into_iter().map(|h| SoftVertexHits { + candidates: h.candidates.start + offset..h.candidates.end + offset, + ..h + })); + candidates.extend(chunk_candidates); + } + return; + } + let mut scratch = VertexScratch::default(); + for v in 0..n_v { + scan.scan_vertex(v, &mut scratch, hits, candidates); + } +} + +/// The per-thread buffers of a vertex pass: the candidates of the vertex at hand, and the +/// self-exclusion stamp table (see `SoftCollisionMesh::mark_self_contact_exclusions`). +#[derive(Default)] +struct VertexScratch { + candidates: Vec, + stamps: Vec, +} + +/// Vertices (or elements) per parallel chunk of the vertex and edge passes. +#[cfg(feature = "parallel")] +pub(super) const PARALLEL_CHUNK: usize = 512; + +/// The read-only state of a vertex pass's per-vertex enumeration (see [`VertexScan::scan_vertex`]). +struct VertexScan<'a> { + eb: &'a SoftBody, + eb_mesh: &'a SoftCollisionMesh, + eb_co: &'a crate::geometry::Collider, + eb_bvh: &'a parry::partitioning::Bvh, + eb_inv_pose: crate::math::Pose, + /// The surface side's world bounds, loosened by the reach. + bounds: crate::geometry::Aabb, + vb: &'a SoftBody, + vb_mesh: &'a SoftCollisionMesh, + vb_co: &'a crate::geometry::Collider, + is_self: bool, + params: &'a crate::dynamics::IntegrationParameters, + step_dt: Real, + reach: Real, + prediction: Real, + skins: Real, + closed: bool, + rest_gaps: bool, + tangled_vertices: &'a [bool], + tangled_elements: &'a [bool], + repel: bool, + targets: &'a [Vec], + vertex_elements: &'a [Vec], + cross_tangled_vertices: &'a [bool], } + +impl VertexScan<'_> { + /// The candidates of the vertex `v` (appended to `out_candidates`, with its hit record in + /// `hits`); `scratch` is the per-vertex candidate buffer. + fn scan_vertex( + &self, + v: usize, + scratch: &mut VertexScratch, + hits: &mut Vec, + out_candidates: &mut Vec, + ) { + let VertexScratch { + candidates: scratch, + stamps, + } = scratch; + // Surface vertices only. + if !self.vb_mesh.vertex_on_surface(v) { + return; + } // A tangled vertex (backed by inverted material, or part of a surface + // self-crossing): its self contacts stand down (see `detect_self_tangles`). + if self.is_self + && self.params.soft_bodies.recovery.self_stand_down + && !self.params.soft_bodies.recovery.crossing_repulsion + && self.tangled_vertices.get(v).copied().unwrap_or(false) + { + return; + } + let vertex_pos = self.vb_mesh.cached_vertex(v); + let mut vertex_vel: Option = None; + // A vertex beyond the surface's reach-loosened bounds has no candidate (its crossing + // repulsion constraints, if flagged, do not go through the reach). + let flagged_vertex = self.repel + && if self.is_self { + self.tangled_vertices.get(v).copied().unwrap_or(false) + } else { + self.cross_tangled_vertices.get(v).copied().unwrap_or(false) + }; + if !self.is_self && !flagged_vertex && !self.bounds.contains_local_point(vertex_pos) { + return; + } + if self.is_self { + // The elements excluded around this vertex, stamped once. + let n_e = self.eb_mesh.indices().len(); + if stamps.len() != n_e { + stamps.clear(); + stamps.resize(n_e, u32::MAX); + } + self.vb_mesh.mark_self_contact_exclusions(v as u32, stamps); + } + + // Candidate elements: the vertex's reach box against the surface's BVH. The + // shape's vertices live in its cluster frame: the world box is localized first. + scratch.clear(); + let aabb = crate::geometry::Aabb::from_half_extents(vertex_pos, Vector::splat(self.reach)) + .transform_by(&self.eb_inv_pose); + for e in self.eb_bvh.intersect_aabb(&aabb) { + let element = self.eb_mesh.element(e as usize); + if self.is_self + && (stamps[e as usize] == v as u32 + || self.vb_mesh.self_contact_excluded_at_rest( + self.vb, + v as u32, + element, + self.vb_co.contact_skin(), + )) + { + continue; + } + // A tangled element (its cell is inverted, or it is part of a surface + // self-crossing): its self contacts stand down (see `detect_self_tangles`). + if self.is_self + && self.params.soft_bodies.recovery.self_stand_down + && !self.params.soft_bodies.recovery.crossing_repulsion + && self.tangled_elements.get(e as usize).copied().unwrap_or(false) + { + continue; + } + let positions: [Vector; DIM] = core::array::from_fn(|k| { + element + .get(k) + .map_or(Vector::ZERO, |v| self.eb_mesh.cached_vertex(*v as usize)) + }); + // A segment element (every mesh in 2D, a wire in 3D) or a triangle: the unused + // weights stay at zero. + let (proj, weights) = if element.len() < DIM { + let (proj, loc) = parry::shape::Segment::new(positions[0], positions[1]) + .project_local_point_and_get_location(vertex_pos, false); + let bcoords = loc.barycentric_coordinates(); + let mut weights = [0.0; DIM]; + weights[0] = bcoords[0]; + weights[1] = bcoords[1]; + (proj, weights) + } else { + #[cfg(feature = "dim2")] + { + let (proj, loc) = parry::shape::Segment::new(positions[0], positions[1]) + .project_local_point_and_get_location(vertex_pos, false); + (proj, loc.barycentric_coordinates()) + } + #[cfg(feature = "dim3")] + { + let (proj, loc) = + parry::shape::Triangle::new(positions[0], positions[1], positions[2]) + .project_local_point_and_get_location(vertex_pos, false); + let Some(weights) = loc.barycentric_coordinates() else { + continue; + }; + (proj, weights) + } + }; + let sep = vertex_pos - proj.point; + let len = sep.length(); if len >= reach || len < 1.0e-6 { + continue; } let dist = len - skins; - // Beyond the prediction distance, only a contact closing fast enough to happen - // `params.dt` is the substep): the rest of the reach is the bodies' motion - // margin, and a resting vertex must not carry rows (warm-started) against every - // a ghost of that internal feature (the neighbor carries the actual contact); + // Beyond the prediction distance, only a contact closing fast enough to happen within + // the whole step is kept (`self.params.dt` is the substep): the rest of the reach is + // the bodies' motion margin, and a resting vertex must not keep constraints in it. + if dist >= self.prediction { + let vertex_vel = + vertex_vel.get_or_insert_with(|| self.vb_mesh.vertex_velocity(self.vb, v)); + let mut surface_vel = Vector::ZERO; + for (k, v) in element.iter().enumerate() { + surface_vel += self.eb_mesh.vertex_velocity(self.eb, *v as usize) * weights[k]; + } + let closing = (surface_vel - *vertex_vel).gdot(sep / len); + if dist - closing * self.step_dt >= self.prediction { + continue; + } + } + // A vertex/edge contact whose vertex projects inside a neighboring element is a ghost + // of that internal feature (the neighbor reports the real contact); vertices thicker + // than the element wrap several elements, so their feature contacts are real support. + let size = (positions[1] - positions[0]).length(); + if self.vb_co.contact_skin() < size + && self.eb_mesh.contact_is_ghost(self.eb, e as usize, &weights, vertex_pos) + { + continue; + } + let outward = if self.closed { + self.eb_mesh + .element_outward_normal(self.eb, e as usize) + .and_then(|n| n.try_normalize()) + } else { + None + }; + scratch.push(SoftVertexCandidate { + element: e, + weights, + dir: -sep / len, + dist, + outward, + interior: weights.iter().all(|w| *w > 0.02), + enabled: true, + impulse: 0.0, + tangent_impulse: Vector::ZERO, + }); + } // Crossing repulsion: the crossing pairs' constraints come before the self.reach test + // (an edge-first crossing vertex has no natural candidate at all). + if self.repel { + let flagged_vertex = if self.is_self { + self.tangled_vertices.get(v).copied().unwrap_or(false) + } else { + self.cross_tangled_vertices.get(v).copied().unwrap_or(false) + }; + if flagged_vertex { + // Only the vertices near the pierced element's plane (the near endpoint // of a piercing edge): a constraint on a far vertex is a long-range hold that + // blocks the resolution instead of driving it. + let bound = 2.0 * self.skins + self.eb_co.contact_skin(); + for &f in &self.vertex_elements[v] { + for &e in &self.targets[f as usize] { + let near = element_plane(self.eb_mesh, e as usize, |v| self.eb_mesh.cached_vertex(v)) + .is_some_and(|(p0, n)| (vertex_pos - p0).dot(n).abs() <= bound); + if near && scratch.iter().all(|c| c.element != e) { + scratch.push(SoftVertexCandidate { + element: e, + weights: [1.0 / DIM as Real; DIM], + dir: Vector::ZERO, + dist: 0.0, + outward: None, + interior: false, + enabled: true, + impulse: 0.0, + tangent_impulse: Vector::ZERO, + }); + } + } + } + } + } + if scratch.is_empty() { + return; + } // Deterministic order, and one constraint per surface vertex touched: the elements sharing + // that vertex all report it as their closest point (edge contacts shared by two + // elements are kept twice, as the manifold path does). + scratch.sort_by_key(|c| c.element); + let mut kept = 0; + for i in 0..scratch.len() { + let c = scratch[i]; + let element = self.eb_mesh.element(c.element as usize); + let vertex = (0..element.len()).find(|&k| c.weights[k] > 0.999); + if let Some(k) = vertex { + let key = element[k]; + if scratch[..kept].iter().any(|prev| { + let prev_element = self.eb_mesh.element(prev.element as usize); + (0..prev_element.len()) + .any(|j| prev.weights[j] > 0.999 && prev_element[j] == key) + }) { + continue; + } + } + scratch[kept] = c; + kept += 1; + } + scratch.truncate(kept); - // A foreign vertex that crossed a closed surface: seen from behind (reversed - // fast incoming vertex also sees the far side of a closed surface as reversed - // through the speculative reach (its near-side sightings often land next to + // A foreign vertex that crossed a closed surface (seen only from behind, inside by parity) + // hands its pair to the recovery. A fast incoming vertex also sees the far side as reversed + // through the speculative reach (near-side sightings often fail the interior gate). + let mut seen_outside_any = false; + let mut seen_reversed = false; + for c in scratch.iter() { + let Some(outward) = c.outward else { + continue; + }; + let side = c.dir.gdot(outward); + if side.abs() <= 0.5 { + continue; + } + if side < 0.0 { + seen_outside_any = true; + } else if c.interior { + seen_reversed = true; + } + } + let crossed = self.closed + && seen_reversed + && !seen_outside_any + && self.eb_mesh.contains_point_parity(self.eb, vertex_pos); + let start = out_candidates.len() as u32; + out_candidates.extend_from_slice(&scratch); + hits.push(SoftVertexHits { + vertex: v as u32, + candidates: start..out_candidates.len() as u32, + crossed, + }); + } +} diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index ac65d76c7..2be02104f 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -607,6 +607,10 @@ fn soft_recovery_section(ui: &mut Ui, r: &mut rapier::dynamics::SoftRecoverySett ui.checkbox(&mut r.edge_stand_down, "Edge-pass stand-down"); ui.checkbox(&mut r.crossing_repulsion, "Crossing repulsion (repel, not drop)"); ui.checkbox(&mut r.crossing_repulsion_guide, "Repulsion guided by the volume normal"); + ui.checkbox( + &mut r.crossing_repulsion_self_guide, + "Self-repulsion guided by the fold's volume normal", + ); ui.separator(); ui.label("Intersection-volume constraints (closed surfaces)"); ui.checkbox(&mut r.overlap_constraints, "Overlap constraints (master)"); From c47f6a9629c4d85c6de34751ac19a050b4b3978f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 10 Sep 2026 15:32:19 +0200 Subject: [PATCH 11/32] feat(soft-body): FEM augmented mass for every constraint, volume elements, a parallel response stage and column responses --- crates/rapier2d/tests/soft_bodies.rs | 123 ++++ crates/rapier3d/tests/soft_bodies.rs | 71 +++ src/dynamics/integration_parameters.rs | 2 - .../collision_mesh_accessors.rs | 21 + .../collision_mesh/collision_mesh_geometry.rs | 57 ++ src/dynamics/soft_body/soft_body_geometry.rs | 24 + src/dynamics/soft_body/soft_body_settings.rs | 7 +- src/dynamics/solver/soft_constraint/mod.rs | 4 +- .../solver/soft_constraint/soft_attachment.rs | 47 +- .../soft_constraint_prepare.rs | 41 ++ .../soft_constraint_rigid_coupling.rs | 2 + .../soft_constraint_solve.rs | 87 +++ .../soft_constraints_set/soft_constraints.rs | 120 +++- .../soft_constraints_set.rs | 27 +- .../solver/soft_constraint/soft_contact.rs | 157 +++-- .../soft_contact_assembly.rs | 5 + .../soft_contact_assembly_body_contacts.rs | 2 + .../soft_contact_assembly_edge_contacts.rs | 1 + .../soft_contact_assembly_vertex_contacts.rs | 1 + .../soft_constraint/soft_contact_chunks.rs | 19 +- .../soft_scalar_constraint.rs | 2 + .../soft_element_constraint_assembly.rs | 22 +- src/dynamics/solver/soft_fem/mod.rs | 2 +- src/dynamics/solver/soft_fem/soft_fem_set.rs | 540 ++++++++++++++++-- .../solver/soft_fem/soft_fem_sparse.rs | 10 + .../system/soft_fem_system_assemble.rs | 29 + .../system/soft_fem_system_elements.rs | 32 ++ .../system/soft_fem_system_prepare.rs | 36 ++ .../system/soft_fem_system_response.rs | 203 +++++++ src/dynamics/solver/soft_fem/system/tests.rs | 99 ++++ .../solver/staged_island_solver/init.rs | 28 +- .../solver/staged_island_solver/solve.rs | 2 + .../solver/staged_island_solver/worker.rs | 25 + 33 files changed, 1729 insertions(+), 119 deletions(-) create mode 100644 src/dynamics/solver/soft_fem/system/soft_fem_system_response.rs create mode 100644 src/dynamics/solver/soft_fem/system/tests.rs diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index ae98a64cb..71576a4ce 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -1541,6 +1541,129 @@ fn fem_box_rests_on_soft_body() { assert!(world.bodies[rb].linvel().length() < 0.05); } +/// A stack of stiff FEM bodies rests on the ground without sinking: contacts see a FEM body +/// through its augmented mass (`J A⁻¹Jᵀ`), not the anchors' lumped masses. +#[cfg(feature = "fem")] +#[test] +fn fem_stiff_stack_rests_without_sinking() { + let mut world = PhysicsWorld::new(); + let ground_top = 1.2; + world.insert( + RigidBodyBuilder::fixed(), + ColliderBuilder::cuboid(25.0, ground_top), + ); + let (half, radius) = (3.0, 0.15); + let mut handles = Vec::new(); + for row in 0..3 { + let center = Vector::new(0.0, ground_top + half + 0.5 + row as Real * (2.0 * half + 1.0)); + let square = SoftBodyBuilder::grid(center, Vector::splat(half), 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e6, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + deformation_damping: 25.0, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(radius) + .self_contacts(true) + .solver(SoftBodySolver::Fem) + .can_sleep(false); + handles.push(world.insert_soft_body(square)); + } + for _ in 0..500 { + world.step(); + } + let mut previous_top = ground_top; + for &h in &handles { + let sb = &world.soft_bodies[h]; + assert_finite(&world, h); + let min_y = sb.particles().iter().map(|p| p.position().y).fold(Real::MAX, Real::min); + let max_y = sb.particles().iter().map(|p| p.position().y).fold(Real::MIN, Real::max); + let max_vel = sb.particles().iter().map(|p| p.velocity().length()).fold(0.0, Real::max); + let penetration = previous_top + radius - min_y; + assert!(penetration < 0.02, "a body sank by {penetration} into the one below"); + assert!(max_vel < 0.1, "the stack did not settle: {max_vel} m/s"); + previous_top = max_y + radius; + } +} + +/// A `Volume` cell-model body on the FEM path keeps its area: its cells are volume elements +/// of the implicit step (no constraint is left on a FEM body). +#[cfg(feature = "fem")] +#[test] +fn fem_volume_cells_keep_their_area() { + let mut world = world_with_ground(); + let square = SoftBodyBuilder::grid(Vector::new(0.0, 2.0), Vector::splat(0.75), 5, 5) + .cell_model(SoftBodyCellModel::Volume) + .solver(SoftBodySolver::Fem) + .particle_mass(0.1); + let handle = world.insert_soft_body(square); + for _ in 0..500 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let max_vel = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + assert!(max_vel < 0.2, "the square still moves at {max_vel} m/s"); + let area = sb.volume(); + assert!( + (area - sb.rest_volume()).abs() < 0.1 * sb.rest_volume(), + "area {area} vs rest {}", + sb.rest_volume() + ); +} + +/// A thin FEM feature (thinner than two skins) with self contacts on stays put: the permanent +/// self contacts across its thickness are excluded (`SoftCollisionMesh::self_contact_excluded`). +#[cfg(feature = "fem")] +#[test] +fn fem_thin_feature_self_contacts_stay_calm() { + let mut world = world_with_ground(); + let bar = SoftBodyBuilder::grid(Vector::new(0.0, 2.0), Vector::new(2.0, 0.2), 11, 2) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e6, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.3) + .self_contacts(true) + .solver(SoftBodySolver::Fem) + .can_sleep(false); + let handle = world.insert_soft_body(bar); + let positions = |world: &PhysicsWorld| -> Vec { + world.soft_bodies[handle] + .particles() + .iter() + .map(|p| p.position()) + .collect() + }; + for _ in 0..400 { + world.step(); + } + let settled = positions(&world); + for _ in 0..800 { + world.step(); + } + assert_finite(&world, handle); + let drift = positions(&world) + .iter() + .zip(&settled) + .map(|(a, b)| (*a - *b).length()) + .fold(0.0, Real::max); + let min_y = settled.iter().map(|p| p.y).fold(Real::MAX, Real::min); + assert!(drift < 1.0e-3, "the bar keeps moving: drift {drift} over 800 steps"); + assert!(min_y > 0.3 - 0.02, "the bar sank into the ground: {min_y}"); +} + /// 2D twin of `violently_dragged_one_particle_cluster_stays_bounded` (rapier3d's /// soft_body_joints.rs): the 2D reduced inertia is a scalar whose plain inverse exploded the /// same way on a one-particle cluster's roundoff inertia. diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index e6968967f..37ab9e056 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -3277,6 +3277,77 @@ fn fem_impact_is_absorbed_by_the_whole_body() { assert!(rebound > 0.0, "the box went through the plank"); } +/// A `Volume` cell-model cube on the FEM path keeps its volume (its cells are volume +/// elements of the implicit step, see the 2D twin `fem_volume_cells_keep_their_area`). +#[cfg(feature = "fem")] +#[test] +fn fem_volume_cells_keep_their_volume() { + let mut world = world_with_ground(); + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 1.5, 0.0), Vector::splat(0.5), 4, 4, 4) + .cell_model(SoftBodyCellModel::Volume) + .solver(SoftBodySolver::Fem) + .particle_mass(0.05); + let handle = world.insert_soft_body(cube); + for _ in 0..400 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let max_vel = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); + assert!(max_vel < 0.2, "the cube still moves at {max_vel} m/s"); + let volume = sb.volume(); + assert!( + (volume - sb.rest_volume()).abs() < 0.1 * sb.rest_volume(), + "volume {volume} vs rest {}", + sb.rest_volume() + ); +} + +/// A stack of stiff FEM cubes rests on the ground without sinking (see the 2D twin +/// `fem_stiff_stack_rests_without_sinking`). +#[cfg(feature = "fem")] +#[test] +fn fem_stiff_stack_rests_without_sinking() { + let mut world = world_with_ground(); + let (half, radius) = (0.5, 0.1); + let mut handles = Vec::new(); + for row in 0..3 { + let center = Vector::new(0.0, half + 0.2 + row as Real * (2.0 * half + 0.3), 0.0); + let cube = SoftBodyBuilder::cuboid(center, Vector::splat(half), 4, 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e6, + poisson_ratio: 0.3, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.05) + .particle_radius(radius) + .solver(SoftBodySolver::Fem) + .can_sleep(false); + handles.push(world.insert_soft_body(cube)); + } + for _ in 0..400 { + world.step(); + } + let mut previous_top = 0.0; + for &h in &handles { + let sb = &world.soft_bodies[h]; + assert_finite(&world, h); + let min_y = sb.particles().iter().map(|p| p.position().y).fold(Real::MAX, Real::min); + let max_y = sb.particles().iter().map(|p| p.position().y).fold(Real::MIN, Real::max); + let max_vel = sb.particles().iter().map(|p| p.velocity().length()).fold(0.0, Real::max); + let penetration = previous_top + radius - min_y; + assert!(penetration < 0.02, "a cube sank by {penetration} into the one below"); + assert!(max_vel < 0.1, "the stack did not settle: {max_vel} m/s"); + previous_top = max_y + radius; + } +} + /// The FEM path's distance elements reproduce the constraint path's springs: the period of a /// mass-spring pair is still set by the material's natural frequency, whatever the substep /// count. diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 7133bbbe3..5e12aeee3 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -160,8 +160,6 @@ impl + Copy> SpringCoefficients { } } - pub propagate_in_relax_pass: bool, - propagate_in_relax_pass: true, /// Configuration parameters that control the physics simulation quality and behavior. /// /// These parameters affect how the physics engine advances time, resolves collisions, and diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs index 5deabbf1a..28a25c970 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs @@ -283,6 +283,27 @@ impl SoftCollisionMesh { Some((cell.vertices, binding.weights, self.vertices[vertex])) } + /// The rest position of the `i`-th vertex (relative to the body's rest center of mass, + /// like `SoftBodyParticle::rest_position`). + pub(crate) fn rest_vertex(&self, body: &SoftBody, i: usize) -> Vector { + match &self.binding { + SoftMeshMapping::Direct { particles } => { + body.particles[particles[i] as usize].rest_position + } + SoftMeshMapping::Skinned { bindings } => { + let binding = &bindings[i]; + let Some(cell) = body.cells.get(binding.cell as usize) else { + return Vector::ZERO; + }; + let mut position = Vector::ZERO; + for (vid, weight) in cell.vertices.iter().zip(&binding.weights) { + position += body.particles[*vid as usize].rest_position * *weight; + } + position + } + } + } + /// The cell id backing each element, `u32::MAX` for none (empty when the elements are not /// cell-backed at all: wires, skinned meshes). pub(crate) fn element_cell_ids<'a>(&'a self, body: &'a SoftBody) -> &'a [u32] { diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs index 464518e15..511e75bbb 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs @@ -77,7 +77,64 @@ impl SoftCollisionMesh { if element.contains(&vertex) { return true; } + if let Some(&start) = self.ring_offsets.get(vertex as usize) { + let end = self.ring_offsets[vertex as usize + 1]; + let ring = &self.ring[start as usize..end as usize]; + if element.iter().any(|v| ring.binary_search(v).is_ok()) { + return true; + } + } + self.self_contact_excluded_at_rest(body, vertex, element, skin) + } + + /// The neighborhood clause of [`Self::self_contact_excluded`] as a stamp table: writes `vertex` + /// into `stamps[e]` (one entry per element) for every element holding the vertex or one of its + /// surface neighbors. + pub(crate) fn mark_self_contact_exclusions(&self, vertex: u32, stamps: &mut [u32]) { + let ring = match self.ring_offsets.get(vertex as usize) { + Some(&start) => { + let end = self.ring_offsets[vertex as usize + 1]; + &self.ring[start as usize..end as usize] + } + None => &[], + }; + for &u in core::iter::once(&vertex).chain(ring) { + for &e in self.vertex_element_ids(u) { + stamps[e as usize] = vertex; + } + } } + + /// The rest-clearance clause of [`Self::self_contact_excluded`] (FEM bodies only). + pub(crate) fn self_contact_excluded_at_rest( + &self, + body: &SoftBody, + vertex: u32, + element: &[u32], + skin: Real, + ) -> bool { + if body.uses_fem() { + let clearance = 2.0 * skin * 1.05; + let p = self.rest_vertex(body, vertex as usize); + let mut rest = [Vector::ZERO; DIM]; + for (k, v) in element.iter().enumerate().take(DIM) { + rest[k] = self.rest_vertex(body, *v as usize); + } + #[cfg(feature = "dim2")] + let dist = Segment::new(rest[0], rest[1]).distance_to_local_point(p, true); + // A 3D wire has segment elements; `solid` does not matter for a 3D triangle. + #[cfg(feature = "dim3")] + let dist = if element.len() == 2 { + Segment::new(rest[0], rest[1]).distance_to_local_point(p, true) + } else { + Triangle::new(rest[0], rest[1], rest[2]).distance_to_local_point(p, true) + }; + if dist < clearance { + } + } + false + } + /// Whether a contact on `element_id` at barycentric `weights`, with the other shape's point at /// `point`, is a ghost of an internal vertex/edge: the point projects inside a neighboring /// element, which reports the real contact, so this one only brings a spurious normal. diff --git a/src/dynamics/soft_body/soft_body_geometry.rs b/src/dynamics/soft_body/soft_body_geometry.rs index 63abc2e3a..6b337ebdf 100644 --- a/src/dynamics/soft_body/soft_body_geometry.rs +++ b/src/dynamics/soft_body/soft_body_geometry.rs @@ -82,6 +82,30 @@ impl SoftBody { } } + /// Gradients of [`Self::cell_volume`] with respect to the cell's vertices. + pub(crate) fn cell_volume_gradients(x: [Vector; DIM + 1]) -> [Vector; DIM + 1] { + let mut grad = [Vector::ZERO; DIM + 1]; + #[cfg(feature = "dim2")] + { + let a = x[1] - x[0]; + let b = x[2] - x[0]; + grad[1] = Vector::new(b.y, -b.x) * 0.5; + grad[2] = Vector::new(-a.y, a.x) * 0.5; + grad[0] = -(grad[1] + grad[2]); + } + #[cfg(feature = "dim3")] + { + let a = x[1] - x[0]; + let b = x[2] - x[0]; + let c = x[3] - x[0]; + grad[1] = b.cross(c) / 6.0; + grad[2] = c.cross(a) / 6.0; + grad[3] = a.cross(b) / 6.0; + grad[0] = -(grad[1] + grad[2] + grad[3]); + } + grad + } + /// The edge matrix `[x1 - x0, x2 - x0, (x3 - x0)]` of a simplex cell. pub(crate) fn cell_edge_matrix(x: [Vector; DIM + 1]) -> Matrix { #[cfg(feature = "dim2")] diff --git a/src/dynamics/soft_body/soft_body_settings.rs b/src/dynamics/soft_body/soft_body_settings.rs index 5a3b09be1..b76634486 100644 --- a/src/dynamics/soft_body/soft_body_settings.rs +++ b/src/dynamics/soft_body/soft_body_settings.rs @@ -62,14 +62,19 @@ pub struct SoftFemParameters { /// A very stiff, finely meshed body can need several hundred iterations to converge; the /// truncated step it gets instead is under-relaxed (safe), only slower to settle. pub max_linear_iterations: usize, - /// + /// Largest number of degrees of freedom (`DIM` per particle) for which the step-start + /// matrix `A` of a body is factorized directly (default: `600`). Each constraint on a FEM body + /// needs one solve against `A` per step; larger bodies fall back to a conjugate gradient. + pub max_dense_dofs: usize, } + #[cfg(feature = "fem")] impl Default for SoftFemParameters { fn default() -> Self { Self { linear_tolerance: 1.0e-5, max_linear_iterations: 20, + max_dense_dofs: 600, } } } diff --git a/src/dynamics/solver/soft_constraint/mod.rs b/src/dynamics/solver/soft_constraint/mod.rs index 9adf2b01f..1b6872f7e 100644 --- a/src/dynamics/solver/soft_constraint/mod.rs +++ b/src/dynamics/solver/soft_constraint/mod.rs @@ -2,9 +2,9 @@ pub(crate) use self::soft_constraints_set::SoftConstraintsSet; -mod soft_attachment; +pub(crate) mod soft_attachment; pub(crate) mod soft_constraints_set; -mod soft_contact; +pub(crate) mod soft_contact; mod soft_contact_assembly; mod soft_contact_chunks; pub(crate) mod soft_element_constraint; diff --git a/src/dynamics/solver/soft_constraint/soft_attachment.rs b/src/dynamics/solver/soft_constraint/soft_attachment.rs index 45ab299d3..f85c4d891 100644 --- a/src/dynamics/solver/soft_constraint/soft_attachment.rs +++ b/src/dynamics/solver/soft_constraint/soft_attachment.rs @@ -3,7 +3,7 @@ //! the body, solved like a locked joint (both passes, no bias in relax, softness, warm start). use crate::dynamics::solver::solver_body::SolverBodies; -use crate::math::{AngVector, AngularInertia, Matrix, Real, Vector}; +use crate::math::{AngVector, AngularInertia, DIM, Matrix, Real, Vector}; use crate::utils::{AngularInertiaOps, ComponentMul, CrossProduct}; #[cfg(not(feature = "std"))] #[allow(unused_imports)] @@ -18,6 +18,9 @@ pub(crate) struct SoftAttachmentConstraint { /// The particle's solver slot and inverse mass. pub particle: u32, pub im_particle: Real, + /// The particle's body is on the FEM path: the constraint acts on it through the body's + /// augmented mass (see [`FemAttachment`]). + pub fem: Option, /// The rigid body's solver slot (`u32::MAX`: fixed or not simulated, the anchor stays at /// `anchor0`) and its attachment point in its CoM-local frame. pub body: u32, @@ -31,13 +34,24 @@ pub(crate) struct SoftAttachmentConstraint { pub body_ii: AngularInertia, /// World lever arm of the anchor about the body's center of mass. pub arm: Vector, - /// The rows' effective-mass matrix and its (softened) inverse. + /// The constraints' effective-mass matrix and its (softened) inverse. pub lhs: Matrix, pub inv_lhs: Matrix, pub rhs: Vector, pub impulse: Vector, } +/// The FEM side of an attachment: `DIM` responses `A⁻¹Jᵀ` (one per particle axis) in +/// `SoftConstraintsSet::fem_responses` from `start`, and their values at the particle, the block +/// `(A⁻¹)_pp` standing in for `im · I` in the constraint's effective mass. +#[derive(Copy, Clone, Debug)] +pub(crate) struct FemAttachment { + pub first_slot: u32, + pub num_particles: u32, + pub start: u32, + pub inv_mass: Matrix, +} + /// The matrix `-[r]× ii [r]×` (3D) or `ii perp(r) perp(r)ᵀ` (2D): the velocity change of the /// point at lever arm `r` per unit impulse applied there, through the body's rotation. fn rotational_effective_mass(arm: Vector, ii: AngularInertia) -> Matrix { @@ -73,7 +87,10 @@ impl SoftAttachmentConstraint { pub fn update(&mut self, bodies: &SolverBodies, wo_bias: bool) { let particle = bodies.get_pose(self.particle).translation; let anchor = self.anchor(bodies); - let mut lhs = Matrix::IDENTITY * self.im_particle; + let mut lhs = match &self.fem { + Some(fem) => fem.inv_mass, + None => Matrix::IDENTITY * self.im_particle, + }; if self.body != u32::MAX { let pose = bodies.get_pose(self.body); self.body_im = pose.im; @@ -101,8 +118,20 @@ impl SoftAttachmentConstraint { /// Applies `impulse` (positive along the constraint: the particle is pulled back, the body pulled /// toward it). #[inline] - fn apply(&self, bodies: &mut SolverBodies, impulse: Vector) { - if (self.particle as usize) < bodies.len() { + fn apply(&self, bodies: &mut SolverBodies, pool: &[Vector], impulse: Vector) { + if let Some(fem) = &self.fem { + let n = fem.num_particles as usize; + let first = fem.first_slot as usize; + for k in 0..DIM { + let u = &pool[fem.start as usize + k * n..fem.start as usize + (k + 1) * n]; + let lambda = impulse[k]; + if lambda != 0.0 { + for (i, ui) in u.iter().enumerate() { + bodies.vels[first + i].linear -= *ui * lambda; + } + } + } + } else if (self.particle as usize) < bodies.len() { bodies.vels[self.particle as usize].linear -= impulse * self.im_particle; } if self.body != u32::MAX && (self.body as usize) < bodies.len() { @@ -114,13 +143,13 @@ impl SoftAttachmentConstraint { } #[inline] - pub fn warmstart(&mut self, bodies: &mut SolverBodies, coeff: Real) { + pub fn warmstart(&mut self, bodies: &mut SolverBodies, pool: &[Vector], coeff: Real) { self.impulse *= coeff; - self.apply(bodies, self.impulse); + self.apply(bodies, pool, self.impulse); } #[inline] - pub fn solve(&mut self, bodies: &mut SolverBodies) { + pub fn solve(&mut self, bodies: &mut SolverBodies, pool: &[Vector]) { let vp = bodies.get_vel(self.particle).linear; let va = if self.body == u32::MAX { Vector::ZERO @@ -133,6 +162,6 @@ impl SoftAttachmentConstraint { + self.inv_lhs * (dv + self.rhs - (self.lhs * self.impulse) * self.cfm_coeff); let delta = total - self.impulse; self.impulse = total; - self.apply(bodies, delta); + self.apply(bodies, pool, delta); } } diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs index 327132a4e..e1fcc3af3 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs @@ -22,7 +22,11 @@ impl SoftConstraintsSet { shape_constraints, volume_constraints, volume_grads, + fem_responses, + .. } = self; + #[cfg(not(feature = "fem"))] + let _ = &fem_responses; let awake = &mut awake[ai]; // SAFETY: read-only access to the soft body's particles and surface. let sb = unsafe { &*awake.ptr }; @@ -204,6 +208,30 @@ impl SoftConstraintsSet { for (i, p) in sb.particles.iter().enumerate() { w += p.inv_mass * grads[i].length_squared(); } + // A FEM body answers the constraint through its augmented mass: the response to the + // fresh gradients, and the augmented gain in place of the lumped one. + #[cfg(feature = "fem")] + if let (Some(fem), Some(side)) = (&awake.fem, &mut vc.fem) { + // SAFETY: one worker per awake body per stage; the system is the body's own. + let system = unsafe { &mut *fem.system }; + let n = sb.particles.len(); + let out = &mut fem_responses[side.start as usize..side.start as usize + n]; + let entries = sb + .particles + .iter() + .enumerate() + .filter(|(i, p)| slots[*i] != u32::MAX && p.inv_mass > 0.0) + .map(|(i, _)| (i as u32, grads[i])); + side.gain = system.response_into(entries, out, &fem.params); + side.u_max = out.iter().map(|u| u.length()).fold(0.0, Real::max); + w = if out.iter().any(|u| u.length() > super::soft_fem_amplification_cap() * side.gain) { + // Ill-conditioned (see `MAX_RESPONSE_AMPLIFICATION`): under-relax with the + // lumped gain, the response still spreads the impulse consistently. + w + } else { + side.gain + }; + } vc.cfm_gain = w * vc.cfm_coeff; vc.inv_lhs = crate::utils::inv(w + vc.cfm_gain); // The position error, biased and capped at solve time. A piece turned inside out is @@ -360,3 +388,16 @@ fn rigid_fit_velocity( (com, vcom, omega) } +/// The response amplification past which a FEM constraint falls back to its lumped gain (see +/// `soft_fem::MAX_RESPONSE_AMPLIFICATION`); the constraint path never asks (no FEM body). +#[inline] +pub(crate) fn soft_fem_amplification_cap() -> crate::math::Real { + #[cfg(feature = "fem")] + { + crate::dynamics::solver::soft_fem::MAX_RESPONSE_AMPLIFICATION + } + #[cfg(not(feature = "fem"))] + { + crate::math::Real::MAX + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs index 486d44d20..1c82b218f 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_rigid_coupling.rs @@ -199,6 +199,8 @@ impl SoftConstraintsSet { attachment: k as u32, particle, im_particle, + fem: None, + body, body_local_point, anchor0, erp_inv_dt, diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs index 9d294d87e..d007cd938 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs @@ -24,10 +24,27 @@ impl SoftConstraintsSet { let sb = unsafe { &*awake.ptr }; let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; let grads = &self.volume_grads[vc.grads.clone()]; + let fem = vc.fem.map(|side| { + let n = sb.particles.len(); + ( + side, + &self.fem_responses[side.start as usize..side.start as usize + n], + slots.first().copied().unwrap_or(u32::MAX) as usize, + ) + }); + // The bias is capped so that it pushes no particle faster than the max corrective // velocity: a crushed closed surface has a large volume error but near-cancelling // gradients, and the uncapped correction would fling its particles. let mut g_max: Real = 0.0; + match &fem { + Some((side, _, _)) => g_max = side.u_max, + None => { + for (i, p) in sb.particles.iter().enumerate() { + g_max = g_max.max(grads[i].length() * p.inv_mass); + } + } + } let max_bias = if g_max * vc.inv_lhs > 0.0 { vc.max_bias_velocity / (g_max * vc.inv_lhs) } else { @@ -35,6 +52,19 @@ impl SoftConstraintsSet { }; let rhs_bias = (vc.rhs * vc.erp_inv_dt).clamp(-max_bias, max_bias); + let apply = |bodies: &mut SolverBodies, impulse: Real| match &fem { + Some((_, u, first)) => { + for (i, ui) in u.iter().enumerate() { + bodies.vels[first + i].linear -= *ui * impulse; + } + } + None => { + for (i, p) in sb.particles.iter().enumerate() { + bodies.vels[s as usize].linear -= grads[i] * (p.inv_mass * impulse); + } + } + }; + if let Some(coeff) = warmstart { vc.impulse *= coeff; apply(bodies, vc.impulse); @@ -67,6 +97,7 @@ impl SoftConstraintsSet { let grads = &self.overlap_grads[constraint.grads.clone()]; let fem_sides = &self.overlap_fem_sides [constraint.fem_sides.start as usize..constraint.fem_sides.end as usize]; + let pool = &self.fem_responses; if constraint.warm_pending { constraint.warm_pending = false; let warm_impulses = &self.overlap_warm_impulses[constraint.grads.clone()]; @@ -75,6 +106,16 @@ impl SoftConstraintsSet { bodies.vels[slot as usize].linear += p * im; } } + for side in fem_sides { + if side.warm != u32::MAX { + let n = side.num_particles as usize; + let u = &pool[side.warm as usize..side.warm as usize + n]; + let first = side.first_slot as usize; + for (i, ui) in u.iter().enumerate() { + bodies.vels[first + i].linear += *ui; + } + } + } if let Some((slot, ..)) = constraint.rigid.filter(|r| live(r.0)) { let pose = bodies.get_pose(slot); let (lin, ang) = constraint.warm_rigid; @@ -90,6 +131,14 @@ impl SoftConstraintsSet { bodies.vels[slot as usize].linear -= g * (im * constraint.impulse); } } + for side in fem_sides { + let n = side.num_particles as usize; + let u = &pool[side.start as usize..side.start as usize + n]; + let first = side.first_slot as usize; + for (i, ui) in u.iter().enumerate() { + bodies.vels[first + i].linear -= *ui * constraint.impulse; + } + } if let Some((slot, g_lin, g_ang)) = constraint.rigid.filter(|r| live(r.0)) { let pose = bodies.get_pose(slot); let ii_g = pose.ii.transform_vector(g_ang); @@ -106,6 +155,7 @@ impl SoftConstraintsSet { let grads: &[(u32, Real, Vector, Vector)] = &self.overlap_grads[constraint.grads.clone()]; let fem_sides = &self.overlap_fem_sides [constraint.fem_sides.start as usize..constraint.fem_sides.end as usize]; + let pool = &self.fem_responses; let mut w = 0.0; let mut g_max: Real = 0.0; for &(slot, im, g, _) in grads { @@ -114,6 +164,10 @@ impl SoftConstraintsSet { g_max = g_max.max(g.length() * im); } } + for side in fem_sides { + w += side.gain; + g_max = g_max.max(side.u_max); + } // The rigid side's effective mass (its mass properties are step-constant, read from // the solver body). let rigid = constraint.rigid.filter(|r| live(r.0)).map(|(slot, g_lin, g_ang)| { @@ -186,6 +240,14 @@ impl SoftConstraintsSet { bodies.vels[slot as usize].linear -= g * (im * delta); } } + for side in fem_sides { + let n = side.num_particles as usize; + let u = &pool[side.start as usize..side.start as usize + n]; + let first = side.first_slot as usize; + for (i, ui) in u.iter().enumerate() { + bodies.vels[first + i].linear -= *ui * delta; + } + } if let Some((slot, g_lin, _, im, ii_g)) = rigid { let v = &mut bodies.vels[slot as usize]; v.linear -= im.component_mul(&g_lin) * delta; @@ -268,6 +330,28 @@ impl SoftConstraintsSet { } } + /// Solves one shape-matching constraint: updated from the current poses when `update`, + /// warm-started with `Some(coefficient)`. + #[inline] + pub fn solve_shape_constraint( + &mut self, + constraint_id: usize, + bodies: &mut SolverBodies, + update: bool, + warmstart: Option, + ) { + let constraint = &mut self.shape_constraints[constraint_id]; + let pool = &self.fem_responses; + if update { + constraint.update(bodies); + } + if let Some(coeff) = warmstart { + constraint.impulse *= coeff; + constraint.warmstart(bodies, pool); + } + constraint.solve(bodies, pool); + } + /// Solves the attachment constraints of `group` (serial): updated from the current poses when /// `update`, warm-started with `Some(coefficient)`, without bias in the relax pass. pub fn solve_attachments( @@ -278,12 +362,15 @@ impl SoftConstraintsSet { update: bool, warmstart: Option, ) { + let pool: &[Vector] = &self.fem_responses; for constraint in &mut self.attachments[self.groups[group].attachments.clone()] { if update { constraint.update(bodies, wo_bias); } if let Some(coeff) = warmstart { + constraint.warmstart(bodies, pool, coeff); } + constraint.solve(bodies, pool); } } } diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs index b4753298c..b16f0dee8 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs @@ -31,23 +31,85 @@ pub(crate) struct SoftShapeConstraint { pub cfm_gain: Vector, pub rhs: Vector, pub impulse: Vector, + /// The particle's body is on the FEM path: the constraint acts through the body's augmented + /// mass (the block `(A⁻¹)_pp` in place of `im`, the responses spread over the body). + pub fem: Option, + /// The FEM path's effective-mass inverse and softness gain (blocks). + pub inv_lhs_block: Matrix, + pub cfm_gain_block: Matrix, } + impl SoftShapeConstraint { #[inline] pub fn update(&mut self, bodies: &SolverBodies) { let pos = bodies.get_pose(self.solver_id).translation; + if let Some(fem) = &self.fem { + let cfm_gain = fem.inv_mass * self.cfm_coeff; + self.cfm_gain_block = cfm_gain; + let lhs = fem.inv_mass + cfm_gain; + let inv = lhs.inverse(); + self.inv_lhs_block = if inv.is_finite() { inv } else { Matrix::ZERO }; + } else { + let cfm_gain = self.im * self.cfm_coeff; + self.cfm_gain = cfm_gain; + let lhs = self.im + cfm_gain; + let mut inv_lhs = Vector::ZERO; + for k in 0..DIM { + inv_lhs[k] = crate::utils::inv(lhs[k]); + } + self.inv_lhs = inv_lhs; + } // Uncapped: the bias is the spring force (see `SoftScalarConstraint::update`). self.rhs = (pos - self.goal) * self.erp_inv_dt; } + /// Applies `impulse` (positive along the constraint: the particle is pulled back). #[inline] + fn apply(&self, bodies: &mut SolverBodies, pool: &[Vector], impulse: Vector) { + if let Some(fem) = &self.fem { + let n = fem.num_particles as usize; + let first = fem.first_slot as usize; + for k in 0..DIM { + let lambda = impulse[k]; + if lambda == 0.0 { + continue; + } + let u = &pool[fem.start as usize + k * n..fem.start as usize + (k + 1) * n]; + for (i, ui) in u.iter().enumerate() { + bodies.vels[first + i].linear -= *ui * lambda; + } + } + } else if (self.solver_id as usize) < bodies.len() { + bodies.vels[self.solver_id as usize].linear -= self.im.component_mul(&impulse); + } + } + #[inline] + pub fn warmstart(&self, bodies: &mut SolverBodies, pool: &[Vector]) { + self.apply(bodies, pool, self.impulse); + } + + #[inline] + pub fn solve(&mut self, bodies: &mut SolverBodies, pool: &[Vector]) { if (self.solver_id as usize) >= bodies.len() { return; } + let v = bodies.vels[self.solver_id as usize].linear; + let dc = v - self.goal_vel; + let total = if self.fem.is_some() { + self.impulse + self.inv_lhs_block * (dc + self.rhs - self.cfm_gain_block * self.impulse) + } else { + self.impulse + + self + .inv_lhs + .component_mul(&(dc + self.rhs - self.cfm_gain.component_mul(&self.impulse))) + }; let delta = total - self.impulse; self.impulse = total; + self.apply(bodies, pool, delta); + } } + /// The global area/volume preservation row of a soft body (touches every particle: solved /// serially). #[derive(Clone, Debug)] @@ -64,7 +126,34 @@ pub(crate) struct SoftVolumeConstraint { pub cfm_gain: Real, pub rhs: Real, pub impulse: Real, + /// The body is on the FEM path: the constraint's response `A⁻¹Jᵀ` (refreshed at each substep's + /// prepare) and augmented gain, see [`FemVolumeSide`]. + pub fem: Option, +} + +/// The FEM side of a volume constraint: its response in `SoftConstraintsSet::fem_responses` from +/// `start` (one vector per particle), its gain `J A⁻¹Jᵀ`, and the largest particle speed per +/// unit impulse (the bias cap's `g_max`). +#[derive(Copy, Clone, Debug)] +pub(crate) struct FemVolumeSide { + pub start: u32, + pub gain: Real, + pub u_max: Real, } + +/// The FEM-body side of an intersection-volume constraint: the gradient response from `start` in +/// `SoftConstraintsSet::fem_responses`, the warm-impulse response (`warm`, `u32::MAX`: none), the +/// augmented gain and speed cap; its `overlap_grads` inverse masses are zeroed (lumped loops skip). +#[derive(Copy, Clone, Debug)] +pub(crate) struct FemOverlapSide { + pub first_slot: u32, + pub num_particles: u32, + pub start: u32, + pub warm: u32, + pub gain: Real, + pub u_max: Real, +} + /// An intersection-volume constraint (see `SoftRecoverySettings::overlap_constraints`): a scalar /// unilateral constraint retracting the part of a closed boundary inside another (or its own /// mirrored lobe) along the gradient of the enclosed area/volume. Solved serially, no warm start. @@ -80,30 +169,31 @@ pub(crate) struct SoftOverlapConstraint { pub max_bias_velocity: Real, pub impulse: Real, /// A negative `rhs` is slack, allowed to close at this rate (the skin band of a hard - /// row may be consumed within the substep, never past it); `0.0` for the rows whose + /// constraint may be consumed within the substep, never past it); `0.0` for the constraints whose /// `rhs` is an error. pub speculative_inv_dt: Real, - /// The rigid side of an overlap row, when the other body is a simulated rigid body: - /// its solver slot, and the volume gradient with respect to its translation and its - /// rotation about its center of mass (the reaction to the soft patch, applied at the - /// patch's vertices). + /// The rigid side, when the other body is a simulated rigid body: its solver slot and the + /// volume gradient with respect to its translation and rotation about its center of mass. pub rigid: Option<(u32, Vector, AngVector)>, - /// A hard row (the skin-volume rows, contacts in their own right): its impulse is not - /// bounded by the pace, and its right-hand side is refreshed every substep from the - /// current poses (the volume linearized with the frozen gradients from `rhs0`), so - /// its slack is consumed exactly, never several times over. + /// A hard constraint (skin-volume constraints, contacts in their own right): its impulse is not + /// bounded by the pace, and its right-hand side is refreshed every substep from the current + /// poses (the volume linearized with frozen gradients from `rhs0`), consuming its slack once. pub hard: bool, /// The assembled right-hand side (see `hard`). pub rhs0: Real, /// The rigid side's center of mass and rotation at assembly (see `hard`). pub rigid_pose0: (Vector, Rotation), - /// Where the row's impulses are carried to for the next step's warm start (see - /// `SoftOverlapWarm`): the awake body, its mesh and the other collider. The carried - /// impulse of every entry lives in `overlap_warm_impulses`, aligned with `grads`, and - /// the entries' particles in `overlap_particles`. + /// Where the impulses are stored for the next step's warm start (see `SoftOverlapWarm`): the + /// awake body, its mesh and the other collider. The warm impulses live in + /// `overlap_warm_impulses` (aligned with `grads`), their particles in `overlap_particles`. pub warm: Option<(u32, SoftMeshId, ColliderHandle)>, - /// The carried per-particle impulses are still to be applied (the first substep). + /// The warm per-particle impulses are still to be applied (the first substep). pub warm_pending: bool, - /// The rigid side's carried linear and angular impulse. + /// The rigid side's warm linear and angular impulse. pub warm_rigid: (Vector, AngVector), + /// The constraint's FEM sides in `overlap_fem_sides` (see [`FemOverlapSide`]). + pub fem_sides: Range, + /// The narrow-phase pair's contact slot of the constraint's bin (the two colliders and the slot, + /// `u32::MAX`: none), which reports its impulse. + pub report: (ColliderHandle, ColliderHandle, u32), } 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 2715342af..7fa5fb7ef 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 @@ -44,12 +44,17 @@ pub(crate) struct AwakeSoftBody { /// 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`: - /// no contact row at all), set by the contact assembly. + /// 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: - /// `(cluster index, rotation)`, refreshed by the per-pass prepare. + /// `(cluster index, rotation)`, updated by the per-pass prepare. pub awake_clusters: Vec, + /// The body's FEM system when it is on the FEM path (set by `SoftFemSet::assemble`): the + /// per-substep constraints (the volume constraints) update their responses through it. + #[cfg(feature = "fem")] + pub fem: Option, } + // SAFETY: the raw pointer is only dereferenced under the staged solver's stage discipline. unsafe impl Send for AwakeSoftBody {} unsafe impl Sync for AwakeSoftBody {} @@ -154,33 +159,39 @@ pub(crate) struct SoftConstraintsSet { /// past the re-sweep threshold at its last update: the re-sweep reads these flags instead /// of every constraint. pub strained: Vec, - /// Row ranges of the parallel colors, group-major then color ascending. + /// Constraint ranges of the parallel colors, group-major then color ascending. pub color_ranges: Vec, pub shape_constraints: Vec, pub volume_constraints: Vec, pub volume_grads: Vec, - /// The intersection-volume rows (see `SoftOverlapRow`), group-major, and their gradients. + /// The intersection-volume constraints (see `SoftOverlapConstraint`), group-major, and their gradients. pub overlap_constraints: Vec, pub overlap_grads: Vec<(u32, Real, Vector, Vector)>, - /// The carried impulse of every overlap gradient entry (see `SoftOverlapRow::warm`). + /// The warm impulse of every overlap gradient entry (see `SoftOverlapConstraint::warm`). pub overlap_warm_impulses: Vec, /// The `(side, particle)` of every overlap gradient entry (`0`: the owner body, `1`: - /// the other soft body), for the write-back of the carried impulses. + /// the other soft body), for the write-back of the warm impulses. pub overlap_particles: Vec<(u8, u32)>, /// Contacts against the awake soft bodies' surface colliders, group-major. pub contacts: Vec, + /// The responses `A⁻¹Jᵀ` of the constraints acting on FEM soft bodies (see + /// `soft_fem::SoftFemSet::assemble_responses`): a vector per particle, per constraint side and + /// direction, addressed by the constraints' `fem` records. Empty without FEM bodies. + pub fem_responses: Vec, + /// The FEM sides of the intersection-volume constraints (see [`FemOverlapSide`]). + pub overlap_fem_sides: Vec, /// The particle attachments of the awake soft bodies, group-major. pub attachments: Vec, /// The active clusters (a joint or an external impulse acts on their proxy), group-major. pub clusters: Vec, - /// The contact rows chunked by body pair (see `soft_contact_chunks`): row indices into + /// The contact constraints chunked by body pair (see `soft_contact_chunks`): constraint indices into /// `contacts` chunk by chunk, the chunks' ranges into them (group-major, parallel colors /// then the serial tail), and the chunk ranges of the parallel colors. pub contact_chunk_constraints: Vec, pub contact_chunks: Vec>, pub contact_colors: Vec>, pub groups: Vec, - /// Scratch of the element-constraint assembly (see `soft_element_constraint_assembly`). + /// Workspace of the element-constraint assembly (see `soft_element_constraint_assembly`). pub(crate) element_constraint_workspace: super::soft_element_constraint_assembly::ElementConstraintAssemblyWorkspace, /// Workspace of the edge-vs-edge contact assembly (see `soft_contact_assembly`). diff --git a/src/dynamics/solver/soft_constraint/soft_contact.rs b/src/dynamics/solver/soft_constraint/soft_contact.rs index e572a80c3..616e1156d 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact.rs @@ -34,9 +34,36 @@ pub(crate) struct SoftContactElement { } /// Number of particles the soft side of a contact is spread over: a surface element's, or a -/// cell's when the body collides through a skin the cells carry. +/// cell's when the body collides through a skin the cells hold. pub(crate) const CONTACT_ANCHORS: usize = DIM + 1; +/// A contact side on a FEM soft body: the constraint acts through the body's augmented mass, not +/// the anchors' lumped masses (see `soft_fem`). The responses `A⁻¹Jᵀ` (normal, then tangents) +/// live in `SoftConstraintsSet::fem_responses`, one vector per particle, from `start`. +#[derive(Copy, Clone, Debug)] +pub(crate) struct FemContactSide { + /// Solver slot of the body's first particle (its particles' slots are contiguous). + pub first_slot: u32, + pub num_particles: u32, + pub start: u32, + /// Gains along the normal and the tangents: the augmented `J A⁻¹Jᵀ`, or the lumped ones when + /// those cannot drive the constraint (see `SoftFemSet::assemble_responses`). + pub gains: [Real; DIM], + /// Both sides of the constraint are on this body (a self contact): the response combines the + /// two sides' jacobians and is applied once. + pub covers_other: bool, +} + +impl FemContactSide { + /// The response along direction `which` (`0`: the normal, `1 + j`: tangent `j`). + #[inline] + fn response<'a>(&self, pool: &'a [Vector], which: usize) -> &'a [Vector] { + let n = self.num_particles as usize; + let start = self.start as usize + which * n; + &pool[start..start + n] + } +} + /// One contact point between a soft surface element and a solver body. #[derive(Copy, Clone, Debug)] pub(crate) struct SoftContact { @@ -73,9 +100,12 @@ pub(crate) struct SoftContact { /// Separation at build time (skins baked in), and its live value. pub dist0: Real, pub friction: Real, - /// The other side is a soft-body particle: the row is solved in the biased pass only, like + /// The other side is a soft-body particle: the constraint is solved in the biased pass only, like /// the springs driving both sides. pub soft_other: bool, + /// The sides belonging to a FEM soft body (the surface side, then the other side), see + /// [`FemContactSide`]. + pub fem: [Option; 2], /// Static or dynamic contact softness. pub erp_inv_dt: Real, pub cfm_factor: Real, @@ -166,35 +196,53 @@ impl SoftContact { *im = [0.0; N]; } } - freeze_levers(&self.weights, &mut self.im_particles); - if let Some(e) = &mut self.element { + // A FEM side answers through its body's augmented mass: its gain is step-constant + // (`fem.gains`), its lumped terms drop out. + let (support_fem, other_fem) = self.fem_sides(); + if !support_fem { + freeze_levers(&self.weights, &mut self.im_particles); + } + if let (Some(e), false) = (&mut self.element, other_fem) { freeze_levers(&e.weights, &mut e.im_particles); } let mut w_particles = 0.0; - for k in 0..CONTACT_ANCHORS { - w_particles += self.weights[k] * self.weights[k] * self.im_particles[k]; + if !support_fem { + for k in 0..CONTACT_ANCHORS { + w_particles += self.weights[k] * self.weights[k] * self.im_particles[k]; + } } - if let Some(e) = &self.element { + if let (Some(e), false) = (&self.element, other_fem) { for k in 0..CONTACT_ANCHORS { w_particles += e.weights[k] * e.weights[k] * e.im_particles[k]; } } + let fem_gain = |which: usize| -> Real { + self.fem + .iter() + .flatten() + .map(|side| side.gains[which]) + .sum() + }; + // The rigid (or lumped soft particle) side, unless it is a FEM particle. + let body_terms = !other_fem || self.element.is_some(); self.torque_dir = self.body_arm.gcross(-self.dir); self.ii_torque_dir = self.body_ii.transform_vector(self.torque_dir); - self.r_normal = crate::utils::inv( - w_particles - + self.dir.gdot(self.body_im.component_mul(&self.dir)) - + self.ii_torque_dir.gdot(self.torque_dir), - ); + let mut r_normal = w_particles + fem_gain(0); + if body_terms { + r_normal += self.dir.gdot(self.body_im.component_mul(&self.dir)) + + self.ii_torque_dir.gdot(self.torque_dir); + } + self.r_normal = crate::utils::inv(r_normal); for j in 0..DIM - 1 { let t = self.tangents[j]; self.torque_tangent[j] = self.body_arm.gcross(-t); self.ii_torque_tangent[j] = self.body_ii.transform_vector(self.torque_tangent[j]); - self.r_tangent[j] = crate::utils::inv( - w_particles - + t.gdot(self.body_im.component_mul(&t)) - + self.ii_torque_tangent[j].gdot(self.torque_tangent[j]), - ); + let mut r_tangent = w_particles + fem_gain(1 + j); + if body_terms { + r_tangent += t.gdot(self.body_im.component_mul(&t)) + + self.ii_torque_tangent[j].gdot(self.torque_tangent[j]); + } + self.r_tangent[j] = crate::utils::inv(r_tangent); } let rhs_wo_bias = dist.max(0.0) * inv_dt; @@ -225,20 +273,56 @@ impl SoftContact { /// Which sides belong to a FEM body: `(surface side, other side)`. #[inline] - fn apply(&self, bodies: &mut SolverBodies, dir: Vector, ii_torque: AngVector, lambda: Real) { - for k in 0..CONTACT_ANCHORS { - let id = self.particles[k]; - if id != u32::MAX && (id as usize) < bodies.len() { - bodies.vels[id as usize].linear += - dir * (self.weights[k] * self.im_particles[k] * lambda); + fn fem_sides(&self) -> (bool, bool) { + let support = self.fem[0].is_some(); + let other = self.fem[1].is_some() || self.fem[0].is_some_and(|s| s.covers_other); + (support, other) + } + + /// Applies an impulse `lambda` along `dir` (given with its torque terms) to both sides. + /// `which` selects the FEM sides' response (`0`: the normal, `1 + j`: tangent `j`). + #[inline] + pub(crate) fn apply( + &self, + bodies: &mut SolverBodies, + pool: &[Vector], + which: usize, + dir: Vector, + ii_torque: AngVector, + lambda: Real, + ) { + let (support_fem, other_fem) = self.fem_sides(); + for side in self.fem.iter().flatten() { + let u = side.response(pool, which); + let first = side.first_slot as usize; + for (i, ui) in u.iter().enumerate() { + bodies.vels[first + i].linear += *ui * lambda; + } + } + if !support_fem { for k in 0..CONTACT_ANCHORS { - let id = e.particles[k]; + let id = self.particles[k]; if id != u32::MAX && (id as usize) < bodies.len() { - bodies.vels[id as usize].linear -= - dir * (e.weights[k] * e.im_particles[k] * lambda); + bodies.vels[id as usize].linear += + dir * (self.weights[k] * self.im_particles[k] * lambda); + } + } + } + if let Some(e) = &self.element { + if !other_fem { + for k in 0..CONTACT_ANCHORS { + let id = e.particles[k]; + if id != u32::MAX && (id as usize) < bodies.len() { + bodies.vels[id as usize].linear -= + dir * (e.weights[k] * e.im_particles[k] * lambda); + } } } - } else if self.body != u32::MAX && (self.body as usize) < bodies.len() { + } else if !other_fem && self.body != u32::MAX && (self.body as usize) < bodies.len() { + let v = &mut bodies.vels[self.body as usize]; + v.linear -= dir.component_mul(&self.body_im) * lambda; + v.angular += ii_torque * lambda; + } } /// Relative velocity of the surface point w.r.t. the other side's contact point, dotted @@ -270,11 +354,13 @@ impl SoftContact { } /// Applies the accumulated (warm-start) impulses. - pub fn warmstart(&self, bodies: &mut SolverBodies) { - self.apply(bodies, self.dir, self.ii_torque_dir, self.impulse_normal); + pub fn warmstart(&self, bodies: &mut SolverBodies, pool: &[Vector]) { + self.apply(bodies, pool, 0, self.dir, self.ii_torque_dir, self.impulse_normal); for j in 0..DIM - 1 { self.apply( bodies, + pool, + 1 + j, self.tangents[j], self.ii_torque_tangent[j], self.impulse_tangent[j], @@ -283,13 +369,13 @@ impl SoftContact { } /// One Gauss-Seidel iteration: normal part then Coulomb friction. - pub fn solve(&mut self, bodies: &mut SolverBodies) { + pub fn solve(&mut self, bodies: &mut SolverBodies, pool: &[Vector]) { // Normal. let dvel = self.relative_velocity(bodies, self.dir, self.torque_dir) + self.rhs_normal; let new_impulse = (self.cfm_normal * (self.impulse_normal - self.r_normal * dvel)).max(0.0); let delta = new_impulse - self.impulse_normal; self.impulse_normal = new_impulse; - self.apply(bodies, self.dir, self.ii_torque_dir, delta); + self.apply(bodies, pool, 0, self.dir, self.ii_torque_dir, delta); // Friction: per-tangent update, then a clamp to the disc of radius μλ. let limit = self.friction * self.impulse_normal; @@ -315,7 +401,14 @@ impl SoftContact { for j in 0..DIM - 1 { let delta = new_tangent[j] - self.impulse_tangent[j]; self.impulse_tangent[j] = new_tangent[j]; - self.apply(bodies, self.tangents[j], self.ii_torque_tangent[j], delta); + self.apply( + bodies, + pool, + 1 + j, + self.tangents[j], + self.ii_torque_tangent[j], + delta, + ); } } diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs index c52522111..303bb6e71 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs @@ -169,6 +169,11 @@ impl SoftConstraintsSet { speculative_inv_dt: if constraint.hard { 1.0 / group_dt(gi) } else { 0.0 }, rigid: constraint.rigid, hard: constraint.hard, + fem_sides: 0..0, + report: constraint.report, + }); + } + } self.groups[gi].overlap_constraints = ostart..self.overlap_constraints.len(); } // Hand the new edge and vertex contacts to their owner bodies. diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs index 418db5c97..5a412cc01 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs @@ -548,6 +548,7 @@ impl SoftConstraintsSet { dist0: dist, friction: manifold.data.friction, soft_other: false, + fem: [None, None], erp_inv_dt, cfm_factor, max_bias: Real::MAX, @@ -696,6 +697,7 @@ impl SoftConstraintsSet { dist0: vc.dist, friction: manifold.data.friction, soft_other: false, + fem: [None, None], erp_inv_dt, cfm_factor, max_bias: Real::MAX, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs index eec3e21d5..598589f59 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs @@ -232,6 +232,7 @@ impl SoftConstraintsSet { dist0: dist, friction, soft_other: true, + fem: [None, None], erp_inv_dt, cfm_factor, max_bias: Real::MAX, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs index d4500fccc..8b9b9990a 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs @@ -461,6 +461,7 @@ impl SoftConstraintsSet { dist0: dist, friction, soft_other: true, + fem: [None, None], erp_inv_dt, cfm_factor, max_bias: Real::MAX, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs b/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs index a454a5d94..2a38083c3 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs @@ -9,6 +9,7 @@ use super::soft_constraints_set::SoftConstraintsSet; use super::soft_contact::SoftContact; use crate::dynamics::IntegrationParameters; use crate::dynamics::solver::solver_body::SolverBodies; +use crate::math::Vector; /// Colors tried before a chunk goes to the serial tail. const MAX_CONTACT_COLORS: usize = 16; @@ -347,8 +348,18 @@ impl SoftConstraintsSet { update: bool, warmstart: bool, ) { - for &row in &self.contact_chunk_rows[self.contact_chunks[chunk].clone()] { - let c = &mut self.contacts[row as usize]; + // Split borrows only: chunks of one color are solved by several workers at once, and + // every one of them reads the shared response pool. + let Self { + contacts, + contact_chunk_constraints, + contact_chunks, + fem_responses, + .. + } = self; + let pool: &[Vector] = fem_responses; + for &constraint in &contact_chunk_constraints[contact_chunks[chunk].clone()] { + let c = &mut contacts[constraint as usize]; if wo_bias && c.soft_other && c.dist0 > 0.0 { continue; } @@ -356,9 +367,9 @@ impl SoftConstraintsSet { c.update(bodies, params, wo_bias); } if warmstart { - c.warmstart(bodies); + c.warmstart(bodies, pool); } - c.solve(bodies); + c.solve(bodies, pool); } } } diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs index d0258a6e7..719297276 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs @@ -159,6 +159,8 @@ impl SoftScalarConstraint { SoftScalarConstraintKind::CellVolume => { let x: [Vector; DIM + 1] = core::array::from_fn(|k| self.pos[k]); let vol = SoftBody::cell_volume(x); + let grad = SoftBody::cell_volume_gradients(x); + self.grad[..DIM + 1].copy_from_slice(&grad); vol - self.rest } } diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs index 70bb25e20..46dc11ac3 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs @@ -25,7 +25,8 @@ use crate::dynamics::{ RigidBodyHandle, RigidBodySet, SoftBody, SoftBodyCellModel, SoftBodyEdgeKind, SoftBodyHandle, SoftBodySet, SpringCoefficients, }; -use crate::math::{AngVector, DIM, Real, Vector}; +use crate::math::{AngVector, DIM, Matrix, Real, Vector}; +use crate::utils::RotationOps; use na::SimdRealField; use super::soft_constraints_set::AwakeCluster; @@ -215,6 +216,7 @@ impl SoftConstraintsSet { inv_lhs: 0.0, cfm_gain: 0.0, rhs: 0.0, + fem: None, }); } } @@ -250,9 +252,11 @@ impl SoftConstraintsSet { }; g.colors = colors_start..self.color_ranges.len(); g.shape_constraints = shape_start..shape_cursor; - g.shape_serial = self.awake[awake_start..awake_cursor] - .iter() - .any(|a| !a.awake_clusters.is_empty()); + // Serial as well when a FEM body has shape constraints: each writes all of its particles. + g.shape_serial = self.awake[awake_start..awake_cursor].iter().any(|a| { + // SAFETY: read-only access during assembly. + !a.awake_clusters.is_empty() || unsafe { &*a.ptr }.uses_fem() + }); g.volume_constraints = volume_start..self.volume_constraints.len(); g.damping = damping; } @@ -473,6 +477,8 @@ impl SoftConstraintsSet { torn: AtomicBool::new(false), contact_approach_speed: None, awake_clusters, + #[cfg(feature = "fem")] + fem: None, }); } } @@ -511,8 +517,11 @@ impl SoftConstraintsSet { let (mu, _) = sb.material.lame_parameters(); for c in &sb.cells[cells] { match sb.cell_model { + // The FEM solver integrates the volume cells itself; they get no constraint. + SoftBodyCellModel::Volume if sb.uses_fem() => {} SoftBodyCellModel::Volume => counts[c.color as usize] += 1, SoftBodyCellModel::Corotational | SoftBodyCellModel::NeoHookean => { + // The FEM solver integrates the elastic cells itself; they get no constraint. if !sb.uses_fem() && mu * c.stiffness_scale > 0.0 && elastic_cell_terms(sb, slots, c).2 > 0.0 @@ -680,6 +689,8 @@ impl SoftConstraintsSet { for (ci, c) in sb.cells.iter().enumerate().take(cells.end).skip(cells.start) { match sb.cell_model { + // Integrated implicitly by the FEM solver instead (see `soft_fem`). + SoftBodyCellModel::Volume if sb.uses_fem() => continue, SoftBodyCellModel::Volume => { let constraint = base_constraint( SoftScalarConstraintKind::CellVolume, @@ -856,6 +867,9 @@ impl SoftConstraintsSet { cfm_gain: Vector::ZERO, rhs: Vector::ZERO, impulse: cluster.shape_impulses.get(k).copied().unwrap_or(Vector::ZERO), + fem: None, + inv_lhs_block: Matrix::ZERO, + cfm_gain_block: Matrix::ZERO, }); } } diff --git a/src/dynamics/solver/soft_fem/mod.rs b/src/dynamics/solver/soft_fem/mod.rs index e58b08485..1f5b27c38 100644 --- a/src/dynamics/solver/soft_fem/mod.rs +++ b/src/dynamics/solver/soft_fem/mod.rs @@ -7,4 +7,4 @@ mod soft_fem_skyline; mod soft_fem_sparse; mod system; -pub(crate) use soft_fem_set::SoftFemSet; +pub(crate) use soft_fem_set::{AwakeFem, MAX_RESPONSE_AMPLIFICATION, SoftFemSet}; diff --git a/src/dynamics/solver/soft_fem/soft_fem_set.rs b/src/dynamics/solver/soft_fem/soft_fem_set.rs index cd6366da7..8a335348c 100644 --- a/src/dynamics/solver/soft_fem/soft_fem_set.rs +++ b/src/dynamics/solver/soft_fem/soft_fem_set.rs @@ -4,9 +4,12 @@ use super::system::SoftFemSystem; use crate::alloc_prelude::*; use crate::dynamics::solver::soft_constraint::SoftConstraintsSet; +use crate::dynamics::solver::soft_constraint::soft_constraints_set::{FemOverlapSide, FemVolumeSide}; +use crate::dynamics::solver::soft_constraint::soft_attachment::FemAttachment; +use crate::dynamics::solver::soft_constraint::soft_contact::{CONTACT_ANCHORS, FemContactSide}; use crate::dynamics::solver::solver_body::SolverBodies; use crate::dynamics::{SoftBody, SoftBodyHandle, SoftBodySet, SoftFemParameters}; -use crate::math::Real; +use crate::math::{DIM, Matrix, Real, Vector}; use core::ops::Range; use parry::utils::hashmap::HashMap; @@ -18,6 +21,22 @@ struct ActiveFem { system: *mut SoftFemSystem, /// The substep length of the body's solve group. dt: Real, + /// Solver slot of the body's first particle (the particles' slots are contiguous). + first_slot: u32, + num_particles: u32, + params: SoftFemParameters, +} + +/// A constraint side answered by a FEM body, filled by the response stage (see +/// `SoftFemSet::plan_responses`). +#[derive(Copy, Clone, Debug)] +enum FemConstraintRef { + /// `contacts[constraint].fem[side]` (the combined same-body response when it `covers_other`). + Contact { constraint: u32, side: u8 }, + Attachment(u32), + Shape(u32), + /// `overlap_fem_sides[side]` of `overlap_constraints[constraint]`. + Overlap { constraint: u32, side: u32 }, } // SAFETY: the pointers are only dereferenced during the solve, where the stage discipline gives @@ -25,6 +44,19 @@ struct ActiveFem { unsafe impl Send for ActiveFem {} unsafe impl Sync for ActiveFem {} +/// A FEM body's system as seen by the soft constraints set (`AwakeSoftBody::fem`), for the +/// constraints updating their responses per substep. +#[derive(Copy, Clone)] +pub(crate) struct AwakeFem { + /// The body's system, valid for the solve's scope (see `ActiveFem::system`). + pub system: *mut SoftFemSystem, + pub params: SoftFemParameters, +} + +// SAFETY: same contract as `ActiveFem`. +unsafe impl Send for AwakeFem {} +unsafe impl Sync for AwakeFem {} + /// The FEM systems of every soft body that selected the FEM solver, looked up by handle and kept /// across steps (the sparsity pattern and element tables are the expensive part; values refill /// every substep); `active` holds the awake ones of the island being solved, group-major. @@ -34,6 +66,10 @@ pub(crate) struct SoftFemSet { active: Vec, /// Slice of `active` belonging to each substep group. groups: Vec>, + /// Index into `active` of every awake soft body (`u32::MAX`: not on the FEM path). + fem_of_awake: Vec, + /// Per active body, the constraint sides it answers this step. + refs: Vec>, } impl SoftFemSet { @@ -58,13 +94,17 @@ impl SoftFemSet { /// in the parallel response stage ([`Self::compute_responses`]). pub fn assemble( &mut self, - soft_constraints: &SoftConstraintsSet, + soft_constraints: &mut SoftConstraintsSet, soft_bodies: &SoftBodySet, num_groups: usize, group_dt: impl Fn(usize) -> Real, + params: &SoftFemParameters, ) { self.active.clear(); self.groups.clear(); + self.fem_of_awake.clear(); + self.fem_of_awake + .resize(soft_constraints.awake.len(), u32::MAX); self.groups.resize(num_groups.max(1), 0..0); if soft_constraints.is_empty() { // Bodies whose soft body was removed would otherwise keep their system forever; the @@ -91,59 +131,429 @@ impl SoftFemSet { } // `awake` is already sorted by group, so a single pass keeps the group-major order. - for awake in &soft_constraints.awake { + let slots_all = &soft_constraints.slots; + for (ai, awake) in soft_constraints.awake.iter_mut().enumerate() { + awake.fem = None; let sb = unsafe { &*awake.ptr }; if awake.frozen || !sb.uses_fem() { continue; } - let slots = - &soft_constraints.slots[awake.slot_start..awake.slot_start + awake.num_particles]; + let slots = &slots_all[awake.slot_start..awake.slot_start + awake.num_particles]; + let dt = group_dt(awake.group as usize); let system = self.systems.entry(awake.handle).or_default(); system.prepare(sb, slots); + awake.fem = Some(AwakeFem { + system: &mut **system as *mut SoftFemSystem, + params: *params, + }); + self.fem_of_awake[ai] = self.active.len() as u32; self.active.push(ActiveFem { body: awake.ptr, system: &mut **system as *mut SoftFemSystem, - dt: group_dt(awake.group as usize), + dt, + params: *params, + first_slot: slots.first().copied().unwrap_or(u32::MAX), + num_particles: awake.num_particles as u32, }); } } - /// The implicit elastic step of `active[index]` (one substep). - /// # Safety - /// The caller must guarantee exclusive access to this index (one claiming worker per index - /// per stage) and that the body's solver-body slots are not written concurrently. - pub unsafe fn predict( - &self, - index: usize, - bodies: &mut SolverBodies, - params: &SoftFemParameters, - ) { - let entry = &self.active[index]; - let system = unsafe { &mut *entry.system }; - system.predict( - bodies, - entry.dt, - params.linear_tolerance, - params.max_linear_iterations, - ); + /// The index into `active` of an awake soft body, if it is on the FEM path. + #[inline] + fn fem_of(&self, awake: u32) -> Option { + if awake == u32::MAX { + return None; + } + let idx = self.fem_of_awake[awake as usize]; + (idx != u32::MAX).then_some(idx as usize) } - /// Records `active[index]`'s velocities at the start of a solve pass. - /// + /// Plans the responses of the step's constraints on the FEM bodies, once all are assembled: + /// picks which constraint sides a FEM body answers, applies the lever freezes, reserves the + /// ranges in `soft.fem_responses` and records the sides [`Self::compute_responses`] fills. + pub fn plan_responses(&mut self, soft: &mut SoftConstraintsSet) { + soft.fem_responses.clear(); + for refs in &mut self.refs { + refs.clear(); + } + self.refs.resize_with(self.active.len(), Vec::new); + if self.active.is_empty() { + return; + } + let SoftConstraintsSet { + awake, + contacts, + attachments, + shape_constraints, + volume_constraints, + overlap_constraints, + overlap_grads, + overlap_warm_impulses, + overlap_fem_sides, + fem_responses, + .. + } = soft; + let mut pool_len = 0u32; + let mut reserve = |count: usize| -> u32 { + let start = pool_len; + pool_len += count as u32; + start + }; + + for (constraint, c) in contacts.iter_mut().enumerate() { + c.fem = [None, None]; + let mut support = self.fem_of(c.support_body); + let mut other = self.fem_of(c.other_body); + if support.is_none() && other.is_none() { + continue; + } + // A side acting through pinned particles but for a sliver of weight on a free one is a + // lever (the constraint would fling the body to move its point): frozen for the constraint, as + // the lumped path does (`SoftContact::update`). + if support.is_some() && is_lever(&c.weights, &c.im_particles) { + c.im_particles = [0.0; CONTACT_ANCHORS]; + support = None; + } + if let (Some(e), true) = (&mut c.element, other.is_some()) { + if is_lever(&e.weights, &e.im_particles) { + e.im_particles = [0.0; CONTACT_ANCHORS]; + other = None; + } + } + // A self contact (two points of one body): one combined response + // `A⁻¹(J_support - J_other)ᵀ`, applied once; separate sides would lose the + // `J_s A⁻¹ J_oᵀ` coupling, as `Multibody::fill_relative_jacobians` explains. + let same_body = support.is_some() && support == other; + for (side, fi) in [(0usize, support), (1, other)] { + let Some(fi) = fi else { + continue; + }; + if side == 1 && same_body { + continue; + } + let entry = &self.active[fi]; + let n = entry.num_particles as usize; + c.fem[side] = Some(FemContactSide { + first_slot: entry.first_slot, + num_particles: entry.num_particles, + start: reserve(DIM * n), + gains: [0.0; DIM], + covers_other: side == 0 && same_body, + }); + self.refs[fi].push(FemConstraintRef::Contact { + constraint: constraint as u32, + side: side as u8, + }); + } + } + + // Attachments and shape-matching constraints: one response per axis of the particle. + for (constraint, r) in attachments.iter_mut().enumerate() { + r.fem = None; + let Some(fi) = self.fem_of(r.soft_body) else { + continue; + }; + let entry = &self.active[fi]; + if r.particle < entry.first_slot || r.particle >= entry.first_slot + entry.num_particles { + continue; + } + r.fem = Some(FemAttachment { + first_slot: entry.first_slot, + num_particles: entry.num_particles, + start: reserve(DIM * entry.num_particles as usize), + inv_mass: Matrix::ZERO, + }); + self.refs[fi].push(FemConstraintRef::Attachment(constraint as u32)); + } + for (ai, body) in awake.iter().enumerate() { + let fem = self.fem_of(ai as u32); + for constraint in body.shape_constraints.clone() { + let r = &mut shape_constraints[constraint]; + r.fem = None; + let Some(fi) = fem else { + continue; + }; + let entry = &self.active[fi]; + if r.solver_id < entry.first_slot + || r.solver_id >= entry.first_slot + entry.num_particles + { + continue; + } + r.fem = Some(FemAttachment { + first_slot: entry.first_slot, + num_particles: entry.num_particles, + start: reserve(DIM * entry.num_particles as usize), + inv_mass: Matrix::ZERO, + }); + self.refs[fi].push(FemConstraintRef::Shape(constraint as u32)); + } + } + + // Volume-piece constraints: the range is reserved here, the response updated with the + // gradients at each substep's prepare (`SoftConstraintsSet::prepare_awake_body`). + for constraint in volume_constraints.iter_mut() { + constraint.fem = None; + let Some(fi) = self.fem_of(constraint.soft_body) else { + continue; + }; + let entry = &self.active[fi]; + constraint.fem = Some(FemVolumeSide { + start: reserve(entry.num_particles as usize), + gain: 0.0, + u_max: 0.0, + }); + } + + // Intersection-volume constraints: one response per FEM body of the patch (its gradients) + // plus the response to the warm impulses; the body's entries then leave the lumped loops + // (zero inverse mass), after taking the lumped gain (ill-conditioned response fallback). + overlap_fem_sides.clear(); + for (constraint_id, constraint) in overlap_constraints.iter_mut().enumerate() { + let sides_start = overlap_fem_sides.len() as u32; + let grads = &mut overlap_grads[constraint.grads.clone()]; + let warm_impulses = &overlap_warm_impulses[constraint.grads.clone()]; + for (fi, entry) in self.active.iter().enumerate() { + let in_body = |slot: u32| { + slot != u32::MAX + && slot >= entry.first_slot + && slot < entry.first_slot + entry.num_particles + }; + if !grads.iter().any(|g| in_body(g.0) && g.1 > 0.0) { + continue; + } + let n = entry.num_particles as usize; + let lumped: Real = grads + .iter() + .filter(|g| in_body(g.0)) + .map(|g| g.1 * g.2.length_squared()) + .sum(); + let warm_pending = constraint.warm_pending + && constraint.warm.is_some() + && grads + .iter() + .zip(warm_impulses) + .any(|(g, p)| in_body(g.0) && g.1 > 0.0 && *p != Vector::ZERO); + let start = reserve(n); + let warm = if warm_pending { reserve(n) } else { u32::MAX }; + for g in grads.iter_mut() { + if in_body(g.0) { + g.1 = 0.0; + } + } + self.refs[fi].push(FemConstraintRef::Overlap { + constraint: constraint_id as u32, + side: overlap_fem_sides.len() as u32, + }); + overlap_fem_sides.push(FemOverlapSide { + first_slot: entry.first_slot, + num_particles: entry.num_particles, + start, + warm, + gain: lumped, + u_max: 0.0, + }); + } + constraint.fem_sides = sides_start..overlap_fem_sides.len() as u32; + } + fem_responses.resize(pool_len as usize, Vector::ZERO); + } + + /// The response stage's work for `active[index]`: factorizes the body's step matrix and fills + /// the responses and gains of every constraint side planned by [`Self::plan_responses`]. /// # Safety - /// Same contract as [`Self::predict`]. - pub unsafe fn snapshot(&self, index: usize, bodies: &SolverBodies) { + /// One worker per index per stage; the body writes only its own system, ranges and side gains. + pub unsafe fn compute_responses(&self, index: usize, soft: *mut SoftConstraintsSet) { let entry = &self.active[index]; let system = unsafe { &mut *entry.system }; - system.snapshot(bodies); + let params = &entry.params; + system.factorize_step_matrix(entry.dt, params); + let soft = unsafe { &mut *soft }; + let n = entry.num_particles as usize; + let first = entry.first_slot; + let pool = soft.fem_responses.as_mut_ptr(); + // SAFETY: the ranges were reserved for this body alone (see `plan_responses`). + let range = |start: u32, len: usize| unsafe { + core::slice::from_raw_parts_mut(pool.add(start as usize), len) + }; + let in_body = |slot: u32| slot != u32::MAX && slot >= first && slot < first + n as u32; + + // The particles with contact, attachment or shape constraints: their axis responses are + // solved once (the columns) and every constraint on them combines those (the overlap constraints' + // patches and the volume constraints keep their own solves). + system.clear_columns(); + for r in &self.refs[index] { + match *r { + FemConstraintRef::Contact { constraint, side } => { + let c = &soft.contacts[constraint as usize]; + let Some(fem) = c.fem[side as usize] else { + continue; + }; + if side == 0 { + for k in 0..CONTACT_ANCHORS { + if c.particles[k] != u32::MAX && c.weights[k] != 0.0 { + system.load_particle(c.support_particle[k]); + } + } + } + if side == 1 || fem.covers_other { + match &c.element { + Some(e) => { + for k in 0..CONTACT_ANCHORS { + if in_body(e.particles[k]) && e.weights[k] != 0.0 { + system.load_particle(e.particles[k] - first); + } + } + } + None => { + if in_body(c.body) { + system.load_particle(c.body - first); + } + } + } + } + } + FemConstraintRef::Attachment(constraint) => { + let r = &soft.attachments[constraint as usize]; + if r.fem.is_some() { + system.load_particle(r.particle - first); + } + } + FemConstraintRef::Shape(constraint) => { + let r = &soft.shape_constraints[constraint as usize]; + if r.fem.is_some() { + system.load_particle(r.solver_id - first); + } + } + FemConstraintRef::Overlap { .. } => {} + } + } + system.compute_columns(params); + + for r in &self.refs[index] { + match *r { + FemConstraintRef::Contact { constraint, side } => { + let c = &mut soft.contacts[constraint as usize]; + let side = side as usize; + let Some(fem) = c.fem[side] else { + continue; + }; + // The anchors of each side with their barycentric weights, the other + // side's negated (the constraint pushes the support along `+d` and the other + // side along `-d`). + let mut anchors: [(u32, Real); 2 * CONTACT_ANCHORS] = + [(0, 0.0); 2 * CONTACT_ANCHORS]; + let mut num = 0; + let mut lumped = [0.0; DIM]; + let support_side = side == 0; + if support_side { + for k in 0..CONTACT_ANCHORS { + if c.particles[k] != u32::MAX && c.weights[k] != 0.0 { + anchors[num] = (c.support_particle[k], c.weights[k]); + num += 1; + let w = c.weights[k] * c.weights[k] * c.im_particles[k]; + lumped.iter_mut().for_each(|l| *l += w); + } + } + } + if !support_side || fem.covers_other { + match &c.element { + Some(e) => { + for k in 0..CONTACT_ANCHORS { + if in_body(e.particles[k]) && e.weights[k] != 0.0 { + anchors[num] = (e.particles[k] - first, -e.weights[k]); + num += 1; + let w = e.weights[k] * e.weights[k] * e.im_particles[k]; + lumped.iter_mut().for_each(|l| *l += w); + } + } + } + None => { + if in_body(c.body) { + let p = c.body - first; + anchors[num] = (p, -1.0); + num += 1; + // SAFETY: read-only access to the body's particles. + let im = unsafe { &*entry.body }.particles[p as usize].inv_mass; + lumped.iter_mut().for_each(|l| *l += im); + } + } + } + } + let anchors = &anchors[..num]; + let directions: [Vector; DIM] = core::array::from_fn(|k| { + if k == 0 { c.dir } else { c.tangents[k - 1] } + }); + let mut gains = [0.0; DIM]; + for (k, d) in directions.iter().enumerate() { + let d = *d; + let entries = anchors.iter().map(move |&(p, w)| (p, d * w)); + let out = range(fem.start + (k * n) as u32, n); + gains[k] = match system.response_from_columns(entries.clone(), out) { + Some(gain) => gain, + None => system.response_into(entries, out, params), + }; + } + if !well_conditioned(range(fem.start, DIM * n), &gains, n) { + // Ill-conditioned (`MAX_RESPONSE_AMPLIFICATION`): the lumped gain (never + // below the augmented one, `A ≥ M`) under-relaxes it; its impulses still + // spread through the true response, keeping its constraints consistent. + gains = lumped; + } + c.fem[side].as_mut().unwrap().gains = gains; + } + FemConstraintRef::Attachment(constraint) => { + let r = &mut soft.attachments[constraint as usize]; + let Some(fem) = r.fem.as_mut() else { + continue; + }; + let p = r.particle - first; + fem.inv_mass = particle_response(system, p, range(fem.start, DIM * n), params); + } + FemConstraintRef::Shape(constraint) => { + let r = &mut soft.shape_constraints[constraint as usize]; + let Some(fem) = r.fem.as_mut() else { + continue; + }; + let p = r.solver_id - first; + fem.inv_mass = particle_response(system, p, range(fem.start, DIM * n), params); + } + FemConstraintRef::Overlap { constraint, side } => { + let oc = &soft.overlap_constraints[constraint as usize]; + let grads = &soft.overlap_grads[oc.grads.clone()]; + let warm_impulses = &soft.overlap_warm_impulses[oc.grads.clone()]; + let sd = &mut soft.overlap_fem_sides[side as usize]; + let lumped = sd.gain; + let entries = grads + .iter() + .filter(|g| in_body(g.0)) + .map(|g| (g.0 - first, g.2)); + let out = range(sd.start, n); + let gain = system.response_into(entries, out, params); + let u_max = out.iter().map(|v| v.length()).fold(0.0, Real::max); + sd.u_max = u_max; + sd.gain = if u_max > MAX_RESPONSE_AMPLIFICATION * gain { + lumped + } else { + gain + }; + if sd.warm != u32::MAX { + let entries = grads + .iter() + .zip(warm_impulses) + .filter(|(g, p)| in_body(g.0) && **p != Vector::ZERO) + .map(|(g, p)| (g.0 - first, *p)); + system.response_into(entries, range(sd.warm, n), params); + } + } + } + } } - /// Propagates the impulses `active[index]` received during a solve pass through its - /// elasticity. - /// + /// The implicit elastic step of `active[index]` (one substep). /// # Safety - /// Same contract as [`Self::predict`]. - pub unsafe fn propagate( + /// The caller must guarantee exclusive access to this index (one claiming worker per index + /// per stage) and that the body's solver-body slots are not written concurrently. + pub unsafe fn predict( &self, index: usize, bodies: &mut SolverBodies, @@ -151,8 +561,9 @@ impl SoftFemSet { ) { let entry = &self.active[index]; let system = unsafe { &mut *entry.system }; - system.propagate( + system.predict( bodies, + entry.dt, params.linear_tolerance, params.max_linear_iterations, ); @@ -173,3 +584,62 @@ impl SoftFemSet { self.active.len() } } + +/// The responses of a particle's `DIM` axes (`A⁻¹ e_p ⊗ axis`), into `out` (`DIM` vectors per +/// particle of the body, axis-major), and their values at the particle: the block `(A⁻¹)_pp`, the +/// augmented inverse mass of a constraint acting on that particle alone. +fn particle_response( + system: &mut SoftFemSystem, + particle: u32, + out: &mut [Vector], + params: &SoftFemParameters, +) -> Matrix { + if let Some(block) = system.particle_block_from_columns(particle, out) { + return block; + } + let n = system.num_particles(); + let mut inv_mass = Matrix::ZERO; + for k in 0..DIM { + let mut axis = Vector::ZERO; + axis[k] = 1.0; + let u = &mut out[k * n..(k + 1) * n]; + system.response_into(core::iter::once((particle, axis)), u, params); + *inv_mass.col_mut(k) = u[particle as usize]; + } + // The block is symmetric up to the solve's accuracy: symmetrize it so the constraint's inverse + // stays consistent with the responses. + (inv_mass + inv_mass.transpose()) * 0.5 +} + +/// The lumped path's lever test (`SoftContact::update`): a side whose free-particle weight is +/// a sliver of the pinned ones' cannot be moved by the constraint. +fn is_lever(weights: &[Real; N], im: &[Real; N]) -> bool { + let mut w2 = 0.0; + let mut im_max: Real = 0.0; + for k in 0..N { + w2 += weights[k] * weights[k] * im[k]; + im_max = im_max.max(im[k]); + } + im_max > 0.0 && w2 < 0.04 * im_max +} + +/// Largest velocity anywhere in the body per unit of velocity correction at a constraint's point +/// that a response may produce; past it the constraint is a lever (its point sits next to pinned +/// particles, or its anchor weights nearly cancel) and the side falls back to the lumped anchors. +pub(crate) const MAX_RESPONSE_AMPLIFICATION: Real = 8.0; + +/// Whether every direction's response is usable: a positive gain, and the amplification +/// `max |u| / gain` within `MAX_RESPONSE_AMPLIFICATION`. +fn well_conditioned(responses: &[Vector], gains: &[Real; DIM], n: usize) -> bool { + for (k, gain) in gains.iter().enumerate() { + let u = &responses[k * n..(k + 1) * n]; + let u_max = u.iter().map(|v| v.length()).fold(0.0, Real::max); + if u_max == 0.0 { + continue; + } + if *gain <= 0.0 || u_max > MAX_RESPONSE_AMPLIFICATION * *gain { + return false; + } + } + true +} diff --git a/src/dynamics/solver/soft_fem/soft_fem_sparse.rs b/src/dynamics/solver/soft_fem/soft_fem_sparse.rs index f0801ab95..b48bd62b9 100644 --- a/src/dynamics/solver/soft_fem/soft_fem_sparse.rs +++ b/src/dynamics/solver/soft_fem/soft_fem_sparse.rs @@ -108,6 +108,16 @@ impl BlockMatrix { self.diagonal[row as usize] } + /// Calls `f(col, block)` for every stored block of row `row`. + #[inline] + pub fn for_each_block_of_row(&self, row: u32, mut f: impl FnMut(u32, &Matrix)) { + let start = self.row_offsets[row as usize] as usize; + let end = self.row_offsets[row as usize + 1] as usize; + for k in start..end { + f(self.cols[k], &self.blocks[k]); + } + } + pub fn clear_values(&mut self) { self.blocks.fill(Matrix::ZERO); } diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs index 9838e5e8e..66f6a722f 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs @@ -107,6 +107,8 @@ impl SoftFemSystem { self.assemble_springs(dt); #[cfg(feature = "dim3")] self.assemble_dihedrals(dt); + self.assemble_volume_cells(dt); + // The pinned particles' coupling to the free rows (`A_pq = A_qpᵀ`), then their Dirichlet // rows. self.pinned_coupling.clear(); @@ -141,6 +143,33 @@ impl SoftFemSystem { } } + /// Accumulates the volume elements' force and Gauss-Newton tangent. + fn assemble_volume_cells(&mut self, dt: Real) { + for element in &self.volume_cells { + if element.stiffness <= 0.0 { + continue; + } + let cell = &self.cells[element.cell as usize]; + let x: [Vector; MAX_CONSTRAINT_PARTICLES] = + core::array::from_fn(|k| self.position[cell.vertices[k] as usize]); + let c = SoftBody::cell_volume(x) - element.rest_volume; + let grad = SoftBody::cell_volume_gradients(x); + for k in 0..MAX_CONSTRAINT_PARTICLES { + self.force[cell.vertices[k] as usize] -= grad[k] * (element.stiffness * c); + } + let scale = element.stiffness * (dt * dt + dt * element.beta); + let blocks = &self.cell_blocks[element.cell as usize]; + for p in 0..MAX_CONSTRAINT_PARTICLES { + for q in 0..MAX_CONSTRAINT_PARTICLES { + let slot = blocks[p * MAX_CONSTRAINT_PARTICLES + q]; + if slot != u32::MAX { + self.matrix.blocks[slot as usize] += outer_product(grad[p], grad[q]) * scale; + } + } + } + } + } + /// Accumulates the distance elements' force and Gauss-Newton tangent. fn assemble_springs(&mut self, dt: Real) { for spring in &self.springs { diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs index af4be9bdb..404685ea9 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs @@ -39,6 +39,20 @@ pub(super) struct FemCell { pub(super) tangent_strain: StrainVector, } +/// One volume element of a FEM soft body with the [`SoftBodyCellModel::Volume`] cell model: +/// energy `½ k (V − V₀)²` over the cell's signed area/volume, Gauss-Newton tangent +/// `k ∇V ∇Vᵀ` (the indefinite `(V − V₀) ∂²V` term dropped, like the springs). Its stiffness +/// comes from the material's `volume_softness` the way the row path's cell-volume row does: +/// `k = ω² / w₀`, `w₀` the row's effective mass at rest. +#[derive(Copy, Clone, Debug)] +pub(super) struct FemVolumeCell { + /// Index of the cell in the body (its vertices and cached block slots). + pub(super) cell: u32, + pub(super) rest_volume: Real, + pub(super) stiffness: Real, + pub(super) beta: Real, +} + /// One distance element (a structural or bending edge) of a FEM soft body. /// /// Energy `½ k (|d| − L)²`, with the Gauss-Newton tangent `k ∇C ∇Cᵀ` (the indefinite `C ∂²C` @@ -103,5 +117,23 @@ pub(crate) struct SoftFemSystem { pub(super) rhs: Vec, /// Solution of the last predictor solve (the warm start of the next one). pub(super) delta: Vec, + /// `A` at the step's start (the substep length of the body's group): the operator every + /// constraint of the step sees the body through, kept for the PCG fallback. pub(super) step_matrix: BlockMatrix, + /// The PCG of `step_matrix` (its preconditioner), used when the direct factorization is + /// not available. + pub(super) step_cg: ConjugateGradient, + /// The direct (skyline Cholesky) factorization of `step_matrix`, for bodies up to + /// `SoftFemParameters::max_dense_dofs`; ordering and envelope persist with the pattern (`None` + /// after a rebuild), and `direct_valid` says whether this step's factorization succeeded. + pub(super) direct: Option, + pub(super) direct_valid: bool, + pub(super) response_rhs: Vec, + pub(super) response: Vec, + /// The column responses of the step (see [`Self::load_particle`]): per loaded particle, `DIM` + /// axis-major responses `A_step⁻¹ (e_p ⊗ axis)` of `num_particles` vectors each, plus every + /// particle's column index (`u32::MAX`: not loaded). + pub(super) columns: Vec, + pub(super) column_of: Vec, + pub(super) num_columns: usize, } diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs index 9b3456447..9b35195c5 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs @@ -46,6 +46,10 @@ impl SoftFemSystem { self.force.resize(n, Vector::ZERO); self.rhs.resize(n, Vector::ZERO); self.delta.resize(n, Vector::ZERO); + self.response_rhs.resize(n, Vector::ZERO); + self.response.resize(n, Vector::ZERO); + self.clear_columns(); + let (mu, lambda) = sb.material.lame_parameters(); let zeta = sb.material.elastic_damping_ratio; let elastic = matches!( @@ -84,6 +88,38 @@ impl SoftFemSystem { self.prepare_springs(sb); #[cfg(feature = "dim3")] self.prepare_dihedrals(sb); + self.prepare_volume_cells(sb); + } + + /// Updates the volume elements of a `SoftBodyCellModel::Volume` body: `volume_softness` + /// normalized by the cell-volume constraint's rest effective mass `Σ m⁻¹ |∇V|²`, matching the + /// constraint path's softness exactly. + fn prepare_volume_cells(&mut self, sb: &SoftBody) { + self.volume_cells.clear(); + if sb.cell_model != SoftBodyCellModel::Volume { + return; + } + let softness = sb.material.volume_softness; + for (ci, c) in sb.cells.iter().enumerate() { + if c.rest_volume == 0.0 { + continue; + } + let rest: [Vector; MAX_CONSTRAINT_PARTICLES] = + core::array::from_fn(|k| sb.particles[c.vertices[k] as usize].rest_position); + let grad = SoftBody::cell_volume_gradients(rest); + let w: Real = (0..MAX_CONSTRAINT_PARTICLES) + .map(|k| sb.particles[c.vertices[k] as usize].inv_mass * grad[k].length_squared()) + .sum(); + let (stiffness, beta) = Self::spring_stiffness(&softness, w); + self.volume_cells.push(FemVolumeCell { + cell: ci as u32, + rest_volume: c.rest_volume, + stiffness, + beta, + }); + } + } + /// Rebuilds the sparsity pattern from the element graph and caches every element's block /// slots. fn rebuild_pattern(&mut self, sb: &SoftBody) { diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_response.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_response.rs new file mode 100644 index 000000000..f05d8c52b --- /dev/null +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_response.rs @@ -0,0 +1,203 @@ +//! The step matrix: its factorization at the step start, and the response and column solves the constraints go through. + +use super::SoftFemSystem; +use crate::dynamics::SoftFemParameters; +use super::super::soft_fem_skyline::SkylineCholesky; +use crate::math::{DIM, Matrix, Real, Vector}; + +impl SoftFemSystem { + /// Assembles `A` at the step's start and factorizes it: the operator the constraints + /// answer through for the whole step (see [`Self::response`]). A direct skyline Cholesky + /// up to `max_dense_dofs` degrees of freedom, the block-Jacobi PCG on the kept matrix above. + pub fn factorize_step_matrix(&mut self, dt: Real, params: &SoftFemParameters) { + // The same operator as the first substep's predict (same positions): kept for it. + self.assemble_operator(dt); + self.operator_fresh = true; + self.step_matrix.clone_from(&self.matrix); + let dofs = self.matrix.num_rows() * DIM; + self.direct_valid = false; + if dofs <= params.max_dense_dofs { + let direct = self + .direct + .get_or_insert_with(|| SkylineCholesky::new(&self.step_matrix)); + self.direct_valid = direct.factorize(&self.step_matrix); + } + if !self.direct_valid { + self.step_cg.update_preconditioner(&self.step_matrix); + } + } + + /// Solves `A_step x = b`. + fn solve_step(&mut self, b: &[Vector], x: &mut [Vector], params: &SoftFemParameters) { + if let (true, Some(direct)) = (self.direct_valid, self.direct.as_mut()) { + direct.solve(b, x); + } else { + x.fill(Vector::ZERO); + self.step_cg.solve( + &self.step_matrix, + b, + x, + params.linear_tolerance, + params.max_linear_iterations, + ); + } + } + + /// The body's answer to a constraint: for its jacobian `entries` `(particle, gradient)` on + /// this body, solves `A_step u = Jᵀ` into `out` (one vector per particle, zero when pinned) + /// and returns the gain `J · u` (augmented inverse mass), like `Multibody::fill_jacobians`. + pub fn response_into( + &mut self, + entries: impl Iterator + Clone, + out: &mut [Vector], + params: &SoftFemParameters, + ) -> Real { + let n = self.num_particles(); + debug_assert_eq!(out.len(), n); + let mut rhs = core::mem::take(&mut self.response_rhs); + rhs.clear(); + rhs.resize(n, Vector::ZERO); + let mut any = false; + for (p, g) in entries.clone() { + if !self.pinned[p as usize] { + rhs[p as usize] += g; + any = true; + } + } + if !any { + out.fill(Vector::ZERO); + self.response_rhs = rhs; + return 0.0; + } + self.solve_step(&rhs, out, params); + let mut gain = 0.0; + for (p, g) in entries { + if !self.pinned[p as usize] { + gain += g.dot(out[p as usize]); + } + } + self.response_rhs = rhs; + gain.max(0.0) + } + + pub fn num_particles(&self) -> usize { + self.mass.len() + } + + /// Forgets the step's column responses. + pub fn clear_columns(&mut self) { + let n = self.num_particles(); + self.column_of.clear(); + self.column_of.resize(n, u32::MAX); + self.columns.clear(); + self.num_columns = 0; + } + + /// Marks `particle` as constrained this step: [`Self::compute_columns`] then solves its `DIM` + /// axis responses once, and its constraints combine them ([`Self::response_from_columns`]) + /// instead of solving on their own. A pinned particle answers nothing and is not loaded. + pub fn load_particle(&mut self, particle: u32) { + let p = particle as usize; + if !self.pinned[p] && self.column_of[p] == u32::MAX { + self.column_of[p] = self.num_columns as u32; + self.num_columns += 1; + } + } + + /// Solves the axis responses of every loaded particle. + pub fn compute_columns(&mut self, params: &SoftFemParameters) { + let n = self.num_particles(); + let mut columns = core::mem::take(&mut self.columns); + columns.clear(); + columns.resize(self.num_columns * DIM * n, Vector::ZERO); + for p in 0..n { + let column = self.column_of[p]; + if column == u32::MAX { + continue; + } + for k in 0..DIM { + let mut axis = Vector::ZERO; + axis[k] = 1.0; + let start = (column as usize * DIM + k) * n; + self.response_into( + core::iter::once((p as u32, axis)), + &mut columns[start..start + n], + params, + ); + } + } + self.columns = columns; + } + + /// The response of loaded `particle` to a unit impulse along axis `k`. + #[inline] + fn column(&self, particle: u32, k: usize) -> Option<&[Vector]> { + let column = self.column_of[particle as usize]; + if column == u32::MAX { + return None; + } + let n = self.num_particles(); + let start = (column as usize * DIM + k) * n; + Some(&self.columns[start..start + n]) + } + + /// [`Self::response_into`] from the column responses, `u = Σ C_p g_p` over the entries, with + /// no solve; `None` when an entry's particle was not loaded (the caller solves). + pub fn response_from_columns( + &self, + entries: impl Iterator + Clone, + out: &mut [Vector], + ) -> Option { + let n = self.num_particles(); + debug_assert_eq!(out.len(), n); + out.fill(Vector::ZERO); + let mut any = false; + for (p, g) in entries.clone() { + if self.pinned[p as usize] { + continue; + } + any = true; + for k in 0..DIM { + let weight = g[k]; + if weight == 0.0 { + continue; + } + let column = self.column(p, k)?; + for (o, c) in out.iter_mut().zip(column) { + *o += *c * weight; + } + } + } + if !any { + return Some(0.0); + } + let mut gain = 0.0; + for (p, g) in entries { + if !self.pinned[p as usize] { + gain += g.dot(out[p as usize]); + } + } + Some(gain.max(0.0)) + } + + /// The `DIM` axis responses of loaded `particle`, copied into `out` (axis-major), and their + /// values at the particle: the block `(A⁻¹)_pp`, the augmented inverse mass of a constraint + /// acting on the particle alone (an attachment). `None` when the particle was not loaded. + pub fn particle_block_from_columns(&self, particle: u32, out: &mut [Vector]) -> Option { + let n = self.num_particles(); + debug_assert_eq!(out.len(), DIM * n); + let mut inv_mass = Matrix::ZERO; + if self.pinned[particle as usize] { + out.fill(Vector::ZERO); + return Some(inv_mass); + } + for k in 0..DIM { + let column = self.column(particle, k)?; + out[k * n..(k + 1) * n].copy_from_slice(column); + *inv_mass.col_mut(k) = column[particle as usize]; + } + // The block is symmetric up to the solve's accuracy: symmetrize it so the constraint's + // inverse stays consistent with the responses. + Some((inv_mass + inv_mass.transpose()) * 0.5) + } +} diff --git a/src/dynamics/solver/soft_fem/system/tests.rs b/src/dynamics/solver/soft_fem/system/tests.rs new file mode 100644 index 000000000..f5f37b4cc --- /dev/null +++ b/src/dynamics/solver/soft_fem/system/tests.rs @@ -0,0 +1,99 @@ +//! Unit tests of the FEM system: the step-matrix responses and the column solves. + +use super::SoftFemSystem; +use crate::alloc_prelude::*; +use crate::dynamics::SoftFemParameters; +use crate::math::Vector; +use crate::dynamics::{SoftBodyBuilder, SoftBodyCellModel, SoftBodyMaterial}; + +/// A response solves `A_step u = Jᵀ` (checked against the matrix), is zero at the pinned +/// particles and has gain `J · u`; the dense and conjugate-gradient paths agree. +#[test] +fn response_solves_the_step_matrix() { + #[cfg(feature = "dim2")] + let builder = SoftBodyBuilder::grid(Vector::ZERO, Vector::splat(0.5), 4, 4); + #[cfg(feature = "dim3")] + let builder = SoftBodyBuilder::cuboid(Vector::ZERO, Vector::splat(0.5), 3, 3, 3); + let builder = builder + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e5, + poisson_ratio: 0.3, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .pinned_particles(vec![0]); + let mut set = crate::dynamics::SoftBodySet::new(); + let mut bodies = crate::dynamics::RigidBodySet::new(); + let mut colliders = crate::geometry::ColliderSet::new(); + let handle = set.insert(builder, &mut bodies, &mut colliders); + let sb = &set[handle]; + let n = sb.particles().len(); + let slots: Vec = (0..n as u32).collect(); + let dt = 1.0 / 240.0; + let entries = [(1u32, Vector::ONE), (0u32, Vector::ONE)]; + let mut results = Vec::new(); + for max_dense_dofs in [usize::MAX, 0] { + let params = SoftFemParameters { + max_dense_dofs, + linear_tolerance: 1.0e-8, + max_linear_iterations: 10_000, + ..Default::default() + }; + let mut system = SoftFemSystem::default(); + system.prepare(sb, &slots); + system.factorize_step_matrix(dt, ¶ms); + let mut pool = vec![Vector::ZERO; n]; + let gain = system.response_into(entries.iter().copied(), &mut pool, ¶ms); + assert_eq!(pool[0], Vector::ZERO, "pinned particle answered"); + let mut lhs = vec![Vector::ZERO; n]; + system.step_matrix.mul(&pool, &mut lhs); + for (k, l) in lhs.iter().enumerate() { + let rhs = if k == 1 { Vector::ONE } else { Vector::ZERO }; + assert!((*l - rhs).length() < 1.0e-3 * (1.0 + rhs.length()), "row {k}: {l:?} vs {rhs:?}"); + } + assert!((gain - Vector::ONE.dot(pool[1])).abs() < 1.0e-6); + results.push((gain, pool)); + } + // The column responses reproduce the direct solve. + { + let params = SoftFemParameters { + linear_tolerance: 1.0e-8, + max_linear_iterations: 10_000, + ..Default::default() + }; + let mut system = SoftFemSystem::default(); + system.prepare(sb, &slots); + system.factorize_step_matrix(dt, ¶ms); + #[cfg(feature = "dim2")] + let entries = [(1u32, Vector::new(0.3, -0.7)), (2u32, Vector::ONE * 0.5)]; + #[cfg(feature = "dim3")] + let entries = [(1u32, Vector::new(0.3, -0.7, 0.2)), (2u32, Vector::ONE * 0.5)]; + let mut direct = vec![Vector::ZERO; n]; + let gain = system.response_into(entries.iter().copied(), &mut direct, ¶ms); + system.clear_columns(); + system.load_particle(1); + system.load_particle(2); + system.compute_columns(¶ms); + let mut combined = vec![Vector::ZERO; n]; + let gain_columns = system + .response_from_columns(entries.iter().copied(), &mut combined) + .unwrap(); + assert!((gain - gain_columns).abs() < 1.0e-5 * gain.max(1.0e-6)); + for (a, b) in direct.iter().zip(&combined) { + assert!((*a - *b).length() < 1.0e-5 * (1.0 + a.length())); + } + assert!( + system + .response_from_columns([(3u32, Vector::ONE)].into_iter(), &mut combined) + .is_none(), + "an unloaded particle must fall back to a solve" + ); + } + let (dense, cg) = (&results[0], &results[1]); + assert!((dense.0 - cg.0).abs() < 1.0e-3 * dense.0, "gains {} vs {}", dense.0, cg.0); + for (a, b) in dense.1.iter().zip(&cg.1) { + assert!((*a - *b).length() < 1.0e-3 * (1.0 + a.length())); + } +} diff --git a/src/dynamics/solver/staged_island_solver/init.rs b/src/dynamics/solver/staged_island_solver/init.rs index d432c4fd0..a64ab9eca 100644 --- a/src/dynamics/solver/staged_island_solver/init.rs +++ b/src/dynamics/solver/staged_island_solver/init.rs @@ -168,10 +168,13 @@ impl StagedIslandSolver { #[cfg(feature = "fem")] { let groups = &self.groups; - self.soft_fem - .assemble(&self.soft_constraints, soft_bodies, groups.len(), |gi| { - groups[gi].dt - }); + self.soft_fem.assemble( + &mut self.soft_constraints, + soft_bodies, + groups.len(), + |gi| groups[gi].dt, + ¶ms.soft_bodies.fem, + ); } // The persistent solver contact graph already holds two-body manifolds grouped by @@ -231,13 +234,26 @@ impl StagedIslandSolver { let vs = &self.velocity_solver; self.soft_constraints .assemble_clusters(bodies, colliders, &vs.solver_bodies, &jointed_proxies); + + // The FEM bodies' answers to the step's constraints, once every constraint exists: planned here, + // computed by the workers' first stage. + #[cfg(feature = "fem")] + self.soft_fem.plan_responses(&mut self.soft_constraints); } - // Avoid spawning more workers than there is work to distribute. + // Avoid spawning more workers than there is work: soft constraints count like element + // constraints (their chunks are the contact stages' claims), and each FEM body is a + // factorization and solves of its own (the response stage claims one body per worker). let num_two_body = graph.len() - graph.generic().len(); + #[cfg(feature = "fem")] + let fem_chunks = self.soft_fem.num_active() * 16; + #[cfg(not(feature = "fem"))] + let fem_chunks = 0; + let soft = &self.soft_constraints; let approx_chunks = num_two_body / SIMD_WIDTH + joint_indices.len() / 4 - + (self.soft_constraints.rows.len() + self.soft_constraints.shape_rows.len()) / 8; + + (soft.scalar_constraints.len() + soft.shape_constraints.len() + soft.contacts.len()) / 8 + + fem_chunks; let num_workers = num_workers.clamp(1, (approx_chunks / 16).max(1)); // Joints: colored like contacts and laid out color by color (scalar diff --git a/src/dynamics/solver/staged_island_solver/solve.rs b/src/dynamics/solver/staged_island_solver/solve.rs index 10f1cef68..b38aa1e57 100644 --- a/src/dynamics/solver/staged_island_solver/solve.rs +++ b/src/dynamics/solver/staged_island_solver/solve.rs @@ -446,6 +446,7 @@ unsafe fn solve_soft_constraints( let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; let claimed_len = claimed.len(); for constraint_id in claimed { + soft.solve_shape_constraint(constraint_id, solver_bodies, soft_update, soft_warmstart); } done += claimed_len; } @@ -458,6 +459,7 @@ unsafe fn solve_soft_constraints( let soft = unsafe { &mut *ctx.soft_constraints }; let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; for constraint_id in sg.shape_constraints.clone() { + soft.solve_shape_constraint(constraint_id, solver_bodies, soft_update, soft_warmstart); } sync.complete(stage, 1, 1); } diff --git a/src/dynamics/solver/staged_island_solver/worker.rs b/src/dynamics/solver/staged_island_solver/worker.rs index 944fe63c2..4b146a0ea 100644 --- a/src/dynamics/solver/staged_island_solver/worker.rs +++ b/src/dynamics/solver/staged_island_solver/worker.rs @@ -210,6 +210,31 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { // takes the same branches. #[cfg(feature = "fem")] let has_fem = !unsafe { &*ctx.soft_fem }.is_empty(); + + /* + * Stage: the FEM bodies' step factorization and the responses of the constraints acting on + * them (parallel over the FEM bodies, each writing its own system, response ranges and + * constraint-side gains). Runs before the substeps, whose constraint updates read the gains. + */ + #[cfg(feature = "fem")] + if has_fem { + let soft_fem = unsafe { &*ctx.soft_fem }; + let all_bodies = 0..soft_fem.num_active(); + let stage_work = all_bodies.len(); + let mut done = 0; + while let Some(claimed) = sync.claim(stage, &all_bodies, 1, worker_id) { + let claimed_len = claimed.len(); + for index in claimed { + // SAFETY: one worker per claimed index; the body writes only its own system, + // pool ranges and constraint sides. + unsafe { soft_fem.compute_responses(index, ctx.soft_constraints) }; + } + done += claimed_len; + } + sync.complete(stage, done, stage_work); + stage = sync.sync(stage, stage_work); + } + for (group_index, group) in ctx.groups.iter().enumerate() { // This group's substep parameters: identical to `base_params` except // for the substep dt. Everything dt-derived downstream (soft erp/cfm, From 8fbe6a21f74a61f1720d88a383120ba63aa2e344 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 10 Sep 2026 17:12:40 +0200 Subject: [PATCH 12/32] feat(soft-body): tear by force with a smoothed load, per-element resistance, interior strength and damage propagation --- crates/rapier2d/tests/soft_bodies.rs | 151 ++++++++++++++++++ .../soft_body_builder/soft_body_build.rs | 22 +++ .../soft_body_builder/soft_body_builder.rs | 3 + .../soft_body_builder_settings.rs | 41 +++++ .../soft_body_builder_shapes.rs | 1 + src/dynamics/soft_body/soft_body_cluster.rs | 29 ++++ src/dynamics/soft_body/soft_body_elements.rs | 55 +++++++ src/dynamics/soft_body/soft_body_material.rs | 64 ++++++++ src/dynamics/soft_body/soft_body_meshes.rs | 14 ++ .../soft_constraint_solve.rs | 2 + .../soft_constraint_writeback.rs | 33 ++++ .../soft_constraints_set.rs | 3 + .../soft_scalar_constraint.rs | 3 + .../soft_element_constraint/test.rs | 1 + .../soft_element_constraint_assembly.rs | 3 + .../system/soft_fem_system_assemble.rs | 45 ++++++ 16 files changed, 470 insertions(+) diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index 71576a4ce..b5ec242c7 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -1809,3 +1809,154 @@ fn self_contact_blob_survives_grab_crush() { assert!(r > 0.6, "a blob stayed crushed after release: ratio {r}"); } } + +} + +/// A stiff edge holding a weight barely stretches, yet it bears the weight: with a tear force +/// below that weight it breaks, with one above it holds, and its `stress` reads the fraction of +/// the threshold it bears. +#[test] +fn stiff_edge_tears_under_force_not_strain() { + let hang = |tear_force: Real| { + let mut world = PhysicsWorld::new(); + let handle = world.insert_soft_body(hanging_weight(tear_force)); + for _ in 0..60 { + world.step(); + } + let sb = &world.soft_bodies[handle]; + let tested = sb.edges().iter().find(|e| e.vertices == [3, 4]); + let particles: usize = bodies.iter().map(|&h| world.soft_bodies[h].num_particles()).sum(); + ( + bodies.len(), + tested.map(|e| e.stress()), + sb.edges().len(), + ) + }; + // The load is the weight, 9.81 N. + let (particles, pieces, stress, edges) = hang(30.0); + assert_eq!((particles, pieces, edges), (8, 1, 7), "the edge tore under a third of its threshold"); + let stress = stress.unwrap(); + assert!( + (stress - 9.81 / 30.0).abs() < 0.08, + "stress {stress} does not read the weight over the threshold" + ); + let (particles, pieces, stress, edges) = hang(6.0); + assert_eq!((particles, pieces, edges), (9, 2, 4), "the edge held a load above its tear force"); +} + +/// Tear smoothing: a jerk that would snap an edge at once is shrugged off once the load is +/// averaged over a time constant far longer than the spike. +#[test] +fn tear_smoothing_shrugs_off_a_spike() { + let jerk = |smoothing: Real| { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let handle = world.insert_soft_body(hanging_weight(50.0).tear_smoothing(smoothing)); + world.step(); + // A velocity the edge must cancel within a substep: a force spike of hundreds of N. + for _ in 0..30 { + world.step(); + } + // A tear splits a particle (it removes no edge), and the weight falls as its own body. + family(&world, handle).len() + }; + assert_eq!(jerk(0.0), 2, "without smoothing the spike snaps the edge"); + assert_eq!(jerk(1.0), 1, "smoothed over a second, the spike is shrugged off"); +} + +/// A per-edge tear resistance multiplies the edge's threshold: of two edges bearing the same +/// weight past the material's tear force, the reinforced one holds (above the 14 N catch +/// transient of a released chain). Each 1 kg weight is a reinforced chain of three segments. +#[test] +fn tear_resistance_scales_the_threshold() { + let mut world = PhysicsWorld::new(); + let mut positions = vec![Vector::new(0.0, 2.0), Vector::new(1.0, 2.0)]; + for k in 0..4 { + let y = 1.0 - k as Real / 3.0; + positions.extend([Vector::new(0.0, y), Vector::new(1.0, y)]); + } + let builder = SoftBodyBuilder::new(positions) + .edges(vec![[0, 1], [0, 2], [1, 3], [2, 4], [3, 5], [4, 6], [5, 7], [6, 8], [7, 9]]) + .edge_tear_resistance([(2, 3.0), (3, 100.0), (4, 100.0), (5, 100.0), (6, 100.0), (7, 100.0), (8, 100.0)]) + .pinned_particles([0, 1]) + .particle_mass(0.25) + .softness(SpringCoefficients::new(120.0, 1.0)) + .tear_force(6.0) + .no_surface_collider() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + for _ in 0..60 { + world.step(); + } + let sb = &world.soft_bodies[handle]; + let reinforced = sb.edges().iter().find(|e| e.tear_resistance == 3.0); + let stress = reinforced.expect("the reinforced edge tore").stress(); + assert!((stress - 9.81 / 18.0).abs() < 0.08, "stress {stress} ignores the resistance"); +} + +/// An edge whose particles are all interior and undamaged bears its load against +/// `interior_strength` times the threshold, a surface edge against the threshold itself, and a +/// tear through a particle removes the shield. Checked on two worlds differing by that alone. +#[test] +fn interior_strength_shields_undamaged_interior_edges() { + let n = 5usize; + let idx = |i: usize, j: usize| (i * n + j) as u32; + let run = |interior_strength: Real| { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let grid = SoftBodyBuilder::grid(Vector::ZERO, Vector::splat(1.0), n, n) + .material(SoftBodyMaterial { + tear_strain: Some(1.0), + interior_strength, + ..Default::default() + }) + .no_surface_collider() + .can_sleep(false); + let handle = world.insert_soft_body(grid); + let sb = &mut world.soft_bodies[handle]; + for i in 0..sb.num_particles() { + let p = sb.particle_position(i); + sb.set_particle_position(i, p * 1.3); + } + world.step(); + let find = |sb: &SoftBody, a: u32, b: u32| { + sb.edges() + .iter() + .position(|e| { + (e.vertices == [a, b] || e.vertices == [b, a]) + && e.kind == SoftBodyEdgeKind::Structural + }) + .unwrap() + }; + let sb = &world.soft_bodies[handle]; + assert_eq!( + sb.particles().iter().filter(|p| p.is_on_surface()).count(), + 4 * (n - 1), + "the boundary ring is the surface" + ); + let stress = |sb: &SoftBody, a, b| sb.edges()[find(sb, a, b)].stress(); + let surface = stress(sb, idx(0, 0), idx(0, 1)); + let inner = stress(sb, idx(1, 1), idx(1, 2)); + // Tear the inner edge: its particles (and the cells' around it) become damaged. + let torn = find(sb, idx(1, 1), idx(1, 2)); + world.soft_bodies[handle].tear_edge(torn); + // The tear applies at the end of the step; the next step's loads see the damage. + world.step(); + world.step(); + let sb = &world.soft_bodies[handle]; + assert!(sb.particles()[idx(1, 1) as usize].is_damaged()); + assert!(sb.particles()[idx(1, 2) as usize].is_damaged()); + assert!(!sb.particles()[idx(3, 1) as usize].is_damaged()); + let next_to_damage = stress(sb, idx(1, 2), idx(1, 3)); + let far = stress(sb, idx(3, 1), idx(3, 2)); + (surface, inner, next_to_damage, far) + }; + let plain = run(1.0); + let tough = run(4.0); + assert!(plain.0 > 0.05 && plain.1 > 0.02, "the stretched grid reads a load: {plain:?}"); + let close = |a: Real, b: Real| (a - b).abs() <= 1.0e-5 * a.abs().max(1.0); + assert!(close(plain.0, tough.0), "a surface edge is not shielded"); + assert!(close(plain.1, 4.0 * tough.1), "an interior edge bears 4x the threshold"); + assert!(close(plain.2, tough.2), "an edge next to damage lost its shield"); + assert!(close(plain.3, 4.0 * tough.3), "an edge far from damage keeps its shield"); +} 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 4f024bdcc..b1b769257 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 @@ -64,6 +64,8 @@ impl SoftBodyBuilder { inv_mass: if *pinned { 0.0 } else { crate::utils::inv(*m) }, force: Vector::ZERO, next_position: None, + damaged: false, + on_surface: false, }) .collect(); let pos = |i: u32| self.positions[i as usize]; @@ -92,10 +94,13 @@ impl SoftBodyBuilder { impulses: [0.0; super::super::CELL_IMPULSES], rotation: Rotation::IDENTITY, stiffness_scale: 1.0, + tear_resistance: 1.0, color: 0, torn: false, + stress: 0.0, }); } + // Surface: given, or the boundary of the cells. let surface = if !self.surface.is_empty() { self.surface.clone() @@ -141,15 +146,23 @@ impl SoftBodyBuilder { kind: *kind, tension_only: false, softness: None, + tear_resistance: 1.0, impulse: 0.0, color: 0, torn: false, + stress: 0.0, + }) .collect(); for &i in &self.tension_only_edges { if let Some(e) = edges.get_mut(i as usize) { e.tension_only = true; } } + for &(i, resistance) in &self.edge_tear_resistance { + if let Some(e) = edges.get_mut(i as usize) { + e.tear_resistance = resistance.max(0.0); + } + } for &(i, softness) in &self.edge_softness { if let Some(e) = edges.get_mut(i as usize) { e.softness = Some(softness); @@ -179,6 +192,15 @@ impl SoftBodyBuilder { let rest_volume = SoftBody::boundary_volume(&surface, pos); let boundary_closed = surface_is_closed(&surface); let boundary_element_cells = surface_element_cells(&surface, &cells); + let mut particles = particles; + for element in &surface { + for &v in element { + if let Some(p) = particles.get_mut(v as usize) { + p.on_surface = true; + } + } + } + SoftBody { volume_preservation: self.volume_preservation && !surface.is_empty(), boundary_inverted: false, diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs index 415ca2497..5e412d593 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs @@ -61,6 +61,9 @@ pub struct SoftBodyBuilder { /// Per-edge softness overrides (indices into the concatenation of `edges` then /// `bend_edges`); the material's softness applies to the others. pub edge_softness: Vec<(u32, SpringCoefficients)>, + /// Per-edge tear-threshold multipliers (indices into the concatenation of `edges` then + /// `bend_edges`, see [`crate::dynamics::SoftBodyEdge::tear_resistance`]); `1.0` for the others. + pub edge_tear_resistance: Vec<(u32, Real)>, /// Dihedral bending constraints: shared edge then the two opposite vertices. #[cfg(feature = "dim3")] pub dihedrals: Vec<[u32; 4]>, diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs index a70d2bbbb..6d48dc04d 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs @@ -79,6 +79,11 @@ impl SoftBodyBuilder { *i += shift; } } + for (i, _) in &mut self.edge_tear_resistance { + if *i >= first_bend { + *i += shift; + } + } self.edges.extend(inserted); self } @@ -119,6 +124,9 @@ impl SoftBodyBuilder { for (i, _) in &mut self.edge_softness { *i = remap_self(*i); } + for (i, _) in &mut self.edge_tear_resistance { + *i = remap_self(*i); + } self.tension_only_edges .extend(other.tension_only_edges.iter().map(|&i| remap_other(i))); self.edge_softness.extend( @@ -127,6 +135,12 @@ impl SoftBodyBuilder { .iter() .map(|&(i, c)| (remap_other(i), c)), ); + self.edge_tear_resistance.extend( + other + .edge_tear_resistance + .iter() + .map(|&(i, r)| (remap_other(i), r)), + ); let _ = other_bends; let shift = |v: &[u32]| -> Vec { v.iter().map(|&i| i + offset).collect() }; self.positions.extend_from_slice(&other.positions); @@ -246,6 +260,33 @@ impl SoftBodyBuilder { self } + /// Sets the material's tear force (see [`SoftBodyMaterial::tear_force`]). + pub fn tear_force(mut self, force: Real) -> Self { + self.material.tear_force = Some(force); + self + } + + /// Sets the material's tear smoothing time constant (see + /// [`SoftBodyMaterial::tear_smoothing`]). + pub fn tear_smoothing(mut self, seconds: Real) -> Self { + self.material.tear_smoothing = seconds; + self + } + + /// Sets the material's interior strength (see [`SoftBodyMaterial::interior_strength`]). + pub fn interior_strength(mut self, strength: Real) -> Self { + self.material.interior_strength = strength; + self + } + + /// Sets the tear-threshold multiplier of some edges, as `(edge index, resistance)` pairs + /// indexing the concatenation of the structural then the bending edges (see + /// [`crate::dynamics::SoftBodyEdge::tear_resistance`]). + pub fn edge_tear_resistance(mut self, edges: impl IntoIterator) -> Self { + self.edge_tear_resistance.extend(edges); + self + } + /// Sets the constitutive model of the cells. pub fn cell_model(mut self, model: SoftBodyCellModel) -> Self { self.cell_model = model; diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs index 094a3aab6..81baa8e4f 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs @@ -29,6 +29,7 @@ impl SoftBodyBuilder { bend_edges: Vec::new(), tension_only_edges: Vec::new(), edge_softness: Vec::new(), + edge_tear_resistance: Vec::new(), #[cfg(feature = "dim3")] dihedrals: Vec::new(), cells: Vec::new(), diff --git a/src/dynamics/soft_body/soft_body_cluster.rs b/src/dynamics/soft_body/soft_body_cluster.rs index 8b1b9a73f..3ef3e5dfb 100644 --- a/src/dynamics/soft_body/soft_body_cluster.rs +++ b/src/dynamics/soft_body/soft_body_cluster.rs @@ -251,6 +251,34 @@ impl SoftBody { self.modified = true; } + /// Sets the tear-threshold multiplier of every edge and cell fully contained in the `i`-th + /// cluster (see [`super::SoftBodyEdge::tear_resistance`]): a tough region, or a perforation + /// line; `1.0` restores the material's thresholds. + pub fn set_cluster_tear_resistance(&mut self, i: u32, resistance: Real) { + let Some(cluster) = self.clusters.get(i as usize).filter(|c| c.is_live()) else { + return; + }; + let resistance = resistance.max(0.0); + let inside = |vertices: &[u32]| { + vertices + .iter() + .all(|v| cluster.particles.binary_search(v).is_ok()) + }; + let edges: Vec = self.edges.iter().map(|e| inside(&e.vertices)).collect(); + let cells: Vec = self.cells.iter().map(|c| inside(&c.vertices)).collect(); + for (e, inside) in self.edges.iter_mut().zip(edges) { + if inside { + e.tear_resistance = resistance; + } + } + for (c, inside) in self.cells.iter_mut().zip(cells) { + if inside { + c.tear_resistance = resistance; + } + } + self.modified = true; + } + /// Pins (or releases) every particle of the `i`-th cluster at once: the cluster becomes a /// fixed (kinematic) region dragging the rest of the body, driven along a path with /// [`Self::set_cluster_kinematic_target`]; particles keep their individual pinned semantics. @@ -475,6 +503,7 @@ impl SoftBody { }); self.boundary_closed = super::soft_body_builder::surface_is_closed(&self.boundary); + self.update_surface_flags(); self.rebuild_mesh_tables(); self.update_cluster_cells(); self.recolor(); diff --git a/src/dynamics/soft_body/soft_body_elements.rs b/src/dynamics/soft_body/soft_body_elements.rs index ee53ea6ae..5710ae0c8 100644 --- a/src/dynamics/soft_body/soft_body_elements.rs +++ b/src/dynamics/soft_body/soft_body_elements.rs @@ -35,7 +35,14 @@ pub struct SoftBodyParticle { /// its velocity). #[cfg_attr(feature = "serde-serialize", serde(default))] pub(crate) next_position: Option, + /// Set once an element touching the particle tore (see [`Self::is_damaged`]). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) damaged: bool, + /// Whether some surface element has this particle as a vertex (updated with the boundary). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) on_surface: bool, } + impl SoftBodyParticle { /// The world-space position of this particle. pub fn position(&self) -> Vector { @@ -81,7 +88,20 @@ impl SoftBodyParticle { } /// Whether an element touching this particle has torn (or [`crate::dynamics::SoftBody::set_particle_damaged`] + /// marked it). The elements around a damaged particle lose the interior strength of + /// [`SoftBodyMaterial::interior_strength`]: a tear runs on from where it started. + pub fn is_damaged(&self) -> bool { + self.damaged + } + + /// Whether this particle is a vertex of the body's surface (a boundary segment in 2D, a + /// boundary triangle in 3D). An element whose particles are all interior gets the material's + /// [`SoftBodyMaterial::interior_strength`]. + pub fn is_on_surface(&self) -> bool { + self.on_surface + } } + /// The role of a soft-body edge (distance constraint). #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -109,6 +129,11 @@ pub struct SoftBodyEdge { /// Softness of this edge, overriding the material's (`edge_softness` / `bend_softness`) /// when set: anisotropic cloth (warp, weft and shear stiffness), stiffer seams... pub softness: Option>, + /// Multiplier of the material's tear thresholds for this edge (default `1.0`): a seam that + /// holds, a perforation that gives. Usually set through [`crate::dynamics::SoftBody::set_edge_tear_resistance`] + /// or [`crate::dynamics::SoftBody::set_cluster_tear_resistance`]. + #[cfg_attr(feature = "serde-serialize", serde(default = "one"))] + pub tear_resistance: Real, /// Accumulated impulse of the last step (warm-start state). pub(crate) impulse: Real, /// Parallel solve color. @@ -116,7 +141,12 @@ pub struct SoftBodyEdge { /// (or by [`crate::dynamics::SoftBody::tear_edge`]): removed by the tearing pass at the end of the step. #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) torn: bool, + /// The load of the edge as a fraction of its tear threshold, smoothed over the material's + /// `tear_smoothing` (see [`Self::stress`]). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) stress: Real, } + impl SoftBodyEdge { /// The impulse this edge applied during the last substep, along the edge direction /// (positive when the edge was resisting stretching). @@ -124,6 +154,14 @@ impl SoftBodyEdge { self.impulse } + /// The load this edge carries as a fraction of its tear threshold, smoothed over the + /// material's [`SoftBodyMaterial::tear_smoothing`]: `0.0` slack, `1.0` tearing. The larger of + /// the stretch over [`SoftBodyMaterial::tear_strain`] and the force over + /// [`SoftBodyMaterial::tear_force`]; stays `0.0` while the material has neither. + pub fn stress(&self) -> Real { + self.stress + } + } /// A dihedral bending constraint between two triangles sharing an edge (3D only). /// @@ -206,10 +244,27 @@ pub struct SoftBodyCell { /// stiffness, usually set through [`crate::dynamics::SoftBody::set_cluster_stiffness_scale`]. #[cfg_attr(feature = "serde-serialize", serde(default = "one"))] pub stiffness_scale: Real, + /// Multiplier of the material's tear strain for this cell (default `1.0`), see + /// [`crate::dynamics::SoftBody::set_cell_tear_resistance`]. + #[cfg_attr(feature = "serde-serialize", serde(default = "one"))] + pub tear_resistance: Real, /// Parallel solve color. pub(crate) color: u8, /// Set when the cell was strained past the material's `tear_strain` during the last step /// (or by [`crate::dynamics::SoftBody::tear_cell`]): removed by the tearing pass at the end of the step. #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) torn: bool, + /// The tensile strain of the cell as a fraction of its tear threshold, smoothed over the + /// material's `tear_smoothing` (see [`Self::stress`]). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) stress: Real, +} + +impl SoftBodyCell { + /// The largest tensile strain of this cell as a fraction of its tear threshold, smoothed + /// over the material's [`SoftBodyMaterial::tear_smoothing`]: `0.0` slack, `1.0` tearing. + /// Stays `0.0` while the material has no `tear_strain` (cells only tear on their strain). + pub fn stress(&self) -> Real { + self.stress + } } diff --git a/src/dynamics/soft_body/soft_body_material.rs b/src/dynamics/soft_body/soft_body_material.rs index 3db8b5490..4cb9c3bf9 100644 --- a/src/dynamics/soft_body/soft_body_material.rs +++ b/src/dynamics/soft_body/soft_body_material.rs @@ -63,6 +63,26 @@ pub struct SoftBodyMaterial { #[cfg_attr(feature = "serde-serialize", serde(default))] pub tear_strain: Option, /// [`crate::dynamics::SoftBody::tear_edge`]). Unlike the strain, the force reports the load of an edge that + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub tear_force: Option, + /// Time constant, in seconds, over which an element's load is averaged before it is compared + /// to its tear threshold (default: `0.0`, none). At `0.0` one step past the threshold tears; + /// with a positive value the load is smoothed exponentially over about that long, so a brief + /// spike (an impact, a jerk) is shrugged off and only a sustained pull tears. Applies to the + /// strain and the force criteria alike; [`crate::dynamics::SoftBodyEdge::stress`] and [`crate::dynamics::SoftBodyCell::stress`] + /// read the smoothed load back. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub tear_smoothing: Real, + /// How much tougher an element deep inside the body is than one at its surface (default: + /// `1.0`, no difference). An element whose particles are all interior (none on the surface) + /// and undamaged needs this many times its tear threshold; elements at the surface, or next + /// to a particle an earlier tear passed through, tear at the threshold itself. Tears then + /// start at the surface or at existing damage and run inward, where the material is + /// weakened, instead of anywhere a particle happens to be pulled. Multiplies the per-element + /// [`crate::dynamics::SoftBodyEdge::tear_resistance`] / [`crate::dynamics::SoftBodyCell::tear_resistance`]. + #[cfg_attr(feature = "serde-serialize", serde(default = "one"))] + pub interior_strength: Real, + /// ones (see [`crate::dynamics::SoftBodyEdge::stress`]) go first, the others wait for the next step (edges /// projectile being caught like in a net while its rim tears one edge at a time; the /// default lets a puncture open at once. #[cfg_attr( @@ -102,6 +122,9 @@ impl Default for SoftBodyMaterial { plastic_max: 1.0, deformation_damping: 0.0, tear_strain: None, + tear_force: None, + tear_smoothing: 0.0, + interior_strength: 1.0, max_tears_per_step: u32::MAX, } } @@ -120,6 +143,47 @@ impl SoftBodyMaterial { } } + /// Whether some tear criterion is set (`tear_strain` or `tear_force`). + pub fn tears(&self) -> bool { + self.tear_strain.is_some() || self.tear_force.is_some() + } + + /// Load of an edge as a fraction of its tear threshold: the larger of its stretch (`length` + /// over its initial rest length `rest`) over `tear_strain` and its `force` along its direction + /// (positive when resisting stretching) over `tear_force`; compression or no threshold: `0.0`. + pub(crate) fn edge_tear_load(&self, length: Real, rest: Real, force: Real) -> Real { + let mut load: Real = 0.0; + if let Some(tear) = self.tear_strain { + if rest > 0.0 { + load = (length / rest - 1.0).max(0.0) / tear.max(Real::EPSILON); + } + } + if let Some(tear) = self.tear_force { + load = load.max(force.max(0.0) / tear.max(Real::EPSILON)); + } + load + } + + /// The load of a cell whose largest tensile strain is `strain`, as a fraction of its tear + /// threshold (`0.0` without a `tear_strain`). + pub(crate) fn cell_tear_load(&self, strain: Real) -> Real { + match self.tear_strain { + Some(tear) => strain.max(0.0) / tear.max(Real::EPSILON), + None => 0.0, + } + } + + /// Blends the load `load` measured over a step of length `dt` into the smoothed load + /// `stress`, over the `tear_smoothing` time constant (no smoothing: the load itself). + pub(crate) fn smooth_stress(&self, stress: Real, load: Real, dt: Real) -> Real { + if self.tear_smoothing <= 0.0 { + load + } else { + let blend = 1.0 - (-dt / self.tear_smoothing).exp(); + stress + (load - stress) * blend + } + } + /// The Lamé parameters `(μ, λ)` of the elastic cells. pub fn lame_parameters(&self) -> (Real, Real) { let nu = self.poisson_ratio.clamp(0.0, 0.499); diff --git a/src/dynamics/soft_body/soft_body_meshes.rs b/src/dynamics/soft_body/soft_body_meshes.rs index 1c0a41dd7..b7d1efa7f 100644 --- a/src/dynamics/soft_body/soft_body_meshes.rs +++ b/src/dynamics/soft_body/soft_body_meshes.rs @@ -204,6 +204,20 @@ impl SoftBody { self.clusters = clusters; } + /// Recomputes which particles are vertices of the surface. + pub(crate) fn update_surface_flags(&mut self) { + for p in &mut self.particles { + p.on_surface = false; + } + for element in &self.boundary { + for &v in element { + if let Some(p) = self.particles.get_mut(v as usize) { + p.on_surface = true; + } + } + } + } + /// Updates the meshes' cached vertices from the current particle positions. pub(crate) fn update_meshes(&mut self) { self.for_each_mesh_mut(|body, mesh| mesh.update(&body.cells, &body.particles)); diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs index d007cd938..2b3aba654 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs @@ -278,6 +278,8 @@ impl SoftConstraintsSet { let i = range.scalar_constraints.start + idx; let constraint = &mut self.scalar_constraints[i]; if update { + // A new substep begins: the impulse is the previous one's total. + constraint.peak_impulse = constraint.peak_impulse.max(constraint.impulse); constraint.update(bodies); self.strained[i] = constraint.is_strained(min_strain); } 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 0949ee581..c49cecf20 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 @@ -24,11 +24,32 @@ impl SoftConstraintsSet { let constraint = &self.scalar_constraints[idx]; let awake = &self.awake[constraint.soft_body as usize]; let sb = unsafe { &mut *awake.ptr }; + let material = sb.material; let impulse = crate::utils::canonicalize_zero(constraint.impulse); match constraint.writeback { SoftScalarConstraintWriteback::Edge => { + let resistance = if material.tears() { + let e = &sb.edges[constraint.element as usize]; + sb.effective_tear_resistance(e.tear_resistance, &e.vertices) + } else { + 1.0 + }; let edge = &mut sb.edges[constraint.element as usize]; edge.impulse = impulse; + if material.tears() { + // The force is the step's peak substep impulse over the substep length + // (a spike must not hide behind the last substep); the stretch is + // measured against the initial rest length, so plastic flow counts. + let peak = constraint.peak_impulse.max(constraint.impulse); + let force = peak * crate::utils::inv(awake.substep_dt); + let load = + material.edge_tear_load(len, edge.initial_rest_length(), force) + * crate::utils::inv(resistance); + edge.stress = material.smooth_stress(edge.stress, load, dt); + if edge.stress > 1.0 { + edge.torn = true; + awake.torn.store(true, Ordering::Relaxed); + } } } #[cfg(feature = "dim3")] @@ -44,6 +65,12 @@ impl SoftConstraintsSet { let awake = &self.awake[constraint.soft_body as usize]; let sb = unsafe { &mut *awake.ptr }; let material = sb.material; + let resistance = if material.tear_strain.is_some() { + let c = &sb.cells[constraint.element as usize]; + sb.effective_tear_resistance(c.tear_resistance, &c.vertices) + } else { + 1.0 + }; let cell = &mut sb.cells[constraint.element as usize]; let impulses = (constraint.strain_impulse.iter()).chain(core::iter::once(&constraint.vol_impulse)); @@ -57,6 +84,12 @@ impl SoftConstraintsSet { { awake.plastic_flow.store(true, Ordering::Relaxed); } + if material.tear_strain.is_some() { + let load = material.cell_tear_load(tensile) * crate::utils::inv(resistance); + cell.stress = material.smooth_stress(cell.stress, load, dt); + if cell.stress > 1.0 { + cell.torn = true; + awake.torn.store(true, Ordering::Relaxed); } } } 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 7fa5fb7ef..196fc6a91 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 @@ -23,6 +23,9 @@ pub(crate) struct AwakeSoftBody { pub handle: SoftBodyHandle, /// Substep solve-group of the body's particles. pub group: u16, + /// Length of the group's substeps (the edge impulses are per substep: this turns them into + /// forces for the tear criterion). + pub substep_dt: Real, /// No particle is a solver DOF this step (every one pinned, or the free ones asleep): the /// body only lends its surface to the contact constraints holding the awake bodies touching it. pub frozen: bool, diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs index 719297276..8501cebe0 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_scalar_constraint.rs @@ -62,6 +62,9 @@ pub(crate) struct SoftScalarConstraint { pub cfm_gain: Real, pub rhs: Real, pub impulse: Real, + /// The largest impulse the constraint had accumulated at the end of a substep of this step (the + /// force tear criterion reads the step's peak, not its last substep). + pub peak_impulse: Real, pub impulse_bounds: [Real; 2], } diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs index 8e1ec44c9..464ab8658 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs @@ -47,6 +47,7 @@ fn gradients_match_finite_differences() { cfm_gain: 0.0, rhs: 0.0, impulse: 0.0, + peak_impulse: 0.0, impulse_bounds: [-Real::MAX, Real::MAX], }; #[cfg(feature = "dim2")] diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs index 46dc11ac3..cff1faafa 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs @@ -138,6 +138,7 @@ impl SoftConstraintsSet { // particles' solver slots: contiguous per body, bodies in awake order. self.awake.sort_by_key(|a| a.group); for awake in &mut self.awake { + awake.substep_dt = group_dt(awake.group as usize); awake.slot_start = self.slots.len(); let first = slot_base + self.slots.len(); self.slots @@ -466,6 +467,7 @@ impl SoftConstraintsSet { ptr: sb as *mut SoftBody, handle, group, + substep_dt: step_dt, frozen, slot_start: 0, num_particles: sb.particles.len(), @@ -630,6 +632,7 @@ impl SoftConstraintsSet { cfm_gain: 0.0, rhs: 0.0, impulse, + peak_impulse: 0.0, impulse_bounds: [-Real::MAX, Real::MAX], } }; diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs index 66f6a722f..cf2e46960 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs @@ -274,6 +274,23 @@ impl SoftFemSystem { let mut flowing = false; let mut torn = false; // The interior strength reads the particles' flags: taken before the cells are borrowed. + let cell_resistance: Vec = if material.tear_strain.is_some() { + sb.cells + .iter() + .map(|c| sb.effective_tear_resistance(c.tear_resistance, &c.vertices)) + .collect() + } else { + Vec::new() + }; + let edge_resistance: Vec = if material.tears() { + sb.edges + .iter() + .map(|e| sb.effective_tear_resistance(e.tear_resistance, &e.vertices)) + .collect() + } else { + Vec::new() + }; + for ((cell, out), ci) in self.cells.iter().zip(sb.cells.iter_mut()).zip(0..) { out.rotation = cell.rotation; if cell.mu <= 0.0 { continue; @@ -281,6 +298,34 @@ impl SoftFemSystem { if plastic && plastic_flow(out, &cell.strain, &material, step_dt) { flowing = true; } + if material.tear_strain.is_some() { + let load = + material.cell_tear_load(tensile) * crate::utils::inv(cell_resistance[ci]); + out.stress = material.smooth_stress(out.stress, load, step_dt); + if out.stress > 1.0 { + out.torn = true; + torn = true; + } + } + } + let edge_plasticity = material.edge_plastic_yield > 0.0 && material.edge_plastic_creep > 0.0; + if material.tears() || edge_plasticity { + for ((spring, edge), ei) in self.springs.iter().zip(sb.edges.iter_mut()).zip(0..) { + if spring.rest_length <= 0.0 { + continue; + } + let d = self.position[spring.vertices[1] as usize] + - self.position[spring.vertices[0] as usize]; + let length = d.length(); + // The elastic force of the implicit step's end state, resisting stretching; the + let force = spring.stiffness * (length - spring.rest_length).max(0.0); + let load = material.edge_tear_load(length, edge.initial_rest_length(), force) + * crate::utils::inv(edge_resistance[ei]); + edge.stress = material.smooth_stress(edge.stress, load, step_dt); + if edge.stress > 1.0 { + edge.torn = true; + torn = true; + } } } sb.plastic_flowing |= flowing; From 4be54fa17950e6272bf863d00e298b0f11c70818 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 10 Sep 2026 17:49:09 +0200 Subject: [PATCH 13/32] feat(soft-body): plastic flow of the edge rest lengths with yield, creep, cap and direction --- crates/rapier2d/tests/soft_bodies.rs | 83 +++++++++++++++++++ src/dynamics/soft_body/mod.rs | 2 +- .../soft_body_builder/soft_body_build.rs | 1 + src/dynamics/soft_body/soft_body_elements.rs | 14 ++-- src/dynamics/soft_body/soft_body_material.rs | 64 ++++++++------ .../soft_body/soft_body_plasticity.rs | 72 ++++++++++++++++ .../soft_constraint_plasticity.rs | 74 +++++++++++++++++ .../soft_constraint_writeback.rs | 5 ++ .../system/soft_fem_system_assemble.rs | 7 ++ 9 files changed, 290 insertions(+), 32 deletions(-) create mode 100644 src/dynamics/soft_body/soft_body_plasticity.rs create mode 100644 src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index b5ec242c7..c4bca57f3 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -1960,3 +1960,86 @@ fn interior_strength_shields_undamaged_interior_edges() { assert!(close(plain.2, tough.2), "an edge next to damage lost its shield"); assert!(close(plain.3, 4.0 * tough.3), "an edge far from damage keeps its shield"); } + +/// A squeezed edge past its yield takes a permanent set (only the excess strain flows), bounded +/// by the plastic maximum; the flow direction picks squeeze, stretch or both, and the tear +/// strain measures against the original length, so a flowed edge tears at the original stretch. +#[test] +fn edge_plasticity_keeps_a_dent() { + // The tested edge joins two pinned particles, so its length is exactly what the test sets + // and it never tears (its tear load is read through its stress). A slack edge to a free + // particle keeps the body from being frozen; each end hangs on a pinned three-segment chain. + let squeeze = |material: SoftBodyMaterial| { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let builder = SoftBodyBuilder::new(vec![ + Vector::new(0.0, 0.0), + Vector::new(1.0, 0.0), + Vector::new(5.0, 0.0), + Vector::new(6.0, 0.0), + Vector::new(0.0, 1.0), + Vector::new(1.0, 1.0), + Vector::new(0.0, 2.0), + Vector::new(1.0, 2.0), + Vector::new(0.0, 3.0), + Vector::new(1.0, 3.0), + ]) + .edges(vec![[0, 1], [2, 3], [0, 4], [1, 5], [4, 6], [5, 7], [6, 8], [7, 9]]) + .pinned_particles([0, 1, 3, 4, 5, 6, 7, 8, 9]) + .particle_mass(1.0) + .material(material) + .no_surface_collider() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + // Squeezed to 0.7: a strain of -0.3. + world.soft_bodies[handle].set_particle_position(1, Vector::new(0.7, 0.0)); + for _ in 0..30 { + world.step(); + } + (world, handle) + }; + let clay = |flow: SoftEdgePlasticFlow, max: Real| SoftBodyMaterial { + edge_softness: SpringCoefficients::new(30.0, 1.0), + edge_plastic_yield: 0.1, + edge_plastic_creep: Real::INFINITY, + edge_plastic_max: max, + edge_plastic_flow: flow, + tear_strain: Some(0.25), + ..Default::default() + }; + + // Both directions: the rest length flows to what leaves exactly the yield strain. + let (mut world, handle) = squeeze(clay(SoftEdgePlasticFlow::Both, 1.0)); + let sb = &world.soft_bodies[handle]; + let e = &sb.edges()[0]; + assert!((e.rest_length - 0.7 / 0.9).abs() < 1.0e-4, "rest length {}", e.rest_length); + assert!((e.initial_rest_length() - 1.0).abs() < 1.0e-4); + assert!((e.plastic_strain() - (0.7 / 0.9 - 1.0)).abs() < 1.0e-4); + + // The plastic maximum bounds the set. + let (world, handle) = squeeze(clay(SoftEdgePlasticFlow::Both, 0.15)); + let e = &world.soft_bodies[handle].edges()[0]; + assert!((e.rest_length - 0.85).abs() < 1.0e-4, "rest length {}", e.rest_length); + + // Tension-only clay does not dent. + let (world, handle) = squeeze(clay(SoftEdgePlasticFlow::Tension, 1.0)); + let e = &world.soft_bodies[handle].edges()[0]; + assert!((e.rest_length - 1.0).abs() < 1.0e-6, "rest length {}", e.rest_length); + + // Compression-only clay dents, then springs back from a stretch (the rest stays where it + // flowed to), and the tear strain is measured against the initial length: 20% past it is + // below the tear strain even though it is 54% past the flowed rest length. + let (mut world, handle) = squeeze(clay(SoftEdgePlasticFlow::Compression, 1.0)); + world.soft_bodies[handle].set_particle_position(1, Vector::new(1.2, 0.0)); + for _ in 0..30 { + world.step(); + } + let sb = &world.soft_bodies[handle]; + let e = &sb.edges()[0]; + assert!((e.rest_length - 0.7 / 0.9).abs() < 1.0e-4, "a stretch flowed: {}", e.rest_length); + // 30% past the initial length: past the tear strain. + world.soft_bodies[handle].set_particle_position(1, Vector::new(1.3, 0.0)); + for _ in 0..30 { + world.step(); + } +} diff --git a/src/dynamics/soft_body/mod.rs b/src/dynamics/soft_body/mod.rs index e05965c16..2aeff1505 100644 --- a/src/dynamics/soft_body/mod.rs +++ b/src/dynamics/soft_body/mod.rs @@ -11,8 +11,8 @@ pub use self::collision_mesh::{ pub use self::soft_body_elements::SoftBodyDihedral; #[cfg(feature = "fem")] pub use self::soft_body_elements::SoftBodySolver; -pub use self::soft_body::{SOFT_BODY_MAX_CONSTRAINT_PARTICLES, SoftBody}; pub use self::soft_body_contacts::SoftVolumeContact; +pub use self::soft_body::{SOFT_BODY_MAX_CONSTRAINT_PARTICLES, SoftBody, SoftVolumePiece}; pub use self::soft_body_elements::{ SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyEdgeKind, SoftBodyParticle, }; 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 b1b769257..c43c387a8 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 @@ -148,6 +148,7 @@ impl SoftBodyBuilder { softness: None, tear_resistance: 1.0, impulse: 0.0, + plastic_strain: 0.0, color: 0, torn: false, stress: 0.0, diff --git a/src/dynamics/soft_body/soft_body_elements.rs b/src/dynamics/soft_body/soft_body_elements.rs index 5710ae0c8..6ccdee2ad 100644 --- a/src/dynamics/soft_body/soft_body_elements.rs +++ b/src/dynamics/soft_body/soft_body_elements.rs @@ -136,8 +136,13 @@ pub struct SoftBodyEdge { pub tear_resistance: Real, /// Accumulated impulse of the last step (warm-start state). pub(crate) impulse: Real, + /// Accumulated plastic strain: `rest_length = initial rest length * (1 + plastic_strain)` + /// (see [`Self::plastic_strain`]). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) plastic_strain: Real, /// Parallel solve color. pub(crate) color: u8, + /// Set when the edge was loaded past the material's tear threshold during the last step /// (or by [`crate::dynamics::SoftBody::tear_edge`]): removed by the tearing pass at the end of the step. #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) torn: bool, @@ -154,15 +159,14 @@ impl SoftBodyEdge { self.impulse } - /// The load this edge carries as a fraction of its tear threshold, smoothed over the - /// material's [`SoftBodyMaterial::tear_smoothing`]: `0.0` slack, `1.0` tearing. The larger of - /// the stretch over [`SoftBodyMaterial::tear_strain`] and the force over - /// [`SoftBodyMaterial::tear_force`]; stays `0.0` while the material has neither. + /// Load of this edge as a fraction of its tear threshold, smoothed over + /// [`SoftBodyMaterial::tear_smoothing`]: `0.0` slack, `1.0` tearing. The larger of the stretch + /// over `tear_strain` and the force over `tear_force`; `0.0` while the material has neither. pub fn stress(&self) -> Real { self.stress } - } + /// A dihedral bending constraint between two triangles sharing an edge (3D only). /// /// `vertices[0..2]` is the shared edge, `vertices[2]` and `vertices[3]` the two opposite diff --git a/src/dynamics/soft_body/soft_body_material.rs b/src/dynamics/soft_body/soft_body_material.rs index 4cb9c3bf9..7ad304f8b 100644 --- a/src/dynamics/soft_body/soft_body_material.rs +++ b/src/dynamics/soft_body/soft_body_material.rs @@ -16,45 +16,53 @@ pub struct SoftBodyMaterial { pub edge_softness: SpringCoefficients, /// Softness of the bending edges and dihedral constraints. pub bend_softness: SpringCoefficients, - /// Softness of the per-cell volume rows ([`crate::dynamics::SoftBodyCellModel::Volume`]) and of the global - /// volume-preservation row. + /// Softness of the per-cell volume constraints ([`crate::dynamics::SoftBodyCellModel::Volume`]) and of the global + /// volume-preservation constraint. pub volume_softness: SpringCoefficients, - /// Softness of the shape-matching rows. + /// Softness of the shape-matching constraints. pub shape_matching_softness: SpringCoefficients, - /// Young's modulus of the elastic cells ([`crate::dynamics::SoftBodyCellModel::Corotational`] and - /// [`crate::dynamics::SoftBodyCellModel::NeoHookean`]), in force per unit area (3D) or per unit length (2D). - /// - /// Unlike the other families, the elastic cells are parameterized physically: their per-cell - /// natural frequency is derived from the modulus, the cell's rest volume and the particle - /// masses, so a finer mesh of the same material behaves like the coarser one (up to the - /// discretization) instead of getting stiffer. + /// Young's modulus of the elastic cells ([`crate::dynamics::SoftBodyCellModel::Corotational`] + /// and [`crate::dynamics::SoftBodyCellModel::NeoHookean`]), in force per unit area (3D) or + /// length (2D); their natural frequency derives from it, so a finer mesh does not get stiffer. pub young_modulus: Real, /// Poisson's ratio of the elastic cells, in `[0, 0.5)`. pub poisson_ratio: Real, /// Damping ratio of the elastic cells. pub elastic_damping_ratio: Real, - /// Plastic yield of the elastic cells (corotational and Neo-Hookean models): the strain - /// magnitude beyond which a cell's rest shape starts flowing toward its current shape - /// (default: `0.0`, meaning no plasticity). Strain here is the Frobenius norm of the + /// Plastic yield of the elastic cells: the strain (Frobenius norm of the deviatoric part of + /// `sym(RᵀF) - I`, read from stress by the constraint solver) beyond which a cell's + /// rest shape flows toward its current shape (default: `0.0`, no plasticity). pub plastic_yield: Real, /// Rate (per second) at which the strain in excess of the yield is absorbed into the rest /// shape (default: `1.0`); the flow is deviatoric (volume preserving). pub plastic_creep: Real, - /// Largest accumulated plastic deformation of a cell (default: `1.0`): the Frobenius norm of - /// `P - I`, `P` the plastic stretch of its rest shape (a cell flattened to 40% of its rest - /// thickness is at about `1.0`). Flow past it is discarded, which keeps a repeatedly crushed - /// cell from flowing toward a sliver (and from there to inversions and instability). + /// Largest accumulated plastic deformation of a cell, the Frobenius norm of `P - I` with `P` + /// its rest shape's plastic stretch (default: `1.0`). Flow past it is projected back onto the + /// bound, so a crushed cell cannot flow toward a sliver; an inverted cell never flows. pub plastic_max: Real, - /// Rate (per second) at which the particles' velocities are pulled toward the body's - /// best-fit rigid motion, damping every deformation mode without slowing the body down as a - /// whole (default: `0.0`, off). - /// - /// The material rows only damp their own strain rates: the residual bending of a stiff - /// slender body (a standing letter, a cantilever) is left by the solver's finite convergence - /// and is not damped by them, so such bodies sway for a long time. A rate of a few units per - /// second settles them within a second or two; soft jelly-like bodies keep it at zero (it - /// would damp their wobble too). + /// Rate (per second) at which the particles' velocities are pulled toward the body's best-fit + /// rigid motion, damping every deformation mode but not the whole-body motion; a few units per + /// second settle a stiff slender body's residual bending sway (default: `0.0`, off). pub deformation_damping: Real, + /// Plastic yield of the edges (structural and bending): the strain `|length / rest_length - 1|` + /// beyond which the rest length flows toward the current length (default: `0.0`, none), at the + /// rate [`Self::edge_plastic_creep`], up to `edge_plastic_max`; tears use the initial length. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub edge_plastic_yield: Real, + /// Rate (per second) at which an edge's strain in excess of its yield is absorbed into its + /// rest length (default: `1.0`); `Real::INFINITY` (or anything above the step rate) sets it at + /// once. + #[cfg_attr(feature = "serde-serialize", serde(default = "one"))] + pub edge_plastic_creep: Real, + /// Largest permanent set an edge may accumulate, as a fraction of the length it was created + /// with (default: `1.0`): past it the edge stops yielding and stays elastic from where it + /// flowed to (a repeatedly crushed edge cannot flow to nothing). + #[cfg_attr(feature = "serde-serialize", serde(default = "half"))] + pub edge_plastic_max: Real, + /// Whether the edges take a permanent set under a squeeze, a stretch, or both (default: + /// both). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub edge_plastic_flow: SoftEdgePlasticFlow, /// Strain beyond which the elements break (default: `None`, unbreakable): a structural or /// cell whose largest tensile principal strain exceeds it, is torn at the end of the step /// (see [`crate::dynamics::SoftBody::tear_edge`] for what a tear removes). Cells of the volume model do not @@ -121,6 +129,10 @@ impl Default for SoftBodyMaterial { plastic_creep: 1.0, plastic_max: 1.0, deformation_damping: 0.0, + edge_plastic_yield: 0.0, + edge_plastic_creep: 1.0, + edge_plastic_max: 0.5, + edge_plastic_flow: SoftEdgePlasticFlow::Both, tear_strain: None, tear_force: None, tear_smoothing: 0.0, diff --git a/src/dynamics/soft_body/soft_body_plasticity.rs b/src/dynamics/soft_body/soft_body_plasticity.rs new file mode 100644 index 000000000..4578ecf3f --- /dev/null +++ b/src/dynamics/soft_body/soft_body_plasticity.rs @@ -0,0 +1,72 @@ +//! Soft-body plasticity: edge flow modes, the rest-length and rest-angle flow rules, the fit of +//! the rest positions to the flowed rest shapes, and the reset. The cells' flow rule runs on the +//! solver's strain rows (`soft_constraints_set::plastic_flow`). +#[cfg(feature = "dim3")] +use crate::dynamics::{SoftBody, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial}; +use crate::math::{DIM, Matrix, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +/// Which strains make a soft-body edge flow plastically (see +/// [`SoftBodyMaterial::edge_plastic_yield`]). +#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum SoftEdgePlasticFlow { + /// Both a squeeze and a stretch past the yield take a permanent set. + #[default] + Both, + /// Only a squeeze sets; a stretched edge always springs back (clay dents, but does not + /// stay stretched). + Compression, + /// Only a stretch sets; a squeezed edge always springs back. + Tension, +} + +impl SoftBodyEdge { + /// The permanent set of this edge: how far its rest length has flowed from the length it was + /// created with, as a fraction of that length (negative: a squeeze that stayed, positive: a + /// stretch that stayed; see [`SoftBodyMaterial::edge_plastic_yield`]). + pub fn plastic_strain(&self) -> Real { + self.plastic_strain + } + + /// The rest length this edge was created with (its `rest_length` before any plastic flow). + /// The tear strain is measured against it, so plastic flow counts toward a tear. + pub fn initial_rest_length(&self) -> Real { + self.rest_length / (1.0 + self.plastic_strain) + } + + /// Flows the rest length toward `length` when the strain exceeds the material's edge yield + /// ([`SoftBodyMaterial::edge_plastic_yield`]; `dt`: the step length). Returns whether the edge + /// flowed significantly (such a body is kept awake). + pub(crate) fn plastic_flow(&mut self, length: Real, material: &SoftBodyMaterial, dt: Real) -> bool { + let yield_strain = material.edge_plastic_yield; + if yield_strain <= 0.0 || material.edge_plastic_creep <= 0.0 || self.rest_length <= 0.0 { + return false; + } + let strain = length / self.rest_length - 1.0; + let allowed = match material.edge_plastic_flow { + SoftEdgePlasticFlow::Both => true, + SoftEdgePlasticFlow::Compression => strain < 0.0, + SoftEdgePlasticFlow::Tension => strain > 0.0, + }; + if !allowed || strain.abs() <= yield_strain { + return false; + } + // The rest length that would leave exactly the yield strain: only the excess flows. + let signed_yield = if strain > 0.0 { yield_strain } else { -yield_strain }; + let target = length / (1.0 + signed_yield); + let blend = (material.edge_plastic_creep * dt).min(1.0); + let initial = self.initial_rest_length(); + let max = material.edge_plastic_max.max(0.0); + let new_rest = (self.rest_length + (target - self.rest_length) * blend).clamp( + initial * (1.0 - max).max(Real::EPSILON), + initial * (1.0 + max), + ); + let increment = (new_rest - self.rest_length).abs() / initial; + self.rest_length = new_rest; + self.plastic_strain = new_rest / initial - 1.0; + increment > 1.0e-4 + } +} diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs new file mode 100644 index 000000000..e78d6ca9c --- /dev/null +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs @@ -0,0 +1,74 @@ +//! Plastic flow of the elastic cells from the strain of the last solve, shared by the constraint +//! writeback and the FEM writeback, and the total (elastic plus plastic) stretch a cell tears on. + +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use crate::math::{DIM, Matrix, Real}; + +use super::super::soft_element_constraint::{StrainVector, strain_matrix}; + +/// Plastic flow of an elastic cell (see `SoftBodyMaterial::plastic_yield`): past the yield the rest +/// shape flows toward the deviatoric stretch at the creep rate (`Dm' = S_p Dm`, unit determinant, +/// `Dm = P Dm₀` bounded by `plastic_max`); inverted cells do not flow. Returns whether it flowed. +pub(crate) fn plastic_flow( + cell: &mut crate::dynamics::SoftBodyCell, + strain: &StrainVector, + material: &crate::dynamics::SoftBodyMaterial, + dt: Real, +) -> bool { + let diagonal = strain.fixed_rows::(0); + if diagonal.iter().any(|e| *e <= -0.9) { + } + let eps = strain_matrix(strain); + let trace = diagonal.sum(); + let dev = eps - Matrix::IDENTITY * (trace / DIM as Real); + let norm = frobenius_norm(&dev); + if norm <= material.plastic_yield { + return false; + } + let gamma = ((norm - material.plastic_yield) / norm * material.plastic_creep * dt).min(1.0); + // The plastic stretch: a fraction of the full stretch `S = I + ε`, normalized to a unit + // determinant so the flow keeps the rest volume. + let s_p = Matrix::IDENTITY + eps * gamma; + let det = s_p.determinant(); + if det <= 1.0e-6 { + return false; + } + let s_p = s_p * (1.0 / det.powf(1.0 / DIM as Real)); + let total = s_p * cell.plastic_stretch; + let deviation = total - Matrix::IDENTITY; + let deviation_norm = frobenius_norm(&deviation); + let total = if deviation_norm > material.plastic_max { + Matrix::IDENTITY + deviation * (material.plastic_max / deviation_norm) + } else { + total + }; + let total_det = total.determinant(); + if total_det <= 1.0e-6 { + return false; + } + let total = total * (1.0 / total_det.powf(1.0 / DIM as Real)); + // The new rest edge matrix `P' Dm₀` and its signed volume (`det(Dm) / DIM!`). + cell.inv_rest_matrix = cell.inv_rest_matrix * cell.plastic_stretch * total.inverse(); + let increment = frobenius_norm(&(total - cell.plastic_stretch)); + cell.plastic_stretch = total; + // Rest volume from the new rest matrix (signed, `det(Dm) / DIM!`). + let inv_det = cell.inv_rest_matrix.determinant(); + if inv_det != 0.0 { + let factorial = if DIM == 2 { 2.0 } else { 6.0 }; + cell.rest_volume = 1.0 / (inv_det * factorial); + } + increment > 1.0e-4 +} + +/// Frobenius norm of a matrix. +#[inline] +fn frobenius_norm(m: &Matrix) -> Real { + let mut norm_sq = 0.0; + for j in 0..DIM { + norm_sq += m.col(j).length_squared(); + } + norm_sq.sqrt() +} 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 c49cecf20..949d18386 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 @@ -36,6 +36,11 @@ impl SoftConstraintsSet { }; let edge = &mut sb.edges[constraint.element as usize]; edge.impulse = impulse; + // Length as seen at the last substep's update. + let len = (constraint.pos[1] - constraint.pos[0]).length(); + if edge.plastic_flow(len, &material, dt) { + awake.plastic_flow.store(true, Ordering::Relaxed); + } if material.tears() { // The force is the step's peak substep impulse over the substep length // (a spike must not hide behind the last substep); the stretch is diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs index cf2e46960..4004b7b7d 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs @@ -317,7 +317,14 @@ impl SoftFemSystem { let d = self.position[spring.vertices[1] as usize] - self.position[spring.vertices[0] as usize]; let length = d.length(); + if edge.plastic_flow(length, &material, step_dt) { + flowing = true; + } + if !material.tears() { + continue; + } // The elastic force of the implicit step's end state, resisting stretching; the + // stretch is measured against the initial rest length (plastic flow counts). let force = spring.stiffness * (length - spring.rest_length).max(0.0); let load = material.edge_tear_load(length, edge.initial_rest_length(), force) * crate::utils::inv(edge_resistance[ei]); From 80bffe3025facdf2468b87215f1c89013c103568 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 11 Sep 2026 15:55:07 +0200 Subject: [PATCH 14/32] feat(soft-body): tear events and pieces, blade cuts, point and radial impulses, tear-load coloring and 2D demos --- ...sue_718_contact_reporting_feature_churn.rs | 3 +- crates/rapier2d/tests/soft_bodies.rs | 235 ++++++++++- .../tests/soft_contact_debug_render.rs | 52 +++ .../tests/contact_force_event_first_tick.rs | 2 + .../tests/issue_253_unsync_callbacks.rs | 2 + crates/rapier3d/tests/soft_bodies.rs | 137 +++++++ crates/rapier3d/tests/soft_body_clusters.rs | 2 +- crates/rapier3d/tests/soft_body_joints.rs | 2 +- examples2d/all_examples2.rs | 6 + examples2d/ccd2.rs | 4 +- examples2d/sensor2.rs | 4 +- examples2d/soft_cutting2.rs | 123 ++++++ examples2d/soft_force_tearing2.rs | 188 +++++++++ examples2d/soft_stress2.rs | 110 +++++ examples3d/ccd3.rs | 4 +- examples3d/sensor3.rs | 4 +- src/dynamics/soft_body/mod.rs | 1 + src/dynamics/soft_body/soft_body_motion.rs | 60 +++ .../soft_body_set/soft_body_set_step_sync.rs | 382 +++++++++++++++++- src/dynamics/soft_body/tearing/mod.rs | 13 + src/dynamics/soft_body/tearing/tearing.rs | 200 +++++++++ .../soft_body/tearing/tearing_event.rs | 35 ++ .../soft_body/tearing/tearing_helpers.rs | 102 +++++ .../tearing/tearing_particle_split.rs | 373 +++++++++++++++++ .../soft_body/tearing/tearing_tables.rs | 28 ++ .../soft_body/tearing/tearing_validate.rs | 172 ++++++++ .../debug_render_pipeline.rs | 76 +++- .../debug_render_style.rs | 9 + src/pipeline/event_handler.rs | 16 +- src/pipeline/physics_world.rs | 44 +- src_testbed/debug_render.rs | 6 +- src_testbed/ui.rs | 7 + src_testbed/viewer.rs | 11 + 33 files changed, 2372 insertions(+), 41 deletions(-) create mode 100644 examples2d/soft_cutting2.rs create mode 100644 examples2d/soft_force_tearing2.rs create mode 100644 examples2d/soft_stress2.rs create mode 100644 src/dynamics/soft_body/tearing/mod.rs create mode 100644 src/dynamics/soft_body/tearing/tearing.rs create mode 100644 src/dynamics/soft_body/tearing/tearing_event.rs create mode 100644 src/dynamics/soft_body/tearing/tearing_helpers.rs create mode 100644 src/dynamics/soft_body/tearing/tearing_particle_split.rs create mode 100644 src/dynamics/soft_body/tearing/tearing_tables.rs create mode 100644 src/dynamics/soft_body/tearing/tearing_validate.rs diff --git a/crates/rapier2d/tests/issue_718_contact_reporting_feature_churn.rs b/crates/rapier2d/tests/issue_718_contact_reporting_feature_churn.rs index 7d21b11ee..0a26d38ba 100644 --- a/crates/rapier2d/tests/issue_718_contact_reporting_feature_churn.rs +++ b/crates/rapier2d/tests/issue_718_contact_reporting_feature_churn.rs @@ -34,7 +34,8 @@ fn sliding_cuboid_contact_reporting_does_not_panic() { let (collision_send, collision_recv) = channel(); let (force_send, force_recv) = channel(); - let events = ChannelEventCollector::new(collision_send, force_send); + let (tear_send, _tear_recv) = channel(); + let events = ChannelEventCollector::new(collision_send, force_send, tear_send); let mut num_collision_events = 0; for _ in 0..300 { diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index c4bca57f3..1c5d68333 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -1233,13 +1233,13 @@ fn tearing_splits_the_particle_two_fans_share() { let idx = |i: u32, j: u32| i * ny + j; .edges() .iter() - assert!( - world - .soft_bodies - .tear(handle, &[], &torn, &mut world.bodies, &mut world.colliders) - ); + let event = world.tear_soft_body(handle, &[edge], &[]) + .expect("nothing tore"); let sb = &world.soft_bodies[handle]; sb.validate_topology().unwrap(); + assert_eq!(event.soft_body, handle); + assert_eq!(event.torn_edges.len(), 1); + assert_eq!(event.pieces.len(), 2); let left: Vec = (0..=top).collect(); let right: Vec = (top + 1..num_particles + 2).collect(); assert_eq!(sb.connected_pieces(), vec![left, right]); @@ -1810,6 +1810,25 @@ fn self_contact_blob_survives_grab_crush() { } } +/// A 1 kg weight hung from a pinned bar by the tested edge `(3, 4)`, which bears the whole +/// weight; a tear splits the edge's pinned end and the free copy falls with the weight. Bar and +/// weight are three segments each, the smallest piece a tear may split off. +fn hanging_weight(tear_force: Real) -> SoftBodyBuilder { + SoftBodyBuilder::new(vec![ + Vector::new(-1.0, 2.0), + Vector::new(0.0, 2.0), + Vector::new(0.0, 1.0), + Vector::new(0.0, 0.0), + ]) + // The pinned bar, the tested edge, and the weight's own edges (reinforced). + .edges(vec![[0, 1], [1, 2], [2, 3], [3, 4], [4, 5], [5, 6], [6, 7]]) + .edge_tear_resistance([(4, 100.0), (5, 100.0), (6, 100.0)]) + .pinned_particles([0, 1, 2, 3]) + .particle_mass(0.25) + .softness(SpringCoefficients::new(120.0, 1.0)) + .tear_force(tear_force) + .no_surface_collider() + .can_sleep(false) } /// A stiff edge holding a weight barely stretches, yet it bears the weight: with a tear force @@ -2043,3 +2062,209 @@ fn edge_plasticity_keeps_a_dent() { world.step(); } } + +/// Tears are reported to the event handler and keep every segment: a seven-particle rope whose +/// middle edge snaps reports the torn edge, the split particle and two three-segment pieces. +/// A three-segment piece cannot tear again, and an overloaded bottom edge never sheds a chip. +#[test] +fn tear_events_and_rope_ends() { + let rope = |num_particles: usize| { + let positions = (0..num_particles) + .map(|i| Vector::new(0.0, 3.0 - i as Real)) + .collect(); + let edges = (0..num_particles as u32 - 1).map(|i| [i, i + 1]).collect(); + SoftBodyBuilder::new(positions) + .edges(edges) + .pinned_particles([0]) + .particle_mass(1.0) + .softness(SpringCoefficients::new(120.0, 1.0)) + .no_surface_collider() + .can_sleep(false) + }; + + // Seven particles: the middle edge bears three masses, past the tear force; the edges above + // it (four to six masses) are reinforced. + let mut world = PhysicsWorld::new(); + let handle = world.insert_soft_body( + rope(7) + .edge_tear_resistance([(0, 4.0), (1, 4.0), (2, 4.0)]) + .tear_force(24.0), + ); + let log = TearLog::default(); + for _ in 0..60 { + world.step_with_events(&(), &log); + } + { + let events = log.0.lock().unwrap(); + assert_eq!(events.len(), 1, "one tear expected, got {events:?}"); + let event = &events[0]; + assert_eq!(event.soft_body, handle); + assert_eq!(event.torn_edges, vec![[3, 4]]); + assert!(event.torn_cells.is_empty()); + assert_eq!(event.split_particles, vec![(7, 3)]); +} +} + +/// A blade across a grid between two of its columns splits the particles of the edges it meets, +/// each cell going whole to the side of its centroid, leaving two pieces and removing no cell. +/// A blade crossing nothing, and the same blade again, are no-ops. +#[test] +fn cut_splits_a_grid_along_a_segment() { + let mut world = world_with_ground(); + let n = 5usize; + let grid = SoftBodyBuilder::grid(Vector::new(0.0, 2.0), Vector::splat(1.0), n, n) + .particle_mass(0.2); + let handle = world.insert_soft_body(grid); + world.step(); + let sb = &world.soft_bodies[handle]; + let (edges, cells) = sb.crossing_elements(&blade); + assert!(!edges.is_empty() && !cells.is_empty()); + assert!( + world + .cut_soft_body(handle, &[Vector::new(5.0, -5.0), Vector::new(5.0, 10.0)]) + .is_none(), + "a blade beside the body cut something" + ); + let event = world + .cut_soft_body(handle, &blade) + .expect("the blade crossed the grid"); + assert_eq!(event.pieces.len(), 2); + let bodies: Vec = event.bodies().collect(); + let mut sides = Vec::new(); + let (mut cells_after, mut mass_after, mut measure_after) = (0, 0.0, 0.0); + for &h in &bodies { + let sb = &world.soft_bodies[h]; + sb.validate_topology().unwrap(); + assert_eq!(sb.connected_pieces().len(), 1); + cells_after += sb.cells().len(); + mass_after += sb.mass(); + measure_after += sb.rest_measure(); + let mut side = None; + for c in sb.cells() { + let centroid: Vector = c + .vertices + .iter() + .map(|&v| sb.particle_position(v as usize)) + .sum::() + / 3.0; + assert_eq!(*side.get_or_insert(centroid.x < 0.25), centroid.x < 0.25); + } + sides.push(side.unwrap()); + } + assert_eq!(cells_after, num_cells); + assert!((mass_after - mass).abs() < 1.0e-5 && (measure_after - measure).abs() < 1.0e-5); + assert_ne!(sides[0], sides[1]); + for &h in &bodies { + assert!( + world.cut_soft_body(h, &blade).is_none(), + "the same cut twice changed something" + ); + } + for _ in 0..60 { + world.step(); +} + +/// Point and radial impulses with a linear falloff: the particle on the point takes the whole +/// impulse over its mass, one halfway to the radius half of it, one at the radius or beyond +/// nothing, and a pinned particle nothing at all; the radial variant pushes away from the center. +#[test] +fn impulses_fall_off_with_the_distance() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let builder = SoftBodyBuilder::new(vec![ + Vector::new(0.0, 0.0), + Vector::new(1.0, 0.0), + Vector::new(2.0, 0.0), + Vector::new(0.0, 1.0), + Vector::new(-1.0, 0.0), + ]) + .edges(vec![[0, 1], [1, 2], [0, 3], [0, 4]]) + .pinned_particles([3]) + .masses(vec![1.0, 2.0, 1.0, 1.0, 1.0]) + .no_surface_collider(); + let handle = world.insert_soft_body(builder); + let sb = &mut world.soft_bodies[handle]; + sb.apply_impulse_at_point(Vector::new(0.0, 4.0), Vector::ZERO, 2.0, true); + let v = |i: usize| sb.particle_velocity(i); + assert!((v(0) - Vector::new(0.0, 4.0)).length() < 1.0e-6, "on the point: {:?}", v(0)); + assert!((v(1) - Vector::new(0.0, 1.0)).length() < 1.0e-6, "halfway, twice the mass: {:?}", v(1)); + assert_eq!(v(2), Vector::ZERO, "at the radius"); + assert_eq!(v(3), Vector::ZERO, "pinned"); + assert!((v(4) - Vector::new(0.0, 2.0)).length() < 1.0e-6, "halfway: {:?}", v(4)); + + for i in 0..sb.num_particles() { + sb.set_particle_velocity(i, Vector::ZERO); + } + sb.apply_radial_impulse(Vector::ZERO, 4.0, 2.0, true); + let v = |i: usize| sb.particle_velocity(i); + assert_eq!(v(0), Vector::ZERO, "on the center: no direction"); + assert!((v(1) - Vector::new(1.0, 0.0)).length() < 1.0e-6, "pushed right: {:?}", v(1)); + assert_eq!(v(2), Vector::ZERO, "at the radius"); + assert_eq!(v(3), Vector::ZERO, "pinned"); + assert!((v(4) - Vector::new(-2.0, 0.0)).length() < 1.0e-6, "pushed left: {:?}", v(4)); + + // Without a falloff radius every free particle gets the whole impulse. + for i in 0..sb.num_particles() { + sb.set_particle_velocity(i, Vector::ZERO); + } + sb.apply_impulse_at_point(Vector::new(0.0, 4.0), Vector::ZERO, 0.0, true); + assert!((sb.particle_velocity(2) - Vector::new(0.0, 4.0)).length() < 1.0e-6); +} + +/// The contact-modification hook sees the contacts between two soft surfaces (their +/// candidates) and the soft-body solver follows its edits: a hook disabling every candidate +/// between the two lets a blob fall through a pinned rope it otherwise rests on. +#[test] +fn modify_solver_contacts_edits_soft_soft_contacts() { + struct DropSoftSoft; + impl PhysicsHooks for DropSoftSoft { + fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { + let soft = |h: ColliderHandle| context.colliders[h].user_data == 7; + if soft(context.collider1) && soft(context.collider2) { + // Two soft surfaces: their contacts are candidates, not solver contacts. + if let Some(candidates) = context.soft_mut() { + candidates.disable_all(); + } + } + } + } + let scene = |hooked: bool| { + let mut world = world_with_ground(); + let n = 11; + let template = ColliderBuilder::ball(0.04) + .user_data(7) + .active_hooks(if hooked { + ActiveHooks::MODIFY_SOLVER_CONTACTS + } else { + ActiveHooks::empty() + }); + world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(-0.6, 1.0), Vector::new(0.6, 1.0), n) + .pinned_particles(0..n as u32) + .particle_mass(0.05) + .particle_radius(0.04) + .surface_collider(template.clone()), + ); + let blob = world.insert_soft_body( + SoftBodyBuilder::disk(Vector::new(0.0, 1.6), 0.35, 20) + .softness(SpringCoefficients::new(20.0, 1.0)) + .volume_factor(1.1) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(template), + ); + for _ in 0..240 { + world.step_with_events(&DropSoftSoft, &()); + } + assert_finite(&world, blob); + let sb = &world.soft_bodies[blob]; + (0..sb.num_particles()) + .map(|i| sb.particle_position(i).y) + .sum::() + / sb.num_particles() as Real + }; + let resting = scene(false); + let dropped = scene(true); + assert!(resting > 1.1, "the blob did not rest on the rope: y = {resting}"); + assert!(dropped < 0.6, "the hook did not drop the soft-soft contacts: y = {dropped}"); +} diff --git a/crates/rapier2d/tests/soft_contact_debug_render.rs b/crates/rapier2d/tests/soft_contact_debug_render.rs index d09326ff1..9493fc5c8 100644 --- a/crates/rapier2d/tests/soft_contact_debug_render.rs +++ b/crates/rapier2d/tests/soft_contact_debug_render.rs @@ -123,3 +123,55 @@ fn soft_contacts_are_drawn_at_their_witness_points() { } } } + +/// With `SOFT_BODY_STRESS`, an element's color runs from the slack color to the loaded color +/// with its tear load: an edge bearing half its tear force is drawn halfway, an unloaded edge +/// in the slack color; without the flag every element keeps the element color. +#[test] +fn soft_elements_are_colored_by_their_load() { + let mut world = PhysicsWorld::new(); + // Edge 0 hangs a unit mass (9.81 N, half the tear force); edge 1 joins two pinned + // particles and bears nothing. + let builder = SoftBodyBuilder::new(vec![ + Vector::new(0.0, 2.0), + Vector::new(1.0, 2.0), + Vector::new(0.0, 1.0), + Vector::new(1.0, 1.0), + ]) + .edges(vec![[0, 2], [1, 3]]) + .pinned_particles([0, 1, 3]) + .particle_mass(1.0) + .softness(SpringCoefficients::new(120.0, 1.0)) + .tear_force(2.0 * 9.81) + .no_surface_collider() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + for _ in 0..60 { + world.step(); + } + let sb = &world.soft_bodies[handle]; + let loaded = sb.edges()[0].stress(); + assert!((loaded - 0.5).abs() < 0.05, "edge 0 bears {loaded} of its threshold"); + assert!(sb.edges()[1].stress().abs() < 1.0e-6); + + let style = DebugRenderStyle::default(); + let render = |mode: DebugRenderMode| { + let mut pipeline = DebugRenderPipeline::new(style.clone(), mode); + let mut backend = LineCollector::default(); + pipeline.render_soft_bodies(&mut backend, &world.soft_bodies); + backend.lines + }; + // The lines come out sorted by particle pair: edge 0 first. + let plain = render(DebugRenderMode::SOFT_BODIES); + assert_eq!(plain.len(), 2); + assert!(plain.iter().all(|(.., c)| *c == style.soft_body_element_color)); + + let by_load = render(DebugRenderMode::SOFT_BODIES | DebugRenderMode::SOFT_BODY_STRESS); + assert_eq!(by_load.len(), 2); + let (slack, full) = (style.soft_body_slack_color, style.soft_body_loaded_color); + let expected: DebugColor = core::array::from_fn(|k| slack[k] + (full[k] - slack[k]) * loaded); + for k in 0..4 { + assert!((by_load[0].2[k] - expected[k]).abs() < 1.0e-4, "loaded edge color {:?}", by_load[0].2); + } + assert_eq!(by_load[1].2, slack); +} diff --git a/crates/rapier3d/tests/contact_force_event_first_tick.rs b/crates/rapier3d/tests/contact_force_event_first_tick.rs index c8637a5dd..2cb624374 100644 --- a/crates/rapier3d/tests/contact_force_event_first_tick.rs +++ b/crates/rapier3d/tests/contact_force_event_first_tick.rs @@ -47,6 +47,8 @@ impl EventHandler for Events { .unwrap() .push((*self.step.lock().unwrap(), event.started)); } + + fn handle_soft_body_tear_event(&self, _: &SoftBodySet, _: &SoftBodyTearEvent) {} } #[test] diff --git a/crates/rapier3d/tests/issue_253_unsync_callbacks.rs b/crates/rapier3d/tests/issue_253_unsync_callbacks.rs index 594453db4..4194986ed 100644 --- a/crates/rapier3d/tests/issue_253_unsync_callbacks.rs +++ b/crates/rapier3d/tests/issue_253_unsync_callbacks.rs @@ -32,6 +32,8 @@ impl EventHandler for UnsyncEventCollector { _total_force_magnitude: Real, ) { } + + fn handle_soft_body_tear_event(&self, _soft_bodies: &SoftBodySet, _event: &SoftBodyTearEvent) {} } #[test] diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index 37ab9e056..62e30c8f7 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -2054,6 +2054,10 @@ fn torn_soft_bodies_serialize() { }) .map(|(i, _)| i as u32) .collect(); + assert!( + world.tear_soft_body(handle, &diagonal, &[]) + .is_some() + ); for _ in 0..30 { world.step(); } @@ -3695,3 +3699,136 @@ fn fem_cloth_tears_when_pulled_apart() { "the topology version was not bumped" ); } + +/// A cut along a triangle blade follows the cells' facets: a blade through a tetrahedralized box, +/// between two of its particle layers, splits the particles of the edges it meets, every cell +/// going whole to the side of its centroid, and leaves two pieces without removing any cell. +#[test] +fn cut_splits_a_box_along_a_triangle() { + let mut world = world_with_ground(); + let cuboid = SoftBodyBuilder::cuboid( + Vector::new(0.0, 2.0, 0.0), + Vector::new(1.0, 0.5, 0.5), + 5, + 3, + 3, + ) + .particle_mass(0.2); + let handle = world.insert_soft_body(cuboid); + world.step(); + // A large triangle in the plane x = 0.2, between the particle layers x = 0 and x = 0.5 and + // away from the cell centroids (x = 0.125, 0.25 or 0.375 there). + let blade = [ + Vector::new(0.2, -10.0, -10.0), + Vector::new(0.2, 20.0, -10.0), + Vector::new(0.2, -10.0, 20.0), + ]; + let aside = blade.map(|v| v + Vector::X * 5.0); + assert!( + world.cut_soft_body(handle, &aside).is_none(), + "a blade beside the body cut something" + ); + let event = world + .cut_soft_body(handle, &blade) + .expect("the blade crossed the box"); + assert!(!event.torn_cells.is_empty() && !event.split_particles.is_empty()); + assert_eq!(event.pieces.len(), 2); + // The two pieces are bodies of their own; every cell of a piece has its centroid on one + // side of the blade. + let bodies: Vec = event.bodies().collect(); + let mut sides = Vec::new(); + let (mut cells_after, mut mass_after, mut measure_after) = (0, 0.0, 0.0); + for &h in &bodies { + let sb = &world.soft_bodies[h]; + sb.validate_topology().unwrap(); + assert_eq!(sb.connected_pieces().len(), 1); + cells_after += sb.cells().len(); + mass_after += sb.mass(); + measure_after += sb.rest_measure(); + let mut side = None; + for c in sb.cells() { + let centroid: Vector = c + .vertices + .iter() + .map(|&v| sb.particle_position(v as usize)) + .sum::() + / 4.0; + assert_eq!(*side.get_or_insert(centroid.x < 0.2), centroid.x < 0.2); + } + sides.push(side.unwrap()); + } + assert_eq!(cells_after, num_cells); + assert!((mass_after - mass).abs() < 1.0e-4 && (measure_after - measure).abs() < 1.0e-5); + assert_ne!(sides[0], sides[1]); + for &h in &bodies { + assert!( + world.cut_soft_body(h, &blade).is_none(), + "the same cut twice changed something" + ); + } + for _ in 0..60 { + world.step(); +} + +/// The contact-modification hook sees the contacts between two soft surfaces (their +/// candidates) and the soft-body solver follows its edits: a hook disabling every candidate +/// between the two lets a jelly fall through a pinned cloth it otherwise rests on. +#[test] +fn modify_solver_contacts_edits_soft_soft_contacts() { + struct DropSoftSoft; + impl PhysicsHooks for DropSoftSoft { + fn modify_solver_contacts(&self, context: &mut ContactModificationContext) { + let soft = |h: ColliderHandle| context.colliders[h].user_data == 7; + if soft(context.collider1) && soft(context.collider2) { + // Two soft surfaces: their contacts are candidates, not solver contacts. + if let Some(candidates) = context.soft_mut() { + candidates.disable_all(); + } + } + } + } + let scene = |hooked: bool| { + let mut world = world_with_ground(); + let n = 12; + let pinned: Vec = (0..(n * n) as u32).collect(); + let template = ColliderBuilder::ball(0.04) + .user_data(7) + .active_hooks(if hooked { + ActiveHooks::MODIFY_SOLVER_CONTACTS + } else { + ActiveHooks::empty() + }); + world.insert_soft_body( + SoftBodyBuilder::cloth( + Vector::new(-0.55, 1.0, -0.55), + Vector::X * 0.1, + Vector::Z * 0.1, + n, + n, + ) + .pinned_particles(pinned) + .particle_mass(0.05) + .surface_collider(template.clone()), + ); + let jelly = world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(0.0, 1.5, 0.0), Vector::splat(0.25), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .particle_mass(0.05) + .particle_radius(0.04) + .surface_collider(template), + ); + for _ in 0..240 { + world.step_with_events(&DropSoftSoft, &()); + } + assert_finite(&world, jelly); + let sb = &world.soft_bodies[jelly]; + (0..sb.num_particles()) + .map(|i| sb.particle_position(i).y) + .sum::() + / sb.num_particles() as Real + }; + let resting = scene(false); + let dropped = scene(true); + assert!(resting > 1.1, "the jelly did not rest on the cloth: y = {resting}"); + assert!(dropped < 0.5, "the hook did not drop the soft-soft contacts: y = {dropped}"); +} diff --git a/crates/rapier3d/tests/soft_body_clusters.rs b/crates/rapier3d/tests/soft_body_clusters.rs index ef5a61654..1696391ad 100644 --- a/crates/rapier3d/tests/soft_body_clusters.rs +++ b/crates/rapier3d/tests/soft_body_clusters.rs @@ -251,7 +251,7 @@ fn tear_duplicates_join_their_source_clusters() { let before = world.soft_bodies[h].num_particles(); // Tear a middle edge immediately. let torn = world.tear_soft_body(h, &[30], &[]); - assert!(torn); + assert!(torn.is_some()); let sb = &world.soft_bodies[h]; let after = sb.num_particles(); // Whatever was duplicated must be covered by cluster 0 (refcounts stay consistent). diff --git a/crates/rapier3d/tests/soft_body_joints.rs b/crates/rapier3d/tests/soft_body_joints.rs index ba8189d4d..4b4118f7c 100644 --- a/crates/rapier3d/tests/soft_body_joints.rs +++ b/crates/rapier3d/tests/soft_body_joints.rs @@ -391,7 +391,7 @@ fn tearing_keeps_cluster_joints_alive() { world.step(); // Tear a couple of middle edges. let torn = world.tear_soft_body(h, &[28, 30], &[]); - assert!(torn); + assert!(torn.is_some()); world.soft_bodies[h].validate_topology().unwrap(); for _ in 0..60 { world.step(); diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 2accc00c1..f1835a25a 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -58,7 +58,9 @@ mod s2d_pyramid; mod sensor2; mod soft_blobs2; mod soft_bodies2; +mod soft_cutting2; mod soft_fem2; +mod soft_force_tearing2; mod soft_jelly2; mod soft_joints2; // The letters come from a tessellated SVG (usvg), which doesn't build for wasm. @@ -66,6 +68,7 @@ mod soft_joints2; mod soft_letters2; mod soft_pile2; mod soft_plasticity2; +mod soft_stress2; mod soft_surface2; mod soft_tearing2; mod soft_thin_features2; @@ -148,6 +151,9 @@ pub async fn main() { SOFT, "Soft letters", soft_letters2::run; SOFT, "Plasticity", soft_plasticity2::run; SOFT, "Tearing", soft_tearing2::run; + SOFT, "Force tearing", soft_force_tearing2::run; + SOFT, "Cutting", soft_cutting2::run; + SOFT, "Stress coloring", soft_stress2::run; SOFT, "Soft FEM", soft_fem2::run; SOFT, "Soft joints", soft_joints2::run; // ── Controls ──────────────────────────────────────────────────────── diff --git a/examples2d/ccd2.rs b/examples2d/ccd2.rs index 8656254c8..6607adbbb 100644 --- a/examples2d/ccd2.rs +++ b/examples2d/ccd2.rs @@ -88,7 +88,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let (collision_send, collision_recv) = std::sync::mpsc::channel(); let (contact_force_send, _contact_force_recv) = std::sync::mpsc::channel(); - let event_handler = ChannelEventCollector::new(collision_send, contact_force_send); + let (soft_body_tear_send, _soft_body_tear_recv) = std::sync::mpsc::channel(); + let event_handler = + ChannelEventCollector::new(collision_send, contact_force_send, soft_body_tear_send); while viewer.render_frame(&mut world).await { if viewer.simulating() { diff --git a/examples2d/sensor2.rs b/examples2d/sensor2.rs index 3bbe2bd6a..694df3c0a 100644 --- a/examples2d/sensor2.rs +++ b/examples2d/sensor2.rs @@ -67,7 +67,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { viewer.set_world(&mut world); let (collision_send, collision_recv) = std::sync::mpsc::channel(); let (contact_force_send, _contact_force_recv) = std::sync::mpsc::channel(); - let event_handler = ChannelEventCollector::new(collision_send, contact_force_send); + let (soft_body_tear_send, _soft_body_tear_recv) = std::sync::mpsc::channel(); + let event_handler = + ChannelEventCollector::new(collision_send, contact_force_send, soft_body_tear_send); viewer.look_at(Vec2::new(0.0, 1.0), 100.0); while viewer.render_frame(&mut world).await { diff --git a/examples2d/soft_cutting2.rs b/examples2d/soft_cutting2.rs new file mode 100644 index 000000000..811ec892e --- /dev/null +++ b/examples2d/soft_cutting2.rs @@ -0,0 +1,123 @@ +//! Cutting soft bodies (`PhysicsWorld::cut_soft_body`): hold `C`, move the mouse, release `C` to +//! cut every soft body along the line from key-down to key-up; a saw blade also rises through the +//! suspended slab. Cuts remove no material and separated pieces become soft bodies. + +use kiss3d::color::Color; +use rapier2d::prelude::*; +use rapier_testbed2d::{Key, TestbedViewer}; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(30.0, 0.5), + ); + + let jelly = |young: Real| SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.35, + elastic_damping_ratio: 1.0, + ..Default::default() + }; + + /* + * A jelly block to slice by hand. + */ + let block = SoftBodyBuilder::grid(Vector::new(-8.0, 2.0), Vector::new(2.0, 2.0), 13, 13) + .cell_model(SoftBodyCellModel::Corotational) + .material(jelly(2.0e4)) + .particle_mass(0.05) + .particle_radius(0.15) + .surface_collider(ColliderBuilder::ball(0.15).friction(0.8)); + world.insert_soft_body(block); + + /* + * A blob: a ring of particles holding its area. Cut open, it falls limp. + */ + let blob = SoftBodyBuilder::disk(Vector::new(-2.0, 2.0), 1.6, 40) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + world.insert_soft_body(blob); + + /* + * A curtain pinned along its top edge. + */ + let (cx, cy) = (9usize, 31usize); + let cidx = |i: usize, j: usize| (i * cy + j) as u32; + let curtain = SoftBodyBuilder::grid(Vector::new(3.0, 4.5), Vector::new(0.6, 3.0), cx, cy) + .cell_model(SoftBodyCellModel::Corotational) + .material(jelly(3.0e4)) + .pinned_particles((0..cx).map(|i| cidx(i, cy - 1))) + .particle_mass(0.05) + .particle_radius(0.1) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + world.insert_soft_body(curtain); + + /* + * A slab suspended between two posts, with a saw rising through it. + */ + let (sx, sy) = (25usize, 5usize); + let sidx = |i: usize, j: usize| (i * sy + j) as u32; + let slab = SoftBodyBuilder::grid(Vector::new(10.0, 4.0), Vector::new(3.0, 0.5), sx, sy) + .cell_model(SoftBodyCellModel::Corotational) + .material(jelly(5.0e4)) + .pinned_particles((0..sy).flat_map(|j| [sidx(0, j), sidx(sx - 1, j)])) + .particle_mass(0.05) + .particle_radius(0.1) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + world.insert_soft_body(slab); + // The saw: a kinematic blade drawn as a thin plate, a sensor so it pushes nothing (the + // cut does the work). + let saw_start = Vector::new(10.0, 1.5); + let saw = world.insert_body( + RigidBodyBuilder::kinematic_position_based().translation(saw_start), + ); + world.insert_collider(ColliderBuilder::cuboid(0.05, 1.0).sensor(true), Some(saw)); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(1.0, 3.5), 40.0); + + let handles: Vec = world.soft_bodies().map(|(h, _)| h).collect(); + let mut blade_start: Option = None; + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + // The hand blade: from where `C` went down to the cursor. + let cursor = viewer.mouse().point.map(|p| Vector::new(p.x, p.y)); + if viewer.keys().pressed(Key::C) { + if blade_start.is_none() { + blade_start = cursor; + } + if let (Some(a), Some(b)) = (blade_start, cursor) { + viewer.window_mut().draw_line_2d( + Vec2::new(a.x, a.y), + Vec2::new(b.x, b.y), + Color::new(1.0, 0.35, 0.25, 1.0), + 3.0, + ); + } + } else if let (Some(a), Some(b)) = (blade_start.take(), cursor) { + for &handle in &handles { + world.cut_soft_body(handle, &[a, b]); + } + } + + if viewer.simulating() { + t += world.integration_parameters.dt; + // The saw rises at a fifth of a meter per second, from a second in. + let rise = ((t - 1.0).max(0.0) * 0.2).min(4.5); + let position = saw_start + Vector::new(0.0, rise); + world.bodies[saw].set_next_kinematic_translation(position); + let edge = [position - Vector::Y, position + Vector::Y]; + for &handle in &handles { + world.cut_soft_body(handle, &edge); + } + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/soft_force_tearing2.rs b/examples2d/soft_force_tearing2.rs new file mode 100644 index 000000000..ffbff2229 --- /dev/null +++ b/examples2d/soft_force_tearing2.rs @@ -0,0 +1,188 @@ +//! Force-based tearing (`SoftBodyMaterial::tear_force`), interior strength and tear smoothing: +//! bars hanging a crate (the strain criterion never fires, the force one tears), a notched slab +//! pulled apart with a tougher interior, and crate bars struck by a disk, one of them smoothed. + +use rapier2d::pipeline::DebugRenderMode; +use rapier2d::prelude::*; +use rapier_testbed2d::TestbedViewer; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let settings = viewer.example_settings_mut(); + let tear_force = settings.get_or_set_f32("Tear force (crate bar)", 8.0, 2.0..=30.0); + let interior_strength = settings.get_or_set_f32("Interior strength (slab)", 4.0, 1.0..=8.0); + let smoothing = settings.get_or_set_f32("Tear smoothing (right bar, s)", 1.0, 0.0..=3.0); + + let mut world = PhysicsWorld::new(); + + /* + * Ground and the beam everything hangs from. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(30.0, 0.5), + ); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, 9.3)), + ColliderBuilder::cuboid(30.0, 0.3), + ); + + // A stiff bar: its edges stretch by a few percent under the loads below. + let stiff = |material: SoftBodyMaterial| SoftBodyMaterial { + edge_softness: SpringCoefficients::new(150.0, 1.0), + volume_softness: SpringCoefficients::new(150.0, 1.0), + ..material + }; + let (nx, ny) = (3usize, 13usize); + let idx = |i: usize, j: usize| (i * ny + j) as u32; + let hanging_bar = |x: Real, material: SoftBodyMaterial| { + SoftBodyBuilder::grid(Vector::new(x, 7.5), Vector::new(0.3, 1.5), nx, ny) + .material(material) + .pinned_particles((0..nx).map(|i| idx(i, ny - 1))) + .particle_mass(0.05) + .particle_radius(0.1) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)) + }; + + /* + * Strain vs force: the same crate hung from a strain-tearing bar and a force-tearing bar; + * it loads the top edges with about 11 N, which stretch by 4%. + */ + for (x, material) in [ + ( + -13.0, + stiff(SoftBodyMaterial { + tear_strain: Some(0.5), + ..Default::default() + }), + ), + ( + -10.0, + stiff(SoftBodyMaterial { + tear_force: Some(tear_force), + tear_smoothing: 0.5, + ..Default::default() + }), + ), + ] { + let bar = world.insert_soft_body(hanging_bar(x, material)); + let (crate_body, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(x, 5.4)), + ColliderBuilder::cuboid(0.6, 0.5).density(1.2), + ); + for i in 0..nx { + world.soft_bodies[bar].attach_particle(idx(i, 0) as usize, crate_body, &world.bodies); + } + } + + /* + * Interior strength: a slab clamped over two columns at each end and pulled apart; its + * weight loads the clamps with 20 N at most, the pull reaches 250 N. + */ + let (sx, sy) = (25usize, 7usize); + let sidx = |i: usize, j: usize| (i * sy + j) as u32; + let slab = SoftBodyBuilder::grid(Vector::new(-1.0, 2.0), Vector::new(3.0, 0.75), sx, sy) + .material(SoftBodyMaterial { + edge_softness: SpringCoefficients::new(60.0, 1.0), + volume_softness: SpringCoefficients::new(60.0, 1.0), + tear_force: Some(60.0), + tear_smoothing: 0.05, + interior_strength, + ..Default::default() + }) + .pinned_particles( + (0..sy).flat_map(|j| [sidx(0, j), sidx(1, j), sidx(sx - 2, j), sidx(sx - 1, j)]), + ) + .particle_mass(0.05) + .particle_radius(0.1) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + let slab = world.insert_soft_body(slab); + // The notch: the top-skin edges of three middle columns give at 40% of the threshold. + { + let sb = &mut world.soft_bodies[slab]; + let top = |p: u32| p % sy as u32 == sy as u32 - 1; + let middle = |p: u32| (11..=13).contains(&(p / sy as u32)); + let notched: Vec = sb + .edges() + .iter() + .enumerate() + .filter(|(_, e)| e.vertices.iter().all(|&p| top(p) && middle(p))) + .map(|(i, _)| i) + .collect(); + for i in notched { + sb.set_edge_tear_resistance(i, 0.4); + } + } + let mut right_end: Vec<((SoftBodyHandle, u32), Vector)> = (0..sy) + .flat_map(|j| [sidx(sx - 2, j), sidx(sx - 1, j)]) + .map(|i| ((slab, i), world.soft_bodies[slab].particle_position(i as usize))) + .collect(); + + /* + * Smoothing: two identical bars hanging a wide crate (15 N steady load on their top edges, + * threshold 25 N), each struck by a light disk falling on the crate; the jerk loads the + * edges with over 60 N for a few steps. + */ + let shot_bar = |smoothing: Real| { + stiff(SoftBodyMaterial { + tear_force: Some(25.0), + tear_smoothing: smoothing, + ..Default::default() + }) + }; + let mut disks = Vec::new(); + for (x, smoothing) in [(8.0, 0.0), (13.0, smoothing)] { + let bar = world.insert_soft_body(hanging_bar(x, shot_bar(smoothing))); + let (crate_body, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(x, 5.4)), + ColliderBuilder::cuboid(1.2, 0.4).density(0.75), + ); + for i in 0..nx { + world.soft_bodies[bar].attach_particle(idx(i, 0) as usize, crate_body, &world.bodies); + } + // Held under the beam until the bars hang still. + let (disk, _) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(x + 0.8, 8.6)) + .enabled(false), + ColliderBuilder::ball(0.2).density(3.0), + ); + disks.push(disk); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 4.5), 40.0); + viewer.set_debug_render( + true, + DebugRenderMode::SOFT_BODIES | DebugRenderMode::SOFT_BODY_STRESS, + ); + + let mut t: Real = 0.0; + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + let dt = world.integration_parameters.dt; + // The disks are shot down at the crates once the bars hang still. + if t < 1.0 && t + dt >= 1.0 { + for &disk in &disks { + let disk = &mut world.bodies[disk]; + disk.set_enabled(true); + disk.set_linvel(Vector::new(0.0, -15.0), true); + } + } + t += dt; + // The slab's right clamp starts moving after two seconds, at a quarter meter per + // second, and stops once the slab has stretched by half. + let shift = ((t - 2.0).max(0.0) * 0.25).min(3.0); + for &((body, i), rest) in &right_end { + world.soft_bodies[body] + .set_particle_kinematic_target(i as usize, rest + Vector::new(shift, 0.0)); + } + world.step_with_events(&(), &events); + let tears: Vec = tear_recv.try_iter().collect(); + for (particle, _) in &mut right_end { + follow_tears(&tears, particle); + } + } + } + Ok(()) +} + diff --git a/examples2d/soft_stress2.rs b/examples2d/soft_stress2.rs new file mode 100644 index 000000000..d793a82f2 --- /dev/null +++ b/examples2d/soft_stress2.rs @@ -0,0 +1,110 @@ +//! Stress coloring (`DebugRenderMode::SOFT_BODY_STRESS`): elements are colored by their smoothed +//! `SoftBodyEdge::stress`, blue when slack, red at the tear threshold; nothing tears. Scenes: a +//! bridge, a bar and a block, each under a crate, and a blob colored by its stretch instead. + +use rapier2d::pipeline::DebugRenderMode; +use rapier2d::prelude::*; +use rapier_testbed2d::TestbedViewer; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = PhysicsWorld::new(); + + /* + * Ground and the beam the bar hangs from. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(30.0, 0.5), + ); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(8.0, 9.3)), + ColliderBuilder::cuboid(3.0, 0.3), + ); + + let stiff = |tear_force: Real| SoftBodyMaterial { + edge_softness: SpringCoefficients::new(100.0, 1.0), + volume_softness: SpringCoefficients::new(100.0, 1.0), + tear_force: Some(tear_force), + // Averaged over half a second, so the crates landing do not tear anything. + tear_smoothing: 0.5, + ..Default::default() + }; + + /* + * The bridge and its crate. + */ + let (bx, by) = (31usize, 5usize); + let bidx = |i: usize, j: usize| (i * by + j) as u32; + let bridge = SoftBodyBuilder::grid(Vector::new(-7.0, 3.0), Vector::new(4.5, 0.6), bx, by) + .material(stiff(65.0)) + .pinned_particles((0..by).flat_map(|j| [bidx(0, j), bidx(bx - 1, j)])) + .particle_mass(0.05) + .particle_radius(0.15) + .surface_collider(ColliderBuilder::ball(0.15).friction(0.8)); + world.insert_soft_body(bridge); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(-7.0, 4.4)), + ColliderBuilder::cuboid(0.6, 0.6).density(1.5), + ); + + /* + * The hanging bar and its crate. + */ + let (hx, hy) = (3usize, 17usize); + let hidx = |i: usize, j: usize| (i * hy + j) as u32; + let bar = SoftBodyBuilder::grid(Vector::new(8.0, 6.8), Vector::new(0.3, 2.2), hx, hy) + .material(stiff(16.0)) + .pinned_particles((0..hx).map(|i| hidx(i, hy - 1))) + .particle_mass(0.05) + .particle_radius(0.1) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + let bar = world.insert_soft_body(bar); + let (crate_body, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(8.0, 4.0)), + ColliderBuilder::cuboid(0.6, 0.5).density(1.0), + ); + for i in 0..hx { + world.soft_bodies[bar].attach_particle(hidx(i, 0) as usize, crate_body, &world.bodies); + } + + /* + * The block under a crate. + */ + let block = SoftBodyBuilder::grid(Vector::new(2.0, 1.2), Vector::new(1.2, 1.2), 9, 9) + .material(stiff(10.0)) + .particle_mass(0.05) + .particle_radius(0.15) + .surface_collider(ColliderBuilder::ball(0.15).friction(0.8)); + world.insert_soft_body(block); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(2.0, 3.2)), + ColliderBuilder::cuboid(0.8, 0.6).density(2.0), + ); + + /* + * A blob without tear threshold: colored by its stretch. + */ + let blob = SoftBodyBuilder::disk(Vector::new(13.0, 1.6), 1.5, 36) + .softness(SpringCoefficients::new(8.0, 1.0)) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + world.insert_soft_body(blob); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(13.0, 4.0)), + ColliderBuilder::cuboid(0.6, 0.4).density(1.0), + ); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(2.0, 4.0), 40.0); + viewer.set_debug_render( + true, + DebugRenderMode::SOFT_BODIES | DebugRenderMode::SOFT_BODY_STRESS, + ); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/ccd3.rs b/examples3d/ccd3.rs index 20243e724..9a3171501 100644 --- a/examples3d/ccd3.rs +++ b/examples3d/ccd3.rs @@ -109,7 +109,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { viewer.set_world(&mut world); let (collision_send, collision_recv) = std::sync::mpsc::channel(); let (contact_force_send, _contact_force_recv) = std::sync::mpsc::channel(); - let event_handler = ChannelEventCollector::new(collision_send, contact_force_send); + let (soft_body_tear_send, _soft_body_tear_recv) = std::sync::mpsc::channel(); + let event_handler = + ChannelEventCollector::new(collision_send, contact_force_send, soft_body_tear_send); viewer.look_at(Vec3::new(100.0, 100.0, 100.0), Vec3::ZERO); while viewer.render_frame(&mut world).await { diff --git a/examples3d/sensor3.rs b/examples3d/sensor3.rs index 257180d2e..99f2fe5c9 100644 --- a/examples3d/sensor3.rs +++ b/examples3d/sensor3.rs @@ -71,7 +71,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { viewer.set_world(&mut world); let (collision_send, collision_recv) = std::sync::mpsc::channel(); let (contact_force_send, _contact_force_recv) = std::sync::mpsc::channel(); - let event_handler = ChannelEventCollector::new(collision_send, contact_force_send); + let (soft_body_tear_send, _soft_body_tear_recv) = std::sync::mpsc::channel(); + let event_handler = + ChannelEventCollector::new(collision_send, contact_force_send, soft_body_tear_send); viewer.look_at(Vec3::new(6.0, 4.0, 6.0), Vec3::new(0.0, 1.0, 0.0)); while viewer.render_frame(&mut world).await { diff --git a/src/dynamics/soft_body/mod.rs b/src/dynamics/soft_body/mod.rs index 2aeff1505..d6f3ece67 100644 --- a/src/dynamics/soft_body/mod.rs +++ b/src/dynamics/soft_body/mod.rs @@ -53,3 +53,4 @@ mod soft_body_plasticity; mod soft_body_set; pub(crate) mod soft_body_shape_matching; mod tearing; +pub use self::tearing::{SoftBodyPiece, SoftBodyTearEvent, SoftClusterSplit, SoftJointMove}; diff --git a/src/dynamics/soft_body/soft_body_motion.rs b/src/dynamics/soft_body/soft_body_motion.rs index 110ed61d1..3e1773d87 100644 --- a/src/dynamics/soft_body/soft_body_motion.rs +++ b/src/dynamics/soft_body/soft_body_motion.rs @@ -137,4 +137,64 @@ impl SoftBody { } } + /// Applies `impulse` to every free particle within `falloff_radius` of world point `point`, + /// scaled linearly to zero at that radius and divided by the particle's mass; a + /// `falloff_radius` of zero or less gives every particle the whole impulse. + pub fn apply_impulse_at_point( + &mut self, + impulse: Vector, + point: Vector, + falloff_radius: Real, + wake_up: bool, + ) { + for p in &mut self.particles { + if p.inv_mass == 0.0 { + continue; + } + let scale = if falloff_radius > 0.0 { + 1.0 - (p.position - point).length() / falloff_radius + } else { + 1.0 + }; + if scale > 0.0 { + p.velocity += impulse * (scale * p.inv_mass); + } + } + if wake_up { + self.wake_up(); + } + } + + /// Applies an impulse of `magnitude` pointing away from world point `center` to every free + /// particle within `falloff_radius`, scaled linearly to zero at that radius (a particle on the + /// center gets nothing); a `falloff_radius` of zero or less pushes every particle fully. + pub fn apply_radial_impulse( + &mut self, + center: Vector, + magnitude: Real, + falloff_radius: Real, + wake_up: bool, + ) { + for p in &mut self.particles { + if p.inv_mass == 0.0 { + continue; + } + let offset = p.position - center; + let distance = offset.length(); + if distance <= Real::EPSILON { + continue; + } + let scale = if falloff_radius > 0.0 { + 1.0 - distance / falloff_radius + } else { + 1.0 + }; + if scale > 0.0 { + p.velocity += offset * (magnitude * scale * p.inv_mass / distance); + } + } + if wake_up { + self.wake_up(); + } + } } 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 399ba0e0e..4e94e930f 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 @@ -1,14 +1,382 @@ - /// refreshes the attachment index and island links of the bodies whose attachments changed. +//! Per-step maintenance of the set: applying user changes, updating proxies and colliders, syncing particle positions, tearing and cutting. +use crate::alloc_prelude::*; +use crate::dynamics::{ + ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, RigidBodySet, +}; +use crate::geometry::ColliderSet; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; +use super::soft_body_set_proxies::{rebuilt_surface_shape, sync_soft_body}; +use crate::dynamics::soft_body::{SoftBody, SoftBodyHandle, SoftBodyTearEvent}; +use super::{SoftBodyIslandEvent, SoftBodySet}; +use crate::pipeline::EventHandler; +use crate::math::{DIM, Pose, Real, Vector}; + +impl SoftBodySet { + /// Applies the user's changes since the last step: wakes the soft bodies whose settings + /// changed, enables or disables their root body, moves the colliders of moved particles, and + /// updates attachment index and island links. A non-finite particle quarantines its body. + pub(crate) fn apply_user_changes( + &mut self, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + params: &IntegrationParameters, + quarantined: &mut Vec, + ) { + let mut relink = false; + let handles: Vec = self + .bodies + .iter() + .filter(|(_, sb)| sb.modified || sb.positions_modified || sb.attachments_modified) + .map(|(h, _)| SoftBodyHandle(h)) + .collect(); + for handle in handles { + let sb = &mut self.bodies[handle.0]; + if core::mem::take(&mut sb.modified) { + if sb.enabled && !sb.is_finite() { + sb.sanitize_dynamics(); + sb.enabled = false; + quarantined.push(handle); + } + if let Some(rb) = bodies.get_mut(sb.root_body) { + if rb.is_enabled() != sb.enabled { + rb.set_enabled(sb.enabled); + } + if sb.enabled { + rb.wake_up(true); + } + } + } + if core::mem::take(&mut sb.positions_modified) && sb.enabled { + Self::update_colliders(sb, bodies, colliders, false); + for mesh in sb.meshes_mut() { + // A teleport can author a tangle with no accumulated travel: the next + // self-crossing sweep must run. + mesh.crossing_sweep_travel = Real::MAX; + } + } + // Velocities authored between steps outrun the speculative margin computed at the + // end of the last step, so the margin is raised here to cover the coming step's reach. + if sb.enabled && params.soft_bodies.recovery.authored_velocity_margin { + let mut max_speed: Real = 0.0; + for p in &sb.particles { + max_speed = max_speed.max(p.velocity.length()); + } + let margin = max_speed * params.dt; + if margin > 0.0 { + for cluster in sb.clusters.iter().filter(|cluster| cluster.is_live()) { + 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. - /// Refreshes every live cluster's proxy rigid body from the current particles: its pose is - // Dead zone: the frame fit carries a little noise even at rest (the least-squares - // so a one-refresh-stale proxy pose is fine. - /// Refreshes every soft body's derived state at the end of a step: the surface orientation, + for mesh in cluster.meshes() { + let _ = colliders.get_mut(mesh.collider()); + } + } + } + } + if core::mem::take(&mut sb.attachments_modified) { + relink = true; + self.island_events.push(SoftBodyIslandEvent { handle }); + } + } + if relink || self.attached_to_stale { + self.attached_to_stale = false; + self.attached_to.clear(); + for (h, sb) in self.bodies.iter() { + for (i, a) in sb.attachments.iter().enumerate() { + self.attached_to + .entry(a.body) + .or_default() + .push((SoftBodyHandle(h), i as u32)); + } + } + } + } + + /// Updates every live cluster's proxy rigid body from the particles: pose from the cluster's + /// frame, velocity from its rigid fit, mass properties from its reduced mass matrix. The + /// proxies' colliders keep their world pose (local pose re-based) until the frame-local phase. + pub(crate) fn update_cluster_proxies( + sb: &mut SoftBody, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) { + let _ = colliders; + for ci in 0..sb.clusters.len() { + let Some(mut frame) = sb.cluster_frame(ci) else { + continue; + }; + if !frame.pose.is_finite() || !frame.linvel.is_finite() || !frame.angvel.is_finite() { + // A non-finite fit means non-finite particles: the quarantine handles the body; + // the proxy keeps its previous (finite) state meanwhile. + continue; + } + let proxy = sb.clusters[ci].proxy; + let Some(rb) = bodies.get_mut_internal(proxy) else { + continue; + }; + // Dead zone: a pose change below the frame fit's rest noise keeps the previous pose + // 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(); + #[cfg(feature = "dim2")] + let dr = frame.pose.rotation.angle_between(&prev.rotation); + #[cfg(feature = "dim3")] + let dr = frame.pose.rotation.angle_between(prev.rotation); + if dt < 1.0e-6 && dr < 1.0e-6 { + frame.pose = prev; + } + sb.clusters[ci].rotation = frame.pose.rotation; + sb.clusters[ci].last_gather = (frame.linvel, frame.angvel); + rb.pos.position = frame.pose; + rb.pos.next_position = frame.pose; + rb.vels.linvel = frame.linvel; + rb.vels.angvel = frame.angvel; + rb.mprops.local_mprops.inv_mass = frame.inv_mass; + rb.mprops.local_mprops.local_com = Vector::ZERO; + rb.mprops.world_com = frame.pose.translation; + rb.mprops.effective_inv_mass = Vector::splat(frame.inv_mass); + rb.mprops.effective_world_inv_inertia = frame.inv_inertia; + } + } + + /// Moves the soft body's colliders to its current particle positions (the surface shape, or + /// the particle balls). `rebuild`: the surface topology changed (a tear), the surface shape is + /// rebuilt instead of deformed in place. + pub(super) fn update_colliders( + sb: &SoftBody, + bodies: &RigidBodySet, + colliders: &mut ColliderSet, + rebuild: bool, + ) { + // The mesh geometry is expressed in its collider's frame (proxy pose composed with the + // collider's local pose); any shared frame works, so a proxy pose one update stale is fine. + for cluster in sb.clusters.iter().filter(|cluster| cluster.is_live()) { + let frame = bodies + .get(cluster.proxy()) + .map(|rb| rb.pos.position) + .unwrap_or(Pose::IDENTITY); + for mesh in cluster.meshes() { + let Some(co) = colliders.get_mut(mesh.collider()) else { + continue; + }; + co.deform_pose(frame); + let pose = *co.position(); + if rebuild { + match rebuilt_surface_shape( + co.shape(), + mesh.local_vertices(sb, &pose), + mesh.indices(), + ) { + Some(shape) => co.replace_deformed_shape(shape), + None => co.set_enabled(false), + } + } else { + co.deform_shape(|shape| mesh.deform_shape(sb, &pose, shape)); + } + } + } + } + + /// Updates each soft body's derived state at the end of a step: surface orientation, sleep + /// state, colliders and particle balls of the awake ones, the coming step's speculative margin + /// and the impact-adaptive substep request. A body with a non-finite particle is quarantined. + pub fn sync_particle_positions( + &mut self, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + params: &IntegrationParameters, + quarantined: &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 + // alone, like sleeping ones. + let mut handles = Vec::with_capacity(self.bodies.len()); + let mut soft_bodies: Vec<(&mut SoftBody, bool)> = self + .bodies + .iter_mut() + .map(|(h, sb)| { + handles.push(SoftBodyHandle(h)); + let inactive = bodies + .get(sb.root_body) + .is_none_or(|rb| rb.is_sleeping() || !rb.is_enabled()); + (sb, inactive) + }) + .collect(); + let mut outcomes = Vec::with_capacity(soft_bodies.len()); + let sync = + |(sb, inactive): &mut (&mut SoftBody, bool)| sync_soft_body(sb, *inactive, params); + #[cfg(feature = "parallel")] + { + use rayon::prelude::*; + soft_bodies + .par_iter_mut() + .map(sync) + .collect_into_vec(&mut outcomes); + } + #[cfg(not(feature = "parallel"))] + outcomes.extend(soft_bodies.iter_mut().map(sync)); + + for (((sb, _), outcome), handle) in soft_bodies.iter_mut().zip(outcomes).zip(handles) { + let Some(margin) = outcome.margin else { + continue; + }; + if !outcome.finite { + // Non-finite state: contained here, before it reaches the colliders. + sb.sanitize_dynamics(); + sb.enabled = false; + if let Some(rb) = bodies.get_mut(sb.root_body) { + rb.set_enabled(false); + } + quarantined.push(handle); + continue; + } + let Some(rb) = bodies.get_mut_internal(sb.root_body) else { + continue; + }; + rb.additional_solver_iterations = outcome.additional_solver_iterations; + // The cluster proxies first (pose, velocity, reduced mass): the colliders are + // expressed in the fresh whole-body frame. + Self::update_cluster_proxies(sb, bodies, colliders); + for cluster in sb.clusters.iter().filter(|cluster| cluster.is_live()) { + let frame = bodies + .get(cluster.proxy()) + .map(|rb| rb.pos.position) + .unwrap_or(Pose::IDENTITY); + for mesh in cluster.meshes() { + if let Some(co) = colliders.get_mut(mesh.collider()) { + // Deformed in place, in the collider's frame: parry refits the BVH. + co.deform_pose(frame); + let pose = *co.position(); + co.deform_shape(|shape| mesh.deform_shape(sb, &pose, shape)); + } + } + // 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. + 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. - /// Tears a soft body right away: removes the given edges and cells and everything spanning - /// them, splits the particles the tear passes through (the velocity of the original kept, its + let proxy_colliders = rb.colliders().to_vec(); + for handle in proxy_colliders { + let _ = colliders.get_mut(handle); + } + } + } + } + + /// Applies the tear marks left by the solver (elements loaded past their tear threshold) and + /// by `SoftBody::tear_edge`/`tear_cell`, and reports each tear to `events`. Runs at the end + /// of a step, before [`Self::sync_particle_positions`]. + pub(crate) fn apply_pending_tears( + &mut self, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + events: &dyn EventHandler, + ) { + let pending: Vec = self + .bodies + .iter() + .filter(|(_, sb)| sb.tearing_pending) + .map(|(h, _)| SoftBodyHandle(h)) + .collect(); + let mut torn = Vec::new(); + for handle in pending { + let (edges, cells) = self.bodies[handle.0].take_torn(); + if let Some(event) = self.tear( + handle, + &edges, + &cells, + islands, + bodies, + colliders, + impulse_joints, + multibody_joints, + ) { + torn.push(event); + } + } + for event in &torn { + events.handle_soft_body_tear_event(self, event); + } + } + + /// Tears a soft body right away and rebuilds its colliders, losing no material: a crack opens + /// through a particle of every torn edge and cell (see [`SoftBody::tear_edge`]), disconnected + /// pieces become soft bodies, crossed clusters split. Returns the event, `None` if unchanged. + #[allow(clippy::too_many_arguments)] + pub fn tear( + &mut self, + handle: SoftBodyHandle, + edges: &[u32], + cells: &[u32], + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + ) -> Option { + let sb = self.bodies.get_mut(handle.0)?; + let mut event = SoftBodyTearEvent { + soft_body: handle, + ..Default::default() + }; + if !change(sb, &mut event) { + return None; + } + if let Some(rb) = bodies.get_mut(sb.root_body) { rb.wake_up(true); + } + Some(event) + } + + /// Cuts a soft body right away along a blade, a world segment (2D) or triangle (3D), splitting + /// the particles of the edges the blade meets (pinned ones never) and losing no material (see + /// [`SoftBody::rest_measure`]). Returns the tear event, or `None` when nothing changed. + #[allow(clippy::too_many_arguments)] + pub fn cut( + &mut self, + handle: SoftBodyHandle, + blade: &[Vector; DIM], + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + ) -> Option { + let _ = multibody_joints; + } + + /// Refreshes every mesh's vertex cache from the particles, before a narrow-phase update + /// (see `SoftCollisionMesh::cached_vertex`); in parallel under the `parallel` feature. + pub(crate) fn refresh_vertex_caches(&mut self) { + #[cfg(feature = "parallel")] + { + use rayon::prelude::*; + let mut bodies: Vec<&mut SoftBody> = self.bodies.iter_mut().map(|(_, sb)| sb).collect(); + bodies.par_iter_mut().for_each(|sb| sb.refresh_vertex_caches()); + } + #[cfg(not(feature = "parallel"))] + for (_, sb) in self.bodies.iter_mut() { + sb.refresh_vertex_caches(); + } + } + + /// Wakes up the given soft body (effective at the start of the next step). + pub fn wake_up(&mut self, handle: SoftBodyHandle, bodies: &mut RigidBodySet, strong: bool) { + if let Some(sb) = self.bodies.get_mut(handle.0) { + sb.wake_up(); + if let Some(rb) = bodies.get_mut(sb.root_body) { + rb.wake_up(strong); + } + } + } } diff --git a/src/dynamics/soft_body/tearing/mod.rs b/src/dynamics/soft_body/tearing/mod.rs new file mode 100644 index 000000000..cdf2e5157 --- /dev/null +++ b/src/dynamics/soft_body/tearing/mod.rs @@ -0,0 +1,13 @@ +//! Tearing and cutting of soft bodies: the cracks opened by duplicating the particles a tear or a +//! cut passes through, the particles a cut inserts into segments, and the surface and coloring +//! rebuild that follows. + +mod tearing; +mod tearing_event; +mod tearing_helpers; +mod tearing_particle_split; +mod tearing_tables; +mod tearing_validate; + +pub use self::tearing_event::SoftBodyTearEvent; +pub(super) use super::collision_mesh; diff --git a/src/dynamics/soft_body/tearing/tearing.rs b/src/dynamics/soft_body/tearing/tearing.rs new file mode 100644 index 000000000..1f0827e93 --- /dev/null +++ b/src/dynamics/soft_body/tearing/tearing.rs @@ -0,0 +1,200 @@ +//! The tear API of a soft body: tear marks, blade crossings, and the update of the state derived +//! from the elements after a tear or a cut. + +use crate::alloc_prelude::*; +use crate::math::{DIM, Real, Vector}; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::super::SoftBody; +use super::tearing_event::{CATASTROPHIC_STRESS, SoftBodyTearEvent}; + +/// A sorted particle-index pair. +pub(super) fn pair(a: u32, b: u32) -> [u32; 2] { + [a.min(b), a.max(b)] +} + +/// Number of vertices two faces share, the vertex `v` excluded. +pub(super) fn shared_others(a: &[u32], b: &[u32], v: u32) -> usize { + a.iter().filter(|&&x| x != v && b.contains(&x)).count() +} + +impl SoftBody { + /// Marks the `i`-th edge as torn: the tear is applied at the end of the next step (use + /// [`crate::dynamics::SoftBodySet::tear`] to tear immediately). A tear loses no material (see + /// [`Self::rest_measure`]); pieces it disconnects become their own bodies ([`Self::origin`]). + pub fn tear_edge(&mut self, i: usize) { + if let Some(e) = self.edges.get_mut(i) { + e.torn = true; + // A requested tear is never paced by `max_tears_per_step`. + e.stress = e.stress.max(CATASTROPHIC_STRESS); + self.tearing_pending = true; + } + } + + /// Marks the `i`-th cell as torn: the tear is applied at the end of the next step. No cell is + /// removed: one particle (nearest the centroid first) splits along the plane perpendicular to + /// the cell's principal rest stretch direction (see [`Self::tear_edge`]). + pub fn tear_cell(&mut self, i: usize) { + if let Some(c) = self.cells.get_mut(i) { + c.torn = true; + self.tearing_pending = true; + } + } + + /// Whether some element has a pending tear mark. + pub fn has_pending_tears(&self) -> bool { + self.tearing_pending + } + + /// The elements a blade (world segment in 2D, triangle in 3D) meets, as `(edge indices, cell + /// indices)`: an edge when they intersect (endpoints included), a cell when one of its edges + /// does. A preview of [`crate::dynamics::SoftBodySet::cut`], which removes none of them. + pub fn crossing_elements(&self, blade: &[Vector; DIM]) -> (Vec, Vec) { + let position = |i: u32| self.particles[i as usize].position; + let crosses = |a: u32, b: u32| blade_hit(blade, position(a), position(b)).is_some(); + let edges = self + .edges + .iter() + .enumerate() + .filter(|(_, e)| crosses(e.vertices[0], e.vertices[1])) + .map(|(i, _)| i as u32) + .collect(); + let cells = self + .cells + .iter() + .enumerate() + .filter(|(_, c)| { + (0..DIM + 1).any(|a| (a + 1..DIM + 1).any(|b| crosses(c.vertices[a], c.vertices[b]))) + }) + .map(|(i, _)| i as u32) + .collect(); + (edges, cells) + } + + /// Takes and clears the pending tear marks as (edge, cell) indices. At most + /// `SoftBodyMaterial::max_tears_per_step` edges are taken (most loaded first, by smoothed + /// `stress`); the others lose their mark (marked again next step if still overloaded). + pub(crate) fn take_torn(&mut self) -> (Vec, Vec) { + self.tearing_pending = false; + let mut edges: Vec<(Real, u32)> = Vec::new(); + for (i, e) in self.edges.iter_mut().enumerate() { + if core::mem::take(&mut e.torn) { + edges.push((e.stress, i as u32)); + } + } + let limit = self.material.max_tears_per_step as usize; + if edges.len() > limit { + // The most loaded first; ties by index (deterministic). Edges loaded past twice + // their threshold (the rim of a projectile punching through) all tear at once: the + // limit paces the cracks that creep near the threshold, not a puncture. + edges.sort_by(|x, y| y.0.total_cmp(&x.0).then(x.1.cmp(&y.1))); + let keep = edges + .iter() + .filter(|(stress, _)| *stress > CATASTROPHIC_STRESS) + .count() + .max(limit); + edges.truncate(keep); + } + let mut edges: Vec = edges.into_iter().map(|(_, i)| i).collect(); + edges.sort_unstable(); + let mut cells = Vec::new(); + for (i, c) in self.cells.iter_mut().enumerate() { + if core::mem::take(&mut c.torn) { + cells.push(i as u32); + } + } + (edges, cells) + } + + /// Applies the given tears (see [`Self::tear_edge`]) and records them in `event`. Returns + /// whether the topology changed; the colliders are not updated here (see `SoftBodySet::tear`). + pub(crate) fn tear_topology( + &mut self, + torn_edges: &[u32], + torn_cells: &[u32], + event: &mut SoftBodyTearEvent, + let any_cell_removed = cell_removed.contains(&true); + if any_cell_removed { + let mut kept_pairs: HashMap<[u32; 2], ()> = HashMap::default(); + let mut dead_pairs: HashMap<[u32; 2], ()> = HashMap::default(); + for (ci, c) in self.cells.iter().enumerate() { + if !cell_removed[ci] { + for i in 0..=DIM { + for j in i + 1..=DIM { + kept_pairs.insert(pair(c.vertices[i], c.vertices[j]), ()); + } + } + } + } + for (ci, c) in self.cells.iter().enumerate() { + if cell_removed[ci] { + for i in 0..=DIM { + for j in i + 1..=DIM { + let key = pair(c.vertices[i], c.vertices[j]); + if !kept_pairs.contains_key(&key) { + dead_pairs.insert(key, ()); + } + } + } + } + } + if !edge_removed[ei] && dead_pairs.contains_key(&key) { + for (key, ()) in dead_pairs { + torn_pairs.insert(key, ()); + } + } + // Surface elements owned by a removed cell or spanning a torn pair. A 3D cloth (no + let mut surface_removed = vec![false; self.boundary.len()]; + for (si, element) in self.boundary.iter().enumerate() { + let owner = self + .boundary_element_cells + .get(si) + .copied() + .unwrap_or(u32::MAX); + let owner_removed = + owner != u32::MAX && cell_removed.get(owner as usize) == Some(&true); + if owner_removed + || (!cloth && !torn_pairs.is_empty() && spans_pair(element, &torn_pairs)) + { + surface_removed[si] = true; + } + } + + // Dihedrals spanning a torn pair or bending over a removed triangle. + #[cfg(feature = "dim3")] + let mut dihedral_removed = vec![false; self.dihedrals.len()]; + #[cfg(feature = "dim3")] + { + let mut removed_tris: HashMap<[u32; 3], ()> = HashMap::default(); + for (si, element) in self.boundary.iter().enumerate() { + if surface_removed[si] { + let mut key = *element; + key.sort_unstable(); + removed_tris.insert(key, ()); + } + } + for (di, d) in self.dihedrals.iter().enumerate() { + let v = d.vertices; + let mut t1 = [v[0], v[1], v[2]]; + let mut t2 = [v[0], v[1], v[3]]; + t1.sort_unstable(); + t2.sort_unstable(); + if spans_pair(&v, &torn_pairs) + || removed_tris.contains_key(&t1) + || removed_tris.contains_key(&t2) + { + dihedral_removed[di] = true; + } + } + } + held >= 2 + // Split copies join the clusters of their source particle; the clusters' cell matches + // follow the compacted cell list. + self.inherit_cluster_membership(&duplicated); + // the split copies, so the direct meshes keep their indices). + cells: &cell_remap, + true + } +} diff --git a/src/dynamics/soft_body/tearing/tearing_event.rs b/src/dynamics/soft_body/tearing/tearing_event.rs new file mode 100644 index 000000000..a56b10163 --- /dev/null +++ b/src/dynamics/soft_body/tearing/tearing_event.rs @@ -0,0 +1,35 @@ +//! The tear event and the stress level past which a tear is never paced. + +use crate::alloc_prelude::*; +use crate::math::{DIM, Real}; + +use super::super::SoftBodyHandle; + +/// What a tear did to a soft body (see `EventHandler::handle_soft_body_tear_event` and +/// [`crate::dynamics::SoftBodySet::tear`]). +/// +/// The particle indices of `torn_edges` and `torn_cells` are the ones before the tear; those of +/// `split_particles` and `detached_particles` the ones right after it, before any detached +/// particle was dropped (see [`crate::dynamics::SoftBody::set_drop_detached_particles`]): apply `particle_remap` +/// to get the final ones. +#[derive(Clone, Debug, Default)] +pub struct SoftBodyTearEvent { + /// The soft body that tore. + pub soft_body: SoftBodyHandle, + /// The particle pairs of the removed edges (the ones marked torn, and the ones removed with + /// the torn cells). + pub torn_edges: Vec<[u32; 2]>, + /// The vertices of the removed cells (the ones marked torn, and the ones spanning a torn + /// edge). + pub torn_cells: Vec<[u32; DIM + 1]>, + /// The particles the tear passed through, duplicated one copy per piece: `(copy, source)`. + pub split_particles: Vec<(u32, u32)>, + /// The particles the tear left without any element. + /// The number of connected pieces of the body after the tear, the dropped particles gone + /// (see [`crate::dynamics::SoftBody::connected_pieces`]). + pub pieces: Vec, +} + +/// The smoothed load (a fraction of the tear threshold) past which a torn edge is never paced +/// by `SoftBodyMaterial::max_tears_per_step`. +pub(super) const CATASTROPHIC_STRESS: Real = 2.0; diff --git a/src/dynamics/soft_body/tearing/tearing_helpers.rs b/src/dynamics/soft_body/tearing/tearing_helpers.rs new file mode 100644 index 000000000..e984d16c3 --- /dev/null +++ b/src/dynamics/soft_body/tearing/tearing_helpers.rs @@ -0,0 +1,102 @@ +//! Tearing-related helpers of a soft body: connected pieces, tear resistance and particle damage. +use crate::alloc_prelude::*; +use super::super::SoftBody; +use crate::math::Real; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +impl SoftBody { + /// The connected pieces of this soft body (particles joined by its elements), each sorted and + /// ordered by first particle; linear in the elements. Tears split pieces off as soft bodies + /// ([`Self::tear_edge`]), so only a body inserted as disconnected parts has more than one. + pub fn connected_pieces(&self) -> Vec> { + let n = self.particles.len(); + let mut parent: Vec = (0..n as u32).collect(); + fn find(parent: &mut [u32], mut i: u32) -> u32 { + while parent[i as usize] != i { + let up = parent[parent[i as usize] as usize]; + parent[i as usize] = up; + i = up; + } + i + } + let mut union = |vertices: &[u32]| { + for w in vertices.windows(2) { + let (a, b) = (find(&mut parent, w[0]), find(&mut parent, w[1])); + if a != b { + parent[a.max(b) as usize] = a.min(b); + } + } + }; + for e in &self.edges { + union(&e.vertices); + } + for c in &self.cells { + union(&c.vertices); + } + #[cfg(feature = "dim3")] + for d in &self.dihedrals { + union(&d.vertices); + } + for s in &self.boundary { + union(s); + } + // Roots are the smallest index of their piece, so the pieces come out ordered. + let mut piece_of_root = vec![u32::MAX; n]; + let mut pieces: Vec> = Vec::new(); + for v in 0..n as u32 { + let root = find(&mut parent, v) as usize; + if piece_of_root[root] == u32::MAX { + piece_of_root[root] = pieces.len() as u32; + pieces.push(Vec::new()); + } + pieces[piece_of_root[root] as usize].push(v); + } + pieces + } + + /// Sets the tear-threshold multiplier of the `i`-th edge (see + /// [`crate::dynamics::SoftBodyEdge::tear_resistance`]); `1.0` restores the material's threshold. + pub fn set_edge_tear_resistance(&mut self, i: usize, resistance: Real) { + if let Some(e) = self.edges.get_mut(i) { + e.tear_resistance = resistance.max(0.0); + self.modified = true; + } + } + + /// Sets the tear-threshold multiplier of the `i`-th cell (see + /// [`crate::dynamics::SoftBodyCell::tear_resistance`]); `1.0` restores the material's threshold. + pub fn set_cell_tear_resistance(&mut self, i: usize, resistance: Real) { + if let Some(c) = self.cells.get_mut(i) { + c.tear_resistance = resistance.max(0.0); + self.modified = true; + } + } + + /// Marks the `i`-th particle as damaged, or repairs it (see + /// [`crate::dynamics::SoftBodyParticle::is_damaged`]): a way to seed a weak spot where a tear should start. + pub fn set_particle_damaged(&mut self, i: usize, damaged: bool) { + if let Some(p) = self.particles.get_mut(i) { + p.damaged = damaged; + self.modified = true; + } + } + + /// The tear-threshold multiplier of an element of resistance `resistance` over the particles + /// `vertices`: the material's interior strength applies on top when every particle is + /// interior and undamaged. + pub(crate) fn effective_tear_resistance(&self, resistance: Real, vertices: &[u32]) -> Real { + let interior = self.material.interior_strength; + if interior > 1.0 + && vertices.iter().all(|&v| { + let p = &self.particles[v as usize]; + !p.on_surface && !p.damaged + }) + { + resistance * interior + } else { + resistance + } + } +} diff --git a/src/dynamics/soft_body/tearing/tearing_particle_split.rs b/src/dynamics/soft_body/tearing/tearing_particle_split.rs new file mode 100644 index 000000000..029fbda1a --- /dev/null +++ b/src/dynamics/soft_body/tearing/tearing_particle_split.rs @@ -0,0 +1,373 @@ +//! Duplication of the particles a tear passes through: the measure elements around a particle +//! (its fan), their split along a plane or into disconnected groups, and the elements that follow +//! the copies. + +use crate::alloc_prelude::*; +use crate::math::{DIM, Real, Vector}; +use parry::utils::hashmap::HashMap; +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + +use super::tearing::{pair, shared_others}; +use super::super::{SoftBody, SoftBodyParticle}; + +impl SoftBody { + /// The kind of this body's measure elements, by priority: its cells, its surface triangles + /// (3D, without cells), its structural edges. `None` when it has none of them. + pub(super) fn split_particles(&mut self, candidates: &[u32]) -> Vec<(u32, u32)> { + let mut duplicated = Vec::new(); + // Faces: cells when the body has some, the surface elements otherwise (3D only: the + // segments of a 2D polyline never split, a polyline is cut by removing a segment). + let use_cells = !self.cells.is_empty(); + let use_surface = !use_cells && DIM == 3 && !self.boundary.is_empty(); + if !use_cells && !use_surface { + return duplicated; + } + // Two faces around `v` are adjacent when they share a facet through `v`. + let needed = if use_cells { DIM - 1 } else { DIM - 2 }; + + for &v in candidates { + let face_ids: Vec = if use_cells { + self.cells + .iter() + .enumerate() + .filter(|(_, c)| c.vertices.contains(&v)) + .map(|(i, _)| i as u32) + .collect() + } else { + self.boundary + .iter() + .enumerate() + .filter(|(_, s)| s.contains(&v)) + .map(|(i, _)| i as u32) + .collect() + }; + if face_ids.len() < 2 { + continue; + } + let face_vertices = |f: u32| -> Vec { + if use_cells { + self.cells[f as usize].vertices.to_vec() + } else { + self.boundary[f as usize].to_vec() + } + }; + // Union-find over the faces at `v`, groups numbered by first appearance. + let n = face_ids.len(); + let mut parent: Vec = (0..n).collect(); + fn find(parent: &mut [usize], mut i: usize) -> usize { + while parent[i] != i { + parent[i] = parent[parent[i]]; + i = parent[i]; + } + i + } + let verts: Vec> = face_ids.iter().map(|&f| face_vertices(f)).collect(); + for i in 0..n { + for j in i + 1..n { + if shared_others(&verts[i], &verts[j], v) >= needed { + let (ri, rj) = (find(&mut parent, i), find(&mut parent, j)); + if ri != rj { + parent[ri.max(rj)] = ri.min(rj); + } + } + } + } + let mut group_of_root: Vec = vec![usize::MAX; n]; + let mut group: Vec = vec![0; n]; + let mut num_groups = 0; + for i in 0..n { + let r = find(&mut parent, i); + if group_of_root[r] == usize::MAX { + group_of_root[r] = num_groups; + num_groups += 1; + } + group[i] = group_of_root[r]; + } + if num_groups < 2 { + continue; + } + + self.split_vertex( + v, + use_cells, + &face_ids, + &verts, + &group, + num_groups, + &mut duplicated, + ); + } + duplicated + } + + /// Opens a cloth crack across the torn edge `(a, b)`: one endpoint (the first, in index + /// order, whose fan has triangles on both sides) is split along the plane through it + /// perpendicular to the edge in the rest shape, the triangles on the other endpoint's side + /// going to the copy. The material is left whole; the tension the edge carried is released + /// through the crack instead. + pub(super) fn crack_across(&mut self, a: u32, b: u32, duplicated: &mut Vec<(u32, u32)>) { + for (v, other) in [(a, b), (b, a)] { + let face_ids: Vec = self + .boundary + .iter() + .enumerate() + .filter(|(_, s)| s.contains(&v)) + .map(|(i, _)| i as u32) + .collect(); + if face_ids.len() < 2 { + continue; + } + let rest = |i: u32| self.particles[i as usize].rest_position; + let axis = rest(other) - rest(v); + let verts: Vec> = face_ids + .iter() + .map(|&f| self.boundary[f as usize].to_vec()) + .collect(); + // Group 1: the triangles whose rest centroid lies on the other endpoint's side. + let group: Vec = verts + .iter() + .map(|fv| { + let mut c = Vector::ZERO; + for &w in fv { + c += rest(w) - rest(v); + } + (c.dot(axis) > 0.0) as usize + }) + .collect(); + if group.iter().all(|&g| g == 0) || group.iter().all(|&g| g == 1) { + /// Whether the plane split of `v` given by `fan` leaves at least `MIN_PIECE` triangles + /// reachable on each side without passing through `v` (no confetti). Always `true` outside + /// 3D triangle bodies. + pub(super) fn opens_without_confetti(&self, v: u32, fan: &Fan) -> bool { + if fan.kind != MeasureKind::Triangles { + return true; + } + let piece_ok = |side: usize| -> bool { + .filter(|&i| fan.sides[i] == side) + } + self.reachable_triangles(&seeds, v, MIN_PIECE) >= MIN_PIECE + }; + piece_ok(0) && piece_ok(1) + const MIN_PIECE: usize = 10; + .iter() + .zip(&group) + .map(|(&f, _)| f) + continue; + } + self.split_vertex(v, false, &face_ids, &verts, &group, 2, duplicated); + /// Number of surface triangles reachable from `seeds` through shared edges not touching + /// `pivot`, counting up to `limit`. + fn reachable_triangles(&self, seeds: &[u32], pivot: u32, limit: usize) -> usize { + // Edge -> triangles map of the whole surface (a tear is a rare event). + let mut by_edge: HashMap<[u32; 2], Vec> = HashMap::default(); + for (ti, t) in self.boundary.iter().enumerate() { + for i in 0..DIM { + let key = pair(t[i], t[(i + 1) % DIM]); + if key[0] != pivot && key[1] != pivot { + by_edge.entry(key).or_default().push(ti as u32); + while let Some(ti) = stack.pop() { + let t = self.boundary[ti as usize]; + for i in 0..DIM { + let key = pair(t[i], t[(i + 1) % DIM]); + if let Some(neighbors) = by_edge.get(&key) { + } + } + } + + /// Splits the particle `v` into one copy per group of its faces (`face_ids`, their + /// vertices in `verts`, `group[i]` in `0..num_groups`): group 0 keeps `v`, the others get a + /// copy; the mass follows, and so do the other elements around `v` (see `split_particles`). + #[allow(clippy::too_many_arguments)] + fn split_vertex( + use_cells: bool, + face_ids: &[u32], + verts: &[Vec], + group: &[usize], + num_groups: usize, + duplicated: &mut Vec<(u32, u32)>, + ) { + // A cloth fan group holding no spring at `v` (all its edges there torn) is a loose bit + // of fabric: its triangles are dropped instead of a copy of `v` that nothing holds + // (it would fly off with the triangle it renders). + if !use_cells { + for g in 0..num_groups { + if has_spring[g] { + } + let mut kept_ids = Vec::new(); + let mut kept_verts = Vec::new(); + let mut kept_group = Vec::new(); + v, + use_cells, + &kept_ids, + &kept_verts, + &kept_group, + kept_groups, + duplicated, + ); + let mut keep = + return; + } + } + // Mass split by rest volume (cells) or face count. + let mut weights = vec![0.0; num_groups]; + for (i, &f) in face_ids.iter().enumerate() { + weights[group[i]] += if use_cells { + self.cells[f as usize].rest_volume.abs() + } else { + 1.0 + }; + } + let new_index = |g: usize, particles: &mut Vec| -> u32 { + source.mass * weights[g] / total + } else { + source.mass / num_groups as Real + let p = &mut particles[v as usize]; + v + // A split particle is never pinned, so its copies are free like it. + particles.push(SoftBodyParticle { + particles.len() as u32 - 1 + } + }; + let mut copies: Vec = Vec::with_capacity(num_groups); + for g in 0..num_groups { + let idx = new_index(g, &mut self.particles); + if g > 0 { + duplicated.push((idx, v)); + } + copies.push(idx); + } + + // Reassign the faces of the other groups to their copies. + for (i, &f) in face_ids.iter().enumerate() { + let g = group[i]; + let target: &mut [u32] = if use_cells { + &mut self.cells[f as usize].vertices + } else { + &mut self.boundary[f as usize] + }; + for x in target.iter_mut() { + if *x == v { + *x = copies[g]; + } + } + } + + // The other elements at `v`: the group of the faces they share a vertex with, or + // (unattached) the group whose fan points their way in the rest shape. + let group_of_element = |vertices: &[u32], this: &SoftBody| -> usize { + let mut best: Option = None; + for (i, fv) in verts.iter().enumerate() { + if fv.iter().any(|x| *x != v && vertices.contains(x)) { + best = Some(best.map_or(group[i], |b: usize| b.min(group[i]))); + } + } + if let Some(g) = best { + return g; + } + let rest = |i: u32| this.particles[i as usize].rest_position; + let mut dir = Vector::ZERO; + for &w in vertices.iter().filter(|&&w| w != v) { + dir += rest(w) - rest(v); + } + let mut best_score = -Real::MAX; + let mut best_group = 0; + for (i, fv) in verts.iter().enumerate() { + for &w in fv.iter().filter(|&&w| w != v) { + let d = rest(w) - rest(v); + let score = d.dot(dir) / (d.length() * dir.length()).max(1.0e-12); + if score > best_score { + best_score = score; + best_group = group[i]; + } + } + } + best_group + }; + let vertices = self.edges[ei].vertices; + if !vertices.contains(&v) { + } + let x = if vertices[0] == v { + vertices[1] + } else { + vertices[0] + }; + let mut groups: Vec = verts + .iter() + .enumerate() + .filter(|(_, fv)| fv.contains(&x)) + .map(|(i, _)| group[i]) + .collect(); + groups.sort_unstable(); + groups.dedup(); + let g = if groups.is_empty() { + group_of_element(&vertices, self) + } else { + groups[0] + }; + for x in &mut self.edges[ei].vertices { + if *x == v { + *x = copies[g]; + } + } + for x in &mut copy.vertices { + if *x == copies[g] { + *x = copies[other]; + } + } + #[cfg(feature = "dim3")] + { + let mut removed = Vec::new(); + for di in 0..self.dihedrals.len() { + let vertices = self.dihedrals[di].vertices; + if !vertices.contains(&v) { + continue; + } + // A dihedral bending across the tear (its two triangles in different + // fans) is dropped. + let mut groups: Vec = Vec::new(); + for (i, fv) in verts.iter().enumerate() { + if fv.iter().any(|x| *x != v && vertices.contains(x)) { + groups.push(group[i]); + } + } + groups.sort_unstable(); + groups.dedup(); + if groups.len() > 1 { + removed.push(di); + continue; + } + let g = group_of_element(&vertices, self); + for x in &mut self.dihedrals[di].vertices { + if *x == v { + *x = copies[g]; + } + } + } + for di in removed.into_iter().rev() { + self.dihedrals.swap_remove(di); + } + } + if use_cells { + // A surface element follows the cell that owns it (the one holding all its + // vertices): the fan of a shared vertex alone would let a face bridge two pieces. + for si in 0..self.boundary.len() { + let vertices = self.boundary[si]; + if !vertices.contains(&v) { + } + let owner = verts + .iter() + .enumerate() + .find(|(_, fv)| vertices.iter().all(|x| fv.contains(x))); + let g = match owner { + Some((i, _)) => group[i], + None => group_of_element(&vertices, self), + }; + for x in &mut self.boundary[si] { + if *x == v { + *x = copies[g]; + } + } +} +} diff --git a/src/dynamics/soft_body/tearing/tearing_tables.rs b/src/dynamics/soft_body/tearing/tearing_tables.rs new file mode 100644 index 000000000..41e2edb49 --- /dev/null +++ b/src/dynamics/soft_body/tearing/tearing_tables.rs @@ -0,0 +1,28 @@ +//! The boundary tables and the parallel coloring rebuilt after a topology change. + +use super::super::soft_body_builder::{surface_element_cells, surface_is_closed}; +use super::super::{SoftBody, soft_body_coloring}; + +impl SoftBody { + /// Rebuilds the tables derived from the boundary and the cells (closedness, owning cells, + /// volume pieces), and the collision meshes' own tables. + pub(crate) fn rebuild_boundary_tables(&mut self) { + self.boundary_closed = surface_is_closed(&self.boundary); + self.boundary_element_cells = surface_element_cells(&self.boundary, &self.cells); + self.update_surface_flags(); + self.rebuild_mesh_tables(); + } + + /// Recomputes the parallel colors of every element. + pub(crate) fn recolor(&mut self) { + let (num_colors, has_overflow) = soft_body_coloring::assign_colors( + self.particles.len(), + &mut self.edges, + &mut self.cells, + #[cfg(feature = "dim3")] + &mut self.dihedrals, + ); + self.num_colors = num_colors; + self.has_overflow_color = has_overflow; + } +} diff --git a/src/dynamics/soft_body/tearing/tearing_validate.rs b/src/dynamics/soft_body/tearing/tearing_validate.rs new file mode 100644 index 000000000..26100add6 --- /dev/null +++ b/src/dynamics/soft_body/tearing/tearing_validate.rs @@ -0,0 +1,172 @@ +//! Consistency checks of a soft body topology and its derived tables after a tear. + +use crate::alloc_prelude::*; +#[cfg(feature = "dim3")] +use crate::math::DIM; +use alloc::format; +use parry::utils::hashmap::HashMap; + +#[cfg(feature = "dim3")] +use super::tearing::pair; +use super::super::soft_body::SOFT_BODY_OVERFLOW_COLOR; +use super::super::SoftBody; + +impl SoftBody { + /// Checks the topology and its derived tables (element indices, every particle in a measure + /// element (see [`Self::rest_measure`]), surface rings and incidence, owning cells, the 3D edge + /// table, the coloring) for consistency; returns the first inconsistency found. + pub fn validate_topology(&self) -> Result<(), String> { + let n = self.particles.len() as u32; + let check = |what: &str, vertices: &[u32]| -> Result<(), String> { + for &v in vertices { + if v >= n { + return Err(format!("{what} references particle {v} of {n}")); + } + } + Ok(()) + }; + for (i, e) in self.edges.iter().enumerate() { + check(&format!("edge {i}"), &e.vertices)?; + if e.vertices[0] == e.vertices[1] { + return Err(format!("edge {i} is degenerate")); + } + } + for (i, c) in self.cells.iter().enumerate() { + check(&format!("cell {i}"), &c.vertices)?; + } + #[cfg(feature = "dim3")] + for (i, d) in self.dihedrals.iter().enumerate() { + check(&format!("dihedral {i}"), &d.vertices)?; + } + for (i, s) in self.boundary.iter().enumerate() { + check(&format!("surface element {i}"), s)?; + } + // Every particle belongs to some measure element (a tear never removes one), unless the + // body has none. + } + for (mi, mesh) in self.meshes().enumerate() { + let nv = mesh.vertex_count(); + for (i, s) in mesh.indices().iter().enumerate() { + for &v in s { + } + } + if mesh.ring_offsets.len() != nv + 1 { + return Err(format!("mesh {mi} ring offsets do not cover every vertex")); + } + if mesh.vertex_elements_offsets.len() != nv + 1 { + return Err(format!( + "mesh {mi} vertex-element offsets do not cover every vertex" + )); + } + for (i, s) in mesh.indices().iter().enumerate() { + for &v in s { + let (start, end) = ( + mesh.vertex_elements_offsets[v as usize] as usize, + mesh.vertex_elements_offsets[v as usize + 1] as usize, + ); + if !mesh.vertex_elements[start..end].contains(&(i as u32)) { + return Err(format!("mesh {mi} element {i} missing from vertex {v}'s list")); + } + let (start, end) = ( + mesh.ring_offsets[v as usize] as usize, + mesh.ring_offsets[v as usize + 1] as usize, + ); + let ring = &mesh.ring[start..end]; + for &w in s.iter().filter(|&&w| w != v) { + if ring.binary_search(&w).is_err() { + return Err(format!("mesh {mi}: vertex {w} missing from {v}'s ring")); + } + } + } + } + for (v, window) in mesh.vertex_elements_offsets.windows(2).enumerate() { + for &e in &mesh.vertex_elements[window[0] as usize..window[1] as usize] { + let ok = mesh + .indices() + .get(e as usize) + .is_some_and(|s| s.contains(&(v as u32))); + if !ok { + return Err(format!("mesh {mi}: vertex {v} lists element {e} it is not in")); + } + } + } + #[cfg(feature = "dim3")] + { + if mesh.element_edges.len() != mesh.indices().len() + || mesh.edge_owners.len() != mesh.edges.len() + { + return Err(format!("mesh {mi} edge tables do not cover its elements")); + } + for (i, s) in mesh.indices().iter().enumerate() { + for k in 0..DIM { + let id = mesh.element_edges[i][k] as usize; + let expected = pair(s[(k + 1) % DIM], s[(k + 2) % DIM]); + if mesh.edges.get(id) != Some(&expected) { + return Err(format!("mesh {mi} element {i} edge {k} mismatch")); + } + } + } + for (id, &owner) in mesh.edge_owners.iter().enumerate() { + let ok = mesh + .element_edges + .get(owner as usize) + .is_some_and(|edges| edges.contains(&(id as u32))); + if !ok { + return Err(format!("mesh {mi} edge {id} owner mismatch")); + } + } + } + } + if !self.cells.is_empty() { + if self.boundary_element_cells.len() != self.boundary.len() { + return Err("boundary element owners do not cover the boundary".into()); + } + for (i, &c) in self.boundary_element_cells.iter().enumerate() { + if c == u32::MAX { + continue; + } + let ok = self + .cells + .get(c as usize) + .is_some_and(|cell| self.boundary[i].iter().all(|v| cell.vertices.contains(v))); + if !ok { + return Err(format!( + "boundary element {i} owned by a cell not containing it" + )); + } + } + } + // Coloring: elements of one parallel color share no particle. + let mut seen: HashMap<(u8, u32), ()> = HashMap::default(); + let mut check_color = |color: u8, vertices: &[u32]| -> Result<(), String> { + if color == SOFT_BODY_OVERFLOW_COLOR { + return Ok(()); + } + if color >= self.num_colors { + return Err(format!( + "color {color} beyond num_colors {}", + self.num_colors + )); + } + for &v in vertices { + if seen.insert((color, v), ()).is_some() { + return Err(format!( + "particle {v} shared by two elements of color {color}" + )); + } + } + Ok(()) + }; + for e in &self.edges { + check_color(e.color, &e.vertices)?; + } + for c in &self.cells { + check_color(c.color, &c.vertices)?; + } + #[cfg(feature = "dim3")] + for d in &self.dihedrals { + check_color(d.color, &d.vertices)?; + } + Ok(()) + } +} diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs index b512ed229..ec6cf5960 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs @@ -45,9 +45,13 @@ bitflags::bitflags! { /// that have them will be rendered. const PSEUDO_NORMALS = 1 << 8; /// If this flag is set, the soft bodies' volume constraints (the intersection-volume - /// rows) will be rendered: each constraint's normal at its patch center, and the + /// constraints) will be rendered: each constraint's normal at its patch center, and the /// volume gradient at every particle it acts on. const SOFT_VOLUME_CONTACTS = 1 << 9; + /// With [`Self::SOFT_BODIES`], colors elements by load, not `soft_body_element_color`: + /// `soft_body_slack_color` unloaded to `soft_body_loaded_color` at the tear threshold (the + /// smoothed element `stress`); without a threshold the load is the stretch, full at 50%. + const SOFT_BODY_STRESS = 1 << 10; } } @@ -76,7 +80,7 @@ pub struct DebugRenderPipeline { pub mode: DebugRenderMode, /// The soft-body edges to draw for the body being rendered, with their load (a cage edge is /// shared by every cell around it: the largest). Kept here to be reused from frame to frame. - drawn_edges: HashMap<[u32; 2], ()>, + drawn_edges: HashMap<[u32; 2], f32>, } impl Default for DebugRenderPipeline { @@ -160,38 +164,74 @@ impl DebugRenderPipeline { if !backend.filter_object(object) { continue; } - let color = self.style.soft_body_element_color; + let element_color = self.style.soft_body_element_color; + let by_stress = self.mode.contains(DebugRenderMode::SOFT_BODY_STRESS); + // The load an element is drawn with: its smoothed tear load when the material tears, + // its stretch (full at 50%) otherwise, so the picture stays meaningful for a body + // that never tears. + let tears = sb.material().tears(); + let stretch = |a: u32, b: u32| -> f32 { + let (pa, pb) = (&sb.particles()[a as usize], &sb.particles()[b as usize]); + let rest = (pa.rest_position() - pb.rest_position()).length(); + let len = (pa.position() - pb.position()).length(); + if rest > 0.0 { + ((len / rest - 1.0).abs() / 0.5) as f32 + } else { + 0.0 + } + }; // The cage is drawn edge by edge rather than cell by cell (an edge is shared by every // cell around it, and duplicates only thicken the picture); each edge keeps the largest // load of the elements sharing it. self.drawn_edges.clear(); - let draw_once = |backend: &mut B, drawn: &mut HashMap<[u32; 2], ()>, e: [u32; 2]| { + let mut record = |drawn: &mut HashMap<[u32; 2], f32>, e: [u32; 2], load: f32| { let key = [e[0].min(e[1]), e[0].max(e[1])]; - if drawn.insert(key, ()).is_none() { - backend.draw_line( - object, - sb.particle_position(key[0] as usize), - sb.particle_position(key[1] as usize), - color, - ); - } + let slot = drawn.entry(key).or_insert(0.0); + *slot = slot.max(load); }; for e in sb.edges() { if e.kind == SoftBodyEdgeKind::Structural { - draw_once(backend, &mut self.drawn_edges, e.vertices); + let load = match (by_stress, tears) { + (false, _) => 0.0, + (true, true) => e.stress() as f32, + (true, false) => stretch(e.vertices[0], e.vertices[1]), + }; + record(&mut self.drawn_edges, e.vertices, load); } } for c in sb.cells() { for a in 0..DIM + 1 { for b in a + 1..DIM + 1 { - draw_once( - backend, - &mut self.drawn_edges, - [c.vertices[a], c.vertices[b]], - ); + let edge = [c.vertices[a], c.vertices[b]]; + let load = match (by_stress, tears) { + (false, _) => 0.0, + (true, true) => c.stress() as f32, + (true, false) => stretch(edge[0], edge[1]), + }; + record(&mut self.drawn_edges, edge, load); } } } + // Sorted: the backend sees the same lines in the same order every frame. + let mut edges: Vec<([u32; 2], f32)> = + self.drawn_edges.iter().map(|(k, v)| (*k, *v)).collect(); + edges.sort_unstable_by(|a, b| a.0.cmp(&b.0)); + for (key, load) in edges { + let color = if by_stress { + let t = load.clamp(0.0, 1.0); + let (slack, loaded) = + (self.style.soft_body_slack_color, self.style.soft_body_loaded_color); + core::array::from_fn(|k| slack[k] + (loaded[k] - slack[k]) * t) + } else { + element_color + }; + backend.draw_line( + object, + sb.particle_position(key[0] as usize), + sb.particle_position(key[1] as usize), + color, + ); + } // Same reading as the rigid contacts: the depth segment joins the two witness // points, and the normal starts on the surface side. A contact sitting exactly on // the surface has no direction to show, so it gets the segment alone. diff --git a/src/pipeline/debug_render_pipeline/debug_render_style.rs b/src/pipeline/debug_render_pipeline/debug_render_style.rs index b93dbfe50..10b0e633d 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_style.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_style.rs @@ -59,6 +59,13 @@ pub struct DebugRenderStyle { pub contact_normal_length: Real, /// The color of the soft bodies' elements (structural edges, cell edges). pub soft_body_element_color: DebugColor, + /// The color of an unloaded soft-body element when the elements are colored by their load + /// (`DebugRenderMode::SOFT_BODY_STRESS`); the color runs from it to + /// `soft_body_loaded_color`, component by component (hue included), as the load grows. + pub soft_body_slack_color: DebugColor, + /// The color of a soft-body element at its tear threshold when the elements are colored by + /// their load (`DebugRenderMode::SOFT_BODY_STRESS`). + pub soft_body_loaded_color: DebugColor, /// Color of the soft-body cluster frames (the joint anchors' axes). pub soft_body_frame_color: DebugColor, /// The color of the colliders' [`Aabb`](crate::geometry::Aabb)s. @@ -98,6 +105,8 @@ impl Default for DebugRenderStyle { contact_normal_color: [0.0, 1.0, 1.0, 1.0], contact_normal_length: 0.3, soft_body_element_color: [200.0, 0.8, 0.5, 1.0], + soft_body_slack_color: [220.0, 0.8, 0.5, 1.0], + soft_body_loaded_color: [0.0, 1.0, 0.5, 1.0], soft_body_frame_color: [40.0, 0.9, 0.6, 1.0], collider_aabb_color: [124.0, 1.0, 0.4, 1.0], vertex_pseudo_normal_color: [180.0, 1.0, 0.6, 1.0], diff --git a/src/pipeline/event_handler.rs b/src/pipeline/event_handler.rs index 602b9403d..11dd9536b 100644 --- a/src/pipeline/event_handler.rs +++ b/src/pipeline/event_handler.rs @@ -1,5 +1,5 @@ #[cfg(feature = "alloc")] -use crate::dynamics::RigidBodySet; +use crate::dynamics::{RigidBodySet, SoftBodySet, SoftBodyTearEvent}; #[cfg(all(feature = "std", feature = "alloc"))] use crate::geometry::ContactForceEvent; #[cfg(feature = "alloc")] @@ -141,6 +141,11 @@ pub trait EventHandler: crate::utils::MaybeSync { contact_pair: &ContactPair, total_force_magnitude: Real, ); + + /// Called at the end of a step for every soft body that tore during it (an element past its + /// tear threshold, or a `SoftBody::tear_edge`/`tear_cell` request), once the topology change is + /// applied ([`SoftBodyTearEvent`]); immediate `SoftBodySet::tear`/`cut` return theirs instead. + fn handle_soft_body_tear_event(&self, soft_bodies: &SoftBodySet, event: &SoftBodyTearEvent); } #[cfg(feature = "alloc")] @@ -163,6 +168,8 @@ impl EventHandler for () { _total_force_magnitude: Real, ) { } + + fn handle_soft_body_tear_event(&self, _soft_bodies: &SoftBodySet, _event: &SoftBodyTearEvent) {} } /// A ready-to-use event handler that collects events into channels for later processing. @@ -196,6 +203,7 @@ impl EventHandler for () { pub struct ChannelEventCollector { collision_event_sender: std::sync::mpsc::Sender, contact_force_event_sender: std::sync::mpsc::Sender, + soft_body_tear_event_sender: std::sync::mpsc::Sender, } #[cfg(feature = "std")] @@ -206,10 +214,12 @@ impl ChannelEventCollector { pub fn new( collision_event_sender: std::sync::mpsc::Sender, contact_force_event_sender: std::sync::mpsc::Sender, + soft_body_tear_event_sender: std::sync::mpsc::Sender, ) -> Self { Self { collision_event_sender, contact_force_event_sender, + soft_body_tear_event_sender, } } } @@ -237,4 +247,8 @@ impl EventHandler for ChannelEventCollector { let result = ContactForceEvent::from_contact_pair(dt, contact_pair, total_force_magnitude); let _ = self.contact_force_event_sender.send(result); } + + fn handle_soft_body_tear_event(&self, _soft_bodies: &SoftBodySet, event: &SoftBodyTearEvent) { + let _ = self.soft_body_tear_event_sender.send(event.clone()); + } } diff --git a/src/pipeline/physics_world.rs b/src/pipeline/physics_world.rs index 582df741e..3f6110374 100644 --- a/src/pipeline/physics_world.rs +++ b/src/pipeline/physics_world.rs @@ -1,11 +1,11 @@ use crate::alloc_prelude::*; use crate::dynamics::{CCDSolver, GenericJoint, ImpulseJoint, ImpulseJointHandle, ImpulseJointSet, IntegrationParameters, IslandManager, Multibody, MultibodyJointHandle, MultibodyJointSet, MultibodyLink, MultibodyLinkId, RigidBody, RigidBodyHandle, RigidBodySet, SoftBindingError, SoftBody, SoftBodyBuilder, SoftBodyHandle, SoftBodySet, - SoftClusterRemoval, SoftMeshBinding}; + SoftBodyTearEvent, SoftClusterRemoval, SoftMeshBinding}; use crate::geometry::{ BroadPhaseBvh, Collider, ColliderHandle, ColliderSet, ContactPair, DefaultBroadPhase, NarrowPhase, }; -use crate::math::{Real, Vector}; +use crate::math::{DIM, Real, Vector}; use crate::pipeline::{ EventHandler, PhysicsHooks, PhysicsPipeline, Quarantine, QueryFilter, QueryPipeline, }; @@ -433,6 +433,46 @@ impl PhysicsWorld { ) } + /// Cuts a soft body along a blade (a world segment in 2D, a triangle in 3D) at once, without + /// removing any material (see [`SoftBodySet::cut`]). Returns the tear event, or `None` when + /// the cut changed nothing. + pub fn cut_soft_body( + &mut self, + handle: SoftBodyHandle, + blade: &[Vector; DIM], + ) -> Option { + self.soft_bodies.cut( + handle, + blade, + &mut self.islands, + &mut self.bodies, + &mut self.colliders, + &mut self.impulse_joints, + &mut self.multibody_joints, + ) + } + + /// Tears a soft body at once along the given edges and through the given cells, without + /// removing any material (see [`SoftBodySet::tear`] and [`SoftBody::tear_edge`]). Returns + /// the tear event, or `None` when nothing changed. + pub fn tear_soft_body( + &mut self, + handle: SoftBodyHandle, + edges: &[u32], + cells: &[u32], + ) -> Option { + self.soft_bodies.tear( + handle, + edges, + cells, + &mut self.islands, + &mut self.bodies, + &mut self.colliders, + &mut self.impulse_joints, + &mut self.multibody_joints, + ) + } + /// Adds a cluster to a soft body: a set of its particles backed by a fresh proxy rigid /// body ([`RigidBodyType::SoftFrame`]) that impulse joints and colliders can attach to. /// Returns the cluster's index (see [`SoftBodySet::add_cluster`]). diff --git a/src_testbed/debug_render.rs b/src_testbed/debug_render.rs index 290b5ed31..8a69d4465 100644 --- a/src_testbed/debug_render.rs +++ b/src_testbed/debug_render.rs @@ -17,9 +17,13 @@ pub struct DebugRenderPipelineResource { impl Default for DebugRenderPipelineResource { fn default() -> Self { Self { + // AABBs and pseudo-normals are opt-in (both bury the scene under lines), and so is + // the soft-body stress coloring (it replaces the elements' color). pipeline: DebugRenderPipeline::new( Default::default(), - !(DebugRenderMode::COLLIDER_AABBS | DebugRenderMode::PSEUDO_NORMALS), + !(DebugRenderMode::COLLIDER_AABBS + | DebugRenderMode::PSEUDO_NORMALS + | DebugRenderMode::SOFT_BODY_STRESS), ), enabled: false, } diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 2be02104f..dc48fd4d7 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -755,6 +755,13 @@ fn debug_render_tab(ui: &mut Ui, debug_render: &mut DebugRenderPipelineResource) "The soft bodies' intersection-volume constraints: each one's normal at its \ patch center, and the volume gradient at every particle it acts on.", ), + ( + DebugRenderMode::SOFT_BODY_STRESS, + "Soft-body stress", + "Colors the soft bodies' elements by their load, blue when slack to red at \ + their tear threshold (by their stretch, full at 50%, for a body that never \ + tears). Needs the soft bodies drawn.", + ), ]; for (flag, label, hover) in FLAGS { diff --git a/src_testbed/viewer.rs b/src_testbed/viewer.rs index 3624fd2f1..c18406a4c 100644 --- a/src_testbed/viewer.rs +++ b/src_testbed/viewer.rs @@ -261,10 +261,21 @@ impl TestbedViewer { self.camera = Camera::default(); self.state.camera_locked = false; self.state.example_settings.clear(); + // An example may switch the debug renderer on for itself (see + // `set_debug_render`): the next one starts from the defaults. + self.debug_render = DebugRenderPipelineResource::default(); } self.state.preserve_settings_on_switch = false; } + /// Switches the debug renderer on or off, drawing what `mode` selects (see + /// `DebugRenderMode`): what the debug-render tab of the settings panel toggles, for an + /// example whose point is a debug overlay. Reset when another example is selected. + pub fn set_debug_render(&mut self, enabled: bool, mode: rapier::pipeline::DebugRenderMode) { + self.debug_render.enabled = enabled; + self.debug_render.pipeline.mode = mode; + } + // ──────────────────────────── camera / scene ──────────────────────────── pub fn allow_grabbing_behind_ground(&mut self, allow: bool) { From 3ee4747a99ab99f11655119d8bf6996f7d8fc9a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 11 Sep 2026 16:19:06 +0200 Subject: [PATCH 15/32] feat(testbed): show the measured frame time and the recent step peak in the performance tab --- src_testbed/testbed/state.rs | 54 ++++++++++++++++++++++++++++++++++++ src_testbed/ui.rs | 20 +++++++++++-- src_testbed/viewer.rs | 7 +++++ 3 files changed, 78 insertions(+), 3 deletions(-) diff --git a/src_testbed/testbed/state.rs b/src_testbed/testbed/state.rs index 44ce9753c..500fd5e0a 100644 --- a/src_testbed/testbed/state.rs +++ b/src_testbed/testbed/state.rs @@ -95,6 +95,57 @@ impl ExampleEntry { } } +/// Wall-clock times of the recent frames, for the performance tab: the step counters only hold +/// the last step, and a frame also spends time outside of it (rendering, UI). +#[derive(Default)] +pub struct FrameStats { + last_frame: Option, + last_timestep: usize, + /// `(frame_ms, step_ms)` of the recent frames, oldest first. + samples: std::collections::VecDeque<(f64, f64)>, +} + +impl FrameStats { + /// The number of frames the statistics cover. + pub const WINDOW: usize = 60; + + /// Records the frame that just ended, with `step_ms` counted only if a step ran during it. + pub fn record(&mut self, timestep_id: usize, step_ms: f64) { + let now = web_time::Instant::now(); + if let Some(last) = self.last_frame { + let step_ms = if timestep_id != self.last_timestep { + step_ms + } else { + 0.0 + }; + if self.samples.len() == Self::WINDOW { + let _ = self.samples.pop_front(); + } + self.samples + .push_back(((now - last).as_secs_f64() * 1000.0, step_ms)); + } + self.last_frame = Some(now); + self.last_timestep = timestep_id; + } + + /// The mean `(frame, step)` times over the window, in milliseconds. + pub fn mean_ms(&self) -> (f64, f64) { + let n = self.samples.len().max(1) as f64; + let (frame, step) = self + .samples + .iter() + .fold((0.0, 0.0), |acc, s| (acc.0 + s.0, acc.1 + s.1)); + (frame / n, step / n) + } + + /// The longest `(frame, step)` times over the window, in milliseconds. + pub fn max_ms(&self) -> (f64, f64) { + self.samples + .iter() + .fold((0.0, 0.0), |acc, s| (acc.0.max(s.0), acc.1.max(s.1))) + } +} + /// State for the testbed application pub struct TestbedState { pub running: RunMode, @@ -134,6 +185,8 @@ pub struct TestbedState { /// (`-up_axis`) instead of the hard-coded Y-axis it used to assume. /// Defaults to `Vector::Y`. pub up_axis: rapier::math::Vector, + /// Recent frame and step times, shown by the performance tab. + pub frame_stats: FrameStats, /// Thread pool running the physics step, shared across example reloads: sized /// to the performance cores (efficiency cores stall the solver's /// barrier-paced parallel stages). Built lazily on the first `set_world`. @@ -164,6 +217,7 @@ impl Default for TestbedState { selected_tab: UiTab::default(), prev_save_data: SerializableTestbedState::default(), up_axis: rapier::math::Vector::Y, + frame_stats: FrameStats::default(), #[cfg(feature = "parallel")] physics_thread_pool: None, } diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index dc48fd4d7..e910df52e 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -935,13 +935,27 @@ fn performance_tab(ui: &mut Ui, state: &TestbedState, world: &PhysicsWorld) { // ───────────────────────────────────────────────────────────────── let counters = &world.physics_pipeline.counters; let total_ms = counters.step_time_ms(); - let fps = if total_ms > 0.0 { - (1000.0 / total_ms).round() + let (mean_frame_ms, mean_step_ms) = state.frame_stats.mean_ms(); + let (max_frame_ms, max_step_ms) = state.frame_stats.max_ms(); + let fps = if mean_frame_ms > 0.0 { + (1000.0 / mean_frame_ms).round() } else { 0.0 }; - ui.label(RichText::new(format!("Total: {:.2}ms - {:.0} FPS", total_ms, fps)).strong()); + // The step counters only hold the last step: the peaks over the recent frames show the + // spikes a single value flickers past, and the frame time is measured by the viewer. + ui.label( + RichText::new(format!("Total: {:.2}ms (peak {:.2}ms)", total_ms, max_step_ms)).strong(), + ); + ui.label(format!( + "Frame: {:.2}ms (peak {:.2}ms) - {:.0} FPS", + mean_frame_ms, max_frame_ms, fps + )); + ui.label(format!( + "Outside physics: {:.2}ms", + (mean_frame_ms - mean_step_ms).max(0.0) + )); ui.add_space(4.0); // Collision detection diff --git a/src_testbed/viewer.rs b/src_testbed/viewer.rs index c18406a4c..6f6e7705d 100644 --- a/src_testbed/viewer.rs +++ b/src_testbed/viewer.rs @@ -133,6 +133,11 @@ impl TestbedViewer { /// requested from the UI. pub async fn render_frame(&mut self, world: &mut PhysicsWorld) -> bool { profiling::finish_frame!(); + // A whole frame spans two calls: the example's step runs in between. + self.state.frame_stats.record( + self.state.timestep_id, + world.physics_pipeline.counters.step_time_ms(), + ); #[cfg(feature = "dim3")] let keep_open = self @@ -253,6 +258,8 @@ impl TestbedViewer { self.state.transition = None; self.state.snapshot = None; self.state.timestep_id = 0; + // The next example's scene building is not a frame. + self.state.frame_stats = Default::default(); self.state.action_flags = TestbedActionFlags::empty(); if self.state.preserve_settings_on_switch { From 615c7a8e2cfbd11dca771b2088edc89dcfbb21e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 11 Sep 2026 17:41:32 +0200 Subject: [PATCH 16/32] feat(soft-body): predictive vertex contacts against convex rigid shapes --- crates/rapier2d/tests/soft_bodies.rs | 47 +++++ crates/rapier3d/tests/soft_bodies.rs | 99 +++++++++++ .../collision_mesh/collision_mesh_geometry.rs | 18 ++ .../solver/soft_constraint/soft_contact.rs | 7 +- .../soft_contact_assembly_body_contacts.rs | 15 +- ...oft_contact_assembly_volume_constraints.rs | 7 +- .../soft_contact_assembly_workspace.rs | 22 ++- .../soft_contact_assembly_writeback.rs | 23 +++ .../soft_contacts/soft_contacts_pairs.rs | 160 +++++++++++++++--- .../soft_contacts/soft_contacts_types.rs | 31 ++++ 10 files changed, 387 insertions(+), 42 deletions(-) diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index 1c5d68333..680723914 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -2268,3 +2268,50 @@ fn modify_solver_contacts_edits_soft_soft_contacts() { assert!(resting > 1.1, "the blob did not rest on the rope: y = {resting}"); assert!(dropped < 0.6, "the hook did not drop the soft-soft contacts: y = {dropped}"); } + +/// A rope draped over a rigid ball much larger than its segments rests on it: the ball's +/// manifold with each segment holds a single point, so the vertices are held by the pair's +/// predictive vertex contacts (without them they sag into the skin and the rope jitters). +#[test] +fn rope_draped_over_a_large_ball_supports_its_vertices() { + let mut world = world_with_ground(); + let center = Vector::new(0.0, 1.0); + world.insert( + RigidBodyBuilder::fixed().translation(center), + ColliderBuilder::ball(1.0), + ); + let skin = 0.1 / 3.0; + let rope = SoftBodyBuilder::rope(Vector::new(-1.0, 2.1), Vector::new(1.0, 2.1), 21) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.02) + .particle_radius(skin) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.8)); + let handle = world.insert_soft_body(rope); + let mut min_dist = Real::MAX; + let mut max_speed: Real = 0.0; + let mut sum_speed = 0.0; + let mut count = 0; + for step in 0..600 { + world.step(); + if step < 300 { + continue; + } + for p in world.soft_bodies[handle].particles() { + let dist = (p.position() - center).length(); + if dist < 1.2 { + min_dist = min_dist.min(dist); + max_speed = max_speed.max(p.velocity().length()); + sum_speed += p.velocity().length(); + count += 1; + } + } + } + assert!(count > 0, "the rope slid off the ball"); + let sag = 1.0 + skin - min_dist; + let mean_speed = sum_speed / count as Real; + assert!(sag < 0.05 * skin, "the vertices sag into the ball: {sag}"); + assert!( + mean_speed < 0.045 && max_speed < 0.3, + "the rope jitters on the ball: mean speed {mean_speed}, max {max_speed}" + ); +} diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index 62e30c8f7..570fc00a8 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -3832,3 +3832,102 @@ fn modify_solver_contacts_edits_soft_soft_contacts() { assert!(resting > 1.1, "the jelly did not rest on the cloth: y = {resting}"); assert!(dropped < 0.5, "the hook did not drop the soft-soft contacts: y = {dropped}"); } + +/// A cloth draped over a rigid ball much larger than its elements rests on it: the ball's +/// manifold with each element holds a single point, so the vertices are held by the pair's +/// predictive vertex contacts (without them they sag into the skin and the cloth jitters). +#[test] +fn cloth_draped_over_a_large_ball_supports_its_vertices() { + let mut world = world_with_ground(); + let center = Vector::new(0.0, 1.0, 0.0); + world.insert( + RigidBodyBuilder::fixed().translation(center), + ColliderBuilder::ball(1.0), + ); + let n = 20; + let skin = 0.05; + let cloth = SoftBodyBuilder::cloth( + Vector::new(-0.95, 2.1, -0.95), + Vector::X * 0.1, + Vector::Z * 0.1, + n, + n, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.02) + .particle_radius(skin) + .surface_collider(ColliderBuilder::ball(skin).friction(0.8)); + let handle = world.insert_soft_body(cloth); + let mut min_dist = Real::MAX; + let mut max_speed: Real = 0.0; + let mut sum_speed = 0.0; + let mut count = 0; + for step in 0..600 { + world.step(); + if step < 300 { + continue; + } + for p in world.soft_bodies[handle].particles() { + let dist = (p.position() - center).length(); + if dist < 1.2 { + min_dist = min_dist.min(dist); + max_speed = max_speed.max(p.velocity().length()); + sum_speed += p.velocity().length(); + count += 1; + } + } + } + assert!(count > 0, "the cloth slid off the ball"); + let sag = 1.0 + skin - min_dist; + let mean_speed = sum_speed / count as Real; + assert!(sag < 0.15 * skin, "the vertices sag into the ball: {sag}"); + assert!( + mean_speed < 0.05 && max_speed < 0.7, + "the cloth jitters on the ball: mean speed {mean_speed}, max {max_speed}" + ); +} + +/// A small ball sliding across a cloth lying on the ground does not bump on the cloth's +/// vertices: a vertex contact whose direction is tilted off the surface (the ball beside the +/// vertex, over an incident element) would be a ghost of that element's contact. +#[test] +fn small_ball_slides_over_cloth_without_ghost_bumps() { + let mut world = world_with_ground(); + let n = 30; + let cloth = SoftBodyBuilder::cloth( + Vector::new(-1.5, 0.03, -1.5), + Vector::X * 0.1, + Vector::Z * 0.1, + n, + n, + ) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.02).friction(0.5)); + world.insert_soft_body(cloth); + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(-1.2, 0.15, 0.03)) + .linvel(Vector::new(2.0, 0.0, 0.0)) + .can_sleep(false), + ColliderBuilder::ball(0.05).friction(0.0), + ); + let mut max_vy: Real = 0.0; + let mut min_y = Real::MAX; + let mut max_y: Real = 0.0; + for step in 0..80 { + world.step(); + if step >= 30 { + let rb = &world.bodies[ball]; + max_vy = max_vy.max(rb.linvel().y.abs()); + min_y = min_y.min(rb.translation().y); + max_y = max_y.max(rb.translation().y); + } + } + let x = world.bodies[ball].translation().x; + assert!( + max_y - min_y < 1.0e-3 && max_vy < 0.1, + "the ball bumps on the cloth's vertices: height range {}, vertical speed {max_vy}", + max_y - min_y + ); + assert!(x > 0.6, "the cloth's vertices slowed the ball down: x = {x}"); +} diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs index 511e75bbb..c2f7df307 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs @@ -135,6 +135,24 @@ impl SoftCollisionMesh { false } + /// Whether `vertex` is exposed along `dir`: no surface neighbor lies ahead of it (up to a few + /// degrees), so a body approaching along `-dir` meets the vertex first. A contact at a vertex + /// that is not exposed along its normal is a ghost of an incident element's contact. + pub(crate) fn vertex_exposed(&self, body: &SoftBody, vertex: u32, dir: Vector) -> bool { + const TOLERANCE: Real = 0.05; + let (Some(&start), Some(&end)) = ( + self.ring_offsets.get(vertex as usize), + self.ring_offsets.get(vertex as usize + 1), + ) else { + return false; + }; + let p = self.vertex(body, vertex as usize); + self.ring[start as usize..end as usize].iter().all(|&w| { + let d = self.vertex(body, w as usize) - p; + d.dot(dir) <= TOLERANCE * d.length() + }) + } + /// Whether a contact on `element_id` at barycentric `weights`, with the other shape's point at /// `point`, is a ghost of an internal vertex/edge: the point projects inside a neighboring /// element, which reports the real contact, so this one only brings a spurious normal. diff --git a/src/dynamics/solver/soft_constraint/soft_contact.rs b/src/dynamics/solver/soft_constraint/soft_contact.rs index 616e1156d..70f1d6795 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact.rs @@ -68,11 +68,12 @@ impl FemContactSide { #[derive(Copy, Clone, Debug)] pub(crate) struct SoftContact { pub source: SoftContactSource, - /// Index of the surface's soft body in the solver's awake list, and the element's particle - /// indices in it (the vertex support rows write their warm start back to the particle). + /// Index of the surface's soft body in the solver's awake list, and the particle indices + /// holding the contact point in it (the FEM solver's load). pub support_body: u32, + #[cfg_attr(not(feature = "fem"), allow(dead_code))] pub support_particle: [u32; CONTACT_ANCHORS], - /// Solver slots of the particles carrying the contact point (`u32::MAX`: frozen at + /// Solver slots of the particles the contact point acts through (`u32::MAX`: frozen at /// `frozen_pos`, or an unused anchor when its weight is zero). pub particles: [u32; CONTACT_ANCHORS], pub weights: [Real; CONTACT_ANCHORS], diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs index 5a412cc01..def9c9706 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs @@ -6,6 +6,7 @@ use simba::scalar::{ComplexField as _, RealField as _}; use crate::alloc_prelude::*; +use super::soft_contact_assembly_workspace::SOURCE_RIGID_VERTEX; use super::{AssemblyCtx, BodyContacts, mesh_ref}; use super::super::soft_constraints_set::{SoftConstraintsSet, barycentric_weights}; use super::super::soft_contact::CONTACT_ANCHORS; @@ -197,16 +198,16 @@ impl SoftConstraintsSet { (dyn_erp, dyn_cfm) }; // Adjacent elements of a flat patch all report the shared vertex as a - // contact point: keep one row per (particle, pair) for those (the deepest), + // contact point: keep one constraint per (particle, pair) for those (the deepest), // interior points are kept as they are. let mut vertex_constraints: parry::utils::hashmap::HashMap = Default::default(); - // A particle at a rigid feature (a box edge under a cloth) collects one - // row per adjacent element per feature, up to ~16, all speculative by a - // few mm: in the relax pass each releases a little slack toward the - // feature and the sum is a phantom velocity of the resting particle - // (undone by the next biased pass). Rows of the same rigid feature whose - // point is dominated by the same particle are merged into the deepest one. + // The vertices a manifold constraint holds at (or next to) their position: they need + // no predictive vertex constraint (see below). + let mut held_vertices: Vec = Vec::new(); + // A particle at a rigid feature (a box edge under a cloth) collects up to ~16 + // speculative constraints whose relax-pass slack sums to a phantom velocity: + // those of one feature dominated by the same particle merge into the deepest. let mut feature_constraints: parry::utils::hashmap::HashMap<(u32, u32), usize> = Default::default(); // The narrow phase localized the anchors in the parent bodies' CoM frames, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs index d026aceaf..239b21c50 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs @@ -322,7 +322,12 @@ impl SoftConstraintsSet { if other_co.deformable_mesh_ref.is_some() || other_co.is_sensor() { continue; } - let Some(patch) = pair.rigid().and_then(|r| r.soft_patch.as_deref()) else { + let Some(patch) = pair + .rigid() + .and_then(|r| r.soft.as_deref()) + .and_then(|s| s.patch.as_ref()) + else { + continue; }; let other_rb = other_co.parent().and_then(|h| bodies.get(h)); let dynamic = other_rb.is_some_and(|rb| rb.is_dynamic()); diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs index 59fa1b268..22201bf71 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs @@ -16,42 +16,46 @@ use crate::geometry::{Collider, ColliderHandle, ColliderSet, NarrowPhase}; use crate::dynamics::soft_body::{SoftOverlapState, SoftVolumeContact}; use crate::math::{AngVector, Real, Rotation, Vector}; -/// The collision mesh a soft body's collider carries. +/// The collision mesh a soft body's collider holds. pub(super) fn mesh_of(sb: &SoftBody, collider: ColliderHandle) -> Option<&SoftCollisionMesh> { sb.mesh_of(collider) } +/// The mesh a deformable collider holds. pub(super) fn mesh_ref(co: &Collider) -> SoftMeshRef { co.deformable_mesh_ref.expect("not a deformable collider") } -/// Scratch of the contact assembly kept across steps. +/// Workspace of the contact assembly kept across steps. #[derive(Default)] pub(crate) struct EdgeContactWorkspace { /// Awake index of every awake soft body, by handle. pub(super) awake_of: HashMap, - /// The rows and new edge contacts of every awake body (assembled in parallel, appended in + /// The constraints and new edge contacts of every awake body (assembled in parallel, appended in /// awake order). pub(super) per_body: Vec, } -/// `SoftContactSource::manifold` of the rows without a manifold point: an edge-vs-edge row (its -/// warm start lives in the owner's `edge_contacts`) or a vertex-vs-surface row (`vertex_contacts`). +/// `SoftContactSource::manifold` of the constraints without a manifold point: an edge-vs-edge constraint (its +/// warm start lives in the owner's `edge_contacts`) or a vertex-vs-surface constraint (`vertex_contacts`). pub(super) const SOURCE_EDGE_CONTACT: u32 = u32::MAX; pub(super) const SOURCE_VERTEX_CONTACT: u32 = u32::MAX - 1; -/// The corrective pace deep recovery is allowed (demoted intruders, the volume rows): +/// `SoftContactSource::manifold` of a soft-rigid pair's predictive vertex constraint (its warm start +/// lives in the pair's `SoftRigidContacts::vertices`). +pub(super) const SOURCE_RIGID_VERTEX: u32 = u32::MAX - 2; +/// The corrective pace deep recovery is allowed (demoted intruders, the volume constraints): /// fast enough to visibly heal, slow enough that a correction cannot pump the material. pub(super) fn material_pace(params: &IntegrationParameters) -> Real { params.soft_bodies.recovery.recovery_pace * params.length_unit } -/// The contact rows of one awake soft body, and its new edge-vs-edge and vertex-vs-surface -/// contacts (the rows' `point` indices are positions in those lists). +/// The contact constraints of one awake soft body, and its new edge-vs-edge and vertex-vs-surface +/// contacts (the constraints' `point` indices are positions in those lists). #[derive(Default)] pub(super) struct BodyContacts { pub(super) contacts: Vec, /// The contact state of every mesh assembled this step, in the order the body's meshes were - /// visited (a row's `point` indexes its own mesh's lists). + /// visited (a constraint's `point` indexes its own mesh's lists). pub(super) meshes: Vec, /// The mesh being assembled. pub(super) current_mesh: usize, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs index 740e81618..bfb91ca6e 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs @@ -6,6 +6,7 @@ use simba::scalar::{ComplexField as _, RealField as _}; use crate::alloc_prelude::*; +use super::soft_contact_assembly_workspace::SOURCE_RIGID_VERTEX; use super::{SOURCE_EDGE_CONTACT, SOURCE_VERTEX_CONTACT}; use super::super::soft_constraints_set::SoftConstraintsSet; use super::super::soft_contact::SoftContact; @@ -194,6 +195,28 @@ impl SoftConstraintsSet { point.data.tangent_impulse = Default::default(); } } + if let Some(soft) = rigid.soft.as_deref_mut() { + for vertex in &mut soft.vertices { + vertex.impulse = 0.0; + vertex.tangent_impulse = Vector::ZERO; + } + } + } + if c.source.manifold == SOURCE_RIGID_VERTEX { + // A predictive vertex constraint: its warm start lives on the pair's vertex contact. + if let Some(vertex) = rigid + .soft + .as_deref_mut() + .and_then(|soft| soft.vertices.get_mut(c.source.point as usize)) + { + let mut tangent = Vector::ZERO; + for k in 0..DIM - 1 { + tangent += c.tangents[k] * c.impulse_tangent[k]; + } + vertex.impulse = crate::utils::canonicalize_zero(c.impulse_normal); + vertex.tangent_impulse = crate::utils::canonicalize_zero(tangent); + } + continue; } let Some(manifold) = rigid.manifolds.get_mut(c.source.manifold as usize) else { continue; diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs index b377d13fa..8975db3b2 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs @@ -2,12 +2,16 @@ use crate::alloc_prelude::*; -use crate::dynamics::{RigidBodySet, SoftMeshRef}; -use crate::geometry::{Collider, ColliderHandle, ContactPair, PairContacts, RigidPairContacts}; +use crate::dynamics::{RigidBodySet, SoftBody, SoftCollisionMesh, SoftMeshRef}; +use crate::geometry::{ + Collider, ColliderHandle, ContactManifold, ContactPair, PairContacts, RigidPairContacts, +}; +use crate::math::{Real, Vector}; #[cfg(not(feature = "std"))] #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; +use super::soft_contacts_types::SoftRigidVertexContact; use super::soft_contacts_volume::{detect_rigid_patch, detect_volume_patch}; use super::{ SoftDetectionCtx, SoftVertexPass, SoftEdgePass, SoftPairContacts, body_frozen, detect_vertex_pass, @@ -44,33 +48,145 @@ pub(crate) fn update_pair_soft_soft( } } +/// The soft contacts of a soft-rigid pair beside its manifolds (computed first): the +/// predictive vertex contacts within `reach` (the solver contacts' separation bound) and the +/// volume patch. pub(crate) fn update_pair_soft_rigid( rigid: &mut RigidPairContacts, - h1: ColliderHandle, - h2: ColliderHandle, - co1: &Collider, - co2: &Collider, + (h1, co1): (ColliderHandle, &Collider), + (h2, co2): (ColliderHandle, &Collider), bodies: &RigidBodySet, + reach: Real, ctx: &SoftDetectionCtx, ) { + let (r, soft_first) = match (co1.deformable_mesh_ref, co2.deformable_mesh_ref) { + (Some(r), None) => (r, true), + (None, Some(r)) => (r, false), + _ => return, + }; + let (soft_co, other_co, handle) = if soft_first { + (co1, co2, h1) + } else { + (co2, co1, h2) + }; + let Some((sb, mesh)) = ctx + .soft_bodies + .get(r.body) + .and_then(|sb| sb.mesh(r.id).map(|mesh| (sb, mesh))) + else { + rigid.soft = None; + return; + }; + let mut soft = rigid.soft.take().unwrap_or_default(); + soft.patch = detect_rigid_patch((sb, mesh, handle, soft_co), other_co, bodies, ctx); + detect_rigid_vertices( + &mut soft.vertices, + &rigid.manifolds, + soft_first, + (sb, mesh, soft_co), + other_co, + reach, + ); + if soft.patch.is_some() || !soft.vertices.is_empty() { + rigid.soft = Some(soft); + } } - match (co1.deformable_mesh_ref, co2.deformable_mesh_ref) { - (Some(r), None) | (None, Some(r)) => { - let (soft_co, other_co, handle) = if co1.deformable_mesh_ref.is_some() { - (co1, co2, h1) - } else { - (co2, co1, h2) - }; - rigid.soft_patch = ctx - .soft_bodies - .get(r.body) - .and_then(|sb| sb.mesh(r.id).map(|mesh| (sb, mesh))) - .and_then(|(sb, mesh)| { - detect_rigid_patch((sb, mesh, handle, soft_co), other_co, bodies, ctx) - }) - .map(Box::new); + +/// The predictive vertex contacts of a soft-rigid pair: every vertex of an element the +/// manifolds reached, within `reach` of a convex rigid collider and exposed toward it (see +/// `SoftCollisionMesh::vertex_exposed`), warm-started from the previous contacts by vertex. +fn detect_rigid_vertices( + out: &mut Vec, + manifolds: &[ContactManifold], + soft_first: bool, + (sb, mesh, soft_co): (&SoftBody, &SoftCollisionMesh, &Collider), + other_co: &Collider, + reach: Real, +) { + let previous = core::mem::take(out); + let shape = other_co.shape(); + // A composite rigid collider meets the elements with manifolds of several points already. + if !shape.is_convex() { + return; + } + let mut candidates: Vec<(u32, u32)> = Vec::new(); + for (mi, manifold) in manifolds.iter().enumerate() { + if manifold.points.is_empty() { + continue; } - _ => {} + let element = if soft_first { + manifold.subshape1 + } else { + manifold.subshape2 + } as usize; + if element < mesh.indices().len() { + candidates.extend(mesh.element(element).iter().map(|&v| (v, mi as u32))); + } + } + candidates.sort_unstable_by_key(|c| c.0); + let pose = other_co.position(); + let skins = soft_co.contact_skin() + other_co.contact_skin(); + // A manifold's force direction on the surface (the mesh's elements and a convex shape + // have no part pose, so the normal is in the first collider's frame). + let pose1 = if soft_first { soft_co } else { other_co }.position(); + let force_dir = |m: &ContactManifold| { + let n = pose1.rotation * m.local_n1; + if soft_first { -n } else { n } + }; + let mut previous = previous.iter().peekable(); + let mut rest = &candidates[..]; + while let Some(&(vertex, _)) = rest.first() { + let count = rest.iter().position(|c| c.0 != vertex).unwrap_or(rest.len()); + let (group, tail) = rest.split_at(count); + rest = tail; + let point = mesh.vertex(sb, vertex as usize); + let local = pose.inverse_transform_point(point); + let proj = shape.project_local_point(local, false); + let delta = local - proj.point; + let len = delta.length(); + // On the boundary itself the direction is undefined: the element's manifold holds it. + if len <= 1.0e-6 { + continue; + } + let (local_dir, separation) = if proj.is_inside { + (-delta / len, -len) + } else { + (delta / len, len) + }; + let dist = separation - skins; + let dir = pose.rotation * local_dir; + if dist >= reach || !mesh.vertex_exposed(sb, vertex, dir) { + continue; + } + // An incident element's contact must agree with the direction: inside a polyhedral + // shape, the feature closest to the vertex can be another face than the one it met. + let Some((manifold, alignment)) = group + .iter() + .map(|&(_, mi)| (mi, force_dir(&manifolds[mi as usize]).dot(dir))) + .max_by(|a, b| a.1.total_cmp(&b.1)) + else { + continue; + }; + if alignment < 0.7 { + continue; + } + while previous.peek().is_some_and(|c| c.vertex < vertex) { + previous.next(); + } + let (impulse, tangent_impulse) = previous + .peek() + .filter(|c| c.vertex == vertex) + .map_or((0.0, Vector::ZERO), |c| (c.impulse, c.tangent_impulse)); + out.push(SoftRigidVertexContact { + vertex, + manifold, + local_point: proj.point, + local_dir, + dist, + impulse, + tangent_impulse, + }); + } } /// The detection of a pair of two soft surfaces (see [`update_pair`]), into the cleared 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 7693ec50e..7c2f7f505 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs @@ -194,6 +194,37 @@ pub struct SoftRigidPatch { pub rot: Rotation, } +/// A predictive contact between a surface vertex of a soft-rigid pair and the convex rigid +/// collider: the support of the vertex itself, which an element's manifold does not give when +/// the rigid shape wraps the element (a single closest point per element). +#[derive(Copy, Clone, Debug)] +pub(crate) struct SoftRigidVertexContact { + /// The vertex of the soft mesh. + pub vertex: u32, + /// A manifold of the pair on an element incident to the vertex (its material). + pub manifold: u32, + /// The closest point on the rigid collider, in that collider's frame. + pub local_point: Vector, + /// The force direction on the vertex (away from the rigid collider), in that collider's + /// frame. + pub local_dir: Vector, + /// The separation, skins deducted (negative: penetrating). + pub dist: Real, + /// The normal impulse the solver applied through it at the last step. + pub impulse: Real, + /// The world-space friction impulse the solver applied through it at the last step. + pub tangent_impulse: Vector, +} + +/// The soft contacts of a soft-rigid pair beside its manifolds, rebuilt by every update. +#[derive(Clone, Default, Debug)] +pub(crate) struct SoftRigidContacts { + /// The predictive vertex contacts, sorted by vertex. + pub vertices: Vec, + /// The volume patch of a closed soft surface intruded by the rigid collider. + pub patch: Option, +} + /// The contacts of a pair of two soft surfaces: the candidates the narrow phase detected between /// them, which the soft-body solver turns into constraints, rebuilt by every update of the pair. /// Each slot (vertex-pass, edge, then volume-bin candidates) reports the solver's impulse. From 76bf93ea789ab9116bb54ca9219b4243d4709c9c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 17 Sep 2026 09:20:12 +0200 Subject: [PATCH 17/32] feat(soft-body): measure-preserving particle splits for cracks and cuts, pieces as soft bodies, min-piece setting, rest contact gaps --- crates/rapier2d/tests/soft_bodies.rs | 968 +++++++++++++++++- crates/rapier2d/tests/soft_body_clusters.rs | 183 ++++ crates/rapier2d/tests/soft_body_grab.rs | 202 ++++ crates/rapier3d/tests/soft_bodies.rs | 720 +++++++++++-- crates/rapier3d/tests/soft_body_clusters.rs | 181 ++++ crates/rapier3d/tests/soft_body_grab.rs | 354 +++++++ crates/rapier3d/tests/soft_body_meshes.rs | 132 ++- examples2d/soft_tearing2.rs | 29 +- examples3d/soft_tearing3.rs | 32 +- .../collision_mesh/collision_mesh.rs | 2 + .../collision_mesh/collision_mesh_binding.rs | 28 +- .../collision_mesh/collision_mesh_geometry.rs | 52 +- .../collision_mesh/collision_mesh_topology.rs | 291 +++++- src/dynamics/soft_body/mod.rs | 4 +- src/dynamics/soft_body/soft_body.rs | 9 + src/dynamics/soft_body/soft_body_accessors.rs | 59 +- .../soft_body/soft_body_builder/mod.rs | 4 +- .../soft_body_builder/soft_body_build.rs | 56 +- .../soft_body_builder_mesh_helpers.rs | 38 +- .../soft_body_builder_settings.rs | 7 + src/dynamics/soft_body/soft_body_cluster.rs | 40 +- src/dynamics/soft_body/soft_body_elements.rs | 20 + src/dynamics/soft_body/soft_body_geometry.rs | 36 +- src/dynamics/soft_body/soft_body_material.rs | 49 +- src/dynamics/soft_body/soft_body_set/mod.rs | 1 + .../soft_body_set/soft_body_set_clusters.rs | 1 + .../soft_body_set_insert_remove.rs | 3 + .../soft_body_set/soft_body_set_proxies.rs | 33 + .../soft_body_set/soft_body_set_split.rs | 567 ++++++++++ .../soft_body_set/soft_body_set_step_sync.rs | 67 +- src/dynamics/soft_body/soft_body_volume.rs | 168 +++ src/dynamics/soft_body/tearing/mod.rs | 6 +- src/dynamics/soft_body/tearing/tearing.rs | 107 +- .../soft_body/tearing/tearing_crack.rs | 267 +++++ src/dynamics/soft_body/tearing/tearing_cut.rs | 358 +++++++ .../soft_body/tearing/tearing_event.rs | 127 ++- .../soft_body/tearing/tearing_helpers.rs | 87 ++ .../tearing/tearing_particle_split.rs | 958 +++++++++++------ .../soft_body/tearing/tearing_tables.rs | 1 + .../soft_body/tearing/tearing_validate.rs | 39 +- src/dynamics/soft_body/tearing/tests.rs | 403 ++++++++ src/dynamics/solver/mod.rs | 1 + .../soft_constraint_prepare.rs | 28 +- .../soft_constraint_solve.rs | 20 +- .../soft_constraint_writeback.rs | 5 +- .../soft_constraints_set/soft_constraints.rs | 9 +- .../soft_constraints_set.rs | 2 +- .../soft_element_constraint_assembly.rs | 39 +- .../narrow_phase/soft_contacts/mod.rs | 4 +- .../soft_contacts/soft_contacts_edge_pass.rs | 42 +- .../soft_contacts/soft_contacts_pairs.rs | 6 +- .../soft_contacts/soft_contacts_types.rs | 23 +- .../soft_contacts_vertex_pass.rs | 70 +- .../soft_contacts/soft_self_contacts.rs | 4 +- 54 files changed, 6312 insertions(+), 630 deletions(-) create mode 100644 crates/rapier2d/tests/soft_body_clusters.rs create mode 100644 crates/rapier2d/tests/soft_body_grab.rs create mode 100644 crates/rapier3d/tests/soft_body_grab.rs create mode 100644 src/dynamics/soft_body/soft_body_set/soft_body_set_split.rs create mode 100644 src/dynamics/soft_body/soft_body_volume.rs create mode 100644 src/dynamics/soft_body/tearing/tearing_crack.rs create mode 100644 src/dynamics/soft_body/tearing/tearing_cut.rs create mode 100644 src/dynamics/soft_body/tearing/tests.rs diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index 680723914..d1bedf86f 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -18,6 +18,67 @@ fn assert_finite(world: &PhysicsWorld, handle: SoftBodyHandle) { } } +/// A soft body and every body split off it by tears, recursively (see `SoftBody::pieces`). +fn family(world: &PhysicsWorld, handle: SoftBodyHandle) -> Vec { + let mut bodies = vec![handle]; + let mut i = 0; + while i < bodies.len() { + bodies.extend(world.soft_bodies[bodies[i]].pieces().iter().copied()); + i += 1; + } + bodies +} + +/// Collects the tear events of every step. +#[derive(Default)] +struct TearLog(std::sync::Mutex>); + +impl TearLog { + fn drain(&self) -> Vec { + std::mem::take(&mut *self.0.lock().unwrap()) + } +} + +impl EventHandler for TearLog { + fn handle_collision_event( + &self, + _: &RigidBodySet, + _: &ColliderSet, + _: CollisionEvent, + _: Option<&ContactPair>, + ) { + } + fn handle_contact_force_event( + &self, + _: Real, + _: &RigidBodySet, + _: &ColliderSet, + _: &ContactPair, + _: Real, + ) { + } + fn handle_soft_body_tear_event(&self, _: &SoftBodySet, event: &SoftBodyTearEvent) { + self.0.lock().unwrap().push(event.clone()); + } +} + +/// Where a particle driven by index is after the tears of a step: the body and index the events +/// moved it to. +fn follow( + events: &[SoftBodyTearEvent], + mut body: SoftBodyHandle, + mut i: u32, +) -> (SoftBodyHandle, u32) { + for event in events { + if event.soft_body == body { + if let Some(destination) = event.particle_destination(i) { + (body, i) = destination; + } + } + } + (body, i) +} + /// The period of a mass-spring pair set by its natural frequency does not depend on the /// substep count. #[test] @@ -1171,6 +1232,8 @@ fn strip_tears_when_pulled_apart() { .particle_mass(0.05); let handle = world.insert_soft_body(strip); let sb = &world.soft_bodies[handle]; + let (num_particles, num_cells, measure) = + (sb.num_particles(), sb.cells().len(), sb.rest_measure()); let left: Vec = (0..sb.num_particles()) .filter(|&i| sb.particle_position(i).x < -1.19) .collect(); @@ -1185,33 +1248,63 @@ fn strip_tears_when_pulled_apart() { .iter() .map(|&i| world.soft_bodies[handle].particle_position(i)) .collect(); + // The driven particles follow the tears: the piece they are in becomes its own body. + let mut right: Vec<(SoftBodyHandle, u32)> = + right.iter().map(|&i| (handle, i as u32)).collect(); + let log = TearLog::default(); for k in 0..240 { let shift = 2.4 * (k as Real / 180.0).min(1.0); - for (&i, p) in right.iter().zip(rest.iter()) { - world.soft_bodies[handle] - .set_particle_kinematic_target(i, *p + Vector::new(shift, 0.0)); + for (&(body, i), p) in right.iter().zip(rest.iter()) { + world.soft_bodies[body] + .set_particle_kinematic_target(i as usize, *p + Vector::new(shift, 0.0)); + } + world.step_with_events(&(), &log); + let events = log.drain(); + for r in &mut right { + *r = follow(&events, r.0, r.1); + } + for h in family(&world, handle) { + assert_finite(&world, h); + world.soft_bodies[h].validate_topology().unwrap(); } - world.step(); - assert_finite(&world, handle); - world.soft_bodies[handle].validate_topology().unwrap(); } - let sb = &world.soft_bodies[handle]; + let bodies = family(&world, handle); + assert!(bodies.len() >= 2, "{model:?}: the strip is still in one piece"); + let sum = |f: &dyn Fn(&SoftBody) -> Real| -> Real { + bodies.iter().map(|&h| f(&world.soft_bodies[h])).sum() + }; + assert_eq!( + sum(&|sb| sb.cells().len() as Real), + num_cells as Real, + "{model:?}: a tear removed cells" + ); assert!( + (sum(&|sb| sb.rest_measure()) - measure).abs() < 1.0e-4 * measure, + "{model:?}: the rest area changed" + ); + assert!( + sum(&|sb| sb.num_particles() as Real) > num_particles as Real, + "{model:?}: no particle split" ); - assert!(sb.num_particles() > num_particles, "{model:?}: no particle split"); // Torn apart: no remaining cell edge is stretched past twice the tear strain. let mut max_stretch: Real = 0.0; - for c in sb.cells() { - for a in 0..3 { - for b in a + 1..3 { - let (pa, pb) = ( - sb.particle_position(c.vertices[a] as usize), - sb.particle_position(c.vertices[b] as usize), - ); - let rest = (sb.particles()[c.vertices[a] as usize].rest_position() - - sb.particles()[c.vertices[b] as usize].rest_position()) - .length(); - max_stretch = max_stretch.max((pa - pb).length() / rest); + for &h in &bodies { + let sb = &world.soft_bodies[h]; + assert!(!sb.boundary().is_empty()); + for c in sb.cells() { + for a in 0..3 { + for b in a + 1..3 { + let (pa, pb) = ( + sb.particle_position(c.vertices[a] as usize), + sb.particle_position(c.vertices[b] as usize), + ); + let rest = (sb.particles()[c.vertices[a] as usize].rest_position() + - sb.particles()[c.vertices[b] as usize].rest_position()) + .length(); + max_stretch = max_stretch.max((pa - pb).length() / rest); + } + } + } } assert!( max_stretch < 1.8, @@ -1226,30 +1319,58 @@ fn strip_tears_when_pulled_apart() { #[test] fn tearing_splits_the_particle_two_fans_share() { let mut world = world_with_ground(); + let ny = 2u32; let grid = SoftBodyBuilder::grid(Vector::new(0.0, 1.0), Vector::new(1.0, 0.25), 5, 2) + .particle_mass(0.5); let handle = world.insert_soft_body(grid); world.step(); let sb = &world.soft_bodies[handle]; let idx = |i: u32, j: u32| i * ny + j; + let (bottom, top) = (idx(2, 0), idx(2, 1)); + let torn = [bottom, idx(3, 0)]; + let edge = sb .edges() .iter() + .position(|e| e.vertices == torn || e.vertices == [torn[1], torn[0]]) + .expect("the bottom edge exists") as u32; + let num_particles = sb.num_particles() as u32; + let (num_cells, mass, measure) = (sb.cells().len(), sb.mass(), sb.rest_measure()); let event = world.tear_soft_body(handle, &[edge], &[]) .expect("nothing tore"); - let sb = &world.soft_bodies[handle]; - sb.validate_topology().unwrap(); + world.soft_bodies[handle].validate_topology().unwrap(); assert_eq!(event.soft_body, handle); assert_eq!(event.torn_edges.len(), 1); + assert!(event.torn_edges[0] == torn || event.torn_edges[0] == [torn[1], torn[0]]); + assert!(event.torn_cells.is_empty()); + assert_eq!( + event.split_particles, + vec![(num_particles, bottom), (num_particles + 1, top)] + ); + // The originals stay with the left half, the copies go with the right one; the halves are + // equal, so the left one (the smaller particle) keeps the handle and the right one is a new + // body. assert_eq!(event.pieces.len(), 2); let left: Vec = (0..=top).collect(); let right: Vec = (top + 1..num_particles + 2).collect(); - assert_eq!(sb.connected_pieces(), vec![left, right]); - assert!((sb.mass() - mass).abs() < 1.0e-5); - assert!((sb.rest_measure() - measure).abs() < 1.0e-5); + assert_eq!(event.pieces[0].soft_body, handle); + assert_eq!(event.pieces[0].particles, left); + assert_eq!(event.pieces[1].particles, right); + let bodies: Vec = event.bodies().collect(); + let sum = |f: &dyn Fn(&SoftBody) -> Real| -> Real { + bodies.iter().map(|&h| f(&world.soft_bodies[h])).sum() + }; + assert_eq!(sum(&|sb| sb.cells().len() as Real), num_cells as Real); + assert!((sum(&|sb| sb.mass()) - mass).abs() < 1.0e-5); + assert!((sum(&|sb| sb.rest_measure()) - measure).abs() < 1.0e-5); for _ in 0..120 { world.step(); - assert_finite(&world, handle); + for &h in &bodies { + assert_finite(&world, h); + } + } + for &h in &bodies { + world.soft_bodies[h].validate_topology().unwrap(); } - world.soft_bodies[handle].validate_topology().unwrap(); } /// `SoftBodyBuilder::trimesh` (2D): the triangles' vertices become the particles, their edges @@ -1815,9 +1936,13 @@ fn self_contact_blob_survives_grab_crush() { /// weight are three segments each, the smallest piece a tear may split off. fn hanging_weight(tear_force: Real) -> SoftBodyBuilder { SoftBodyBuilder::new(vec![ + Vector::new(-3.0, 2.0), + Vector::new(-2.0, 2.0), Vector::new(-1.0, 2.0), Vector::new(0.0, 2.0), Vector::new(0.0, 1.0), + Vector::new(0.0, 2.0 / 3.0), + Vector::new(0.0, 1.0 / 3.0), Vector::new(0.0, 0.0), ]) // The pinned bar, the tested edge, and the weight's own edges (reinforced). @@ -1842,6 +1967,11 @@ fn stiff_edge_tears_under_force_not_strain() { for _ in 0..60 { world.step(); } + // A tear splits the bar off the weight as two bodies: the bar keeps the handle. + let bodies = family(&world, handle); + for &h in &bodies { + world.soft_bodies[h].validate_topology().unwrap(); + } let sb = &world.soft_bodies[handle]; let tested = sb.edges().iter().find(|e| e.vertices == [3, 4]); let particles: usize = bodies.iter().map(|&h| world.soft_bodies[h].num_particles()).sum(); @@ -1873,6 +2003,9 @@ fn tear_smoothing_shrugs_off_a_spike() { let handle = world.insert_soft_body(hanging_weight(50.0).tear_smoothing(smoothing)); world.step(); // A velocity the edge must cancel within a substep: a force spike of hundreds of N. + for i in 4..8 { + world.soft_bodies[handle].apply_particle_impulse(i, Vector::new(0.0, -0.75), true); + } for _ in 0..30 { world.step(); } @@ -1907,6 +2040,20 @@ fn tear_resistance_scales_the_threshold() { for _ in 0..60 { world.step(); } + // The tear splits one particle and removes no edge: the plain chain's top particle splits, + // the chain falls as its own body and the copy stays on the anchor as a stub; the rest keeps + // the handle with particles renumbered. The reinforced edge is still there. + let bodies = family(&world, handle); + assert_eq!(bodies.len(), 2, "exactly one edge should have torn"); + let mut edges = 0; + let mut particles = 0; + for &h in &bodies { + let sb = &world.soft_bodies[h]; + sb.validate_topology().unwrap(); + edges += sb.edges().len(); + particles += sb.num_particles(); + } + assert_eq!((edges, particles), (9, 11)); let sb = &world.soft_bodies[handle]; let reinforced = sb.edges().iter().find(|e| e.tear_resistance == 3.0); let stress = reinforced.expect("the reinforced edge tore").stress(); @@ -2101,8 +2248,194 @@ fn tear_events_and_rope_ends() { assert_eq!(event.soft_body, handle); assert_eq!(event.torn_edges, vec![[3, 4]]); assert!(event.torn_cells.is_empty()); + // Both ends of the torn edge can open: the lower index goes first. assert_eq!(event.split_particles, vec![(7, 3)]); + // Two pieces of equal length: the pinned one (the smaller particle) keeps the handle, + // the falling one is a new body. + assert_eq!(event.pieces.len(), 2); + assert_eq!(event.pieces[0].soft_body, handle); + assert_eq!(event.pieces[0].particles, vec![0, 1, 2, 3]); + assert_eq!(event.pieces[1].particles, vec![4, 5, 6, 7]); + let fallen = &world.soft_bodies[event.pieces[1].soft_body]; + fallen.validate_topology().unwrap(); + assert_eq!(fallen.origin(), Some(handle)); + assert_eq!((fallen.num_particles(), fallen.edges().len()), (4, 3)); + assert_eq!(world.soft_bodies[handle].pieces(), &[event.pieces[1].soft_body]); + } + let sb = &world.soft_bodies[handle]; + sb.validate_topology().unwrap(); + assert_eq!(sb.edges().len(), 3); + assert_eq!(sb.connected_pieces(), vec![vec![0, 1, 2, 3]]); + // The pinned piece is three segments: any split would leave a side below the minimum. + let pinned: Vec = (0..sb.edges().len() as u32).collect(); + assert!( + world.tear_soft_body(handle, &pinned, &[]) + .is_none(), + "a piece of three segments tore" + ); + assert_eq!(world.soft_bodies[handle].num_particles(), 4); + + // The bottom edge is past the tear force. Its end particle cannot open (it has no other + // segment) and the particle above it would shed a single segment: nothing tears. + let mut world = PhysicsWorld::new(); + let handle = world.insert_soft_body( + rope(7) + .edge_tear_resistance([(0, 100.0), (1, 100.0), (2, 100.0), (3, 100.0), (4, 100.0)]) + .tear_force(6.0), + ); + let log = TearLog::default(); + for _ in 0..60 { + world.step_with_events(&(), &log); + } + assert!(log.0.lock().unwrap().is_empty(), "the rope shed a chip"); + let sb = &world.soft_bodies[handle]; + sb.validate_topology().unwrap(); + assert_eq!(sb.num_particles(), 7); + assert_eq!(sb.connected_pieces().len(), 1); +} + +/// A builder rope (`SoftBodyBuilder::rope`) torn in its middle separates: the bending edge over +/// the split particle is removed (it would hold the two pieces together), the other bending +/// edges and the surface segments follow their piece, and the loose end falls away. +#[test] +fn torn_builder_rope_separates() { + let mut world = PhysicsWorld::new(); + let handle = world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(0.0, 3.0), Vector::new(4.0, 3.0), 9) + .pinned_particles([0]) + .can_sleep(false), + ); + let sb = &world.soft_bodies[handle]; + let before = sb.clone(); + let num_bends = |sb: &SoftBody| { + sb.edges() + .iter() + .filter(|e| e.kind == SoftBodyEdgeKind::Bend) + .count() + }; + let torn = sb + .edges() + .iter() + .position(|e| e.vertices == [4, 5] && e.kind == SoftBodyEdgeKind::Structural) + .unwrap() as u32; + let event = world.tear_soft_body(handle, &[torn], &[]) + .expect("the rope tore"); + // Particle 4 splits (the lower index), and the bending edge (3, 5) over it goes. + assert_eq!(event.split_particles, vec![(9, 4)]); + assert_eq!(event.torn_edges, vec![[4, 5]]); + assert_eq!(event.removed_edges, vec![[3, 5]]); + // Two pieces of four segments: the pinned one keeps the handle, the loose one is a new body. + assert_eq!(event.pieces.len(), 2); + assert_eq!(event.pieces[0].soft_body, handle); + assert_eq!(event.pieces[0].particles, vec![0, 1, 2, 3, 4]); + assert_eq!(event.pieces[1].particles, vec![5, 6, 7, 8, 9]); + let loose = event.pieces[1].soft_body; + + let (sb, lb) = (&world.soft_bodies[handle], &world.soft_bodies[loose]); + sb.validate_topology().unwrap(); + lb.validate_topology().unwrap(); + let mass = sb.mass() + lb.mass(); + let measure = sb.rest_measure() + lb.rest_measure(); + assert!((mass - before.mass()).abs() <= 1.0e-5 * before.mass()); + assert!((measure - before.rest_measure()).abs() <= 1.0e-5 * before.rest_measure()); + assert_eq!(sb.edges().len() + lb.edges().len(), before.edges().len() - 1); + assert_eq!(num_bends(sb) + num_bends(lb), num_bends(&before) - 1); + assert_eq!(sb.boundary().len() + lb.boundary().len(), before.boundary().len()); + assert_eq!(sb.connected_pieces().len(), 1); + assert_eq!(lb.connected_pieces().len(), 1); + + // The pinned piece hangs at most 2 m below its pin; the loose one falls freely. + for _ in 0..60 { + world.step(); + } + let (sb, lb) = (&world.soft_bodies[handle], &world.soft_bodies[loose]); + let gap = (lb.particle_position(4) - sb.particle_position(4)).length(); + assert!(gap > 2.0, "the torn ends stayed together: {gap} m apart"); +} + +/// A tear keeps every particle in a measure element, whatever it takes around a particle: every +/// edge at a grid's corner, both ring segments of a disk particle, and every edge of a rope. +#[test] +fn tears_keep_every_particle_in_an_element() { + let mut world = world_with_ground(); + let grid = world.insert_soft_body( + SoftBodyBuilder::grid(Vector::new(0.0, 2.0), Vector::splat(1.0), 5, 5).particle_mass(0.2), + ); + let disk = world.insert_soft_body( + SoftBodyBuilder::disk(Vector::new(4.0, 2.0), 1.6, 40).particle_mass(0.05), + ); + let rope = world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(-4.0, 3.0), Vector::new(-2.0, 3.0), 6) + .pinned_particles([0]), + ); + world.step(); + + // The edges at the particle closest to `point`, optionally structural ones only. + let edges_at = |sb: &SoftBody, point: Vector, structural: bool| -> Vec { + let v = (0..sb.num_particles()) + .min_by(|&i, &j| { + (sb.particle_position(i) - point) + .length() + .total_cmp(&(sb.particle_position(j) - point).length()) + }) + .unwrap() as u32; + (0..sb.edges().len() as u32) + .filter(|&i| { + let e = &sb.edges()[i as usize]; + e.vertices.contains(&v) && (!structural || e.kind == SoftBodyEdgeKind::Structural) + }) + .collect() + }; + let corner = edges_at(&world.soft_bodies[grid], Vector::new(-1.0, 1.0), false); + world.tear_soft_body(grid, &corner, &[]); + let ring = edges_at(&world.soft_bodies[disk], Vector::new(5.6, 2.0), true); + assert_eq!(ring.len(), 2); + world.tear_soft_body(disk, &ring, &[]) + .expect("the ring tore"); + let all_edges: Vec = (0..world.soft_bodies[rope].edges().len() as u32).collect(); + world.tear_soft_body(rope, &all_edges, &[]); + + for _ in 0..60 { + world.step(); + } + for handle in [grid, disk, rope].into_iter().flat_map(|h| family(&world, h)) { + let sb = &world.soft_bodies[handle]; + sb.validate_topology().unwrap(); + assert!(sb.particles().iter().all(|p| p.position().is_finite())); + } } + +/// A cut keeps every particle in a measure element: a blade cutting a grid's corner cells off, and +/// a blade crossing a ring on both sides of one of its particles. +#[test] +fn cuts_keep_every_particle_in_an_element() { + let mut world = world_with_ground(); + let grid = world.insert_soft_body( + SoftBodyBuilder::grid(Vector::new(0.0, 2.0), Vector::splat(1.0), 5, 5).particle_mass(0.2), + ); + let disk = world.insert_soft_body( + SoftBodyBuilder::disk(Vector::new(4.0, 2.0), 1.6, 40).particle_mass(0.05), + ); + world.step(); + // The line x + y = 0.7 separates the two corner cells (centroids at x + y = 0.5) from the + // others; the corner particles' neighbors (x + y = 1) lie across it. + let event = world + .cut_soft_body(grid, &[Vector::new(-1.3, 2.0), Vector::new(-0.2, 0.9)]) + .expect("the blade crossed the corner"); + assert_eq!(event.pieces.len(), 2); + // The blade x = 5.59 crosses both ring segments of the particle (5.6, 2). + let event = world + .cut_soft_body(disk, &[Vector::new(5.59, 1.0), Vector::new(5.59, 3.0)]) + .expect("the blade crossed the ring"); + assert_eq!(event.inserted_particles.len(), 4); + for _ in 0..60 { + world.step(); + } + for handle in [grid, disk].into_iter().flat_map(|h| family(&world, h)) { + let sb = &world.soft_bodies[handle]; + sb.validate_topology().unwrap(); + assert!(sb.particles().iter().all(|p| p.position().is_finite())); + } } /// A blade across a grid between two of its columns splits the particles of the edges it meets, @@ -2116,9 +2449,11 @@ fn cut_splits_a_grid_along_a_segment() { .particle_mass(0.2); let handle = world.insert_soft_body(grid); world.step(); + let blade = [Vector::new(0.25, -5.0), Vector::new(0.25, 10.0)]; let sb = &world.soft_bodies[handle]; let (edges, cells) = sb.crossing_elements(&blade); assert!(!edges.is_empty() && !cells.is_empty()); + let (num_cells, mass, measure) = (sb.cells().len(), sb.mass(), sb.rest_measure()); assert!( world .cut_soft_body(handle, &[Vector::new(5.0, -5.0), Vector::new(5.0, 10.0)]) @@ -2129,6 +2464,10 @@ fn cut_splits_a_grid_along_a_segment() { .cut_soft_body(handle, &blade) .expect("the blade crossed the grid"); assert_eq!(event.pieces.len(), 2); + assert_eq!(event.torn_cells.len(), cells.len()); + assert!(!event.split_particles.is_empty() && event.inserted_particles.is_empty()); + // The two pieces are bodies of their own; every cell of a piece has its centroid on one + // side of the blade. let bodies: Vec = event.bodies().collect(); let mut sides = Vec::new(); let (mut cells_after, mut mass_after, mut measure_after) = (0, 0.0, 0.0); @@ -2162,6 +2501,87 @@ fn cut_splits_a_grid_along_a_segment() { } for _ in 0..60 { world.step(); + for &h in &bodies { + assert_finite(&world, h); + } + } + for &h in &bodies { + world.soft_bodies[h].validate_topology().unwrap(); + } +} + +/// A cut across a rope segment inserts two particles at the crossing: rest lengths split in +/// proportion, the segment ends share their masses with the inserted particles, the bending +/// edges over it are removed and the rope comes apart. A blade left across the cut is a no-op. +#[test] +fn cut_inserts_particles_into_a_rope() { + let mut world = world_with_ground(); + // Particles every 0.25 from x = 0 to x = 2; the blade crosses the segment (3, 4) at t = 0.6. + let rope = SoftBodyBuilder::rope(Vector::new(0.0, 3.0), Vector::new(2.0, 3.0), 9) + .pinned_particles([0]) + .particle_mass(0.1); + let handle = world.insert_soft_body(rope); + let blade = [Vector::new(0.9, 2.0), Vector::new(0.9, 4.0)]; + let event = world + .cut_soft_body(handle, &blade) + .expect("the blade crossed the rope"); + assert_eq!(event.inserted_particles, vec![9, 10]); + assert!(event.split_particles.is_empty()); + assert_eq!(event.torn_edges, vec![[3, 4], [2, 4], [3, 5]]); + let mut removed = event.removed_edges.clone(); + removed.sort_unstable(); + assert_eq!(removed, vec![[2, 4], [3, 5]]); + // The longer, free half keeps the handle; the pinned half is a new body. The event maps + // every particle to its body and index there. + assert_eq!(event.pieces.len(), 2); + assert_eq!(event.pieces[0].particles, vec![4, 5, 6, 7, 8, 10]); + assert_eq!(event.pieces[1].particles, vec![0, 1, 2, 3, 9]); + let at = |i: u32| event.particle_destination(i).unwrap(); + let (free, pinned) = (&world.soft_bodies[handle], &world.soft_bodies[at(0).0]); + assert_eq!((at(3), at(9)), ((at(0).0, 3), (at(0).0, 4))); + assert_eq!((at(10), at(4)), ((handle, 5), (handle, 0))); + free.validate_topology().unwrap(); + pinned.validate_topology().unwrap(); + let close = |a: Real, b: Real| (a - b).abs() < 1.0e-5; + let length = |sb: &SoftBody, a: u32, b: u32| { + sb.edges() + .iter() + .find(|e| e.vertices == [a, b]) + .map(|e| e.rest_length) + }; + assert!(close(length(pinned, 3, 4).unwrap(), 0.15) && close(length(free, 5, 0).unwrap(), 0.1)); + assert!(close(free.rest_measure() + pinned.rest_measure(), 2.0)); + assert!(close(free.mass() + pinned.mass(), 0.9)); + // Each end gives up half its mass (its share of the segment), then the four particles share + // that mass by the halves' lumped masses: 0.3 and 0.2 of it at each end of the halves. + let masses = [ + pinned.particles()[3].mass(), + pinned.particles()[4].mass(), + free.particles()[5].mass(), + free.particles()[0].mass(), + ]; + for (mass, expected) in masses.iter().zip([0.08, 0.03, 0.02, 0.07]) { + assert!(close(*mass, expected), "masses {masses:?}"); + } + assert!(pinned.particles()[0].is_pinned() && !pinned.particles()[4].is_pinned()); + for (sb, i) in [(pinned, 4), (free, 5)] { + assert!((sb.particle_position(i) - Vector::new(0.9, 3.0)).length() < 1.0e-5); + } + assert!(pinned.boundary().contains(&[3, 4]) && free.boundary().contains(&[5, 0])); + + let bodies: Vec = event.bodies().collect(); + for _ in 0..60 { + world.step(); + for &h in &bodies { + let num_particles = world.soft_bodies[h].num_particles(); + assert!(world.cut_soft_body(h, &blade).is_none()); + assert_eq!(world.soft_bodies[h].num_particles(), num_particles); + assert_finite(&world, h); + } + } + for &h in &bodies { + world.soft_bodies[h].validate_topology().unwrap(); + } } /// Point and radial impulses with a linear falloff: the particle on the point takes the whole @@ -2315,3 +2735,499 @@ fn rope_draped_over_a_large_ball_supports_its_vertices() { "the rope jitters on the ball: mean speed {mean_speed}, max {max_speed}" ); } + +/// A tear or a cut never deletes material: the mass and the rest measure (cell areas, or +/// structural edge lengths) are conserved, every particle keeps a measure element and the tables +/// stay consistent. One test per body kind and tear source. +mod tear_and_cut_conserve_mass_and_measure { + use super::*; + + /// Asserts the tear invariants from the body before a tear or cut to the bodies after it. + fn assert_tear_invariants(before: &SoftBody, after: &[&SoftBody]) { + let close = |a: Real, b: Real| (a - b).abs() <= 1.0e-5 * a.abs().max(b.abs()); + let mass: Real = after.iter().map(|sb| sb.mass()).sum(); + let measure: Real = after.iter().map(|sb| sb.rest_measure()).sum(); + assert!( + close(before.mass(), mass), + "mass changed: {} before, {} after", + before.mass(), + mass + ); + assert!( + close(before.rest_measure(), measure), + "rest measure changed: {} before, {} after", + before.rest_measure(), + measure + ); + for after in after { + assert_piece_invariants(before, after); + } + } + + /// Asserts that every particle of a piece keeps a measure element and its tables hold. + fn assert_piece_invariants(before: &SoftBody, after: &SoftBody) { + // The measure elements are chosen by the body kind before the tear. + let elements: Vec> = if !before.cells().is_empty() { + after.cells().iter().map(|c| c.vertices.to_vec()).collect() + } else { + after + .edges() + .iter() + .filter(|e| e.kind == SoftBodyEdgeKind::Structural) + .map(|e| e.vertices.to_vec()) + .collect() + }; + let mut supported = vec![false; after.num_particles()]; + for element in &elements { + for &v in element { + supported[v as usize] = true; + } + } + if let Some(v) = supported.iter().position(|s| !s) { + panic!("particle {v} belongs to no measure element"); + } + after.validate_topology().unwrap(); + } + + /// Inserts the body, tears it with `tear`, checks the invariants, then steps the torn body. + fn check( + builder: SoftBodyBuilder, + tear: impl FnOnce(&mut PhysicsWorld, SoftBodyHandle) -> Option, + ) { + let mut world = PhysicsWorld::new(); + let handle = world.insert_soft_body(builder); + let before = world.soft_bodies[handle].clone(); + let event = tear(&mut world, handle).expect("nothing tore"); + let bodies: Vec = event.bodies().collect(); + let after: Vec<&SoftBody> = bodies.iter().map(|&h| &world.soft_bodies[h]).collect(); + assert_tear_invariants(&before, &after); + for _ in 0..10 { + world.step(); + for &h in &bodies { + assert_finite(&world, h); + } + } + for &h in &bodies { + world.soft_bodies[h].validate_topology().unwrap(); + } + } + + /// Tears the structural edge whose midpoint is closest to `point`. + fn tear_edge_near( + world: &mut PhysicsWorld, + handle: SoftBodyHandle, + point: Vector, + ) -> Option { + let sb = &world.soft_bodies[handle]; + let distance = |e: &SoftBodyEdge| { + let mid = (sb.particle_position(e.vertices[0] as usize) + + sb.particle_position(e.vertices[1] as usize)) + * 0.5; + (mid - point).length() + }; + let edge = (0..sb.edges().len()) + .filter(|&i| sb.edges()[i].kind == SoftBodyEdgeKind::Structural) + .min_by(|&i, &j| distance(&sb.edges()[i]).total_cmp(&distance(&sb.edges()[j]))) + .unwrap() as u32; + world.tear_soft_body(handle, &[edge], &[]) + } + + /// Tears the cell whose centroid is closest to `point`. + fn tear_cell_near( + world: &mut PhysicsWorld, + handle: SoftBodyHandle, + point: Vector, + ) -> Option { + let sb = &world.soft_bodies[handle]; + let distance = |c: &SoftBodyCell| { + let sum: Vector = c + .vertices + .iter() + .map(|&v| sb.particle_position(v as usize)) + .sum(); + (sum / c.vertices.len() as Real - point).length() + }; + let cell = (0..sb.cells().len()) + .min_by(|&i, &j| distance(&sb.cells()[i]).total_cmp(&distance(&sb.cells()[j]))) + .unwrap() as u32; + world.tear_soft_body(handle, &[], &[cell]) + } + + fn grid() -> SoftBodyBuilder { + SoftBodyBuilder::grid(Vector::ZERO, Vector::splat(1.0), 5, 5).particle_mass(0.2) + } + + fn disk() -> SoftBodyBuilder { + SoftBodyBuilder::disk(Vector::ZERO, 1.0, 24).particle_mass(0.05) + } + + fn strip() -> SoftBodyBuilder { + let vertices = (0..8).map(|i| Vector::new(i as Real * 0.25, 0.0)).collect(); + SoftBodyBuilder::polyline(vertices, None).unwrap() + } + + fn rope() -> SoftBodyBuilder { + SoftBodyBuilder::rope(Vector::ZERO, Vector::new(2.0, 0.0), 9) + } + + /// A 3 x 3 particle spring network (no cell). + fn spring_network() -> SoftBodyBuilder { + let idx = |i: u32, j: u32| i * 3 + j; + let mut vertices = Vec::new(); + for i in 0..3 { + for j in 0..3 { + vertices.push(Vector::new(i as Real * 0.5, j as Real * 0.5)); + } + } + let mut indices = Vec::new(); + for i in 0..2 { + for j in 0..2 { + indices.push([idx(i, j), idx(i + 1, j), idx(i + 1, j + 1)]); + indices.push([idx(i, j), idx(i + 1, j + 1), idx(i, j + 1)]); + } + } + SoftBodyBuilder::trimesh(vertices, indices).unwrap() + } + + fn cut( + blade: [Vector; 2], + ) -> impl FnOnce(&mut PhysicsWorld, SoftBodyHandle) -> Option { + move |world, handle| world.cut_soft_body(handle, &blade) + } + + #[test] + fn grid_edge_tear() { + check(grid(), |w, h| tear_edge_near(w, h, Vector::new(0.25, 0.0))); + } + + #[test] + fn grid_cell_tear() { + check(grid(), |w, h| tear_cell_near(w, h, Vector::new(0.1, 0.1))); + } + + #[test] + fn grid_cut() { + check(grid(), cut([Vector::new(0.25, -5.0), Vector::new(0.25, 5.0)])); + } + + #[test] + fn disk_edge_tear() { + check(disk(), |w, h| tear_edge_near(w, h, Vector::new(1.0, 0.1))); + } + + #[test] + fn disk_cut() { + check(disk(), cut([Vector::new(0.5, -5.0), Vector::new(0.5, 5.0)])); + } + + #[test] + fn polyline_edge_tear() { + check(strip(), |w, h| tear_edge_near(w, h, Vector::new(0.875, 0.0))); + } + + #[test] + fn polyline_cut() { + check(strip(), cut([Vector::new(0.8, -1.0), Vector::new(0.8, 1.0)])); + } + + #[test] + fn rope_edge_tear() { + check(rope(), |w, h| tear_edge_near(w, h, Vector::new(0.875, 0.0))); + } + + #[test] + fn rope_cut() { + check(rope(), cut([Vector::new(0.9, -1.0), Vector::new(0.9, 1.0)])); + } + + #[test] + fn spring_network_edge_tear() { + check(spring_network(), |w, h| { + tear_edge_near(w, h, Vector::new(0.75, 0.5)) + }); + } +} + +/// Volume preservation holds every piece of material enclosed by its own closed boundary on its +/// own target: of two disks appended into one body, squeezing one does not inflate the other (a +/// single constraint over both boundaries let the free disk take the area the squeezed one lost). +#[test] +fn appended_disks_keep_their_own_areas() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let n = 24; + let center = Vector::new(-1.5, 0.0); + let disks = SoftBodyBuilder::disk(center, 1.0, n) + .append(SoftBodyBuilder::disk(Vector::new(1.5, 0.0), 1.0, n)) + .pinned_particles(0..n as u32) + .softness(SpringCoefficients::new(20.0, 1.0)) + .can_sleep(false); + let handle = world.insert_soft_body(disks); + let sb = &world.soft_bodies[handle]; + assert!(sb.volume_preservation_enabled()); + let pieces = sb.volume_pieces(); + assert_eq!(pieces.len(), 2); + assert_eq!(pieces[0].particles(), (0..n as u32).collect::>()); + let rest = pieces[1].rest_volume(); + assert!(rest > 3.0, "disk area too small: {rest}"); + assert!((pieces[0].rest_volume() - rest).abs() < 1.0e-4 * rest); + assert!((sb.rest_volume() - 2.0 * rest).abs() < 1.0e-4 * rest); + + // The pinned disk is squeezed to half its area. + let sb = &mut world.soft_bodies[handle]; + let scale = Real::sqrt(0.5); + for i in 0..n { + let p = sb.particle_position(i); + sb.set_particle_position(i, center + (p - center) * scale); + } + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let squeezed = sb.volume_pieces()[0].volume(sb); + let free = sb.volume_pieces()[1].volume(sb); + assert!( + (squeezed - 0.5 * rest).abs() < 0.01 * rest, + "the pinned disk moved: {squeezed} vs {}", + 0.5 * rest + ); + assert!( + (free - rest).abs() < 0.02 * rest, + "the free disk traded area with the squeezed one: {free} vs {rest}" + ); + assert!((sb.volume() - squeezed - free).abs() < 1.0e-4 * rest); + + // Disks sewn together by an edge are one piece of material. + let sewn = SoftBodyBuilder::disk(center, 1.0, n) + .append(SoftBodyBuilder::disk(Vector::new(1.5, 0.0), 1.0, n)) + .add_edges(vec![[0, n as u32]]); + let handle = world.insert_soft_body(sewn); + assert_eq!(world.soft_bodies[handle].volume_pieces().len(), 1); +} + +/// Whether two segments cross at a point interior to both (touching or collinear segments do +/// not cross). +fn segments_cross(a: [Vector; 2], b: [Vector; 2]) -> bool { + let orient = |p: Vector, q: Vector, r: Vector| (q - p).perp_dot(r - p); + let (o1, o2) = (orient(a[0], a[1], b[0]), orient(a[0], a[1], b[1])); + let (o3, o4) = (orient(b[0], b[1], a[0]), orient(b[0], b[1], a[1])); + o1 * o2 < 0.0 && o3 * o4 < 0.0 +} + +/// The number of inverted cells and of crossing pairs of boundary segments of a body. +fn tangles(sb: &SoftBody) -> (usize, usize) { + let area = |x: [Vector; 3]| (x[1] - x[0]).perp_dot(x[2] - x[0]); + let inverted = sb + .cells() + .iter() + .filter(|c| { + let now = area(c.vertices.map(|v| sb.particle_position(v as usize))); + let rest = area(c.vertices.map(|v| sb.particles()[v as usize].rest_position())); + now * rest <= 0.0 + }) + .count(); + let boundary = sb.boundary(); + let mut crossings = 0; + for (i, a) in boundary.iter().enumerate() { + for b in &boundary[i + 1..] { + if a.iter().any(|v| b.contains(v)) { + continue; + } + let pos = |s: &[u32; 2]| s.map(|v| sb.particle_position(v as usize)); + if segments_cross(pos(a), pos(b)) { + crossings += 1; + } + } + } + (inverted, crossings) +} + +/// The faces of a crack collide: a FEM grid with self contacts, cut halfway through, opened and +/// squeezed shut across the cut, keeps its crack faces from passing through each other. The faces +/// rest at distance zero, so `SoftCollisionMesh::self_contact_excluded` must keep their contacts. +#[cfg(feature = "fem")] +#[test] +fn crack_faces_collide_when_squeezed_shut() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let grid = SoftBodyBuilder::grid(Vector::ZERO, Vector::splat(1.0), 9, 9) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e3, + poisson_ratio: 0.3, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.05) + .self_contacts(true) + .solver(SoftBodySolver::Fem) + .can_sleep(false); + let handle = world.insert_soft_body(grid); + world.step(); + + // A cut from the bottom edge to the middle, along the particle column at x = 0. + let blade = [Vector::new(0.05, -2.0), Vector::new(0.05, 0.0)]; + let event = world + .cut_soft_body(handle, &blade) + .expect("the blade crossed the grid"); + assert!(event.pieces.is_empty(), "a partial cut separated the grid"); + assert!(!event.split_particles.is_empty()); + let sb = &world.soft_bodies[handle]; + sb.validate_topology().unwrap(); + assert_eq!(tangles(sb), (0, 0)); + + // The flaps under the crack tip, told apart by the side of their cells (the crack faces share + // their rest positions): pulled apart to open the crack, then pushed toward each other. + let mut side = vec![0.0; sb.num_particles()]; + for c in sb.cells() { + let x: Real = c + .vertices + .iter() + .map(|&v| sb.particles()[v as usize].rest_position().x) + .sum(); + for &v in &c.vertices { + side[v as usize] += x; + } + } + let left: Vec = side.iter().map(|x| *x < 0.0).collect(); + let push = |world: &mut PhysicsWorld, magnitude: Real| { + let sb = &mut world.soft_bodies[handle]; + sb.reset_forces(true); + for i in 0..sb.num_particles() { + if sb.particles()[i].rest_position().y < -0.2 { + let dir = if left[i] { 1.0 } else { -1.0 }; + sb.add_particle_force(i, Vector::new(dir * magnitude, 0.0), true); + } + } + }; + // The narrowest opening between a split particle and its copy (negative: passed through). + let gap = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[handle]; + event + .split_particles + .iter() + .map(|&(copy, source)| { + let (l, r) = if left[source as usize] { (source, copy) } else { (copy, source) }; + sb.particle_position(r as usize).x - sb.particle_position(l as usize).x + }) + .fold(Real::MAX, Real::min) + }; + push(&mut world, -2.0); + for _ in 0..60 { + world.step(); + } + assert!(gap(&world) > 0.05, "the crack did not open: {}", gap(&world)); + assert_eq!(tangles(&world.soft_bodies[handle]), (0, 0)); + + push(&mut world, 4.0); + let mut worst = (0, 0); + for _ in 0..240 { + world.step(); + let (inverted, crossings) = tangles(&world.soft_bodies[handle]); + worst = (worst.0.max(inverted), worst.1.max(crossings)); + } + assert_finite(&world, handle); + assert_eq!( + worst, + (0, 0), + "the crack faces passed through each other (inverted cells, crossings)" + ); +} + +/// A body with volume preservation cut in two holds each half on its own area: both halves keep +/// their area under a heavy lid, instead of trading it through one constraint over both. +#[test] +fn cut_halves_keep_their_own_areas() { + let mut world = world_with_ground(); + let grid = SoftBodyBuilder::grid(Vector::new(0.0, 1.0), Vector::splat(1.0), 6, 6) + .volume_preservation(true) + .particle_mass(0.1) + .can_sleep(false); + let handle = world.insert_soft_body(grid); + world.step(); + let rest_volume = world.soft_bodies[handle].rest_volume(); + + let blade = [Vector::new(0.0, -5.0), Vector::new(0.0, 10.0)]; + let event = world + .cut_soft_body(handle, &blade) + .expect("the blade crossed the grid"); + // Each half is a body of its own, on its own volume constraint. + assert_eq!(event.pieces.len(), 2); + let halves: Vec = event.bodies().collect(); + let mut rest = Vec::new(); + for &h in &halves { + let sb = &world.soft_bodies[h]; + assert!(sb.volume_preservation_enabled()); + assert_eq!(sb.volume_pieces().len(), 1); + rest.push(sb.volume_pieces()[0].rest_volume()); + } + assert!(rest.iter().all(|r| *r > 0.5), "a half is too small: {rest:?}"); + assert!((rest[0] + rest[1] - rest_volume).abs() < 1.0e-4 * rest_volume); + + // A heavy lid presses both halves into the ground. + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 2.4)), + ColliderBuilder::cuboid(2.0, 0.2).density(10.0), + ); + for _ in 0..300 { + world.step(); + } + for (&h, rest) in halves.iter().zip(&rest) { + assert_finite(&world, h); + let sb = &world.soft_bodies[h]; + let area = sb.volume_pieces()[0].volume(sb); + assert!( + (area - rest).abs() < 0.03 * rest, + "a half lost its area under the lid: {area} vs {rest}" + ); + } +} + +/// Two pieces of one body keep the gaps their features had at rest: the halves of a cut body, +/// interlocked along the cells' facets within the contact skins, rest exactly as cut instead of +/// being pushed apart by the skins; shoved into each other, they collide and come back out. +#[test] +fn cut_halves_rest_as_cut_and_collide_when_shoved() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let grid = SoftBodyBuilder::grid(Vector::ZERO, Vector::splat(1.0), 5, 5) + .particle_mass(0.1) + .can_sleep(false); + let handle = world.insert_soft_body(grid); + world.step(); + let event = world + .cut_soft_body(handle, &[Vector::new(0.25, -5.0), Vector::new(0.25, 5.0)]) + .expect("the blade crossed the grid"); + let halves: Vec = event.bodies().collect(); + assert_eq!(halves.len(), 2); + let centroid = |world: &PhysicsWorld, h: SoftBodyHandle| world.soft_bodies[h].center_of_mass(); + let offset = |world: &PhysicsWorld| (centroid(world, halves[1]) - centroid(world, halves[0])).x; + let rest_offset = offset(&world); + for _ in 0..60 { + world.step(); + for &h in &halves { + let sb = &world.soft_bodies[h]; + let speed = sb.particle_velocities().map(|v| v.length()).fold(0.0, Real::max); + assert!(speed < 1.0e-3, "the halves moved on their own: {speed} m/s"); + } + } + assert!((offset(&world) - rest_offset).abs() < 1.0e-3); + + // Shoved into each other by a fraction of a cell, the halves collide and come back out. + let sign = rest_offset.signum(); + let shove = 0.15 * sign; + for i in 0..world.soft_bodies[halves[1]].num_particles() { + let p = world.soft_bodies[halves[1]].particle_position(i); + world.soft_bodies[halves[1]].set_particle_position(i, p - Vector::X * shove); + } + assert!((offset(&world) - rest_offset) * sign < -0.1); + for _ in 0..120 { + world.step(); + for &h in &halves { + assert_finite(&world, h); + } + } + let pushed_back = (offset(&world) - rest_offset) * sign; + assert!(pushed_back > -0.05, "the shoved halves stayed interlocked: {pushed_back}"); +} diff --git a/crates/rapier2d/tests/soft_body_clusters.rs b/crates/rapier2d/tests/soft_body_clusters.rs new file mode 100644 index 000000000..74bcc7ef2 --- /dev/null +++ b/crates/rapier2d/tests/soft_body_clusters.rs @@ -0,0 +1,183 @@ +//! Soft-body clusters (2D): how they come apart when a tear runs through them. + +use rapier2d::prelude::*; + +/// Checks that a chain torn at a one-particle cluster splits the cluster with it: the copy keeps +/// the proxy, the joint follows the source at the same world anchor, and each chain piece gets +/// its own body, mesh and collider. +#[test] +fn torn_cluster_splits_and_its_joint_follows_the_source() { + let xs = [0.0, 1.0, 2.0, 3.0, 4.0, 6.0, 7.0, 8.0, 9.0]; + let positions: Vec = xs.iter().map(|&x| Vector::X * x).collect(); + let segments: Vec<[u32; 2]> = (0..8).map(|i| [i, i + 1]).collect(); + let builder = SoftBodyBuilder::new(positions) + .edges(segments.clone()) + .particle_mass(0.3) + .pinned_particles([0]); + let builder = builder.surface(segments); + let mut world = PhysicsWorld::new(); + let h = world.insert_soft_body(builder); + let cluster = world.add_soft_body_cluster(h, &[4]).unwrap(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let anchor = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(Vector::X * 4.0)); + let joint = world.insert_impulse_joint( + anchor, + proxy, + GenericJointBuilder::new(JointAxesMask::LIN_AXES).build(), + ); + + // The chain tears at particle 4 (the endpoints tie, the smaller index goes first), then + // comes apart: the right piece is the longer one and keeps the handle. + let event = world.tear_soft_body(h, &[4], &[]).expect("nothing tore"); + assert_eq!(event.split_particles, vec![(9, 4)]); + assert_eq!(event.pieces.len(), 2); + let right = &event.pieces[0]; + let left = &event.pieces[1]; + assert_eq!(right.soft_body, h); + assert_eq!(right.particles, vec![5, 6, 7, 8, 9]); + assert_eq!(left.particles, vec![0, 1, 2, 3, 4]); + let lh = left.soft_body; + assert_eq!(world.soft_bodies[lh].origin(), Some(h)); + assert_eq!(world.soft_bodies[h].pieces(), &[lh]); + assert!(world.soft_bodies[h].origin().is_none()); + + // The grab cluster split with the particle: the copy (particle 4 of the right body after + // compaction) is the heavier and kept the proxy, the source (particle 4 of the left body) + // got a fresh one. + let splits: Vec<_> = event + .clusters + .iter() + .filter(|c| c.source_cluster == cluster) + .collect(); + assert_eq!(splits.len(), 2, "{:?}", event.clusters); + let kept = splits.iter().find(|c| c.keeps_proxy).unwrap(); + let fresh = splits.iter().find(|c| !c.keeps_proxy).unwrap(); + assert_eq!((kept.soft_body, kept.cluster, kept.proxy), (h, cluster, proxy)); + assert_eq!(fresh.soft_body, lh); + let (rb, lb) = (&world.soft_bodies[h], &world.soft_bodies[lh]); + assert_eq!(rb.cluster(kept.cluster).unwrap().particles(), &[4]); + assert_eq!(lb.cluster(fresh.cluster).unwrap().particles(), &[4]); + assert_eq!(world.bodies[fresh.proxy].soft_body(), Some(lh)); + assert_eq!(world.bodies[fresh.proxy].soft_cluster(), Some(fresh.cluster)); + assert!(rb.cluster(kept.cluster).unwrap().meshes().next().is_none()); + // The left body took the fresh pieces of both clusters (made in cluster order). + assert_eq!(left.clusters, vec![[2, 0], [3, 1]]); + assert_eq!(fresh.cluster, 1); + + // The joint sits on the split particle, at the same rest distance from both pieces: the + // source's wins the tie, and the anchor stays where the particle is. + assert_eq!(event.moved_joints.len(), 1); + let moved = event.moved_joints[0]; + assert_eq!((moved.joint, moved.from, moved.to), (joint, proxy, fresh.proxy)); + let j = world.impulse_joints.get(joint).unwrap(); + assert_eq!((j.body1(), j.body2()), (anchor, fresh.proxy)); + let world_anchor = *world.bodies[fresh.proxy].position() * j.data.local_frame2.translation; + assert!((world_anchor - lb.particle_position(4)).length() < 1.0e-4); + + // Each body has the whole-body cluster of its piece (the left one's was cluster 2 of the + // torn body, the crack's fresh piece of cluster 0), with its own collision mesh. + for (body, piece, source) in [(h, right, 0), (lh, left, 2)] { + let sb = &world.soft_bodies[body]; + assert_eq!(sb.num_particles(), 5); + assert_eq!(sb.connected_pieces().len(), 1); + let (ci, c) = sb.live_clusters().next().unwrap(); + assert_eq!(c.proxy(), sb.root_body()); + assert_eq!(c.particles(), &[0, 1, 2, 3, 4]); + assert_eq!(world.bodies[c.proxy()].soft_body(), Some(body)); + assert_eq!(world.bodies[c.proxy()].soft_cluster(), Some(ci)); + assert!(piece.clusters.contains(&[source, ci])); + let mesh = c.meshes().next().expect("the piece lost its mesh"); + assert_eq!(mesh.vertex_count(), 5); + assert_eq!(world.colliders[mesh.collider()].parent(), Some(c.proxy())); + assert_eq!( + world.colliders[mesh.collider()] + .deformable_mesh_ref() + .map(|r| (r.body, r.id)), + Some((body, mesh.id())) + ); + sb.validate_topology().unwrap(); + } + assert!(lb.particles()[0].is_pinned() && !rb.particles()[0].is_pinned()); + + for _ in 0..60 { + world.step(); + } + for body in [h, lh] { + let sb = &world.soft_bodies[body]; + assert!(sb.particles().iter().all(|p| p.position().is_finite())); + } + // The pinned piece hangs from its pin, the joint holds it to the anchor; the right piece + // fell away. + let (rb, lb) = (&world.soft_bodies[h], &world.soft_bodies[lh]); + assert!((lb.particle_position(4) - Vector::X * 4.0).length() < 0.5); + assert!(rb.particle_position(0).y < -2.0); +} + +/// A cluster whose two particles are joined only through particles outside it is one piece as +/// long as those links hold: a tear elsewhere leaves it alone, a tear through a link splits it, +/// the particle the tear never reached staying with the retained piece. +#[test] +fn cluster_joined_outside_itself_splits_only_through_its_link() { + let mut world = PhysicsWorld::new(); + let rope = SoftBodyBuilder::rope(Vector::ZERO, Vector::X * 12.0, 13) + .particle_mass(0.1) + .pinned_particles([0]); + let h = world.insert_soft_body(rope); + let cluster = world.add_soft_body_cluster(h, &[3, 7]).unwrap(); + + // Far from the cluster: the body splits (the piece past the tear falls away as a new body, + // the cluster's piece keeps the handle), the cluster does not. + let event = world.tear_soft_body(h, &[8], &[]).expect("nothing tore"); + assert_eq!(event.split_particles, vec![(13, 8)]); + assert!(event.clusters.iter().all(|c| c.source_cluster == 0)); + assert_eq!(event.clusters.len(), 2); + assert!(event.moved_joints.is_empty()); + assert_eq!(event.pieces.len(), 2); + assert_eq!(event.pieces[0].soft_body, h); + assert_eq!(event.pieces[0].particles, (0..9).collect::>()); + assert_eq!(event.pieces[0].clusters, vec![[0, 0], [cluster, cluster]]); + assert_eq!(world.soft_bodies[h].num_particles(), 9); + assert_eq!( + world.soft_bodies[h].cluster(cluster).unwrap().particles(), + &[3, 7] + ); + + // Through the link: particle 3 splits, its copy 9 takes the segment towards 4, and the + // three pinned segments come apart from the rest as a new body. + let event = world.tear_soft_body(h, &[3], &[]).expect("nothing tore"); + assert_eq!(event.split_particles, vec![(9, 3)]); + assert_eq!(event.pieces.len(), 2); + assert_eq!(event.pieces[0].particles, vec![4, 5, 6, 7, 8, 9]); + assert_eq!(event.pieces[1].particles, vec![0, 1, 2, 3]); + let tail = event.pieces[1].soft_body; + let splits: Vec<_> = event + .clusters + .iter() + .filter(|c| c.source_cluster == cluster) + .collect(); + // The crack split the cluster into the source's piece (with 7, unreached by the tear) and the + // copy's; the body split then parted 3 from 7, the heavier 7 keeping the proxy: three pieces. + assert_eq!(splits.len(), 3, "{:?}", event.clusters); + let kept = splits.iter().find(|c| c.keeps_proxy).unwrap(); + assert_eq!((kept.soft_body, kept.cluster), (h, cluster)); + let sb = &world.soft_bodies[h]; + let tail_body = &world.soft_bodies[tail]; + assert_eq!(sb.cluster(cluster).unwrap().particles(), &[3]); + for fresh in splits.iter().filter(|c| !c.keeps_proxy) { + let body = &world.soft_bodies[fresh.soft_body]; + let particles = body.cluster(fresh.cluster).unwrap().particles(); + assert_eq!(particles, if fresh.soft_body == h { &[5] } else { &[3] }); + assert_eq!(world.bodies[fresh.proxy].soft_body(), Some(fresh.soft_body)); + } + assert_eq!(tail_body.num_live_clusters(), 2); + sb.validate_topology().unwrap(); + tail_body.validate_topology().unwrap(); + for _ in 0..30 { + world.step(); + } + for body in [h, tail] { + let sb = &world.soft_bodies[body]; + assert!(sb.particles().iter().all(|p| p.position().is_finite())); + } +} diff --git a/crates/rapier2d/tests/soft_body_grab.rs b/crates/rapier2d/tests/soft_body_grab.rs new file mode 100644 index 000000000..02659d43c --- /dev/null +++ b/crates/rapier2d/tests/soft_body_grab.rs @@ -0,0 +1,202 @@ +//! Mouse-grab replay on a torn soft body (2D): the testbed grab (a one-particle cluster whose +//! proxy is pulled by a motor-only joint from a kinematic body) drags the broken bridge of the +//! `soft_tearing2` demo around; the simulation stays finite and the grab cluster in one piece. + +use rapier2d::prelude::*; + +/// Particle speed bound. The mouse target circles at 75 m/s and the grabbed piece, a soft body +/// of its own since the tear, follows it with some overshoot (healthy runs peak near 107 m/s); +/// the divergent grab exceeded 300,000 m/s. +const MAX_PARTICLE_SPEED: Real = 130.0; +/// Gains of the testbed mouse joint (`src_testbed/grab.rs`). +const GRAB_STIFFNESS: Real = 1000.0; +const GRAB_DAMPING: Real = 50.0; +/// The mouse lifts by this height over one second, then circles with this radius and frequency. +const GRAB_LIFT: Real = 3.0; +const GRAB_RADIUS: Real = 4.0; +const GRAB_FREQUENCY: Real = 3.0; + +#[derive(Default, Debug)] +struct Outcome { + peak_speed: Real, + peak_time: Real, + non_finite: Option, + /// First (time, speed) past `MAX_PARTICLE_SPEED`. + too_fast: Option<(Real, Real)>, + /// First (time, piece count) with the grab cluster spanning several pieces. + split_cluster: Option<(Real, usize)>, + end_time: Real, +} + +/// The bridge of `soft_tearing2` (pinned at both ends) with the heavy disk that breaks it. +fn bridge_scene() -> (PhysicsWorld, SoftBodyHandle) { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(30.0, 0.5), + ); + let (nx, ny) = (31usize, 6usize); + let idx = move |i: usize, j: usize| (i * ny + j) as u32; + let bridge = SoftBodyBuilder::grid(Vector::new(-8.0, 4.0), Vector::new(3.0, 0.5), nx, ny) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e6, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + tear_strain: Some(0.35), + ..Default::default() + }) + .pinned_particles((0..ny).flat_map(move |j| [idx(0, j), idx(nx - 1, j)])) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + let bridge = world.insert_soft_body(bridge); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(-8.0, 9.0)), + ColliderBuilder::ball(0.6).density(20.0), + ); + (world, bridge) +} + +/// The free particle of the biggest piece nearest to that piece's free-particle centroid. +fn grab_particle(sb: &SoftBody) -> u32 { + let pieces = sb.connected_pieces(); + let biggest = pieces.iter().max_by_key(|p| p.len()).unwrap(); + let free: Vec = biggest + .iter() + .copied() + .filter(|&v| !sb.particles()[v as usize].is_pinned()) + .collect(); + let centroid = free + .iter() + .map(|&v| sb.particle_position(v as usize)) + .sum::() + / free.len() as Real; + *free + .iter() + .min_by(|a, b| { + let da = (sb.particle_position(**a as usize) - centroid).length(); + let db = (sb.particle_position(**b as usize) - centroid).length(); + da.total_cmp(&db) + }) + .unwrap() +} + +/// Grabs the bridge at `grab_time` and drags it until `end_time`, recording the first violation +/// of each check. +fn simulate(grab_time: Real, end_time: Real) -> Outcome { + let (mut world, bridge) = bridge_scene(); + let dt = world.integration_parameters.dt; + let mut outcome = Outcome::default(); + let mut grab: Option<(RigidBodyHandle, RigidBodyHandle, Vector)> = None; + let mut t: Real = 0.0; + + while t < end_time { + if grab.is_none() && t >= grab_time { + let particle = grab_particle(&world.soft_bodies[bridge]); + let anchor = world.soft_bodies[bridge].particle_position(particle as usize); + let cluster = world + .add_soft_body_cluster(bridge, &[particle]) + .expect("grab cluster refused"); + let proxy = world.soft_bodies[bridge].cluster_proxy(cluster).unwrap(); + let mouse = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); + let joint = GenericJointBuilder::new(JointAxesMask::empty()) + .motor_position(JointAxis::LinX, 0.0, GRAB_STIFFNESS, GRAB_DAMPING) + .motor_position(JointAxis::LinY, 0.0, GRAB_STIFFNESS, GRAB_DAMPING); + world.insert_impulse_joint(mouse, proxy, joint); + grab = Some((mouse, proxy, anchor)); + } + if let Some((mouse, _, anchor)) = grab { + let tg = t - grab_time; + let lift = tg.min(1.0) * GRAB_LIFT; + let phase = (tg - 1.0).max(0.0) * GRAB_FREQUENCY * std::f64::consts::TAU as Real; + let circle = Vector::new(phase.sin(), 1.0 - phase.cos()) * GRAB_RADIUS; + world.bodies[mouse].set_next_kinematic_translation(anchor + Vector::Y * lift + circle); + } + world.step(); + t += dt; + outcome.end_time = t; + + let mut finite = world + .bodies + .iter() + .all(|(_, rb)| rb.translation().is_finite() && rb.linvel().is_finite()); + let mut speed: Real = 0.0; + for (_, sb) in world.soft_bodies.iter() { + for p in sb.particles() { + finite &= p.position().is_finite() && p.velocity().is_finite(); + speed = speed.max(p.velocity().length()); + } + } + if speed > outcome.peak_speed { + outcome.peak_speed = speed; + outcome.peak_time = t; + } + if speed > MAX_PARTICLE_SPEED && outcome.too_fast.is_none() { + outcome.too_fast = Some((t, speed)); + } + if let Some((_, proxy, _)) = grab { + // The cluster follows the tears: its proxy says which body holds it now, and the + // cluster must lie within one connected piece of that body. + let rb = world.bodies.get(proxy).expect("grab proxy removed"); + let body = rb.soft_body().expect("grab proxy lost its soft body"); + let cluster = rb.soft_cluster().expect("grab proxy lost its cluster"); + let sb = &world.soft_bodies[body]; + let mut piece_of = vec![0usize; sb.num_particles()]; + for (k, piece) in sb.connected_pieces().iter().enumerate() { + for &v in piece { + piece_of[v as usize] = k; + } + } + let cl = sb.cluster(cluster).expect("grab cluster removed"); + let mut pieces: Vec = cl + .particles() + .iter() + .map(|&v| piece_of[v as usize]) + .collect(); + pieces.sort_unstable(); + pieces.dedup(); + if pieces.len() > 1 && outcome.split_cluster.is_none() { + outcome.split_cluster = Some((t, pieces.len())); + } + } + if !finite { + outcome.non_finite = Some(t); + break; + } + // Past this speed the run has diverged; the rest of it is noise. + if speed > 1.0e3 { + break; + } + } + outcome +} + +/// Checks that grabbing the broken bridge at t = 3 s, lifting it 3 m and circling it (4 m radius, +/// 3 Hz) stays bounded. +#[test] +fn grabbing_the_broken_bridge_stays_bounded() { + let outcome = simulate(3.0, 6.0); + let mut failures = Vec::new(); + if let Some(t) = outcome.non_finite { + failures.push(format!("non-finite state at t = {t:.3} s")); + } + if let Some((t, speed)) = outcome.too_fast { + failures.push(format!( + "particle speed {speed:.1} m/s past {MAX_PARTICLE_SPEED} m/s at t = {t:.3} s" + )); + } + if let Some((t, pieces)) = outcome.split_cluster { + failures.push(format!( + "grab cluster spans {pieces} pieces at t = {t:.3} s" + )); + } + assert!( + failures.is_empty(), + "{} (peak {:.1} m/s at t = {:.3} s, stopped at t = {:.3} s)", + failures.join("; "), + outcome.peak_speed, + outcome.peak_time, + outcome.end_time, + ); +} diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index 570fc00a8..8bd9440b9 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -30,6 +30,67 @@ fn assert_finite(world: &PhysicsWorld, handle: SoftBodyHandle) { } } +/// A soft body and every body split off it by tears, recursively (see `SoftBody::pieces`). +fn family(world: &PhysicsWorld, handle: SoftBodyHandle) -> Vec { + let mut bodies = vec![handle]; + let mut i = 0; + while i < bodies.len() { + bodies.extend(world.soft_bodies[bodies[i]].pieces().iter().copied()); + i += 1; + } + bodies +} + +/// Collects the tear events of every step. +#[derive(Default)] +struct TearLog(std::sync::Mutex>); + +impl TearLog { + fn drain(&self) -> Vec { + std::mem::take(&mut *self.0.lock().unwrap()) + } +} + +impl EventHandler for TearLog { + fn handle_collision_event( + &self, + _: &RigidBodySet, + _: &ColliderSet, + _: CollisionEvent, + _: Option<&ContactPair>, + ) { + } + fn handle_contact_force_event( + &self, + _: Real, + _: &RigidBodySet, + _: &ColliderSet, + _: &ContactPair, + _: Real, + ) { + } + fn handle_soft_body_tear_event(&self, _: &SoftBodySet, event: &SoftBodyTearEvent) { + self.0.lock().unwrap().push(event.clone()); + } +} + +/// Where a particle driven by index is after the tears of a step: the body and index the events +/// moved it to. +fn follow( + events: &[SoftBodyTearEvent], + mut body: SoftBodyHandle, + mut i: u32, +) -> (SoftBodyHandle, u32) { + for event in events { + if event.soft_body == body { + if let Some(destination) = event.particle_destination(i) { + (body, i) = destination; + } + } + } + (body, i) +} + /// The period of a mass-spring pair set by its natural frequency must not depend on the /// substep count: this is the property that separates the material model from position-based /// `rigidity` knobs. @@ -1840,41 +1901,54 @@ fn cloth_tears_when_pulled_apart() { .map(|j| world.soft_bodies[handle].particle_position(idx(nu - 1, j))) .collect(); let width = step * (nu - 1) as Real; + // The driven column and the middle one follow the tears: the pieces become bodies. + let mut right: Vec<(SoftBodyHandle, u32)> = + (0..nv).map(|j| (handle, idx(nu - 1, j) as u32)).collect(); + let mut middle: Vec<(SoftBodyHandle, u32)> = + (0..nv).map(|j| (handle, idx(nu / 2, j) as u32)).collect(); + let log = TearLog::default(); for k in 0..180 { let shift = width * (k as Real / 120.0).min(1.0); - for (j, p) in rest.iter().enumerate() { - world.soft_bodies[handle] - .set_particle_kinematic_target(idx(nu - 1, j), *p + Vector::new(shift, 0.0, 0.0)); + for (&(body, i), p) in right.iter().zip(&rest) { + world.soft_bodies[body] + .set_particle_kinematic_target(i as usize, *p + Vector::new(shift, 0.0, 0.0)); + } + world.step_with_events(&(), &log); + let events = log.drain(); + for r in right.iter_mut().chain(middle.iter_mut()) { + *r = follow(&events, r.0, r.1); + } + for h in family(&world, handle) { + assert_finite(&world, h); + world.soft_bodies[h].validate_topology().unwrap(); } - world.step(); - assert_finite(&world, handle); - world.soft_bodies[handle].validate_topology().unwrap(); } - let sb = &world.soft_bodies[handle]; - assert_eq!( - sb.boundary().len(), - num_surface, - "a cloth loses no triangle when it tears" - assert!( - sb.num_particles() > nu * nv, - "the crack did not split any particle" - ); + let bodies = family(&world, handle); + assert!(bodies.len() >= 2, "the cloth is still in one piece"); + let surface: usize = bodies.iter().map(|&h| world.soft_bodies[h].boundary().len()).sum(); + assert_eq!(surface, num_surface, "the torn cloth lost triangles"); + let particles: usize = bodies.iter().map(|&h| world.soft_bodies[h].num_particles()).sum(); + assert!(particles > nu * nv, "the crack did not split any particle"); // The two pinned columns are now more than twice the rest width apart: what still ties // them would be strained way past the threshold, so nothing does. The middle sagged under // gravity instead of being held taut. - let mid = (0..nv) - .map(|j| sb.particle_position(idx(nu / 2, j)).y) + let mid = middle + .iter() + .map(|&(body, i)| world.soft_bodies[body].particle_position(i as usize).y) .fold(Real::MAX, Real::min); assert!(mid < 1.9, "the torn cloth did not sag: {mid}"); - for e in sb.edges() { - let len = (sb.particle_position(e.vertices[0] as usize) - - sb.particle_position(e.vertices[1] as usize)) - .length(); - assert!( - len < e.rest_length * 1.6, - "an edge past the tear strain survived: {len} vs rest {}", - e.rest_length - ); + for &h in &bodies { + let sb = &world.soft_bodies[h]; + for e in sb.edges() { + let len = (sb.particle_position(e.vertices[0] as usize) + - sb.particle_position(e.vertices[1] as usize)) + .length(); + assert!( + len < e.rest_length * 1.6, + "an edge past the tear strain survived: {len} vs rest {}", + e.rest_length + ); + } } } @@ -1914,14 +1988,22 @@ fn tearing_splits_the_particles_a_crack_passes_through() { .map(|(i, _)| i as u32) .collect(); assert_eq!(warp.len(), n); - let torn = world.tear_soft_body(handle, &warp, &[]); - assert!(torn.is_some()); - let sb = &world.soft_bodies[handle]; - sb.validate_topology().unwrap(); - // One split per torn edge (its first endpoint, `(col, j)`), the triangles all still there. - assert_eq!(sb.num_particles(), num_particles + n); - assert_eq!(sb.boundary().len(), num_surface); - let mass: Real = sb.particles().iter().map(|p| p.mass()).sum(); + let event = world.tear_soft_body(handle, &warp, &[]).expect("nothing tore"); + // One split per torn edge (its first endpoint, `(col, j)`), the triangles all still there, + // and the cloth in two bodies: the wider left piece keeps the handle. + assert_eq!(event.pieces.len(), 2); + assert_eq!(event.pieces[0].soft_body, handle); + let bodies: Vec = event.bodies().collect(); + let (mut particles, mut surface, mut mass) = (0, 0, 0.0); + for &h in &bodies { + let sb = &world.soft_bodies[h]; + sb.validate_topology().unwrap(); + particles += sb.num_particles(); + surface += sb.boundary().len(); + mass += sb.particles().iter().map(|p| p.mass()).sum::(); + } + assert_eq!(particles, num_particles + n); + assert_eq!(surface, num_surface); assert!( (mass - num_particles as Real).abs() < 1.0e-4, "mass changed: {mass}" @@ -1930,15 +2012,21 @@ fn tearing_splits_the_particles_a_crack_passes_through() { // drift apart. for _ in 0..120 { world.step(); - assert_finite(&world, handle); + for &h in &bodies { + assert_finite(&world, h); + } + } + for &h in &bodies { + world.soft_bodies[h].validate_topology().unwrap(); } - let sb = &world.soft_bodies[handle]; - sb.validate_topology().unwrap(); // The copies are appended in crack order: the copy of `(col, j)` is particle // `num_particles + j`; the event says where each ended up. - let copy = num_particles + n / 2; - let gap = - (sb.particle_position(copy) - sb.particle_position(idx(col, n / 2) as usize)).length(); + let at = |i: u32| event.particle_destination(i).unwrap(); + let (copy, source) = (at((num_particles + n / 2) as u32), at(idx(col, n / 2))); + assert_ne!(copy.0, source.0, "the split particles stayed in one body"); + let position = + |(body, i): (SoftBodyHandle, u32)| world.soft_bodies[body].particle_position(i as usize); + let gap = (position(copy) - position(source)).length(); assert!(gap > 0.05, "the split particles did not separate: {gap}"); } @@ -1967,7 +2055,7 @@ fn jelly_bar_tears_when_pulled_apart() { .particle_mass(0.05); let handle = world.insert_soft_body(bar); let sb = &world.soft_bodies[handle]; - let num_cells = sb.cells().len(); + let (num_cells, num_particles) = (sb.cells().len(), sb.num_particles()); let left: Vec = (0..sb.num_particles()) .filter(|&i| sb.particle_position(i).x < -1.19) .collect(); @@ -1982,40 +2070,50 @@ fn jelly_bar_tears_when_pulled_apart() { .iter() .map(|&i| world.soft_bodies[handle].particle_position(i)) .collect(); + // The driven particles follow the tears: the piece they are in becomes its own body. + let mut right: Vec<(SoftBodyHandle, u32)> = + right.iter().map(|&i| (handle, i as u32)).collect(); + let log = TearLog::default(); for k in 0..240 { let shift = 2.4 * (k as Real / 180.0).min(1.0); - for (&i, p) in right.iter().zip(rest.iter()) { - world.soft_bodies[handle] - .set_particle_kinematic_target(i, *p + Vector::new(shift, 0.0, 0.0)); + for (&(body, i), p) in right.iter().zip(rest.iter()) { + world.soft_bodies[body] + .set_particle_kinematic_target(i as usize, *p + Vector::new(shift, 0.0, 0.0)); } - world.step(); - assert_finite(&world, handle); - world.soft_bodies[handle].validate_topology().unwrap(); - } - let sb = &world.soft_bodies[handle]; - assert!( - sb.cells().len() < num_cells, - "no cell tore ({num_cells} before and after)" - ); - assert!(!sb.boundary().is_empty()); - for c in sb.cells() { - for &v in &c.vertices { - assert!((v as usize) < sb.num_particles()); + world.step_with_events(&(), &log); + let events = log.drain(); + for r in &mut right { + *r = follow(&events, r.0, r.1); + } + for h in family(&world, handle) { + assert_finite(&world, h); + world.soft_bodies[h].validate_topology().unwrap(); } } + let bodies = family(&world, handle); + assert!(bodies.len() >= 2, "the bar is still in one piece"); + let cells: usize = bodies.iter().map(|&h| world.soft_bodies[h].cells().len()).sum(); + assert_eq!(cells, num_cells, "a tear removed cells"); + let particles: usize = bodies.iter().map(|&h| world.soft_bodies[h].num_particles()).sum(); + assert!(particles > num_particles, "no particle split"); // The bar is now torn: no remaining cell is stretched past twice the tear strain. let mut max_stretch: Real = 0.0; - for c in sb.cells() { - for a in 0..4 { - for b in a + 1..4 { - let (pa, pb) = ( - sb.particle_position(c.vertices[a] as usize), - sb.particle_position(c.vertices[b] as usize), - ); - let rest = (sb.particles()[c.vertices[a] as usize].rest_position() - - sb.particles()[c.vertices[b] as usize].rest_position()) - .length(); - max_stretch = max_stretch.max((pa - pb).length() / rest); + for &h in &bodies { + let sb = &world.soft_bodies[h]; + assert!(!sb.boundary().is_empty()); + for c in sb.cells() { + for a in 0..4 { + for b in a + 1..4 { + let (pa, pb) = ( + sb.particle_position(c.vertices[a] as usize), + sb.particle_position(c.vertices[b] as usize), + ); + let rest = (sb.particles()[c.vertices[a] as usize].rest_position() + - sb.particles()[c.vertices[b] as usize].rest_position()) + .length(); + max_stretch = max_stretch.max((pa - pb).length() / rest); + } + } } } assert!( @@ -3675,6 +3773,7 @@ fn fem_cloth_tears_when_pulled_apart() { .iter() .map(|&i| world.soft_bodies[handle].particle_position(i)) .collect(); + let particles_before = world.soft_bodies[handle].num_particles(); let triangles_before = world.soft_bodies[handle].boundary().len(); let version_before = world.soft_bodies[handle].topology_version(); for step in 0..400 { @@ -3692,8 +3791,11 @@ fn fem_cloth_tears_when_pulled_apart() { assert_finite(&world, handle); let sb = &world.soft_bodies[handle]; assert!( + sb.num_particles() > particles_before, "the FEM cloth did not tear: no particle split" ); + assert_eq!(sb.boundary().len(), triangles_before, "the torn FEM cloth lost triangles"); + sb.validate_topology().unwrap(); assert!( sb.topology_version() > version_before, "the topology version was not bumped" @@ -3728,6 +3830,8 @@ fn cut_splits_a_box_along_a_triangle() { world.cut_soft_body(handle, &aside).is_none(), "a blade beside the body cut something" ); + let sb = &world.soft_bodies[handle]; + let (num_cells, mass, measure) = (sb.cells().len(), sb.mass(), sb.rest_measure()); let event = world .cut_soft_body(handle, &blade) .expect("the blade crossed the box"); @@ -3768,6 +3872,173 @@ fn cut_splits_a_box_along_a_triangle() { } for _ in 0..60 { world.step(); + for &h in &bodies { + assert_finite(&world, h); + } + } + for &h in &bodies { + world.soft_bodies[h].validate_topology().unwrap(); + } +} + +/// A rope cut inserts two particles at the crossing: proportional rest lengths, the ends' masses +/// shared with the inserted particles, the wire segment split the same way, the rope apart. A +/// blade left across the cut ends inserts nothing more. +#[test] +fn cut_inserts_particles_into_a_rope() { + let mut world = world_with_ground(); + // Particles every 0.25 from x = 0 to x = 2; the blade crosses the segment (3, 4) at t = 0.6. + let rope = SoftBodyBuilder::rope(Vector::new(0.0, 3.0, 0.0), Vector::new(2.0, 3.0, 0.0), 9) + .pinned_particles([0]) + .particle_mass(0.1); + let handle = world.insert_soft_body(rope); + let blade = [ + Vector::new(0.9, -10.0, -10.0), + Vector::new(0.9, 20.0, -10.0), + Vector::new(0.9, -10.0, 20.0), + ]; + let event = world + .cut_soft_body(handle, &blade) + .expect("the blade crossed the rope"); + assert_eq!(event.inserted_particles, vec![9, 10]); + assert!(event.split_particles.is_empty()); + let mut removed = event.removed_edges.clone(); + removed.sort_unstable(); + assert_eq!(removed, vec![[2, 4], [3, 5]]); + // The longer, free half keeps the handle; the pinned half is a new body. The event maps + // every particle to its body and index there. + assert_eq!(event.pieces.len(), 2); + assert_eq!(event.pieces[0].particles, vec![4, 5, 6, 7, 8, 10]); + assert_eq!(event.pieces[1].particles, vec![0, 1, 2, 3, 9]); + let at = |i: u32| event.particle_destination(i).unwrap(); + let (free, pinned) = (&world.soft_bodies[handle], &world.soft_bodies[at(0).0]); + assert_eq!((at(3), at(9)), ((at(0).0, 3), (at(0).0, 4))); + assert_eq!((at(10), at(4)), ((handle, 5), (handle, 0))); + free.validate_topology().unwrap(); + pinned.validate_topology().unwrap(); + let close = |a: Real, b: Real| (a - b).abs() < 1.0e-5; + let length = |sb: &SoftBody, a: u32, b: u32| { + sb.edges() + .iter() + .find(|e| e.vertices == [a, b]) + .map(|e| e.rest_length) + }; + assert!(close(length(pinned, 3, 4).unwrap(), 0.15) && close(length(free, 5, 0).unwrap(), 0.1)); + assert!(close(free.rest_measure() + pinned.rest_measure(), 2.0)); + assert!(close(free.mass() + pinned.mass(), 0.9)); + // Each end gives up half its mass (its share of the segment), then the four particles share + // that mass by the halves' lumped masses: 0.3 and 0.2 of it at each end of the halves. + let masses = [ + pinned.particles()[3].mass(), + pinned.particles()[4].mass(), + free.particles()[5].mass(), + free.particles()[0].mass(), + ]; + for (mass, expected) in masses.iter().zip([0.08, 0.03, 0.02, 0.07]) { + assert!(close(*mass, expected), "masses {masses:?}"); + } + assert!(pinned.particles()[0].is_pinned() && !pinned.particles()[4].is_pinned()); + // The wire came apart with the body: one wire per piece, over its particles. + for sb in [free, pinned] { + let wire = sb.meshes().next().unwrap(); + assert!(wire.is_wire()); + assert_eq!(wire.vertex_count(), sb.num_particles()); + assert_eq!(wire.indices().len(), sb.num_particles() - 1); + } + + let bodies: Vec = event.bodies().collect(); + for _ in 0..60 { + world.step(); + for &h in &bodies { + let num_particles = world.soft_bodies[h].num_particles(); + assert!(world.cut_soft_body(h, &blade).is_none()); + assert_eq!(world.soft_bodies[h].num_particles(), num_particles); + assert_finite(&world, h); + } + } + for &h in &bodies { + world.soft_bodies[h].validate_topology().unwrap(); + } +} + +/// A tear keeps every particle in a measure element, whatever it takes around a particle: tearing +/// every edge at a cloth corner, every cell at a box corner, and every edge of a rope. +#[test] +fn tears_keep_every_particle_in_an_element() { + let mut world = world_with_ground(); + let cloth = world.insert_soft_body( + SoftBodyBuilder::cloth( + Vector::new(0.0, 3.0, 0.0), + Vector::X * 0.2, + Vector::Z * 0.2, + 6, + 6, + ) + .pinned_particles([5]), + ); + let torn_box = world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(4.0, 2.0, 0.0), Vector::new(1.0, 0.5, 0.5), 5, 3, 3) + .particle_mass(0.2), + ); + let rope = world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(0.0, 3.0, 4.0), Vector::new(2.0, 3.0, 4.0), 6) + .pinned_particles([0]), + ); + world.step(); + + let touching = |elements: Vec| -> Vec { + (0..elements.len() as u32) + .filter(|&i| elements[i as usize]) + .collect() + }; + let sb = &world.soft_bodies[cloth]; + let corner_edges = touching(sb.edges().iter().map(|e| e.vertices.contains(&0)).collect()); + let event = world.tear_soft_body(cloth, &corner_edges, &[]) + .expect("the corner edges tore"); + // The torn edges reported are the ones a crack opened across, before the tear. + assert!(!event.torn_edges.is_empty() && event.torn_edges.iter().all(|e| e.contains(&0))); + + let sb = &world.soft_bodies[torn_box]; + let corner_cells = touching(sb.cells().iter().map(|c| c.vertices.contains(&0)).collect()); + world.tear_soft_body(torn_box, &[], &corner_cells); + + let all_edges: Vec = (0..world.soft_bodies[rope].edges().len() as u32).collect(); + world.tear_soft_body(rope, &all_edges, &[]); + + for _ in 0..60 { + world.step(); + } + for handle in [cloth, torn_box, rope] { + world.soft_bodies[handle].validate_topology().unwrap(); + assert_finite(&world, handle); + } +} + +/// A cut keeps every particle in a measure element: a blade cutting a box corner off. +#[test] +fn cuts_keep_every_particle_in_an_element() { + let mut world = world_with_ground(); + let cut_box = world.insert_soft_body( + SoftBodyBuilder::cuboid(Vector::new(-4.0, 2.0, 0.0), Vector::new(1.0, 0.5, 0.5), 5, 3, 3) + .particle_mass(0.2), + ); + world.step(); + // A triangle in the plane through the middle of the corner cube of the grid, across the + // diagonal from the corner. + let corner = world.soft_bodies[cut_box].particle_position(0); + let center = corner + Vector::splat(0.25); + let (u, w) = (Vector::new(1.0, -1.0, 0.0), Vector::new(1.0, 1.0, -2.0)); + let blade = [center + u * 2.0, center - u + w, center - u - w]; + let event = world + .cut_soft_body(cut_box, &blade) + .expect("the blade crossed the corner"); + assert_eq!(event.pieces.len(), 2); + world.soft_bodies[cut_box].validate_topology().unwrap(); + for _ in 0..60 { + world.step(); + } + world.soft_bodies[cut_box].validate_topology().unwrap(); + assert_finite(&world, cut_box); } /// The contact-modification hook sees the contacts between two soft surfaces (their @@ -3931,3 +4202,316 @@ fn small_ball_slides_over_cloth_without_ghost_bumps() { ); assert!(x > 0.6, "the cloth's vertices slowed the ball down: x = {x}"); } + +/// A tear or a cut never deletes material: the mass and the rest measure (cell volumes, surface +/// triangle areas without cells, or structural edge lengths) are conserved, every particle keeps +/// a measure element and the tables stay consistent. One test per body kind and tear source. +mod tear_and_cut_conserve_mass_and_measure { + use super::*; + + /// Asserts the tear invariants from the body before a tear or cut to the bodies after it. + fn assert_tear_invariants(before: &SoftBody, after: &[&SoftBody]) { + let close = |a: Real, b: Real| (a - b).abs() <= 1.0e-5 * a.abs().max(b.abs()); + let mass: Real = after.iter().map(|sb| sb.mass()).sum(); + let measure: Real = after.iter().map(|sb| sb.rest_measure()).sum(); + assert!( + close(before.mass(), mass), + "mass changed: {} before, {} after", + before.mass(), + mass + ); + assert!( + close(before.rest_measure(), measure), + "rest measure changed: {} before, {} after", + before.rest_measure(), + measure + ); + for after in after { + assert_piece_invariants(before, after); + } + } + + /// Asserts that every particle of a piece keeps a measure element and its tables hold. + fn assert_piece_invariants(before: &SoftBody, after: &SoftBody) { + // The measure elements are chosen by the body kind before the tear. + let elements: Vec> = if !before.cells().is_empty() { + after.cells().iter().map(|c| c.vertices.to_vec()).collect() + } else if !before.boundary().is_empty() { + after.boundary().iter().map(|t| t.to_vec()).collect() + } else { + after + .edges() + .iter() + .filter(|e| e.kind == SoftBodyEdgeKind::Structural) + .map(|e| e.vertices.to_vec()) + .collect() + }; + let mut supported = vec![false; after.num_particles()]; + for element in &elements { + for &v in element { + supported[v as usize] = true; + } + } + if let Some(v) = supported.iter().position(|s| !s) { + panic!("particle {v} belongs to no measure element"); + } + after.validate_topology().unwrap(); + } + + /// Inserts the body, tears it with `tear`, checks the invariants, then steps the torn body. + fn check( + builder: SoftBodyBuilder, + tear: impl FnOnce(&mut PhysicsWorld, SoftBodyHandle) -> Option, + ) { + let mut world = PhysicsWorld::new(); + let handle = world.insert_soft_body(builder); + let before = world.soft_bodies[handle].clone(); + let event = tear(&mut world, handle).expect("nothing tore"); + let bodies: Vec = event.bodies().collect(); + let after: Vec<&SoftBody> = bodies.iter().map(|&h| &world.soft_bodies[h]).collect(); + assert_tear_invariants(&before, &after); + for _ in 0..10 { + world.step(); + for &h in &bodies { + assert_finite(&world, h); + } + } + for &h in &bodies { + world.soft_bodies[h].validate_topology().unwrap(); + } + } + + /// Tears the structural edge whose midpoint is closest to `point`. + fn tear_edge_near( + world: &mut PhysicsWorld, + handle: SoftBodyHandle, + point: Vector, + ) -> Option { + let sb = &world.soft_bodies[handle]; + let distance = |e: &SoftBodyEdge| { + let mid = (sb.particle_position(e.vertices[0] as usize) + + sb.particle_position(e.vertices[1] as usize)) + * 0.5; + (mid - point).length() + }; + let edge = (0..sb.edges().len()) + .filter(|&i| sb.edges()[i].kind == SoftBodyEdgeKind::Structural) + .min_by(|&i, &j| distance(&sb.edges()[i]).total_cmp(&distance(&sb.edges()[j]))) + .unwrap() as u32; + world.tear_soft_body(handle, &[edge], &[]) + } + + /// Tears the cell whose centroid is closest to `point`. + fn tear_cell_near( + world: &mut PhysicsWorld, + handle: SoftBodyHandle, + point: Vector, + ) -> Option { + let sb = &world.soft_bodies[handle]; + let distance = |c: &SoftBodyCell| { + let sum: Vector = c + .vertices + .iter() + .map(|&v| sb.particle_position(v as usize)) + .sum(); + (sum / c.vertices.len() as Real - point).length() + }; + let cell = (0..sb.cells().len()) + .min_by(|&i, &j| distance(&sb.cells()[i]).total_cmp(&distance(&sb.cells()[j]))) + .unwrap() as u32; + world.tear_soft_body(handle, &[], &[cell]) + } + + fn cuboid() -> SoftBodyBuilder { + SoftBodyBuilder::cuboid(Vector::ZERO, Vector::new(1.0, 0.5, 0.5), 5, 3, 3) + .particle_mass(0.2) + } + + fn cloth() -> SoftBodyBuilder { + SoftBodyBuilder::cloth(Vector::ZERO, Vector::X * 0.2, Vector::Z * 0.2, 6, 6) + .particle_mass(0.05) + } + + fn rope() -> SoftBodyBuilder { + SoftBodyBuilder::rope(Vector::ZERO, Vector::new(2.0, 0.0, 0.0), 9) + } + + /// Cuts along a large triangle blade in the plane `x = x`. + fn cut_at( + x: Real, + ) -> impl FnOnce(&mut PhysicsWorld, SoftBodyHandle) -> Option { + let blade = [ + Vector::new(x, -10.0, -10.0), + Vector::new(x, 20.0, -10.0), + Vector::new(x, -10.0, 20.0), + ]; + move |world, handle| world.cut_soft_body(handle, &blade) + } + + #[test] + fn cuboid_edge_tear() { + check(cuboid(), |w, h| { + tear_edge_near(w, h, Vector::new(0.25, 0.0, 0.0)) + }); + } + + #[test] + fn cuboid_cell_tear() { + check(cuboid(), |w, h| tear_cell_near(w, h, Vector::ZERO)); + } + + #[test] + fn cuboid_cut() { + check(cuboid(), cut_at(0.25)); + } + + #[test] + fn cloth_edge_tear() { + check(cloth(), |w, h| { + tear_edge_near(w, h, Vector::new(0.5, 0.0, 0.4)) + }); + } + + #[test] + fn cloth_two_sides_tear() { + // Two sides of the triangle `(2, 2), (3, 2), (3, 3)` of the cloth grid. + check(cloth(), |w, h| { + let first = tear_edge_near(w, h, Vector::new(0.5, 0.0, 0.4))?; + let second = tear_edge_near(w, h, Vector::new(0.6, 0.0, 0.5))?; + Some(SoftBodyTearEvent { + split_particles: [first.split_particles, second.split_particles].concat(), + ..second + }) + }); + } + + #[test] + fn cloth_cut() { + check(cloth(), cut_at(0.5)); + } + + #[test] + fn sphere_shell_edge_tear() { + check(SoftBodyBuilder::sphere(Vector::ZERO, 1.0, 2), |w, h| { + let e = w.soft_bodies[h].edges()[0].vertices; + let mid = (w.soft_bodies[h].particle_position(e[0] as usize) + + w.soft_bodies[h].particle_position(e[1] as usize)) + * 0.5; + tear_edge_near(w, h, mid) + }); + } + + #[test] + fn rope_edge_tear() { + check(rope(), |w, h| { + tear_edge_near(w, h, Vector::new(0.875, 0.0, 0.0)) + }); + } + + #[test] + fn rope_cut() { + check(rope(), cut_at(0.9)); + } +} + +/// Volume preservation holds every piece of material enclosed by its own closed boundary on its +/// own target: of two spheres appended into one body, squeezing one does not inflate the other. +#[test] +fn appended_spheres_keep_their_own_volumes() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let center = Vector::new(-1.5, 0.0, 0.0); + let first = SoftBodyBuilder::sphere(center, 1.0, 2); + let n = first.particle_positions().len(); + let spheres = first + .append(SoftBodyBuilder::sphere(Vector::new(1.5, 0.0, 0.0), 1.0, 2)) + .pinned_particles(0..n as u32) + .softness(SpringCoefficients::new(20.0, 1.0)) + .can_sleep(false); + let handle = world.insert_soft_body(spheres); + let sb = &world.soft_bodies[handle]; + assert!(sb.volume_preservation_enabled()); + let pieces = sb.volume_pieces(); + assert_eq!(pieces.len(), 2); + assert_eq!(pieces[0].particles(), (0..n as u32).collect::>()); + let rest = pieces[1].rest_volume(); + assert!(rest > 3.0, "icosphere volume too small: {rest}"); + assert!((pieces[0].rest_volume() - rest).abs() < 1.0e-4 * rest); + + // The pinned sphere is squeezed to half its volume. + let sb = &mut world.soft_bodies[handle]; + let scale = Real::cbrt(0.5); + for i in 0..n { + let p = sb.particle_position(i); + sb.set_particle_position(i, center + (p - center) * scale); + } + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let squeezed = sb.volume_pieces()[0].volume(sb); + let free = sb.volume_pieces()[1].volume(sb); + assert!( + (squeezed - 0.5 * rest).abs() < 0.01 * rest, + "the pinned sphere moved: {squeezed} vs {}", + 0.5 * rest + ); + assert!( + (free - rest).abs() < 0.02 * rest, + "the free sphere traded volume with the squeezed one: {free} vs {rest}" + ); +} + +/// Two pieces of one body keep the gaps their features had at rest: the halves of a cut body, +/// interlocked along the cells' facets within the contact skins, rest exactly as cut instead of +/// being pushed apart by the skins; shoved into each other, they collide and come back out. +#[test] +fn cut_halves_rest_as_cut_and_collide_when_shoved() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let cuboid = SoftBodyBuilder::cuboid(Vector::ZERO, Vector::new(1.0, 0.5, 0.5), 5, 3, 3) + .particle_mass(0.1) + .can_sleep(false); + let handle = world.insert_soft_body(cuboid); + world.step(); + let blade = [ + Vector::new(0.2, -10.0, -10.0), + Vector::new(0.2, 20.0, -10.0), + Vector::new(0.2, -10.0, 20.0), + ]; + let event = world + .cut_soft_body(handle, &blade) + .expect("the blade crossed the box"); + let halves: Vec = event.bodies().collect(); + assert_eq!(halves.len(), 2); + let centroid = |world: &PhysicsWorld, h: SoftBodyHandle| world.soft_bodies[h].center_of_mass(); + let offset = |world: &PhysicsWorld| (centroid(world, halves[1]) - centroid(world, halves[0])).x; + let rest_offset = offset(&world); + for _ in 0..60 { + world.step(); + for &h in &halves { + let sb = &world.soft_bodies[h]; + let speed = sb.particle_velocities().map(|v| v.length()).fold(0.0, Real::max); + assert!(speed < 1.0e-3, "the halves moved on their own: {speed} m/s"); + } + } + assert!((offset(&world) - rest_offset).abs() < 1.0e-3); + + // Shoved into each other by a fraction of a cell, the halves collide and come back out. + let sign = rest_offset.signum(); + let shove = 0.15 * sign; + for i in 0..world.soft_bodies[halves[1]].num_particles() { + let p = world.soft_bodies[halves[1]].particle_position(i); + world.soft_bodies[halves[1]].set_particle_position(i, p - Vector::X * shove); + } + assert!((offset(&world) - rest_offset) * sign < -0.1); + for _ in 0..120 { + world.step(); + for &h in &halves { + assert_finite(&world, h); + } + } + let pushed_back = (offset(&world) - rest_offset) * sign; + assert!(pushed_back > -0.05, "the shoved halves stayed interlocked: {pushed_back}"); +} diff --git a/crates/rapier3d/tests/soft_body_clusters.rs b/crates/rapier3d/tests/soft_body_clusters.rs index 1696391ad..a4f045acf 100644 --- a/crates/rapier3d/tests/soft_body_clusters.rs +++ b/crates/rapier3d/tests/soft_body_clusters.rs @@ -640,3 +640,184 @@ fn a_rigid_collider_on_a_cluster_brings_the_world_to_the_particles() { let weight_y = world.bodies[weight].translation().y; assert!(weight_y > 0.2, "the weight fell through: {weight_y}"); } + +/// A one-particle cluster on a chain torn at that particle splits with it: the copy (the heavier +/// share) keeps the proxy and the joint follows the source at the same world anchor. The chain +/// splits into two soft bodies, the longer one keeping the handle, each with its own collider. +#[test] +fn torn_cluster_splits_and_its_joint_follows_the_source() { + let xs = [0.0, 1.0, 2.0, 3.0, 4.0, 6.0, 7.0, 8.0, 9.0]; + let positions: Vec = xs.iter().map(|&x| Vector::X * x).collect(); + let segments: Vec<[u32; 2]> = (0..8).map(|i| [i, i + 1]).collect(); + let builder = SoftBodyBuilder::new(positions) + .edges(segments.clone()) + .particle_mass(0.3) + .pinned_particles([0]); + let builder = builder.wire(segments); + let mut world = PhysicsWorld::new(); + let h = world.insert_soft_body(builder); + let cluster = world.add_soft_body_cluster(h, &[4]).unwrap(); + let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); + let anchor = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(Vector::X * 4.0)); + let joint = world.insert_impulse_joint( + anchor, + proxy, + GenericJointBuilder::new(JointAxesMask::LIN_AXES).build(), + ); + + // The chain tears at particle 4 (the endpoints tie, the smaller index goes first), then + // comes apart: the right piece is the longer one and keeps the handle. + let event = world.tear_soft_body(h, &[4], &[]).expect("nothing tore"); + assert_eq!(event.split_particles, vec![(9, 4)]); + assert_eq!(event.pieces.len(), 2); + let right = &event.pieces[0]; + let left = &event.pieces[1]; + assert_eq!(right.soft_body, h); + assert_eq!(right.particles, vec![5, 6, 7, 8, 9]); + assert_eq!(left.particles, vec![0, 1, 2, 3, 4]); + let lh = left.soft_body; + assert_eq!(world.soft_bodies[lh].origin(), Some(h)); + assert_eq!(world.soft_bodies[h].pieces(), &[lh]); + assert!(world.soft_bodies[h].origin().is_none()); + + // The grab cluster split with the particle: the copy (particle 4 of the right body after + // compaction) is the heavier and kept the proxy, the source (particle 4 of the left body) + // got a fresh one. + let splits: Vec<_> = event + .clusters + .iter() + .filter(|c| c.source_cluster == cluster) + .collect(); + assert_eq!(splits.len(), 2, "{:?}", event.clusters); + let kept = splits.iter().find(|c| c.keeps_proxy).unwrap(); + let fresh = splits.iter().find(|c| !c.keeps_proxy).unwrap(); + assert_eq!((kept.soft_body, kept.cluster, kept.proxy), (h, cluster, proxy)); + assert_eq!(fresh.soft_body, lh); + let (rb, lb) = (&world.soft_bodies[h], &world.soft_bodies[lh]); + assert_eq!(rb.cluster(kept.cluster).unwrap().particles(), &[4]); + assert_eq!(lb.cluster(fresh.cluster).unwrap().particles(), &[4]); + assert_eq!(world.bodies[fresh.proxy].soft_body(), Some(lh)); + assert_eq!(world.bodies[fresh.proxy].soft_cluster(), Some(fresh.cluster)); + assert!(rb.cluster(kept.cluster).unwrap().meshes().next().is_none()); + // The left body took the fresh pieces of both clusters (made in cluster order). + assert_eq!(left.clusters, vec![[2, 0], [3, 1]]); + assert_eq!(fresh.cluster, 1); + + // The joint sits on the split particle, at the same rest distance from both pieces: the + // source's wins the tie, and the anchor stays where the particle is. + assert_eq!(event.moved_joints.len(), 1); + let moved = event.moved_joints[0]; + assert_eq!((moved.joint, moved.from, moved.to), (joint, proxy, fresh.proxy)); + let j = world.impulse_joints.get(joint).unwrap(); + assert_eq!((j.body1(), j.body2()), (anchor, fresh.proxy)); + let world_anchor = *world.bodies[fresh.proxy].position() * j.data.local_frame2.translation; + assert!((world_anchor - lb.particle_position(4)).length() < 1.0e-4); + + // Each body has the whole-body cluster of its piece (the left one's was cluster 2 of the + // torn body, the crack's fresh piece of cluster 0), with its own collision mesh. + for (body, piece, source) in [(h, right, 0), (lh, left, 2)] { + let sb = &world.soft_bodies[body]; + assert_eq!(sb.num_particles(), 5); + assert_eq!(sb.connected_pieces().len(), 1); + let (ci, c) = sb.live_clusters().next().unwrap(); + assert_eq!(c.proxy(), sb.root_body()); + assert_eq!(c.particles(), &[0, 1, 2, 3, 4]); + assert_eq!(world.bodies[c.proxy()].soft_body(), Some(body)); + assert_eq!(world.bodies[c.proxy()].soft_cluster(), Some(ci)); + assert!(piece.clusters.contains(&[source, ci])); + let mesh = c.meshes().next().expect("the piece lost its mesh"); + assert_eq!(mesh.vertex_count(), 5); + assert_eq!(world.colliders[mesh.collider()].parent(), Some(c.proxy())); + assert_eq!( + world.colliders[mesh.collider()] + .deformable_mesh_ref() + .map(|r| (r.body, r.id)), + Some((body, mesh.id())) + ); + sb.validate_topology().unwrap(); + } + assert!(lb.particles()[0].is_pinned() && !rb.particles()[0].is_pinned()); + + for _ in 0..60 { + world.step(); + } + for body in [h, lh] { + let sb = &world.soft_bodies[body]; + assert!(sb.particles().iter().all(|p| p.position().is_finite())); + } + // The pinned piece hangs from its pin, the joint holds it to the anchor; the right piece + // fell away. + let (rb, lb) = (&world.soft_bodies[h], &world.soft_bodies[lh]); + assert!((lb.particle_position(4) - Vector::X * 4.0).length() < 0.5); + assert!(rb.particle_position(0).y < -2.0); +} + +/// A cluster whose two particles are joined only through particles outside it is one piece as +/// long as those links hold: a tear elsewhere leaves it alone, a tear through a link splits it, +/// the particle the tear never reached staying with the retained piece. +#[test] +fn cluster_joined_outside_itself_splits_only_through_its_link() { + let mut world = PhysicsWorld::new(); + let rope = SoftBodyBuilder::rope(Vector::ZERO, Vector::X * 12.0, 13) + .particle_mass(0.1) + .pinned_particles([0]); + let h = world.insert_soft_body(rope); + let cluster = world.add_soft_body_cluster(h, &[3, 7]).unwrap(); + + // Far from the cluster: the body splits (the piece past the tear falls away as a new body, + // the cluster's piece keeps the handle), the cluster does not. + let event = world.tear_soft_body(h, &[8], &[]).expect("nothing tore"); + assert_eq!(event.split_particles, vec![(13, 8)]); + assert!(event.clusters.iter().all(|c| c.source_cluster == 0)); + assert_eq!(event.clusters.len(), 2); + assert!(event.moved_joints.is_empty()); + assert_eq!(event.pieces.len(), 2); + assert_eq!(event.pieces[0].soft_body, h); + assert_eq!(event.pieces[0].particles, (0..9).collect::>()); + assert_eq!(event.pieces[0].clusters, vec![[0, 0], [cluster, cluster]]); + assert_eq!(world.soft_bodies[h].num_particles(), 9); + assert_eq!( + world.soft_bodies[h].cluster(cluster).unwrap().particles(), + &[3, 7] + ); + + // Through the link: particle 3 splits, its copy 9 takes the segment towards 4, and the + // three pinned segments come apart from the rest as a new body. + let event = world.tear_soft_body(h, &[3], &[]).expect("nothing tore"); + assert_eq!(event.split_particles, vec![(9, 3)]); + assert_eq!(event.pieces.len(), 2); + assert_eq!(event.pieces[0].particles, vec![4, 5, 6, 7, 8, 9]); + assert_eq!(event.pieces[1].particles, vec![0, 1, 2, 3]); + let tail = event.pieces[1].soft_body; + let splits: Vec<_> = event + .clusters + .iter() + .filter(|c| c.source_cluster == cluster) + .collect(); + // The crack split the cluster into the source's piece (with 7, unreached by the tear) and the + // copy's; the body split then parted 3 from 7, the heavier 7 keeping the proxy. Three pieces: + // 7 and the copy in the retained body, 3 in the pinned tail. + assert_eq!(splits.len(), 3, "{:?}", event.clusters); + let kept = splits.iter().find(|c| c.keeps_proxy).unwrap(); + assert_eq!((kept.soft_body, kept.cluster), (h, cluster)); + let sb = &world.soft_bodies[h]; + let tail_body = &world.soft_bodies[tail]; + assert_eq!(sb.cluster(cluster).unwrap().particles(), &[3]); + for fresh in splits.iter().filter(|c| !c.keeps_proxy) { + let body = &world.soft_bodies[fresh.soft_body]; + let particles = body.cluster(fresh.cluster).unwrap().particles(); + assert_eq!(particles, if fresh.soft_body == h { &[5] } else { &[3] }); + assert_eq!(world.bodies[fresh.proxy].soft_body(), Some(fresh.soft_body)); + } + assert_eq!(tail_body.num_live_clusters(), 2); + sb.validate_topology().unwrap(); + tail_body.validate_topology().unwrap(); + for _ in 0..30 { + world.step(); + } + for body in [h, tail] { + let sb = &world.soft_bodies[body]; + assert!(sb.particles().iter().all(|p| p.position().is_finite())); + } +} diff --git a/crates/rapier3d/tests/soft_body_grab.rs b/crates/rapier3d/tests/soft_body_grab.rs new file mode 100644 index 000000000..d6b4c7130 --- /dev/null +++ b/crates/rapier3d/tests/soft_body_grab.rs @@ -0,0 +1,354 @@ +//! Mouse-grab replays on tearable soft bodies (3D): the testbed grab (a one-particle cluster whose +//! proxy a motor-only joint pulls from a kinematic body) drags the `soft_tearing3` cloths and bar. +//! The simulation must stay finite and bounded, and the grab cluster within one connected piece. + +use rapier3d::prelude::*; + +/// Collects the tear events of every step. +#[derive(Default)] +struct TearLog(std::sync::Mutex>); + +impl TearLog { + fn drain(&self) -> Vec { + std::mem::take(&mut *self.0.lock().unwrap()) + } +} + +impl EventHandler for TearLog { + fn handle_collision_event( + &self, + _: &RigidBodySet, + _: &ColliderSet, + _: CollisionEvent, + _: Option<&ContactPair>, + ) { + } + fn handle_contact_force_event( + &self, + _: Real, + _: &RigidBodySet, + _: &ColliderSet, + _: &ContactPair, + _: Real, + ) { + } + fn handle_soft_body_tear_event(&self, _: &SoftBodySet, event: &SoftBodyTearEvent) { + self.0.lock().unwrap().push(event.clone()); + } +} + +/// Particle speed bound. Healthy runs of this scene peak under 37 m/s (the ball shot through the +/// curtain) and the mouse target moves at under 10 m/s; the divergent grabs exceed 400 m/s. +const MAX_PARTICLE_SPEED: Real = 80.0; +/// Gains of the testbed mouse joint (`src_testbed/grab.rs`). +const GRAB_STIFFNESS: Real = 1000.0; +const GRAB_DAMPING: Real = 50.0; +/// The mouse lifts by this height over one second, then circles with this radius and frequency. +const GRAB_LIFT: Real = 1.0; +const GRAB_RADIUS: Real = 1.5; +const GRAB_FREQUENCY: Real = 1.0; + +#[derive(Copy, Clone, PartialEq, Eq, Debug)] +enum Target { + Sheet, + Curtain, + Bar, +} + +#[derive(Default, Debug)] +struct Outcome { + peak_speed: Real, + peak_time: Real, + non_finite: Option, + /// First (time, speed) past `MAX_PARTICLE_SPEED`. + too_fast: Option<(Real, Real)>, + /// First (time, piece count) with the grab cluster spanning several pieces. + split_cluster: Option<(Real, usize)>, + end_time: Real, +} + +struct Scene { + world: PhysicsWorld, + soft_bodies: Vec<(Target, SoftBodyHandle)>, + /// Right-end bar particles (holding body, index there) and their rest positions pulled from + /// t = 1 s; they follow the bar's pieces through the tears. + bar_pull: Vec<(SoftBodyHandle, u32, Vector)>, +} + +fn cloth_material() -> SoftBodyMaterial { + SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + tear_strain: Some(0.4), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(100.0, 1.0)) + } +} + +/// The `soft_tearing3` scene: a pinned sheet hit by a falling ball, a curtain shot through by a +/// fast ball, and a bar pulled apart by its kinematic ends. +fn scene() -> Scene { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(30.0, 0.1, 30.0), + ); + + let n = 40usize; + let border = (0..n * n).filter_map(move |k| { + let (i, j) = (k / n, k % n); + (i == 0 || j == 0 || i == n - 1 || j == n - 1).then_some(k as u32) + }); + let sheet = SoftBodyBuilder::cloth( + Vector::new(-6.0, 3.0, -2.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .pinned_particles(border) + .material(cloth_material()) + .particle_mass(0.02) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.8)); + let sheet = world.insert_soft_body(sheet); + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(-4.0, 6.0, 0.0)), + ColliderBuilder::ball(0.6).density(15.0), + ); + + let curtain = SoftBodyBuilder::cloth( + Vector::new(0.0, 4.5, 4.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, -0.1, 0.0), + 50, + 40, + ) + .pinned_particles((0..50).map(|i| (i * 40) as u32)) + .material(cloth_material()) + .particle_mass(0.02); + let curtain = world.insert_soft_body(curtain); + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(2.5, 2.5, 12.0)) + .linvel(Vector::new(0.0, 0.0, -25.0)), + ColliderBuilder::ball(0.5).density(10.0), + ); + + let bar = SoftBodyBuilder::cuboid( + Vector::new(3.0, 1.0, -4.0), + Vector::new(2.0, 0.4, 0.4), + 21, + 5, + 5, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 5.0e4, + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + tear_strain: Some(0.4), + ..Default::default() + }) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); + let bar = world.insert_soft_body(bar); + let sb = &mut world.soft_bodies[bar]; + let mut bar_pull = Vec::new(); + for i in 0..sb.num_particles() { + let p = sb.particle_position(i); + if p.x < 1.01 || p.x > 4.99 { + sb.set_particle_pinned(i, true); + } + if p.x > 4.99 { + bar_pull.push((bar, i as u32, p)); + } + } + + Scene { + world, + soft_bodies: vec![ + (Target::Sheet, sheet), + (Target::Curtain, curtain), + (Target::Bar, bar), + ], + bar_pull, + } +} + +/// The free particle of the biggest piece nearest to that piece's free-particle centroid. +fn grab_particle(sb: &SoftBody) -> u32 { + let pieces = sb.connected_pieces(); + let biggest = pieces.iter().max_by_key(|p| p.len()).unwrap(); + let free: Vec = biggest + .iter() + .copied() + .filter(|&v| !sb.particles()[v as usize].is_pinned()) + .collect(); + let centroid = free + .iter() + .map(|&v| sb.particle_position(v as usize)) + .sum::() + / free.len() as Real; + *free + .iter() + .min_by(|a, b| { + let da = (sb.particle_position(**a as usize) - centroid).length(); + let db = (sb.particle_position(**b as usize) - centroid).length(); + da.total_cmp(&db) + }) + .unwrap() +} + +/// Grabs `target` at `grab_time` and drags it until `end_time`, recording the first violation of +/// each check. +fn simulate(target: Target, grab_time: Real, end_time: Real) -> Outcome { + let Scene { + mut world, + soft_bodies, + mut bar_pull, + } = scene(); + let grabbed = soft_bodies.iter().find(|(t, _)| *t == target).unwrap().1; + let log = TearLog::default(); + let dt = world.integration_parameters.dt; + let mut outcome = Outcome::default(); + let mut grab: Option<(RigidBodyHandle, RigidBodyHandle, Vector)> = None; + let mut t: Real = 0.0; + + while t < end_time { + if grab.is_none() && t >= grab_time { + let particle = grab_particle(&world.soft_bodies[grabbed]); + let anchor = world.soft_bodies[grabbed].particle_position(particle as usize); + let cluster = world + .add_soft_body_cluster(grabbed, &[particle]) + .expect("grab cluster refused"); + let proxy = world.soft_bodies[grabbed].cluster_proxy(cluster).unwrap(); + let mouse = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); + let joint = GenericJointBuilder::new(JointAxesMask::empty()) + .motor_position(JointAxis::LinX, 0.0, GRAB_STIFFNESS, GRAB_DAMPING) + .motor_position(JointAxis::LinY, 0.0, GRAB_STIFFNESS, GRAB_DAMPING) + .motor_position(JointAxis::LinZ, 0.0, GRAB_STIFFNESS, GRAB_DAMPING); + world.insert_impulse_joint(mouse, proxy, joint); + grab = Some((mouse, proxy, anchor)); + } + if let Some((mouse, _, anchor)) = grab { + let tg = t - grab_time; + let lift = tg.min(1.0) * GRAB_LIFT; + let phase = (tg - 1.0).max(0.0) * GRAB_FREQUENCY * std::f64::consts::TAU as Real; + let circle = Vector::new(phase.sin(), 1.0 - phase.cos(), 0.0) * GRAB_RADIUS; + world.bodies[mouse].set_next_kinematic_translation(anchor + Vector::Y * lift + circle); + } + let shift = ((t - 1.0).max(0.0) * 0.5).min(4.0); + for &(body, i, rest) in &bar_pull { + world.soft_bodies[body] + .set_particle_kinematic_target(i as usize, rest + Vector::new(shift, 0.0, 0.0)); + } + world.step_with_events(&(), &log); + for event in log.drain() { + for pull in &mut bar_pull { + if event.soft_body == pull.0 { + if let Some((body, i)) = event.particle_destination(pull.1) { + (pull.0, pull.1) = (body, i); + } + } + } + } + t += dt; + outcome.end_time = t; + + let mut finite = world + .bodies + .iter() + .all(|(_, rb)| rb.translation().is_finite() && rb.linvel().is_finite()); + let mut speed: Real = 0.0; + for (_, sb) in world.soft_bodies.iter() { + for p in sb.particles() { + finite &= p.position().is_finite() && p.velocity().is_finite(); + speed = speed.max(p.velocity().length()); + } + } + if speed > outcome.peak_speed { + outcome.peak_speed = speed; + outcome.peak_time = t; + } + if speed > MAX_PARTICLE_SPEED && outcome.too_fast.is_none() { + outcome.too_fast = Some((t, speed)); + } + if let Some((_, proxy, _)) = grab { + // The cluster follows the tears: its proxy says which body holds it now, and the + // cluster must lie within one connected piece of that body. + let rb = world.bodies.get(proxy).expect("grab proxy removed"); + let body = rb.soft_body().expect("grab proxy lost its soft body"); + let cluster = rb.soft_cluster().expect("grab proxy lost its cluster"); + let sb = &world.soft_bodies[body]; + let mut piece_of = vec![0usize; sb.num_particles()]; + for (k, piece) in sb.connected_pieces().iter().enumerate() { + for &v in piece { + piece_of[v as usize] = k; + } + } + let cl = sb.cluster(cluster).expect("grab cluster removed"); + let mut pieces: Vec = cl + .particles() + .iter() + .map(|&v| piece_of[v as usize]) + .collect(); + pieces.sort_unstable(); + pieces.dedup(); + if pieces.len() > 1 && outcome.split_cluster.is_none() { + outcome.split_cluster = Some((t, pieces.len())); + } + } + if !finite { + outcome.non_finite = Some(t); + break; + } + // Past this speed the run has diverged; the rest of it is noise. + if speed > 1.0e3 { + break; + } + } + outcome +} + +fn check(target: Target, grab_time: Real, end_time: Real) { + let outcome = simulate(target, grab_time, end_time); + let mut failures = Vec::new(); + if let Some(t) = outcome.non_finite { + failures.push(format!("non-finite state at t = {t:.3} s")); + } + if let Some((t, speed)) = outcome.too_fast { + failures.push(format!( + "particle speed {speed:.1} m/s past {MAX_PARTICLE_SPEED} m/s at t = {t:.3} s" + )); + } + if let Some((t, pieces)) = outcome.split_cluster { + failures.push(format!( + "grab cluster spans {pieces} pieces at t = {t:.3} s" + )); + } + assert!( + failures.is_empty(), + "{target:?} grabbed at t = {grab_time} s: {} (peak {:.1} m/s at t = {:.3} s, stopped at t = {:.3} s)", + failures.join("; "), + outcome.peak_speed, + outcome.peak_time, + outcome.end_time, + ); +} + +/// Checks the sheet stays bounded when grabbed before the ball lands. +#[test] +fn grabbing_the_sheet_stays_bounded() { + check(Target::Sheet, 0.5, 2.0); +} + +/// Checks the torn curtain stays bounded when grabbed after the ball went through. +#[test] +fn grabbing_the_torn_curtain_stays_bounded() { + check(Target::Curtain, 4.0, 8.0); +} + +/// Checks the stretched bar stays bounded when grabbed while its ends are pulled apart. +#[test] +fn grabbing_the_stretched_bar_stays_bounded() { + check(Target::Bar, 4.0, 6.0); +} diff --git a/crates/rapier3d/tests/soft_body_meshes.rs b/crates/rapier3d/tests/soft_body_meshes.rs index a071b7a63..9b31e20b3 100644 --- a/crates/rapier3d/tests/soft_body_meshes.rs +++ b/crates/rapier3d/tests/soft_body_meshes.rs @@ -378,11 +378,11 @@ fn a_skinned_mesh_follows_its_cells_through_a_tear() { }; // Tear a cell of the ball: a crack opens through it, under the skin. - let cells_before = world.soft_bodies[handle].cells().len(); + let particles_before = world.soft_bodies[handle].num_particles(); world.soft_bodies[handle].tear_cell(0); world.step(); assert!( - world.soft_bodies[handle].cells().len() < cells_before, + world.soft_bodies[handle].num_particles() > particles_before, "nothing was torn" ); @@ -450,3 +450,131 @@ fn a_skinned_mesh_binds_where_it_is_given() { ); } } + +/// The surface of a box (`center`, `half_extents`) with `m × m` quads per face, shared vertices +/// along the box edges, and outward triangles. +fn box_surface(center: Vector, half_extents: Vector, m: usize) -> (Vec, Vec<[u32; 3]>) { + use std::collections::HashMap; + let mut ids: HashMap<[usize; 3], u32> = HashMap::new(); + let mut vertices = Vec::new(); + let mut vertex = |g: [usize; 3], vertices: &mut Vec| { + *ids.entry(g).or_insert_with(|| { + let t = |k: usize| -1.0 + 2.0 * g[k] as Real / m as Real; + vertices.push(center + Vector::new(t(0), t(1), t(2)) * half_extents); + vertices.len() as u32 - 1 + }) + }; + let mut indices = Vec::new(); + for axis in 0..3 { + let (u, w) = ((axis + 1) % 3, (axis + 2) % 3); + for side in [0, m] { + for i in 0..m { + for j in 0..m { + let corner = |di: usize, dj: usize| { + let mut g = [0; 3]; + g[axis] = side; + g[u] = i + di; + g[w] = j + dj; + g + }; + let q = [corner(0, 0), corner(1, 0), corner(1, 1), corner(0, 1)] + .map(|g| vertex(g, &mut vertices)); + for mut tri in [[q[0], q[1], q[2]], [q[0], q[2], q[3]]] { + let p = tri.map(|v| vertices[v as usize]); + let normal = (p[1] - p[0]).cross(p[2] - p[0]); + if normal.dot(p[0] - center) < 0.0 { + tri.swap(1, 2); + } + indices.push(tri); + } + } + } + } + } + (vertices, indices) +} + +/// A cut through a body wearing a skin separates the skin too: every skin element ends on the +/// cells of one piece (the vertices of an element spanning the crack are duplicated onto the +/// majority side), whether the skin collides or is only drawn. +#[test] +fn a_skinned_mesh_comes_apart_with_its_cells() { + for collides in [true, false] { + let mut world = world_with_ground(); + let center = Vector::new(0.0, 1.5, 0.0); + let half_extents = Vector::new(1.0, 0.5, 0.5); + let (vertices, indices) = box_surface(center, half_extents * 0.95, 8); + let handle = world.insert_soft_body( + SoftBodyBuilder::cuboid(center, half_extents, 5, 3, 3) + .skin(vertices, indices) + .skin_collision(collides) + .particle_mass(0.05), + ); + world.step(); + let skins = |sb: &SoftBody| -> Vec { + sb.meshes().filter(|mesh| mesh.is_skinned()).cloned().collect() + }; + let vertex_count = skins(&world.soft_bodies[handle])[0].vertex_count(); + + let blade = [ + Vector::new(0.1, -5.0, -5.0), + Vector::new(0.1, 15.0, -5.0), + Vector::new(0.1, -5.0, 15.0), + ]; + let event = world + .cut_soft_body(handle, &blade) + .expect("the blade crossed the box"); + // The box is two bodies now, the skin came apart with them: one skin per body, over + // that body's cells alone. + assert_eq!(event.pieces.len(), 2); + let bodies: Vec = event.bodies().collect(); + let mut vertices = 0; + let mut positions = Vec::new(); + for &h in &bodies { + let sb = &world.soft_bodies[h]; + sb.validate_topology().unwrap(); + assert_eq!(sb.connected_pieces().len(), 1); + let meshes = skins(sb); + assert_eq!(meshes.len(), 1); + let mesh = &meshes[0]; + assert_eq!(mesh.collision_enabled(), collides); + assert!(!mesh.indices().is_empty()); + vertices += mesh.vertex_count(); + let SoftMeshMapping::Skinned { bindings } = mesh.binding() else { + panic!("the skin is not skinned"); + }; + for (i, element) in mesh.indices().iter().enumerate() { + for &v in element { + assert!( + (bindings[v as usize].cell as usize) < sb.cells().len(), + "skin element {i} rides a cell of the other piece (collides: {collides})" + ); + } + } + positions.push(mesh.vertex_positions(sb).collect::>()); + } + assert!(vertices > vertex_count, "no skin vertex was duplicated across the cut"); + // The duplicated vertices start where their source was: nothing jumps. + world.step(); + let mut moved: Real = 0.0; + for (&h, then) in bodies.iter().zip(&positions) { + let sb = &world.soft_bodies[h]; + for (now, then) in skins(sb)[0].vertex_positions(sb).zip(then) { + moved = moved.max((now - *then).length()); + } + } + assert!(moved < 0.1, "a skin vertex jumped {moved} through the cut (collides: {collides})"); + + for _ in 0..120 { + world.step(); + } + for &h in &bodies { + let sb = &world.soft_bodies[h]; + sb.validate_topology().unwrap(); + assert!(sb.particle_positions().all(|p| p.is_finite())); + for mesh in skins(sb) { + assert!(mesh.vertex_positions(sb).all(|p| p.is_finite())); + } + } + } +} diff --git a/examples2d/soft_tearing2.rs b/examples2d/soft_tearing2.rs index 230b1cad0..33ce2b299 100644 --- a/examples2d/soft_tearing2.rs +++ b/examples2d/soft_tearing2.rs @@ -2,8 +2,11 @@ //! hanging strip shot through by a fast disk, and a jelly bar pulled apart by its kinematic ends. //! Cracks split particles and shed pieces as new soft bodies; the panel sets `min_piece`. -use rapier_testbed2d::TestbedViewer; use rapier2d::prelude::*; +use rapier_testbed2d::{ + TestbedViewer, + egui::{Align2, Slider, Window}, +}; pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let mut world = PhysicsWorld::new(); @@ -96,8 +99,32 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { viewer.set_world(&mut world); viewer.look_at(Vec2::new(0.0, 4.0), 40.0); + // Demo UI: the smallest piece a tear may split off, applied to every soft body (the pieces + // a tear splits off inherit their origin's material). + let mut min_piece_default = true; + let mut min_piece: u32 = 3; let mut t: Real = 0.0; while viewer.render_frame(&mut world).await { + let mut changed = false; + Window::new("Tearing") + .anchor(Align2::RIGHT_TOP, [-15.0, 15.0]) + .show(viewer.egui_context(), |ui| { + changed |= ui + .checkbox(&mut min_piece_default, "Default minimum piece (3 cells)") + .changed(); + changed |= ui + .add_enabled( + !min_piece_default, + Slider::new(&mut min_piece, 1..=40).text("minimum piece (elements)"), + ) + .changed(); + }); + if changed { + let value = (!min_piece_default).then_some(min_piece); + for (_, sb) in world.soft_bodies.iter_mut() { + sb.material_mut().min_piece = value; + } + } if viewer.simulating() { t += world.integration_parameters.dt; // The right end of the bar starts moving after a second, at half a meter per diff --git a/examples3d/soft_tearing3.rs b/examples3d/soft_tearing3.rs index b4a1f8334..82e6c1431 100644 --- a/examples3d/soft_tearing3.rs +++ b/examples3d/soft_tearing3.rs @@ -2,8 +2,11 @@ //! shot through by a ball, and a jelly bar pulled apart by its kinematic ends. Cloth springs are //! stiff (100 Hz) so only impacts pass the 0.4 tear strain; the panel sets `min_piece`. -use rapier_testbed3d::TestbedViewer; use rapier3d::prelude::*; +use rapier_testbed3d::{ + TestbedViewer, + egui::{Align2, Slider, Window}, +}; pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let mut world = PhysicsWorld::new(); @@ -114,8 +117,35 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { viewer.set_world(&mut world); viewer.look_at(Vec3::new(9.0, 8.0, 16.0), Vec3::new(0.0, 1.5, 1.0)); + // Demo UI: the smallest piece a tear may split off, applied to every soft body (the pieces + // a tear splits off inherit their origin's material). + let mut min_piece_default = true; + let mut min_piece: u32 = 6; let mut t: Real = 0.0; while viewer.render_frame(&mut world).await { + let mut changed = false; + Window::new("Tearing") + .anchor(Align2::RIGHT_TOP, [-15.0, 15.0]) + .show(viewer.egui_context(), |ui| { + changed |= ui + .checkbox( + &mut min_piece_default, + "Default minimum piece (10 triangles or 6 cells)", + ) + .changed(); + changed |= ui + .add_enabled( + !min_piece_default, + Slider::new(&mut min_piece, 1..=40).text("minimum piece (elements)"), + ) + .changed(); + }); + if changed { + let value = (!min_piece_default).then_some(min_piece); + for (_, sb) in world.soft_bodies.iter_mut() { + sb.material_mut().min_piece = value; + } + } if viewer.simulating() { t += world.integration_parameters.dt; // The right end of the bar starts moving after a second, at half a meter per diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh.rs index 09a36d9d4..d7104d5dc 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh.rs @@ -163,4 +163,6 @@ pub struct SoftCollisionMesh { pub(crate) struct SoftTopologyRemap<'a> { pub cells: &'a [u32], pub particles: &'a [u32], + pub split: &'a [(u32, u32)], + pub inserted: &'a [[u32; 4]], } diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_binding.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_binding.rs index ba21b2426..056abe01a 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_binding.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_binding.rs @@ -234,5 +234,31 @@ pub enum SoftBindingError { /// The binding of `position` to the closest cell of `body` accepted by `allowed` (`None` without /// any such cell). // TODO: optimize neighrest neighbor search with the BVH? -pub(super) fn closest_cell_binding(body: &SoftBody, position: Vector) -> Option { +pub(super) fn closest_cell_binding( + body: &SoftBody, + position: Vector, + allowed: impl Fn(u32) -> bool, +) -> Option { + let cell_points = + |cell: &SoftBodyCell| cell.vertices.map(|v| body.particles[v as usize].position); + let mut by_distance: Vec<(Real, u32)> = body + .cells + .iter() + .enumerate() + .filter(|(id, _)| allowed(*id as u32)) + .map(|(id, cell)| { + ( + distance_squared_to_cell(position, cell_points(cell)), + id as u32, + ) + }) + .collect(); + by_distance.sort_unstable_by(|a, b| a.0.total_cmp(&b.0)); + by_distance.iter().find_map(|(_, cell)| { + let weights = barycentric(position, cell_points(&body.cells[*cell as usize]))?; + Some(SoftMeshCellBinding { + cell: *cell, + weights, + }) + }) } diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs index c2f7df307..1e534ea7a 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs @@ -129,12 +129,40 @@ impl SoftCollisionMesh { } else { Triangle::new(rest[0], rest[1], rest[2]).distance_to_local_point(p, true) }; - if dist < clearance { + if dist < clearance && !self.across_crack(body, vertex, element) { + return true; } } false } + /// Whether `element` holds a copy of the particle of `vertex` or of one of its surface + /// neighbors: the pair faces across a crack, which rests at distance zero yet must collide. + /// Always `false` for a skinned mesh, whose vertices are not particles. + fn across_crack(&self, body: &SoftBody, vertex: u32, element: &[u32]) -> bool { + let super::SoftMeshMapping::Direct { particles } = &self.binding else { + return false; + }; + let family = |v: u32| { + let p = particles[v as usize]; + (p, body.particles[p as usize].split_root(p)) + }; + let (start, end) = match ( + self.ring_offsets.get(vertex as usize), + self.ring_offsets.get(vertex as usize + 1), + ) { + (Some(&start), Some(&end)) => (start as usize, end as usize), + _ => (0, 0), + }; + let near = core::iter::once(&vertex).chain(&self.ring[start..end]); + near.map(|&w| family(w)).any(|(p, root)| { + element.iter().any(|&e| { + let (q, other_root) = family(e); + other_root == root && q != p + }) + }) + } + /// Whether `vertex` is exposed along `dir`: no surface neighbor lies ahead of it (up to a few /// degrees), so a body approaching along `-dir` meets the vertex first. A contact at a vertex /// that is not exposed along its normal is a ghost of an incident element's contact. @@ -290,6 +318,28 @@ impl SoftCollisionMesh { }) } + /// The outward direction of a closed mesh at one of its vertices: the normalized sum of the + /// outward normals of the elements around it (`None` for an open mesh, and where no element + /// can orient itself). + pub(crate) fn vertex_outward_normal(&self, body: &SoftBody, vertex: u32) -> Option { + let mut sum = Vector::ZERO; + let (Some(&start), Some(&end)) = ( + self.vertex_elements_offsets.get(vertex as usize), + self.vertex_elements_offsets.get(vertex as usize + 1), + ) else { + return None; + }; + for &e in &self.vertex_elements[start as usize..end as usize] { + if let Some(n) = self + .element_outward_normal(body, e as usize) + .and_then(|n| n.try_normalize()) + { + sum += n; + } + } + sum.try_normalize() + } + /// The sign turning this mesh's raw winding into its geometric orientation: `-1.0` for /// a mesh oriented inward at rest or turned inside out (see `element_outward_normal`). pub(crate) fn winding_sign(&self) -> Real { diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs index f021bd38f..2ab30ead7 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs @@ -4,13 +4,16 @@ use crate::alloc_prelude::*; use crate::geometry::ColliderHandle; use crate::math::{DIM, Pose, Real, Vector}; +use parry::utils::hashmap::HashMap; #[cfg(not(feature = "std"))] #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; use super::collision_mesh_binding::{bind_to_cells, closest_cell_binding}; +use super::soft_body_builder::{element_vertices, element_vertices_mut}; use super::{ - SoftBody, SoftBodyCell, SoftCollisionMesh, SoftMeshId, SoftMeshMapping, SoftTopologyRemap, + SoftBody, SoftBodyCell, SoftCollisionMesh, SoftMeshCellBinding, SoftMeshId, SoftMeshMapping, + SoftTopologyRemap, surface_element_cells, surface_is_closed, surface_rings, surface_vertex_elements, }; @@ -345,10 +348,13 @@ impl SoftCollisionMesh { .collect(); for vertex in dangling { let position = self.vertices[vertex]; - if let Some(binding) = closest_cell_binding(body, position) { + if let Some(binding) = closest_cell_binding(body, position, |_| true) { bindings[vertex] = binding; } } + if !remap.split.is_empty() { + follow_pieces(bindings, &mut self.vertices, &mut self.indices, body); + } } SoftMeshMapping::Direct { particles } if self.follows_boundary => { self.indices = body.boundary.clone(); @@ -358,31 +364,103 @@ impl SoftCollisionMesh { }); } SoftMeshMapping::Direct { particles } => { - if remap.particles.is_empty() { - return; - } - let mut vertex_remap = vec![u32::MAX; particles.len()]; - let mut kept = Vec::with_capacity(particles.len()); - for (vertex, particle) in particles.iter().enumerate() { - let new = remap.particles.get(*particle as usize).copied(); - if let Some(new) = new.filter(|p| *p != u32::MAX) { - vertex_remap[vertex] = kept.len() as u32; - kept.push(new); + if !remap.particles.is_empty() { + // The vertices whose particle died go with it, and so do the elements they + // were in; the survivors are compacted. + let mut vertex_remap = vec![u32::MAX; particles.len()]; + let mut kept = Vec::with_capacity(particles.len()); + for (vertex, particle) in particles.iter().enumerate() { + let new = remap.particles.get(*particle as usize).copied(); + if let Some(new) = new.filter(|p| *p != u32::MAX) { + vertex_remap[vertex] = kept.len() as u32; + kept.push(new); + } } - } - *particles = kept; - self.indices.retain(|element| { - element - .iter() - .all(|v| vertex_remap[*v as usize] != u32::MAX) - }); - for element in &mut self.indices { - for v in element.iter_mut() { - *v = vertex_remap[*v as usize]; + *particles = kept; + // A wire element pads its unused slot with `u32::MAX`: only the used + // vertices are looked up. + self.indices.retain(|element| { + element_vertices(element) + .iter() + .all(|v| vertex_remap[*v as usize] != u32::MAX) + }); + for element in &mut self.indices { + for v in element_vertices_mut(element) { + *v = vertex_remap[*v as usize]; + } } } + if !remap.inserted.is_empty() { + follow_insertions(particles, &mut self.indices, remap.inserted); + } + if !remap.split.is_empty() { + follow_splits(particles, &mut self.indices, body, remap.split); + } + } + } + } + + /// A copy of this mesh over the vertices `keep` accepts: other vertices and their elements + /// dropped, tables rebuilt, contact state cleared, no collider or id. `None` when no element + /// remains; how a mesh follows a cluster split. + pub(crate) fn restricted_to( + &self, + body: &SoftBody, + keep: impl Fn(u32) -> bool, + ) -> Option { + let n = self.vertex_count(); + let mut vertex_remap = vec![u32::MAX; n]; + let mut kept = 0u32; + for v in 0..n { + if keep(v as u32) { + vertex_remap[v] = kept; + kept += 1; + } + } + let indices: Vec<[u32; DIM]> = self + .indices + .iter() + .filter(|element| { + element_vertices(element) + .iter() + .all(|v| vertex_remap[*v as usize] != u32::MAX) + }) + .map(|element| { + let mut element = *element; + for v in element_vertices_mut(&mut element) { + *v = vertex_remap[*v as usize]; + } + element + }) + .collect(); + if indices.is_empty() { + return None; + } + let mut mesh = self.clone(); + mesh.indices = indices; + let mut keep = (0..n).map(|v| vertex_remap[v] != u32::MAX); + match &mut mesh.binding { + SoftMeshMapping::Direct { particles } => particles.retain(|_| keep.next().unwrap()), + SoftMeshMapping::Skinned { bindings } => { + let mut keep_vertices = keep.clone(); + bindings.retain(|_| keep.next().unwrap()); + mesh.vertices.retain(|_| keep_vertices.next().unwrap()); } } + mesh.collider = ColliderHandle::invalid(); + mesh.id = SoftMeshId::default(); + mesh.clear_contacts(); + mesh.overlap_states.clear(); + mesh.overlap_warm.clear(); + mesh.volume_contacts.clear(); + mesh.rebuild_tables(body); + mesh.rest_signed_volume = if mesh.is_wire() { + 0.0 + } else { + SoftBody::boundary_volume(&mesh.indices, |i| mesh.rest_vertex(body, i as usize)) + }; + mesh.update_orientation(body); + Some(mesh) } /// Clears the contact state kept across steps (a topology change invalidated its ids). @@ -393,3 +471,172 @@ impl SoftCollisionMesh { self.crossed_partners.clear(); } } + +/// Follows the particles a cut inserted into segments (`inserted`: `[a, b, p, q]`, segment `(a, b)` +/// became `(a, p)` and `(q, b)`) in a direct mesh (`particles`: the particle of each vertex): a +/// mesh segment over `a` and `b` is split the same way, with a new vertex per inserted particle. +fn follow_insertions(particles: &mut Vec, indices: &mut Vec<[u32; DIM]>, inserted: &[[u32; 4]]) { + for &[a, b, p, q] in inserted { + for i in 0..indices.len() { + let element = indices[i]; + if element_vertices(&element).len() != 2 { + continue; + } + let ends = [particles[element[0] as usize], particles[element[1] as usize]]; + let (first, second) = if ends == [a, b] { + (p, q) + } else if ends == [b, a] { + (q, p) + } else { + continue; + }; + particles.push(first); + particles.push(second); + let n = particles.len() as u32; + indices[i][1] = n - 2; + let mut rest = element; + rest[0] = n - 1; + indices.push(rest); + } + } +} + +/// Separates a skinned mesh along the cracks between the body's pieces: in an element whose +/// vertices ride cells of different pieces, each vertex outside the majority piece is replaced by +/// a copy bound to the closest cell of that piece (one shared copy per vertex and piece). +fn follow_pieces( + bindings: &mut Vec, + vertices: &mut Vec, + indices: &mut [[u32; DIM]], + body: &SoftBody, +) { + let pieces = body.connected_pieces(); + if pieces.len() < 2 { + return; + } + let mut particle_piece = vec![u32::MAX; body.particles.len()]; + for (k, piece) in pieces.iter().enumerate() { + for &p in piece { + particle_piece[p as usize] = k as u32; + } + } + let cell_piece = |cell: u32| { + body.cells + .get(cell as usize) + .map_or(u32::MAX, |c| particle_piece[c.vertices[0] as usize]) + }; + let mut copies: HashMap<(u32, u32), u32> = HashMap::default(); + for element in indices.iter_mut() { + let used = element_vertices(element).len(); + let piece_of: Vec = element[..used] + .iter() + .map(|&w| bindings.get(w as usize).map_or(u32::MAX, |b| cell_piece(b.cell))) + .collect(); + // The most frequent piece, ties going to the one met first. + let mut majority = (u32::MAX, 0); + for &piece in piece_of.iter().filter(|&&piece| piece != u32::MAX) { + let count = piece_of.iter().filter(|&&other| other == piece).count(); + if count > majority.1 { + majority = (piece, count); + } + } + let majority = majority.0; + if piece_of.iter().all(|&piece| piece == majority || piece == u32::MAX) { + continue; + } + for k in 0..used { + if piece_of[k] == majority || piece_of[k] == u32::MAX { + continue; + } + let w = element[k]; + let copy = match copies.get(&(w, majority)) { + Some(©) => copy, + None => { + let position = vertices[w as usize]; + let Some(binding) = + closest_cell_binding(body, position, |cell| cell_piece(cell) == majority) + else { + continue; + }; + bindings.push(binding); + vertices.push(position); + let copy = vertices.len() as u32 - 1; + let _ = copies.insert((w, majority), copy); + copy + } + }; + element[k] = copy; + } + } +} + +/// The largest number of particle combinations [`follow_splits`] tries for one element. +const MAX_SPLIT_COMBINATIONS: usize = 64; + +/// Follows particle splits (`split`: `(copy, source)` in creation order) in a direct mesh +/// (`particles`: the particle of each vertex): an element whose particles no longer share a +/// measure element of `body` switches to the combination of copies that does, adding vertices. +fn follow_splits( + particles: &mut Vec, + indices: &mut [[u32; DIM]], + body: &SoftBody, + split: &[(u32, u32)], +) { + // Every split particle's family: the particle it was first split from, then its copies. + let mut root: HashMap = HashMap::default(); + let mut families: HashMap> = HashMap::default(); + for &(copy, source) in split { + let first = root.get(&source).copied().unwrap_or(source); + root.insert(copy, first); + families.entry(first).or_insert_with(|| vec![first]).push(copy); + } + let mut vertex_of: HashMap = HashMap::default(); + for (vertex, &particle) in particles.iter().enumerate() { + vertex_of.entry(particle).or_insert(vertex as u32); + } + for element in indices.iter_mut() { + let used = element_vertices(element).len(); + let current: Vec = element[..used] + .iter() + .map(|&w| particles[w as usize]) + .collect(); + let candidates: Vec<&[u32]> = current + .iter() + .map(|p| { + let first = root.get(p).copied().unwrap_or(*p); + families + .get(&first) + .map_or(core::slice::from_ref(p), Vec::as_slice) + }) + .collect(); + let combinations: usize = candidates.iter().map(|c| c.len()).product(); + if combinations == 1 + || combinations > MAX_SPLIT_COMBINATIONS + || body.measure_element_holds(¤t) + { + continue; + } + for index in 0..combinations { + let mut digits = index; + let combination: Vec = candidates + .iter() + .map(|c| { + let p = c[digits % c.len()]; + digits /= c.len(); + p + }) + .collect(); + if body.measure_element_holds(&combination) { + for (k, &p) in combination.iter().enumerate() { + if p != current[k] { + element[k] = *vertex_of.entry(p).or_insert_with(|| { + particles.push(p); + particles.len() as u32 - 1 + }); + } + } + break; + } + } + } +} diff --git a/src/dynamics/soft_body/mod.rs b/src/dynamics/soft_body/mod.rs index d6f3ece67..df704b888 100644 --- a/src/dynamics/soft_body/mod.rs +++ b/src/dynamics/soft_body/mod.rs @@ -11,13 +11,14 @@ pub use self::collision_mesh::{ pub use self::soft_body_elements::SoftBodyDihedral; #[cfg(feature = "fem")] pub use self::soft_body_elements::SoftBodySolver; +pub use self::soft_body::{SOFT_BODY_MAX_CONSTRAINT_PARTICLES, SoftBody}; pub use self::soft_body_contacts::SoftVolumeContact; -pub use self::soft_body::{SOFT_BODY_MAX_CONSTRAINT_PARTICLES, SoftBody, SoftVolumePiece}; pub use self::soft_body_elements::{ SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyEdgeKind, SoftBodyParticle, }; pub use self::soft_body_material::SoftBodyMaterial; pub use self::soft_body_motion::SoftParticleAttachment; +pub use self::soft_body_volume::SoftVolumePiece; pub use self::soft_body_builder::{SoftBodyBuilder, SoftBodyParticleSettings}; pub use self::soft_body_handle::SoftBodyHandle; pub use self::soft_body_set::SoftBodySet; @@ -52,5 +53,6 @@ mod soft_body_motion; mod soft_body_plasticity; mod soft_body_set; pub(crate) mod soft_body_shape_matching; +mod soft_body_volume; mod tearing; pub use self::tearing::{SoftBodyPiece, SoftBodyTearEvent, SoftClusterSplit, SoftJointMove}; diff --git a/src/dynamics/soft_body/soft_body.rs b/src/dynamics/soft_body/soft_body.rs index 8e6cb7e0a..854f8d6dc 100644 --- a/src/dynamics/soft_body/soft_body.rs +++ b/src/dynamics/soft_body/soft_body.rs @@ -80,6 +80,8 @@ pub struct SoftBody { pub(crate) volume_preservation: bool, /// The pieces of material enclosed by a closed boundary, each with its own volume constraint /// (rebuilt with the boundary tables). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) volume_pieces: Vec, /// Multiplier of the target volume (`> 1` inflates the body). pub(crate) volume_factor: Real, /// The rest shape's center of mass at insertion (world space). @@ -127,6 +129,13 @@ pub struct SoftBody { pub(crate) contact_load: Real, #[cfg_attr(feature = "serde-serialize", serde(default))] pub(crate) load_extra_substeps: u8, + /// The soft body this one was split off from by a tear or a cut (see + /// [`Self::origin`]). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) origin: Option, + /// The soft bodies split off from this one, in creation order (see [`Self::pieces`]). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) pieces: Vec, /// User-defined data associated to this soft body. pub user_data: u128, } diff --git a/src/dynamics/soft_body/soft_body_accessors.rs b/src/dynamics/soft_body/soft_body_accessors.rs index f25110d2c..8e2dccf87 100644 --- a/src/dynamics/soft_body/soft_body_accessors.rs +++ b/src/dynamics/soft_body/soft_body_accessors.rs @@ -1,5 +1,8 @@ //! Accessors and simple setters of a soft body: particles, elements, material, volume preservation, sleeping and enabling. -use super::{SoftBody, SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial, SoftBodyParticle}; +use super::{ + SoftBody, SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial, SoftBodyParticle, + SoftVolumePiece, +}; #[cfg(feature = "dim3")] use super::SoftBodyDihedral; #[cfg(feature = "fem")] @@ -148,36 +151,44 @@ impl SoftBody { } } - /// Whether the global area/volume preservation row is enabled. + /// Whether the area/volume preservation constraints are enabled (see [`Self::volume_pieces`]). pub fn volume_preservation_enabled(&self) -> bool { self.volume_preservation } - /// Enables or disables the global area/volume preservation row (requires a closed - /// surface). + /// Enables or disables area/volume preservation: one constraint per volume piece (see + /// [`Self::volume_pieces`]); it stays disabled for a body without any. pub fn enable_volume_preservation(&mut self, enabled: bool) { - self.volume_preservation = enabled && !self.boundary.is_empty(); + self.volume_preservation = enabled && !self.volume_pieces.is_empty(); self.modified = true; } - /// The signed area (2D) or volume (3D) enclosed by the surface at rest. + /// The pieces of material enclosed by a closed boundary, each with its own volume constraint + /// so squeezing one does not inflate another. Pieces joined by no element (appended bodies, + /// torn-apart halves) are separate; the holes or cavities of a piece belong to it. + pub fn volume_pieces(&self) -> &[SoftVolumePiece] { + &self.volume_pieces + } + + /// The signed area (2D) or volume (3D) enclosed by the volume pieces at rest (`0.0` for a + /// body without a closed boundary). pub fn rest_volume(&self) -> Real { - self.rest_volume + self.volume_pieces.iter().map(|piece| piece.rest_volume).sum() } - /// The signed area (2D) or volume (3D) currently enclosed by the surface. + /// The signed area (2D) or volume (3D) currently enclosed by the volume pieces. pub fn volume(&self) -> Real { - Self::boundary_volume(&self.boundary, |i| self.particles[i as usize].position) + self.volume_pieces.iter().map(|piece| piece.volume(self)).sum() } - /// The multiplier applied to the rest volume to obtain the volume-preservation target - /// (`> 1` inflates the body). + /// The multiplier applied to each volume piece's rest volume to obtain its + /// volume-preservation target (`> 1` inflates the body). pub fn volume_factor(&self) -> Real { self.volume_factor } - /// Sets the multiplier applied to the rest volume to obtain the volume-preservation - /// target (`> 1` inflates the body). + /// Sets the multiplier applied to each volume piece's rest volume to obtain its + /// volume-preservation target (`> 1` inflates the body). pub fn set_volume_factor(&mut self, factor: Real) { self.volume_factor = factor; self.modified = true; @@ -189,15 +200,27 @@ impl SoftBody { self.particle_radius } - /// The hidden rigid body standing for this soft body in the islands and carrying its - /// colliders (see the type-level documentation; invalid until the soft body is inserted in a - /// set). Never move, remove or attach joints to it: it is only useful to recognize the soft - /// body's colliders (their parent) in query results and events, or to exclude them from a - /// query. + /// 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. pub fn root_body(&self) -> RigidBodyHandle { self.root_body } + /// The soft body this one was split off from by a tear or a cut (`None` for a user-inserted + /// body): the piece that kept the handle (see [`crate::dynamics::SoftBodyTearEvent::pieces`]). + /// Informational only; the handle dangles once that body is removed. + pub fn origin(&self) -> Option { + self.origin + } + + /// The soft bodies split off from this one by tears and cuts, in creation order (a piece + /// torn off a piece is listed by that piece, not here). Informational: the engine never + /// reads it, and a handle dangles once that body is removed. + pub fn pieces(&self) -> &[crate::dynamics::SoftBodyHandle] { + &self.pieces + } + /// The dynamics settings of the particles (damping, gravity scale, sleeping...). pub fn particle_settings(&self) -> &super::SoftBodyParticleSettings { &self.particle_settings diff --git a/src/dynamics/soft_body/soft_body_builder/mod.rs b/src/dynamics/soft_body/soft_body_builder/mod.rs index 3d5dab3ef..f0295e322 100644 --- a/src/dynamics/soft_body/soft_body_builder/mod.rs +++ b/src/dynamics/soft_body/soft_body_builder/mod.rs @@ -8,8 +8,8 @@ mod soft_body_builder_shapes; pub use self::soft_body_builder::{SoftBodyBuilder, SoftBodyParticleSettings}; pub(crate) use self::soft_body_builder_mesh_helpers::{ - cell_faces, element_vertices, surface_element_cells, surface_is_closed, surface_rings, - surface_vertex_elements, + cell_faces, element_vertices, element_vertices_mut, surface_element_cells, surface_is_closed, + surface_rings, surface_vertex_elements, }; #[cfg(feature = "dim3")] pub(crate) use self::soft_body_builder_mesh_helpers::surface_edge_table; 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 c43c387a8..868eabb6f 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 @@ -56,7 +56,8 @@ impl SoftBodyBuilder { .iter() .zip(masses.iter()) .zip(pinned.iter()) - .map(|((p, m), pinned)| SoftBodyParticle { + .enumerate() + .map(|(i, ((p, m), pinned))| SoftBodyParticle { position: *p, velocity: Vector::ZERO, rest_position: *p - rest_com, @@ -66,6 +67,7 @@ impl SoftBodyBuilder { next_position: None, damaged: false, on_surface: false, + split_root: i as u32, }) .collect(); let pos = |i: u32| self.positions[i as usize]; @@ -190,7 +192,6 @@ impl SoftBodyBuilder { &mut dihedrals, ); - let rest_volume = SoftBody::boundary_volume(&surface, pos); let boundary_closed = surface_is_closed(&surface); let boundary_element_cells = surface_element_cells(&surface, &cells); let mut particles = particles; @@ -202,12 +203,51 @@ impl SoftBodyBuilder { } } - SoftBody { - volume_preservation: self.volume_preservation && !surface.is_empty(), - boundary_inverted: false, - rest_volume, - volume_impulse: 0.0, - } + let mut body = SoftBody { + particles, + clusters: vec![], + cluster_refs: Vec::new(), + root_body: RigidBodyHandle::invalid(), + attachments: Vec::new(), + sleeping: false, + enabled: true, + positions_modified: false, + attachments_modified: false, + edges, + #[cfg(feature = "dim3")] + dihedrals, + cells, + volume_preservation: self.volume_preservation, + boundary: surface, + boundary_closed, + boundary_element_cells, + material: self.material, + cell_model: self.cell_model, + #[cfg(feature = "fem")] + solver: self.solver, + volume_pieces: Vec::new(), + volume_factor: self.volume_factor, + rest_com, + particle_radius: self.particle_radius, + particle_settings: self.particle_settings, + num_colors, + has_overflow_color, + modified: false, + plastic_flowing: false, + sleep_speed: 0.0, + tearing_pending: false, + topology_version: 0, + contact_approach_speeds: [None; 2], + contact_load: 0.0, + load_extra_substeps: 0, + origin: None, + pieces: Vec::new(), + user_data: self.user_data, + }; + // From the builder's positions: the particles' rest positions are centered on `rest_com`. + body.volume_pieces = body.compute_volume_pieces(&[], pos); + body.volume_preservation &= !body.volume_pieces.is_empty(); + body } } diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs index 39ddbbf3c..461273eef 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs @@ -50,6 +50,15 @@ pub(crate) fn element_vertices(element: &[u32; DIM]) -> &[u32] { &element[..used] } +/// The used vertices of a surface element, mutably (see [`element_vertices`]). +pub(crate) fn element_vertices_mut(element: &mut [u32; DIM]) -> &mut [u32] { + let used = element + .iter() + .position(|v| *v == u32::MAX) + .unwrap_or(element.len()); + &mut element[..used] +} + pub(crate) fn surface_is_closed(surface: &[[u32; DIM]]) -> bool { if surface.is_empty() { return false; @@ -77,7 +86,34 @@ pub(crate) fn surface_is_closed(surface: &[[u32; DIM]]) -> bool { *counts.entry(facet).or_insert(0) += 1; } } - counts.values().all(|&c| c == 2) + if counts.values().all(|&c| c == 2) { + return true; + } + // A crack reaching the surface of a body with cells joins four elements at a facet (two per + // side): the surface still encloses its material when every facet is crossed as often in + // both directions. + let mut balance: HashMap<[u32; DIM - 1], i32> = HashMap::default(); + for element in surface { + for k in 0..DIM { + let mut facet = [0u32; DIM - 1]; + let mut n = 0; + for (j, &v) in element.iter().enumerate() { + if j != k { + facet[n] = v; + n += 1; + } + } + #[allow(unused_mut)] + let mut sign = if k % 2 == 0 { 1 } else { -1 }; + #[cfg(feature = "dim3")] + if facet[0] > facet[1] { + facet.swap(0, 1); + sign = -sign; + } + *balance.entry(facet).or_insert(0) += sign; + } + } + balance.values().all(|&b| b == 0) } /// The cell owning each surface element (`u32::MAX` when no cell contains its vertices). diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs index 6d48dc04d..c1ec9604a 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs @@ -266,6 +266,13 @@ impl SoftBodyBuilder { self } + /// Sets the smallest piece, in measure elements, a tear may split off (see + /// [`SoftBodyMaterial::min_piece`]). + pub fn min_piece(mut self, elements: u32) -> Self { + self.material.min_piece = Some(elements); + self + } + /// Sets the material's tear smoothing time constant (see /// [`SoftBodyMaterial::tear_smoothing`]). pub fn tear_smoothing(mut self, seconds: Real) -> Self { diff --git a/src/dynamics/soft_body/soft_body_cluster.rs b/src/dynamics/soft_body/soft_body_cluster.rs index 3ef3e5dfb..a7eddf63f 100644 --- a/src/dynamics/soft_body/soft_body_cluster.rs +++ b/src/dynamics/soft_body/soft_body_cluster.rs @@ -77,6 +77,21 @@ impl SoftBodyCluster { !self.proxy.is_invalid() } + /// Turns this slot into a dead one, returning its particles and its proxy. The meshes are + /// dropped: their colliders are the caller's to deal with. + pub(crate) fn tombstone(&mut self) -> (Vec, RigidBodyHandle) { + self.cell = u32::MAX; + self.shape_matching = false; + self.shape_matching_target = None; + self.prev_shape_matching_target = None; + self.shape_impulses.clear(); + self.meshes.clear(); + ( + core::mem::take(&mut self.particles), + core::mem::replace(&mut self.proxy, RigidBodyHandle::invalid()), + ) + } + /// The particles of this cluster (sorted, unique). pub fn particles(&self) -> &[u32] { &self.particles @@ -486,6 +501,23 @@ impl SoftBody { } } + // A split family keeps one root: the new index of its first surviving member. + let mut family_root = vec![u32::MAX; self.particles.len()]; + for (i, p) in self.particles.iter_mut().enumerate() { + if dead[i] { + continue; + } + let old = p.split_root(i as u32) as usize; + if let Some(root) = family_root.get_mut(old) { + if *root == u32::MAX { + *root = remap[i]; + } + p.split_root = *root; + } else { + p.split_root = remap[i]; + } + } + // The particles themselves, and the per-particle tables. let mut keep = dead.iter().map(|d| !d); self.particles.retain(|_| keep.next().unwrap()); @@ -496,15 +528,17 @@ impl SoftBody { let remap_tables = super::collision_mesh::SoftTopologyRemap { cells: &cell_remap, particles: &remap, + split: &[], + inserted: &[], }; self.for_each_mesh_mut(|body, mesh| { mesh.remap_topology(body, &remap_tables); mesh.clear_contacts(); }); - self.boundary_closed = super::soft_body_builder::surface_is_closed(&self.boundary); - self.update_surface_flags(); - self.rebuild_mesh_tables(); + // The boundary tables (closedness, owning cells, volume pieces) and the meshes' own. + self.rebuild_boundary_tables(); + self.rebuild_volume_pieces(&remap); self.update_cluster_cells(); self.recolor(); self.modified = true; diff --git a/src/dynamics/soft_body/soft_body_elements.rs b/src/dynamics/soft_body/soft_body_elements.rs index 6ccdee2ad..64e53cacf 100644 --- a/src/dynamics/soft_body/soft_body_elements.rs +++ b/src/dynamics/soft_body/soft_body_elements.rs @@ -41,9 +41,29 @@ pub struct SoftBodyParticle { /// Whether some surface element has this particle as a vertex (updated with the boundary). #[cfg_attr(feature = "serde-serialize", serde(default))] pub(crate) on_surface: bool, + /// The particle this one was split from, shared by every copy of a split particle (its own + /// index when it was never split; `u32::MAX` also reads as its own index, see + /// [`Self::split_root`]). + #[cfg_attr(feature = "serde-serialize", serde(default = "no_split_root"))] + pub(crate) split_root: u32, +} + +#[cfg(feature = "serde-serialize")] +fn no_split_root() -> u32 { + u32::MAX } impl SoftBodyParticle { + /// The root of this particle's split family, `index` being this particle's own index: two + /// particles with the same root are copies of one particle a crack went through. + pub(crate) fn split_root(&self, index: u32) -> u32 { + if self.split_root == u32::MAX { + index + } else { + self.split_root + } + } + /// The world-space position of this particle. pub fn position(&self) -> Vector { self.position diff --git a/src/dynamics/soft_body/soft_body_geometry.rs b/src/dynamics/soft_body/soft_body_geometry.rs index 6b337ebdf..14a65a986 100644 --- a/src/dynamics/soft_body/soft_body_geometry.rs +++ b/src/dynamics/soft_body/soft_body_geometry.rs @@ -9,13 +9,21 @@ impl SoftBody { /// Updates the state derived from the particle positions at the end of a step: the volume /// pieces' inside-out flags (their pressure targets) and the collision mesh's orientation. pub(crate) fn update_orientation(&mut self) { - // With hysteresis: a crumpled body hovering around a zero volume must not flip its - // pressure target every step. - let volume = - Self::boundary_volume(&self.boundary, |i| self.particles[i as usize].position); - let ratio = volume / self.rest_volume; - if ratio < -0.25 { - } else if ratio > 0.25 { + let (particles, boundary) = (&self.particles, &self.boundary); + for piece in &mut self.volume_pieces { + if piece.rest_volume != 0.0 { + // With hysteresis: a crumpled piece hovering around a zero volume must not flip + // its pressure target every step. + let volume = Self::elements_volume(piece.boundary_elements(boundary), |i| { + particles[i as usize].position + }); + let ratio = volume / piece.rest_volume; + if ratio < -0.25 { + piece.inverted = true; + } else if ratio > 0.25 { + piece.inverted = false; + } + } } self.for_each_mesh_mut(|body, mesh| mesh.update_orientation(body)); } @@ -24,9 +32,17 @@ impl SoftBody { pub(crate) fn boundary_volume( boundary: &[[u32; DIM]], position: impl Fn(u32) -> Vector, + ) -> Real { + Self::elements_volume(boundary, position) + } + + /// The signed area (2D) or volume (3D) enclosed by `elements` (segments or triangles). + pub(crate) fn elements_volume<'a>( + elements: impl IntoIterator, + position: impl Fn(u32) -> Vector, ) -> Real { let mut vol = 0.0; - for element in boundary { + for element in elements { #[cfg(feature = "dim2")] { let a = position(element[0]); @@ -45,8 +61,8 @@ impl SoftBody { } /// Accumulates the gradient of the enclosed area/volume with respect to every particle. - pub(crate) fn boundary_volume_gradients( - boundary: &[[u32; DIM]], + pub(crate) fn boundary_volume_gradients<'a>( + boundary: impl IntoIterator, position: impl Fn(u32) -> Vector, gradients: &mut [Vector], ) { diff --git a/src/dynamics/soft_body/soft_body_material.rs b/src/dynamics/soft_body/soft_body_material.rs index 7ad304f8b..d36de7760 100644 --- a/src/dynamics/soft_body/soft_body_material.rs +++ b/src/dynamics/soft_body/soft_body_material.rs @@ -54,50 +54,48 @@ pub struct SoftBodyMaterial { /// once. #[cfg_attr(feature = "serde-serialize", serde(default = "one"))] pub edge_plastic_creep: Real, - /// Largest permanent set an edge may accumulate, as a fraction of the length it was created - /// with (default: `1.0`): past it the edge stops yielding and stays elastic from where it - /// flowed to (a repeatedly crushed edge cannot flow to nothing). + /// Largest permanent set an edge may accumulate, as a fraction of its initial length (default: + /// `0.5`); past it the edge stops yielding and stays elastic, so a crushed edge cannot flow to + /// nothing. #[cfg_attr(feature = "serde-serialize", serde(default = "half"))] pub edge_plastic_max: Real, /// Whether the edges take a permanent set under a squeeze, a stretch, or both (default: /// both). #[cfg_attr(feature = "serde-serialize", serde(default))] pub edge_plastic_flow: SoftEdgePlasticFlow, - /// Strain beyond which the elements break (default: `None`, unbreakable): a structural or - /// cell whose largest tensile principal strain exceeds it, is torn at the end of the step - /// (see [`crate::dynamics::SoftBody::tear_edge`] for what a tear removes). Cells of the volume model do not - /// tear on their own, their edges do. The cell strain is the one seen by the solver, capped - /// at `1.0`, so a cell threshold of one or more never fires. + /// Strain beyond which elements break (default: `None`, unbreakable): an edge stretched past + /// this fraction of its initial rest length, or a corotational cell whose largest tensile + /// principal strain (capped at `1.0`) exceeds it, tears at step end; volume cells never do. #[cfg_attr(feature = "serde-serialize", serde(default))] pub tear_strain: Option, - /// [`crate::dynamics::SoftBody::tear_edge`]). Unlike the strain, the force reports the load of an edge that + /// Force beyond which the edges break (default: `None`): an edge whose force along its + /// direction (its impulse over the substep, positive when resisting stretching) exceeds it is + /// torn at the end of the step. Either criterion tears an edge; cells tear on strain only. #[cfg_attr(feature = "serde-serialize", serde(default))] pub tear_force: Option, - /// Time constant, in seconds, over which an element's load is averaged before it is compared - /// to its tear threshold (default: `0.0`, none). At `0.0` one step past the threshold tears; - /// with a positive value the load is smoothed exponentially over about that long, so a brief - /// spike (an impact, a jerk) is shrugged off and only a sustained pull tears. Applies to the - /// strain and the force criteria alike; [`crate::dynamics::SoftBodyEdge::stress`] and [`crate::dynamics::SoftBodyCell::stress`] - /// read the smoothed load back. + /// Time constant, in seconds, over which an element's load is smoothed exponentially before the + /// tear test (default: `0.0`, one step past the threshold tears); applies to the strain and + /// force criteria alike, and [`crate::dynamics::SoftBodyEdge::stress`] reads the smoothed load. #[cfg_attr(feature = "serde-serialize", serde(default))] pub tear_smoothing: Real, - /// How much tougher an element deep inside the body is than one at its surface (default: - /// `1.0`, no difference). An element whose particles are all interior (none on the surface) - /// and undamaged needs this many times its tear threshold; elements at the surface, or next - /// to a particle an earlier tear passed through, tear at the threshold itself. Tears then - /// start at the surface or at existing damage and run inward, where the material is - /// weakened, instead of anywhere a particle happens to be pulled. Multiplies the per-element - /// [`crate::dynamics::SoftBodyEdge::tear_resistance`] / [`crate::dynamics::SoftBodyCell::tear_resistance`]. + /// How much tougher an undamaged interior element (no particle on the surface or on an earlier + /// tear) is than a surface one (default: `1.0`): it needs this many times its tear threshold, + /// so tears start at the surface or existing damage and run inward; scales `tear_resistance`. #[cfg_attr(feature = "serde-serialize", serde(default = "one"))] pub interior_strength: Real, - /// ones (see [`crate::dynamics::SoftBodyEdge::stress`]) go first, the others wait for the next step (edges - /// projectile being caught like in a net while its rim tears one edge at a time; the - /// default lets a puncture open at once. + /// Maximum number of edges torn per step (default: `u32::MAX`, no limit): the most loaded go + /// first, the others wait for the next step (edges past twice their threshold always tear). + /// Bounding it paces the cracks of a taut sheet but lets a projectile be caught as in a net. #[cfg_attr( feature = "serde-serialize", serde(default = "default_max_tears_per_step") )] pub max_tears_per_step: u32, + /// The smallest piece, in measure elements, a tear may split off (default: `None`: ten + /// triangles for a 3D cloth, three cells in 2D or six in 3D, three segments for a rope). A tear + /// leaving a smaller piece waits (cuts are exempt); `Some(1)` lets a tear shed single elements. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub min_piece: Option, } #[cfg(feature = "serde-serialize")] @@ -138,6 +136,7 @@ impl Default for SoftBodyMaterial { tear_smoothing: 0.0, interior_strength: 1.0, max_tears_per_step: u32::MAX, + min_piece: None, } } } diff --git a/src/dynamics/soft_body/soft_body_set/mod.rs b/src/dynamics/soft_body/soft_body_set/mod.rs index 489671d3e..87db21abe 100644 --- a/src/dynamics/soft_body/soft_body_set/mod.rs +++ b/src/dynamics/soft_body/soft_body_set/mod.rs @@ -3,6 +3,7 @@ mod soft_body_set; mod soft_body_set_clusters; mod soft_body_set_insert_remove; mod soft_body_set_proxies; +mod soft_body_set_split; mod soft_body_set_step_sync; pub use self::soft_body_set::SoftBodySet; diff --git a/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs b/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs index 7713c6187..ecb8357cb 100644 --- a/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs @@ -114,6 +114,7 @@ impl SoftBodySet { if !c.is_live() { return None; } + let (particles, proxy) = c.tombstone(); // Reference counts: the particles only this cluster covered die. let mut dead = alloc::vec![false; sb.particles.len()]; 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 2721bf309..7d339e4c5 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 @@ -46,6 +46,7 @@ impl SoftBodySet { #[cfg(feature = "fem")] solver: soft_body.solver, volume_preservation: false, + volume_pieces: Vec::new(), volume_factor: 1.0, rest_com: soft_body.rest_com, particle_radius: soft_body.particle_radius, @@ -60,6 +61,8 @@ impl SoftBodySet { contact_approach_speeds: [None; 2], contact_load: 0.0, load_extra_substeps: 0, + origin: None, + pieces: Vec::new(), user_data: 0, })); 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 aa605aa1a..0ad661e1e 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 @@ -113,6 +113,39 @@ pub(super) fn spawn_mesh_collider( Some(co_handle) } +/// Creates the collider of a mesh split off another (see `SoftCollisionMesh::restricted_to`): a +/// copy of `source` (material, groups, events, user data) holding the new mesh's shape, parented +/// to `proxy` in `frame` (the proxy's pose). `None` if the source is gone or the mesh has no shape. +#[allow(clippy::too_many_arguments)] +pub(super) fn clone_mesh_collider( + source: ColliderHandle, + handle: SoftBodyHandle, + mesh: &super::SoftCollisionMesh, + body: &SoftBody, + proxy: RigidBodyHandle, + frame: &Pose, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, +) -> Option { + let mut collider = colliders.get(source)?.clone(); + let shape = rebuilt_surface_shape( + collider.shape(), + mesh.local_vertices(body, frame), + mesh.indices(), + )?; + collider.set_shape(shape); + collider.set_position(Pose::IDENTITY); + collider.set_enabled(true); + let co_handle = colliders.insert_with_parent(collider, proxy, bodies); + let co = colliders.index_mut_internal(co_handle); + co.deformable_mesh_ref = Some(SoftMeshRef { + body: handle, + id: mesh.id(), + }); + co.deform_pose(*frame); + Some(co_handle) +} + /// Creates the proxy rigid body of cluster `cluster` of soft body `handle`: a /// [`crate::dynamics::RigidBodyType::SoftFrame`] body standing for the cluster in the islands /// and joints and holding its colliders. Cluster 0's proxy is the soft body's root body. 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 new file mode 100644 index 000000000..d4287d27c --- /dev/null +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_split.rs @@ -0,0 +1,567 @@ +//! Splitting clusters and soft bodies along the cracks a tear or a cut opened: a cluster comes +//! apart with the material it covers, its meshes and joints following the pieces, and a piece the +//! crack disconnected from the rest of the body becomes a soft body of its own. +use crate::alloc_prelude::*; +use crate::dynamics::soft_body::{ + SoftBody, SoftBodyCluster, SoftBodyHandle, SoftBodyPiece, SoftBodyTearEvent, SoftClusterSplit, + SoftCollisionMesh, SoftJointMove, SoftMeshId, SoftMeshMapping, SoftMeshRef, +}; +use crate::dynamics::{ + ImpulseJointHandle, ImpulseJointSet, IslandManager, RigidBodyHandle, RigidBodySet, +}; +use crate::geometry::{ColliderHandle, ColliderSet}; +use crate::math::{Pose, Real, Vector}; +use super::soft_body_set_proxies::{clone_mesh_collider, spawn_proxy}; +use super::{SoftBodyIslandEvent, SoftBodySet}; + +/// The poses the proxies had before a split, by proxy (a fresh proxy records the pose of the +/// proxy it was split from): what the joints and rigid colliders hung on them are re-based from +/// once the pieces' frames are known (see [`rebase_proxy_attachments`]). +#[derive(Default)] +pub(super) struct ProxyPoses(Vec<(RigidBodyHandle, Pose)>); + +impl ProxyPoses { + fn record(&mut self, proxy: RigidBodyHandle, pose: Pose) { + if !self.0.iter().any(|(p, _)| *p == proxy) { + self.0.push((proxy, pose)); + } + } +} + +/// The pieces a cluster or a body splits into: the seeded `components` of its `particles`, the +/// largest by `measure` first (ties to the one with the smallest particle), with the particles no +/// component holds (the parts the tear did not reach) joined to it. +fn pieces_keeping_the_largest( + sb: &SoftBody, + particles: &[u32], + mut components: Vec>, + measure: impl Fn(&SoftBody, &[u32]) -> Real, +) -> Vec> { + let largest = (0..components.len()) + .max_by(|&i, &j| { + measure(sb, &components[i]) + .total_cmp(&measure(sb, &components[j])) + .then(j.cmp(&i)) + }) + .unwrap_or(0); + let mut retained = components.remove(largest); + let mut reached = vec![false; sb.num_particles()]; + for &v in components.iter().flatten().chain(&retained) { + reached[v as usize] = true; + } + retained.extend(particles.iter().filter(|&&v| !reached[v as usize])); + retained.sort_unstable(); + let mut pieces = vec![retained]; + pieces.extend(components); + pieces +} + +/// The rest centroid of the given particles (nominal masses): where the frame of a cluster over +/// them sits in the rest shape. +fn rest_centroid(sb: &SoftBody, particles: &[u32]) -> Vector { + let mut com = Vector::ZERO; + let mut mass = 0.0; + for &v in particles { + let p = &sb.particles[v as usize]; + com += p.rest_position * p.mass; + mass += p.mass; + } + if mass > 0.0 { + com / mass + } else { + com + } +} + +impl SoftBodySet { + /// Splits every cluster of `handle` whose own graph the tear disconnected (see + /// [`SoftBody::seeded_components`]): the heaviest piece keeps the cluster, every other becomes + /// a new cluster. Returns whether some cluster was split. + #[allow(clippy::too_many_arguments)] + pub(super) fn split_clusters_along( + &mut self, + handle: SoftBodyHandle, + seeds: &[[u32; 2]], + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + event: &mut SoftBodyTearEvent, + poses: &mut ProxyPoses, + ) -> bool { + let num_clusters = self.bodies[handle.0].clusters.len(); + let mut any = false; + for ci in 0..num_clusters as u32 { + let pieces = { + let sb = &self.bodies[handle.0]; + let Some(cluster) = sb.cluster(ci) else { + continue; + }; + let components = sb.seeded_components(Some(cluster.particles()), seeds); + if components.len() < 2 { + continue; + } + pieces_keeping_the_largest(sb, cluster.particles(), components, SoftBody::mass_of) + }; + self.split_cluster( + handle, + ci, + &pieces, + islands, + bodies, + colliders, + impulse_joints, + event, + poses, + ); + any = true; + } + any + } + + /// Splits the `cluster`-th cluster of `handle` into `pieces` (disjoint sorted particle sets + /// covering it): the first keeps the slot and proxy, the others get fresh clusters and proxies, + /// meshes restricted per piece, joints following the nearest piece; frames not updated here. + #[allow(clippy::too_many_arguments)] + pub(super) fn split_cluster( + &mut self, + handle: SoftBodyHandle, + cluster: u32, + pieces: &[Vec], + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + event: &mut SoftBodyTearEvent, + poses: &mut ProxyPoses, + ) { + let sb = &mut self.bodies[handle.0]; + let source = &sb.clusters[cluster as usize]; + let proxy = source.proxy; + let old_pose = bodies.get(proxy).map_or(Pose::IDENTITY, |rb| *rb.position()); + poses.record(proxy, old_pose); + let shape_matching = source.shape_matching; + let rest_com = rest_centroid(sb, &source.particles); + let source_meshes: Vec> = source.meshes.clone(); + + // The new clusters, one per extra piece. + let mut new_clusters: Vec = Vec::new(); + for piece in &pieces[1..] { + let index = sb.clusters.len() as u32; + let new_proxy = + spawn_proxy(&sb.particle_settings, sb.user_data, handle, index, bodies); + let mut c = SoftBodyCluster::new(piece.clone(), new_proxy, shape_matching); + c.cell = sb.matching_cell(piece); + sb.clusters.push(c); + new_clusters.push(index); + poses.record(new_proxy, old_pose); + } + // The retained piece: the warm shape impulses follow their particles. + let cell = sb.matching_cell(&pieces[0]); + { + let c = &mut sb.clusters[cluster as usize]; + if c.shape_impulses.len() == c.particles.len() { + let mut keep = c.particles.iter().map(|v| pieces[0].binary_search(v).is_ok()); + c.shape_impulses.retain(|_| keep.next().unwrap()); + } else { + c.shape_impulses.clear(); + } + c.particles = pieces[0].clone(); + c.cell = cell; + c.meshes.clear(); + } + + // The meshes, restricted to every piece holding some of their elements. + let mut piece_of_particle = vec![u32::MAX; sb.particles.len()]; + for (k, piece) in pieces.iter().enumerate() { + for &v in piece { + piece_of_particle[v as usize] = k as u32; + } + } + for mesh in &source_meshes { + let Some(mesh) = mesh else { + sb.clusters[cluster as usize].meshes.push(None); + continue; + }; + let vertex_piece = |v: u32| match mesh.binding() { + SoftMeshMapping::Direct { particles } => { + piece_of_particle[particles[v as usize] as usize] + } + SoftMeshMapping::Skinned { bindings } => sb + .cells + .get(bindings[v as usize].cell as usize) + .and_then(|c| { + c.vertices + .iter() + .map(|&w| piece_of_particle[w as usize]) + .find(|p| *p != u32::MAX) + }) + .unwrap_or(0), + }; + for (k, _) in pieces.iter().enumerate() { + let restricted = mesh.restricted_to(sb, |v| vertex_piece(v) == k as u32); + if k == 0 { + // The retained slot keeps its id and its collider (rebuilt by the caller). + let slot = &mut sb.clusters[cluster as usize].meshes; + match restricted { + Some(mut m) => { + m.set_id(mesh.id()); + m.collider = mesh.collider; + m.collision_enabled = mesh.collision_enabled; + slot.push(Some(m)); + } + None => { + // No element left on this piece: the mesh dies with its collider. + slot.push(None); + if mesh.collider != ColliderHandle::invalid() { + colliders.remove_internal(mesh.collider, islands, bodies, false); + } + } + } + continue; + } + let Some(mut m) = restricted else { + continue; + }; + let ci = new_clusters[k - 1]; + let new_proxy = sb.clusters[ci as usize].proxy; + let id = SoftMeshId { + cluster: ci, + mesh: sb.clusters[ci as usize].meshes.len() as u32, + }; + m.set_id(id); + m.collision_enabled = + mesh.collision_enabled && mesh.collider != ColliderHandle::invalid(); + if m.collision_enabled { + match clone_mesh_collider( + mesh.collider, + handle, + &m, + sb, + new_proxy, + &old_pose, + bodies, + colliders, + ) { + Some(co) => m.collider = co, + None => m.collision_enabled = false, + } + } + sb.clusters[ci as usize].meshes.push(Some(m)); + } + } + + // The joints: each follows the piece closest to its anchor in the rest shape. + let attached: Vec<(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, Vector)> = + impulse_joints + .attached_joints(proxy) + .filter(|(_, _, _, j)| (j.body1 == proxy) != (j.body2 == proxy)) + .map(|(_, _, h, j)| { + let local = if j.body1 == proxy { + j.data.local_frame1.translation + } else { + j.data.local_frame2.translation + }; + (j.body1, j.body2, h, local) + }) + .collect(); + for (b1, b2, joint, local) in attached { + let anchor = rest_com + local; + let closest = pieces + .iter() + .enumerate() + .min_by(|(_, a), (_, b)| { + sb.rest_distance_to(a, anchor) + .total_cmp(&sb.rest_distance_to(b, anchor)) + .then(a[0].cmp(&b[0])) + }) + .map_or(0, |(k, _)| k); + if closest == 0 { + continue; + } + let to = sb.clusters[new_clusters[closest - 1] as usize].proxy; + let n1 = if b1 == proxy { to } else { b1 }; + let n2 = if b2 == proxy { to } else { b2 }; + impulse_joints.set_bodies(joint, n1, n2, true); + event.moved_joints.push(SoftJointMove { + joint, + from: proxy, + to, + }); + } + + // A cluster split by the crack and again by the body split keeps one retained entry. + let retained = SoftClusterSplit { + source_cluster: cluster, + soft_body: handle, + cluster, + proxy, + keeps_proxy: true, + }; + if !event.clusters.contains(&retained) { + event.clusters.push(retained); + } + for &ci in &new_clusters { + event.clusters.push(SoftClusterSplit { + source_cluster: cluster, + soft_body: handle, + cluster: ci, + proxy: sb.clusters[ci as usize].proxy, + keeps_proxy: false, + }); + } + sb.modified = true; + self.island_events.push(SoftBodyIslandEvent { handle }); + } +} + +/// Retained particles after `remap` (old to new, `u32::MAX` if gone), as `particles[new] = old`. +fn kept_particles(remap: &[u32], num_particles: usize) -> Vec { + if remap.is_empty() { + return (0..num_particles as u32).collect(); + } + (0..remap.len() as u32) + .filter(|&old| remap[old as usize] != u32::MAX) + .collect() +} + +impl SoftBodySet { + /// Splits the soft body `handle` along the pieces the tear disconnected (see + /// [`SoftBody::seeded_components`]): the largest piece (by rest measure) keeps the body, every + /// other becomes its own soft body. Returns the new handles, recording the pieces in `event`. + #[allow(clippy::too_many_arguments)] + pub(super) fn split_body_along( + &mut self, + handle: SoftBodyHandle, + seeds: &[[u32; 2]], + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + event: &mut SoftBodyTearEvent, + poses: &mut ProxyPoses, + ) -> Vec { + let pieces = { + let sb = &self.bodies[handle.0]; + let components = sb.seeded_components(None, seeds); + if components.len() < 2 { + return Vec::new(); + } + let all: Vec = (0..sb.particles.len() as u32).collect(); + pieces_keeping_the_largest(sb, &all, components, SoftBody::rest_measure_of) + }; + let mut piece_of_particle = vec![0u32; self.bodies[handle.0].particles.len()]; + for (k, piece) in pieces.iter().enumerate() { + for &v in piece { + piece_of_particle[v as usize] = k as u32; + } + } + + // A cluster straddling several pieces (one disconnected at creation, since a cluster the + // tear reached was split already) is partitioned by piece, the heaviest part keeping it. + let num_clusters = self.bodies[handle.0].clusters.len(); + for ci in 0..num_clusters as u32 { + let groups = { + let sb = &self.bodies[handle.0]; + let Some(cluster) = sb.cluster(ci) else { + continue; + }; + let mut groups: Vec> = vec![Vec::new(); pieces.len()]; + for &v in cluster.particles() { + groups[piece_of_particle[v as usize] as usize].push(v); + } + groups.retain(|group| !group.is_empty()); + if groups.len() < 2 { + continue; + } + let heaviest = (0..groups.len()) + .max_by(|&i, &j| { + sb.mass_of(&groups[i]) + .total_cmp(&sb.mass_of(&groups[j])) + .then(groups[j][0].cmp(&groups[i][0])) + }) + .unwrap_or(0); + groups.swap(0, heaviest); + groups + }; + self.split_cluster( + handle, + ci, + &groups, + islands, + bodies, + colliders, + impulse_joints, + event, + poses, + ); + } + + // Every other piece becomes a soft body: a copy of the torn body reduced to the piece's + // clusters and particles, its proxies and colliders re-tagged. + let mut new_handles = Vec::new(); + let mut moved: Vec<(u32, SoftBodyHandle, u32)> = Vec::new(); + for k in 1..pieces.len() as u32 { + let (piece_body, moves, remap) = { + let sb = &self.bodies[handle.0]; + extract_piece(sb, handle, &piece_of_particle, k) + }; + let num_particles = remap.len(); + let new_handle = SoftBodyHandle(self.bodies.insert(piece_body)); + for &(old_ci, new_ci) in &moves { + let cluster = &self.bodies[new_handle.0].clusters[new_ci as usize]; + if let Some(rb) = bodies.get_mut_internal(cluster.proxy) { + rb.soft_body = new_handle; + rb.soft_cluster = new_ci; + } + for mesh in cluster.meshes.iter().flatten() { + if let Some(co) = colliders.get_mut_internal(mesh.collider) { + co.deformable_mesh_ref = Some(SoftMeshRef { + body: new_handle, + id: mesh.id(), + }); + } + } + for split in event.clusters.iter_mut() { + if split.soft_body == handle && split.cluster == old_ci { + split.soft_body = new_handle; + split.cluster = new_ci; + } + } + moved.push((old_ci, new_handle, new_ci)); + } + event.pieces.push(SoftBodyPiece { + soft_body: new_handle, + particles: kept_particles(&remap, num_particles), + clusters: moves.iter().map(|&(old, new)| [old, new]).collect(), + }); + self.island_events.push(SoftBodyIslandEvent { handle: new_handle }); + new_handles.push(new_handle); + } + + // The retained body: the moved clusters become dead slots, their particles leave. + let sb = &mut self.bodies[handle.0]; + for &(old_ci, _, _) in &moved { + let _ = sb.clusters[old_ci as usize].tombstone(); + } + let dead: Vec = piece_of_particle.iter().map(|&p| p != 0).collect(); + let num_particles = sb.particles.len(); + let (_, remap) = sb.remove_dead_particles(&dead); + if !sb.live_clusters().any(|(_, c)| c.proxy == sb.root_body) { + let root = sb + .live_clusters() + .next() + .map_or(RigidBodyHandle::invalid(), |(_, c)| c.proxy); + sb.root_body = root; + } + sb.pieces.extend(new_handles.iter().copied()); + sb.attachments_modified = true; + event.pieces.insert( + 0, + SoftBodyPiece { + soft_body: handle, + particles: kept_particles(&remap, num_particles), + clusters: sb.live_clusters().map(|(ci, _)| [ci, ci]).collect(), + }, + ); + self.attached_to_stale = true; + if !self.island_events.iter().any(|e| e.handle == handle) { + self.island_events.push(SoftBodyIslandEvent { handle }); + } + new_handles + } +} + +/// A copy of `sb` reduced to piece `k` of `piece_of_particle`: clusters renumbered (the caller +/// re-tags their proxies and colliders), particles compacted, per-body state reset with `origin`. +/// Returns the body, its clusters `(in sb, in copy)` and the particle remap (`u32::MAX`: dropped). +fn extract_piece( + sb: &SoftBody, + origin: SoftBodyHandle, + piece_of_particle: &[u32], + k: u32, +) -> (SoftBody, Vec<(u32, u32)>, Vec) { + let mut body = sb.clone(); + body.clusters = Vec::new(); + let mut moves = Vec::new(); + for (ci, cluster) in sb.clusters.iter().enumerate() { + let inside = cluster + .particles + .first() + .is_some_and(|&v| piece_of_particle[v as usize] == k); + if !cluster.is_live() || !inside { + continue; + } + let new_ci = body.clusters.len() as u32; + let mut cluster = cluster.clone(); + for (mi, mesh) in cluster.meshes.iter_mut().enumerate() { + if let Some(mesh) = mesh { + mesh.set_id(SoftMeshId { + cluster: new_ci, + mesh: mi as u32, + }); + } + } + body.clusters.push(cluster); + moves.push((ci as u32, new_ci)); + } + let dead: Vec = piece_of_particle.iter().map(|&p| p != k).collect(); + let (_, remap) = body.remove_dead_particles(&dead); + let root = body + .live_clusters() + .next() + .map_or(RigidBodyHandle::invalid(), |(_, c)| c.proxy); + body.root_body = root; + body.origin = Some(origin); + body.pieces = Vec::new(); + body.sleeping = false; + body.positions_modified = false; + body.attachments_modified = true; + body.modified = true; + body.tearing_pending = false; + body.contact_approach_speeds = [None; 2]; + body.contact_load = 0.0; + body.load_extra_substeps = 0; + (body, moves, remap) +} + +/// Re-bases what hangs on the proxies of `poses` onto their fresh frames: the local frames of +/// their impulse joints and of the rigid (non-deformable) colliders a user hung on them, so each +/// keeps its world pose. A fresh proxy was recorded with the pose of the proxy it was split from. +pub(super) fn rebase_proxy_attachments( + poses: &ProxyPoses, + bodies: &RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, +) { + for &(proxy, old_pose) in &poses.0 { + let Some(rb) = bodies.get(proxy) else { + continue; + }; + let new_pose = *rb.position(); + if new_pose.translation == old_pose.translation && new_pose.rotation == old_pose.rotation + { + continue; + } + let delta = new_pose.inverse() * old_pose; + impulse_joints.map_attached_joints_mut(proxy, |_, _, _, joint| { + if joint.body1 == proxy { + joint.data.local_frame1 = delta * joint.data.local_frame1; + } + if joint.body2 == proxy { + joint.data.local_frame2 = delta * joint.data.local_frame2; + } + }); + for co_handle in rb.colliders().to_vec() { + let Some(co) = colliders.get_mut(co_handle) else { + continue; + }; + if co.is_deformable_collider() { + continue; + } + if let Some(pos) = co.position_wrt_parent().copied() { + co.set_position_wrt_parent(delta * pos); + } + } + } +} 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 4e94e930f..e1f2610f5 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 @@ -8,6 +8,7 @@ use crate::geometry::ColliderSet; #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; use super::soft_body_set_proxies::{rebuilt_surface_shape, sync_soft_body}; +use super::soft_body_set_split::{ProxyPoses, rebase_proxy_attachments}; use crate::dynamics::soft_body::{SoftBody, SoftBodyHandle, SoftBodyTearEvent}; use super::{SoftBodyIslandEvent, SoftBodySet}; use crate::pipeline::EventHandler; @@ -321,6 +322,24 @@ impl SoftBodySet { colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, + ) -> Option { + let _ = multibody_joints; + self.change_topology(handle, islands, bodies, colliders, impulse_joints, |sb, event| { + sb.tear_topology(edges, cells, event) + }) + } + + /// Applies a topology change to a soft body (`change` returns whether anything changed and + /// records it in the event), then splits the clusters the change disconnected, rebuilds the + /// colliders and wakes the body up. + fn change_topology( + &mut self, + handle: SoftBodyHandle, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + change: impl FnOnce(&mut SoftBody, &mut SoftBodyTearEvent) -> bool, ) -> Option { let sb = self.bodies.get_mut(handle.0)?; let mut event = SoftBodyTearEvent { @@ -331,9 +350,50 @@ impl SoftBodySet { return None; } + // The clusters the crack ran through come apart with their material, then so does the + // body: every piece the crack disconnected becomes a soft body of its own. + let seeds = event.seeds(); + let mut poses = ProxyPoses::default(); + let split = self.split_clusters_along( + handle, + &seeds, + islands, + bodies, + colliders, + impulse_joints, + &mut event, + &mut poses, + ); + let mut handles = self.split_body_along( + handle, + &seeds, + islands, + bodies, + colliders, + impulse_joints, + &mut event, + &mut poses, + ); + let split = split || !handles.is_empty(); + handles.insert(0, handle); - if let Some(rb) = bodies.get_mut(sb.root_body) { - rb.wake_up(true); + for &piece in &handles { + let sb = &mut self.bodies[piece.0]; + if split { + // The pieces' frames, fresh and retained: the colliders are expressed in them. + Self::update_cluster_proxies(sb, bodies, colliders); + } + // The colliders follow the new surface at once: the next step's narrow phase must + // not report elements that no longer exist. + Self::update_colliders(sb, bodies, colliders, true); + } + if split { + rebase_proxy_attachments(&poses, bodies, colliders, impulse_joints); + } + for &piece in &handles { + if let Some(rb) = bodies.get_mut(self.bodies[piece.0].root_body) { + rb.wake_up(true); + } } Some(event) } @@ -353,6 +413,9 @@ impl SoftBodySet { multibody_joints: &mut MultibodyJointSet, ) -> Option { let _ = multibody_joints; + self.change_topology(handle, islands, bodies, colliders, impulse_joints, |sb, event| { + sb.cut_topology(blade, event) + }) } /// Refreshes every mesh's vertex cache from the particles, before a narrow-phase update diff --git a/src/dynamics/soft_body/soft_body_volume.rs b/src/dynamics/soft_body/soft_body_volume.rs new file mode 100644 index 000000000..bd2907fea --- /dev/null +++ b/src/dynamics/soft_body/soft_body_volume.rs @@ -0,0 +1,168 @@ +//! Volume preservation per piece: the pieces of material enclosed by a closed boundary, each with its own volume constraint. +use super::SoftBody; +use super::soft_body_builder::surface_is_closed; +use crate::alloc_prelude::*; +use crate::math::{DIM, Real, Vector}; + +/// A piece of a soft body's material enclosed by closed boundary elements (see +/// [`SoftBody::volume_pieces`]): its volume constraint holds the area/volume it encloses at its +/// rest one times the body's volume factor. +#[derive(Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SoftVolumePiece { + /// The piece's boundary elements (increasing indices into the body's boundary). + pub(crate) elements: Vec, + /// The particles of those elements (increasing). + pub(crate) particles: Vec, + /// The signed area (2D) or volume (3D) the elements enclose at rest. + pub(crate) rest_volume: Real, + /// Accumulated volume impulse (warm-start state). + pub(crate) impulse: Real, + /// Whether the piece is currently turned inside out (its signed volume has the opposite sign + /// to the rest one): its pressure target flips with it (a mirrored balloon is the same + /// balloon). + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) inverted: bool, +} + +impl SoftVolumePiece { + /// The piece's boundary elements, as indices into [`SoftBody::boundary`]. + pub fn elements(&self) -> &[u32] { + &self.elements + } + + /// The particles of the piece's boundary elements, in increasing order. + pub fn particles(&self) -> &[u32] { + &self.particles + } + + /// The signed area (2D) or volume (3D) enclosed by the piece at rest. + pub fn rest_volume(&self) -> Real { + self.rest_volume + } + + /// The signed area (2D) or volume (3D) currently enclosed by this piece of `body`. + pub fn volume(&self, body: &SoftBody) -> Real { + SoftBody::elements_volume(self.boundary_elements(&body.boundary), |i| { + body.particles[i as usize].position + }) + } + + /// The piece's elements, read from the body's `boundary`. + pub(crate) fn boundary_elements<'a>( + &'a self, + boundary: &'a [[u32; DIM]], + ) -> impl Iterator + Clone + 'a { + self.elements.iter().map(move |&e| &boundary[e as usize]) + } +} + +impl SoftBody { + /// Rebuilds the volume pieces after a topology change (rest volumes from the rest positions) + /// and disables volume preservation if none is left. `remap` maps the previous particle + /// indices to the current ones (empty: unchanged; `u32::MAX`: removed). + pub(crate) fn rebuild_volume_pieces(&mut self, remap: &[u32]) { + self.volume_pieces = + self.compute_volume_pieces(remap, |i| self.particles[i as usize].rest_position); + self.volume_preservation &= !self.volume_pieces.is_empty(); + } + + /// The volume pieces of the current topology: the particles connected by edges, cells or + /// boundary elements, kept when their boundary elements are closed. Each piece takes the + /// warm-start impulse and orientation of the previous piece holding its first particle. + pub(crate) fn compute_volume_pieces( + &self, + remap: &[u32], + rest_position: impl Fn(u32) -> Vector, + ) -> Vec { + fn find(parent: &mut [u32], mut i: u32) -> u32 { + while parent[i as usize] != i { + let up = parent[parent[i as usize] as usize]; + parent[i as usize] = up; + i = up; + } + i + } + fn link(parent: &mut [u32], vertices: &[u32]) { + let n = parent.len(); + let mut vertices = vertices.iter().copied().filter(|&v| (v as usize) < n); + let Some(first) = vertices.next() else { + return; + }; + let mut root = find(parent, first); + for v in vertices { + let other = find(parent, v); + if other != root { + let (low, high) = (root.min(other), root.max(other)); + parent[high as usize] = low; + root = low; + } + } + } + + let n = self.particles.len(); + let mut parent: Vec = (0..n as u32).collect(); + for edge in &self.edges { + link(&mut parent, &edge.vertices); + } + for cell in &self.cells { + link(&mut parent, &cell.vertices); + } + for element in &self.boundary { + link(&mut parent, element); + } + + // The boundary elements of each connected piece, in boundary order. + let mut group_of_root = vec![u32::MAX; n]; + let mut groups: Vec> = Vec::new(); + for (e, element) in self.boundary.iter().enumerate() { + let Some(&v) = element.iter().find(|&&v| (v as usize) < n) else { + continue; + }; + let root = find(&mut parent, v) as usize; + if group_of_root[root] == u32::MAX { + group_of_root[root] = groups.len() as u32; + groups.push(Vec::new()); + } + groups[group_of_root[root] as usize].push(e as u32); + } + + // The previous piece of every current particle, for the warm start. + let mut previous = vec![u32::MAX; n]; + for (pi, piece) in self.volume_pieces.iter().enumerate() { + for &q in &piece.particles { + let p = if remap.is_empty() { + q + } else { + remap.get(q as usize).copied().unwrap_or(u32::MAX) + }; + if let Some(slot) = previous.get_mut(p as usize) { + *slot = pi as u32; + } + } + } + + let mut pieces = Vec::new(); + let mut elements: Vec<[u32; DIM]> = Vec::new(); + for group in groups { + elements.clear(); + elements.extend(group.iter().map(|&e| self.boundary[e as usize])); + if !surface_is_closed(&elements) { + continue; + } + let mut particles: Vec = elements.iter().flatten().copied().collect(); + particles.sort_unstable(); + particles.dedup(); + let rest_volume = Self::boundary_volume(&elements, &rest_position); + let old = self.volume_pieces.get(previous[particles[0] as usize] as usize); + pieces.push(SoftVolumePiece { + elements: group, + particles, + rest_volume, + impulse: old.map_or(0.0, |piece| piece.impulse), + inverted: old.is_some_and(|piece| piece.inverted), + }); + } + pieces + } +} diff --git a/src/dynamics/soft_body/tearing/mod.rs b/src/dynamics/soft_body/tearing/mod.rs index cdf2e5157..46e0979d4 100644 --- a/src/dynamics/soft_body/tearing/mod.rs +++ b/src/dynamics/soft_body/tearing/mod.rs @@ -3,11 +3,15 @@ //! rebuild that follows. mod tearing; +mod tearing_crack; +mod tearing_cut; mod tearing_event; mod tearing_helpers; mod tearing_particle_split; mod tearing_tables; mod tearing_validate; +#[cfg(test)] +mod tests; -pub use self::tearing_event::SoftBodyTearEvent; +pub use self::tearing_event::{SoftBodyPiece, SoftBodyTearEvent, SoftClusterSplit, SoftJointMove}; pub(super) use super::collision_mesh; diff --git a/src/dynamics/soft_body/tearing/tearing.rs b/src/dynamics/soft_body/tearing/tearing.rs index 1f0827e93..36cffb524 100644 --- a/src/dynamics/soft_body/tearing/tearing.rs +++ b/src/dynamics/soft_body/tearing/tearing.rs @@ -8,7 +8,9 @@ use crate::math::{DIM, Real, Vector}; use simba::scalar::{ComplexField as _, RealField as _}; use super::super::SoftBody; +use super::tearing_cut::blade_hit; use super::tearing_event::{CATASTROPHIC_STRESS, SoftBodyTearEvent}; +use super::tearing_particle_split::SplitLog; /// A sorted particle-index pair. pub(super) fn pair(a: u32, b: u32) -> [u32; 2] { @@ -115,86 +117,31 @@ impl SoftBody { torn_edges: &[u32], torn_cells: &[u32], event: &mut SoftBodyTearEvent, - let any_cell_removed = cell_removed.contains(&true); - if any_cell_removed { - let mut kept_pairs: HashMap<[u32; 2], ()> = HashMap::default(); - let mut dead_pairs: HashMap<[u32; 2], ()> = HashMap::default(); - for (ci, c) in self.cells.iter().enumerate() { - if !cell_removed[ci] { - for i in 0..=DIM { - for j in i + 1..=DIM { - kept_pairs.insert(pair(c.vertices[i], c.vertices[j]), ()); - } - } - } - } - for (ci, c) in self.cells.iter().enumerate() { - if cell_removed[ci] { - for i in 0..=DIM { - for j in i + 1..=DIM { - let key = pair(c.vertices[i], c.vertices[j]); - if !kept_pairs.contains_key(&key) { - dead_pairs.insert(key, ()); - } - } - } - } - } - if !edge_removed[ei] && dead_pairs.contains_key(&key) { - for (key, ()) in dead_pairs { - torn_pairs.insert(key, ()); - } - } - // Surface elements owned by a removed cell or spanning a torn pair. A 3D cloth (no - let mut surface_removed = vec![false; self.boundary.len()]; - for (si, element) in self.boundary.iter().enumerate() { - let owner = self - .boundary_element_cells - .get(si) - .copied() - .unwrap_or(u32::MAX); - let owner_removed = - owner != u32::MAX && cell_removed.get(owner as usize) == Some(&true); - if owner_removed - || (!cloth && !torn_pairs.is_empty() && spans_pair(element, &torn_pairs)) - { - surface_removed[si] = true; - } - } + ) -> bool { + self.crack_body(torn_edges, torn_cells, event) + } - // Dihedrals spanning a torn pair or bending over a removed triangle. - #[cfg(feature = "dim3")] - let mut dihedral_removed = vec![false; self.dihedrals.len()]; - #[cfg(feature = "dim3")] - { - let mut removed_tris: HashMap<[u32; 3], ()> = HashMap::default(); - for (si, element) in self.boundary.iter().enumerate() { - if surface_removed[si] { - let mut key = *element; - key.sort_unstable(); - removed_tris.insert(key, ()); - } - } - for (di, d) in self.dihedrals.iter().enumerate() { - let v = d.vertices; - let mut t1 = [v[0], v[1], v[2]]; - let mut t2 = [v[0], v[1], v[3]]; - t1.sort_unstable(); - t2.sort_unstable(); - if spans_pair(&v, &torn_pairs) - || removed_tris.contains_key(&t1) - || removed_tris.contains_key(&t2) - { - dihedral_removed[di] = true; - } - } - } - held >= 2 - // Split copies join the clusters of their source particle; the clusters' cell matches - // follow the compacted cell list. - self.inherit_cluster_membership(&duplicated); - // the split copies, so the direct meshes keep their indices). - cells: &cell_remap, - true + /// Follows a topology change through the derived state: the split copies and inserted + /// particles of `log` join the clusters of their source or neighboring segment end, the meshes + /// follow, and the boundary tables, rest volume and coloring are rebuilt. + pub(super) fn finish_topology_change(&mut self, log: &SplitLog) { + let mut sources = log.split_particles.clone(); + sources.extend(log.inserted.iter().flat_map(|&[a, b, p, q]| [(p, a), (q, b)])); + // This also matches the clusters to the updated cell list. + self.inherit_cluster_membership(&sources); + let remap_tables = super::collision_mesh::SoftTopologyRemap { + cells: &[], + particles: &[], + split: &log.split_particles, + inserted: &log.inserted, + }; + self.for_each_mesh_mut(|body, mesh| { + mesh.remap_topology(body, &remap_tables); + mesh.clear_contacts(); + }); + self.rebuild_boundary_tables(); + self.recolor(); + self.modified = true; + self.topology_version = self.topology_version.wrapping_add(1); } } diff --git a/src/dynamics/soft_body/tearing/tearing_crack.rs b/src/dynamics/soft_body/tearing/tearing_crack.rs new file mode 100644 index 000000000..ab31fd1ce --- /dev/null +++ b/src/dynamics/soft_body/tearing/tearing_crack.rs @@ -0,0 +1,267 @@ +//! Tears of soft bodies: a torn edge or cell opens a crack by splitting one of its particles along +//! a plane in the rest shape, so no measure element is ever removed. + +use crate::alloc_prelude::*; +use crate::dynamics::solver::symmetric_eigen; +use crate::math::{DIM, Real, Vector}; +use parry::utils::hashmap::HashMap; + +use super::super::SoftBody; +use super::tearing_event::SoftBodyTearEvent; +use super::tearing_particle_split::{Fan, MeasureKind, SplitLog}; +use super::tearing::pair; + +/// The mean load of the fan elements on the given side of the split plane. +fn side_load(fan: &Fan, side: usize, load: impl Fn(u32) -> Real) -> Real { + let (sum, count) = fan + .elements + .iter() + .zip(&fan.sides) + .filter(|(_, s)| **s == side) + .fold((0.0, 0usize), |(sum, count), (&e, _)| (sum + load(e), count + 1)); + sum / count.max(1) as Real +} + +impl SoftBody { + /// Opens a crack across every torn edge and cell (see [`SoftBody::tear_edge`]), then splits the + /// particles left joining two pieces alone. Returns whether the topology changed; `event` gets + /// the opened elements (pre-tear indices), the removed straddling edges and split particles. + pub(super) fn crack_body( + &mut self, + torn_edges: &[u32], + torn_cells: &[u32], + event: &mut SoftBodyTearEvent, + ) -> bool { + let edges: Vec<[u32; 2]> = torn_edges + .iter() + .filter_map(|&e| self.edges.get(e as usize)) + .map(|e| e.vertices) + .collect(); + let mut cells: Vec = torn_cells + .iter() + .copied() + .filter(|&c| (c as usize) < self.cells.len()) + .collect(); + cells.sort_unstable(); + cells.dedup(); + let Some(kind) = self.measure_kind() else { + return false; + }; + if edges.is_empty() && cells.is_empty() { + return false; + } + // Splits rewire the cells and triangles in place: their indices stay valid through the + // whole tear. Edge indices do not (straddlers are removed), so segment loads are read at + // each crack. + let loads = self.element_loads(kind); + let cell_vertices: Vec<[u32; DIM + 1]> = cells + .iter() + .map(|&c| self.cells[c as usize].vertices) + .collect(); + + let mut log = SplitLog::default(); + for &[a, b] in &edges { + let first = log.split_particles.len(); + // An edge an earlier split removed (a straddler) is already open. + let Some((x, y)) = self.current_pair(a, b, &log) else { + continue; + }; + if self.crack_across_edge(x, y, &loads, &mut log) { + event.torn_edges.push([a, b]); + self.mark_damaged(&[x, y], &log.split_particles[first..]); + } + } + for (&c, original) in cells.iter().zip(&cell_vertices) { + let first = log.split_particles.len(); + let vertices = self.cells[c as usize].vertices; + if self.crack_through_cell(c, &mut log) { + event.torn_cells.push(*original); + self.mark_damaged(&vertices, &log.split_particles[first..]); + } + } + if log.split_particles.is_empty() { + return false; + } + + self.split_pinched(kind, &mut log); + self.remove_straddlers_across_pieces(&mut log); + event.removed_edges.extend_from_slice(&log.removed_edges); + event + .split_particles + .extend_from_slice(&log.split_particles); + self.finish_topology_change(&log); + true + } + + /// Splits the particles an opened facet left joining two pieces alone (a strip one element + /// thick, a cloth crack reaching the border): those of the elements touching a split in `log` + /// whose fans are no longer connected. Segments share their facet, so only cells and triangles. + pub(super) fn split_pinched(&mut self, kind: MeasureKind, log: &mut SplitLog) { + if kind == MeasureKind::Segments { + return; + } + let mut near = vec![false; self.particles.len()]; + for &(copy, source) in &log.split_particles { + near[copy as usize] = true; + near[source as usize] = true; + } + let mut candidates: Vec = Vec::new(); + self.for_each_measure_element(kind, |_, vertices| { + if vertices.iter().any(|&v| near[v as usize]) { + candidates.extend_from_slice(vertices); + } + }); + candidates.sort_unstable(); + candidates.dedup(); + self.split_particles(&candidates, log); + } + + /// The load of every cell or triangle (by `kind`; empty for segments, whose load is their own + /// stress): the largest of its own stress (cells) and the stresses of the edges between its + /// particles. + fn element_loads(&self, kind: MeasureKind) -> Vec { + let mut edge_stress: HashMap<[u32; 2], Real> = HashMap::default(); + for e in &self.edges { + let stress = edge_stress + .entry(pair(e.vertices[0], e.vertices[1])) + .or_insert(0.0); + *stress = stress.max(e.stress); + } + let load = |own: Real, vertices: &[u32]| { + let mut load = own; + for i in 0..vertices.len() { + for j in i + 1..vertices.len() { + if let Some(s) = edge_stress.get(&pair(vertices[i], vertices[j])) { + load = load.max(*s); + } + } + } + load + }; + match kind { + MeasureKind::Cells => self.cells.iter().map(|c| load(c.stress, &c.vertices)).collect(), + MeasureKind::Triangles => self.boundary.iter().map(|t| load(0.0, t)).collect(), + MeasureKind::Segments => Vec::new(), + } + } + + /// The particles standing for the torn edge `(a, b)` after the splits of `log`: the first + /// pair of copies (or originals) still sharing a measure element, else the first still + /// joined by an edge (a bend edge follows its copies). `None` when no such pair is left. + fn current_pair(&self, a: u32, b: u32, log: &SplitLog) -> Option<(u32, u32)> { + let family = |root: u32| { + let mut family = vec![root]; + for &(copy, source) in &log.split_particles { + if family.contains(&source) { + family.push(copy); + } + } + family + }; + let (fa, fb) = (family(a), family(b)); + for &x in &fa { + for &y in &fb { + if self.measure_element_holds(&[x, y]) { + return Some((x, y)); + } + } + } + for &x in &fa { + for &y in &fb { + if self.edges.iter().any(|e| e.vertices == [x, y] || e.vertices == [y, x]) { + return Some((x, y)); + } + } + } + None + } + + /// Marks the given particles and copies as damaged: the elements around them lose their + /// interior strength. + fn mark_damaged(&mut self, vertices: &[u32], copies: &[(u32, u32)]) { + for &v in vertices.iter().chain(copies.iter().map(|(copy, _)| copy)) { + self.particles[v as usize].damaged = true; + } + } + + /// Opens a crack across the torn edge `(a, b)`: an endpoint (the more loaded far side first) is + /// split along the rest-shape plane through it normal to the edge, its far side going to the + /// copy; one with all elements on one side, or leaving a piece below the minimum size, cannot. + fn crack_across_edge(&mut self, a: u32, b: u32, loads: &[Real], log: &mut SplitLog) -> bool { + let mut options: Vec<(Real, u32, Fan)> = Vec::new(); + for (v, other) in [(a, b), (b, a)] { + let origin = self.particles[v as usize].rest_position; + let normal = self.particles[other as usize].rest_position - origin; + if let Some(fan) = self.plane_fan(v, origin, normal) { + let load = side_load(&fan, 1, |e| match fan.kind { + MeasureKind::Segments => self.edges[e as usize].stress, + _ => loads[e as usize], + }); + options.push((load, v, fan)); + } + } + options.sort_by(|x, y| y.0.total_cmp(&x.0).then(x.1.cmp(&y.1))); + let Some((_, v, fan)) = options + .into_iter() + .find(|(_, v, fan)| self.opens_without_confetti(*v, fan)) + else { + return false; + }; + let first = log.split_particles.len(); + if !self.split_fan(v, &fan, log) { + return false; + } + // The torn edge no longer bears its load: its warm start and stress would pull the fresh + // copy and tear the edge again at its other endpoint next step. + let other = if v == a { b } else { a }; + let opened: Vec = core::iter::once(v) + .chain(log.split_particles[first..].iter().map(|(copy, _)| *copy)) + .collect(); + for e in &mut self.edges { + if e.vertices.contains(&other) && e.vertices.iter().any(|w| opened.contains(w)) { + e.impulse = 0.0; + e.stress = 0.0; + } + } + true + } + + /// Opens a crack through the torn cell `c`: a particle is split along the plane through it + /// perpendicular to the cell's rest-frame principal stretch, closest to the parallel plane + /// through the centroid first; a split leaving a piece below the minimum size is skipped. + fn crack_through_cell(&mut self, c: u32, log: &mut SplitLog) -> bool { + let vertices = self.cells[c as usize].vertices; + let rest: [Vector; DIM + 1] = + core::array::from_fn(|k| self.particles[vertices[k] as usize].rest_position); + let current: [Vector; DIM + 1] = + core::array::from_fn(|k| self.particles[vertices[k] as usize].position); + let rest_matrix = Self::cell_edge_matrix(rest); + let det = rest_matrix.determinant(); + if det == 0.0 || !det.is_finite() { + return false; + } + // `F = Ds Dm⁻¹`; the eigenvectors of `FᵀF` are the rest-frame stretch directions. + let f = Self::cell_edge_matrix(current) * rest_matrix.inverse(); + let (values, vectors) = symmetric_eigen(&(f.transpose() * f)); + let k = (0..DIM) + .max_by(|&i, &j| values[i].total_cmp(&values[j])) + .unwrap_or(0); + let normal = vectors.col(k); + let centroid = rest.iter().copied().sum::() / (DIM + 1) as Real; + let mut order: Vec<(Real, u32)> = vertices + .iter() + .zip(&rest) + .map(|(&v, &r)| ((r - centroid).dot(normal).abs(), v)) + .collect(); + order.sort_by(|x, y| x.0.total_cmp(&y.0).then(x.1.cmp(&y.1))); + for (_, v) in order { + let origin = self.particles[v as usize].rest_position; + if let Some(fan) = self.plane_fan(v, origin, normal) { + if self.opens_without_confetti(v, &fan) && self.split_fan(v, &fan, log) { + return true; + } + } + } + false + } +} diff --git a/src/dynamics/soft_body/tearing/tearing_cut.rs b/src/dynamics/soft_body/tearing/tearing_cut.rs new file mode 100644 index 000000000..5f09b0462 --- /dev/null +++ b/src/dynamics/soft_body/tearing/tearing_cut.rs @@ -0,0 +1,358 @@ +//! Cuts of soft bodies along a blade: cells and triangles snap the cut to their facets by +//! splitting particles, segments get two particles inserted at the crossing. + +use crate::alloc_prelude::*; +use crate::math::{DIM, Real, Vector}; + +use super::super::{SoftBody, SoftBodyEdgeKind}; +use super::tearing_event::SoftBodyTearEvent; +use super::tearing_particle_split::{MeasureKind, SplitLog}; + +/// The shortest part of a segment, as a fraction of its length, a cut inserts particles for: a +/// crossing closer to an endpoint splits that endpoint instead. +const MIN_INSERTION_FRACTION: Real = 0.2; + +/// The normal of the blade's supporting line (2D) or plane (3D); zero for a degenerate blade. +fn blade_normal(blade: &[Vector; DIM]) -> Vector { + #[cfg(feature = "dim2")] + { + let d = blade[1] - blade[0]; + Vector::new(-d.y, d.x) + } + #[cfg(feature = "dim3")] + { + (blade[1] - blade[0]).cross(blade[2] - blade[0]) + } +} + +/// Where segment `[p, q]` meets the blade, as `t` in `p + (q - p) * t`: the endpoints lie on or +/// across the blade's supporting line (2D) or plane (3D) and the meeting point lies on the blade +/// (closed tests). `None` for a segment within the line or plane, or a degenerate blade. +pub(super) fn blade_hit(blade: &[Vector; DIM], p: Vector, q: Vector) -> Option { + let n = blade_normal(blade); + let (dp, dq) = (n.dot(p - blade[0]), n.dot(q - blade[0])); + if dp * dq > 0.0 || dp == dq { + return None; + } + let t = (dp / (dp - dq)).clamp(0.0, 1.0); + let hit = p + (q - p) * t; + #[cfg(feature = "dim2")] + { + let d = blade[1] - blade[0]; + let s = (hit - blade[0]).dot(d); + (s >= 0.0 && s <= d.length_squared()).then_some(t) + } + #[cfg(feature = "dim3")] + { + let [a, b, c] = *blade; + let inside = (b - a).cross(hit - a).dot(n) >= 0.0 + && (c - b).cross(hit - b).dot(n) >= 0.0 + && (a - c).cross(hit - c).dot(n) >= 0.0; + inside.then_some(t) + } +} + +impl SoftBody { + /// Cuts this body along the blade (see [`crate::dynamics::SoftBodySet::cut`]). Returns whether + /// the topology changed, recording in `event` the crossed elements (particle indices before + /// the cut), the removed straddlers, the split particles and the inserted ones. + pub(crate) fn cut_topology( + &mut self, + blade: &[Vector; DIM], + event: &mut SoftBodyTearEvent, + ) -> bool { + let Some(kind) = self.measure_kind() else { + return false; + }; + let (edges, cells) = self.crossing_elements(blade); + if edges.is_empty() && cells.is_empty() { + return false; + } + let crossed_edges: Vec<[u32; 2]> = edges + .iter() + .map(|&e| self.edges[e as usize].vertices) + .collect(); + let crossed_cells: Vec<[u32; DIM + 1]> = cells + .iter() + .map(|&c| self.cells[c as usize].vertices) + .collect(); + + let mut log = SplitLog::default(); + if kind == MeasureKind::Segments { + self.cut_segments(blade, &mut log); + } else { + self.cut_facets(kind, blade, &mut log); + } + if log.split_particles.is_empty() && log.inserted.is_empty() { + return false; + } + // The cut passed through these particles: the elements around them lose their interior + // strength. + for &(copy, source) in &log.split_particles { + self.particles[copy as usize].damaged = true; + self.particles[source as usize].damaged = true; + } + for inserted in &log.inserted { + for &v in inserted { + self.particles[v as usize].damaged = true; + } + } + + self.remove_straddlers_across_pieces(&mut log); + event.torn_edges.extend(crossed_edges); + event.torn_cells.extend(crossed_cells); + event.removed_edges.extend_from_slice(&log.removed_edges); + event + .split_particles + .extend_from_slice(&log.split_particles); + event + .inserted_particles + .extend(log.inserted.iter().flat_map(|&[_, _, p, q]| [p, q])); + self.finish_topology_change(&log); + true + } + + /// Splits `v` by the blade's supporting line or plane, each element of its fan going to the + /// side of its current centroid; the particle itself stays with the elements on its own side + /// (the first element's side when it lies on the blade). Returns whether it split. + fn split_by_blade( + &mut self, + kind: MeasureKind, + v: u32, + blade: &[Vector; DIM], + log: &mut SplitLog, + ) -> bool { + let mut fan = self.fan(kind, v); + if fan.elements.len() < 2 { + return false; + } + let normal = blade_normal(blade); + let offset = |point: Vector| normal.dot(point - blade[0]); + let raw: Vec = fan + .vertices + .iter() + .map(|vertices| { + let sum: Vector = vertices + .iter() + .map(|&w| self.particles[w as usize].position) + .sum(); + offset(sum / vertices.len() as Real) > 0.0 + }) + .collect(); + let own = offset(self.particles[v as usize].position); + let kept = if own != 0.0 { own > 0.0 } else { raw[0] }; + fan.sides = raw.iter().map(|&r| (r != kept) as usize).collect(); + fan.assign_groups(v); + self.split_fan(v, &fan, log) + } + + /// Cuts a body of cells or triangles: every particle ending an element edge the blade meets is + /// split by the blade ([`Self::split_by_blade`]), so crossed elements go whole to one side and + /// the cut follows their facets; particles then left joining two pieces alone are split. + fn cut_facets(&mut self, kind: MeasureKind, blade: &[Vector; DIM], log: &mut SplitLog) { + let mut candidate = vec![false; self.particles.len()]; + self.for_each_measure_element(kind, |_, vertices| { + for i in 0..vertices.len() { + for j in i + 1..vertices.len() { + let (x, y) = (vertices[i] as usize, vertices[j] as usize); + let (px, py) = (self.particles[x].position, self.particles[y].position); + if blade_hit(blade, px, py).is_some() { + candidate[x] = true; + candidate[y] = true; + } + } + } + }); + for v in 0..candidate.len() { + if candidate[v] { + self.split_by_blade(kind, v as u32, blade, log); + } + } + if !log.split_particles.is_empty() { + self.split_pinched(kind, log); + } + } + + /// Cuts a body of segments: a structural edge crossed away from its endpoints gets two + /// particles inserted at the crossing ([`Self::insert_into_segment`]); one crossed near or at + /// an endpoint splits it instead, unless pinned (never split), which inserts the crossing. + fn cut_segments(&mut self, blade: &[Vector; DIM], log: &mut SplitLog) { + let crossings: Vec<(usize, Real)> = self + .edges + .iter() + .enumerate() + .filter(|(_, e)| e.kind == SoftBodyEdgeKind::Structural) + .filter_map(|(i, e)| { + let [a, b] = e.vertices; + let (pa, pb) = ( + self.particles[a as usize].position, + self.particles[b as usize].position, + ); + blade_hit(blade, pa, pb).map(|t| (i, t)) + }) + .collect(); + // Insertions rewrite edges in place or append them: the crossing indices stay valid. + let mut straddling = Vec::new(); + let mut snapped = Vec::new(); + for (ei, t) in crossings { + let [a, b] = self.edges[ei].vertices; + let near = if t < 0.5 { a } else { b }; + let force = self.particles[near as usize].inv_mass == 0.0; + if !self.insert_into_segment(ei, t, force, &mut straddling, log) { + snapped.push(near); + } + } + if !straddling.is_empty() { + straddling.sort_unstable(); + straddling.dedup(); + let mut removed = vec![false; self.edges.len()]; + for ei in straddling { + removed[ei] = true; + log.removed_edges.push(self.edges[ei].vertices); + } + let mut keep = removed.iter().map(|r| !r); + self.edges.retain(|_| keep.next().unwrap()); + } + snapped.sort_unstable(); + snapped.dedup(); + for v in snapped { + let first = log.split_particles.len(); + if self.split_by_blade(MeasureKind::Segments, v, blade, log) { + // The segments moved to a copy no longer bear their load: their warm start and + // stress would pull the fresh copy. + let copies: Vec = log.split_particles[first..] + .iter() + .map(|(c, _)| *c) + .collect(); + for e in &mut self.edges { + if e.vertices.iter().any(|w| copies.contains(w)) { + e.impulse = 0.0; + e.stress = 0.0; + } + } + } + } + } + + /// Inserts `p`, `q` where the blade crosses structural edge `ei` at `t`: the edge becomes + /// `(a, p)`, `(q, b)` (proportional rest lengths and mass, surface segment too) and spanning + /// edges go to `straddling`. `false` for a massless segment or, unless `force`, an endpoint. + fn insert_into_segment( + &mut self, + ei: usize, + t: Real, + straddling: &mut Vec, + log: &mut SplitLog, + ) -> bool { + let range = MIN_INSERTION_FRACTION..=1.0 - MIN_INSERTION_FRACTION; + if !force && !range.contains(&t) { + return false; + } + let edge = self.edges[ei]; + let [a, b] = edge.vertices; + let length = edge.initial_rest_length(); + let share = |v: u32| { + let total: Real = self + .edges + .iter() + .filter(|e| e.kind == SoftBodyEdgeKind::Structural && e.vertices.contains(&v)) + .map(|e| e.initial_rest_length()) + .sum(); + if total > 0.0 { + self.particles[v as usize].mass * length / total + } else { + 0.0 + } + }; + let (share_a, share_b) = (share(a), share(b)); + let mass = share_a + share_b; + if !(mass > 0.0) || !(length > 0.0) { + return false; + } + + // The non-structural edges from `a`'s side of the segment to `b`'s: bending edges over it. + let neighbors = |v: u32, other: u32| -> Vec { + let mut side = vec![v]; + for e in &self.edges { + if e.kind == SoftBodyEdgeKind::Structural && e.vertices.contains(&v) { + let w = if e.vertices[0] == v { + e.vertices[1] + } else { + e.vertices[0] + }; + if w != other { + side.push(w); + } + } + } + side + }; + let (side_a, side_b) = (neighbors(a, b), neighbors(b, a)); + for (i, e) in self.edges.iter().enumerate() { + if e.kind == SoftBodyEdgeKind::Structural { + continue; + } + let [x, y] = e.vertices; + let only = + |side: &[u32], other: &[u32], w: u32| side.contains(&w) && !other.contains(&w); + if (only(&side_a, &side_b, x) && only(&side_b, &side_a, y)) + || (only(&side_b, &side_a, x) && only(&side_a, &side_b, y)) + { + straddling.push(i); + } + } + + let (pa, pb) = (self.particles[a as usize], self.particles[b as usize]); + for (v, share, kept) in [(a, share_a, t), (b, share_b, 1.0 - t)] { + let p = &mut self.particles[v as usize]; + p.mass -= share - mass * kept * 0.5; + if p.inv_mass != 0.0 { + p.inv_mass = crate::utils::inv(p.mass); + } + } + let mut inserted = [0; 2]; + for (k, kept) in [t, 1.0 - t].into_iter().enumerate() { + let mass = mass * kept * 0.5; + self.particles.push(super::super::SoftBodyParticle { + position: pa.position.lerp(pb.position, t), + velocity: pa.velocity.lerp(pb.velocity, t), + rest_position: pa.rest_position.lerp(pb.rest_position, t), + mass, + inv_mass: crate::utils::inv(mass), + force: Vector::ZERO, + next_position: None, + // New material, not a copy: its own root. + split_root: self.particles.len() as u32, + ..if k == 0 { pa } else { pb } + }); + inserted[k] = self.particles.len() as u32 - 1; + } + let [p, q] = inserted; + + // Plastic flow keeps its strain: the initial rest lengths scale like the rest lengths. + let half = |vertices: [u32; 2], fraction: Real| { + let mut half = edge; + half.vertices = vertices; + half.rest_length = edge.rest_length * fraction; + half.impulse = 0.0; + half.stress = 0.0; + half.torn = false; + half + }; + self.edges[ei] = half([a, p], t); + self.edges.push(half([q, b], 1.0 - t)); + #[cfg(feature = "dim2")] + for si in 0..self.boundary.len() { + let s = self.boundary[si]; + if s == [a, b] { + self.boundary[si] = [a, p]; + self.boundary.push([q, b]); + } else if s == [b, a] { + self.boundary[si] = [b, q]; + self.boundary.push([p, a]); + } + } + log.inserted.push([a, b, p, q]); + true + } +} diff --git a/src/dynamics/soft_body/tearing/tearing_event.rs b/src/dynamics/soft_body/tearing/tearing_event.rs index a56b10163..dfbd799ca 100644 --- a/src/dynamics/soft_body/tearing/tearing_event.rs +++ b/src/dynamics/soft_body/tearing/tearing_event.rs @@ -1,33 +1,132 @@ //! The tear event and the stress level past which a tear is never paced. use crate::alloc_prelude::*; +use crate::dynamics::{ImpulseJointHandle, RigidBodyHandle}; use crate::math::{DIM, Real}; use super::super::SoftBodyHandle; -/// What a tear did to a soft body (see `EventHandler::handle_soft_body_tear_event` and -/// [`crate::dynamics::SoftBodySet::tear`]). -/// -/// The particle indices of `torn_edges` and `torn_cells` are the ones before the tear; those of -/// `split_particles` and `detached_particles` the ones right after it, before any detached -/// particle was dropped (see [`crate::dynamics::SoftBody::set_drop_detached_particles`]): apply `particle_remap` -/// to get the final ones. +/// A cluster piece a tear left: where a cluster the crack ran through ended up (see +/// [`SoftBodyTearEvent::clusters`]). +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub struct SoftClusterSplit { + /// The index of the cluster that was split, in the torn body, before the tear. + pub source_cluster: u32, + /// The soft body holding the piece. + pub soft_body: SoftBodyHandle, + /// The piece's cluster index in that body. + pub cluster: u32, + /// The piece's proxy rigid body. + pub proxy: RigidBodyHandle, + /// Whether the piece kept the source cluster's proxy (the joints and colliders that were not + /// moved stay on it); the other pieces got a fresh one. + pub keeps_proxy: bool, +} + +/// A soft body a tear left: the torn body itself, or a body split off it (see +/// [`SoftBodyTearEvent::pieces`]). +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct SoftBodyPiece { + /// The soft body holding the piece. + pub soft_body: SoftBodyHandle, + /// The particles of the piece: `particles[i]` is the index, in the torn body after the tear + /// (the indices the other fields of the event use), of the piece's `i`-th particle. + pub particles: Vec, + /// The clusters of the piece, as `[index in the torn body before the split, index in the + /// piece]` (a cluster split by the tear is listed under the index its piece got, see + /// [`SoftBodyTearEvent::clusters`]). + pub clusters: Vec<[u32; 2]>, +} + +/// An impulse joint a tear re-attached: its proxy was split, and the piece closest to the joint's +/// anchor in the rest shape is not the one that kept the proxy (see +/// [`SoftBodyTearEvent::moved_joints`]). +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub struct SoftJointMove { + /// The joint. + pub joint: ImpulseJointHandle, + /// The proxy it was attached to. + pub from: RigidBodyHandle, + /// The proxy it is attached to now. + pub to: RigidBodyHandle, +} + +/// What a tear or a cut did to a soft body (see `EventHandler::handle_soft_body_tear_event`, +/// [`crate::dynamics::SoftBodySet::tear`] and [`crate::dynamics::SoftBodySet::cut`]); the +/// indices of `torn_edges`/`torn_cells` predate the tear, the others follow it, before the split. #[derive(Clone, Debug, Default)] pub struct SoftBodyTearEvent { /// The soft body that tore. pub soft_body: SoftBodyHandle, - /// The particle pairs of the removed edges (the ones marked torn, and the ones removed with - /// the torn cells). + /// The particle pairs of the torn edges a crack opened across. In a cut, the edges the blade + /// meets (see [`crate::dynamics::SoftBody::crossing_elements`]). pub torn_edges: Vec<[u32; 2]>, - /// The vertices of the removed cells (the ones marked torn, and the ones spanning a torn - /// edge). + /// The vertices of the torn cells a crack opened through (a tear removes no cell). In a cut, + /// the cells the blade meets. pub torn_cells: Vec<[u32; DIM + 1]>, + /// The particle pairs of the edges removed for straddling an opened crack: bending edges over + /// a split particle, quad diagonals across an opened triangle side, edges joining two pieces. + pub removed_edges: Vec<[u32; 2]>, /// The particles the tear passed through, duplicated one copy per piece: `(copy, source)`. pub split_particles: Vec<(u32, u32)>, - /// The particles the tear left without any element. - /// The number of connected pieces of the body after the tear, the dropped particles gone - /// (see [`crate::dynamics::SoftBody::connected_pieces`]). + /// The particles a cut inserted where it crosses a rope, polyline or wire segment: two per + /// crossed segment, both at the crossing, the first joined to the segment's first particle. + pub inserted_particles: Vec, + /// The soft bodies the torn body came apart into, the piece with the largest rest measure + /// (which keeps the handle) first, the others new soft bodies remembering their origin. Empty + /// when nothing split off; else the first entry maps the retained body's compacted particles. pub pieces: Vec, + /// The pieces of every cluster the crack split (see + /// [`crate::dynamics::SoftBody::seeded_components`]): one piece keeps the proxy, the others + /// get a fresh one. Empty when no cluster was split. + pub clusters: Vec, + /// The impulse joints moved to another proxy by a cluster split: a joint follows the piece + /// closest to its anchor in the rest shape. + pub moved_joints: Vec, +} + +impl SoftBodyTearEvent { + /// The soft bodies the torn body is in after the tear, the one keeping the handle first: the + /// torn body alone when nothing was split off, the [`Self::pieces`] otherwise. + pub fn bodies(&self) -> impl Iterator + '_ { + let whole = self.pieces.is_empty().then_some(self.soft_body); + whole + .into_iter() + .chain(self.pieces.iter().map(|piece| piece.soft_body)) + } + + /// Where the particle `particle` of the torn body (a post-tear index, as the other fields + /// use) is now: the soft body holding it and its index there. `None` for an index the torn + /// body never had. + pub fn particle_destination(&self, particle: u32) -> Option<(SoftBodyHandle, u32)> { + if self.pieces.is_empty() { + return Some((self.soft_body, particle)); + } + self.pieces.iter().find_map(|piece| { + piece + .particles + .binary_search(&particle) + .ok() + .map(|i| (piece.soft_body, i as u32)) + }) + } + + /// The particle pairs this tear or cut separated: every split copy with its source, and the + /// two particles a cut inserted on each side of a crossed segment. The seeds of + /// [`crate::dynamics::SoftBody::seeded_components`] (post-tear indices). + pub fn seeds(&self) -> Vec<[u32; 2]> { + let mut seeds: Vec<[u32; 2]> = self + .split_particles + .iter() + .map(|&(copy, source)| [copy, source]) + .collect(); + seeds.extend( + self.inserted_particles + .chunks_exact(2) + .map(|pair| [pair[0], pair[1]]), + ); + seeds + } } /// The smoothed load (a fraction of the tear threshold) past which a torn edge is never paced diff --git a/src/dynamics/soft_body/tearing/tearing_helpers.rs b/src/dynamics/soft_body/tearing/tearing_helpers.rs index e984d16c3..b391180bf 100644 --- a/src/dynamics/soft_body/tearing/tearing_helpers.rs +++ b/src/dynamics/soft_body/tearing/tearing_helpers.rs @@ -56,6 +56,93 @@ impl SoftBody { pieces } + /// The components of the body's element graph restricted to `members` (all particles when + /// `None`) that contain a particle of some `seeds` pair (the pairs a tear or cut separated, see + /// [`crate::dynamics::SoftBodyTearEvent::split_particles`]), sorted; empty unless at least two. + pub fn seeded_components(&self, members: Option<&[u32]>, seeds: &[[u32; 2]]) -> Vec> { + let n = self.particles.len(); + let member = + |v: u32| (v as usize) < n && members.is_none_or(|m| m.binary_search(&v).is_ok()); + let mut parent: Vec = (0..n as u32).collect(); + fn find(parent: &mut [u32], mut i: u32) -> u32 { + while parent[i as usize] != i { + let up = parent[parent[i as usize] as usize]; + parent[i as usize] = up; + i = up; + } + i + } + // Pairwise over the in-set vertices: a chain through an out-of-set vertex must not break. + let mut union = |vertices: &[u32]| { + for i in 0..vertices.len() { + if !member(vertices[i]) { + continue; + } + for j in i + 1..vertices.len() { + if !member(vertices[j]) { + continue; + } + let (a, b) = (find(&mut parent, vertices[i]), find(&mut parent, vertices[j])); + if a != b { + parent[a.max(b) as usize] = a.min(b); + } + } + } + }; + for e in &self.edges { + union(&e.vertices); + } + for c in &self.cells { + union(&c.vertices); + } + #[cfg(feature = "dim3")] + for d in &self.dihedrals { + union(&d.vertices); + } + for s in &self.boundary { + union(s); + } + let mut seeded = vec![false; n]; + for pair in seeds { + for &v in pair { + if member(v) { + seeded[find(&mut parent, v) as usize] = true; + } + } + } + if seeded.iter().filter(|s| **s).count() < 2 { + return Vec::new(); + } + // Roots are the smallest index of their component, so the components come out ordered. + let mut piece_of_root = vec![u32::MAX; n]; + let mut pieces: Vec> = Vec::new(); + let mut visit = |v: u32, parent: &mut [u32]| { + let root = find(parent, v) as usize; + if !seeded[root] { + return; + } + if piece_of_root[root] == u32::MAX { + piece_of_root[root] = pieces.len() as u32; + pieces.push(Vec::new()); + } + pieces[piece_of_root[root] as usize].push(v); + }; + match members { + Some(members) => members.iter().for_each(|&v| visit(v, &mut parent)), + None => (0..n as u32).for_each(|v| visit(v, &mut parent)), + } + pieces + } + + /// The summed nominal mass of the given particles (out-of-range indices ignored). + pub fn mass_of(&self, particles: &[u32]) -> Real { + particles + .iter() + .filter_map(|&v| self.particles.get(v as usize)) + .map(|p| p.mass) + .sum() + } + /// Sets the tear-threshold multiplier of the `i`-th edge (see /// [`crate::dynamics::SoftBodyEdge::tear_resistance`]); `1.0` restores the material's threshold. pub fn set_edge_tear_resistance(&mut self, i: usize, resistance: Real) { diff --git a/src/dynamics/soft_body/tearing/tearing_particle_split.rs b/src/dynamics/soft_body/tearing/tearing_particle_split.rs index 029fbda1a..4ff2b8351 100644 --- a/src/dynamics/soft_body/tearing/tearing_particle_split.rs +++ b/src/dynamics/soft_body/tearing/tearing_particle_split.rs @@ -9,365 +9,733 @@ use parry::utils::hashmap::HashMap; #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use super::tearing::{pair, shared_others}; -use super::super::{SoftBody, SoftBodyParticle}; +use super::super::soft_body_builder::cell_faces; +use super::super::{SoftBody, SoftBodyEdgeKind, SoftBodyMaterial, SoftBodyParticle}; +use super::tearing::shared_others; + +/// The elements carrying a body's rest measure (see [`SoftBody::rest_measure`]). +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub(super) enum MeasureKind { + /// The cells (triangles in 2D, tetrahedra in 3D). + Cells, + /// The surface triangles of a 3D body without cells (cloth, shells). + Triangles, + /// The structural edges (ropes, polylines, polygons, wires, spring networks). + Segments, +} + +impl MeasureKind { + /// The number of vertices besides `v` two elements have in common when they share a facet + /// through `v` (segments always do: their facet is `v` itself). + fn facet_others(self) -> usize { + match self { + MeasureKind::Cells => DIM - 1, + MeasureKind::Triangles => 1, + MeasureKind::Segments => 0, + } + } +} + +/// The measure elements containing a particle, and the groups a split of the particle gives them. +#[derive(Clone, Debug)] +pub(super) struct Fan { + /// The kind of the elements. + pub kind: MeasureKind, + /// The element indices: into the cells, the boundary or the edges, by `kind`. + pub elements: Vec, + /// The vertices of each element, as they were when the fan was taken. + pub vertices: Vec>, + /// The rest measure of each element. + pub measures: Vec, + /// The side of the split plane each element lies on (`0` or `1`; `0` without a plane). + pub sides: Vec, + /// The group of each element in `0..num_groups`: group 0 keeps the particle, every other + /// group gets a copy. + pub groups: Vec, + /// The number of groups. + pub num_groups: usize, +} + +impl Fan { + /// Groups the elements into the connected components of their adjacency at `v` (sharing a + /// facet through `v`) within each side, numbered side 0 first, then by first element. + pub(super) fn assign_groups(&mut self, v: u32) { + let n = self.elements.len(); + let mut parent: Vec = (0..n).collect(); + fn find(parent: &mut [usize], mut i: usize) -> usize { + while parent[i] != i { + parent[i] = parent[parent[i]]; + i = parent[i]; + } + i + } + let needed = self.kind.facet_others(); + for i in 0..n { + for j in i + 1..n { + if self.sides[i] == self.sides[j] + && shared_others(&self.vertices[i], &self.vertices[j], v) >= needed + { + let (ri, rj) = (find(&mut parent, i), find(&mut parent, j)); + if ri != rj { + parent[ri.max(rj)] = ri.min(rj); + } + } + } + } + let mut group_of_root = vec![usize::MAX; n]; + self.num_groups = 0; + for side in 0..2 { + for i in (0..n).filter(|&i| self.sides[i] == side) { + let root = find(&mut parent, i); + if group_of_root[root] == usize::MAX { + group_of_root[root] = self.num_groups; + self.num_groups += 1; + } + self.groups[i] = group_of_root[root]; + } + } + } +} + +/// What the particle splits of a tear did. +#[derive(Clone, Debug, Default)] +pub(super) struct SplitLog { + /// The particle copies, as `(copy, source)`, in creation order. + pub split_particles: Vec<(u32, u32)>, + /// The particle pairs of the non-measure edges removed for straddling an opened crack. + pub removed_edges: Vec<[u32; 2]>, + /// The segments a cut inserted particles into, as `[a, b, p, q]`: the segment `(a, b)` became + /// `(a, p)` and `(q, b)`. + pub inserted: Vec<[u32; 4]>, +} + +/// Replaces every occurrence of `from` in `vertices` by `to`. +fn replace(vertices: &mut [u32], from: u32, to: u32) { + for x in vertices.iter_mut() { + if *x == from { + *x = to; + } + } +} impl SoftBody { /// The kind of this body's measure elements, by priority: its cells, its surface triangles /// (3D, without cells), its structural edges. `None` when it has none of them. - pub(super) fn split_particles(&mut self, candidates: &[u32]) -> Vec<(u32, u32)> { - let mut duplicated = Vec::new(); - // Faces: cells when the body has some, the surface elements otherwise (3D only: the - // segments of a 2D polyline never split, a polyline is cut by removing a segment). - let use_cells = !self.cells.is_empty(); - let use_surface = !use_cells && DIM == 3 && !self.boundary.is_empty(); - if !use_cells && !use_surface { - return duplicated; - } - // Two faces around `v` are adjacent when they share a facet through `v`. - let needed = if use_cells { DIM - 1 } else { DIM - 2 }; + pub(super) fn measure_kind(&self) -> Option { + if !self.cells.is_empty() { + Some(MeasureKind::Cells) + } else if DIM == 3 && !self.boundary.is_empty() { + Some(MeasureKind::Triangles) + } else if self + .edges + .iter() + .any(|e| e.kind == SoftBodyEdgeKind::Structural) + { + Some(MeasureKind::Segments) + } else { + None + } + } - for &v in candidates { - let face_ids: Vec = if use_cells { - self.cells - .iter() - .enumerate() - .filter(|(_, c)| c.vertices.contains(&v)) - .map(|(i, _)| i as u32) - .collect() - } else { - self.boundary - .iter() - .enumerate() - .filter(|(_, s)| s.contains(&v)) - .map(|(i, _)| i as u32) - .collect() - }; - if face_ids.len() < 2 { - continue; - } - let face_vertices = |f: u32| -> Vec { - if use_cells { - self.cells[f as usize].vertices.to_vec() - } else { - self.boundary[f as usize].to_vec() + /// Calls `f` with the index and the vertices of every measure element of the given kind. + pub(super) fn for_each_measure_element( + &self, + kind: MeasureKind, + mut f: impl FnMut(u32, &[u32]), + ) { + match kind { + MeasureKind::Cells => { + for (i, c) in self.cells.iter().enumerate() { + f(i as u32, &c.vertices); } - }; - // Union-find over the faces at `v`, groups numbered by first appearance. - let n = face_ids.len(); - let mut parent: Vec = (0..n).collect(); - fn find(parent: &mut [usize], mut i: usize) -> usize { - while parent[i] != i { - parent[i] = parent[parent[i]]; - i = parent[i]; + } + MeasureKind::Triangles => { + for (i, t) in self.boundary.iter().enumerate() { + f(i as u32, t); } - i } - let verts: Vec> = face_ids.iter().map(|&f| face_vertices(f)).collect(); - for i in 0..n { - for j in i + 1..n { - if shared_others(&verts[i], &verts[j], v) >= needed { - let (ri, rj) = (find(&mut parent, i), find(&mut parent, j)); - if ri != rj { - parent[ri.max(rj)] = ri.min(rj); - } + MeasureKind::Segments => { + for (i, e) in self.edges.iter().enumerate() { + if e.kind == SoftBodyEdgeKind::Structural { + f(i as u32, &e.vertices); } } } - let mut group_of_root: Vec = vec![usize::MAX; n]; - let mut group: Vec = vec![0; n]; - let mut num_groups = 0; - for i in 0..n { - let r = find(&mut parent, i); - if group_of_root[r] == usize::MAX { - group_of_root[r] = num_groups; - num_groups += 1; - } - group[i] = group_of_root[r]; + } + } + + /// The total rest measure of this body's measure elements, by priority: the rest area (2D) or + /// volume (3D) of its cells, else (3D) of its surface triangles, else the rest length of its + /// structural edges, before plastic flow; `0.0` without any, and no tear or cut changes it. + pub fn rest_measure(&self) -> Real { + let Some(kind) = self.measure_kind() else { + return 0.0; + }; + let mut total = 0.0; + self.for_each_measure_element(kind, |i, _| total += self.element_rest_measure(kind, i)); + total + } + + /// The rest measure of the measure elements whose vertices all belong to the given sorted + /// particle set (see [`Self::rest_measure`]): the measure of a piece of the body. + pub fn rest_measure_of(&self, particles: &[u32]) -> Real { + let Some(kind) = self.measure_kind() else { + return 0.0; + }; + let mut total = 0.0; + self.for_each_measure_element(kind, |i, vertices| { + if vertices.iter().all(|v| particles.binary_search(v).is_ok()) { + total += self.element_rest_measure(kind, i); } - if num_groups < 2 { - continue; + }); + total + } + + /// The rest-shape distance from `point` to the material of the given sorted particle set: the + /// closest measure element with all vertices in the set (zero in a cell), else the closest + /// particle of the set. What a joint reads to follow a split cluster's piece. + pub(crate) fn rest_distance_to(&self, particles: &[u32], point: Vector) -> Real { + use parry::query::PointQuery; + let rest = |v: u32| self.particles[v as usize].rest_position; + let mut best = Real::MAX; + if let Some(kind) = self.measure_kind() { + self.for_each_measure_element(kind, |_, vertices| { + if !vertices.iter().all(|v| particles.binary_search(v).is_ok()) { + return; + } + let distance = match vertices.len() { + 2 => parry::shape::Segment::new(rest(vertices[0]), rest(vertices[1])) + .distance_to_local_point(point, true), + 3 => parry::shape::Triangle::new( + rest(vertices[0]), + rest(vertices[1]), + rest(vertices[2]), + ) + .distance_to_local_point(point, true), + #[cfg(feature = "dim3")] + 4 => parry::shape::Tetrahedron::new( + rest(vertices[0]), + rest(vertices[1]), + rest(vertices[2]), + rest(vertices[3]), + ) + .distance_to_local_point(point, true), + _ => return, + }; + best = best.min(distance); + }); + } + if best == Real::MAX { + for &v in particles { + if let Some(p) = self.particles.get(v as usize) { + best = best.min((p.rest_position - point).length()); + } } + } + best + } - self.split_vertex( - v, - use_cells, - &face_ids, - &verts, - &group, - num_groups, - &mut duplicated, - ); + /// The rest measure of the `i`-th measure element of the given kind: a cell's rest area or + /// volume, a triangle's rest area, an edge's rest length before any plastic flow. + fn element_rest_measure(&self, kind: MeasureKind, i: u32) -> Real { + match kind { + MeasureKind::Cells => self.cells[i as usize].rest_volume.abs(), + MeasureKind::Triangles => { + #[cfg(feature = "dim3")] + { + let rest = |v: u32| self.particles[v as usize].initial_rest_position; + let t = self.boundary[i as usize]; + (rest(t[1]) - rest(t[0])).cross(rest(t[2]) - rest(t[0])).length() * 0.5 + } + #[cfg(feature = "dim2")] + { + 0.0 + } + } + MeasureKind::Segments => self.edges[i as usize].initial_rest_length(), } - duplicated } - /// Opens a cloth crack across the torn edge `(a, b)`: one endpoint (the first, in index - /// order, whose fan has triangles on both sides) is split along the plane through it - /// perpendicular to the edge in the rest shape, the triangles on the other endpoint's side - /// going to the copy. The material is left whole; the tension the edge carried is released - /// through the crack instead. - pub(super) fn crack_across(&mut self, a: u32, b: u32, duplicated: &mut Vec<(u32, u32)>) { - for (v, other) in [(a, b), (b, a)] { - let face_ids: Vec = self - .boundary - .iter() - .enumerate() - .filter(|(_, s)| s.contains(&v)) - .map(|(i, _)| i as u32) - .collect(); - if face_ids.len() < 2 { - continue; + /// Whether some measure element of this body contains all the given particles. + pub(crate) fn measure_element_holds(&self, particles: &[u32]) -> bool { + let Some(kind) = self.measure_kind() else { + return false; + }; + let mut found = false; + self.for_each_measure_element(kind, |_, vertices| { + found |= particles.iter().all(|p| vertices.contains(p)); + }); + found + } + + /// The fan of `v`: the measure elements of the given kind containing it, in one group. + pub(super) fn fan(&self, kind: MeasureKind, v: u32) -> Fan { + let mut elements = Vec::new(); + let mut vertices = Vec::new(); + self.for_each_measure_element(kind, |i, element| { + if element.contains(&v) { + elements.push(i); + vertices.push(element.to_vec()); } - let rest = |i: u32| self.particles[i as usize].rest_position; - let axis = rest(other) - rest(v); - let verts: Vec> = face_ids + }); + let n = elements.len(); + Fan { + kind, + measures: elements .iter() - .map(|&f| self.boundary[f as usize].to_vec()) - .collect(); - // Group 1: the triangles whose rest centroid lies on the other endpoint's side. - let group: Vec = verts - .iter() - .map(|fv| { - let mut c = Vector::ZERO; - for &w in fv { - c += rest(w) - rest(v); - } - (c.dot(axis) > 0.0) as usize - }) - .collect(); - if group.iter().all(|&g| g == 0) || group.iter().all(|&g| g == 1) { - /// Whether the plane split of `v` given by `fan` leaves at least `MIN_PIECE` triangles - /// reachable on each side without passing through `v` (no confetti). Always `true` outside - /// 3D triangle bodies. - pub(super) fn opens_without_confetti(&self, v: u32, fan: &Fan) -> bool { - if fan.kind != MeasureKind::Triangles { - return true; + .map(|&i| self.element_rest_measure(kind, i)) + .collect(), + elements, + vertices, + sides: vec![0; n], + groups: vec![0; n], + num_groups: (n > 0) as usize, } - let piece_ok = |side: usize| -> bool { - .filter(|&i| fan.sides[i] == side) - } - self.reachable_triangles(&seeds, v, MIN_PIECE) >= MIN_PIECE + } + + /// Duplicates every candidate particle whose measure elements no longer form one connected + /// fan (a crack left the particle joining two pieces alone): one copy per extra group, the + /// other elements around the particle following the group they attach to. + pub(super) fn split_particles(&mut self, candidates: &[u32], log: &mut SplitLog) { + let Some(kind) = self.measure_kind() else { + return; }; - piece_ok(0) && piece_ok(1) - const MIN_PIECE: usize = 10; - .iter() - .zip(&group) - .map(|(&f, _)| f) + for &v in candidates { + let mut fan = self.fan(kind, v); + if fan.elements.len() < 2 { continue; } - self.split_vertex(v, false, &face_ids, &verts, &group, 2, duplicated); - /// Number of surface triangles reachable from `seeds` through shared edges not touching - /// `pivot`, counting up to `limit`. - fn reachable_triangles(&self, seeds: &[u32], pivot: u32, limit: usize) -> usize { - // Edge -> triangles map of the whole surface (a tear is a rare event). - let mut by_edge: HashMap<[u32; 2], Vec> = HashMap::default(); - for (ti, t) in self.boundary.iter().enumerate() { - for i in 0..DIM { - let key = pair(t[i], t[(i + 1) % DIM]); - if key[0] != pivot && key[1] != pivot { - by_edge.entry(key).or_default().push(ti as u32); - while let Some(ti) = stack.pop() { - let t = self.boundary[ti as usize]; - for i in 0..DIM { - let key = pair(t[i], t[(i + 1) % DIM]); - if let Some(neighbors) = by_edge.get(&key) { + fan.assign_groups(v); + if fan.num_groups >= 2 { + self.split_fan(v, &fan, log); } } } - /// Splits the particle `v` into one copy per group of its faces (`face_ids`, their - /// vertices in `verts`, `group[i]` in `0..num_groups`): group 0 keeps `v`, the others get a - /// copy; the mass follows, and so do the other elements around `v` (see `split_particles`). - #[allow(clippy::too_many_arguments)] - fn split_vertex( - use_cells: bool, - face_ids: &[u32], - verts: &[Vec], - group: &[usize], - num_groups: usize, - duplicated: &mut Vec<(u32, u32)>, - ) { - // A cloth fan group holding no spring at `v` (all its edges there torn) is a loose bit - // of fabric: its triangles are dropped instead of a copy of `v` that nothing holds - // (it would fly off with the triangle it renders). - if !use_cells { - for g in 0..num_groups { - if has_spring[g] { + /// The fan of `v` split by the plane through `origin` with normal `normal`, in the rest + /// shape: every element goes to the side of its rest centroid, and the groups are the + /// connected components within a side. `None` when the fan lies on one side only. + pub(super) fn plane_fan(&self, v: u32, origin: Vector, normal: Vector) -> Option { + let kind = self.measure_kind()?; + let mut fan = self.fan(kind, v); + let rest = |i: u32| self.particles[i as usize].rest_position; + for (side, vertices) in fan.sides.iter_mut().zip(&fan.vertices) { + let offset: Vector = vertices.iter().map(|&w| rest(w) - origin).sum(); + *side = (offset.dot(normal) > 0.0) as usize; + } + if !fan.sides.contains(&0) || !fan.sides.contains(&1) { + return None; + } + fan.assign_groups(v); + Some(fan) + } + + /// The smallest piece, in measure elements of the given kind, a tear may split off this + /// body: the material's [`SoftBodyMaterial::min_piece`], else the kind's default (see + /// [`default_min_piece`]). + pub(super) fn min_piece(&self, kind: MeasureKind) -> usize { + self.material + .min_piece + .map_or(default_min_piece(kind), |n| n.max(1) as usize) + } + + /// Whether the split of `v` given by `fan` leaves every group at least [`Self::min_piece`] + /// measure elements reachable without passing through `v` (no chips). Tears consult it, cuts + /// do not. + pub(super) fn opens_without_confetti(&self, v: u32, fan: &Fan) -> bool { + let min = self.min_piece(fan.kind); + (0..fan.num_groups).all(|g| { + let seeds: Vec = (0..fan.elements.len()) + .filter(|&i| fan.groups[i] == g) + .map(|i| fan.elements[i]) + .collect(); + self.reachable_elements(fan.kind, &seeds, v, min) >= min + }) + } + + /// Number of measure elements of the given kind reachable from `seeds` through shared facets + /// not containing `pivot`, counting up to `limit`. + fn reachable_elements( + &self, + kind: MeasureKind, + seeds: &[u32], + pivot: u32, + limit: usize, + ) -> usize { + // Calls `f` with every facet of an element not containing `pivot`: its vertices but one, + // sorted and padded with `u32::MAX`. + fn facets(vertices: &[u32], pivot: u32, mut f: impl FnMut([u32; 3])) { + for skip in 0..vertices.len() { + let mut key = [u32::MAX; 3]; + let mut k = 0; + for (i, &w) in vertices.iter().enumerate() { + if i != skip { + key[k] = w; + k += 1; } - let mut kept_ids = Vec::new(); - let mut kept_verts = Vec::new(); - let mut kept_group = Vec::new(); - v, - use_cells, - &kept_ids, - &kept_verts, - &kept_group, - kept_groups, - duplicated, - ); - let mut keep = - return; - } - } - // Mass split by rest volume (cells) or face count. - let mut weights = vec![0.0; num_groups]; - for (i, &f) in face_ids.iter().enumerate() { - weights[group[i]] += if use_cells { - self.cells[f as usize].rest_volume.abs() - } else { - 1.0 - }; + } + if !key[..k].contains(&pivot) { + key[..k].sort_unstable(); + f(key); + } + } } - let new_index = |g: usize, particles: &mut Vec| -> u32 { - source.mass * weights[g] / total - } else { - source.mass / num_groups as Real - let p = &mut particles[v as usize]; - v - // A split particle is never pinned, so its copies are free like it. - particles.push(SoftBodyParticle { - particles.len() as u32 - 1 + // Facet -> elements map of the whole body (a tear is a rare event). + let mut by_facet: HashMap<[u32; 3], Vec> = HashMap::default(); + let mut elements: HashMap> = HashMap::default(); + self.for_each_measure_element(kind, |i, vertices| { + elements.insert(i, vertices.to_vec()); + facets(vertices, pivot, |key| by_facet.entry(key).or_default().push(i)); + }); + let mut seen: HashMap = HashMap::default(); + let mut stack: Vec = seeds.to_vec(); + for &s in seeds { + seen.insert(s, ()); + } + while let Some(e) = stack.pop() { + if seen.len() >= limit { + return seen.len(); } - }; - let mut copies: Vec = Vec::with_capacity(num_groups); - for g in 0..num_groups { - let idx = new_index(g, &mut self.particles); - if g > 0 { - duplicated.push((idx, v)); + facets(&elements[&e], pivot, |key| { + if let Some(neighbors) = by_facet.get(&key) { + for &n in neighbors { + if !seen.contains_key(&n) { + seen.insert(n, ()); + stack.push(n); + } + } + } + }); + } + seen.len() + } + + /// Splits `v` by the groups of its fan (see [`Self::split_vertex`]); returns whether the + /// topology changed. + pub(super) fn split_fan(&mut self, v: u32, fan: &Fan, log: &mut SplitLog) -> bool { + // A pinned (fixed or kinematic) particle is never split: a clamp keeps its material. + if fan.num_groups < 2 || self.particles[v as usize].inv_mass == 0.0 { + return false; + } + self.split_vertex(v, fan, log); + true + } + + /// The group an element around `v` that holds no measure element follows: the lowest group + /// of the fan elements it shares a vertex with, or the group whose fan points its way in the + /// rest shape. + fn group_toward(&self, v: u32, fan: &Fan, vertices: &[u32]) -> usize { + let mut best: Option = None; + for (i, fv) in fan.vertices.iter().enumerate() { + if fv.iter().any(|x| *x != v && vertices.contains(x)) { + best = Some(best.map_or(fan.groups[i], |b| b.min(fan.groups[i]))); } - copies.push(idx); } + if let Some(g) = best { + return g; + } + let rest = |i: u32| self.particles[i as usize].rest_position; + let mut dir = Vector::ZERO; + for &w in vertices.iter().filter(|&&w| w != v) { + dir += rest(w) - rest(v); + } + let mut best_score = -Real::MAX; + let mut best_group = 0; + for (i, fv) in fan.vertices.iter().enumerate() { + for &w in fv.iter().filter(|&&w| w != v) { + let d = rest(w) - rest(v); + let score = d.dot(dir) / (d.length() * dir.length()).max(1.0e-12); + if score > best_score { + best_score = score; + best_group = fan.groups[i]; + } + } + } + best_group + } - // Reassign the faces of the other groups to their copies. - for (i, &f) in face_ids.iter().enumerate() { - let g = group[i]; - let target: &mut [u32] = if use_cells { - &mut self.cells[f as usize].vertices + /// Whether some measure element of the given kind contains both `x` and `y`. + fn share_measure_element(&self, kind: MeasureKind, x: u32, y: u32) -> bool { + let mut found = false; + self.for_each_measure_element(kind, |_, vertices| { + found |= vertices.contains(&x) && vertices.contains(&y); + }); + found + } + + /// Splits the particle `v` into one particle per group of its fan: group 0 keeps `v`, every + /// other group gets a copy with its fan elements, and the mass is shared by the groups' + /// rest measures; the remaining elements at `v` are handled per [`Self::group_toward`]. + fn split_vertex(&mut self, v: u32, fan: &Fan, log: &mut SplitLog) { + let kind = fan.kind; + let n = fan.elements.len(); + let mut weights = vec![0.0; fan.num_groups]; + for (&g, &measure) in fan.groups.iter().zip(&fan.measures) { + weights[g] += measure; + } + let total: Real = weights.iter().sum(); + let source = self.particles[v as usize]; + let mut copies: Vec = Vec::with_capacity(fan.num_groups); + for (g, weight) in weights.iter().enumerate() { + let mass = if total > 0.0 { + source.mass * weight / total } else { - &mut self.boundary[f as usize] + source.mass / fan.num_groups as Real }; - for x in target.iter_mut() { - if *x == v { - *x = copies[g]; + if g == 0 { + let p = &mut self.particles[v as usize]; + p.mass = mass; + if p.inv_mass != 0.0 { + p.inv_mass = crate::utils::inv(mass); } + copies.push(v); + } else { + // A split particle is never pinned, so its copies are free like it. + self.particles.push(SoftBodyParticle { + mass, + inv_mass: crate::utils::inv(mass), + next_position: None, + split_root: source.split_root(v), + ..source + }); + let copy = self.particles.len() as u32 - 1; + log.split_particles.push((copy, v)); + copies.push(copy); } } - // The other elements at `v`: the group of the faces they share a vertex with, or - // (unattached) the group whose fan points their way in the rest shape. - let group_of_element = |vertices: &[u32], this: &SoftBody| -> usize { - let mut best: Option = None; - for (i, fv) in verts.iter().enumerate() { - if fv.iter().any(|x| *x != v && vertices.contains(x)) { - best = Some(best.map_or(group[i], |b: usize| b.min(group[i]))); + // The groups of the fan elements holding each neighbor of `v`, sorted. + let mut ring: HashMap> = HashMap::default(); + for (vertices, &g) in fan.vertices.iter().zip(&fan.groups) { + for &w in vertices.iter().filter(|&&w| w != v) { + let groups = ring.entry(w).or_default(); + if let Err(pos) = groups.binary_search(&g) { + groups.insert(pos, g); } } - if let Some(g) = best { - return g; - } - let rest = |i: u32| this.particles[i as usize].rest_position; - let mut dir = Vector::ZERO; - for &w in vertices.iter().filter(|&&w| w != v) { - dir += rest(w) - rest(v); - } - let mut best_score = -Real::MAX; - let mut best_group = 0; - for (i, fv) in verts.iter().enumerate() { - for &w in fv.iter().filter(|&&w| w != v) { - let d = rest(w) - rest(v); - let score = d.dot(dir) / (d.length() * dir.length()).max(1.0e-12); - if score > best_score { - best_score = score; - best_group = group[i]; + } + // The facets through `v` shared by cells of different groups: the crack opens there. + let mut crack_facets: Vec<(usize, usize)> = Vec::new(); + if kind == MeasureKind::Cells { + for i in 0..n { + for j in i + 1..n { + if fan.groups[i] != fan.groups[j] + && shared_others(&fan.vertices[i], &fan.vertices[j], v) == DIM - 1 + { + crack_facets.push((i, j)); } } } - best_group + } + + for (i, &e) in fan.elements.iter().enumerate() { + let g = fan.groups[i]; + if g == 0 { + continue; + } + let target: &mut [u32] = match kind { + MeasureKind::Cells => &mut self.cells[e as usize].vertices, + MeasureKind::Triangles => &mut self.boundary[e as usize], + MeasureKind::Segments => &mut self.edges[e as usize].vertices, + }; + replace(target, v, copies[g]); + } + + // The other edges at `v`. One shared by fan elements of several groups (the crack ends + // there) gets a copy per group, so no side stretches freely across the crack. + let is_measure = |e: &super::super::SoftBodyEdge| { + kind == MeasureKind::Segments && e.kind == SoftBodyEdgeKind::Structural }; - let vertices = self.edges[ei].vertices; - if !vertices.contains(&v) { + let mut new_edges = Vec::new(); + for ei in 0..self.edges.len() { + let edge = self.edges[ei]; + if !edge.vertices.contains(&v) || is_measure(&edge) { + continue; } - let x = if vertices[0] == v { - vertices[1] + let x = if edge.vertices[0] == v { + edge.vertices[1] } else { - vertices[0] + edge.vertices[0] }; - let mut groups: Vec = verts - .iter() - .enumerate() - .filter(|(_, fv)| fv.contains(&x)) - .map(|(i, _)| group[i]) - .collect(); - groups.sort_unstable(); - groups.dedup(); - let g = if groups.is_empty() { - group_of_element(&vertices, self) - } else { - groups[0] + let groups = ring.get(&x).map(Vec::as_slice).unwrap_or(&[]); + let g = match groups.first() { + Some(&g) => g, + None => self.group_toward(v, fan, &edge.vertices), }; - for x in &mut self.edges[ei].vertices { - if *x == v { - *x = copies[g]; - } + replace(&mut self.edges[ei].vertices, v, copies[g]); + for &other in groups.iter().skip(1) { + let mut copy = self.edges[ei]; + replace(&mut copy.vertices, copies[g], copies[other]); + copy.impulse = 0.0; + new_edges.push(copy); } - for x in &mut copy.vertices { - if *x == copies[g] { - *x = copies[other]; - } - } + } + self.edges.extend(new_edges); + + // Straddlers: a non-measure edge between two neighbors of `v` on disjoint groups, not + // held together by a measure element (a bend edge over `v`, a quad diagonal). + let mut straddling = vec![false; self.edges.len()]; + for (ei, e) in self.edges.iter().enumerate() { + if is_measure(e) || e.vertices.iter().any(|w| copies.contains(w)) { + continue; + } + let [x, y] = e.vertices; + let (Some(gx), Some(gy)) = (ring.get(&x), ring.get(&y)) else { + continue; + }; + if gx.iter().all(|g| gy.binary_search(g).is_err()) + && !self.share_measure_element(kind, x, y) + { + straddling[ei] = true; + log.removed_edges.push(e.vertices); + } + } + if straddling.contains(&true) { + let mut keep = straddling.iter().map(|s| !s); + self.edges.retain(|_| keep.next().unwrap()); + } + #[cfg(feature = "dim3")] { - let mut removed = Vec::new(); + // The fan element holding each triangle of a dihedral, if any. + let holder = |t: [u32; 3]| { + fan.vertices + .iter() + .position(|fv| t.iter().all(|w| fv.contains(w))) + }; + let mut removed = vec![false; self.dihedrals.len()]; for di in 0..self.dihedrals.len() { - let vertices = self.dihedrals[di].vertices; - if !vertices.contains(&v) { + let d = self.dihedrals[di].vertices; + if !d.contains(&v) { continue; } - // A dihedral bending across the tear (its two triangles in different - // fans) is dropped. - let mut groups: Vec = Vec::new(); - for (i, fv) in verts.iter().enumerate() { - if fv.iter().any(|x| *x != v && vertices.contains(x)) { - groups.push(group[i]); + let g = match (holder([d[0], d[1], d[2]]), holder([d[0], d[1], d[3]])) { + // Bending across the crack. + (Some(i), Some(j)) if fan.groups[i] != fan.groups[j] => { + removed[di] = true; + continue; } - } - groups.sort_unstable(); - groups.dedup(); - if groups.len() > 1 { - removed.push(di); + (Some(i), _) | (None, Some(i)) => fan.groups[i], + (None, None) => self.group_toward(v, fan, &d), + }; + replace(&mut self.dihedrals[di].vertices, v, copies[g]); + } + if removed.contains(&true) { + let mut keep = removed.iter().map(|r| !r); + self.dihedrals.retain(|_| keep.next().unwrap()); + } + } + + if kind != MeasureKind::Triangles { + // A surface element follows the measure element holding it (its cell, or the segment + // it is): the fan of a shared vertex alone would let a face bridge two pieces. + for si in 0..self.boundary.len() { + let element = self.boundary[si]; + if !element.contains(&v) { continue; } - let g = group_of_element(&vertices, self); - for x in &mut self.dihedrals[di].vertices { - if *x == v { - *x = copies[g]; - } + let holder = fan + .vertices + .iter() + .position(|fv| element.iter().all(|x| fv.contains(x))); + let g = match holder { + Some(i) => fan.groups[i], + None => self.group_toward(v, fan, &element), + }; + replace(&mut self.boundary[si], v, copies[g]); + } + } + for &(i, j) in &crack_facets { + for k in [i, j] { + let cell = self.cells[fan.elements[k] as usize].vertices; + let copy = copies[fan.groups[k]]; + let on_facet = |w: &u32| { + *w == copy || (*w != v && fan.vertices[i].contains(w) && fan.vertices[j].contains(w)) + }; + if let Some(face) = cell_faces(cell) + .into_iter() + .find(|face| face.iter().all(on_facet)) + { + self.boundary.push(face); } } - for di in removed.into_iter().rev() { - self.dihedrals.swap_remove(di); + } + } + + /// Removes the non-measure elements (edges, dihedrals, surface elements of a body of cells or + /// segments) joining pieces that the measure elements alone leave disconnected: the catch-all + /// for straddlers a split misses. Elements touching a measure-less particle stay. + pub(super) fn remove_straddlers_across_pieces(&mut self, log: &mut SplitLog) { + let Some(kind) = self.measure_kind() else { + return; + }; + let n = self.particles.len(); + let mut parent: Vec = (0..n as u32).collect(); + let mut supported = vec![false; n]; + fn find(parent: &mut [u32], mut i: u32) -> u32 { + while parent[i as usize] != i { + let up = parent[parent[i as usize] as usize]; + parent[i as usize] = up; + i = up; } + i } - if use_cells { - // A surface element follows the cell that owns it (the one holding all its - // vertices): the fan of a shared vertex alone would let a face bridge two pieces. - for si in 0..self.boundary.len() { - let vertices = self.boundary[si]; - if !vertices.contains(&v) { + self.for_each_measure_element(kind, |_, vertices| { + for &w in vertices { + supported[w as usize] = true; + } + for w in vertices.windows(2) { + let (a, b) = (find(&mut parent, w[0]), find(&mut parent, w[1])); + if a != b { + parent[a.max(b) as usize] = a.min(b); + } } - let owner = verts + }); + let roots: Vec = (0..n as u32).map(|i| find(&mut parent, i)).collect(); + let spans = |vertices: &[u32]| { + vertices.iter().all(|&w| supported[w as usize]) + && vertices .iter() - .enumerate() - .find(|(_, fv)| vertices.iter().all(|x| fv.contains(x))); - let g = match owner { - Some((i, _)) => group[i], - None => group_of_element(&vertices, self), - }; - for x in &mut self.boundary[si] { - if *x == v { - *x = copies[g]; - } + .any(|&w| roots[w as usize] != roots[vertices[0] as usize]) + }; + + let removed: Vec = self + .edges + .iter() + .map(|e| { + !(kind == MeasureKind::Segments && e.kind == SoftBodyEdgeKind::Structural) + && spans(&e.vertices) + }) + .collect(); + if removed.contains(&true) { + for (e, _) in self.edges.iter().zip(&removed).filter(|(_, r)| **r) { + log.removed_edges.push(e.vertices); + } + let mut keep = removed.iter().map(|r| !r); + self.edges.retain(|_| keep.next().unwrap()); + } + #[cfg(feature = "dim3")] + self.dihedrals.retain(|d| !spans(&d.vertices)); + if kind != MeasureKind::Triangles { + self.boundary.retain(|s| !spans(s)); + } } } + +/// The smallest piece, in triangles, a tear may split off a 3D cloth or shell. +pub(super) const MIN_PIECE_TRIANGLES: usize = 10; +/// The smallest piece, in cells, a tear may split off: more than a grid quad (2D) or a lattice +/// cube (3D). +pub(super) const MIN_PIECE_CELLS: usize = if DIM == 2 { 3 } else { 6 }; +/// The smallest piece, in structural edges, a tear may split off a rope, polyline or wire. +pub(super) const MIN_PIECE_SEGMENTS: usize = 3; + +/// The smallest piece, in measure elements of the given kind, a tear may split off by default +/// (see [`SoftBodyMaterial::min_piece`]). +pub(super) fn default_min_piece(kind: MeasureKind) -> usize { + match kind { + MeasureKind::Cells => MIN_PIECE_CELLS, + MeasureKind::Triangles => MIN_PIECE_TRIANGLES, + MeasureKind::Segments => MIN_PIECE_SEGMENTS, + } } diff --git a/src/dynamics/soft_body/tearing/tearing_tables.rs b/src/dynamics/soft_body/tearing/tearing_tables.rs index 41e2edb49..7011ee33e 100644 --- a/src/dynamics/soft_body/tearing/tearing_tables.rs +++ b/src/dynamics/soft_body/tearing/tearing_tables.rs @@ -11,6 +11,7 @@ impl SoftBody { self.boundary_element_cells = surface_element_cells(&self.boundary, &self.cells); self.update_surface_flags(); self.rebuild_mesh_tables(); + self.rebuild_volume_pieces(&[]); } /// Recomputes the parallel colors of every element. diff --git a/src/dynamics/soft_body/tearing/tearing_validate.rs b/src/dynamics/soft_body/tearing/tearing_validate.rs index 26100add6..9f94fa2b4 100644 --- a/src/dynamics/soft_body/tearing/tearing_validate.rs +++ b/src/dynamics/soft_body/tearing/tearing_validate.rs @@ -9,6 +9,7 @@ use parry::utils::hashmap::HashMap; #[cfg(feature = "dim3")] use super::tearing::pair; use super::super::soft_body::SOFT_BODY_OVERFLOW_COLOR; +use super::super::soft_body_builder::element_vertices; use super::super::SoftBody; impl SoftBody { @@ -43,11 +44,38 @@ impl SoftBody { } // Every particle belongs to some measure element (a tear never removes one), unless the // body has none. + if let Some(kind) = self.measure_kind() { + let mut supported = vec![false; n as usize]; + self.for_each_measure_element(kind, |_, vertices| { + for &v in vertices { + if let Some(s) = supported.get_mut(v as usize) { + *s = true; + } + } + }); + if let Some(v) = supported.iter().position(|s| !s) { + return Err(format!("particle {v} belongs to no measure element")); + } } for (mi, mesh) in self.meshes().enumerate() { let nv = mesh.vertex_count(); + if let super::collision_mesh::SoftMeshMapping::Skinned { bindings } = mesh.binding() { + if bindings.len() != nv { + return Err(format!("mesh {mi} has {} bindings for {nv} vertices", bindings.len())); + } + // A body left without any cell has nothing to bind to. + let dead = |b: &super::collision_mesh::SoftMeshCellBinding| { + !self.cells.is_empty() && b.cell as usize >= self.cells.len() + }; + if let Some(v) = bindings.iter().position(dead) { + return Err(format!("mesh {mi} vertex {v} is bound to a dead cell")); + } + } for (i, s) in mesh.indices().iter().enumerate() { - for &v in s { + for &v in element_vertices(s) { + if v as usize >= nv { + return Err(format!("mesh {mi} element {i} has an out-of-range vertex")); + } } } if mesh.ring_offsets.len() != nv + 1 { @@ -59,6 +87,7 @@ impl SoftBody { )); } for (i, s) in mesh.indices().iter().enumerate() { + let s = element_vertices(s); for &v in s { let (start, end) = ( mesh.vertex_elements_offsets[v as usize] as usize, @@ -98,6 +127,14 @@ impl SoftBody { return Err(format!("mesh {mi} edge tables do not cover its elements")); } for (i, s) in mesh.indices().iter().enumerate() { + // A wire segment (padded with `u32::MAX`) is its own single edge. + if element_vertices(s).len() < DIM { + let id = mesh.element_edges[i][0] as usize; + if mesh.edges.get(id) != Some(&pair(s[0], s[1])) { + return Err(format!("mesh {mi} segment {i} edge mismatch")); + } + continue; + } for k in 0..DIM { let id = mesh.element_edges[i][k] as usize; let expected = pair(s[(k + 1) % DIM], s[(k + 2) % DIM]); diff --git a/src/dynamics/soft_body/tearing/tests.rs b/src/dynamics/soft_body/tearing/tests.rs new file mode 100644 index 000000000..283eae561 --- /dev/null +++ b/src/dynamics/soft_body/tearing/tests.rs @@ -0,0 +1,403 @@ +//! Unit tests of the particle split core on tiny meshes. + +use crate::alloc_prelude::*; +use crate::prelude::*; + +use super::tearing_particle_split::{MeasureKind, SplitLog}; + +/// Every structural edge of a rope, then every edge and cell of a grid (2D) or a box (3D), torn +/// over and over: the bodies come apart, but no tear splits off a piece smaller than the minimum +/// of its measure kind, and the mass and rest measure are kept. +#[test] +fn tears_never_split_off_a_chip() { + let rope = SoftBodyBuilder::rope(Vector::ZERO, Vector::X * 3.0, 13).particle_mass(0.1); + #[cfg(feature = "dim2")] + let block = SoftBodyBuilder::grid(Vector::ZERO, Vector::splat(1.0), 7, 7).particle_mass(0.1); + #[cfg(feature = "dim3")] + let block = + SoftBodyBuilder::cuboid(Vector::ZERO, Vector::splat(1.0), 4, 4, 4).particle_mass(0.1); + for (builder, kind) in [(rope, MeasureKind::Segments), (block, MeasureKind::Cells)] { + let (mut world, handle) = world_with(builder); + let (mass, measure) = { + let sb = &world.soft_bodies[handle]; + (sb.mass(), sb.rest_measure()) + }; + for _ in 0..4 { + for body in family(&world, handle) { + let sb = &world.soft_bodies[body]; + let edges: Vec = (0..sb.edges().len() as u32).collect(); + let cells: Vec = (0..sb.cells().len() as u32).collect(); + let _ = world.tear_soft_body(body, &edges, &cells); + } + } + + // Every piece is a body of its own now: the original and the ones split off it. + let bodies = family(&world, handle); + assert!(bodies.len() >= 2, "{kind:?}: nothing tore"); + let relative = |a: Real, b: Real| (a - b).abs() <= 1.0e-5 * a.abs().max(b.abs()); + let mut total_mass = 0.0; + let mut total_measure = 0.0; + for &body in &bodies { + let sb = &world.soft_bodies[body]; + sb.validate_topology().unwrap(); + assert_eq!(sb.connected_pieces().len(), 1); + assert_eq!(sb.origin(), (body != handle).then_some(handle)); + total_mass += sb.mass(); + total_measure += sb.rest_measure(); + let mut count = 0; + sb.for_each_measure_element(kind, |_, _| count += 1); + assert!( + count >= sb.min_piece(kind), + "{kind:?}: a tear split off a chip of {count} elements" + ); + } + assert!(relative(total_mass, mass), "{kind:?}: mass changed"); + assert!(relative(total_measure, measure), "{kind:?}: rest measure changed"); + } +} + +/// Checks the seeded components of a torn rope, of a cluster of it, and of a body made of +/// disconnected ropes: measure and mass add up, a never-connected cluster reports separate +/// components, and a seed pair reaches only the ropes it touches. +#[test] +fn seeded_components_follow_the_tear() { + let rope = SoftBodyBuilder::rope(Vector::ZERO, Vector::X * 8.0, 9).particle_mass(0.5); + let (mut world, handle) = world_with(rope); + let (total_measure, total_mass) = { + let sb = &world.soft_bodies[handle]; + (sb.rest_measure(), sb.mass()) + }; + let event = world.tear_soft_body(handle, &[4], &[]).expect("nothing tore"); + // The tear split the rope in two bodies of four segments; the event says which is which. + assert_eq!(event.seeds(), vec![[9, 4]]); + assert_eq!(event.pieces.len(), 2); + assert_eq!(event.pieces[0].soft_body, handle); + assert_eq!(event.pieces[0].particles, vec![0, 1, 2, 3, 4]); + assert_eq!(event.pieces[1].particles, vec![5, 6, 7, 8, 9]); + let mut measure = 0.0; + let mut mass = 0.0; + for piece in &event.pieces { + let sb = &world.soft_bodies[piece.soft_body]; + assert_eq!(sb.connected_pieces().len(), 1); + assert!(sb.seeded_components(None, &[[0, 1]]).is_empty()); + let all: Vec = (0..sb.num_particles() as u32).collect(); + assert!(close(sb.rest_measure_of(&all), sb.rest_measure())); + assert!(close(sb.mass_of(&all), sb.mass())); + measure += sb.rest_measure(); + mass += sb.mass(); + } + assert!(close(measure, total_measure)); + assert!(close(mass, total_mass)); + + let mut positions = Vec::new(); + let mut edges = Vec::new(); + for rope in 0..3u32 { + for i in 0..3u32 { + positions.push(Vector::X * (rope * 3 + i) as Real); + } + edges.push([rope * 3, rope * 3 + 1]); + edges.push([rope * 3 + 1, rope * 3 + 2]); + } + let ropes = SoftBodyBuilder::new(positions).edges(edges).no_surface_collider(); + let (world, handle) = world_with(ropes); + let sb = &world.soft_bodies[handle]; + // Restricted graphs: particles 0 and 2 share no element, 0, 1 and 2 chain through 1. + assert_eq!(sb.seeded_components(Some(&[0, 2]), &[[0, 2]]), vec![vec![0], vec![2]]); + assert!(sb.seeded_components(Some(&[0, 1, 2]), &[[0, 2]]).is_empty()); + assert_eq!( + sb.seeded_components(None, &[[2, 3]]), + vec![vec![0, 1, 2], vec![3, 4, 5]] + ); + assert!(sb.seeded_components(None, &[[7, 8]]).is_empty()); +} + +/// `SoftBodyMaterial::min_piece` sets the smallest piece a tear may split off: a rope of five +/// particles refuses to tear at its middle segment (a two-segment piece on each side), and tears +/// there once the material allows two-segment pieces. +#[test] +fn min_piece_sets_the_smallest_piece_a_tear_leaves() { + let rope = || SoftBodyBuilder::rope(Vector::ZERO, Vector::X * 4.0, 5).particle_mass(0.5); + let (mut world, handle) = world_with(rope()); + assert!(world.tear_soft_body(handle, &[2], &[]).is_none()); + let (mut world, handle) = world_with(rope().min_piece(2)); + let event = world.tear_soft_body(handle, &[2], &[]).expect("nothing tore"); + assert_eq!(event.pieces.len(), 2); + for body in event.bodies() { + let sb = &world.soft_bodies[body]; + sb.validate_topology().unwrap(); + assert_eq!(sb.min_piece(MeasureKind::Segments), 2); + } +} + +/// A soft body and every body split off it by tears, recursively (see `SoftBody::pieces`). +fn family(world: &PhysicsWorld, handle: SoftBodyHandle) -> Vec { + let mut bodies = vec![handle]; + let mut i = 0; + while i < bodies.len() { + bodies.extend(world.soft_bodies[bodies[i]].pieces().iter().copied()); + i += 1; + } + bodies +} + +/// A soft body inserted in a fresh world. +fn world_with(builder: SoftBodyBuilder) -> (PhysicsWorld, SoftBodyHandle) { + let mut world = PhysicsWorld::new(); + let handle = world.insert_soft_body(builder); + (world, handle) +} + +/// The particle pairs of the body's edges, each sorted, in sorted order. +fn edge_pairs(sb: &SoftBody) -> Vec<[u32; 2]> { + let mut pairs: Vec<[u32; 2]> = sb + .edges() + .iter() + .map(|e| { + [ + e.vertices[0].min(e.vertices[1]), + e.vertices[0].max(e.vertices[1]), + ] + }) + .collect(); + pairs.sort_unstable(); + pairs +} + +fn close(a: Real, b: Real) -> bool { + (a - b).abs() < 1.0e-5 +} + +/// A chain of five particles with unequal segments, split at its middle particle across the +/// segment towards the longer side: that segment moves to the copy, the mass is shared by length, +/// the bending edge over the particle is removed and the chain comes apart in two pieces. +#[test] +fn split_chain_at_its_middle_particle() { + let positions: Vec = [0.0, 1.0, 2.0, 4.0, 5.0] + .iter() + .map(|&x| Vector::X * x) + .collect(); + let segments = vec![[0, 1], [1, 2], [2, 3], [3, 4]]; + let builder = SoftBodyBuilder::new(positions) + .edges(segments.clone()) + .bend_edges(vec![[0, 2], [1, 3], [2, 4]]) + .particle_mass(0.9); + #[cfg(feature = "dim2")] + let builder = builder.surface(segments); + #[cfg(feature = "dim3")] + let builder = builder.wire(segments); + let (mut world, handle) = world_with(builder); + let sb = world.soft_bodies.get_mut(handle).unwrap(); + let measure = sb.rest_measure(); + + let origin = sb.particles[2].rest_position; + let normal = sb.particles[3].rest_position - origin; + let fan = sb.plane_fan(2, origin, normal).unwrap(); + assert_eq!(fan.elements, vec![1, 2]); + assert_eq!(fan.groups, vec![0, 1]); + let mut log = SplitLog::default(); + assert!(sb.split_fan(2, &fan, &mut log)); + sb.remove_straddlers_across_pieces(&mut log); + sb.finish_topology_change(&log); + + assert_eq!(log.split_particles, vec![(5, 2)]); + assert_eq!(log.removed_edges, vec![[1, 3]]); + assert!(close(sb.particles[2].mass, 0.3) && close(sb.particles[5].mass, 0.6)); + assert_eq!( + edge_pairs(sb), + vec![[0, 1], [0, 2], [1, 2], [3, 4], [3, 5], [4, 5]] + ); + assert_eq!(sb.connected_pieces(), vec![vec![0, 1, 2], vec![3, 4, 5]]); + assert!(close(sb.rest_measure(), measure)); + #[cfg(feature = "dim2")] + assert_eq!(sb.boundary(), &[[0, 1], [1, 2], [5, 3], [3, 4]]); + #[cfg(feature = "dim3")] + { + // The wire segment follows its edge to a new vertex bound to the copy. + let mesh = sb.meshes().next().unwrap(); + assert_eq!(mesh.vertex_count(), 6); + assert_eq!(mesh.vertex(sb, 5), sb.particles[5].position); + assert_eq!(mesh.indices()[2][..2], [5, 3]); + } + sb.validate_topology().unwrap(); +} + +/// A kite of two triangles split at a vertex of their shared diagonal: the diagonal opens (a +/// crack face per cell, oriented outward), the edge along it is duplicated and the mass is +/// shared by area. +#[cfg(feature = "dim2")] +#[test] +fn split_kite_across_its_diagonal() { + let positions = vec![ + Vector::new(0.0, 0.0), + Vector::new(0.0, 1.0), + Vector::new(2.0, 0.0), + Vector::new(1.0, 1.0), + ]; + let builder = SoftBodyBuilder::new(positions) + .cells(vec![[0, 2, 3], [0, 3, 1]]) + .particle_mass(0.9); + let (mut world, handle) = world_with(builder); + let sb = world.soft_bodies.get_mut(handle).unwrap(); + assert_eq!(sb.boundary(), &[[0, 2], [2, 3], [3, 1], [1, 0]]); + let measure = sb.rest_measure(); + + let origin = sb.particles[0].rest_position; + let fan = sb.plane_fan(0, origin, Vector::new(1.0, -1.0)).unwrap(); + assert_eq!(fan.elements, vec![0, 1]); + assert_eq!(fan.sides, vec![1, 0]); + assert_eq!(fan.groups, vec![1, 0]); + let mut log = SplitLog::default(); + assert!(sb.split_fan(0, &fan, &mut log)); + sb.remove_straddlers_across_pieces(&mut log); + sb.finish_topology_change(&log); + + assert_eq!(log.split_particles, vec![(4, 0)]); + assert!(log.removed_edges.is_empty()); + assert_eq!(sb.cells[0].vertices, [4, 2, 3]); + assert_eq!(sb.cells[1].vertices, [0, 3, 1]); + // The copy's triangle has twice the area of the original's. + assert!(close(sb.particles[0].mass, 0.3) && close(sb.particles[4].mass, 0.6)); + assert_eq!( + edge_pairs(sb), + vec![[0, 1], [0, 3], [1, 3], [2, 3], [2, 4], [3, 4]] + ); + assert_eq!( + sb.boundary(), + &[[4, 2], [2, 3], [3, 1], [1, 0], [3, 4], [0, 3]] + ); + let rest = |i: u32| sb.particles[i as usize].rest_position; + for (face, cell) in [([3, 4], 0), ([0, 3], 1)] { + let centroid: Vector = sb.cells[cell] + .vertices + .iter() + .map(|&v| rest(v)) + .sum::() + / 3.0; + let (a, b) = (rest(face[0]), rest(face[1])); + assert!( + (b - a).perp_dot(centroid - a) > 0.0, + "crack face {face:?} is not outward" + ); + } + assert_eq!(sb.connected_pieces().len(), 1); + assert!(close(sb.rest_measure(), measure)); + sb.validate_topology().unwrap(); +} + +/// Two tetrahedra sharing a face, split at a vertex of that face along its plane: the face opens +/// (a crack face per cell, oriented outward), the edges along it are duplicated and the mass is +/// shared by volume. +#[cfg(feature = "dim3")] +#[test] +fn split_tetrahedra_across_their_shared_face() { + let positions = vec![ + Vector::new(0.0, 0.0, 0.0), + Vector::new(1.0, 0.0, 0.0), + Vector::new(0.0, 1.0, 0.0), + Vector::new(0.25, 0.25, 1.0), + Vector::new(0.25, 0.25, -2.0), + ]; + let builder = SoftBodyBuilder::new(positions) + .cells(vec![[0, 1, 2, 3], [0, 1, 2, 4]]) + .particle_mass(0.9); + let (mut world, handle) = world_with(builder); + let sb = world.soft_bodies.get_mut(handle).unwrap(); + assert_eq!(sb.cells[1].vertices, [0, 2, 1, 4]); + assert_eq!(sb.boundary().len(), 6); + let measure = sb.rest_measure(); + + let origin = sb.particles[0].rest_position; + let fan = sb.plane_fan(0, origin, Vector::Z).unwrap(); + assert_eq!(fan.sides, vec![1, 0]); + assert_eq!(fan.groups, vec![1, 0]); + let mut log = SplitLog::default(); + assert!(sb.split_fan(0, &fan, &mut log)); + sb.remove_straddlers_across_pieces(&mut log); + sb.finish_topology_change(&log); + + assert_eq!(log.split_particles, vec![(5, 0)]); + assert!(log.removed_edges.is_empty()); + assert_eq!(sb.cells[0].vertices, [5, 1, 2, 3]); + assert_eq!(sb.cells[1].vertices, [0, 2, 1, 4]); + // The original keeps the lower tetrahedron, twice the volume of the upper one. + assert!(close(sb.particles[0].mass, 0.6) && close(sb.particles[5].mass, 0.3)); + assert_eq!( + edge_pairs(sb), + vec![ + [0, 1], + [0, 2], + [0, 4], + [1, 2], + [1, 3], + [1, 4], + [1, 5], + [2, 3], + [2, 4], + [2, 5], + [3, 5] + ] + ); + assert_eq!(sb.boundary().len(), 8); + let rest = |i: u32| sb.particles[i as usize].rest_position; + for (k, cell) in [(6, 0), (7, 1)] { + let face = sb.boundary()[k]; + let mut sorted = face; + sorted.sort_unstable(); + assert_eq!(sorted, if cell == 0 { [1, 2, 5] } else { [0, 1, 2] }); + let centroid: Vector = sb.cells[cell] + .vertices + .iter() + .map(|&v| rest(v)) + .sum::() + / 4.0; + let (a, b, c) = (rest(face[0]), rest(face[1]), rest(face[2])); + assert!( + (b - a).cross(c - a).dot(centroid - a) < 0.0, + "crack face {face:?} is not outward" + ); + } + assert_eq!(sb.connected_pieces().len(), 1); + assert!(close(sb.rest_measure(), measure)); + sb.validate_topology().unwrap(); +} + +/// A cloth quad split at a corner of its diagonal: the diagonal opens between the two triangles, +/// the spring along it is duplicated, the other diagonal (now across an opened side) is removed +/// and the mass is shared by area. The no-confetti rule refuses the same split. +#[cfg(feature = "dim3")] +#[test] +fn split_cloth_quad_across_its_diagonal() { + let builder = + SoftBodyBuilder::cloth(Vector::ZERO, Vector::X, Vector::Z, 2, 2).particle_mass(0.8); + let (mut world, handle) = world_with(builder); + let sb = world.soft_bodies.get_mut(handle).unwrap(); + assert_eq!(sb.boundary(), &[[0, 2, 3], [0, 3, 1]]); + assert_eq!( + edge_pairs(sb), + vec![[0, 1], [0, 2], [0, 3], [1, 2], [1, 3], [2, 3]] + ); + let measure = sb.rest_measure(); + + let origin = sb.particles[0].rest_position; + let normal = Vector::new(1.0, 0.0, -1.0); + let fan = sb.plane_fan(0, origin, normal).unwrap(); + assert!(!sb.opens_without_confetti(0, &fan)); + assert_eq!(fan.sides, vec![1, 0]); + assert_eq!(fan.groups, vec![1, 0]); + let mut log = SplitLog::default(); + assert!(sb.split_fan(0, &fan, &mut log)); + sb.remove_straddlers_across_pieces(&mut log); + sb.finish_topology_change(&log); + + assert_eq!(log.split_particles, vec![(4, 0)]); + assert_eq!(log.removed_edges, vec![[2, 1]]); + assert_eq!(sb.boundary(), &[[4, 2, 3], [0, 3, 1]]); + assert!(close(sb.particles[0].mass, 0.4) && close(sb.particles[4].mass, 0.4)); + assert_eq!( + edge_pairs(sb), + vec![[0, 1], [0, 3], [1, 3], [2, 3], [2, 4], [3, 4]] + ); + assert_eq!(sb.connected_pieces().len(), 1); + assert!(close(sb.rest_measure(), measure)); + sb.validate_topology().unwrap(); +} diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs index 3fa989f5a..a14ec2863 100644 --- a/src/dynamics/solver/mod.rs +++ b/src/dynamics/solver/mod.rs @@ -12,6 +12,7 @@ mod interaction_groups; mod joint_constraint; pub(crate) mod manifold_store; mod soft_constraint; +pub(crate) use soft_constraint::soft_element_constraint::symmetric_eigen; #[cfg(feature = "fem")] pub(crate) mod soft_fem; mod solver_body; diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs index e1fcc3af3..e034ab92e 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs @@ -197,16 +197,23 @@ impl SoftConstraintsSet { if !update { return; } - if let Some(vi) = awake.volume_constraint { + for vi in awake.volume_constraints.clone() { let vc = &mut volume_constraints[vi]; + let piece = &sb.volume_pieces[vc.piece as usize]; let grads = &mut volume_grads[vc.grads.clone()]; - grads.fill(Vector::ZERO); + for &i in &piece.particles { + grads[i as usize] = Vector::ZERO; + } let position = |i: u32| pos(i as usize); - let volume = SoftBody::boundary_volume(&sb.boundary, position); - SoftBody::boundary_volume_gradients(&sb.boundary, position, grads); + let elements = || piece.boundary_elements(&sb.boundary); + let volume = SoftBody::elements_volume(elements(), position); + SoftBody::boundary_volume_gradients(elements(), position, grads); let mut w = 0.0; - for (i, p) in sb.particles.iter().enumerate() { - w += p.inv_mass * grads[i].length_squared(); + for &i in &piece.particles { + let i = i as usize; + if slots[i] != u32::MAX { + w += sb.particles[i].inv_mass * grads[i].length_squared(); + } } // A FEM body answers the constraint through its augmented mass: the response to the // fresh gradients, and the augmented gain in place of the lumped one. @@ -216,12 +223,13 @@ impl SoftConstraintsSet { let system = unsafe { &mut *fem.system }; let n = sb.particles.len(); let out = &mut fem_responses[side.start as usize..side.start as usize + n]; - let entries = sb + let entries = piece .particles .iter() - .enumerate() - .filter(|(i, p)| slots[*i] != u32::MAX && p.inv_mass > 0.0) - .map(|(i, _)| (i as u32, grads[i])); + .filter(|&&i| { + slots[i as usize] != u32::MAX && sb.particles[i as usize].inv_mass > 0.0 + }) + .map(|&i| (i, grads[i as usize])); side.gain = system.response_into(entries, out, &fem.params); side.u_max = out.iter().map(|u| u.length()).fold(0.0, Real::max); w = if out.iter().any(|u| u.length() > super::soft_fem_amplification_cap() * side.gain) { diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs index 2b3aba654..94ddcf51f 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs @@ -24,6 +24,7 @@ impl SoftConstraintsSet { let sb = unsafe { &*awake.ptr }; let slots = &self.slots[awake.slot_start..awake.slot_start + awake.num_particles]; let grads = &self.volume_grads[vc.grads.clone()]; + let piece = &sb.volume_pieces[vc.piece as usize]; let fem = vc.fem.map(|side| { let n = sb.particles.len(); ( @@ -40,8 +41,11 @@ impl SoftConstraintsSet { match &fem { Some((side, _, _)) => g_max = side.u_max, None => { - for (i, p) in sb.particles.iter().enumerate() { - g_max = g_max.max(grads[i].length() * p.inv_mass); + for &i in &piece.particles { + let i = i as usize; + if slots[i] != u32::MAX { + g_max = g_max.max(grads[i].length() * sb.particles[i].inv_mass); + } } } } @@ -59,8 +63,13 @@ impl SoftConstraintsSet { } } None => { - for (i, p) in sb.particles.iter().enumerate() { - bodies.vels[s as usize].linear -= grads[i] * (p.inv_mass * impulse); + for &i in &piece.particles { + let i = i as usize; + let s = slots[i]; + if s != u32::MAX && (s as usize) < bodies.len() { + bodies.vels[s as usize].linear -= + grads[i] * (sb.particles[i].inv_mass * impulse); + } } } }; @@ -71,7 +80,8 @@ impl SoftConstraintsSet { } let mut dc = 0.0; - for i in 0..sb.particles.len() { + for &i in &piece.particles { + let i = i as usize; let s = slots[i]; if s != u32::MAX { dc += grads[i].gdot(bodies.get_vel(s).linear); 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 949d18386..529ff46ad 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 @@ -154,8 +154,9 @@ impl SoftConstraintsSet { } } } - if let Some(vi) = awake.volume_constraint { - sb.volume_impulse = crate::utils::canonicalize_zero(self.volume_constraints[vi].impulse); + for vc in &self.volume_constraints[awake.volume_constraints.clone()] { + sb.volume_pieces[vc.piece as usize].impulse = + crate::utils::canonicalize_zero(vc.impulse); } sb.plastic_flowing = awake.plastic_flow.load(Ordering::Relaxed); sb.tearing_pending |= awake.torn.load(Ordering::Relaxed); diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs index b16f0dee8..77d811839 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs @@ -110,13 +110,16 @@ impl SoftShapeConstraint { } } -/// The global area/volume preservation row of a soft body (touches every particle: solved -/// serially). +/// The area/volume preservation constraint of one volume piece of a soft body (touches every +/// particle of the piece: solved serially). #[derive(Clone, Debug)] pub(crate) struct SoftVolumeConstraint { pub soft_body: u32, + /// The constrained piece in the body's `volume_pieces`. + pub piece: u32, pub target: Real, - /// Range of this row's particle gradients in `volume_grads`. + /// Range of the body's particle gradients in `volume_grads`, shared by its pieces (their + /// particles are disjoint). pub grads: Range, pub erp_inv_dt: Real, pub cfm_coeff: Real, 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 196fc6a91..091123afd 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 @@ -37,7 +37,7 @@ pub(crate) struct AwakeSoftBody { /// Range of this body's constraints in `shape_constraints`. pub shape_constraints: Range, /// Range of this body's volume constraints in `volume_constraints` (one per volume piece). - pub volume_constraint: Option, + pub volume_constraints: Range, /// Fraction of the non-rigid velocity removed per substep (`deformation_damping`). pub damping_factor: Real, /// Set by the writeback stage when a cell flowed plastically by a significant amount diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs index cff1faafa..86e9f082b 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs @@ -202,23 +202,28 @@ impl SoftConstraintsSet { awake.damping_factor = 1.0 - (-material.deformation_damping * dt).exp(); damping = true; } - if sb.volume_preservation && !sb.boundary.is_empty() { + if sb.volume_preservation && !sb.volume_pieces.is_empty() { let grads_start = self.volume_grads.len(); self.volume_grads .resize(grads_start + sb.particles.len(), Vector::ZERO); - awake.volume_constraint = Some(self.volume_constraints.len()); - self.volume_constraints.push(SoftVolumeConstraint { - soft_body: ai as u32, - target: sb.rest_volume * sb.volume_factor, - grads: grads_start..grads_start + sb.particles.len(), - erp_inv_dt: material.volume_softness.erp_inv_dt(dt), - cfm_coeff: material.volume_softness.cfm_coeff(dt), - max_bias_velocity: max_corrective_velocity, - inv_lhs: 0.0, - cfm_gain: 0.0, - rhs: 0.0, - fem: None, - }); + let first = self.volume_constraints.len(); + for (pi, piece) in sb.volume_pieces.iter().enumerate() { + self.volume_constraints.push(SoftVolumeConstraint { + soft_body: ai as u32, + piece: pi as u32, + target: piece.rest_volume * sb.volume_factor, + grads: grads_start..grads_start + sb.particles.len(), + erp_inv_dt: material.volume_softness.erp_inv_dt(dt), + cfm_coeff: material.volume_softness.cfm_coeff(dt), + max_bias_velocity: max_corrective_velocity, + inv_lhs: 0.0, + cfm_gain: 0.0, + rhs: 0.0, + impulse: piece.impulse, + fem: None, + }); + } + awake.volume_constraints = first..self.volume_constraints.len(); } } let colors_start = self.color_ranges.len(); @@ -429,6 +434,10 @@ impl SoftConstraintsSet { let Some(sb) = soft_bodies.get_mut(handle) else { continue; }; + // A body deserialized from a snapshot predating the volume pieces has none yet. + if sb.volume_preservation && sb.volume_pieces.is_empty() { + sb.rebuild_volume_pieces(&[]); + } let awake_clusters = sb @@ -473,7 +482,7 @@ impl SoftConstraintsSet { num_particles: sb.particles.len(), shape_com: Vector::ZERO, shape_constraints: 0..0, - volume_constraint: None, + volume_constraints: 0..0, damping_factor: 0.0, plastic_flow: AtomicBool::new(false), torn: AtomicBool::new(false), diff --git a/src/geometry/narrow_phase/soft_contacts/mod.rs b/src/geometry/narrow_phase/soft_contacts/mod.rs index f14b0d4e7..83ca08f72 100644 --- a/src/geometry/narrow_phase/soft_contacts/mod.rs +++ b/src/geometry/narrow_phase/soft_contacts/mod.rs @@ -18,7 +18,9 @@ pub use soft_contacts_types::{ SoftContactImpulse, SoftEdgeCandidate, SoftEdgePass, SoftPairContacts, SoftRigidPatch, SoftVertexCandidate, SoftVertexHits, SoftVertexPass, SoftVolumePatch, }; -pub(crate) use soft_contacts_types::{SelfTangles, SoftDetectionCtx, SoftRigidContacts, body_frozen}; +pub(crate) use soft_contacts_types::{ + SelfTangles, SoftDetectionCtx, SoftRigidContacts, body_frozen, rest_gap_skins, +}; use soft_contacts_types::Side; pub(crate) use soft_contacts_vertex_pass::{detect_vertex_pass, element_plane, elements_cross}; pub use soft_contacts_volume::VolumeBin; diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs index 14c6c7ea2..3cb3a3c5c 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs @@ -13,7 +13,10 @@ use simba::scalar::{ComplexField as _, RealField as _}; #[cfg(feature = "parallel")] use super::soft_contacts_vertex_pass::PARALLEL_CHUNK; use super::soft_contacts_vertex_pass::element_points; -use super::{SelfTangles, Side, SoftDetectionCtx, SoftEdgeCandidate, SoftEdgePass, elements_cross}; +use super::{ + SelfTangles, Side, SoftDetectionCtx, SoftEdgeCandidate, SoftEdgePass, elements_cross, + rest_gap_skins, +}; /// The edge-vs-edge candidates between the surfaces of `own` and `other` (itself for a self pass, /// with `tangles` set); complements the vertex-vs-surface constraints, keeping only contacts @@ -23,6 +26,7 @@ pub(crate) fn detect_edges( (sb, mesh, surface_handle, surface_co): Side<'_>, (other, other_mesh, other_surface_handle, other_co): Side<'_>, tangles: Option>, + rest_gaps: bool, ctx: &SoftDetectionCtx, ) -> bool { let params = ctx.params; @@ -239,18 +243,50 @@ impl EdgeScan<'_> { continue; } let (loc_a, loc_b) = - // Endpoint contacts are vertex contacts, carried by the vertex rows. + parry::query::details::closest_points_segment_segment_with_locations_nD( + (&pa[0], &pa[1]), + (&pb[0], &pb[1]), + ); + // Endpoint contacts are vertex contacts, handled by the vertex constraints. + let ( + parry::shape::SegmentPointLocation::OnEdge(ba), + parry::shape::SegmentPointLocation::OnEdge(bb), ) = (loc_a, loc_b) else { continue; + }; let point_a = pa[0] * ba[0] + pa[1] * ba[1]; let point_b = pb[0] * bb[0] + pb[1] * bb[1]; let sep = point_b - point_a; let len = sep.length(); if len >= self.reach || len < 1.0e-6 { - let dist = len - skins; continue; } + // Two pieces of one torn body keep the gap the edges had at rest. + let pair_skins = if self.rest_gaps { + let ra = [ + self.mesh.rest_vertex(self.sb, va[0] as usize), + self.mesh.rest_vertex(self.sb, va[1] as usize), + ]; + let rb = [ + self.other_mesh.rest_vertex(self.other, vb[0] as usize), + self.other_mesh.rest_vertex(self.other, vb[1] as usize), + ]; + let (la, lb) = + parry::query::details::closest_points_segment_segment_with_locations_nD( + (&ra[0], &ra[1]), + (&rb[0], &rb[1]), + ); + let (ca, cb) = (la.barycentric_coordinates(), lb.barycentric_coordinates()); + let rest_gap = (ra[0] * ca[0] + ra[1] * ca[1] + - rb[0] * cb[0] + - rb[1] * cb[1]) + .length(); + rest_gap_skins(self.skins, rest_gap) + } else { + self.skins + }; + let dist = len - pair_skins; // Beyond the prediction distance, only a contact closing fast enough // to happen within the whole step is kept (the vertex pass's rule): // the rest of the self.reach is the bodies' motion margin. diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs index 8975db3b2..e87b5701b 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs @@ -222,6 +222,8 @@ fn detect_soft_pair( if frozen1 && frozen2 { return; } + // Two pieces of one torn body keep the gaps their features had at rest. + let pieces = ctx.pieces_of_one_body((sb1, r1.body), (sb2, r2.body)); // Both vertex passes: each surface's owner consumes the other's vertices against it (and // the reverse pass too when the other body is not simulated). let mut pass1 = SoftVertexPass::default(); @@ -231,6 +233,7 @@ fn detect_soft_pair( (sb1, mesh1, h1, co1), (sb2, mesh2, h2, co2), None, + pieces, ctx, ); detect_vertex_pass( @@ -238,6 +241,7 @@ fn detect_soft_pair( (sb2, mesh2, h2, co2), (sb1, mesh1, h1, co1), None, + pieces, ctx, ); // The edge pass, oriented from its owner: a crossed closed-closed pair whose volume constraint @@ -257,7 +261,7 @@ fn detect_soft_pair( && recovery.overlap_edge_stand_down && !owner_pass.cross_tangled_elements.is_empty(); let mut edges = SoftEdgePass::default(); - let has_edges = !stood_down && detect_edges(&mut edges, own, other, None, ctx); + let has_edges = !stood_down && detect_edges(&mut edges, own, other, None, pieces, ctx); // The volume patches, from the vertex pass of the pair's lower surface. let (lower_pass, lower, higher) = if h1.into_raw_parts() < h2.into_raw_parts() { (&pass1, (sb1, mesh1, h1, co1), (sb2, mesh2, h2, co2)) 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 7c2f7f505..e24c9152e 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs @@ -4,7 +4,8 @@ use crate::alloc_prelude::*; use core::ops::Range; use crate::dynamics::{ - IntegrationParameters, RigidBodySet, SoftBody, SoftBodySet, SoftCollisionMesh, + IntegrationParameters, RigidBodySet, SoftBody, SoftBodyHandle, SoftBodySet, + SoftCollisionMesh, }; use crate::geometry::{Collider, ColliderHandle, ColliderSet}; use crate::math::{DIM, Real, Rotation, Vector}; @@ -25,6 +26,19 @@ pub(crate) struct SoftDetectionCtx<'a> { } impl SoftDetectionCtx<'_> { + /// Whether two soft bodies are pieces of one torn body (one is the other's origin, or both + /// share one): their rest shapes live in the same frame, and the contact skin between two of + /// their features is capped by the gap those features had at rest (see `rest_gap_skins`). + pub fn pieces_of_one_body( + &self, + (sb1, h1): (&SoftBody, SoftBodyHandle), + (sb2, h2): (&SoftBody, SoftBodyHandle), + ) -> bool { + sb1.origin() == Some(h2) + || sb2.origin() == Some(h1) + || (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). pub fn motion_margin(&self, co: &Collider) -> Real { @@ -407,3 +421,10 @@ pub(super) type Side<'a> = ( ColliderHandle, &'a Collider, ); + +/// The contact skins between two features of two pieces of one torn body: the full `skins`, capped +/// by the features' rest-shape distance. Crack faces rest at distance zero (a crack snapped to the +/// cells' facets interlocks the sides), so features keep their rest gap and collide from there. +pub(crate) fn rest_gap_skins(skins: Real, rest_gap: Real) -> Real { + skins.min(rest_gap.max(0.0)) +} diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs index 0f0d1c29f..02ca033c3 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs @@ -9,13 +9,17 @@ use crate::math::{DIM, Real, Vector}; use crate::geometry::PointQueryWithLocation; use crate::utils::DotProduct; use parry::bounding_volume::BoundingVolume; +use parry::query::PointQuery; #[cfg(not(feature = "std"))] #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; use super::soft_contacts_classify::{classify_inside, classify_inside_self, project_on_element, ring_depths}; use super::soft_contacts_volume::{VolumeSide, fill_depths, patch_vertices, volume_bins, volume_split}; -use super::{SelfTangles, Side, SoftDetectionCtx, SoftVertexPass, SoftVertexCandidate, SoftVertexHits}; +use super::{ + SelfTangles, Side, SoftDetectionCtx, SoftVertexCandidate, SoftVertexHits, SoftVertexPass, + rest_gap_skins, +}; /// Whether two elements' boundaries cross transversally, whatever their arities (a /// segment through a triangle covers a wire through a surface; two wires cross with @@ -90,6 +94,7 @@ pub(crate) fn detect_vertex_pass( (eb, eb_mesh, surface_handle, eb_co): Side<'_>, (vb, vb_mesh, vertices_handle, vb_co): Side<'_>, tangles: Option>, + rest_gaps: bool, ctx: &SoftDetectionCtx, ) { let params = ctx.params; @@ -534,10 +539,69 @@ impl VertexScan<'_> { }; let sep = vertex_pos - proj.point; let len = sep.length(); - if len >= reach || len < 1.0e-6 { + if len >= self.reach { + continue; + } + // A vertex on the surface itself has no direction of its own. Between two pieces of + // one torn body it is a crack vertex, held along the normal of the face it was split + // from when its own surface faces the other way (the crack's sides face each other). + let coincident = len < 1.0e-6; + if coincident { + if !self.rest_gaps || !self.closed { + continue; + } + let Some(outward) = self.eb_mesh + .element_outward_normal(self.eb, e as usize) + .and_then(|n| n.try_normalize()) + else { + continue; + }; + let Some(own) = self.vb_mesh.vertex_outward_normal(self.vb, v as u32) else { + continue; + }; + if own.gdot(outward) > -0.5 { + continue; + } + scratch.push(SoftVertexCandidate { + element: e, + weights, + dir: -outward, + dist: 0.0, + outward: Some(outward), + interior: false, + enabled: true, + impulse: 0.0, + tangent_impulse: Vector::ZERO, + }); continue; } - let dist = len - skins; + let pair_skins = if self.rest_gaps { + let rest_vertex = self.vb_mesh.rest_vertex(self.vb, v); + let rest: [Vector; DIM] = core::array::from_fn(|k| { + element + .get(k) + .map_or(Vector::ZERO, |v| self.eb_mesh.rest_vertex(self.eb, *v as usize)) + }); + let rest_gap = if element.len() < DIM { + parry::shape::Segment::new(rest[0], rest[1]) + .distance_to_local_point(rest_vertex, true) + } else { + #[cfg(feature = "dim2")] + { + parry::shape::Segment::new(rest[0], rest[1]) + .distance_to_local_point(rest_vertex, true) + } + #[cfg(feature = "dim3")] + { + parry::shape::Triangle::new(rest[0], rest[1], rest[2]) + .distance_to_local_point(rest_vertex, true) + } + }; + rest_gap_skins(self.skins, rest_gap) + } else { + self.skins + }; + let dist = len - pair_skins; // Beyond the prediction distance, only a contact closing fast enough to happen within // the whole step is kept (`self.params.dt` is the substep): the rest of the reach is // the bodies' motion margin, and a resting vertex must not keep constraints in it. diff --git a/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs b/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs index 3222730c5..69e1171ab 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs @@ -373,10 +373,10 @@ pub(super) fn update_self( detect_self_tangles(sb, mesh, co, run_crossing_sweep, out); let side = (sb, mesh, handle, co); let mut vertex_pass = core::mem::take(&mut out.vertex_pass); - detect_vertex_pass(&mut vertex_pass, side, side, Some(out.tangles()), ctx); + detect_vertex_pass(&mut vertex_pass, side, side, Some(out.tangles()), false, ctx); out.vertex_pass = vertex_pass; let mut edges = out.edges.take().unwrap_or_default(); - let has_edges = detect_edges(&mut edges, side, side, Some(out.tangles()), ctx); + let has_edges = detect_edges(&mut edges, side, side, Some(out.tangles()), false, ctx); out.edges = has_edges.then_some(edges); detect_self_regions(out, sb, mesh, co, ctx); } From e49538b07a8b0e2ae3302df36ab654f8e060ee34 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 17 Sep 2026 09:53:12 +0200 Subject: [PATCH 18/32] feat(soft-body): never split a pinned particle; the testbed draws and grabs the torn-off pieces --- crates/rapier2d/tests/soft_bodies.rs | 24 +++++- examples2d/soft_force_tearing2.rs | 18 +++++ examples2d/soft_tearing2.rs | 41 +++++++--- examples3d/soft_tearing3.rs | 41 +++++++--- src/dynamics/soft_body/tearing/tearing_cut.rs | 2 + .../tearing/tearing_particle_split.rs | 4 + src/dynamics/soft_body/tearing/tests.rs | 43 ++++++++++ src_testbed/grab.rs | 47 ++++++++--- src_testbed/graphics/graphics_soft_bodies.rs | 81 ++++++++++++++++++- src_testbed/viewer.rs | 7 ++ 10 files changed, 274 insertions(+), 34 deletions(-) diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index d1bedf86f..999326be4 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -1976,6 +1976,7 @@ fn stiff_edge_tears_under_force_not_strain() { let tested = sb.edges().iter().find(|e| e.vertices == [3, 4]); let particles: usize = bodies.iter().map(|&h| world.soft_bodies[h].num_particles()).sum(); ( + particles, bodies.len(), tested.map(|e| e.stress()), sb.edges().len(), @@ -1989,8 +1990,16 @@ fn stiff_edge_tears_under_force_not_strain() { (stress - 9.81 / 30.0).abs() < 0.08, "stress {stress} does not read the weight over the threshold" ); + // The tear keeps every edge and never splits the pinned end: the weight's top particle + // splits, its copy stays on the bar as a stub and the weight falls as its own body. Edge + // `[3, 4]` now bears the stub alone (three quarters of the split particle's mass). let (particles, pieces, stress, edges) = hang(6.0); assert_eq!((particles, pieces, edges), (9, 2, 4), "the edge held a load above its tear force"); + let stress = stress.unwrap(); + assert!( + (stress - 0.1875 * 9.81 / 6.0).abs() < 0.03, + "the tested edge does not bear the stub alone: {stress}" + ); } /// Tear smoothing: a jerk that would snap an edge at once is shrugged off once the load is @@ -2203,11 +2212,24 @@ fn edge_plasticity_keeps_a_dent() { let sb = &world.soft_bodies[handle]; let e = &sb.edges()[0]; assert!((e.rest_length - 0.7 / 0.9).abs() < 1.0e-4, "a stretch flowed: {}", e.rest_length); - // 30% past the initial length: past the tear strain. + let stress = e.stress(); + assert!( + (stress - 0.2 / 0.25).abs() < 0.02, + "the tear load reads the flowed rest length instead of the initial one: {stress}" + ); + // 30% past the initial length: past the tear strain (the edge would tear, were an end free). world.soft_bodies[handle].set_particle_position(1, Vector::new(1.3, 0.0)); for _ in 0..30 { world.step(); } + let sb = &world.soft_bodies[handle]; + sb.validate_topology().unwrap(); + assert_eq!(sb.num_particles(), 10, "an edge between two pinned particles tore"); + let stress = sb.edges()[0].stress(); + assert!( + (stress - 0.3 / 0.25).abs() < 0.02, + "the overstretched edge does not read past its threshold: {stress}" + ); } /// Tears are reported to the event handler and keep every segment: a seven-particle rope whose diff --git a/examples2d/soft_force_tearing2.rs b/examples2d/soft_force_tearing2.rs index ffbff2229..498216c6a 100644 --- a/examples2d/soft_force_tearing2.rs +++ b/examples2d/soft_force_tearing2.rs @@ -156,6 +156,11 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { DebugRenderMode::SOFT_BODIES | DebugRenderMode::SOFT_BODY_STRESS, ); + // The tears of every step, to follow the driven particles through them. + let (collision_send, _collision_recv) = std::sync::mpsc::channel(); + let (force_send, _force_recv) = std::sync::mpsc::channel(); + let (tear_send, tear_recv) = std::sync::mpsc::channel(); + let events = ChannelEventCollector::new(collision_send, force_send, tear_send); let mut t: Real = 0.0; while viewer.render_frame(&mut world).await { if viewer.simulating() { @@ -186,3 +191,16 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { Ok(()) } +/// Where a particle driven by index is after the tears of a step: a tear compacts the torn +/// body's particles and splits the disconnected pieces off as soft bodies of their own, so a +/// particle's body and index follow the events. +fn follow_tears(events: &[SoftBodyTearEvent], particle: &mut (SoftBodyHandle, u32)) { + for event in events { + if event.soft_body == particle.0 { + if let Some(destination) = event.particle_destination(particle.1) { + *particle = destination; + } + } + } +} + diff --git a/examples2d/soft_tearing2.rs b/examples2d/soft_tearing2.rs index 33ce2b299..40c81f4ec 100644 --- a/examples2d/soft_tearing2.rs +++ b/examples2d/soft_tearing2.rs @@ -80,25 +80,30 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); let bar = world.insert_soft_body(bar); let sb = &world.soft_bodies[bar]; - let ends: Vec<(usize, Vector, bool)> = (0..sb.num_particles()) + let mut ends: Vec<((SoftBodyHandle, u32), Vector, bool)> = (0..sb.num_particles()) .filter_map(|i| { let p = sb.particle_position(i); if p.x < -4.99 { - Some((i, p, false)) + Some(((bar, i as u32), p, false)) } else if p.x > -1.01 { - Some((i, p, true)) + Some(((bar, i as u32), p, true)) } else { None } }) .collect(); - for &(i, _, _) in &ends { - world.soft_bodies[bar].set_particle_pinned(i, true); + for &((_, i), _, _) in &ends { + world.soft_bodies[bar].set_particle_pinned(i as usize, true); } viewer.set_world(&mut world); viewer.look_at(Vec2::new(0.0, 4.0), 40.0); + // The tears of every step, to follow the driven particles through them. + let (collision_send, _collision_recv) = std::sync::mpsc::channel(); + let (force_send, _force_recv) = std::sync::mpsc::channel(); + let (tear_send, tear_recv) = std::sync::mpsc::channel(); + let events = ChannelEventCollector::new(collision_send, force_send, tear_send); // Demo UI: the smallest piece a tear may split off, applied to every soft body (the pieces // a tear splits off inherit their origin's material). let mut min_piece_default = true; @@ -130,14 +135,32 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { // The right end of the bar starts moving after a second, at half a meter per // second, and stops once the bar has doubled its length. let shift = ((t - 1.0).max(0.0) * 0.5).min(4.0); - let sb = &mut world.soft_bodies[bar]; - for &(i, rest, right) in &ends { + for &((body, i), rest, right) in &ends { if right { - sb.set_particle_kinematic_target(i, rest + Vector::new(shift, 0.0)); + world.soft_bodies[body] + .set_particle_kinematic_target(i as usize, rest + Vector::new(shift, 0.0)); } } - world.step(); + world.step_with_events(&(), &events); + let tears: Vec = tear_recv.try_iter().collect(); + for (particle, _, _) in &mut ends { + follow_tears(&tears, particle); + } } } Ok(()) } + +/// Where a particle driven by index is after the tears of a step: a tear compacts the torn +/// body's particles and splits the disconnected pieces off as soft bodies of their own, so a +/// particle's body and index follow the events. +fn follow_tears(events: &[SoftBodyTearEvent], particle: &mut (SoftBodyHandle, u32)) { + for event in events { + if event.soft_body == particle.0 { + if let Some(destination) = event.particle_destination(particle.1) { + *particle = destination; + } + } + } +} + diff --git a/examples3d/soft_tearing3.rs b/examples3d/soft_tearing3.rs index 82e6c1431..51d3f8192 100644 --- a/examples3d/soft_tearing3.rs +++ b/examples3d/soft_tearing3.rs @@ -98,25 +98,30 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { .surface_collider(ColliderBuilder::ball(0.1).friction(0.8)); let bar = world.insert_soft_body(bar); let sb = &world.soft_bodies[bar]; - let ends: Vec<(usize, Vector, bool)> = (0..sb.num_particles()) + let mut ends: Vec<((SoftBodyHandle, u32), Vector, bool)> = (0..sb.num_particles()) .filter_map(|i| { let p = sb.particle_position(i); if p.x < 1.01 { - Some((i, p, false)) + Some(((bar, i as u32), p, false)) } else if p.x > 4.99 { - Some((i, p, true)) + Some(((bar, i as u32), p, true)) } else { None } }) .collect(); - for &(i, _, _) in &ends { - world.soft_bodies[bar].set_particle_pinned(i, true); + for &((_, i), _, _) in &ends { + world.soft_bodies[bar].set_particle_pinned(i as usize, true); } viewer.set_world(&mut world); viewer.look_at(Vec3::new(9.0, 8.0, 16.0), Vec3::new(0.0, 1.5, 1.0)); + // The tears of every step, to follow the driven particles through them. + let (collision_send, _collision_recv) = std::sync::mpsc::channel(); + let (force_send, _force_recv) = std::sync::mpsc::channel(); + let (tear_send, tear_recv) = std::sync::mpsc::channel(); + let events = ChannelEventCollector::new(collision_send, force_send, tear_send); // Demo UI: the smallest piece a tear may split off, applied to every soft body (the pieces // a tear splits off inherit their origin's material). let mut min_piece_default = true; @@ -151,14 +156,32 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { // The right end of the bar starts moving after a second, at half a meter per // second, and stops once the bar has doubled its length. let shift = ((t - 1.0).max(0.0) * 0.5).min(4.0); - let sb = &mut world.soft_bodies[bar]; - for &(i, rest, right) in &ends { + for &((body, i), rest, right) in &ends { if right { - sb.set_particle_kinematic_target(i, rest + Vector::new(shift, 0.0, 0.0)); + let target = rest + Vector::new(shift, 0.0, 0.0); + world.soft_bodies[body].set_particle_kinematic_target(i as usize, target); } } - world.step(); + world.step_with_events(&(), &events); + let tears: Vec = tear_recv.try_iter().collect(); + for (particle, _, _) in &mut ends { + follow_tears(&tears, particle); + } } } Ok(()) } + +/// Where a particle driven by index is after the tears of a step: a tear compacts the torn +/// body's particles and splits the disconnected pieces off as soft bodies of their own, so a +/// particle's body and index follow the events. +fn follow_tears(events: &[SoftBodyTearEvent], particle: &mut (SoftBodyHandle, u32)) { + for event in events { + if event.soft_body == particle.0 { + if let Some(destination) = event.particle_destination(particle.1) { + *particle = destination; + } + } + } +} + diff --git a/src/dynamics/soft_body/tearing/tearing_cut.rs b/src/dynamics/soft_body/tearing/tearing_cut.rs index 5f09b0462..110cb8f56 100644 --- a/src/dynamics/soft_body/tearing/tearing_cut.rs +++ b/src/dynamics/soft_body/tearing/tearing_cut.rs @@ -241,6 +241,7 @@ impl SoftBody { &mut self, ei: usize, t: Real, + force: bool, straddling: &mut Vec, log: &mut SplitLog, ) -> bool { @@ -248,6 +249,7 @@ impl SoftBody { if !force && !range.contains(&t) { return false; } + let t = t.clamp(*range.start(), *range.end()); let edge = self.edges[ei]; let [a, b] = edge.vertices; let length = edge.initial_rest_length(); diff --git a/src/dynamics/soft_body/tearing/tearing_particle_split.rs b/src/dynamics/soft_body/tearing/tearing_particle_split.rs index 4ff2b8351..f3469169b 100644 --- a/src/dynamics/soft_body/tearing/tearing_particle_split.rs +++ b/src/dynamics/soft_body/tearing/tearing_particle_split.rs @@ -316,6 +316,10 @@ impl SoftBody { /// shape: every element goes to the side of its rest centroid, and the groups are the /// connected components within a side. `None` when the fan lies on one side only. pub(super) fn plane_fan(&self, v: u32, origin: Vector, normal: Vector) -> Option { + // A pinned particle is never split: the crack must open elsewhere. + if self.particles[v as usize].inv_mass == 0.0 { + return None; + } let kind = self.measure_kind()?; let mut fan = self.fan(kind, v); let rest = |i: u32| self.particles[i as usize].rest_position; diff --git a/src/dynamics/soft_body/tearing/tests.rs b/src/dynamics/soft_body/tearing/tests.rs index 283eae561..1c45ab67b 100644 --- a/src/dynamics/soft_body/tearing/tests.rs +++ b/src/dynamics/soft_body/tearing/tests.rs @@ -129,6 +129,49 @@ fn min_piece_sets_the_smallest_piece_a_tear_leaves() { } } +/// Checks that a pinned particle is never split: a rope pinned at both ends tears at the free end +/// of its first segment, a segment between two pinned particles never tears, and a cut next to a +/// pinned end inserts its particles a fifth of the segment in. +#[test] +fn pinned_particles_are_never_split() { + let rope = SoftBodyBuilder::rope(Vector::ZERO, Vector::X * 8.0, 9) + .particle_mass(0.5) + .min_piece(1) + .pinned_particles([0, 8]); + let (mut world, handle) = world_with(rope); + let event = world.tear_soft_body(handle, &[0], &[]).expect("nothing tore"); + assert_eq!(event.split_particles, vec![(9, 1)]); + assert_eq!(event.pieces.len(), 2); + let stub = &world.soft_bodies[event.pieces[1].soft_body]; + assert_eq!(event.pieces[1].particles, vec![0, 9]); + assert!(stub.particles()[0].is_pinned() && !stub.particles()[1].is_pinned()); + + let both = SoftBodyBuilder::rope(Vector::ZERO, Vector::X * 2.0, 3) + .min_piece(1) + .pinned_particles([0, 1]); + let (mut world, handle) = world_with(both); + assert!(world.tear_soft_body(handle, &[0], &[]).is_none()); + assert_eq!(world.soft_bodies[handle].num_particles(), 3); + + let rope = SoftBodyBuilder::rope(Vector::ZERO, Vector::X * 8.0, 9).pinned_particles([0]); + let (mut world, handle) = world_with(rope); + #[cfg(feature = "dim2")] + let blade = [Vector::new(0.05, -1.0), Vector::new(0.05, 1.0)]; + #[cfg(feature = "dim3")] + let blade = [ + Vector::new(0.05, -1.0, -1.0), + Vector::new(0.05, 2.0, -1.0), + Vector::new(0.05, -1.0, 2.0), + ]; + let event = world.cut_soft_body(handle, &blade).expect("the blade crossed the rope"); + assert!(event.split_particles.is_empty()); + assert_eq!(event.inserted_particles.len(), 2); + let stub = &world.soft_bodies[event.pieces[1].soft_body]; + assert_eq!(stub.num_particles(), 2); + assert!(stub.particles()[0].is_pinned()); + assert!((stub.particle_position(1).x - 0.2).abs() < 1.0e-5); +} + /// A soft body and every body split off it by tears, recursively (see `SoftBody::pieces`). fn family(world: &PhysicsWorld, handle: SoftBodyHandle) -> Vec { let mut bodies = vec![handle]; diff --git a/src_testbed/grab.rs b/src_testbed/grab.rs index c6bfb1620..61ee4a228 100644 --- a/src_testbed/grab.rs +++ b/src_testbed/grab.rs @@ -6,7 +6,7 @@ #![allow(clippy::unnecessary_cast)] use crate::mouse::SceneMouse; -use rapier::dynamics::{RigidBodyBuilder, RigidBodyHandle, SoftBodyHandle}; +use rapier::dynamics::{ImpulseJointHandle, RigidBodyBuilder, RigidBodyHandle}; use rapier::math::{Real, Vector}; use rapier::pipeline::PhysicsWorld; use rapier::prelude::{ @@ -32,12 +32,25 @@ struct Grabbed { /// Invisible kinematic body following the cursor. mouse_body: RigidBodyHandle, /// The mouse joint. - soft_cluster: Option<(SoftBodyHandle, u32)>, + joint: ImpulseJointHandle, + /// Whether the grab holds a soft body through a temporary cluster proxy, torn down on + /// release together with whatever proxy the joint ended on. + soft: bool, /// Anchor of the plane the cursor ray is intersected with while dragging. #[cfg(feature = "dim3")] plane_point: Vector, } +impl Grabbed { + /// The body the joint pulls now: the picked body, or the proxy a tear handed the joint to. + fn pulled(&self, world: &PhysicsWorld) -> RigidBodyHandle { + world + .impulse_joints + .get(self.joint) + .map_or(self.body, |joint| joint.body2()) + } +} + impl MouseGrab { pub fn active(&self) -> bool { self.grabbed.is_some() @@ -47,7 +60,7 @@ impl MouseGrab { pub fn cue_line(&self, world: &PhysicsWorld) -> Option<(Vector, Vector)> { let g = self.grabbed.as_ref()?; let mouse = world.bodies.get(g.mouse_body)?.position().translation; - let body = world.bodies.get(g.body)?.position().translation; + let body = world.bodies.get(g.pulled(world))?.position().translation; Some((mouse, body)) } @@ -93,11 +106,12 @@ impl MouseGrab { }; let mouse_body = world .insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); - world.insert_impulse_joint(mouse_body, proxy, mouse_joint(Vector::ZERO)); + let joint = world.insert_impulse_joint(mouse_body, proxy, mouse_joint(Vector::ZERO)); Grabbed { body: proxy, mouse_body, - soft_cluster: Some((sb_handle, cluster)), + joint, + soft: true, #[cfg(feature = "dim3")] plane_point: anchor, } @@ -107,11 +121,13 @@ impl MouseGrab { .inverse_transform_point(grab_point); let mouse_body = world .insert_body(RigidBodyBuilder::kinematic_position_based().translation(grab_point)); - world.insert_impulse_joint(mouse_body, body_handle, mouse_joint(local_anchor)); + let joint = + world.insert_impulse_joint(mouse_body, body_handle, mouse_joint(local_anchor)); Grabbed { body: body_handle, mouse_body, - soft_cluster: None, + joint, + soft: false, #[cfg(feature = "dim3")] plane_point: grab_point, } @@ -136,7 +152,8 @@ impl MouseGrab { return; }; // The example may have removed the body (or the whole soft body) while we held it. - if world.bodies.get(g.body).is_none() || world.bodies.get(g.mouse_body).is_none() { + let pulled = g.pulled(world); + if world.bodies.get(pulled).is_none() || world.bodies.get(g.mouse_body).is_none() { self.release(world); return; } @@ -165,7 +182,7 @@ impl MouseGrab { }; world.bodies[g.mouse_body].set_next_kinematic_translation(target); - world.bodies[g.body].wake_up(true); + world.bodies[pulled].wake_up(true); } /// Removes the mouse joint, its kinematic anchor, and any temporary cluster proxy (the one @@ -175,14 +192,18 @@ impl MouseGrab { let Some(g) = self.grabbed.take() else { return; }; - if let Some(rb) = world.bodies.get_mut(g.body) { + let pulled = g.pulled(world); + if let Some(rb) = world.bodies.get_mut(pulled) { rb.wake_up(true); } // Removing the body also removes the attached mouse joint. world.remove_body(g.mouse_body); - if let Some((sb_handle, cluster)) = g.soft_cluster { - if world.soft_bodies.get(sb_handle).is_some() { - world.remove_soft_body_cluster(sb_handle, cluster); + if g.soft { + for proxy in [g.body, pulled] { + if world.bodies.get(proxy).is_some_and(|rb| rb.is_soft_frame()) { + world.remove_body(proxy); + } + } } } } diff --git a/src_testbed/graphics/graphics_soft_bodies.rs b/src_testbed/graphics/graphics_soft_bodies.rs index d9ac54a3f..093624504 100644 --- a/src_testbed/graphics/graphics_soft_bodies.rs +++ b/src_testbed/graphics/graphics_soft_bodies.rs @@ -8,8 +8,8 @@ use super::graphics_polyline::stroked_polyline; use super::graphics_polyline::tube_polyline; use super::{GraphicsManager, IndividualNode, SceneNode}; use kiss3d::prelude::*; -use rapier::dynamics::{SoftBodyHandle, SoftBodySet, SoftMeshRef}; -use rapier::geometry::{Collider, Shape, SharedShape}; +use rapier::dynamics::{RigidBodySet, SoftBodyHandle, SoftBodySet, SoftMeshRef}; +use rapier::geometry::{Collider, ColliderSet, Shape, SharedShape}; use std::collections::HashMap; /// A render-only node following a mesh a soft body only *draws*: the skin it wears without @@ -199,6 +199,83 @@ impl GraphicsManager { } } + /// Registers render nodes for what the soft bodies gained since the last frame: a tear splits + /// pieces as new soft bodies (fresh proxies and colliders) and moves clusters between bodies. + /// Each proxy wears its body's color; drawn skins get nodes, moved-away meshes lose theirs. + pub fn add_missing_soft_body_graphics( + &mut self, + window: &mut Window, + bodies: &RigidBodySet, + colliders: &ColliderSet, + soft_bodies: &SoftBodySet, + ) { + for (handle, sb) in soft_bodies.iter() { + let color = self.soft_body_color(handle); + for (_, cluster) in sb.live_clusters() { + let proxy = cluster.proxy(); + self.set_initial_body_color(proxy, color); + let Some(rb) = bodies.get(proxy) else { + continue; + }; + for &co_handle in rb.colliders() { + if self.c2nodes.contains_key(&co_handle) { + continue; + } + let Some(co) = colliders.get(co_handle) else { + continue; + }; + self.add_shape( + window, + co_handle, + Some(proxy), + co.shape(), + co.is_sensor(), + rapier::math::Pose::IDENTITY, + color, + ); + } + } + for mesh in sb.meshes().filter(|mesh| is_drawn_skin(mesh)) { + let mesh_ref = SoftMeshRef { + body: handle, + id: mesh.id(), + }; + if self.soft_graphics.mesh_nodes.iter().any(|n| n.mesh == mesh_ref) { + continue; + } + let Some(shape) = + drawn_mesh_shape(mesh.vertex_positions(sb).collect(), mesh.indices()) + else { + continue; + }; + let smooth = self.smooth_mesh_colliders; + let Some(mut node) = + Self::create_individual_node(&mut self.scene, &*shape, color, false, smooth) + else { + continue; + }; + node.set_visible(self.draw_surfaces && self.colliders_visible); + self.soft_graphics.mesh_nodes.push(SoftMeshNode { + node, + mesh: mesh_ref, + color, + smooth, + }); + } + } + // The nodes of meshes that are gone (a removed body, a mesh moved to a piece). + self.soft_graphics.mesh_nodes.retain_mut(|n| { + let live = soft_bodies + .get(n.mesh.body) + .and_then(|sb| sb.mesh(n.mesh.id)) + .is_some(); + if !live { + n.node.detach(); + } + live + }); + } + /// Follows the drawn soft-body meshes: their vertices move every step, so each node's vertex /// buffer is rewritten in place. A node whose mesh is gone (a removed body) is hidden, and /// one whose geometry no longer matches (a shading change, or a tear) is built anew. diff --git a/src_testbed/viewer.rs b/src_testbed/viewer.rs index 6f6e7705d..2e386dda4 100644 --- a/src_testbed/viewer.rs +++ b/src_testbed/viewer.rs @@ -172,6 +172,13 @@ impl TestbedViewer { self.autosave(); highlight_hovered_body(&mut self.graphics, &self.scene_mouse, world); + // The pieces a tear split off during the step, and what moved between bodies. + self.graphics.add_missing_soft_body_graphics( + &mut self.window, + &world.bodies, + &world.colliders, + &world.soft_bodies, + ); self.graphics.draw( self.state.flags, &world.bodies, From 3b207923464fe093e7a8e5a744f5e43372f3dd4a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 17 Sep 2026 12:05:36 +0200 Subject: [PATCH 19/32] feat(soft-body): stress-based plasticity with rest shapes following the flow, dihedral creases, reset and per-body PGS iterations --- crates/rapier2d/tests/soft_bodies.rs | 22 +++ crates/rapier3d/tests/soft_bodies.rs | 102 +++++++++- src/dynamics/island_manager/substep_groups.rs | 29 ++- src/dynamics/rigid_body.rs | 35 ++++ src/dynamics/soft_body/soft_body.rs | 5 + src/dynamics/soft_body/soft_body_accessors.rs | 7 + .../soft_body_builder/soft_body_build.rs | 3 + .../soft_body_builder/soft_body_builder.rs | 5 + .../soft_body_builder_settings.rs | 9 + src/dynamics/soft_body/soft_body_elements.rs | 53 ++++- src/dynamics/soft_body/soft_body_material.rs | 5 + .../soft_body/soft_body_plasticity.rs | 184 ++++++++++++++++++ .../soft_body_set_insert_remove.rs | 1 + .../soft_body_set/soft_body_set_proxies.rs | 6 + .../soft_body_set/soft_body_set_step_sync.rs | 1 + src/dynamics/soft_body/tearing/tearing_cut.rs | 1 + .../soft_constraint_plasticity.rs | 150 ++++++++++++-- .../soft_constraint_writeback.rs | 41 +++- .../soft_elastic_constraint.rs | 15 +- .../soft_element_constraint/test.rs | 2 + .../soft_element_constraint_assembly.rs | 11 +- .../system/soft_fem_system_assemble.rs | 28 ++- .../system/soft_fem_system_elements.rs | 16 +- .../system/soft_fem_system_prepare.rs | 1 + .../solver/staged_island_solver/init.rs | 11 ++ .../solver/staged_island_solver/mod.rs | 3 + .../solver/staged_island_solver/worker.rs | 12 +- src/pipeline/physics_pipeline/solve.rs | 6 +- src_testbed/save.rs | 3 + src_testbed/testbed/state.rs | 3 + src_testbed/ui.rs | 18 ++ 31 files changed, 723 insertions(+), 65 deletions(-) diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index 999326be4..edc720a41 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -2085,6 +2085,10 @@ fn interior_strength_shields_undamaged_interior_edges() { interior_strength, ..Default::default() }) + // The crack opens at a free endpoint of the torn edge, chosen by the loads around + // it, which the shield changes: pinning one endpoint makes both runs open the same + // crack, so their geometries stay identical and the loads comparable. + .pinned_particles([idx(1, 1)]) .no_surface_collider() .can_sleep(false); let handle = world.insert_soft_body(grid); @@ -2190,6 +2194,24 @@ fn edge_plasticity_keeps_a_dent() { assert!((e.rest_length - 0.7 / 0.9).abs() < 1.0e-4, "rest length {}", e.rest_length); assert!((e.initial_rest_length() - 1.0).abs() < 1.0e-4); assert!((e.plastic_strain() - (0.7 / 0.9 - 1.0)).abs() < 1.0e-4); + // The rest positions followed the flow: the edge is as long at rest as its rest length. + let rest_distance = |sb: &SoftBody| { + (sb.particles()[1].rest_position() - sb.particles()[0].rest_position()).length() + }; + let rest = rest_distance(sb); + assert!( + (rest - 0.7 / 0.9).abs() < 5.0e-3, + "the rest positions did not follow the flow: rest distance {rest}" + ); + let initial = + sb.particles()[1].initial_rest_position() - sb.particles()[0].initial_rest_position(); + assert!((initial.length() - 1.0).abs() < 1.0e-6); + // The reset undoes the set: rest length and rest positions as created. + world.soft_bodies[handle].reset_plasticity(); + let sb = &world.soft_bodies[handle]; + assert_eq!(sb.edges()[0].rest_length, 1.0); + assert_eq!(sb.edges()[0].plastic_strain(), 0.0); + assert_eq!(rest_distance(sb), 1.0); // The plastic maximum bounds the set. let (world, handle) = squeeze(clay(SoftEdgePlasticFlow::Both, 0.15)); diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index 8bd9440b9..05027bdcd 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -700,8 +700,10 @@ fn deformation_damping_settles_stiff_bodies() { }; let undamped = tip_speed_after(0.0, 6.0); let damped = tip_speed_after(3.0, 6.0); + // The soft PGS iterations take part of the residual sway away (0.9 m/s with a single + // pass); the damping takes the rest. assert!( - undamped > 0.5, + undamped > 0.3, "the undamped beam already stopped: {undamped}" ); assert!(damped < 0.01, "the damped beam still swings: {damped}"); @@ -745,7 +747,9 @@ fn deformation_damping_settles_stiff_bodies() { undamped > 0.1, "the undamped block already stopped: {undamped}" ); - assert!(damped < 0.01, "the damped block still sways: {damped}"); + // The residual is the block rocking on its surface colliders, which the damping leaves + // alone by design. + assert!(damped < 0.03, "the damped block still sways: {damped}"); } /// Two-way coupling: a rigid box dropped on a cloth pinned by its four corners is held up by @@ -1039,7 +1043,9 @@ fn self_contacts_keep_folded_cloth_apart() { ((n - 1) * 3 + 2) as u32, ]) .self_contacts(self_contacts) - .softness(SpringCoefficients::new(30.0, 1.0)) + // Floppy enough to touch itself once folded (the soft PGS iterations stiffen the + // bending: at 30 Hz the loop stays open on its own). + .softness(SpringCoefficients::new(10.0, 1.0)) .particle_mass(0.05); let handle = world.insert_soft_body(strip); // Let it sag, then move the right end onto the left end. @@ -1468,10 +1474,26 @@ fn plastic_cells_keep_their_deformation() { .map(|&i| sb.particle_position(i).x) .sum::() / bottom.len() as Real; - top_x - bottom_x + // The skew of the rest shape: the angle its vertical and horizontal mid-lines lost + // from a right angle. Rotation-invariant: a symmetric plastic stretch leaves the rest + // shape rotated by the shear's polar angle, so a lean read along `x` would undercount. + let left: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particles()[i].initial_rest_position().x < -0.74) + .collect(); + let right: Vec = (0..sb.num_particles()) + .filter(|&i| sb.particles()[i].initial_rest_position().x > 0.74) + .collect(); + let mean = |set: &[usize]| { + set.iter().map(|&i| sb.particles()[i].rest_position()).sum::() + / set.len() as Real + }; + let vertical = mean(&top) - mean(&bottom); + let horizontal = mean(&right) - mean(&left); + let skew = (vertical.dot(horizontal) / (vertical.length() * horizontal.length())).asin(); + (top_x - bottom_x, skew) }; - let elastic = lean_after(0.0); - let plastic = lean_after(0.05); + let (elastic, elastic_skew) = lean_after(0.0); + let (plastic, plastic_skew) = lean_after(0.05); assert!( elastic.abs() < 0.15, "the elastic block did not spring back: lean {elastic}" @@ -1480,6 +1502,65 @@ fn plastic_cells_keep_their_deformation() { plastic > 0.4, "the plastic block did not keep its lean: {plastic} (elastic {elastic})" ); + // The rest positions followed the flow: the plastic block's rest shape is skewed like it + // (a lean of 0.4 over a height of 1.5 is a skew of about 15 degrees). + assert_eq!(elastic_skew, 0.0, "an elastic body's rest shape moved"); + assert!( + plastic_skew.to_degrees() > 12.0, + "the rest shape did not follow the plastic flow: skew {} degrees (lean {plastic})", + plastic_skew.to_degrees() + ); +} + +/// Dihedral plasticity: a cloth fold past the yield (read in radians) keeps a crease, the +/// rest angle flowing to what leaves exactly the yield; the reset flattens it again. +#[test] +fn dihedral_plasticity_keeps_a_crease() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + // Two triangles sharing the edge `[0, 1]`, flat at rest; a free particle on a slack edge + // keeps the body from being frozen once the four of the fold are pinned. + let builder = SoftBodyBuilder::new(vec![ + Vector::new(0.0, 0.0, 0.0), + Vector::new(1.0, 0.0, 0.0), + Vector::new(0.5, 0.0, 1.0), + Vector::new(0.5, 0.0, -1.0), + Vector::new(3.0, 0.0, 0.0), + ]) + .edges(vec![[0, 1], [0, 2], [1, 2], [0, 3], [1, 3], [2, 4]]) + .dihedrals(vec![[0, 1, 2, 3]]) + .pinned_particles([0, 1, 2, 3]) + .particle_mass(1.0) + .material(SoftBodyMaterial { + edge_plastic_yield: 0.2, + edge_plastic_creep: Real::INFINITY, + edge_plastic_max: 1.0, + ..Default::default() + }) + .no_surface_collider() + .can_sleep(false); + let handle = world.insert_soft_body(builder); + let flat = world.soft_bodies[handle].dihedrals()[0].rest_angle; + // Fold particle 3 up by 60 degrees about the shared edge: the fold angle is 120 degrees. + let sixty: Real = 60.0; + let (s, c) = sixty.to_radians().sin_cos(); + world.soft_bodies[handle].set_particle_position(3, Vector::new(0.5, s, -c)); + for _ in 0..30 { + world.step(); + } + let folded: Real = 2.0 * core::f32::consts::FRAC_PI_3; + let d = &world.soft_bodies[handle].dihedrals()[0]; + assert!( + (d.rest_angle - (folded + 0.2)).abs() < 1.0e-3, + "the rest angle did not flow to the yield: {} (flat {flat}, folded {folded})", + d.rest_angle + ); + assert!((d.initial_rest_angle() - flat).abs() < 1.0e-6); + assert!((d.plastic_set() - (folded + 0.2 - flat)).abs() < 1.0e-3); + world.soft_bodies[handle].reset_plasticity(); + let d = &world.soft_bodies[handle].dihedrals()[0]; + assert_eq!(d.rest_angle, flat); + assert_eq!(d.plastic_set(), 0.0); } /// Plastic flow is bounded: a clay slab stamped again and again by a kinematic wedge keeps @@ -1604,6 +1685,11 @@ fn creeping_body_stays_awake() { world.step(); } let plastic_top_early = top(&world, handles[1]); + // Still creeping at 3 s (it collapses into a heap by 4 s, and a heap may sleep). + assert!( + !world.soft_bodies[handles[1]].is_sleeping(), + "the creeping column fell asleep" + ); for _ in 0..180 { world.step(); } @@ -1611,10 +1697,6 @@ fn creeping_body_stays_awake() { world.soft_bodies[handles[0]].is_sleeping(), "the elastic column did not sleep" ); - assert!( - !world.soft_bodies[handles[1]].is_sleeping(), - "the creeping column fell asleep" - ); let plastic_top = top(&world, handles[1]); assert!( plastic_top < plastic_top_early - 0.3, diff --git a/src/dynamics/island_manager/substep_groups.rs b/src/dynamics/island_manager/substep_groups.rs index 449111264..27c1ac03f 100644 --- a/src/dynamics/island_manager/substep_groups.rs +++ b/src/dynamics/island_manager/substep_groups.rs @@ -22,6 +22,22 @@ pub(crate) struct SolveGroup { /// Extra substeps for this group, on top of /// `IntegrationParameters::num_solver_iterations`. pub extra_iters: u32, + /// Extra internal PGS iterations per substep for this group, on top of + /// `IntegrationParameters::num_internal_pgs_iterations`. + pub extra_pgs: u32, +} + +/// The partition key of a body: its extra substeps in the high half, its extra PGS iterations +/// in the low half, so the groups sort by substep cadence first. +#[inline] +fn key(extra_substeps: u32, extra_pgs: u32) -> u32 { + (extra_substeps.min(0xFFFF) << 16) | extra_pgs.min(0xFFFF) +} + +/// The element-wise maximum of two keys (a component takes the largest of each count). +#[inline] +fn max_key(a: u32, b: u32) -> u32 { + key((a >> 16).max(b >> 16), (a & 0xFFFF).max(b & 0xFFFF)) } /// Workspace state for [`IslandManager::update_substep_groups`], kept to reuse @@ -138,10 +154,14 @@ impl IslandManager { ws.keys.resize(num_bodies, 0); for i in 0..num_bodies { let handle = self.islands[awake_id].bodies[i]; - let extra = bodies[handle].additional_solver_iterations() as u32; - if extra > 0 { + let rb = &bodies[handle]; + let body_key = key( + rb.additional_solver_iterations() as u32, + rb.additional_pgs_iterations() as u32, + ); + if body_key > 0 { let root = ws.uf.find(i as u32) as usize; - ws.keys[root] = ws.keys[root].max(extra); + ws.keys[root] = max_key(ws.keys[root], body_key); } } for i in 0..num_bodies { @@ -253,7 +273,8 @@ fn push_group_ranges(groups: &mut Vec, keys: &[u32]) { if i == keys.len() || keys[i] != keys[start] { groups.push(SolveGroup { body_range: start..i, - extra_iters: keys[start], + extra_iters: keys[start] >> 16, + extra_pgs: keys[start] & 0xFFFF, }); start = i; } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index ccb693d7a..abab5bf6e 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -65,6 +65,7 @@ pub struct RigidBody { pub(crate) dominance: RigidBodyDominance, pub(crate) enabled: bool, pub(crate) additional_solver_iterations: usize, + pub(crate) additional_pgs_iterations: usize, /// The soft body this rigid body is the root of (invalid for regular rigid bodies). #[cfg_attr( feature = "serde-serialize", @@ -112,6 +113,7 @@ impl RigidBody { enabled: true, user_data: 0, additional_solver_iterations: 0, + additional_pgs_iterations: 0, soft_body: crate::dynamics::SoftBodyHandle::invalid(), soft_cluster: u32::MAX, soft_motion_margin: 0.0, @@ -160,6 +162,7 @@ impl RigidBody { dominance, enabled, additional_solver_iterations, + additional_pgs_iterations, soft_body: _soft_body, // The soft-body markers belong to the proxy, not to its state. soft_cluster: _soft_cluster, soft_motion_margin: _soft_motion_margin, // Engine-managed, like the markers. @@ -178,6 +181,7 @@ impl RigidBody { self.dominance = *dominance; self.enabled = *enabled; self.additional_solver_iterations = *additional_solver_iterations; + self.additional_pgs_iterations = *additional_pgs_iterations; self.user_data = *user_data; self.changes = RigidBodyChanges::all(); @@ -220,6 +224,21 @@ impl RigidBody { self.additional_solver_iterations = additional_iterations; } + /// The additional number of internal PGS iterations run per substep for the simulation + /// island component containing this rigid-body (default: 0). + /// + /// See [`Self::set_additional_pgs_iterations`] for additional information. + pub fn additional_pgs_iterations(&self) -> usize { + self.additional_pgs_iterations + } + + /// Set the additional number of internal PGS iterations run per substep for the simulation + /// island component containing this rigid-body (default: 0); the component runs the largest + /// request, soft bodies asking three ([`crate::dynamics::SoftBodyParticleSettings`]). + pub fn set_additional_pgs_iterations(&mut self, additional_iterations: usize) { + self.additional_pgs_iterations = additional_iterations; + } + /// The activation status of this rigid-body. pub fn activation(&self) -> &RigidBodyActivation { &self.activation @@ -1666,6 +1685,11 @@ pub struct RigidBodyBuilder { /// /// See [`RigidBody::set_additional_solver_iterations`] for additional information. pub additional_solver_iterations: usize, + /// The additional number of internal PGS iterations run per substep for the island + /// component of this rigid-body. + /// + /// See [`RigidBody::set_additional_pgs_iterations`] for additional information. + pub additional_pgs_iterations: usize, /// Are gyroscopic forces enabled for this rigid-body? pub gyroscopic_forces_enabled: bool, } @@ -1703,6 +1727,7 @@ impl RigidBodyBuilder { enabled: true, user_data: 0, additional_solver_iterations: 0, + additional_pgs_iterations: 0, gyroscopic_forces_enabled: true, } } @@ -1780,6 +1805,15 @@ impl RigidBodyBuilder { self } + /// Sets the additional number of internal PGS iterations run per substep for the island + /// component of this rigid-body. + /// + /// See [`RigidBody::set_additional_pgs_iterations`] for additional information. + pub fn additional_pgs_iterations(mut self, additional_iterations: usize) -> Self { + self.additional_pgs_iterations = additional_iterations; + self + } + /// Sets the scale applied to the gravity force affecting the rigid-body to be created. pub fn gravity_scale(mut self, scale_factor: Real) -> Self { self.gravity_scale = scale_factor; @@ -2101,6 +2135,7 @@ impl RigidBodyBuilder { rb.body_type = self.body_type; rb.user_data = self.user_data; rb.additional_solver_iterations = self.additional_solver_iterations; + rb.additional_pgs_iterations = self.additional_pgs_iterations; if self.additional_mass_properties != RigidBodyAdditionalMassProps::MassProps(MassProperties::default()) diff --git a/src/dynamics/soft_body/soft_body.rs b/src/dynamics/soft_body/soft_body.rs index 854f8d6dc..2673fd918 100644 --- a/src/dynamics/soft_body/soft_body.rs +++ b/src/dynamics/soft_body/soft_body.rs @@ -105,6 +105,11 @@ pub struct SoftBody { /// step: the body is kept awake so its creep goes on. #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) plastic_flowing: bool, + /// Set when the elements flowed plastically: the particles' rest positions are fitted to the + /// flowed rest shapes at the end of the step, over the following steps until they settle + /// (see `SoftBody::fit_rest_positions`). + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) rest_fit_pending: bool, /// The speed the sleep rule reads for this body's cluster proxies: the fastest particle at the /// end of the last step, or `Real::MAX` while the body flows plastically (kept awake). /// `RigidBodyActivation::update_energy` compares it against the proxies' activation threshold. diff --git a/src/dynamics/soft_body/soft_body_accessors.rs b/src/dynamics/soft_body/soft_body_accessors.rs index 8e2dccf87..93e66c491 100644 --- a/src/dynamics/soft_body/soft_body_accessors.rs +++ b/src/dynamics/soft_body/soft_body_accessors.rs @@ -226,6 +226,13 @@ impl SoftBody { &self.particle_settings } + /// Sets the extra internal PGS iterations per substep requested for this body's island + /// component (see [`super::SoftBodyParticleSettings::additional_pgs_iterations`]); takes + /// effect at the next step. + pub fn set_additional_pgs_iterations(&mut self, iterations: usize) { + self.particle_settings.additional_pgs_iterations = iterations; + } + /// The number of parallel solver colors used by this soft body's elements (informational: /// elements of a same color share no particle and are solved concurrently). pub fn num_solver_colors(&self) -> usize { 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 868eabb6f..af840d587 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 @@ -61,6 +61,7 @@ impl SoftBodyBuilder { position: *p, velocity: Vector::ZERO, rest_position: *p - rest_com, + initial_rest_position: *p - rest_com, mass: *m, inv_mass: if *pinned { 0.0 } else { crate::utils::inv(*m) }, force: Vector::ZERO, @@ -179,6 +180,7 @@ impl SoftBodyBuilder { .map(|v| SoftBodyDihedral { vertices: *v, rest_angle: dihedral_angle(pos(v[0]), pos(v[1]), pos(v[2]), pos(v[3])), + plastic_set: 0.0, impulse: 0.0, color: 0, }) @@ -234,6 +236,7 @@ impl SoftBodyBuilder { has_overflow_color, modified: false, plastic_flowing: false, + rest_fit_pending: false, sleep_speed: 0.0, tearing_pending: false, topology_version: 0, diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs index 5e412d593..d6bda9435 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs @@ -20,6 +20,10 @@ pub struct SoftBodyParticleSettings { pub gravity_scale: Real, /// Extra solver substeps requested for the particles (and everything they touch). pub additional_solver_iterations: usize, + /// Extra internal PGS iterations per substep for the particles and everything they touch, on + /// top of `IntegrationParameters::num_internal_pgs_iterations`; stamped on the root proxy each + /// step (see [`crate::dynamics::RigidBody::set_additional_pgs_iterations`]) (default: `3`). + pub additional_pgs_iterations: usize, /// Whether the soft body may fall asleep. pub can_sleep: bool, /// Dominance group of the soft body (see [`crate::dynamics::RigidBody::dominance_group`]). @@ -32,6 +36,7 @@ impl Default for SoftBodyParticleSettings { linear_damping: 0.0, gravity_scale: 1.0, additional_solver_iterations: 0, + additional_pgs_iterations: 3, can_sleep: true, dominance_group: 0, } diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs index c1ec9604a..33df49e92 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs @@ -380,6 +380,15 @@ impl SoftBodyBuilder { self } + /// Requests extra internal PGS iterations per substep for the particles (and everything + /// they touch); see [`SoftBodyParticleSettings::additional_pgs_iterations`]. + /// + /// [`SoftBodyParticleSettings::additional_pgs_iterations`]: super::SoftBodyParticleSettings::additional_pgs_iterations + pub fn additional_pgs_iterations(mut self, iterations: usize) -> Self { + self.particle_settings.additional_pgs_iterations = iterations; + self + } + /// Whether the particles may fall asleep. pub fn can_sleep(mut self, can_sleep: bool) -> Self { self.particle_settings.can_sleep = can_sleep; diff --git a/src/dynamics/soft_body/soft_body_elements.rs b/src/dynamics/soft_body/soft_body_elements.rs index 64e53cacf..ea08e570d 100644 --- a/src/dynamics/soft_body/soft_body_elements.rs +++ b/src/dynamics/soft_body/soft_body_elements.rs @@ -24,6 +24,9 @@ pub struct SoftBodyParticle { /// shape-matching goal shape). Follows the plastic flow of the elements (see /// [`Self::initial_rest_position`]). pub(crate) rest_position: Vector, + /// The rest position before any plastic flow: the world-space position at insertion, + /// relative to the rest center of mass. The cells' initial rest shapes are read from it. + pub(crate) initial_rest_position: Vector, /// Nominal mass (used by shape matching even for pinned particles). pub(crate) mass: Real, /// `0.0` for pinned particles. @@ -92,6 +95,12 @@ impl SoftBodyParticle { self.rest_position } + /// The position of this particle in the soft body's rest shape before any plastic flow + /// (its position at insertion, relative to the rest center of mass). + pub fn initial_rest_position(&self) -> Vector { + self.initial_rest_position + } + /// The nominal mass of this particle. pub fn mass(&self) -> Real { self.mass @@ -201,10 +210,29 @@ pub struct SoftBodyDihedral { pub rest_angle: Real, /// Accumulated impulse of the last step (warm-start state). pub(crate) impulse: Real, + /// Accumulated plastic set of the rest angle: `rest_angle = initial rest angle + + /// plastic_set` (see [`Self::plastic_set`]). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) plastic_set: Real, /// Parallel solve color. pub(crate) color: u8, } +#[cfg(feature = "dim3")] +impl SoftBodyDihedral { + /// The permanent set of this dihedral: how far its rest angle has flowed from the angle it + /// was created with, in radians (see [`SoftBodyMaterial::edge_plastic_yield`]). + pub fn plastic_set(&self) -> Real { + self.plastic_set + } + + /// The rest angle this dihedral was created with (its `rest_angle` before any plastic + /// flow). + pub fn initial_rest_angle(&self) -> Real { + self.rest_angle - self.plastic_set + } +} + /// Which solver simulates a soft body's elasticity (requires the `fem` cargo feature). #[cfg(feature = "fem")] #[derive(Copy, Clone, Debug, PartialEq, Eq, Default)] @@ -254,15 +282,16 @@ pub struct SoftBodyCell { pub rest_volume: Real, /// Inverse of the rest edge matrix `[x1 - x0, x2 - x0, (x3 - x0)]`. pub(crate) inv_rest_matrix: Matrix, - /// Accumulated plastic stretch of the rest shape (material frame, unit determinant): the - /// current rest edge matrix is `plastic_stretch * Dm₀`. Bounded by the material's - /// `plastic_max`, which keeps flowing cells from degenerating into slivers. + /// Accumulated plastic stretch of the rest shape (material frame, unit determinant): the rest + /// edge matrix is `plastic_stretch * Dm₀`, `Dm₀` the initial rest edge matrix. Bounded by the + /// material's `plastic_max`, which keeps flowing cells from degenerating into slivers. + #[cfg_attr(feature = "serde-serialize", serde(default = "identity_matrix"))] pub(crate) plastic_stretch: Matrix, - /// Accumulated impulses of the last step (warm-start state): the volume row's in `[0]`, or - /// the corotational rows' (strain rows in the cell frame, then volumetric). + /// Accumulated impulses of the last step (warm-start state): the volume constraint's in `[0]`, or + /// the corotational constraints' (strain rows in the cell frame, then volumetric). pub(crate) impulses: [Real; CELL_IMPULSES], /// Rotation of the polar decomposition of the cell's deformation gradient at the last step - /// (warm start of the corotational rows' rotation extraction). + /// (warm start of the corotational constraints' rotation extraction). pub(crate) rotation: Rotation, /// Multiplier of the material's Young modulus for this cell (default `1.0`): per-region /// stiffness, usually set through [`crate::dynamics::SoftBody::set_cluster_stiffness_scale`]. @@ -284,7 +313,19 @@ pub struct SoftBodyCell { pub(crate) stress: Real, } +#[cfg(feature = "serde-serialize")] +fn identity_matrix() -> Matrix { + Matrix::IDENTITY +} + impl SoftBodyCell { + /// The accumulated plastic stretch of this cell's rest shape (material frame, unit + /// determinant): its current rest shape is this stretch applied to the shape it was created + /// with (see [`SoftBodyMaterial::plastic_yield`]); the identity before any flow. + pub fn plastic_stretch(&self) -> Matrix { + self.plastic_stretch + } + /// The largest tensile strain of this cell as a fraction of its tear threshold, smoothed /// over the material's [`SoftBodyMaterial::tear_smoothing`]: `0.0` slack, `1.0` tearing. /// Stays `0.0` while the material has no `tear_strain` (cells only tear on their strain). diff --git a/src/dynamics/soft_body/soft_body_material.rs b/src/dynamics/soft_body/soft_body_material.rs index d36de7760..15f1ee087 100644 --- a/src/dynamics/soft_body/soft_body_material.rs +++ b/src/dynamics/soft_body/soft_body_material.rs @@ -113,6 +113,11 @@ pub(super) fn one() -> Real { 1.0 } +#[cfg(feature = "serde-serialize")] +fn half() -> Real { + 0.5 +} + impl Default for SoftBodyMaterial { fn default() -> Self { Self { diff --git a/src/dynamics/soft_body/soft_body_plasticity.rs b/src/dynamics/soft_body/soft_body_plasticity.rs index 4578ecf3f..d9623c34d 100644 --- a/src/dynamics/soft_body/soft_body_plasticity.rs +++ b/src/dynamics/soft_body/soft_body_plasticity.rs @@ -1,7 +1,9 @@ //! Soft-body plasticity: edge flow modes, the rest-length and rest-angle flow rules, the fit of //! the rest positions to the flowed rest shapes, and the reset. The cells' flow rule runs on the //! solver's strain rows (`soft_constraints_set::plastic_flow`). +use crate::alloc_prelude::*; #[cfg(feature = "dim3")] +use crate::dynamics::SoftBodyDihedral; use crate::dynamics::{SoftBody, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial}; use crate::math::{DIM, Matrix, Real, Vector}; #[cfg(not(feature = "std"))] @@ -23,6 +25,12 @@ pub enum SoftEdgePlasticFlow { Tension, } +/// Jacobi sweeps of the rest-position fit per step. +const REST_FIT_SWEEPS: usize = 8; +/// The fit has settled when no particle moved by more than this fraction of the rest size in a +/// sweep. +const REST_FIT_TOLERANCE: Real = 1.0e-5; + impl SoftBodyEdge { /// The permanent set of this edge: how far its rest length has flowed from the length it was /// created with, as a fraction of that length (negative: a squeeze that stayed, positive: a @@ -65,8 +73,184 @@ impl SoftBodyEdge { initial * (1.0 + max), ); let increment = (new_rest - self.rest_length).abs() / initial; + // The warm-start impulse followed the old length error; it scales with what is left of it. + let error = length - self.rest_length; + if error != 0.0 { + self.impulse *= ((length - new_rest) / error).clamp(0.0, 1.0); + } self.rest_length = new_rest; self.plastic_strain = new_rest / initial - 1.0; increment > 1.0e-4 } } + +#[cfg(feature = "dim3")] +impl SoftBodyDihedral { + /// Flows the rest angle toward the current `angle` when the fold exceeds the material's + /// edge yield, read in radians (see [`SoftBodyMaterial::edge_plastic_yield`]; `dt`: the step + /// length). Returns whether the dihedral flowed by a significant amount. + pub(crate) fn plastic_flow( + &mut self, + angle: Real, + material: &SoftBodyMaterial, + dt: Real, + ) -> bool { + let yield_angle = material.edge_plastic_yield; + if yield_angle <= 0.0 || material.edge_plastic_creep <= 0.0 { + return false; + } + let deviation = angle - self.rest_angle; + if deviation.abs() <= yield_angle { + return false; + } + // The rest angle that would leave exactly the yield: only the excess flows. + let target = angle - yield_angle.copysign(deviation); + let blend = (material.edge_plastic_creep * dt).min(1.0); + let initial = self.initial_rest_angle(); + let max = material.edge_plastic_max.max(0.0); + let new_rest = (self.rest_angle + (target - self.rest_angle) * blend) + .clamp(initial - max, initial + max); + let increment = (new_rest - self.rest_angle).abs(); + // The warm-start impulse followed the old angle error; it scales with what is left of it. + self.impulse *= ((angle - new_rest) / deviation).clamp(0.0, 1.0); + self.rest_angle = new_rest; + self.plastic_set = new_rest - initial; + increment > 1.0e-4 + } +} + +impl SoftBody { + /// Moves the particles' rest positions toward the flowed rest shapes of the elements that can + /// flow (cells with a cell yield, edges with an edge yield): Jacobi sweeps to a least-squares + /// fit. Returns whether the fit settled; until then it continues at the next step. + pub(crate) fn fit_rest_positions(&mut self) -> bool { + let cells_flow = self.material.plastic_yield > 0.0 + && self.cell_model != SoftBodyCellModel::Volume + && !self.cells.is_empty(); + let edges_flow = self.material.edge_plastic_yield > 0.0 && !self.edges.is_empty(); + if !cells_flow && !edges_flow { + return true; + } + let n = self.particles.len(); + let size = self + .particles + .iter() + .map(|p| p.rest_position.length()) + .fold(0.0, Real::max); + let tolerance = size.max(Real::EPSILON) * REST_FIT_TOLERANCE; + let mut sum = vec![Vector::ZERO; n]; + let mut count: Vec = vec![0.0; n]; + let mut settled = false; + for _ in 0..REST_FIT_SWEEPS { + sum.fill(Vector::ZERO); + count.fill(0.0); + if cells_flow { + for c in &self.cells { + if c.rest_volume == 0.0 { + continue; + } + let rest0: [Vector; DIM + 1] = core::array::from_fn(|k| { + self.particles[c.vertices[k] as usize].initial_rest_position + }); + let dm = c.plastic_stretch * Self::cell_edge_matrix(rest0); + let mut offsets = [Vector::ZERO; DIM + 1]; + let mut centroid = Vector::ZERO; + for k in 0..DIM { + offsets[k + 1] = dm.col(k); + } + let mean = offsets.iter().sum::() / (DIM + 1) as Real; + for &v in &c.vertices { + centroid += self.particles[v as usize].rest_position; + } + centroid /= (DIM + 1) as Real; + for (k, &v) in c.vertices.iter().enumerate() { + sum[v as usize] += centroid + offsets[k] - mean; + count[v as usize] += 1.0; + } + } + } + if edges_flow { + for e in &self.edges { + if e.rest_length <= 0.0 { + continue; + } + let [a, b] = e.vertices; + let (ra, rb) = ( + self.particles[a as usize].rest_position, + self.particles[b as usize].rest_position, + ); + let d = rb - ra; + let len = d.length(); + if len <= Real::EPSILON { + continue; + } + let half = d * (0.5 * e.rest_length / len); + let mid = (ra + rb) * 0.5; + sum[a as usize] += mid - half; + count[a as usize] += 1.0; + sum[b as usize] += mid + half; + count[b as usize] += 1.0; + } + } + let mut max_move: Real = 0.0; + for (i, p) in self.particles.iter_mut().enumerate() { + if count[i] > 0.0 { + let target = sum[i] / count[i]; + max_move = max_move.max((target - p.rest_position).length()); + p.rest_position = target; + } + } + if max_move <= tolerance { + settled = true; + break; + } + } + // The rest positions stay relative to the rest center of mass. + let mut com = Vector::ZERO; + let mut mass = 0.0; + for p in &self.particles { + com += p.rest_position * p.mass; + mass += p.mass; + } + if mass > 0.0 { + com /= mass; + for p in &mut self.particles { + p.rest_position -= com; + } + } + settled + } + + /// Resets every plastic flow (edge rest lengths, dihedral rest angles, cell rest shapes, + /// particle rest positions) to the creation state; the particles stay put and spring back + /// elastically from there. + pub fn reset_plasticity(&mut self) { + for p in &mut self.particles { + p.rest_position = p.initial_rest_position; + } + for e in &mut self.edges { + e.rest_length = e.initial_rest_length(); + e.plastic_strain = 0.0; + } + #[cfg(feature = "dim3")] + for d in &mut self.dihedrals { + d.rest_angle = d.initial_rest_angle(); + d.plastic_set = 0.0; + } + for c in &mut self.cells { + let rest0: [Vector; DIM + 1] = core::array::from_fn(|k| { + self.particles[c.vertices[k] as usize].initial_rest_position + }); + let volume = Self::cell_volume(rest0); + c.plastic_stretch = Matrix::IDENTITY; + c.rest_volume = volume; + c.inv_rest_matrix = if volume.abs() > Real::EPSILON { + Self::cell_edge_matrix(rest0).inverse() + } else { + Matrix::ZERO + }; + } + self.rest_fit_pending = false; + self.modified = true; + } +} 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 7d339e4c5..ec3b453ce 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 @@ -55,6 +55,7 @@ impl SoftBodySet { has_overflow_color: false, modified: false, plastic_flowing: false, + rest_fit_pending: false, sleep_speed: 0.0, tearing_pending: false, topology_version: 0, 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 0ad661e1e..5cb19020d 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 @@ -159,6 +159,7 @@ pub(super) fn spawn_proxy( let rb = RigidBodyBuilder::dynamic() .gravity_scale(0.0) .additional_solver_iterations(settings.additional_solver_iterations) + .additional_pgs_iterations(settings.additional_pgs_iterations) .can_sleep(settings.can_sleep) .dominance_group(settings.dominance_group) .user_data(user_data) @@ -214,6 +215,11 @@ pub(super) fn sync_soft_body( let plastic_flowing = core::mem::take(&mut sb.plastic_flowing); if plastic_flowing { sb.sleep_speed = Real::MAX; + sb.rest_fit_pending = true; + } + // The rest positions follow the flow, over the steps it takes them to settle. + if sb.rest_fit_pending && !sleeping && sb.is_finite() && sb.fit_rest_positions() { + sb.rest_fit_pending = false; } let mut outcome = SyncOutcome { margin: None, 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 e1f2610f5..4010e476c 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 @@ -238,6 +238,7 @@ impl SoftBodySet { continue; }; rb.additional_solver_iterations = outcome.additional_solver_iterations; + rb.additional_pgs_iterations = sb.particle_settings.additional_pgs_iterations; // The cluster proxies first (pose, velocity, reduced mass): the colliders are // expressed in the fresh whole-body frame. Self::update_cluster_proxies(sb, bodies, colliders); diff --git a/src/dynamics/soft_body/tearing/tearing_cut.rs b/src/dynamics/soft_body/tearing/tearing_cut.rs index 110cb8f56..92da82959 100644 --- a/src/dynamics/soft_body/tearing/tearing_cut.rs +++ b/src/dynamics/soft_body/tearing/tearing_cut.rs @@ -319,6 +319,7 @@ impl SoftBody { position: pa.position.lerp(pb.position, t), velocity: pa.velocity.lerp(pb.velocity, t), rest_position: pa.rest_position.lerp(pb.rest_position, t), + initial_rest_position: pa.initial_rest_position.lerp(pb.initial_rest_position, t), mass, inv_mass: crate::utils::inv(mass), force: Vector::ZERO, diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs index e78d6ca9c..d55759137 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs @@ -5,9 +5,12 @@ #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use crate::math::{DIM, Matrix, Real}; +use crate::dynamics::SoftBody; +use crate::math::{DIM, Matrix, Real, Vector}; -use super::super::soft_element_constraint::{StrainVector, strain_matrix}; +use super::super::soft_element_constraint::{ + STRAIN_ROWS, StrainVector, max_symmetric_eigenvalue, strain_matrix, +}; /// Plastic flow of an elastic cell (see `SoftBodyMaterial::plastic_yield`): past the yield the rest /// shape flows toward the deviatoric stretch at the creep rate (`Dm' = S_p Dm`, unit determinant, @@ -15,14 +18,17 @@ use super::super::soft_element_constraint::{StrainVector, strain_matrix}; pub(crate) fn plastic_flow( cell: &mut crate::dynamics::SoftBodyCell, strain: &StrainVector, + inverted: bool, + rest0: &[Vector; DIM + 1], material: &crate::dynamics::SoftBodyMaterial, dt: Real, ) -> bool { - let diagonal = strain.fixed_rows::(0); - if diagonal.iter().any(|e| *e <= -0.9) { + if inverted { + return false; } let eps = strain_matrix(strain); - let trace = diagonal.sum(); + // The diagonal strain rows come first. + let trace = strain.fixed_rows::(0).sum(); let dev = eps - Matrix::IDENTITY * (trace / DIM as Real); let norm = frobenius_norm(&dev); if norm <= material.plastic_yield { @@ -51,18 +57,31 @@ pub(crate) fn plastic_flow( } let total = total * (1.0 / total_det.powf(1.0 / DIM as Real)); // The new rest edge matrix `P' Dm₀` and its signed volume (`det(Dm) / DIM!`). - cell.inv_rest_matrix = cell.inv_rest_matrix * cell.plastic_stretch * total.inverse(); + let dm = total * SoftBody::cell_edge_matrix(*rest0); + let factorial = if DIM == 2 { 2.0 } else { 6.0 }; + let volume = dm.determinant() / factorial; + if !volume.is_finite() || volume.abs() <= Real::EPSILON { + return false; + } + cell.inv_rest_matrix = dm.inverse(); + cell.rest_volume = volume; let increment = frobenius_norm(&(total - cell.plastic_stretch)); cell.plastic_stretch = total; - // Rest volume from the new rest matrix (signed, `det(Dm) / DIM!`). - let inv_det = cell.inv_rest_matrix.determinant(); - if inv_det != 0.0 { - let factorial = if DIM == 2 { 2.0 } else { 6.0 }; - cell.rest_volume = 1.0 / (inv_det * factorial); + for impulse in &mut cell.impulses[..STRAIN_ROWS] { + *impulse *= 1.0 - gamma; } increment > 1.0e-4 } +/// The largest tensile strain of a cell, plastic flow included: the largest principal value of the +/// symmetric part of the total stretch `(I + ε) P` minus one (the tearing criterion, so a creeping +/// cell tears at the strain an elastic one would); without plastic flow, that of `ε`. +pub(crate) fn total_tensile_strain(strain: &StrainVector, plastic_stretch: &Matrix) -> Real { + let stretch = (Matrix::IDENTITY + strain_matrix(strain)) * *plastic_stretch; + let sym = (stretch + stretch.transpose()) * 0.5 - Matrix::IDENTITY; + max_symmetric_eigenvalue(&sym) +} + /// Frobenius norm of a matrix. #[inline] fn frobenius_norm(m: &Matrix) -> Real { @@ -72,3 +91,112 @@ fn frobenius_norm(m: &Matrix) -> Real { } norm_sq.sqrt() } + +#[cfg(test)] +mod tests { + use super::*; + use crate::dynamics::{SoftBodyCell, SoftBodyMaterial}; + use crate::math::Rotation; + + /// A unit right simplex cell at rest. + fn unit_cell() -> (SoftBodyCell, [Vector; DIM + 1]) { + let mut rest0 = [Vector::ZERO; DIM + 1]; + for k in 0..DIM { + let mut axis = Vector::ZERO; + axis[k] = 1.0; + rest0[k + 1] = axis; + } + let volume = SoftBody::cell_volume(rest0); + let cell = SoftBodyCell { + vertices: core::array::from_fn(|k| k as u32), + rest_volume: volume, + inv_rest_matrix: SoftBody::cell_edge_matrix(rest0).inverse(), + plastic_stretch: Matrix::IDENTITY, + impulses: [1.0; STRAIN_ROWS + 1], + rotation: Rotation::IDENTITY, + stiffness_scale: 1.0, + tear_resistance: 1.0, + color: 0, + torn: false, + stress: 0.0, + }; + (cell, rest0) + } + + fn clay() -> SoftBodyMaterial { + SoftBodyMaterial { + plastic_yield: 0.05, + plastic_creep: Real::INFINITY, + plastic_max: 1.0, + ..Default::default() + } + } + + /// A shear past the yield flows the rest shape, keeps its volume and rebuilds the rest + /// matrix from the initial rest shape: the elastic strain left is the yield. + #[test] + fn shear_flows_and_keeps_the_rest_volume() { + let (mut cell, rest0) = unit_cell(); + let mut strain = StrainVector::zeros(); + // The first off-diagonal row: an `xy` shear of 0.2. + strain[DIM] = 0.2; + let flowed = plastic_flow(&mut cell, &strain, false, &rest0, &clay(), 1.0 / 60.0); + assert!(flowed); + assert!((cell.rest_volume - SoftBody::cell_volume(rest0)).abs() < 1.0e-5); + assert!((cell.plastic_stretch.determinant() - 1.0).abs() < 1.0e-5); + // `Dm = P Dm₀` exactly. + let dm = cell.plastic_stretch * SoftBody::cell_edge_matrix(rest0); + let err = frobenius_norm(&(cell.inv_rest_matrix * dm - Matrix::IDENTITY)); + assert!(err < 1.0e-5, "rest matrix drifted: {err}"); + // The strain rows' warm start followed the flow (full creep: nothing elastic left). + assert!(cell.impulses[..STRAIN_ROWS].iter().all(|i| i.abs() < 1.0e-6)); + assert_eq!(cell.impulses[STRAIN_ROWS], 1.0); + } + + /// An inverted cell never flows, whatever its strain rows look like. + #[test] + fn inverted_cells_do_not_flow() { + let (mut cell, rest0) = unit_cell(); + let before = cell; + // A reflection about an oblique plane: no diagonal strain reaches the old `-0.9` guard. + let mut strain = StrainVector::zeros(); + for k in 0..DIM { + strain[k] = -2.0 / DIM as Real; + } + for k in DIM..STRAIN_ROWS { + strain[k] = -2.0 / DIM as Real; + } + // A partial creep: the full stretch is the reflection itself, which the determinant + // guard rejects on its own; a fraction of it is a plain squash. + let material = SoftBodyMaterial { + plastic_creep: 1.0, + ..clay() + }; + assert!(!plastic_flow(&mut cell, &strain, true, &rest0, &material, 0.3)); + assert_eq!(cell.plastic_stretch, before.plastic_stretch); + assert_eq!(cell.inv_rest_matrix, before.inv_rest_matrix); + // The same rows on a cell that is not inverted flow, bounded by the plastic maximum. + let mut flowed = 0; + for _ in 0..200 { + flowed += plastic_flow(&mut cell, &strain, false, &rest0, &material, 0.3) as usize; + } + assert!(flowed > 0); + let deviation = frobenius_norm(&(cell.plastic_stretch - Matrix::IDENTITY)); + assert!(deviation <= 1.0 + 1.0e-3, "flow past the maximum: {deviation}"); + } + + /// The tear strain counts the plastic stretch: an unflowed cell reads its largest + /// principal strain, a flowed one its total stretch. + #[test] + fn total_tensile_strain_counts_the_flow() { + let mut strain = StrainVector::zeros(); + strain[0] = 0.3; + strain[1] = -0.1; + assert!((total_tensile_strain(&strain, &Matrix::IDENTITY) - 0.3).abs() < 1.0e-6); + let mut p = Matrix::IDENTITY; + p.x_axis[0] = 1.5; + p.y_axis[1] = 1.0 / 1.5; + assert!((total_tensile_strain(&StrainVector::zeros(), &p) - 0.5).abs() < 1.0e-6); + assert!((total_tensile_strain(&strain, &p) - (1.3 * 1.5 - 1.0)).abs() < 1.0e-6); + } +} 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 529ff46ad..57614db4d 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 @@ -7,8 +7,10 @@ use core::sync::atomic::Ordering; use simba::scalar::{ComplexField as _, RealField as _}; use crate::dynamics::solver::solver_body::SolverBodies; -use crate::math::{Real, Vector}; +use crate::math::{DIM, Real, Vector}; +#[cfg(feature = "dim3")] +use super::super::soft_element_constraint::dihedral_gradients; use super::super::soft_element_constraint::SoftScalarConstraintWriteback; use super::*; @@ -59,7 +61,15 @@ impl SoftConstraintsSet { } #[cfg(feature = "dim3")] SoftScalarConstraintWriteback::Dihedral => { - sb.dihedrals[constraint.element as usize].impulse = impulse + let dihedral = &mut sb.dihedrals[constraint.element as usize]; + dihedral.impulse = impulse; + // Angle as seen at the last substep's update. + let mut grad = [Vector::ZERO; 4]; + let angle = dihedral.rest_angle + + dihedral_gradients(&constraint.pos, dihedral.rest_angle, &mut grad); + if dihedral.plastic_flow(angle, &material, dt) { + awake.plastic_flow.store(true, Ordering::Relaxed); + } } SoftScalarConstraintWriteback::CellVolume => { sb.cells[constraint.element as usize].impulses[0] = impulse @@ -83,11 +93,28 @@ impl SoftConstraintsSet { *dst = crate::utils::canonicalize_zero(*src); } cell.rotation = constraint.rotation; - if material.plastic_yield > 0.0 - && material.plastic_creep > 0.0 - && plastic_flow(cell, &constraint.strain, &material, dt) - { - awake.plastic_flow.store(true, Ordering::Relaxed); + // The tear strain counts the plastic stretch, as it stood before this step's flow. + let tensile = total_tensile_strain(&constraint.strain, &cell.plastic_stretch); + if material.plastic_yield > 0.0 && material.plastic_creep > 0.0 { + let rest0: [Vector; DIM + 1] = core::array::from_fn(|k| { + sb.particles[cell.vertices[k] as usize].initial_rest_position + }); + // The flow reads the strain the cell carries as stress (its last substep's + // impulses over the row stiffness), not the geometric strain: the sweep's + // unconverged residual is geometric but transmits no load. + let stress_strain = constraint + .strain_impulse + .component_mul(&constraint.strain_per_impulse); + if plastic_flow( + cell, + &stress_strain, + constraint.inverted, + &rest0, + &material, + dt, + ) { + awake.plastic_flow.store(true, Ordering::Relaxed); + } } if material.tear_strain.is_some() { let load = material.cell_tear_load(tensile) * crate::utils::inv(resistance); diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs index 76d42e66f..95327d780 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs @@ -156,21 +156,27 @@ pub(crate) struct SoftElasticConstraint { pub strain_cap: StrainVector, /// Strain of the last update (cell frame, clamped). pub strain: StrainVector, + /// Whether the cell was inverted (`det F < 0`) at the last update. + pub inverted: bool, /// Strain impulses (cell frame). pub strain_impulse: StrainVector, + /// Elastic strain per unit of strain-row impulse, `1 / (k_r dt)` (`k_r` = `2μV₀` diagonal, + /// `4μV₀` shear; zero for an inert row): `strain_impulse ∘ strain_per_impulse` is the strain + /// carried as stress, which the plastic flow reads instead of the unconverged geometric strain. + pub strain_per_impulse: StrainVector, /// Volumetric impulse (corotational only). pub vol_impulse: Real, pub model: SoftElasticModel, } -/// The constitutive model of a [`SoftElasticRow`] and its model-specific state. +/// The constitutive model of a [`SoftElasticConstraint`] and its model-specific state. #[derive(Copy, Clone, Debug)] pub(crate) enum SoftElasticModel { Corotational(CorotationalConstraint), NeoHookean(NeoHookeanConstraint), } -/// Corotational state of a [`SoftElasticRow`]: constant per-row spring coefficients and the +/// Corotational state of a [`SoftElasticConstraint`]: constant per-constraint spring coefficients and the /// volumetric row solved through a Schur complement of the strain block. #[derive(Copy, Clone, Debug)] pub(crate) struct CorotationalConstraint { @@ -200,10 +206,6 @@ impl SoftElasticConstraint { self.strain.iter().any(|s| s.abs() > min_strain) } - pub fn max_tensile_strain(&self) -> Real { - max_tensile_strain(&self.strain) - } - /// Deviatoric coefficients of a cell from its inverse rest matrix. pub fn coefficients(inv_rest_matrix: &Matrix) -> [Vector; MAX_CONSTRAINT_PARTICLES] { let constraints = inv_rest_matrix.transpose(); @@ -298,6 +300,7 @@ impl SoftElasticConstraint { // substeps; a cell torn far past rest has its strain error clamped at 100% (or the same cap // when larger) to keep the bias continuous on un-inversion; ordinary strains are unclamped. let inverted = f.determinant() < 0.0; + self.inverted = inverted; let caps = if inverted { self.strain_cap } else { diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs index 464ab8658..b2f564049 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs @@ -107,7 +107,9 @@ fn elastic_cell_gradients_match_finite_differences() { inv_a: StrainMatrix::zeros(), strain_cap: StrainVector::repeat(Real::MAX), strain: StrainVector::zeros(), + inverted: false, strain_impulse: StrainVector::zeros(), + strain_per_impulse: StrainVector::zeros(), vol_impulse: 0.0, model: SoftElasticModel::Corotational(CorotationalConstraint { erp_strain: StrainVector::zeros(), diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs index 86e9f082b..d7854eb9b 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs @@ -756,17 +756,20 @@ impl SoftConstraintsSet { let mut softened = block; let mut inert = [false; STRAIN_ROWS]; let mut damping = StrainVector::zeros(); + let mut strain_per_impulse = StrainVector::zeros(); for r in 0..STRAIN_ROWS { let w = block[(r, r)]; + // The row's deviatoric stiffness: the stress-based strain of both models + // reads through it (an isotropic material's deviatoric response is `2μ`). + let k_dev = if r < DIM { 2.0 * mu * vol } else { 4.0 * mu * vol }; let k = if neo_hookean { NeoHookeanConstraint::rest_stiffness(mu, lambda, r) * vol - } else if r < DIM { - 2.0 * mu * vol } else { - 4.0 * mu * vol + k_dev }; let omega = (k * w).sqrt(); if omega > 0.0 { + strain_per_impulse[r] = 1.0 / (k_dev * dt); let (erp, cfm) = coeffs_of(&SpringCoefficients::new(omega / two_pi, elastic_zeta)); erp_strain[r] = erp; @@ -830,7 +833,9 @@ impl SoftConstraintsSet { inv_a, strain_cap: strain_caps, strain: StrainVector::zeros(), + inverted: false, strain_impulse, + strain_per_impulse, vol_impulse, model, }; diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs index 4004b7b7d..6bc923640 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs @@ -4,7 +4,9 @@ use super::SoftFemSystem; use crate::alloc_prelude::*; use crate::dynamics::SoftBody; use crate::dynamics::soft_body::soft_body_shape_matching::extract_rotation; -use crate::dynamics::solver::soft_constraint::soft_constraints_set::plastic_flow; +use crate::dynamics::solver::soft_constraint::soft_constraints_set::{ + plastic_flow, total_tensile_strain, +}; #[cfg(feature = "dim3")] use crate::dynamics::solver::soft_constraint::soft_element_constraint::dihedral_gradients; use crate::dynamics::solver::soft_constraint::soft_element_constraint::{ @@ -47,6 +49,7 @@ impl SoftFemSystem { let x: [Vector; MAX_CONSTRAINT_PARTICLES] = core::array::from_fn(|k| self.position[cell.vertices[k] as usize]); let f = SoftBody::cell_edge_matrix(x) * cell.inv_rest_matrix; + cell.inverted = f.determinant() < 0.0; cell.rotation = extract_rotation(f, cell.rotation); let s = cell.rotation.inverse().to_mat() * f; cell.strain = strain_rows_of(&(s - Matrix::IDENTITY)); @@ -295,8 +298,15 @@ impl SoftFemSystem { if cell.mu <= 0.0 { continue; } - if plastic && plastic_flow(out, &cell.strain, &material, step_dt) { - flowing = true; + // The tear strain counts the plastic stretch, as it stood before this step's flow. + let tensile = total_tensile_strain(&cell.strain, &out.plastic_stretch); + if plastic { + let rest0: [Vector; MAX_CONSTRAINT_PARTICLES] = core::array::from_fn(|k| { + sb.particles[out.vertices[k] as usize].initial_rest_position + }); + if plastic_flow(out, &cell.strain, cell.inverted, &rest0, &material, step_dt) { + flowing = true; + } } if material.tear_strain.is_some() { let load = @@ -309,6 +319,18 @@ impl SoftFemSystem { } } let edge_plasticity = material.edge_plastic_yield > 0.0 && material.edge_plastic_creep > 0.0; + #[cfg(feature = "dim3")] + if edge_plasticity { + for (dihedral, d) in self.dihedrals.iter().zip(sb.dihedrals.iter_mut()) { + let pos: [Vector; 4] = + core::array::from_fn(|k| self.position[dihedral.vertices[k] as usize]); + let mut grad = [Vector::ZERO; 4]; + let angle = d.rest_angle + dihedral_gradients(&pos, d.rest_angle, &mut grad); + if d.plastic_flow(angle, &material, step_dt) { + flowing = true; + } + } + } if material.tears() || edge_plasticity { for ((spring, edge), ei) in self.springs.iter().zip(sb.edges.iter_mut()).zip(0..) { if spring.rest_length <= 0.0 { diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs index 404685ea9..2926d8817 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_elements.rs @@ -27,23 +27,23 @@ pub(super) struct FemCell { /// Polar rotation of the deformation gradient, frozen over the substep and warm-started /// from the previous one. pub(super) rotation: Rotation, - /// Corotated strain `sym(RᵀF) - I` in the strain-row coordinates. + /// Corotated strain `sym(RᵀF) - I` in the strain-constraint coordinates. pub(super) strain: StrainVector, - /// Whether the cell carries the stable Neo-Hookean energy instead of the linear-elastic + /// Whether the cell was inverted (`det F < 0`) at the last assembly. + pub(super) inverted: bool, + /// Whether the cell uses the stable Neo-Hookean energy instead of the linear-elastic /// (corotational) one. pub(super) neo_hookean: bool, /// The tangent `V₀ ∂²Ψ/∂ε²` last computed, and the strain it was computed at /// (`Real::MAX`: never). Constant for the corotational model; the Neo-Hookean one - /// re-linearizes when the strain moved by more than `STIFFNESS_REFRESH_STRAIN`. + /// re-linearizes when the strain moved by more than `STIFFNESS_UPDATE_STRAIN`. pub(super) tangent: StrainMatrix, pub(super) tangent_strain: StrainVector, } -/// One volume element of a FEM soft body with the [`SoftBodyCellModel::Volume`] cell model: -/// energy `½ k (V − V₀)²` over the cell's signed area/volume, Gauss-Newton tangent -/// `k ∇V ∇Vᵀ` (the indefinite `(V − V₀) ∂²V` term dropped, like the springs). Its stiffness -/// comes from the material's `volume_softness` the way the row path's cell-volume row does: -/// `k = ω² / w₀`, `w₀` the row's effective mass at rest. +/// One volume element of a FEM soft body with the [`SoftBodyCellModel::Volume`] cell model: energy +/// `½ k (V − V₀)²` over the signed area/volume, Gauss-Newton tangent `k ∇V ∇Vᵀ` (indefinite term +/// dropped); `k = ω² / w₀` from `volume_softness`, `w₀` the constraint's rest effective mass. #[derive(Copy, Clone, Debug)] pub(super) struct FemVolumeCell { /// Index of the cell in the body (its vertices and cached block slots). diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs index 9b35195c5..70864af7e 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs @@ -79,6 +79,7 @@ impl SoftFemSystem { beta, rotation: c.rotation, strain: StrainVector::zeros(), + inverted: false, neo_hookean: sb.cell_model == SoftBodyCellModel::NeoHookean, tangent: Self::corotational_hessian(mu * volume, lambda * volume), tangent_strain: StrainVector::repeat(Real::MAX), diff --git a/src/dynamics/solver/staged_island_solver/init.rs b/src/dynamics/solver/staged_island_solver/init.rs index a64ab9eca..f464c3cf3 100644 --- a/src/dynamics/solver/staged_island_solver/init.rs +++ b/src/dynamics/solver/staged_island_solver/init.rs @@ -65,10 +65,18 @@ impl StagedIslandSolver { .first() .map(|g| g.extra_iters as usize) .unwrap_or(0); + let max_extra_pgs = islands + .solve_groups + .iter() + .map(|g| g.extra_pgs as usize) + .max() + .unwrap_or(0); self.groups.clear(); if multi_group { for group in &islands.solve_groups { let num_substeps = base_params.num_solver_iterations + group.extra_iters as usize; + let num_pgs_iterations = + base_params.num_internal_pgs_iterations + group.extra_pgs as usize; self.groups.push(GroupLayout { bodies: group.body_range.clone(), soft_slots: 0..0, @@ -80,11 +88,13 @@ impl StagedIslandSolver { joint_builders: 0..0, joint_overflow: 0..0, num_substeps, + num_pgs_iterations, dt: base_params.dt / num_substeps as Real, }); } } else { let num_substeps = base_params.num_solver_iterations + max_extra; + let num_pgs_iterations = base_params.num_internal_pgs_iterations + max_extra_pgs; self.groups.push(GroupLayout { bodies: 0..island_bodies.len(), soft_slots: 0..0, @@ -96,6 +106,7 @@ impl StagedIslandSolver { joint_builders: 0..0, joint_overflow: 0..0, num_substeps, + num_pgs_iterations, dt: base_params.dt / num_substeps as Real, }); } diff --git a/src/dynamics/solver/staged_island_solver/mod.rs b/src/dynamics/solver/staged_island_solver/mod.rs index b8cf873b5..193e5d443 100644 --- a/src/dynamics/solver/staged_island_solver/mod.rs +++ b/src/dynamics/solver/staged_island_solver/mod.rs @@ -138,6 +138,9 @@ struct GroupLayout { joint_overflow: Range, /// Substeps to run for this group. num_substeps: usize, + /// Internal PGS iterations per substep for this group (`num_internal_pgs_iterations` plus the + /// group's `additional_pgs_iterations`). + num_pgs_iterations: usize, /// This group's substep length (`base_dt / num_substeps`). dt: Real, } diff --git a/src/dynamics/solver/staged_island_solver/worker.rs b/src/dynamics/solver/staged_island_solver/worker.rs index 4b146a0ea..f38c2e542 100644 --- a/src/dynamics/solver/staged_island_solver/worker.rs +++ b/src/dynamics/solver/staged_island_solver/worker.rs @@ -609,12 +609,14 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { } /* - * Stages: solve with bias. + * Stages: solve with bias. The group's count includes the extra iterations its bodies + * request (`RigidBody::additional_pgs_iterations`, 3 by default for soft bodies), so + * the substep's constraints converge against each other at the same positions. */ - for pgs_iter in 0..params.num_internal_pgs_iterations { - // Joint warm-starting is fused into the first biased pass: each claimed - // joint applies its carried impulse right before being solved (sound - // within a color; Gauss-Seidel-ordered across colors). + for pgs_iter in 0..group.num_pgs_iterations { + // Joint warm-starting is fused into the first biased pass: each claimed joint + // applies its warm impulse right before being solved. Soft-body constraints do + // the same and update their geometry on the first iteration of the pass. let warmstart_joints = params.warmstart_joints && pgs_iter == 0; let soft_warmstart = (pgs_iter == 0 && params.warmstart_coefficient != 0.0) .then_some(params.warmstart_coefficient); diff --git a/src/pipeline/physics_pipeline/solve.rs b/src/pipeline/physics_pipeline/solve.rs index b90a18296..838a4cedd 100644 --- a/src/pipeline/physics_pipeline/solve.rs +++ b/src/pipeline/physics_pipeline/solve.rs @@ -249,7 +249,8 @@ impl PhysicsPipeline { let effective_mass = rb.mprops.effective_mass(); rb.forces .compute_effective_force_and_torque(gravity, effective_mass); - any_extra_iterations |= rb.additional_solver_iterations() > 0; + any_extra_iterations |= + rb.additional_solver_iterations() > 0 || rb.additional_pgs_iterations() > 0; bid(rb, &islands.persistent, &mut split_bid); observe(rb, observations); } @@ -280,7 +281,8 @@ impl PhysicsPipeline { let effective_mass = rb.mprops.effective_mass(); rb.forces .compute_effective_force_and_torque(gravity, effective_mass); - any_extra |= rb.additional_solver_iterations() > 0; + any_extra |= rb.additional_solver_iterations() > 0 + || rb.additional_pgs_iterations() > 0; bid(rb, persistent, &mut chunk_bid); observe(rb, &mut observations); } diff --git a/src_testbed/save.rs b/src_testbed/save.rs index 0688ddaef..b0ef344e5 100644 --- a/src_testbed/save.rs +++ b/src_testbed/save.rs @@ -10,6 +10,7 @@ pub struct SerializableTestbedState { pub selected_example: usize, pub example_settings: ExampleSettings, pub soft_recovery: rapier::dynamics::SoftRecoverySettings, + pub soft_additional_pgs: usize, pub camera: Camera, } @@ -21,6 +22,7 @@ impl TestbedState { selected_example: self.selected_display_index, example_settings: self.example_settings.clone(), soft_recovery: self.soft_recovery, + soft_additional_pgs: self.soft_additional_pgs, camera, } } @@ -32,6 +34,7 @@ impl TestbedState { self.selected_display_index = state.selected_example; self.example_settings = state.example_settings; self.soft_recovery = state.soft_recovery; + self.soft_additional_pgs = state.soft_additional_pgs; *camera = state.camera; } } diff --git a/src_testbed/testbed/state.rs b/src_testbed/testbed/state.rs index 500fd5e0a..718d89a11 100644 --- a/src_testbed/testbed/state.rs +++ b/src_testbed/testbed/state.rs @@ -170,6 +170,8 @@ pub struct TestbedState { /// and stamped onto the running world every frame, so the panel's choices survive /// demo restarts and switches. pub soft_recovery: rapier::dynamics::SoftRecoverySettings, + /// Extra PGS iterations the solver panel's slider applies to every soft body when moved. + pub soft_additional_pgs: usize, pub broad_phase_type: RapierBroadPhaseType, pub snapshot: Option, /// Number of physics steps run since the example was (re)started. Bumped by @@ -211,6 +213,7 @@ impl Default for TestbedState { example_groups: Vec::new(), example_settings: ExampleSettings::default(), soft_recovery: Default::default(), + soft_additional_pgs: 3, selected_display_index: 0, broad_phase_type: RapierBroadPhaseType::default(), camera_locked: false, diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index e910df52e..d22bfeee4 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -425,6 +425,24 @@ fn settings_tab(ui: &mut Ui, state: &mut TestbedState, world: &mut PhysicsWorld) ) .on_hover_text("Internal Projected Gauss-Seidel iterations."); + // Applied to every soft body of the world when moved, so a demo's own per-body + // choices stand until the user touches the slider. + if ui + .add(Slider::new(&mut state.soft_additional_pgs, 0..=15).text("Soft extra PGS iters")) + .on_hover_text( + "Extra internal PGS iterations per substep for the island components holding \ + a soft body (applied to every soft body when moved). Converges the elastic \ + rows and the contacts together: stiff or slender soft bodies get closer to \ + their nominal stiffness, resting bodies lose their residual roughness, cloth \ + stretches less and plastic flow reads the stress it should.", + ) + .changed() + { + for (_, sb) in world.soft_bodies.iter_mut() { + sb.set_additional_pgs_iterations(state.soft_additional_pgs); + } + } + ui.add( Slider::new( &mut integration_parameters.num_internal_stabilization_iterations, From 4c7b3f0dcbeb0361ece39be52ff8db1734cba871 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 17 Sep 2026 16:42:52 +0200 Subject: [PATCH 20/32] feat(testbed): soft-body stress scenes, cloth on a Keva tower, translucent bins and lighting tweaks --- examples2d/all_examples2.rs | 7 ++ examples2d/soft_blobs2.rs | 22 ++-- examples2d/stress_tests/mod.rs | 7 ++ examples2d/stress_tests/soft_blobs2.rs | 67 +++++++++++ examples2d/stress_tests/soft_cloth_keva2.rs | 78 ++++++++++++ examples2d/stress_tests/soft_fem_beams2.rs | 74 ++++++++++++ examples2d/stress_tests/soft_jellies2.rs | 71 +++++++++++ examples2d/stress_tests/soft_ropes2.rs | 61 ++++++++++ examples2d/stress_tests/soft_slab2.rs | 85 +++++++++++++ examples2d/stress_tests/soft_strips2.rs | 72 +++++++++++ examples3d/all_examples3.rs | 7 ++ examples3d/stress_tests/mod.rs | 7 ++ examples3d/stress_tests/soft_blobs3.rs | 106 ++++++++++++++++ examples3d/stress_tests/soft_cloth_drape3.rs | 85 +++++++++++++ examples3d/stress_tests/soft_cloth_keva3.rs | 87 ++++++++++++++ examples3d/stress_tests/soft_fem_beams3.rs | 75 ++++++++++++ examples3d/stress_tests/soft_jellies3.rs | 98 +++++++++++++++ examples3d/stress_tests/soft_ropes3.rs | 100 ++++++++++++++++ examples3d/stress_tests/soft_slab3.rs | 120 +++++++++++++++++++ src_testbed/graphics.rs | 58 ++++++++- src_testbed/graphics/graphics_soft_bodies.rs | 21 +++- 21 files changed, 1290 insertions(+), 18 deletions(-) create mode 100644 examples2d/stress_tests/soft_blobs2.rs create mode 100644 examples2d/stress_tests/soft_cloth_keva2.rs create mode 100644 examples2d/stress_tests/soft_fem_beams2.rs create mode 100644 examples2d/stress_tests/soft_jellies2.rs create mode 100644 examples2d/stress_tests/soft_ropes2.rs create mode 100644 examples2d/stress_tests/soft_slab2.rs create mode 100644 examples2d/stress_tests/soft_strips2.rs create mode 100644 examples3d/stress_tests/soft_blobs3.rs create mode 100644 examples3d/stress_tests/soft_cloth_drape3.rs create mode 100644 examples3d/stress_tests/soft_cloth_keva3.rs create mode 100644 examples3d/stress_tests/soft_fem_beams3.rs create mode 100644 examples3d/stress_tests/soft_jellies3.rs create mode 100644 examples3d/stress_tests/soft_ropes3.rs create mode 100644 examples3d/stress_tests/soft_slab3.rs diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index f1835a25a..b98674ebf 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -205,6 +205,13 @@ pub async fn main() { STRESS, "(Stress test) joint ball", stress_tests::joint_ball2::run; STRESS, "(Stress test) joint fixed", stress_tests::joint_fixed2::run; STRESS, "(Stress test) joint prismatic", stress_tests::joint_prismatic2::run; + STRESS, "Soft blobs", stress_tests::soft_blobs2::run; + STRESS, "Soft jellies", stress_tests::soft_jellies2::run; + STRESS, "Soft ropes", stress_tests::soft_ropes2::run; + STRESS, "Soft strips", stress_tests::soft_strips2::run; + STRESS, "Soft cloth on Keva tower", stress_tests::soft_cloth_keva2::run; + STRESS, "Soft slab shower", stress_tests::soft_slab2::run; + STRESS, "Soft FEM beams", stress_tests::soft_fem_beams2::run; ]; let (entries, run_fns): (Vec<_>, Vec) = examples.into_iter().unzip(); diff --git a/examples2d/soft_blobs2.rs b/examples2d/soft_blobs2.rs index 012f6e0a4..ec9122f46 100644 --- a/examples2d/soft_blobs2.rs +++ b/examples2d/soft_blobs2.rs @@ -12,30 +12,30 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { */ world.insert( RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), - ColliderBuilder::cuboid(20.0, 0.5), + ColliderBuilder::cuboid(6.0, 0.5), ); world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(-20.0, 30.0)), - ColliderBuilder::cuboid(0.5, 30.0), + RigidBodyBuilder::fixed().translation(Vector::new(-6.0, 6.0)), + ColliderBuilder::cuboid(0.5, 6.0), ); world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(20.0, 30.0)), - ColliderBuilder::cuboid(0.5, 30.0), + RigidBodyBuilder::fixed().translation(Vector::new(6.0, 6.0)), + ColliderBuilder::cuboid(0.5, 6.0), ); /* * Blobs. */ let mut k = 0; - for j in 0..60 { - for i in 0..19 { + for j in 0..15 { + for i in 0..5 { let radius = 0.45 + 0.1 * ((i + j) % 3) as Real; - let x = -19.0 + i as Real * 2.0 + (j % 2) as Real * 0.5; + let x = -4.0 + i as Real * 2.0 + (j % 2) as Real * 0.5; let y = 3.0 + j as Real * 2.0; // Self-contacts keep a hard squeeze from pushing the loop through itself (a // crossed loop is a degenerate pressure vessel that stays crushed under the pile). let blob = SoftBodyBuilder::disk(Vector::new(x, y), radius, 20) - .softness(SpringCoefficients::new(60.0, 3.0)) + .softness(SpringCoefficients::new(20.0, 1.0)) .volume_factor(1.05) .self_contacts(true) .particle_mass(0.05); @@ -49,9 +49,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { * A long floppy strip with self-contacts, dropped on the pile. */ let strip = SoftBodyBuilder::grid(Vector::new(0.0, 16.0), Vector::new(3.0, 0.15), 40, 3) - .softness(SpringCoefficients::new(120.0, 1.0)) + .softness(SpringCoefficients::new(30.0, 1.0)) .self_contacts(true) - .particle_mass(0.1); + .particle_mass(0.02); world.insert_soft_body(strip); viewer.set_world(&mut world); diff --git a/examples2d/stress_tests/mod.rs b/examples2d/stress_tests/mod.rs index bfc501925..72a2f852f 100644 --- a/examples2d/stress_tests/mod.rs +++ b/examples2d/stress_tests/mod.rs @@ -13,4 +13,11 @@ pub mod many_pyramids2; pub mod pyramid2; pub mod ragdolls2; pub mod ropes2; +pub mod soft_blobs2; +pub mod soft_cloth_keva2; +pub mod soft_fem_beams2; +pub mod soft_jellies2; +pub mod soft_ropes2; +pub mod soft_slab2; +pub mod soft_strips2; pub mod vertical_stacks2; diff --git a/examples2d/stress_tests/soft_blobs2.rs b/examples2d/stress_tests/soft_blobs2.rs new file mode 100644 index 000000000..2235c8cae --- /dev/null +++ b/examples2d/stress_tests/soft_blobs2.rs @@ -0,0 +1,67 @@ +//! Stress test: more than a thousand pressurized blobs poured into a tall container, each with +//! self-contacts so a hard squeeze cannot push its loop through itself, and a long floppy strip +//! dropped onto the pile. Soft-vs-soft contacts through deformable polylines, under load. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + /* + * Container. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(20.0, 0.5), + ); + for x in [-20.0, 20.0] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 30.0)), + ColliderBuilder::cuboid(0.5, 30.0), + ); + } + + /* + * Blobs. + */ + for j in 0..60 { + for i in 0..19 { + let radius = 0.45 + 0.1 * ((i + j) % 3) as Real; + let x = -19.0 + i as Real * 2.0 + (j % 2) as Real * 0.5; + let y = 3.0 + j as Real * 2.0; + let blob = SoftBodyBuilder::disk(Vector::new(x, y), radius, 20) + .softness(SpringCoefficients::new(60.0, 3.0)) + .volume_factor(1.05) + .self_contacts(true) + .particle_mass(0.05); + world.insert_soft_body(blob); + } + } + + /* + * A long floppy strip with self-contacts, dropped on the pile. + */ + let strip = SoftBodyBuilder::grid(Vector::new(0.0, 16.0), Vector::new(3.0, 0.15), 40, 3) + .softness(SpringCoefficients::new(120.0, 1.0)) + .self_contacts(true) + .particle_mass(0.1); + world.insert_soft_body(strip); + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 6.0), 30.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/stress_tests/soft_cloth_keva2.rs b/examples2d/stress_tests/soft_cloth_keva2.rs new file mode 100644 index 000000000..5dbfbc780 --- /dev/null +++ b/examples2d/stress_tests/soft_cloth_keva2.rs @@ -0,0 +1,78 @@ +//! Stress test: a big rectangle of cloth (a long thin elastic strip with self-contacts) dropped +//! flat onto a tapered Keva-style tower of planks, which it drapes over and topples. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(40.0, 0.5), + ); + + /* + * The tower: pairs of layers, standing planks bridged by flat ones, one span narrower + * per pair. + */ + let (thickness, length) = (0.1, 1.0); + let spans = 20usize; + let pair_height = 2.0 * (length + thickness); + let mut y = 0.0; + for pair in 0..spans - 2 { + let n = spans - pair; + let x0 = -(n as Real) * length; + for k in 0..=n { + world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(x0 + k as Real * 2.0 * length, y + length)), + ColliderBuilder::cuboid(thickness, length), + ); + } + for k in 0..n { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new( + x0 + (k as Real * 2.0 + 1.0) * length, + y + 2.0 * length + thickness, + )), + ColliderBuilder::cuboid(length, thickness), + ); + } + y += pair_height; + } + + /* + * The cloth, centered on the tower. + */ + let cloth = SoftBodyBuilder::grid(Vector::new(0.0, y + 6.0), Vector::new(24.0, 0.1), 241, 2) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .self_contacts(true) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)); + world.insert_soft_body(cloth); + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 20.0), 12.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/stress_tests/soft_fem_beams2.rs b/examples2d/stress_tests/soft_fem_beams2.rs new file mode 100644 index 000000000..69ec11290 --- /dev/null +++ b/examples2d/stress_tests/soft_fem_beams2.rs @@ -0,0 +1,74 @@ +//! Stress test for the FEM soft-body solver: a grid of fifty stiff cantilevers, each solved by +//! [`SoftBodySolver::Fem`] (one factorization per step and per body) and loaded by a heavy box +//! dropped on its free end. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(40.0, 0.5), + ); + + /* + * The cantilevers, bolted at their left end, stiffer on the upper rows. + */ + let (length, thickness) = (4.0, 0.4); + for row in 0..5 { + for col in 0..10 { + let x0 = -30.0 + col as Real * 6.0; + let y = 3.0 + row as Real * 5.0; + let beam = SoftBodyBuilder::grid( + Vector::new(x0 + length * 0.5, y), + Vector::new(length * 0.5, thickness * 0.5), + 33, + 5, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e6 * (1.0 + row as Real), + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .mass(8.0) + .solver(SoftBodySolver::Fem); + let pinned: Vec = beam + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x < x0 + 1.0e-4) + .map(|(i, _)| i as u32) + .collect(); + world.insert_soft_body(beam.pinned_particles(pinned)); + + /* + * The load, dropped on the free end. + */ + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(x0 + length - 0.5, y + 2.0)), + ColliderBuilder::cuboid(0.4, 0.4).density(20.0), + ); + } + } + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 12.0), 15.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/stress_tests/soft_jellies2.rs b/examples2d/stress_tests/soft_jellies2.rs new file mode 100644 index 000000000..17ec9b006 --- /dev/null +++ b/examples2d/stress_tests/soft_jellies2.rs @@ -0,0 +1,71 @@ +//! Stress test: hundreds of elastic jelly squares (corotational and Neo-Hookean cells, assorted +//! stiffnesses) poured into a bin, colliding through their deformable boundaries. Mostly +//! soft-vs-soft contacts against elastic cells under the weight of the pile. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + /* + * Bin. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(16.0, 0.5), + ); + for x in [-16.0, 16.0] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 30.0)), + ColliderBuilder::cuboid(0.5, 30.0), + ); + } + + /* + * Layers of jelly squares: every fifth stiffness step, every third one Neo-Hookean. + */ + let mut k = 0usize; + for layer in 0..40 { + for i in 0..12 { + let x = -14.0 + i as Real * 2.5 + (layer % 2) as Real * 0.8; + let y = 2.0 + layer as Real * 2.4; + let young = 2.0e3 * (1.0 + (k % 5) as Real * 3.0); + let model = if k.is_multiple_of(3) { + SoftBodyCellModel::NeoHookean + } else { + SoftBodyCellModel::Corotational + }; + let square = SoftBodyBuilder::grid(Vector::new(x, y), Vector::splat(0.6), 5, 5) + .cell_model(model) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.08) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)); + world.insert_soft_body(square); + k += 1; + } + } + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 8.0), 25.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/stress_tests/soft_ropes2.rs b/examples2d/stress_tests/soft_ropes2.rs new file mode 100644 index 000000000..434c48281 --- /dev/null +++ b/examples2d/stress_tests/soft_ropes2.rs @@ -0,0 +1,61 @@ +//! Stress test: a rain of soft ropes (structural and bending edges, colliding as deformable +//! polylines) poured over a row of pins into a bin, where they tangle into a heap. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + /* + * Bin, with pins across it. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(10.0, 0.5), + ); + for x in [-10.0, 10.0] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 5.0)), + ColliderBuilder::cuboid(0.5, 10.0), + ); + } + for i in 0..5 { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(-6.0 + i as Real * 3.0, 5.0)), + ColliderBuilder::capsule_x(0.6, 0.1), + ); + } + + /* + * Layers of ropes, slightly tilted so they slide off each other. + */ + for layer in 0..80 { + for i in 0..4 { + let x0 = -8.6 + i as Real * 4.4 + (layer % 2) as Real * 0.6; + let y = 8.0 + layer as Real * 1.2; + let rope = + SoftBodyBuilder::rope(Vector::new(x0, y), Vector::new(x0 + 4.0, y + 0.4), 40) + .particle_mass(0.03) + .surface_collider(ColliderBuilder::ball(0.4).friction(0.6)); + world.insert_soft_body(rope); + } + } + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 8.0), 25.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/stress_tests/soft_slab2.rs b/examples2d/stress_tests/soft_slab2.rs new file mode 100644 index 000000000..1b91965e4 --- /dev/null +++ b/examples2d/stress_tests/soft_slab2.rs @@ -0,0 +1,85 @@ +//! Stress test: one large elastic slab (a corotational grid of a few thousand particles) laid +//! over a bed of rigid balls and crates, showered with hundreds more. One big body's worth of cells, plus +//! many rigid-vs-soft contacts on a single deformable surface. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + /* + * Ground and side walls. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(14.0, 0.5), + ); + for x in [-14.0, 14.0] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 30.0)), + ColliderBuilder::cuboid(0.5, 30.0), + ); + } + + /* + * The bed of rigid objects the slab lands on. + */ + for i in 0..12 { + let x = -11.0 + i as Real * 2.0; + let body = RigidBodyBuilder::dynamic().translation(Vector::new(x, 0.5)); + if i % 2 == 0 { + world.insert(body, ColliderBuilder::ball(0.5)); + } else { + world.insert(body, ColliderBuilder::cuboid(0.5, 0.5)); + } + } + + /* + * The slab. + */ + let slab = SoftBodyBuilder::grid(Vector::new(0.0, 3.0), Vector::new(12.0, 1.5), 161, 21) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 4.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.2) + .surface_collider(ColliderBuilder::ball(0.1).friction(0.7)); + world.insert_soft_body(slab); + + /* + * The shower: alternating balls and boxes. + */ + for j in 0..40 { + for i in 0..24 { + let x = -11.5 + i as Real + (j % 2) as Real * 0.5; + let y = 7.5 + j as Real * 1.2; + let body = RigidBodyBuilder::dynamic().translation(Vector::new(x, y)); + if (i + j) % 2 == 0 { + world.insert(body, ColliderBuilder::ball(0.3)); + } else { + world.insert(body, ColliderBuilder::cuboid(0.3, 0.3)); + } + } + } + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 8.0), 25.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/stress_tests/soft_strips2.rs b/examples2d/stress_tests/soft_strips2.rs new file mode 100644 index 000000000..4c1e3e549 --- /dev/null +++ b/examples2d/stress_tests/soft_strips2.rs @@ -0,0 +1,72 @@ +//! Stress test: long floppy elastic strips with self-contacts dropped through a lattice of pegs +//! into a bin, where they fold onto themselves and onto each other. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + /* + * Bin, with a lattice of pegs. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(10.0, 0.5), + ); + for x in [-10.0, 10.0] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 30.0)), + ColliderBuilder::cuboid(0.5, 30.0), + ); + } + for row in 0..4 { + for i in 0..6 { + let x = -7.5 + i as Real * 3.0 + (row % 2) as Real * 1.5; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 4.0 + row as Real * 3.0)), + ColliderBuilder::ball(0.25), + ); + } + } + + /* + * Layers of strips. + */ + for layer in 0..30 { + for i in 0..5 { + let x = -7.8 + i as Real * 3.7 + (layer % 2) as Real * 0.7; + let y = 17.0 + layer as Real * 1.5; + let strip = SoftBodyBuilder::grid(Vector::new(x, y), Vector::new(1.6, 0.1), 33, 2) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .self_contacts(true) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)); + world.insert_soft_body(strip); + } + } + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(0.0, 8.0), 25.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 2d4a6bd55..56e63a481 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -246,6 +246,13 @@ pub async fn main() { STRESS, "Many pyramids", stress_tests::many_pyramids3::run; STRESS, "Keva tower", stress_tests::keva3::run; STRESS, "Ray cast", stress_tests::ray_cast3::run; + STRESS, "Soft blobs", stress_tests::soft_blobs3::run; + STRESS, "Soft jellies", stress_tests::soft_jellies3::run; + STRESS, "Soft ropes", stress_tests::soft_ropes3::run; + STRESS, "Soft cloth drape", stress_tests::soft_cloth_drape3::run; + STRESS, "Soft cloth on Keva tower", stress_tests::soft_cloth_keva3::run; + STRESS, "Soft slab shower", stress_tests::soft_slab3::run; + STRESS, "Soft FEM beams", stress_tests::soft_fem_beams3::run; // ── Box3D benchmarks (ports of box3d/benchmark) ───────────────────── B3D, "Large pyramid", b3d_large_pyramid::run; B3D, "Many pyramids", b3d_many_pyramids::run; diff --git a/examples3d/stress_tests/mod.rs b/examples3d/stress_tests/mod.rs index 0c0baacc1..a8f6fb075 100644 --- a/examples3d/stress_tests/mod.rs +++ b/examples3d/stress_tests/mod.rs @@ -20,5 +20,12 @@ pub mod pyramid3; pub mod ragdolls3; pub mod ray_cast3; pub mod ropes3; +pub mod soft_blobs3; +pub mod soft_cloth_drape3; +pub mod soft_cloth_keva3; +pub mod soft_fem_beams3; +pub mod soft_jellies3; +pub mod soft_ropes3; +pub mod soft_slab3; pub mod stacks3; pub mod trimesh3; diff --git a/examples3d/stress_tests/soft_blobs3.rs b/examples3d/stress_tests/soft_blobs3.rs new file mode 100644 index 000000000..fc5bb8a52 --- /dev/null +++ b/examples3d/stress_tests/soft_blobs3.rs @@ -0,0 +1,106 @@ +//! Stress test: hundreds of pressurized balloons poured into a tall bin, and a long floppy cloth +//! strip with self-contacts dropped onto the pile. Soft-vs-soft contacts through deformable +//! surfaces, under the weight of the pile. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + /* + * Bin. + */ + let half = 7.0; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), + ColliderBuilder::cuboid(half + 0.5, 0.5, half + 0.5), + ); + // Half-height of the walls: enough for the settled pile, not for the drop column. + let wall_height = 4.0; + for (dx, dz) in [(1.0, 0.0), (-1.0, 0.0), (0.0, 1.0), (0.0, -1.0)] { + let (hx, hz) = if dx != 0.0 { + (0.25, half + 0.5) + } else { + (half + 0.5, 0.25) + }; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new( + dx * (half + 0.25), + wall_height, + dz * (half + 0.25), + )), + ColliderBuilder::cuboid(hx, wall_height, hz), + ); + } + + /* + * Balloons of three sizes. + */ + for layer in 0..8 { + for i in 0..7 { + for j in 0..7 { + let radius = 0.45 + 0.1 * ((i + j + layer) % 3) as Real; + let x = -5.7 + i as Real * 1.9 + (layer % 2) as Real * 0.4; + let z = -5.7 + j as Real * 1.9 + (layer % 3) as Real * 0.3; + let y = 2.0 + layer as Real * 2.0; + let balloon = SoftBodyBuilder::sphere(Vector::new(x, y, z), radius, 1) + .softness(SpringCoefficients::new(20.0, 1.0)) + .volume_factor(1.1) + .particle_mass(0.03) + .particle_radius(0.08) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.6)); + world.insert_soft_body(balloon); + } + } + } + + /* + * A long floppy strip with self-contacts, dropped on the pile. + */ + let strip = SoftBodyBuilder::cloth( + Vector::new(-4.0, 28.0, -0.6), + Vector::new(0.15, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.15), + 54, + 9, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .self_contacts(true) + .particle_mass(0.02) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.5)); + world.insert_soft_body(strip); + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + // Translucent bin, to see the pile inside. + let bin: Vec<_> = world + .bodies + .iter() + .filter(|(_, body)| body.is_fixed()) + .map(|(handle, _)| handle) + .collect(); + for handle in bin { + viewer.set_body_color(handle, [0.6, 0.7, 1.0, 0.3].into(), false); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(20.0, 18.0, 20.0), Vec3::new(0.0, 6.0, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/stress_tests/soft_cloth_drape3.rs b/examples3d/stress_tests/soft_cloth_drape3.rs new file mode 100644 index 000000000..077123936 --- /dev/null +++ b/examples3d/stress_tests/soft_cloth_drape3.rs @@ -0,0 +1,85 @@ +//! Stress test: one large sheet of cloth (ten thousand particles, self-contacts on) dropped over a field of rigid obstacles, then loaded with a shower of heavy balls. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + /* + * Ground and obstacles. + */ + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(12.0, 0.1, 12.0), + ); + for i in 0..4 { + for j in 0..4 { + let x = -4.5 + i as Real * 3.0; + let z = -4.5 + j as Real * 3.0; + let body = RigidBodyBuilder::fixed().translation(Vector::new(x, 1.0, z)); + match (i + j) % 3 { + 0 => world.insert(body, ColliderBuilder::ball(0.8)), + 1 => world.insert(body, ColliderBuilder::cuboid(0.6, 1.0, 0.6)), + _ => world.insert(body, ColliderBuilder::capsule_y(0.6, 0.4)), + }; + } + } + + /* + * The sheet. + */ + let n = 101; + let sheet = SoftBodyBuilder::cloth( + Vector::new(-6.0, 3.5, -6.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .self_contacts(true) + .particle_mass(0.02) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.6)); + world.insert_soft_body(sheet); + + /* + * Heavy balls dropped on the draped sheet. + */ + for i in 0..5 { + for j in 0..5 { + let x = -4.0 + i as Real * 2.0 + (j % 2) as Real * 0.5; + let z = -4.0 + j as Real * 2.0; + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new( + x, + 6.0 + (i + j) as Real * 0.5, + z, + )), + ColliderBuilder::ball(0.4).density(2.0), + ); + } + } + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(14.0, 10.0, 14.0), Vec3::new(0.0, 1.0, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/stress_tests/soft_cloth_keva3.rs b/examples3d/stress_tests/soft_cloth_keva3.rs new file mode 100644 index 000000000..7d9030a0d --- /dev/null +++ b/examples3d/stress_tests/soft_cloth_keva3.rs @@ -0,0 +1,87 @@ +//! Stress test: a Keva tower of seven hundred planks with a square of cloth sandwiched between +//! its chunks (the runs of rows sharing a footprint) and a bigger one dropped flat on top, all +//! with self-contacts on. The tower drapes, slides and topples through the cloths. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(50.0, 0.1, 50.0), + ); + + // A square of cloth of the given side centered on the tower axis at height `y`, coarse + // enough (`spacing`) to stay cheap. + let radius = 0.15; + let cloth = |side: Real, spacing: Real, y: Real| { + let n = (side / spacing).round() as usize + 1; + let width = (n - 1) as Real * spacing; + SoftBodyBuilder::cloth( + Vector::new(-width * 0.5, y, -width * 0.5), + Vector::new(spacing, 0.0, 0.0), + Vector::new(0.0, 0.0, spacing), + n, + n, + ) + .softness(SpringCoefficients::new(30.0, 1.0)) + .material(SoftBodyMaterial { + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }) + .self_contacts(true) + .particle_mass(0.05) + .particle_radius(radius) + .surface_collider(ColliderBuilder::ball(radius).friction(0.6)) + }; + + /* + * The tower: the three lower chunks of the Keva demo, a cloth sandwiched between chunks. + */ + let half_extents = Vector::new(0.02, 0.1, 0.4) / 2.0 * 10.0; + let numy = [0, 9, 13, 17]; + let mut tower_height = 0.0; + for i in (1..=3).rev() { + let numx = i; + let numy = numy[i]; + let numz = numx * 3 + 1; + let block_width = numx as f32 * half_extents.z * 2.0; + crate::keva3::build_block( + &mut world, + half_extents, + Vector::new(-block_width / 2.0, tower_height, -block_width / 2.0), + (numx, numy, numz), + ); + tower_height += numy as f32 * half_extents.y * 2.0 + half_extents.x * 2.0; + + if i > 1 { + // Overhangs the chunk below; the chunk above rests on it. + world.insert_soft_body(cloth(block_width + 4.0, 0.5, tower_height + radius)); + tower_height += 2.0 * radius; + } + } + + /* + * The big cloth dropped on top. + */ + world.insert_soft_body(cloth(18.0, 0.75, tower_height + 6.0)); + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(50.0, 50.0, 50.0), Vec3::new(0.0, 15.0, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/stress_tests/soft_fem_beams3.rs b/examples3d/stress_tests/soft_fem_beams3.rs new file mode 100644 index 000000000..784955557 --- /dev/null +++ b/examples3d/stress_tests/soft_fem_beams3.rs @@ -0,0 +1,75 @@ +//! Stress test for the FEM soft-body solver: a grid of sixty-four stiff cantilevers, each solved +//! by [`SoftBodySolver::Fem`] (one factorization per step and per body) and loaded by a heavy +//! box dropped on its free end. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.1, 0.0)), + ColliderBuilder::cuboid(30.0, 0.1, 30.0), + ); + + /* + * The cantilevers, bolted at their left end, stiffer on the farther rows. + */ + let (length, thickness) = (3.0, 0.3); + for row in 0..8 { + for col in 0..8 { + let x0 = -20.0 + col as Real * 5.0; + let z = -17.5 + row as Real * 5.0; + let beam = SoftBodyBuilder::cuboid( + Vector::new(x0 + length * 0.5, 2.5, z), + Vector::new(length * 0.5, thickness * 0.5, thickness * 0.5), + 13, + 3, + 3, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e6 * (1.0 + row as Real), + poisson_ratio: 0.3, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .mass(12.0) + .solver(SoftBodySolver::Fem); + let pinned: Vec = beam + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x < x0 + 1.0e-4) + .map(|(i, _)| i as u32) + .collect(); + world.insert_soft_body(beam.pinned_particles(pinned)); + + /* + * The load, dropped on the free end. + */ + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(x0 + length - 0.4, 4.0, z)), + ColliderBuilder::cuboid(0.3, 0.3, 0.3).density(30.0), + ); + } + } + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(0.0, 28.0, 34.0), Vec3::new(0.0, 1.0, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/stress_tests/soft_jellies3.rs b/examples3d/stress_tests/soft_jellies3.rs new file mode 100644 index 000000000..defb30697 --- /dev/null +++ b/examples3d/stress_tests/soft_jellies3.rs @@ -0,0 +1,98 @@ +//! Stress test: hundreds of elastic jelly cubes (corotational and Neo-Hookean tetrahedra, +//! assorted stiffnesses) poured into a bin, colliding through their deformable surfaces. Mostly +//! soft-vs-soft contacts against elastic cells under the weight of the pile. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + /* + * Bin. + */ + let half = 6.0; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), + ColliderBuilder::cuboid(half + 0.5, 0.5, half + 0.5), + ); + // Half-height of the walls: enough for the settled pile, not for the drop column. + let wall_height = 3.5; + for (dx, dz) in [(1.0, 0.0), (-1.0, 0.0), (0.0, 1.0), (0.0, -1.0)] { + let (hx, hz) = if dx != 0.0 { + (0.25, half + 0.5) + } else { + (half + 0.5, 0.25) + }; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new( + dx * (half + 0.25), + wall_height, + dz * (half + 0.25), + )), + ColliderBuilder::cuboid(hx, wall_height, hz), + ); + } + + /* + * Layers of jelly cubes: every fifth stiffness step, every third one Neo-Hookean. + */ + let mut k = 0usize; + for layer in 0..8 { + for i in 0..5 { + for j in 0..5 { + let x = -4.8 + i as Real * 2.4 + (layer % 2) as Real * 0.6; + let z = -4.8 + j as Real * 2.4 + (layer % 3) as Real * 0.4; + let y = 2.0 + layer as Real * 2.2; + let young = 2.0e3 * (1.0 + (k % 5) as Real * 3.0); + let model = if k.is_multiple_of(3) { + SoftBodyCellModel::NeoHookean + } else { + SoftBodyCellModel::Corotational + }; + let cube = + SoftBodyBuilder::cuboid(Vector::new(x, y, z), Vector::splat(0.55), 4, 4, 4) + .cell_model(model) + .material(SoftBodyMaterial { + young_modulus: young, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.08) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)); + world.insert_soft_body(cube); + k += 1; + } + } + } + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + // Translucent bin, to see the pile inside. + let bin: Vec<_> = world + .bodies + .iter() + .filter(|(_, body)| body.is_fixed()) + .map(|(handle, _)| handle) + .collect(); + for handle in bin { + viewer.set_body_color(handle, [0.6, 0.7, 1.0, 0.3].into(), false); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(18.0, 16.0, 18.0), Vec3::new(0.0, 5.0, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/stress_tests/soft_ropes3.rs b/examples3d/stress_tests/soft_ropes3.rs new file mode 100644 index 000000000..2bff244ca --- /dev/null +++ b/examples3d/stress_tests/soft_ropes3.rs @@ -0,0 +1,100 @@ +//! Stress test: a rain of soft ropes (structural and bending edges, colliding through their +//! particles) poured over a few rods into a bin, alternating directions layer by layer so they +//! weave into a heap. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + /* + * Bin, with rods across it. + */ + let half = 6.0; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), + ColliderBuilder::cuboid(half + 0.5, 0.5, half + 0.5), + ); + // Half-height of the walls: enough for the settled pile, not for the drop column. + let wall_height = 3.0; + for (dx, dz) in [(1.0, 0.0), (-1.0, 0.0), (0.0, 1.0), (0.0, -1.0)] { + let (hx, hz) = if dx != 0.0 { + (0.25, half + 0.5) + } else { + (half + 0.5, 0.25) + }; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new( + dx * (half + 0.25), + wall_height, + dz * (half + 0.25), + )), + ColliderBuilder::cuboid(hx, wall_height, hz), + ); + } + for k in 0..4 { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, 4.0, -4.5 + k as Real * 3.0)), + ColliderBuilder::capsule_x(5.0, 0.12), + ); + } + + /* + * Layers of ropes, along X on the even layers and along Z on the odd ones. + */ + let length = 3.5; + for layer in 0..10 { + for i in 0..6 { + for j in 0..3 { + let across = -5.0 + i as Real * 2.0 + (layer % 2) as Real * 0.5; + let along = -5.7 + j as Real * 3.8; + let y = 8.0 + layer as Real * 1.5; + let (start, end) = if layer % 2 == 0 { + ( + Vector::new(along, y, across), + Vector::new(along + length, y + 0.3, across + 0.4), + ) + } else { + ( + Vector::new(across, y, along), + Vector::new(across + 0.4, y + 0.3, along + length), + ) + }; + let rope = SoftBodyBuilder::rope(start, end, 40) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.03) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.6)); + world.insert_soft_body(rope); + } + } + } + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + // Translucent bin, to see the pile inside. + let bin: Vec<_> = world + .bodies + .iter() + .filter(|(_, body)| body.is_fixed()) + .map(|(handle, _)| handle) + .collect(); + for handle in bin { + viewer.set_body_color(handle, [0.6, 0.7, 1.0, 0.3].into(), false); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(16.0, 14.0, 16.0), Vec3::new(0.0, 4.0, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/stress_tests/soft_slab3.rs b/examples3d/stress_tests/soft_slab3.rs new file mode 100644 index 000000000..dbec04255 --- /dev/null +++ b/examples3d/stress_tests/soft_slab3.rs @@ -0,0 +1,120 @@ +//! Stress test: one large elastic slab (a corotational tetrahedral block of a few thousand +//! particles) laid over a bed of rigid balls and crates in a bin, showered with hundreds more. One big body's +//! worth of cells, plus many rigid-vs-soft contacts on a single deformable surface. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +/// The scene alone, so it can be stepped without the testbed. +pub fn build_world() -> PhysicsWorld { + let mut world = PhysicsWorld::new(); + + /* + * Bin. + */ + let half = 9.0; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), + ColliderBuilder::cuboid(half + 0.5, 0.5, half + 0.5), + ); + // Half-height of the walls: enough for the settled pile, not for the drop column. + let wall_height = 3.5; + for (dx, dz) in [(1.0, 0.0), (-1.0, 0.0), (0.0, 1.0), (0.0, -1.0)] { + let (hx, hz) = if dx != 0.0 { + (0.25, half + 0.5) + } else { + (half + 0.5, 0.25) + }; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new( + dx * (half + 0.25), + wall_height, + dz * (half + 0.25), + )), + ColliderBuilder::cuboid(hx, wall_height, hz), + ); + } + + /* + * The bed of rigid objects the slab lands on. + */ + for i in 0..5 { + for j in 0..5 { + let x = -6.0 + i as Real * 3.0; + let z = -6.0 + j as Real * 3.0; + let body = RigidBodyBuilder::dynamic().translation(Vector::new(x, 0.5, z)); + if (i + j) % 2 == 0 { + world.insert(body, ColliderBuilder::ball(0.5)); + } else { + world.insert(body, ColliderBuilder::cuboid(0.5, 0.5, 0.5)); + } + } + } + + /* + * The slab. + */ + let slab = SoftBodyBuilder::cuboid( + Vector::new(0.0, 2.2, 0.0), + Vector::new(8.0, 1.0, 8.0), + 25, + 4, + 25, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 4.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.2) + .surface_collider(ColliderBuilder::ball(0.3).friction(0.7)); + world.insert_soft_body(slab); + + /* + * The shower: three waves of alternating balls and boxes. + */ + for wave in 0..3 { + for i in 0..16 { + for j in 0..16 { + let x = -7.5 + i as Real + (wave % 2) as Real * 0.5; + let z = -7.5 + j as Real + (wave % 3) as Real * 0.3; + let y = 5.5 + wave as Real * 3.0 + ((i + j) % 3) as Real * 0.7; + let body = RigidBodyBuilder::dynamic().translation(Vector::new(x, y, z)); + if (i + j + wave) % 2 == 0 { + world.insert(body, ColliderBuilder::ball(0.35)); + } else { + world.insert(body, ColliderBuilder::cuboid(0.35, 0.35, 0.35)); + } + } + } + } + + world +} + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + let mut world = build_world(); + + // Translucent bin, to see the pile inside. + let bin: Vec<_> = world + .bodies + .iter() + .filter(|(_, body)| body.is_fixed()) + .map(|(handle, _)| handle) + .collect(); + for handle in bin { + viewer.set_body_color(handle, [0.6, 0.7, 1.0, 0.3].into(), false); + } + + viewer.set_world(&mut world); + viewer.look_at(Vec3::new(20.0, 14.0, 20.0), Vec3::new(0.0, 2.0, 0.0)); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs index 730d485fd..d60cef91c 100644 --- a/src_testbed/graphics.rs +++ b/src_testbed/graphics.rs @@ -85,6 +85,19 @@ fn convex_shape_hash(shape: &dyn Shape) -> Option { Some(hasher.finish()) } +/// Whether a triangle mesh has a boundary (an edge belonging to a single triangle): a cloth, a +/// torn piece, an open shell, as opposed to a closed surface. +#[cfg(feature = "dim3")] +pub(super) fn is_open_surface(indices: &[[u32; 3]]) -> bool { + let mut edges = std::collections::HashMap::with_capacity(indices.len() * 3); + for [a, b, c] in indices { + for (i, j) in [(a, b), (b, c), (c, a)] { + *edges.entry((i.min(j), i.max(j))).or_insert(0u32) += 1; + } + } + edges.values().any(|&count| count == 1) +} + /// Build a kiss3d render mesh for a convex polyhedron (used as an instancing /// template). Returns `None` for non-convex shapes. #[cfg(feature = "dim3")] @@ -176,6 +189,8 @@ pub enum NodeLocation { pub struct GraphicsManager { scene: SceneNode, curr_color_index: usize, + /// Number of fixed bodies colored so far (the first one gets the ground color). + num_fixed_colored: usize, /// Template nodes for instanced primitives templates: HashMap, /// Individual nodes for complex shapes (trimesh, heightfield, etc.) @@ -225,6 +240,15 @@ const GROUND_COLOR: Color = Color { a: 1.0, }; +/// Color of the fixed bodies other than the first one (the ground, in practice): lighter than +/// the ground so a fixed box does not blend into the floor it stands on. +const FIXED_COLOR: Color = Color { + r: 0.72, + g: 0.68, + b: 0.62, + a: 1.0, +}; + /// Base-color alpha applied to the transparent instance node of every shape /// template. kiss3d routes an entire instanced node to either the opaque or the /// order-independent-transparency pass based on the node's base-color alpha @@ -265,6 +289,7 @@ impl GraphicsManager { GraphicsManager { scene: Default::default(), curr_color_index: 0, + num_fixed_colored: 0, templates: HashMap::new(), individual_nodes: Vec::new(), body_attached_nodes: Vec::new(), @@ -287,15 +312,18 @@ impl GraphicsManager { pub fn clear(&mut self) { self.scene = SceneNode::empty(); self.curr_color_index = 0; + self.num_fixed_colored = 0; #[cfg(feature = "dim3")] { // A key light off the (1, 1, 1) diagonal, so the three faces of a box seen from the // usual eye get three different shades, and a weak fill from the other side so the // faces it misses are not flat ambient. - let mut light = self + let mut key = self .scene - .add_light(Light::directional(Vec3::new(-1.0, -1.0, -1.0))); - light.set_position(Vec3::new(100.0, 100.0, 100.0)); + .add_light(Light::directional(Vec3::new(-0.35, -1.0, -0.6))); + key.set_position(Vec3::new(100.0, 100.0, 100.0)); + self.scene + .add_light(Light::directional(Vec3::new(0.6, -0.25, 0.75)).with_intensity(0.8)); } self.templates.clear(); @@ -827,7 +855,12 @@ impl GraphicsManager { fn alloc_color(&mut self, handle: RigidBodyHandle, is_fixed: bool) -> Color { let mut color = self.ground_color; - if !is_fixed { + if is_fixed { + if self.num_fixed_colored > 0 { + color = FIXED_COLOR; + } + self.num_fixed_colored += 1; + } else { match self.b2color.get(&handle).cloned() { Some(c) => color = c, None => color = Self::gen_color(&mut self.curr_color_index), @@ -992,13 +1025,26 @@ impl GraphicsManager { use kiss3d::procedural::{IndexBuffer, RenderMesh}; fn to_render_mesh(trimesh: &rapier::geometry::TriMesh, smooth: bool) -> RenderMesh { - let vtx = trimesh + let mut vtx: Vec = trimesh .vertices() .iter() .map(|pt| Vec3::new(pt.x as f32, pt.y as f32, pt.z as f32)) .collect(); - let idx = trimesh.indices().to_vec(); + let mut idx = trimesh.indices().to_vec(); + // An open surface is seen from both sides but the shader lights a face by its own + // normal, so the back side gets its own triangles wound the other way (and its own + // vertices when shared, so normals can differ); `write_mesh_vertices` matches. + if is_open_surface(&idx) { + let n = if smooth { vtx.len() as u32 } else { 0 }; + let back: Vec<[u32; 3]> = + idx.iter().map(|[a, b, c]| [a + n, c + n, b + n]).collect(); + if smooth { + vtx.extend_from_within(..); + } + idx.extend(back); + } let mut mesh = RenderMesh::new(vtx, None, None, Some(IndexBuffer::Unified(idx))); + // Unshared vertices (three per triangle) give per-face normals: flat shading. if !smooth { mesh.replicate_vertices(); } diff --git a/src_testbed/graphics/graphics_soft_bodies.rs b/src_testbed/graphics/graphics_soft_bodies.rs index 093624504..4c5fd36a3 100644 --- a/src_testbed/graphics/graphics_soft_bodies.rs +++ b/src_testbed/graphics/graphics_soft_bodies.rs @@ -82,11 +82,18 @@ fn write_mesh_vertices( vertices: &[rapier::math::Vector], indices: &[[u32; 3]], ) -> bool { - let expected = if smooth { + let single = if smooth { vertices.len() } else { indices.len() * 3 }; + // An open surface is built double-sided (see `create_individual_node`): a second copy of + // the geometry, wound the other way, after the first. + let expected = if super::is_open_surface(indices) { + single * 2 + } else { + single + }; let mut usable = true; node.modify_vertices(&mut |vtx: &mut Vec| { if vtx.len() != expected { @@ -101,12 +108,24 @@ fn write_mesh_vertices( for (v, i) in vtx.iter_mut().zip(0..vertices.len() as u32) { *v = point(i); } + if expected > single { + let (front, back) = vtx.split_at_mut(single); + back.copy_from_slice(front); + } } else { for (triangle, corners) in indices.iter().enumerate() { for (k, corner) in corners.iter().enumerate() { vtx[triangle * 3 + k] = point(*corner); } } + if expected > single { + for (triangle, [a, b, c]) in indices.iter().enumerate() { + let base = single + triangle * 3; + vtx[base] = point(*a); + vtx[base + 1] = point(*c); + vtx[base + 2] = point(*b); + } + } } }); if usable { From 52486e691fe63c60909eeb4950c557029bbad610 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 17 Sep 2026 17:48:55 +0200 Subject: [PATCH 21/32] fix(soft-body): two-sided contacts for open 2D surfaces and per-endpoint interior segment contacts --- crates/rapier2d/tests/soft_bodies.rs | 102 ++++++++++++++++++ crates/rapier3d/tests/soft_wires.rs | 68 ++++++++++++ .../soft_body_set/soft_body_set_proxies.rs | 9 ++ .../soft_body_set/soft_body_set_step_sync.rs | 1 + .../soft_contact_assembly_body_contacts.rs | 2 +- .../soft_contact_assembly_writeback.rs | 12 +++ 6 files changed, 193 insertions(+), 1 deletion(-) diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index edc720a41..73b5d077a 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -2733,6 +2733,108 @@ fn modify_solver_contacts_edits_soft_soft_contacts() { assert!(dropped < 0.6, "the hook did not drop the soft-soft contacts: y = {dropped}"); } +/// A rope shot tip first at a fixed wall stops at the wall: an open polyline collides from +/// both sides (its segments were oriented, and every contact outside the cone of their +/// normals, the wall ahead of the rope's tip included, was dropped: the rope went through). +#[test] +fn a_rope_shot_at_a_wall_stops_tip_first() { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(3.0, 0.0)), + ColliderBuilder::cuboid(0.5, 5.0), + ); + let n = 40; + let rope = SoftBodyBuilder::rope(Vector::new(-4.0, 0.0), Vector::new(0.0, 0.0), n) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.03) + .surface_collider(ColliderBuilder::ball(0.05)); + let handle = world.insert_soft_body(rope); + for i in 0..n { + world.soft_bodies[handle].set_particle_velocity(i, Vector::new(3.0, 0.0)); + } + let mut max_x = Real::MIN; + for _ in 0..200 { + world.step(); + let x = world.soft_bodies[handle] + .particle_positions() + .map(|p| p.x) + .fold(Real::MIN, Real::max); + max_x = max_x.max(x); + } + assert_finite(&world, handle); + // The wall's face is at x = 2.5; the rope's skin is half a segment thick. + assert!(max_x < 2.5, "the rope went into the wall: max x = {max_x}"); + assert!(max_x > 2.3, "the rope never reached the wall: max x = {max_x}"); +} + +/// A rope released hanging over a small fixed ball rests on it: the segments beside the ball +/// touch it at an interior point each, split into endpoint contacts that are not filtered as +/// ghosts of a neighbor's contact. +#[test] +fn rope_draped_over_a_ball_pin_rests() { + let mut world = world_with_ground(); + let center = Vector::new(0.0, 4.0); + let pin_radius = 0.1; + world.insert( + RigidBodyBuilder::fixed().translation(center), + ColliderBuilder::ball(pin_radius), + ); + // The rope's path: down the left side, over the pin, down the right side. + let n = 40; + let length = 4.0; + let segment = length / (n - 1) as Real; + let radius = segment * 0.5; + let r = pin_radius + radius + 0.01; + let arc = core::f32::consts::PI * r; + let side = (length - arc) * 0.5; + let positions: Vec = (0..n) + .map(|i| { + let t = i as Real * segment; + if t < side { + Vector::new(-r, 4.0 - (side - t)) + } else if t < side + arc { + let a = (t - side) / r; + Vector::new(-a.cos() * r, 4.0 + a.sin() * r) + } else { + Vector::new(r, 4.0 - (t - side - arc)) + } + }) + .collect(); + let edges: Vec<[u32; 2]> = (0..n as u32 - 1).map(|i| [i, i + 1]).collect(); + let bend_edges: Vec<[u32; 2]> = (0..n as u32 - 2).map(|i| [i, i + 2]).collect(); + let handle = world.insert_soft_body( + SoftBodyBuilder::new(positions) + .particle_radius(radius) + .surface(edges.clone()) + .edges(edges) + .bend_edges(bend_edges) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.03) + .surface_collider(ColliderBuilder::ball(radius).friction(0.6)), + ); + let mut max_speed: Real = 0.0; + let mut on_pin = 0; + for step in 0..400 { + world.step(); + if step < 200 { + continue; + } + for p in world.soft_bodies[handle].particles() { + if (p.position() - center).length() < r + 0.05 { + on_pin += 1; + max_speed = max_speed.max(p.velocity().length()); + } + } + } + assert_finite(&world, handle); + assert!(on_pin > 0, "the rope slid off the pin"); + assert!( + max_speed < 0.02, + "the rope jitters on the pin: max speed {max_speed}" + ); +} + /// A rope draped over a rigid ball much larger than its segments rests on it: the ball's /// manifold with each segment holds a single point, so the vertices are held by the pair's /// predictive vertex contacts (without them they sag into the skin and the rope jitters). diff --git a/crates/rapier3d/tests/soft_wires.rs b/crates/rapier3d/tests/soft_wires.rs index bb9d1c308..6991aa748 100644 --- a/crates/rapier3d/tests/soft_wires.rs +++ b/crates/rapier3d/tests/soft_wires.rs @@ -170,3 +170,71 @@ fn a_wire_collider_follows_the_particles() { ); } } + +/// A rope released already hanging over a fixed rod rests on it. A segment across a round rod +/// touches it at one point: with a single contact there the chord rocked about it until an +/// endpoint sank into the rod and was kicked back, and the rope jittered on the rod forever. +#[test] +fn a_rope_draped_over_a_rod_rests() { + let mut world = world_with_ground(); + let rod_radius = 0.12; + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, 4.0, 0.0)), + ColliderBuilder::capsule_x(5.0, rod_radius), + ); + // The rope's path: down the -z side, over the rod, down the +z side. + let n = 40; + let length = 3.5; + let segment = length / (n - 1) as Real; + let radius = segment * 0.5; + let r = rod_radius + radius + 0.01; + let arc = core::f32::consts::PI * r; + let side = (length - arc) * 0.5; + let positions: Vec = (0..n) + .map(|i| { + let t = i as Real * segment; + if t < side { + Vector::new(0.0, 4.0 - (side - t), -r) + } else if t < side + arc { + let a = (t - side) / r; + Vector::new(0.0, 4.0 + a.sin() * r, -a.cos() * r) + } else { + Vector::new(0.0, 4.0 - (t - side - arc), r) + } + }) + .collect(); + let edges: Vec<[u32; 2]> = (0..n as u32 - 1).map(|i| [i, i + 1]).collect(); + let bend_edges: Vec<[u32; 2]> = (0..n as u32 - 2).map(|i| [i, i + 2]).collect(); + let handle = world.insert_soft_body( + SoftBodyBuilder::new(positions) + .particle_radius(radius) + .wire(edges.clone()) + .edges(edges) + .bend_edges(bend_edges) + .softness(SpringCoefficients::new(30.0, 1.0)) + .particle_mass(0.03) + .surface_collider(ColliderBuilder::ball(radius).friction(0.6)), + ); + let mut max_speed: Real = 0.0; + let mut on_rod = 0; + for step in 0..400 { + world.step(); + if step < 200 { + continue; + } + for p in world.soft_bodies[handle].particles() { + if p.position().y > 3.9 { + on_rod += 1; + max_speed = max_speed.max(p.velocity().length()); + } + } + } + assert!(on_rod > 0, "the rope slid off the rod"); + let low = lowest(&world, handle); + assert!(low > 2.0 && low < 3.0, "the rope is not hanging from the rod: {low}"); + assert!( + max_speed < 0.02, + "the rope jitters on the rod: max speed {max_speed}" + ); +} + 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 5cb19020d..6955dcda5 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 @@ -12,7 +12,9 @@ use crate::math::{DIM, Pose, Real, Vector}; /// deformable so the narrow phase never trusts contact points cached across a vertex update. /// A closed 2D polyline is one-sided (pushing outward); an open one (a rope) is two-sided. pub(crate) fn surface_shape( + vertices: Vec, surface: &[[u32; DIM]], + closed: bool, ) -> Option { if surface.is_empty() || vertices.is_empty() { return None; @@ -20,6 +22,8 @@ pub(crate) fn surface_shape( #[cfg(feature = "dim2")] { use parry::shape::{Polyline, PolylineFlags}; + let mut flags = PolylineFlags::DEFORMABLE; + flags.set(PolylineFlags::ORIENTED, closed); Some(SharedShape::new(Polyline::with_flags( vertices, Some(surface.to_vec()), @@ -29,6 +33,9 @@ pub(crate) fn surface_shape( #[cfg(feature = "dim3")] { use parry::shape::{Polyline, PolylineFlags, TriMeshFlags}; + let _ = closed; + // A wire's elements are segments (`u32::MAX` fills the unused slot): it collides as a + // polyline, which parry supports in 3D too. if super::soft_body_builder::element_vertices(&surface[0]).len() < DIM { let segments: Vec<[u32; 2]> = surface.iter().map(|e| [e[0], e[1]]).collect(); return Some(SharedShape::new(Polyline::with_flags( @@ -48,6 +55,7 @@ pub(super) fn rebuilt_surface_shape( prev: &dyn parry::shape::Shape, vertices: Vec, surface: &[[u32; DIM]], + closed: bool, ) -> Option { let mut shape = surface_shape(vertices, surface, closed)?; let rebuilt = shape.make_mut(); @@ -132,6 +140,7 @@ pub(super) fn clone_mesh_collider( collider.shape(), mesh.local_vertices(body, frame), mesh.indices(), + mesh.is_closed(), )?; collider.set_shape(shape); collider.set_position(Pose::IDENTITY); 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 4010e476c..f656717e3 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 @@ -170,6 +170,7 @@ impl SoftBodySet { co.shape(), mesh.local_vertices(sb, &pose), mesh.indices(), + mesh.is_closed(), ) { Some(shape) => co.replace_deformed_shape(shape), None => co.set_enabled(false), diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs index def9c9706..65afe1f8e 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs @@ -421,7 +421,7 @@ impl SoftConstraintsSet { let (dir, dist, _expelling) = match (along_normal, reversed) { (Some(constraint), _) => constraint, (None, Some(_)) if is_interior(element, &weights) => continue, - _ => (dir, sc.dist, false), + _ => (dir, sc_dist, false), }; // The point the constraint tracks, expressed in the particles it acts through. let (anchors, anchor_weights, tracked) = diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs index bfb91ca6e..1e997e06f 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs @@ -18,6 +18,11 @@ use crate::math::{AngVector, DIM, Vector}; /// before its pair's constraints report; a point split into endpoint constraints sums them): /// the step's total for the events, the last substep's for the warm start. fn report_impulses(data: &mut crate::geometry::ContactData, c: &SoftContact) { + data.impulse = crate::utils::canonicalize_zero( + data.impulse + c.impulse_normal_acc + c.impulse_normal, + ); + data.warmstart_impulse = + crate::utils::canonicalize_zero(data.warmstart_impulse + c.impulse_normal); #[cfg(feature = "dim2")] { data.tangent_impulse[0] = crate::utils::canonicalize_zero( @@ -38,6 +43,7 @@ fn report_impulses(data: &mut crate::geometry::ContactData, c: &SoftContact) { ); } data.warmstart_tangent_world = crate::utils::canonicalize_zero( + data.warmstart_tangent_world + c.tangents[0] * c.impulse_tangent[0] + c.tangents[1] * c.impulse_tangent[1], ); @@ -193,6 +199,12 @@ impl SoftConstraintsSet { for point in &mut manifold.points { point.data.impulse = 0.0; point.data.tangent_impulse = Default::default(); + point.data.warmstart_impulse = 0.0; + point.data.warmstart_tangent_impulse = Default::default(); + #[cfg(feature = "dim3")] + { + point.data.warmstart_tangent_world = Vector::ZERO; + } } } if let Some(soft) = rigid.soft.as_deref_mut() { From 479e5fb5ab008a9ef27cbfb12f24291c0c210f38 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 17 Sep 2026 19:26:01 +0200 Subject: [PATCH 22/32] fix(testbed): time frames from the renderer's return so vsync waits are left out of the timings --- src_testbed/testbed/state.rs | 23 +++++++++++++++-------- src_testbed/ui.rs | 22 +++++++++------------- src_testbed/viewer.rs | 4 +++- 3 files changed, 27 insertions(+), 22 deletions(-) diff --git a/src_testbed/testbed/state.rs b/src_testbed/testbed/state.rs index 718d89a11..d6fcb19c4 100644 --- a/src_testbed/testbed/state.rs +++ b/src_testbed/testbed/state.rs @@ -96,10 +96,12 @@ impl ExampleEntry { } /// Wall-clock times of the recent frames, for the performance tab: the step counters only hold -/// the last step, and a frame also spends time outside of it (rendering, UI). +/// the last step, and a frame also spends time outside of it (scene update, UI). The renderer's +/// present (which waits for vsync) is left out, so the numbers do not saturate at the refresh rate. #[derive(Default)] pub struct FrameStats { - last_frame: Option, + /// When the renderer last handed control back, or `None` before the first render. + resumed_at: Option, last_timestep: usize, /// `(frame_ms, step_ms)` of the recent frames, oldest first. samples: std::collections::VecDeque<(f64, f64)>, @@ -109,10 +111,10 @@ impl FrameStats { /// The number of frames the statistics cover. pub const WINDOW: usize = 60; - /// Records the frame that just ended, with `step_ms` counted only if a step ran during it. + /// Records the frame that just ended (everything since the last [`Self::resume`]), with + /// `step_ms` counted only if a step ran during it. Called right before rendering. pub fn record(&mut self, timestep_id: usize, step_ms: f64) { - let now = web_time::Instant::now(); - if let Some(last) = self.last_frame { + if let Some(resumed_at) = self.resumed_at.take() { let step_ms = if timestep_id != self.last_timestep { step_ms } else { @@ -121,13 +123,18 @@ impl FrameStats { if self.samples.len() == Self::WINDOW { let _ = self.samples.pop_front(); } - self.samples - .push_back(((now - last).as_secs_f64() * 1000.0, step_ms)); + let frame_ms = resumed_at.elapsed().as_secs_f64() * 1000.0; + self.samples.push_back((frame_ms, step_ms)); } - self.last_frame = Some(now); self.last_timestep = timestep_id; } + /// Starts timing the next frame. Called right after the renderer returns, so the time it + /// spends presenting (and waiting for vsync) is not counted. + pub fn resume(&mut self) { + self.resumed_at = Some(web_time::Instant::now()); + } + /// The mean `(frame, step)` times over the window, in milliseconds. pub fn mean_ms(&self) -> (f64, f64) { let n = self.samples.len().max(1) as f64; diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index d22bfeee4..24d01f992 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -954,24 +954,20 @@ fn performance_tab(ui: &mut Ui, state: &TestbedState, world: &PhysicsWorld) { let counters = &world.physics_pipeline.counters; let total_ms = counters.step_time_ms(); let (mean_frame_ms, mean_step_ms) = state.frame_stats.mean_ms(); - let (max_frame_ms, max_step_ms) = state.frame_stats.max_ms(); - let fps = if mean_frame_ms > 0.0 { - (1000.0 / mean_frame_ms).round() - } else { - 0.0 - }; + let (_, max_step_ms) = state.frame_stats.max_ms(); // The step counters only hold the last step: the peaks over the recent frames show the - // spikes a single value flickers past, and the frame time is measured by the viewer. + // spikes a single value flickers past. The testbed time is the frame minus the step, with + // the frame measured by the viewer without the renderer's present (so no vsync wait). ui.label( - RichText::new(format!("Total: {:.2}ms (peak {:.2}ms)", total_ms, max_step_ms)).strong(), + RichText::new(format!( + "Total physics: {:.2}ms (peak {:.2}ms)", + total_ms, max_step_ms + )) + .strong(), ); ui.label(format!( - "Frame: {:.2}ms (peak {:.2}ms) - {:.0} FPS", - mean_frame_ms, max_frame_ms, fps - )); - ui.label(format!( - "Outside physics: {:.2}ms", + "Testbed: {:.2}ms", (mean_frame_ms - mean_step_ms).max(0.0) )); ui.add_space(4.0); diff --git a/src_testbed/viewer.rs b/src_testbed/viewer.rs index 2e386dda4..b7d10b5a7 100644 --- a/src_testbed/viewer.rs +++ b/src_testbed/viewer.rs @@ -133,7 +133,7 @@ impl TestbedViewer { /// requested from the UI. pub async fn render_frame(&mut self, world: &mut PhysicsWorld) -> bool { profiling::finish_frame!(); - // A whole frame spans two calls: the example's step runs in between. + // A frame spans two calls (the example's step runs in between) minus the render below. self.state.frame_stats.record( self.state.timestep_id, world.physics_pipeline.counters.step_time_ms(), @@ -150,6 +150,8 @@ impl TestbedViewer { .render_2d(self.graphics.scene_mut(), &mut self.camera) .await; + self.state.frame_stats.resume(); + if !keep_open { self.state.transition = Some(Transition::Quit); return false; From 28c4465a5bf23d36f0eb494c351885921a315111 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 17 Sep 2026 21:07:24 +0200 Subject: [PATCH 23/32] chore: switch to parry and glamx git dependencies --- Cargo.toml | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index bd6679827..176d93097 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -150,21 +150,22 @@ tempfile = "3" #kiss3d = { path = "../kiss3d" } # Temporary: the deformable-mesh API (parry branch `soft-bodies`), to be replaced by a # version bump when parry is released. -parry2d = { path = "../parry/crates/parry2d" } -parry3d = { path = "../parry/crates/parry3d" } -parry2d-f64 = { path = "../parry/crates/parry2d-f64" } -parry3d-f64 = { path = "../parry/crates/parry3d-f64" } +#parry2d = { path = "../parry/crates/parry2d" } +#parry3d = { path = "../parry/crates/parry3d" } +#parry2d-f64 = { path = "../parry/crates/parry2d-f64" } +#parry3d-f64 = { path = "../parry/crates/parry3d-f64" } #nalgebra = { path = "../nalgebra" } #simba = { path = "../simba" } #wide = { path = "../wide" } -glamx = { path = "../glamx" } +#glamx = { path = "../glamx" } #kiss3d = { git = "https://github.com/sebcrozet/kiss3d", branch = "render-2d" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } -#parry2d = { git = "https://github.com/dimforge/parry", branch = "master" } -#parry3d = { git = "https://github.com/dimforge/parry", branch = "master" } -#parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "master" } -#parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "master" } +glamx = { git = "https://github.com/dimforge/glamx", branch = "fix-lerp2" } +parry2d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } +parry3d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } +parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } +parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } # See https://github.com/EmbarkStudios/puffin/pull/234 puffin_egui = { git = "https://github.com/tedsteen/puffin.git", rev = "11771ebe00fd257aedbb545df3339ad597b1cc34" } From bfddf8f1af3a90f6c63012a7c09cab7b7a4cb8c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 17 Sep 2026 23:17:04 +0200 Subject: [PATCH 24/32] chore: switch back to the published version of glamx --- Cargo.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 176d93097..a5ffa6101 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -66,7 +66,7 @@ needless_lifetimes = "allow" [workspace.dependencies] # Core math nalgebra = { version = "0.35", default-features = false, features = ["macros"] } -glamx = { version = "0.3", default-features = false } +glamx = { version = "0.3.1", default-features = false } simba = { version = "0.10.2", default-features = false } num-traits = { version = "0.2", default-features = false } approx = { version = "0.5", default-features = false } @@ -161,7 +161,7 @@ tempfile = "3" #kiss3d = { git = "https://github.com/sebcrozet/kiss3d", branch = "render-2d" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } -glamx = { git = "https://github.com/dimforge/glamx", branch = "fix-lerp2" } +#glamx = { git = "https://github.com/dimforge/glamx", branch = "fix-lerp2" } parry2d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } parry3d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } From 23ca5960051a12a39051da02ccf9182a39142aa8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 18 Sep 2026 00:02:27 +0200 Subject: [PATCH 25/32] feat(typescript): expose soft bodies in the JS bindings and testbeds --- typescript/CHANGELOG.md | 58 + typescript/Cargo.toml | 4 + .../prepare_builds/templates/Cargo.toml.tera | 2 +- .../src.ts/dynamics/impulse_joint_set.ts | 12 + typescript/src.ts/dynamics/index.ts | 2 + .../src.ts/dynamics/integration_parameters.ts | 36 + .../src.ts/dynamics/multibody_joint_set.ts | 12 + typescript/src.ts/dynamics/rigid_body.ts | 58 + typescript/src.ts/dynamics/rigid_body_set.ts | 34 + typescript/src.ts/dynamics/soft_body.ts | 2137 +++++++++++++++++ typescript/src.ts/dynamics/soft_body_set.ts | 325 +++ typescript/src.ts/geometry/collider.ts | 38 +- typescript/src.ts/geometry/collider_set.ts | 141 +- typescript/src.ts/geometry/shape.ts | 94 +- .../src.ts/pipeline/debug_render_pipeline.ts | 3 + typescript/src.ts/pipeline/event_queue.ts | 27 +- .../src.ts/pipeline/physics_pipeline.ts | 4 + .../src.ts/pipeline/serialization_pipeline.ts | 3 + typescript/src.ts/pipeline/world.ts | 197 ++ .../src/dynamics/integration_parameters.rs | 30 + typescript/src/dynamics/mod.rs | 2 + typescript/src/dynamics/rigid_body.rs | 23 + typescript/src/dynamics/rigid_body_set.rs | 11 +- typescript/src/dynamics/soft_body.rs | 1549 ++++++++++++ typescript/src/geometry/collider.rs | 29 + typescript/src/geometry/collider_set.rs | 295 ++- typescript/src/geometry/narrow_phase.rs | 5 +- typescript/src/geometry/shape.rs | 54 +- .../src/pipeline/debug_render_pipeline.rs | 7 +- typescript/src/pipeline/event_queue.rs | 22 +- typescript/src/pipeline/physics_pipeline.rs | 6 +- .../src/pipeline/serialization_pipeline.rs | 14 +- typescript/src/utils.rs | 7 +- typescript/testbed2d/src/Graphics.ts | 52 + typescript/testbed2d/src/demos/softBodies.ts | 111 + typescript/testbed2d/src/index.ts | 2 + typescript/testbed3d/src/Graphics.ts | 67 +- typescript/testbed3d/src/demos/softBodies.ts | 112 + typescript/testbed3d/src/demos/softTearing.ts | 169 ++ typescript/testbed3d/src/index.ts | 4 + 40 files changed, 5711 insertions(+), 47 deletions(-) create mode 100644 typescript/src.ts/dynamics/soft_body.ts create mode 100644 typescript/src.ts/dynamics/soft_body_set.ts create mode 100644 typescript/src/dynamics/soft_body.rs create mode 100644 typescript/testbed2d/src/demos/softBodies.ts create mode 100644 typescript/testbed3d/src/demos/softBodies.ts create mode 100644 typescript/testbed3d/src/demos/softTearing.ts diff --git a/typescript/CHANGELOG.md b/typescript/CHANGELOG.md index 51bf56c6f..dbcc4073c 100644 --- a/typescript/CHANGELOG.md +++ b/typescript/CHANGELOG.md @@ -1,3 +1,61 @@ +## Unreleased + +### Breaking changes + +- `World.removeCollider`, `RigidBodySet.remove` and `ColliderSet.remove` take the world's + `SoftBodySet`: removing a rigid body that is a soft-body cluster proxy removes its cluster, + and removing a deformable collider drops its mesh from its soft body. `World` passes its own + set, so only direct users of the sets are affected. +- `PhysicsPipeline.step`, `DebugRenderPipeline.render`, `SerializationPipeline.serializeAll` + and `RigidBodySet.remove` take the `SoftBodySet` as an extra argument, right after the + `ColliderSet`. +- The `World` constructor takes the raw soft-body set right after the raw collider set. +- `RigidBodyType` gained the `SoftFrame` variant: the proxies a soft body creates for itself + and its clusters. `RigidBody.isSoftFrame()` tells them apart. + +### Added + +- Soft bodies: `World.createSoftBody(desc)` inserts a deformable body made of particles linked + by edges, bending constraints and cells, simulated together with the rigid bodies, contacts + and joints. `SoftBodyDesc` offers the generators `rope`, `cloth`, `clothTube`, + `clothAnisotropic`, `cuboid`, `sphere`, `trimesh` and `volumetric` in 3D, and `rope`, + `polyline`, `polygon`, `disk`, `grid` and `volumetric` in 2D, plus setters for pinned + particles, masses, the material, the cell model (`SoftBodyCellModel`), volume preservation, + shape matching, self contacts, the particle radius and the surface collider template. +- `SoftBodyMaterial`: the softness of each constraint kind, the elastic parameters of the cells + (Young's modulus, Poisson's ratio), plasticity (`plasticYield`, `plasticCreep`, + `edgePlasticYield`…) and tearing thresholds (`tearStrain`, `tearForce`, `minPiece`). +- `SoftBody`: particle positions, velocities, masses and pinning; attachments to rigid bodies + (`attachParticle`); forces and impulses (`addForce`, `applyImpulseAtPoint`, + `applyRadialImpulse`…); the edges, cells, dihedrals and boundary of the body with their + stresses; the material; volume preservation; the hidden `rootBody()`; clusters + (`clusterProxy`, `setClusterPinned`, `setClusterKinematicTarget`…) and collision meshes + (`meshVertices`, `meshIndices`). +- Tearing and cutting: `World.tearSoftBody`, `World.cutSoftBody` and `SoftBody.tearEdge` / + `tearCell`; the pieces a tear disconnects become soft bodies of their own. Tears report a + `SoftBodyTearEvent`, also drained from an `EventQueue` with `drainSoftBodyTearEvents`. +- Clusters: `World.addSoftBodyCluster` creates a rigid proxy over some particles that joints + (`World.createImpulseJoint`) and colliders can attach to; `World.removeSoftBodyCluster`. +- Deformable colliders: `World.createDeformableCollider(desc, binding, proxy)` binds a + triangle mesh (3D) or polyline (2D) built with the `DEFORMABLE` flag to a cluster, through a + `SoftMeshBinding` (`direct`, `directByPosition`, `skinned`). `Polyline` and + `ColliderDesc.polyline` take a `PolylineFlags` argument; `Collider.isDeformable()` and + `Collider.softBody()` identify such colliders. +- `IntegrationParameters.softBodiesResweepStrain`, `softBodiesMaxExtraSubsteps` and + `softBodiesContactStiffening` tune the soft-body solver. +- `RigidBody.additionalPgsIterations()` / `setAdditionalPgsIterations` and + `RigidBodyDesc.setAdditionalPgsIterations`: extra internal PGS iterations per substep for the + island component of a body. +- `RigidBody.softBody()` and `RigidBody.softCluster()` identify the soft body and cluster a + proxy stands for. +- Snapshots (`World.takeSnapshot` / `restoreSnapshot`) include the soft bodies. +- `PolylineFlags.ORIENTED` (2D): one-sided polylines whose contact normals are clamped to + the outward side. +- `CompoundFlags.FIX_INTERNAL_EDGES`: the seams between the parts of a compound (or of a + convex decomposition) no longer catch sliding bodies. Taken by `Compound`, + `ColliderDesc.compound` and `ColliderDesc.convexDecomposition`. +- The testbeds gained soft-body demos ("soft bodies" in 2D and 3D, "soft tearing" in 3D). + ## 0.20.0 (08 August 2026) ### Breaking changes diff --git a/typescript/Cargo.toml b/typescript/Cargo.toml index 6b4d76565..349abf9b3 100644 --- a/typescript/Cargo.toml +++ b/typescript/Cargo.toml @@ -16,6 +16,10 @@ strip = true # Workaround for https://github.com/bevyengine/bevy/issues/16030 (t # must stay semver-compatible with the in-repo crate version for this to apply. rapier2d = { path = "../crates/rapier2d" } rapier3d = { path = "../crates/rapier3d" } +# Temporary: the deformable-mesh API (parry branch `soft-bodies`), to be replaced by a +# version bump when parry is released. Mirrors the patch of the root workspace. +parry2d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } +parry3d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } # parry, nalgebra and simba are still pulled from crates.io, matching the # versions the in-repo rapier crates depend on. Uncomment to build against local diff --git a/typescript/builds/prepare_builds/templates/Cargo.toml.tera b/typescript/builds/prepare_builds/templates/Cargo.toml.tera index ad6a86e41..68bd7273b 100644 --- a/typescript/builds/prepare_builds/templates/Cargo.toml.tera +++ b/typescript/builds/prepare_builds/templates/Cargo.toml.tera @@ -40,7 +40,7 @@ rapier{{ dimension }}d = { version = "0.35.0", features = [ ] } # The explicit dependency to parry only needed to pin the version; it must match # the version (and math-backend feature set) the in-repo rapier crate depends on. -parry{{ dimension }}d = { version = "0.29", default-features = false, features = [ +parry{{ dimension }}d = { version = "0.30", default-features = false, features = [ "required-features", ] } ref-cast = "1" diff --git a/typescript/src.ts/dynamics/impulse_joint_set.ts b/typescript/src.ts/dynamics/impulse_joint_set.ts index 2d382514b..46aafccc8 100644 --- a/typescript/src.ts/dynamics/impulse_joint_set.ts +++ b/typescript/src.ts/dynamics/impulse_joint_set.ts @@ -118,6 +118,18 @@ export class ImpulseJointSet { this.map.delete(handle); } + /** + * Drops the wrappers of the joints the engine removed on its own (the joints of removed + * soft bodies and clusters). + */ + public unmapRemovedJoints() { + for (let joint of this.map.getAll()) { + if (!this.raw.contains(joint.handle)) { + this.map.delete(joint.handle); + } + } + } + /** * The number of joints on this set. */ diff --git a/typescript/src.ts/dynamics/index.ts b/typescript/src.ts/dynamics/index.ts index 4141658e9..e3c132e09 100644 --- a/typescript/src.ts/dynamics/index.ts +++ b/typescript/src.ts/dynamics/index.ts @@ -8,3 +8,5 @@ export * from "./multibody_joint_set"; export * from "./coefficient_combine_rule"; export * from "./ccd_solver"; export * from "./island_manager"; +export * from "./soft_body"; +export * from "./soft_body_set"; diff --git a/typescript/src.ts/dynamics/integration_parameters.ts b/typescript/src.ts/dynamics/integration_parameters.ts index c0263bb3b..6461fc05d 100644 --- a/typescript/src.ts/dynamics/integration_parameters.ts +++ b/typescript/src.ts/dynamics/integration_parameters.ts @@ -75,10 +75,46 @@ export class IntegrationParameters { return this.raw.maxCcdSubsteps; } + /** + * Strain beyond which a soft-body constraint is re-solved after the contacts inside every + * substep, so a light body buried under heavier ones is not torn (default: `0.75`). + */ + get softBodiesResweepStrain(): number { + return this.raw.softBodiesResweepStrain; + } + + /** + * Maximum number of extra substeps a soft body requests for its island while it is hit + * fast (default: `4`; `0` disables the impact-adaptive substeps). + */ + get softBodiesMaxExtraSubsteps(): number { + return this.raw.softBodiesMaxExtraSubsteps; + } + + /** + * Factor applied to the contact softness natural frequencies for the soft-body contacts + * (default: `4.0`). + */ + get softBodiesContactStiffening(): number { + return this.raw.softBodiesContactStiffening; + } + set dt(value: number) { this.raw.dt = value; } + set softBodiesResweepStrain(value: number) { + this.raw.softBodiesResweepStrain = value; + } + + set softBodiesMaxExtraSubsteps(value: number) { + this.raw.softBodiesMaxExtraSubsteps = value; + } + + set softBodiesContactStiffening(value: number) { + this.raw.softBodiesContactStiffening = value; + } + set contact_natural_frequency(value: number) { this.raw.contact_natural_frequency = value; } diff --git a/typescript/src.ts/dynamics/multibody_joint_set.ts b/typescript/src.ts/dynamics/multibody_joint_set.ts index 2fbb34030..2eed76c30 100644 --- a/typescript/src.ts/dynamics/multibody_joint_set.ts +++ b/typescript/src.ts/dynamics/multibody_joint_set.ts @@ -98,6 +98,18 @@ export class MultibodyJointSet { this.map.delete(handle); } + /** + * Drops the wrappers of the joints the engine removed on its own (the joints of removed + * soft bodies and clusters). + */ + public unmapRemovedJoints() { + for (let joint of this.map.getAll()) { + if (!this.raw.contains(joint.handle)) { + this.map.delete(joint.handle); + } + } + } + /** * The number of joints on this set. */ diff --git a/typescript/src.ts/dynamics/rigid_body.ts b/typescript/src.ts/dynamics/rigid_body.ts index 8363f3754..ad6880c23 100644 --- a/typescript/src.ts/dynamics/rigid_body.ts +++ b/typescript/src.ts/dynamics/rigid_body.ts @@ -41,6 +41,12 @@ export enum RigidBodyType { * modified by the user and is independent from any contact or joint it is involved in. */ KinematicVelocityBased, + /** + * A `RigidBodyType::SoftFrame` body is the proxy of a soft-body cluster: it stands for the + * cluster in joints and islands, holds its colliders, and is moved by the cluster's + * particles. Its velocity and mass properties come from the cluster. + */ + SoftFrame, } /** @@ -247,6 +253,45 @@ export class RigidBody { return this.rawSet.rbAdditionalSolverIterations(this.handle); } + /** + * The additional number of internal PGS iterations run per substep for the island + * component containing this rigid-body (default: `0`). + */ + public additionalPgsIterations(): number { + return this.rawSet.rbAdditionalPgsIterations(this.handle); + } + + /** + * Sets the additional number of internal PGS iterations run per substep for the island + * component containing this rigid-body; the component runs the largest request. + */ + public setAdditionalPgsIterations(iterations: number) { + this.rawSet.rbSetAdditionalPgsIterations(this.handle, iterations); + } + + /** + * Is this rigid-body the proxy of a soft-body cluster? + */ + public isSoftFrame(): boolean { + return this.rawSet.rbIsSoftFrame(this.handle); + } + + /** + * The handle of the soft body this rigid-body is a cluster proxy of, if any. + */ + public softBody(): number | null { + let handle = this.rawSet.rbSoftBody(this.handle); + return handle === undefined ? null : handle; + } + + /** + * The index of the cluster this proxy stands for in its soft body, if any. + */ + public softCluster(): number | null { + let cluster = this.rawSet.rbSoftCluster(this.handle); + return cluster === undefined ? null : cluster; + } + /** * Sets the number of additional solver iterations that will be run for this * rigid-body and everything that interacts with it directly or indirectly @@ -1215,6 +1260,7 @@ export class RigidBodyDesc { softCcdPrediction: number; dominanceGroup: number; additionalSolverIterations: number; + additionalPgsIterations: number; userData?: unknown; constructor(status: RigidBodyType) { @@ -1251,6 +1297,7 @@ export class RigidBodyDesc { this.softCcdPrediction = 0.0; this.dominanceGroup = 0; this.additionalSolverIterations = 0; + this.additionalPgsIterations = 0; } /** @@ -1338,6 +1385,17 @@ export class RigidBodyDesc { return this; } + /** + * Sets the additional number of internal PGS iterations run per substep for the island + * component of this rigid-body (default: `0`). + * + * The component runs the largest request of its bodies; soft bodies ask for three. + */ + public setAdditionalPgsIterations(iters: number): RigidBodyDesc { + this.additionalPgsIterations = iters; + return this; + } + /** * Sets whether the created rigid-body will be enabled or disabled. * @param enabled − If set to `false` the rigid-body will be disabled at creation. diff --git a/typescript/src.ts/dynamics/rigid_body_set.ts b/typescript/src.ts/dynamics/rigid_body_set.ts index 4c2e47bd5..cb6191d1f 100644 --- a/typescript/src.ts/dynamics/rigid_body_set.ts +++ b/typescript/src.ts/dynamics/rigid_body_set.ts @@ -11,6 +11,7 @@ import {ColliderSet} from "../geometry"; import {ImpulseJointSet} from "./impulse_joint_set"; import {MultibodyJointSet} from "./multibody_joint_set"; import {IslandManager} from "./island_manager"; +import {SoftBodySet} from "./soft_body_set"; /** * A set of rigid bodies that can be handled by a physics pipeline. @@ -115,6 +116,7 @@ export class RigidBodySet { desc.ccdEnabled, desc.dominanceGroup, desc.additionalSolverIterations, + desc.additionalPgsIterations, ); rawTra.free(); @@ -143,6 +145,7 @@ export class RigidBodySet { * * @param handle - The integer handle of the rigid-body to remove. * @param colliders - The set of colliders that may contain colliders attached to the removed rigid-body. + * @param softBodies - The set of soft bodies: removing a cluster proxy removes its cluster. * @param impulseJoints - The set of impulse joints that may contain joints attached to the removed rigid-body. * @param multibodyJoints - The set of multibody joints that may contain joints attached to the removed rigid-body. */ @@ -150,6 +153,7 @@ export class RigidBodySet { handle: RigidBodyHandle, islands: IslandManager, colliders: ColliderSet, + softBodies: SoftBodySet, impulseJoints: ImpulseJointSet, multibodyJoints: MultibodyJointSet, ) { @@ -171,10 +175,40 @@ export class RigidBodySet { handle, islands.raw, colliders.raw, + softBodies.raw, impulseJoints.raw, multibodyJoints.raw, ); this.map.delete(handle); + // A cluster proxy takes its cluster's colliders with it. + colliders.unmapRemovedColliders(); + } + + /** + * Wraps the rigid-bodies the engine created on its own (the proxies of soft bodies and + * their clusters) that have no JavaScript wrapper yet. + */ + public mapNewBodies(colliders: ColliderSet) { + this.raw.forEachRigidBodyHandle((handle: RigidBodyHandle) => { + if (!this.map.get(handle)) { + this.map.set( + handle, + new RigidBody(this.raw, colliders, handle), + ); + } + }); + } + + /** + * Drops the wrappers of the rigid-bodies the engine removed on its own (the proxies of + * removed soft bodies and clusters). + */ + public unmapRemovedBodies() { + for (let body of this.map.getAll()) { + if (!this.raw.contains(body.handle)) { + this.map.delete(body.handle); + } + } } /** diff --git a/typescript/src.ts/dynamics/soft_body.ts b/typescript/src.ts/dynamics/soft_body.ts new file mode 100644 index 000000000..54b302176 --- /dev/null +++ b/typescript/src.ts/dynamics/soft_body.ts @@ -0,0 +1,2137 @@ +import { + RawSoftBodyBuilder, + RawSoftBodyMaterial, + RawSoftBodySet, + RawSoftBodyTearEvent, + RawSoftBodyCellModel, + RawSoftEdgePlasticFlow, + RawSoftMeshBindingMode, +} from "../raw"; +import {Rotation, RotationOps, Vector, VectorOps, scratchBuffer} from "../math"; +import {RigidBody, RigidBodyHandle} from "./rigid_body"; +import {RigidBodySet} from "./rigid_body_set"; +import {Collider, ColliderDesc, ColliderSet} from "../geometry"; + +/** + * The integer identifier of a soft body added to a `SoftBodySet`. + */ +export type SoftBodyHandle = number; + +/** + * The constitutive model of a soft body's cells (triangles in 2D, tetrahedra in 3D). + */ +export enum SoftBodyCellModel { + /** + * Per-cell area/volume preservation constraints (the cell's shape is held by its edges). + */ + Volume = 0, + /** + * Corotational linear elasticity: a linear material in the cell's rotated frame. + */ + Corotational = 1, + /** + * Neo-Hookean hyperelasticity, which resists inversion and large compressions. + */ + NeoHookean = 2, +} + +/** + * Which strains make a soft body's edge rest lengths flow plastically. + */ +export enum SoftEdgePlasticFlow { + Both = 0, + Compression = 1, + Tension = 2, +} + +/** + * How the vertices of a deformable collider follow the particles of its cluster. + */ +export enum SoftMeshBindingMode { + /** + * Vertex `i` of the mesh follows the `i`-th particle of the binding's particle list. + */ + Direct = 0, + /** + * Every vertex follows the particle of the cluster closest to it. + */ + DirectByPosition = 1, + /** + * Every vertex is embedded in the cell of the cluster holding it (cage simulation). + */ + Skinned = 2, +} + +/** + * Describes how a deformable collider is bound to the cluster of a soft body. + */ +export class SoftMeshBinding { + mode: SoftMeshBindingMode; + /** + * For `SoftMeshBindingMode.Direct`: the particle followed by each vertex of the mesh. + */ + particles: Uint32Array; + /** + * For `SoftMeshBindingMode.DirectByPosition`: the largest distance between a vertex and + * the particle it follows. + */ + eps: number; + /** + * Whether the mesh collides with itself. + */ + selfContacts: boolean; + + constructor(mode: SoftMeshBindingMode) { + this.mode = mode; + this.particles = new Uint32Array(0); + this.eps = 0.0; + this.selfContacts = false; + } + + /** + * Binds vertex `i` of the mesh to `particles[i]`, which must belong to the cluster. + */ + public static direct(particles: Uint32Array | number[]): SoftMeshBinding { + let res = new SoftMeshBinding(SoftMeshBindingMode.Direct); + res.particles = Uint32Array.from(particles); + return res; + } + + /** + * Binds every vertex to the particle of the cluster closest to it, within `eps`. + */ + public static directByPosition(eps: number): SoftMeshBinding { + let res = new SoftMeshBinding(SoftMeshBindingMode.DirectByPosition); + res.eps = eps; + return res; + } + + /** + * Binds every vertex to the cell of the cluster holding it (cage simulation). + */ + public static skinned(): SoftMeshBinding { + return new SoftMeshBinding(SoftMeshBindingMode.Skinned); + } + + /** + * Enables collisions of this mesh with itself. + */ + public setSelfContacts(enabled: boolean): SoftMeshBinding { + this.selfContacts = enabled; + return this; + } + + /** @internal */ + public rawMode(): RawSoftMeshBindingMode { + return this.mode as number as RawSoftMeshBindingMode; + } +} + +/** + * Spring coefficients of an elastic constraint: a natural frequency (Hz) and a damping ratio. + */ +export interface SpringCoefficients { + naturalFrequency: number; + dampingRatio: number; +} + +/** + * The material of a soft body: the softness of its constraints, its elasticity, plasticity + * and tearing thresholds. + * + * A material is a plain JavaScript object; pass it to `SoftBodyDesc.setMaterial` or + * `SoftBody.setMaterial` to apply it. + */ +export class SoftBodyMaterial { + /** + * Softness of the structural edges. + */ + edgeSoftness: SpringCoefficients; + /** + * Softness of the bending constraints (bend edges and dihedrals). + */ + bendSoftness: SpringCoefficients; + /** + * Softness of the area/volume preservation constraints. + */ + volumeSoftness: SpringCoefficients; + /** + * Softness of the shape-matching constraints. + */ + shapeMatchingSoftness: SpringCoefficients; + /** + * Young's modulus of the elastic cells (Corotational and NeoHookean cell models). + */ + youngModulus: number; + /** + * Poisson's ratio of the elastic cells. + */ + poissonRatio: number; + /** + * Damping ratio of the elastic cells. + */ + elasticDampingRatio: number; + /** + * Strain beyond which the rest shape of an elastic cell flows (`0` disables plasticity). + */ + plasticYield: number; + /** + * Rate (per second) at which an elastic cell's rest shape follows its deformation past the + * yield. + */ + plasticCreep: number; + /** + * Largest accumulated plastic deformation of an elastic cell. + */ + plasticMax: number; + /** + * Damping of the deformation velocity of the elastic cells. + */ + deformationDamping: number; + /** + * Strain beyond which the rest length of an edge flows (`0` disables edge plasticity). + */ + edgePlasticYield: number; + /** + * Rate (per second) at which an edge's rest length follows its stretch past the yield. + */ + edgePlasticCreep: number; + /** + * Largest relative change of an edge's rest length. + */ + edgePlasticMax: number; + /** + * Which strains make the edge rest lengths flow. + */ + edgePlasticFlow: SoftEdgePlasticFlow; + /** + * Strain beyond which an element tears, or `null` for no strain-based tearing. + */ + tearStrain: number | null; + /** + * Force beyond which an element tears, or `null` for no force-based tearing. + */ + tearForce: number | null; + /** + * Time constant (seconds) of the smoothing applied to the load before comparing it to + * `tearForce`. + */ + tearSmoothing: number; + /** + * Multiplier of the tear thresholds for the interior elements (relative to the surface). + */ + interiorStrength: number; + /** + * Largest number of elements torn per step. + */ + maxTearsPerStep: number; + /** + * Smallest piece (in elements) a tear may split off, or `null` for the default. + */ + minPiece: number | null; + + constructor() { + SoftBodyMaterial.copyFromRaw(this, new RawSoftBodyMaterial(), true); + } + + /** + * A material whose edge, bend, volume and shape-matching softness all take the same value. + */ + public static uniform( + naturalFrequency: number, + dampingRatio: number, + ): SoftBodyMaterial { + let res = new SoftBodyMaterial(); + let raw = RawSoftBodyMaterial.uniform(naturalFrequency, dampingRatio); + SoftBodyMaterial.copyFromRaw(res, raw, true); + return res; + } + + /** @internal */ + public static fromRaw(raw: RawSoftBodyMaterial): SoftBodyMaterial { + let res = new SoftBodyMaterial(); + SoftBodyMaterial.copyFromRaw(res, raw, true); + return res; + } + + /** @internal */ + private static copyFromRaw( + target: SoftBodyMaterial, + raw: RawSoftBodyMaterial, + freeRaw: boolean, + ) { + target.edgeSoftness = { + naturalFrequency: raw.edgeFrequency, + dampingRatio: raw.edgeDampingRatio, + }; + target.bendSoftness = { + naturalFrequency: raw.bendFrequency, + dampingRatio: raw.bendDampingRatio, + }; + target.volumeSoftness = { + naturalFrequency: raw.volumeFrequency, + dampingRatio: raw.volumeDampingRatio, + }; + target.shapeMatchingSoftness = { + naturalFrequency: raw.shapeMatchingFrequency, + dampingRatio: raw.shapeMatchingDampingRatio, + }; + target.youngModulus = raw.youngModulus; + target.poissonRatio = raw.poissonRatio; + target.elasticDampingRatio = raw.elasticDampingRatio; + target.plasticYield = raw.plasticYield; + target.plasticCreep = raw.plasticCreep; + target.plasticMax = raw.plasticMax; + target.deformationDamping = raw.deformationDamping; + target.edgePlasticYield = raw.edgePlasticYield; + target.edgePlasticCreep = raw.edgePlasticCreep; + target.edgePlasticMax = raw.edgePlasticMax; + target.edgePlasticFlow = + raw.edgePlasticFlow as number as SoftEdgePlasticFlow; + target.tearStrain = raw.tearStrain ?? null; + target.tearForce = raw.tearForce ?? null; + target.tearSmoothing = raw.tearSmoothing; + target.interiorStrength = raw.interiorStrength; + target.maxTearsPerStep = raw.maxTearsPerStep; + target.minPiece = raw.minPiece ?? null; + if (freeRaw) { + raw.free(); + } + } + + /** @internal */ + public intoRaw(): RawSoftBodyMaterial { + let raw = new RawSoftBodyMaterial(); + raw.edgeFrequency = this.edgeSoftness.naturalFrequency; + raw.edgeDampingRatio = this.edgeSoftness.dampingRatio; + raw.bendFrequency = this.bendSoftness.naturalFrequency; + raw.bendDampingRatio = this.bendSoftness.dampingRatio; + raw.volumeFrequency = this.volumeSoftness.naturalFrequency; + raw.volumeDampingRatio = this.volumeSoftness.dampingRatio; + raw.shapeMatchingFrequency = + this.shapeMatchingSoftness.naturalFrequency; + raw.shapeMatchingDampingRatio = this.shapeMatchingSoftness.dampingRatio; + raw.youngModulus = this.youngModulus; + raw.poissonRatio = this.poissonRatio; + raw.elasticDampingRatio = this.elasticDampingRatio; + raw.plasticYield = this.plasticYield; + raw.plasticCreep = this.plasticCreep; + raw.plasticMax = this.plasticMax; + raw.deformationDamping = this.deformationDamping; + raw.edgePlasticYield = this.edgePlasticYield; + raw.edgePlasticCreep = this.edgePlasticCreep; + raw.edgePlasticMax = this.edgePlasticMax; + raw.edgePlasticFlow = this + .edgePlasticFlow as number as RawSoftEdgePlasticFlow; + raw.tearStrain = this.tearStrain ?? undefined; + raw.tearForce = this.tearForce ?? undefined; + raw.tearSmoothing = this.tearSmoothing; + raw.interiorStrength = this.interiorStrength; + raw.maxTearsPerStep = this.maxTearsPerStep; + raw.minPiece = this.minPiece ?? undefined; + return raw; + } +} + +/** + * A soft body: particles linked by elastic constraints (edges, bending constraints and + * cells), simulated together with the rigid bodies, contacts and joints of the world. + * + * A soft body lives in a `SoftBodySet` and is referenced by its integer `handle`. Its hidden + * root rigid body (`rootBody()`) stands for the whole body in joints and islands; its clusters + * add more rigid-body proxies that joints and colliders can attach to. + */ +export class SoftBody { + private rawSet: RawSoftBodySet; // The SoftBody won't need to free this. + private bodies: RigidBodySet; + private colliders: ColliderSet; + readonly handle: SoftBodyHandle; + + /** + * An arbitrary user-defined object associated with this soft body. + */ + public userData?: unknown; + + constructor( + rawSet: RawSoftBodySet, + bodies: RigidBodySet, + colliders: ColliderSet, + handle: SoftBodyHandle, + ) { + this.rawSet = rawSet; + this.bodies = bodies; + this.colliders = colliders; + this.handle = handle; + } + + /** @internal */ + public finalizeDeserialization( + bodies: RigidBodySet, + colliders: ColliderSet, + ) { + this.bodies = bodies; + this.colliders = colliders; + } + + /** + * Checks if this soft body is still valid (i.e. that it has not been deleted from the + * soft-body set yet). + */ + public isValid(): boolean { + return this.rawSet.contains(this.handle); + } + + /** + * A counter incremented by every change of the body's topology (tears, cuts, splits). + */ + public topologyVersion(): number { + return this.rawSet.sbTopologyVersion(this.handle); + } + + /* + * Particles. + */ + + /** + * The number of particles of this soft body. + */ + public numParticles(): number { + return this.rawSet.sbNumParticles(this.handle); + } + + /** + * The world-space position of the `i`-th particle. + */ + public particlePosition(i: number, target?: Vector): Vector { + this.rawSet.sbParticlePosition(this.handle, i, scratchBuffer); + return VectorOps.fromBuffer(scratchBuffer, target); + } + + /** + * The world-space positions of every particle, flattened (two or three floats per + * particle). + */ + public particlePositions(): Float32Array { + return this.rawSet.sbParticlePositions(this.handle); + } + + /** + * The velocity of the `i`-th particle. + */ + public particleVelocity(i: number, target?: Vector): Vector { + this.rawSet.sbParticleVelocity(this.handle, i, scratchBuffer); + return VectorOps.fromBuffer(scratchBuffer, target); + } + + /** + * The velocities of every particle, flattened (two or three floats per particle). + */ + public particleVelocities(): Float32Array { + return this.rawSet.sbParticleVelocities(this.handle); + } + + /** + * The rest position of the `i`-th particle (the position its constraints hold it at). + */ + public particleRestPosition(i: number, target?: Vector): Vector { + this.rawSet.sbParticleRestPosition(this.handle, i, scratchBuffer); + return VectorOps.fromBuffer(scratchBuffer, target); + } + + /** + * The mass of the `i`-th particle. + */ + public particleMass(i: number): number { + return this.rawSet.sbParticleMass(this.handle, i); + } + + /** + * Is the `i`-th particle pinned (held in place)? + */ + public isParticlePinned(i: number): boolean { + return this.rawSet.sbIsParticlePinned(this.handle, i); + } + + /** + * Does the `i`-th particle lie on the body's surface? + */ + public isParticleOnSurface(i: number): boolean { + return this.rawSet.sbIsParticleOnSurface(this.handle, i); + } + + /** + * Has an element of the `i`-th particle torn? + */ + public isParticleDamaged(i: number): boolean { + return this.rawSet.sbIsParticleDamaged(this.handle, i); + } + + /** + * Sets the world-space position of the `i`-th particle. + */ + public setParticlePosition(i: number, position: Vector) { + let rawPos = VectorOps.intoRaw(position); + this.rawSet.sbSetParticlePosition(this.handle, i, rawPos); + rawPos.free(); + } + + /** + * Sets the velocity of the `i`-th particle. + */ + public setParticleVelocity(i: number, velocity: Vector) { + let rawVel = VectorOps.intoRaw(velocity); + this.rawSet.sbSetParticleVelocity(this.handle, i, rawVel); + rawVel.free(); + } + + /** + * Moves the pinned `i`-th particle to `position` over the next step, like a + * position-based kinematic body (it pushes what it meets), then holds it there. + * Ignored for a free particle. + */ + public setParticleKinematicTarget(i: number, position: Vector) { + let rawPos = VectorOps.intoRaw(position); + this.rawSet.sbSetParticleKinematicTarget(this.handle, i, rawPos); + rawPos.free(); + } + + /** + * Pins (or releases) the `i`-th particle. + */ + public setParticlePinned(i: number, pinned: boolean) { + this.rawSet.sbSetParticlePinned(this.handle, i, pinned); + } + + /** + * Attaches the `i`-th particle to a rigid body, at the particle's current position. + */ + public attachParticle(i: number, body: RigidBody) { + this.rawSet.sbAttachParticle( + this.handle, + i, + body.handle, + this.bodies.raw, + ); + } + + /** + * Detaches the `i`-th particle from the rigid body it was attached to. + * + * Returns `false` if the particle was not attached. + */ + public detachParticle(i: number): boolean { + return this.rawSet.sbDetachParticle(this.handle, i); + } + + /** + * The number of particles attached to rigid bodies. + */ + public numAttachments(): number { + return this.rawSet.sbNumAttachments(this.handle); + } + + /** + * The particle of the `i`-th attachment. + */ + public attachmentParticle(i: number): number { + return this.rawSet.sbAttachmentParticle(this.handle, i); + } + + /** + * The rigid body of the `i`-th attachment. + */ + public attachmentBody(i: number): RigidBody { + return this.bodies.get(this.rawSet.sbAttachmentBody(this.handle, i)); + } + + /* + * Elements. + */ + + /** + * The number of edges (structural and bending) of this soft body. + */ + public numEdges(): number { + return this.rawSet.sbNumEdges(this.handle); + } + + /** + * The particle pairs of every edge, flattened (two indices per edge). + */ + public edges(): Uint32Array { + return this.rawSet.sbEdges(this.handle); + } + + /** + * The rest length of the `i`-th edge. + */ + public edgeRestLength(i: number): number { + return this.rawSet.sbEdgeRestLength(this.handle, i); + } + + /** + * Is the `i`-th edge a bending edge (rather than a structural one)? + */ + public isEdgeBend(i: number): boolean { + return this.rawSet.sbEdgeIsBend(this.handle, i); + } + + /** + * The impulse applied by the `i`-th edge during the last step. + */ + public edgeImpulse(i: number): number { + return this.rawSet.sbEdgeImpulse(this.handle, i); + } + + /** + * The load of the `i`-th edge relative to its tear threshold (`1` tears it). + */ + public edgeStress(i: number): number { + return this.rawSet.sbEdgeStress(this.handle, i); + } + + /** + * The relative change of the `i`-th edge's rest length due to plastic flow. + */ + public edgePlasticStrain(i: number): number { + return this.rawSet.sbEdgePlasticStrain(this.handle, i); + } + + /** + * The multiplier of the tear thresholds of the `i`-th edge. + */ + public edgeTearResistance(i: number): number { + return this.rawSet.sbEdgeTearResistance(this.handle, i); + } + + /** + * The number of cells (triangles in 2D, tetrahedra in 3D) of this soft body. + */ + public numCells(): number { + return this.rawSet.sbNumCells(this.handle); + } + + /** + * The particles of every cell, flattened (three indices per cell in 2D, four in 3D). + */ + public cells(): Uint32Array { + return this.rawSet.sbCells(this.handle); + } + + /** + * The rest area/volume of the `i`-th cell. + */ + public cellRestVolume(i: number): number { + return this.rawSet.sbCellRestVolume(this.handle, i); + } + + /** + * The load of the `i`-th cell relative to its tear threshold (`1` tears it). + */ + public cellStress(i: number): number { + return this.rawSet.sbCellStress(this.handle, i); + } + + /** + * The stiffness multiplier of the `i`-th cell. + */ + public cellStiffnessScale(i: number): number { + return this.rawSet.sbCellStiffnessScale(this.handle, i); + } + + /** + * The multiplier of the tear thresholds of the `i`-th cell. + */ + public cellTearResistance(i: number): number { + return this.rawSet.sbCellTearResistance(this.handle, i); + } + + // #if DIM3 + /** + * The number of dihedral bending constraints of this soft body. + */ + public numDihedrals(): number { + return this.rawSet.sbNumDihedrals(this.handle); + } + + /** + * The particles of every dihedral, flattened (four indices per dihedral: the shared edge, + * then the two opposite vertices). + */ + public dihedrals(): Uint32Array { + return this.rawSet.sbDihedrals(this.handle); + } + + /** + * The rest angle of the `i`-th dihedral. + */ + public dihedralRestAngle(i: number): number { + return this.rawSet.sbDihedralRestAngle(this.handle, i); + } + // #endif + + /** + * The boundary elements of this soft body (segments in 2D, triangles in 3D), flattened + * (two or three particle indices per element), oriented outward. + */ + public boundary(): Uint32Array { + return this.rawSet.sbBoundary(this.handle); + } + + /* + * Material. + */ + + /** + * A copy of this soft body's material. + */ + public material(): SoftBodyMaterial { + return SoftBodyMaterial.fromRaw(this.rawSet.sbMaterial(this.handle)); + } + + /** + * Replaces this soft body's material. + */ + public setMaterial(material: SoftBodyMaterial) { + let raw = material.intoRaw(); + this.rawSet.sbSetMaterial(this.handle, raw); + raw.free(); + } + + /** + * The constitutive model of this soft body's cells. + */ + public cellModel(): SoftBodyCellModel { + return this.rawSet.sbCellModel( + this.handle, + ) as number as SoftBodyCellModel; + } + + /** + * Is the global area/volume preservation of this soft body enabled? + */ + public volumePreservationEnabled(): boolean { + return this.rawSet.sbVolumePreservationEnabled(this.handle); + } + + /** + * Enables or disables the global area/volume preservation of this soft body. + */ + public enableVolumePreservation(enabled: boolean) { + this.rawSet.sbEnableVolumePreservation(this.handle, enabled); + } + + /** + * The rest area/volume enclosed by this soft body's closed surfaces. + */ + public restVolume(): number { + return this.rawSet.sbRestVolume(this.handle); + } + + /** + * The current area/volume enclosed by this soft body's closed surfaces. + */ + public volume(): number { + return this.rawSet.sbVolume(this.handle); + } + + /** + * The target volume multiplier of the volume preservation (`> 1` inflates the body). + */ + public volumeFactor(): number { + return this.rawSet.sbVolumeFactor(this.handle); + } + + /** + * Sets the target volume multiplier of the volume preservation (`> 1` inflates the body). + */ + public setVolumeFactor(factor: number) { + this.rawSet.sbSetVolumeFactor(this.handle, factor); + } + + /** + * The thickness of this soft body's particles. + */ + public particleRadius(): number { + return this.rawSet.sbParticleRadius(this.handle); + } + + /** + * Forgets every plastic deformation: the rest shapes return to their initial values. + */ + public resetPlasticity() { + this.rawSet.sbResetPlasticity(this.handle); + } + + /* + * Whole-body state. + */ + + /** + * The hidden rigid body standing for this soft body in joints and islands. + */ + public rootBody(): RigidBody { + return this.bodies.get(this.rawSet.sbRootBody(this.handle)); + } + + /** + * The soft body this one was split off from by a tear, if any. + */ + public origin(): SoftBodyHandle | null { + let origin = this.rawSet.sbOrigin(this.handle); + return origin === undefined ? null : origin; + } + + /** + * The handles of the soft bodies that tears split off from this one. + */ + public pieces(): SoftBodyHandle[] { + return Array.from(this.rawSet.sbPieces(this.handle)); + } + + /** + * The center of mass of this soft body's particles. + */ + public centerOfMass(target?: Vector): Vector { + this.rawSet.sbCenterOfMass(this.handle, scratchBuffer); + return VectorOps.fromBuffer(scratchBuffer, target); + } + + /** + * The total mass of this soft body's particles. + */ + public mass(): number { + return this.rawSet.sbMass(this.handle); + } + + /** + * Is this soft body sleeping? + */ + public isSleeping(): boolean { + return this.rawSet.sbIsSleeping(this.handle); + } + + /** + * Wakes this soft body up. + */ + public wakeUp() { + this.rawSet.sbWakeUp(this.handle); + } + + /** + * Is this soft body enabled (simulated)? + */ + public isEnabled(): boolean { + return this.rawSet.sbIsEnabled(this.handle); + } + + /** + * Enables or disables this soft body. + */ + public setEnabled(enabled: boolean) { + this.rawSet.sbSetEnabled(this.handle, enabled); + } + + /** + * Sets the extra internal PGS iterations run per substep for this soft body and + * everything it touches. + */ + public setAdditionalPgsIterations(iterations: number) { + this.rawSet.sbSetAdditionalPgsIterations(this.handle, iterations); + } + + /** + * The linear damping of this soft body's particles. + */ + public linearDamping(): number { + return this.rawSet.sbLinearDamping(this.handle); + } + + /** + * The gravity scale of this soft body's particles. + */ + public gravityScale(): number { + return this.rawSet.sbGravityScale(this.handle); + } + + /* + * Forces and impulses. + */ + + /** + * Adds a force to every particle of this soft body (spread by mass). + */ + public addForce(force: Vector, wakeUp: boolean) { + let rawForce = VectorOps.intoRaw(force); + this.rawSet.sbAddForce(this.handle, rawForce, wakeUp); + rawForce.free(); + } + + /** + * Adds a force to the `i`-th particle of this soft body. + */ + public addParticleForce(i: number, force: Vector, wakeUp: boolean) { + let rawForce = VectorOps.intoRaw(force); + this.rawSet.sbAddParticleForce(this.handle, i, rawForce, wakeUp); + rawForce.free(); + } + + /** + * Resets the user forces applied to this soft body's particles. + */ + public resetForces(wakeUp: boolean) { + this.rawSet.sbResetForces(this.handle, wakeUp); + } + + /** + * Applies an impulse to every particle of this soft body (spread by mass). + */ + public applyImpulse(impulse: Vector, wakeUp: boolean) { + let rawImpulse = VectorOps.intoRaw(impulse); + this.rawSet.sbApplyImpulse(this.handle, rawImpulse, wakeUp); + rawImpulse.free(); + } + + /** + * Applies an impulse to the `i`-th particle of this soft body. + */ + public applyParticleImpulse(i: number, impulse: Vector, wakeUp: boolean) { + let rawImpulse = VectorOps.intoRaw(impulse); + this.rawSet.sbApplyParticleImpulse(this.handle, i, rawImpulse, wakeUp); + rawImpulse.free(); + } + + /** + * Applies an impulse to the particles within `falloffRadius` of `point`, scaled down + * linearly with their distance to it (`falloffRadius <= 0` applies it to every particle). + */ + public applyImpulseAtPoint( + impulse: Vector, + point: Vector, + falloffRadius: number, + wakeUp: boolean, + ) { + let rawImpulse = VectorOps.intoRaw(impulse); + let rawPoint = VectorOps.intoRaw(point); + this.rawSet.sbApplyImpulseAtPoint( + this.handle, + rawImpulse, + rawPoint, + falloffRadius, + wakeUp, + ); + rawImpulse.free(); + rawPoint.free(); + } + + /** + * Applies an impulse of the given magnitude pushing the particles away from `center` + * (a blast), scaled down linearly up to `falloffRadius`. + */ + public applyRadialImpulse( + center: Vector, + magnitude: number, + falloffRadius: number, + wakeUp: boolean, + ) { + let rawCenter = VectorOps.intoRaw(center); + this.rawSet.sbApplyRadialImpulse( + this.handle, + rawCenter, + magnitude, + falloffRadius, + wakeUp, + ); + rawCenter.free(); + } + + /* + * Tearing. + */ + + /** + * Requests the `i`-th edge to tear at the end of the next step. + */ + public tearEdge(i: number) { + this.rawSet.sbTearEdge(this.handle, i); + } + + /** + * Requests the `i`-th cell to tear at the end of the next step. + */ + public tearCell(i: number) { + this.rawSet.sbTearCell(this.handle, i); + } + + /** + * Are there tears requested for the next step? + */ + public hasPendingTears(): boolean { + return this.rawSet.sbHasPendingTears(this.handle); + } + + /* + * Clusters. + */ + + /** + * The number of cluster slots of this soft body (some may have been removed: see + * `isClusterLive`). + */ + public numClusters(): number { + return this.rawSet.sbNumClusters(this.handle); + } + + /** + * Does the `i`-th cluster still exist? + */ + public isClusterLive(i: number): boolean { + return this.rawSet.sbIsClusterLive(this.handle, i); + } + + /** + * The rigid body standing for the `i`-th cluster: joints and colliders attach to it. + */ + public clusterProxy(i: number): RigidBody | null { + let proxy = this.rawSet.sbClusterProxy(this.handle, i); + return proxy === undefined ? null : this.bodies.get(proxy); + } + + /** + * The particles of the `i`-th cluster. + */ + public clusterParticles(i: number): Uint32Array { + return this.rawSet.sbClusterParticles(this.handle, i); + } + + /** + * Does the `i`-th cluster hold its shape by shape matching? + */ + public clusterShapeMatchingEnabled(i: number): boolean { + return this.rawSet.sbClusterShapeMatchingEnabled(this.handle, i); + } + + /** + * Enables or disables shape matching on the `i`-th cluster. + */ + public enableClusterShapeMatching(i: number, enabled: boolean) { + this.rawSet.sbEnableClusterShapeMatching(this.handle, i, enabled); + } + + /** + * Scales the stiffness of the elements of the `i`-th cluster. + */ + public setClusterStiffnessScale(i: number, scale: number) { + this.rawSet.sbSetClusterStiffnessScale(this.handle, i, scale); + } + + /** + * Overrides the softness of the edges of the `i`-th cluster (`null` restores the + * material's). + */ + public setClusterEdgeSoftness( + i: number, + softness: SpringCoefficients | null, + ) { + this.rawSet.sbSetClusterEdgeSoftness( + this.handle, + i, + softness ? softness.naturalFrequency : undefined, + softness ? softness.dampingRatio : undefined, + ); + } + + /** + * Scales the tear thresholds of the elements of the `i`-th cluster. + */ + public setClusterTearResistance(i: number, resistance: number) { + this.rawSet.sbSetClusterTearResistance(this.handle, i, resistance); + } + + /** + * Pins (or releases) every particle of the `i`-th cluster. + */ + public setClusterPinned(i: number, pinned: boolean) { + this.rawSet.sbSetClusterPinned(this.handle, i, pinned); + } + + /** + * Moves the `i`-th cluster rigidly to the given pose over the next step. + */ + public setClusterKinematicTarget( + i: number, + translation: Vector, + rotation: Rotation, + ) { + let rawTra = VectorOps.intoRaw(translation); + let rawRot = RotationOps.intoRaw(rotation); + this.rawSet.sbSetClusterKinematicTarget(this.handle, i, rawTra, rawRot); + rawTra.free(); + rawRot.free(); + } + + /* + * Collision meshes. + */ + + /** + * The number of collision meshes held by this soft body's clusters (its own deformable + * surface included). + */ + public numMeshes(): number { + return this.rawSet.sbNumMeshes(this.handle); + } + + /** + * The cluster holding the `i`-th collision mesh. + */ + public meshCluster(i: number): number | null { + let cluster = this.rawSet.sbMeshCluster(this.handle, i); + return cluster === undefined ? null : cluster; + } + + /** + * The collider holding the `i`-th collision mesh. + */ + public meshCollider(i: number): Collider | null { + let collider = this.rawSet.sbMeshCollider(this.handle, i); + return collider === undefined ? null : this.colliders.get(collider); + } + + /** + * Is the `i`-th collision mesh skinned (embedded in the cells) rather than bound + * vertex-to-particle? + */ + public isMeshSkinned(i: number): boolean { + return this.rawSet.sbMeshIsSkinned(this.handle, i); + } + + /** + * Does the `i`-th collision mesh collide? + */ + public meshCollisionEnabled(i: number): boolean { + return this.rawSet.sbMeshCollisionEnabled(this.handle, i); + } + + /** + * The world-space vertex positions of the `i`-th collision mesh, flattened. + */ + public meshVertices(i: number): Float32Array { + return this.rawSet.sbMeshVertices(this.handle, i); + } + + /** + * The elements of the `i`-th collision mesh, flattened (two vertex indices per segment in + * 2D, three per triangle in 3D). + */ + public meshIndices(i: number): Uint32Array { + return this.rawSet.sbMeshIndices(this.handle, i); + } + + /** + * The index of the collision mesh held by a deformable collider of this soft body. + */ + public meshOfCollider(collider: Collider): number | null { + let mesh = this.rawSet.sbMeshOfCollider(this.handle, collider.handle); + return mesh === undefined ? null : mesh; + } +} + +/** + * The description of a soft body to create. + * + * Start from one of the generators (`SoftBodyDesc.rope`, `cloth`, `cuboid`, `sphere`, + * `trimesh`, `volumetric` in 3D; `rope`, `polygon`, `disk`, `grid`, `polyline`, `volumetric` + * in 2D) or from raw particle positions, then tune it with the chainable setters. The + * description holds no WASM memory: it is applied when the soft body is created. + */ +export class SoftBodyDesc { + private generator: () => RawSoftBodyBuilder; + private setters: Array<(raw: RawSoftBodyBuilder) => void>; + /** + * The material of the soft body, or `null` to keep the default one (with `softness` + * applied on top when set). + */ + material: SoftBodyMaterial | null; + /** + * A uniform softness applied to every constraint of the material, or `null`. + */ + softness: SpringCoefficients | null; + /** + * The uniform mass of the particles (ignored when `masses` is set). + */ + particleMass: number; + /** + * The total mass of the body, spread over its particles, or `null` to use `particleMass`. + */ + mass: number | null; + /** + * Per-particle masses, or `null` for uniform masses. + */ + masses: Float32Array | null; + /** + * The indices of the pinned particles. + */ + pinnedParticles: Uint32Array; + /** + * The constitutive model of the cells. + */ + cellModel: SoftBodyCellModel; + /** + * Whether the area/volume enclosed by the body's closed surfaces is preserved. + */ + volumePreservation: boolean; + /** + * The target volume multiplier of the volume preservation (`> 1` inflates the body). + */ + volumeFactor: number; + /** + * Whether shape matching holds the body's shape. + */ + shapeMatching: boolean; + /** + * Whether the body's surface collides with itself. + */ + selfContacts: boolean; + /** + * The thickness of the particles. + */ + particleRadius: number; + /** + * The template of the body's colliders (their shape is replaced by the body's deformable + * surface), or `null` for a body without collisions. + */ + surfaceCollider: ColliderDesc | null; + /** + * Whether the body collides through its skin rather than through its cells' boundary. + */ + skinCollision: boolean; + /** + * A translation applied to every particle. + */ + translation: Vector | null; + /** + * Linear damping of the particles. + */ + linearDamping: number; + /** + * Gravity scale of the particles. + */ + gravityScale: number; + /** + * Extra solver substeps requested for the body and everything it touches. + */ + additionalSolverIterations: number; + /** + * Extra internal PGS iterations per substep for the body and everything it touches. + */ + additionalPgsIterations: number; + /** + * Whether the body may fall asleep. + */ + canSleep: boolean; + /** + * The dominance group of the body. + */ + dominanceGroup: number; + /** + * An arbitrary user-defined object associated with the soft body. + */ + userData?: unknown; + + /** + * A description over raw world-space particle positions (flattened), with no element: + * add edges, cells and a surface with the setters. + */ + constructor(positions?: Float32Array | number[]) { + let flat = positions + ? Float32Array.from(positions) + : new Float32Array(0); + this.generator = () => new RawSoftBodyBuilder(flat); + this.setters = []; + this.material = null; + this.softness = null; + this.particleMass = 1.0; + this.mass = null; + this.masses = null; + this.pinnedParticles = new Uint32Array(0); + this.cellModel = SoftBodyCellModel.Volume; + this.volumePreservation = false; + this.volumeFactor = 1.0; + this.shapeMatching = false; + this.selfContacts = false; + this.particleRadius = 0.01; + this.surfaceCollider = ColliderDesc.ball(0.05); + this.skinCollision = false; + this.translation = null; + this.linearDamping = 0.0; + this.gravityScale = 1.0; + this.additionalSolverIterations = 0; + this.additionalPgsIterations = 3; + this.canSleep = true; + this.dominanceGroup = 0; + } + + private static withGenerator( + generator: () => RawSoftBodyBuilder | undefined, + ): SoftBodyDesc | null { + // Run it once to know whether the geometry is valid. + let probe = generator(); + if (!probe) { + return null; + } + probe.free(); + let desc = new SoftBodyDesc(); + desc.generator = generator as () => RawSoftBodyBuilder; + return desc; + } + + /** + * A rope of `numParticles` particles from `start` to `end`. + */ + public static rope( + start: Vector, + end: Vector, + numParticles: number, + ): SoftBodyDesc { + return SoftBodyDesc.withGenerator(() => { + let rawStart = VectorOps.intoRaw(start); + let rawEnd = VectorOps.intoRaw(end); + let raw = RawSoftBodyBuilder.rope(rawStart, rawEnd, numParticles); + rawStart.free(); + rawEnd.free(); + return raw; + }); + } + + /** + * A body filling the closed surface (segments in 2D, triangles in 3D) with cells of the + * given size; `skinned` keeps the surface as a skin embedded in the cells. Returns + * `null` if the surface cannot be meshed. + * + * @param vertices - The flattened world-space vertices of the surface. + * @param indices - The flattened elements of the surface. + */ + public static volumetric( + vertices: Float32Array, + indices: Uint32Array, + cellSize: number, + skinned: boolean = false, + ): SoftBodyDesc | null { + return SoftBodyDesc.withGenerator(() => + RawSoftBodyBuilder.volumetric(vertices, indices, cellSize, skinned), + ); + } + + // #if DIM2 + /** + * A polyline body: the vertices are particles linked by the segments (a line strip when + * `indices` is not given). Returns `null` if the polyline is empty. + */ + public static polyline( + vertices: Float32Array, + indices?: Uint32Array, + ): SoftBodyDesc | null { + let idx = indices ?? new Uint32Array(0); + return SoftBodyDesc.withGenerator(() => + RawSoftBodyBuilder.polyline(vertices, idx), + ); + } + + /** + * A closed polygon of particles (flattened points) holding its area. + */ + public static polygon(points: Float32Array | number[]): SoftBodyDesc { + let flat = Float32Array.from(points); + return SoftBodyDesc.withGenerator(() => + RawSoftBodyBuilder.polygon(flat), + ); + } + + /** + * A disk: a ring of `numParticles` particles holding its area (a pressurized blob). + */ + public static disk( + center: Vector, + radius: number, + numParticles: number, + ): SoftBodyDesc { + return SoftBodyDesc.withGenerator(() => { + let rawCenter = VectorOps.intoRaw(center); + let raw = RawSoftBodyBuilder.disk(rawCenter, radius, numParticles); + rawCenter.free(); + return raw; + }); + } + + /** + * A grid of `nx` by `ny` particles filled with triangle cells. + */ + public static grid( + center: Vector, + halfExtents: Vector, + nx: number, + ny: number, + ): SoftBodyDesc { + return SoftBodyDesc.withGenerator(() => { + let rawCenter = VectorOps.intoRaw(center); + let rawHalf = VectorOps.intoRaw(halfExtents); + let raw = RawSoftBodyBuilder.grid(rawCenter, rawHalf, nx, ny); + rawCenter.free(); + rawHalf.free(); + return raw; + }); + } + // #endif + + // #if DIM3 + /** + * A triangle-mesh body without cells: the vertices are particles, the triangles the + * surface, held by dihedral bending and shape matching. Returns `null` if the mesh is + * empty. + */ + public static trimesh( + vertices: Float32Array, + indices: Uint32Array, + ): SoftBodyDesc | null { + return SoftBodyDesc.withGenerator(() => + RawSoftBodyBuilder.trimesh(vertices, indices), + ); + } + + /** + * A cloth of `nu` by `nv` particles: particle `(i, j)` is at `origin + i * du + j * dv`. + */ + public static cloth( + origin: Vector, + du: Vector, + dv: Vector, + nu: number, + nv: number, + ): SoftBodyDesc { + return SoftBodyDesc.withGenerator(() => { + let rawOrigin = VectorOps.intoRaw(origin); + let rawDu = VectorOps.intoRaw(du); + let rawDv = VectorOps.intoRaw(dv); + let raw = RawSoftBodyBuilder.cloth(rawOrigin, rawDu, rawDv, nu, nv); + rawOrigin.free(); + rawDu.free(); + rawDv.free(); + return raw; + }); + } + + /** + * A tube of cloth around `axis`, from `radiusStart` at `origin` to `radiusEnd` at its + * other end, with `numAround` particles per ring and `numAlong` rings. + */ + public static clothTube( + origin: Vector, + axis: Vector, + radiusStart: number, + radiusEnd: number, + numAround: number, + numAlong: number, + ): SoftBodyDesc { + return SoftBodyDesc.withGenerator(() => { + let rawOrigin = VectorOps.intoRaw(origin); + let rawAxis = VectorOps.intoRaw(axis); + let raw = RawSoftBodyBuilder.clothTube( + rawOrigin, + rawAxis, + radiusStart, + radiusEnd, + numAround, + numAlong, + ); + rawOrigin.free(); + rawAxis.free(); + return raw; + }); + } + + /** + * A cloth with different softness along `du` (warp), along `dv` (weft) and across the + * diagonals (shear). + */ + public static clothAnisotropic( + origin: Vector, + du: Vector, + dv: Vector, + nu: number, + nv: number, + warp: SpringCoefficients, + weft: SpringCoefficients, + shear: SpringCoefficients, + ): SoftBodyDesc { + return SoftBodyDesc.withGenerator(() => { + let rawOrigin = VectorOps.intoRaw(origin); + let rawDu = VectorOps.intoRaw(du); + let rawDv = VectorOps.intoRaw(dv); + let raw = RawSoftBodyBuilder.clothAnisotropic( + rawOrigin, + rawDu, + rawDv, + nu, + nv, + warp.naturalFrequency, + warp.dampingRatio, + weft.naturalFrequency, + weft.dampingRatio, + shear.naturalFrequency, + shear.dampingRatio, + ); + rawOrigin.free(); + rawDu.free(); + rawDv.free(); + return raw; + }); + } + + /** + * A box of `nx` by `ny` by `nz` particles filled with tetrahedral cells. + */ + public static cuboid( + center: Vector, + halfExtents: Vector, + nx: number, + ny: number, + nz: number, + ): SoftBodyDesc { + return SoftBodyDesc.withGenerator(() => { + let rawCenter = VectorOps.intoRaw(center); + let rawHalf = VectorOps.intoRaw(halfExtents); + let raw = RawSoftBodyBuilder.cuboid(rawCenter, rawHalf, nx, ny, nz); + rawCenter.free(); + rawHalf.free(); + return raw; + }); + } + + /** + * A hollow sphere: an icosphere surface with `subdivisions` refinement levels, holding + * its volume (a balloon). + */ + public static sphere( + center: Vector, + radius: number, + subdivisions: number, + ): SoftBodyDesc { + return SoftBodyDesc.withGenerator(() => { + let rawCenter = VectorOps.intoRaw(center); + let raw = RawSoftBodyBuilder.sphere( + rawCenter, + radius, + subdivisions, + ); + rawCenter.free(); + return raw; + }); + } + // #endif + + /* + * Chainable setters. + */ + + /** + * Sets the material of the soft body. + */ + public setMaterial(material: SoftBodyMaterial): SoftBodyDesc { + this.material = material; + return this; + } + + /** + * Sets a uniform softness for every constraint of the material. + */ + public setSoftness( + naturalFrequency: number, + dampingRatio: number, + ): SoftBodyDesc { + this.softness = {naturalFrequency, dampingRatio}; + return this; + } + + /** + * Sets the uniform mass of the particles. + */ + public setParticleMass(mass: number): SoftBodyDesc { + this.particleMass = mass; + this.masses = null; + return this; + } + + /** + * Sets the total mass of the body, spread over its particles. + */ + public setMass(mass: number): SoftBodyDesc { + this.mass = mass; + return this; + } + + /** + * Sets per-particle masses. + */ + public setMasses(masses: Float32Array | number[]): SoftBodyDesc { + this.masses = Float32Array.from(masses); + return this; + } + + /** + * Pins the given particles in place. + */ + public setPinnedParticles(pinned: Uint32Array | number[]): SoftBodyDesc { + this.pinnedParticles = Uint32Array.from(pinned); + return this; + } + + /** + * Replaces the structural edges (flattened particle pairs). + */ + public setEdges(edges: Uint32Array | number[]): SoftBodyDesc { + let flat = Uint32Array.from(edges); + this.setters.push((raw) => raw.setEdges(flat)); + return this; + } + + /** + * Adds structural edges (flattened particle pairs). + */ + public addEdges(edges: Uint32Array | number[]): SoftBodyDesc { + let flat = Uint32Array.from(edges); + this.setters.push((raw) => raw.addEdges(flat)); + return this; + } + + /** + * Replaces the bending edges (flattened particle pairs). + */ + public setBendEdges(edges: Uint32Array | number[]): SoftBodyDesc { + let flat = Uint32Array.from(edges); + this.setters.push((raw) => raw.setBendEdges(flat)); + return this; + } + + /** + * Makes every edge resist stretching only (a rope or a net that folds freely). + */ + public setTensionOnly(): SoftBodyDesc { + this.setters.push((raw) => raw.setTensionOnly()); + return this; + } + + // #if DIM3 + /** + * Replaces the dihedral bending constraints (flattened quadruplets: the shared edge, then + * the two opposite vertices). + */ + public setDihedrals(dihedrals: Uint32Array | number[]): SoftBodyDesc { + let flat = Uint32Array.from(dihedrals); + this.setters.push((raw) => raw.setDihedrals(flat)); + return this; + } + + /** + * Sets the segments a body without surface collides through (a wire). + */ + public setWire(segments: Uint32Array | number[]): SoftBodyDesc { + let flat = Uint32Array.from(segments); + this.setters.push((raw) => raw.setWire(flat)); + return this; + } + // #endif + + /** + * Replaces the cells (flattened triangles in 2D, tetrahedra in 3D). + */ + public setCells(cells: Uint32Array | number[]): SoftBodyDesc { + let flat = Uint32Array.from(cells); + this.setters.push((raw) => raw.setCells(flat)); + return this; + } + + /** + * Replaces the boundary elements (flattened segments in 2D, triangles in 3D), oriented + * outward. + */ + public setSurface(surface: Uint32Array | number[]): SoftBodyDesc { + let flat = Uint32Array.from(surface); + this.setters.push((raw) => raw.setSurface(flat)); + return this; + } + + /** + * Sets a skin: a finer mesh (flattened world-space vertices and elements) embedded in the + * cells, which follows them. + */ + public setSkin( + vertices: Float32Array | number[], + indices: Uint32Array | number[], + ): SoftBodyDesc { + let flatVertices = Float32Array.from(vertices); + let flatIndices = Uint32Array.from(indices); + this.setters.push((raw) => raw.setSkin(flatVertices, flatIndices)); + return this; + } + + /** + * Makes the body collide through its skin rather than through its cells' boundary. + */ + public setSkinCollision(enabled: boolean): SoftBodyDesc { + this.skinCollision = enabled; + return this; + } + + /** + * Overrides the softness of the given edges. + */ + public setEdgeSoftness( + edges: Uint32Array | number[], + softness: SpringCoefficients[], + ): SoftBodyDesc { + let flatEdges = Uint32Array.from(edges); + let frequencies = Float32Array.from( + softness.map((s) => s.naturalFrequency), + ); + let dampings = Float32Array.from(softness.map((s) => s.dampingRatio)); + this.setters.push((raw) => + raw.setEdgeSoftness(flatEdges, frequencies, dampings), + ); + return this; + } + + /** + * Multiplies the tear thresholds of the given edges. + */ + public setEdgeTearResistance( + edges: Uint32Array | number[], + resistances: Float32Array | number[], + ): SoftBodyDesc { + let flatEdges = Uint32Array.from(edges); + let flatResistances = Float32Array.from(resistances); + this.setters.push((raw) => + raw.setEdgeTearResistance(flatEdges, flatResistances), + ); + return this; + } + + /** + * Sets the constitutive model of the cells. + */ + public setCellModel(model: SoftBodyCellModel): SoftBodyDesc { + this.cellModel = model; + return this; + } + + /** + * Enables the preservation of the area/volume enclosed by the body's closed surfaces. + */ + public setVolumePreservation(enabled: boolean): SoftBodyDesc { + this.volumePreservation = enabled; + return this; + } + + /** + * Sets the target volume multiplier (`> 1` inflates the body); this also enables the + * volume preservation. + */ + public setVolumeFactor(factor: number): SoftBodyDesc { + this.volumeFactor = factor; + this.volumePreservation = true; + return this; + } + + /** + * Holds the body's shape by shape matching. + */ + public setShapeMatching(enabled: boolean): SoftBodyDesc { + this.shapeMatching = enabled; + return this; + } + + /** + * Makes the body's surface collide with itself. + */ + public setSelfContacts(enabled: boolean): SoftBodyDesc { + this.selfContacts = enabled; + return this; + } + + /** + * Sets the thickness of the particles. + */ + public setParticleRadius(radius: number): SoftBodyDesc { + this.particleRadius = radius; + return this; + } + + /** + * Sets the template of the body's colliders: its friction, restitution, groups, events + * and other settings are kept, its shape is replaced by the body's deformable surface. + */ + public setSurfaceCollider(collider: ColliderDesc): SoftBodyDesc { + this.surfaceCollider = collider; + return this; + } + + /** + * Removes the body's colliders: it will not collide with anything. + */ + public setNoSurfaceCollider(): SoftBodyDesc { + this.surfaceCollider = null; + return this; + } + + /** + * Translates every particle. + */ + public setTranslation(translation: Vector): SoftBodyDesc { + this.translation = translation; + return this; + } + + /** + * Sets the linear damping of the particles. + */ + public setLinearDamping(damping: number): SoftBodyDesc { + this.linearDamping = damping; + return this; + } + + /** + * Sets the gravity scale of the particles. + */ + public setGravityScale(scale: number): SoftBodyDesc { + this.gravityScale = scale; + return this; + } + + /** + * Sets the extra solver substeps requested for the body and everything it touches. + */ + public setAdditionalSolverIterations(iterations: number): SoftBodyDesc { + this.additionalSolverIterations = iterations; + return this; + } + + /** + * Sets the extra internal PGS iterations per substep for the body and everything it + * touches (default: `3`). + */ + public setAdditionalPgsIterations(iterations: number): SoftBodyDesc { + this.additionalPgsIterations = iterations; + return this; + } + + /** + * Sets whether the body may fall asleep. + */ + public setCanSleep(canSleep: boolean): SoftBodyDesc { + this.canSleep = canSleep; + return this; + } + + /** + * Sets the dominance group of the body. + */ + public setDominanceGroup(group: number): SoftBodyDesc { + this.dominanceGroup = group; + return this; + } + + /** + * Sets the user data associated with the soft body. + */ + public setUserData(data: unknown): SoftBodyDesc { + this.userData = data; + return this; + } + + /** + * Appends the particles and elements of another description to this one. + */ + public append(other: SoftBodyDesc): SoftBodyDesc { + this.setters.push((raw) => { + let rawOther = other.intoRaw(); + raw.append(rawOther); + rawOther.free(); + }); + return this; + } + + /** + * Builds the WASM-side builder this description stands for. The result must be freed. + * @internal + */ + public intoRaw(): RawSoftBodyBuilder { + let raw = this.generator(); + for (let setter of this.setters) { + setter(raw); + } + if (this.material) { + let rawMaterial = this.material.intoRaw(); + raw.setMaterial(rawMaterial); + rawMaterial.free(); + } + if (this.softness) { + raw.setSoftness( + this.softness.naturalFrequency, + this.softness.dampingRatio, + ); + } + if (this.masses) { + raw.setMasses(this.masses); + } else { + raw.setParticleMass(this.particleMass); + } + if (this.mass !== null) { + raw.setMass(this.mass); + } + raw.setPinnedParticles(this.pinnedParticles); + raw.setCellModel(this.cellModel as number as RawSoftBodyCellModel); + raw.setVolumePreservation(this.volumePreservation); + raw.setVolumeFactor(this.volumeFactor); + raw.setShapeMatching(this.shapeMatching); + raw.setSelfContacts(this.selfContacts); + raw.setParticleRadius(this.particleRadius); + raw.setSkinCollision(this.skinCollision); + if (this.surfaceCollider) { + let c = this.surfaceCollider; + raw.setSurfaceCollider( + c.friction, + c.restitution, + c.frictionCombineRule, + c.restitutionCombineRule, + c.isSensor, + c.collisionGroups, + c.solverGroups, + c.activeCollisionTypes, + c.activeHooks, + c.activeEvents, + c.contactForceEventThreshold, + c.contactSkin, + ); + } else { + raw.setNoSurfaceCollider(); + } + if (this.translation) { + let rawTra = VectorOps.intoRaw(this.translation); + raw.translated(rawTra); + rawTra.free(); + } + raw.setLinearDamping(this.linearDamping); + raw.setGravityScale(this.gravityScale); + raw.setAdditionalSolverIterations(this.additionalSolverIterations); + raw.setAdditionalPgsIterations(this.additionalPgsIterations); + raw.setCanSleep(this.canSleep); + raw.setDominanceGroup(this.dominanceGroup); + return raw; + } + + /** + * The number of particles the description currently generates. + */ + public numParticles(): number { + let raw = this.intoRaw(); + let n = raw.numParticles(); + raw.free(); + return n; + } + + /** + * The flattened world-space positions of the particles the description generates. + */ + public particlePositions(): Float32Array { + let raw = this.intoRaw(); + let positions = raw.particlePositions(); + raw.free(); + return positions; + } +} + +/** + * The record of a soft body tearing: the elements it lost, the particles the tear split, + * the pieces the tear separated into soft bodies of their own, and the clusters and joints + * that moved with them. + * + * Events drained from an `EventQueue` are only valid inside the draining closure; events + * returned by `World.tearSoftBody` and `World.cutSoftBody` must be freed with `.free()`. + */ +export class SoftBodyTearEvent { + raw: RawSoftBodyTearEvent; + + constructor(raw?: RawSoftBodyTearEvent) { + this.raw = raw; + } + + public free() { + if (!!this.raw) { + this.raw.free(); + } + this.raw = undefined; + } + + /** + * The soft body that tore. + */ + public softBody(): SoftBodyHandle { + return this.raw.softBody(); + } + + /** + * The particle pairs of the edges that tore, flattened. + */ + public tornEdges(): Uint32Array { + return this.raw.tornEdges(); + } + + /** + * The particles of the cells that tore, flattened. + */ + public tornCells(): Uint32Array { + return this.raw.tornCells(); + } + + /** + * The particle pairs of the edges the tear removed, flattened. + */ + public removedEdges(): Uint32Array { + return this.raw.removedEdges(); + } + + /** + * The particles the tear split, flattened as `(original, copy)` pairs. + */ + public splitParticles(): Uint32Array { + return this.raw.splitParticles(); + } + + /** + * The particles the tear inserted. + */ + public insertedParticles(): Uint32Array { + return this.raw.insertedParticles(); + } + + /** + * The particle pairs (flattened) the tear started from. + */ + public seeds(): Uint32Array { + return this.raw.seeds(); + } + + /** + * The number of pieces the tear split off into soft bodies of their own. + */ + public numPieces(): number { + return this.raw.numPieces(); + } + + /** + * The soft body the `i`-th piece became. + */ + public pieceSoftBody(i: number): SoftBodyHandle { + return this.raw.pieceSoftBody(i); + } + + /** + * The particles (indices in the torn body) that went into the `i`-th piece. + */ + public pieceParticles(i: number): Uint32Array { + return this.raw.pieceParticles(i); + } + + /** + * The clusters that moved into the `i`-th piece, flattened as `(source, destination)` + * index pairs. + */ + public pieceClusters(i: number): Uint32Array { + return this.raw.pieceClusters(i); + } + + /** + * The number of clusters the tear split. + */ + public numClusterSplits(): number { + return this.raw.numClusterSplits(); + } + + /** + * The cluster of the torn body the `i`-th split came from. + */ + public clusterSplitSource(i: number): number { + return this.raw.clusterSplitSource(i); + } + + /** + * The soft body holding the cluster the `i`-th split created. + */ + public clusterSplitSoftBody(i: number): SoftBodyHandle { + return this.raw.clusterSplitSoftBody(i); + } + + /** + * The index of the cluster the `i`-th split created. + */ + public clusterSplitCluster(i: number): number { + return this.raw.clusterSplitCluster(i); + } + + /** + * The proxy rigid body of the cluster the `i`-th split created. + */ + public clusterSplitProxy(i: number): RigidBodyHandle { + return this.raw.clusterSplitProxy(i); + } + + /** + * Does the cluster the `i`-th split created keep the source cluster's proxy? + */ + public clusterSplitKeepsProxy(i: number): boolean { + return this.raw.clusterSplitKeepsProxy(i); + } + + /** + * The number of impulse joints the tear moved to another proxy. + */ + public numMovedJoints(): number { + return this.raw.numMovedJoints(); + } + + /** + * The `i`-th moved impulse joint. + */ + public movedJoint(i: number): number { + return this.raw.movedJoint(i); + } + + /** + * The proxy the `i`-th moved joint was attached to before the tear. + */ + public movedJointFrom(i: number): RigidBodyHandle { + return this.raw.movedJointFrom(i); + } + + /** + * The proxy the `i`-th moved joint is attached to after the tear. + */ + public movedJointTo(i: number): RigidBodyHandle { + return this.raw.movedJointTo(i); + } + + /** + * Where a particle of the torn body is after the tear: its soft body and index there, or + * `null` if the particle was removed. + */ + public particleDestination( + particle: number, + ): {softBody: SoftBodyHandle; particle: number} | null { + let body = this.raw.particleDestinationBody(particle); + let index = this.raw.particleDestinationIndex(particle); + if (body === undefined || index === undefined) { + return null; + } + return {softBody: body, particle: index}; + } +} diff --git a/typescript/src.ts/dynamics/soft_body_set.ts b/typescript/src.ts/dynamics/soft_body_set.ts new file mode 100644 index 000000000..eb38b242e --- /dev/null +++ b/typescript/src.ts/dynamics/soft_body_set.ts @@ -0,0 +1,325 @@ +import {RawSoftBodySet, RawSoftBodyTearEvent} from "../raw"; +import {Coarena} from "../coarena"; +import {VectorOps, Vector} from "../math"; +import { + SoftBody, + SoftBodyDesc, + SoftBodyHandle, + SoftBodyTearEvent, +} from "./soft_body"; +import {RigidBodySet} from "./rigid_body_set"; +import {ColliderSet} from "../geometry"; +import {ImpulseJointSet} from "./impulse_joint_set"; +import {MultibodyJointSet} from "./multibody_joint_set"; +import {IslandManager} from "./island_manager"; + +/** + * A set of soft bodies that can be handled by a physics pipeline. + * + * To avoid leaking WASM resources, this MUST be freed manually with `softBodySet.free()` + * once you are done using it (and all the soft bodies it created). + */ +export class SoftBodySet { + raw: RawSoftBodySet; + private map: Coarena; + + /** + * Release the WASM memory occupied by this soft-body set. + */ + public free() { + if (!!this.raw) { + this.raw.free(); + } + this.raw = undefined; + + if (!!this.map) { + this.map.clear(); + } + this.map = undefined; + } + + constructor(raw?: RawSoftBodySet) { + this.raw = raw || new RawSoftBodySet(); + this.map = new Coarena(); + // deserialize + if (raw) { + raw.forEachSoftBodyHandle((handle: SoftBodyHandle) => { + this.map.set(handle, new SoftBody(raw, null, null, handle)); + }); + } + } + + /** + * Internal method, do not call this explicitly. + */ + public finalizeDeserialization( + bodies: RigidBodySet, + colliders: ColliderSet, + ) { + this.map.forEach((sb) => sb.finalizeDeserialization(bodies, colliders)); + } + + /** + * Creates a new soft body, its hidden root rigid body and its colliders, and returns it. + * + * @param bodies - The set of rigid bodies receiving the body's proxies. + * @param colliders - The set of colliders receiving the body's colliders. + * @param desc - The description of the soft body to create. + */ + public createSoftBody( + bodies: RigidBodySet, + colliders: ColliderSet, + desc: SoftBodyDesc, + ): SoftBody { + let rawBuilder = desc.intoRaw(); + let handle = this.raw.insert(rawBuilder, bodies.raw, colliders.raw); + rawBuilder.free(); + + // Map the proxies and colliders the insertion created. + bodies.mapNewBodies(colliders); + colliders.mapNewColliders(bodies); + + const body = new SoftBody(this.raw, bodies, colliders, handle); + body.userData = desc.userData; + this.map.set(handle, body); + return body; + } + + /** + * Removes a soft body from this set, with its proxies, colliders and attached joints. + */ + public remove( + handle: SoftBodyHandle, + islands: IslandManager, + bodies: RigidBodySet, + colliders: ColliderSet, + impulseJoints: ImpulseJointSet, + multibodyJoints: MultibodyJointSet, + ) { + this.raw.remove( + handle, + islands.raw, + bodies.raw, + colliders.raw, + impulseJoints.raw, + multibodyJoints.raw, + ); + this.map.delete(handle); + // Unmap the proxies, colliders and joints that were removed with the soft body. + bodies.unmapRemovedBodies(); + colliders.unmapRemovedColliders(); + impulseJoints.unmapRemovedJoints(); + multibodyJoints.unmapRemovedJoints(); + } + + /** + * Adds a cluster over the given particles: a rigid proxy that joints and colliders can + * attach to. Returns the cluster's index, or `null` if no particle was valid. + */ + public addCluster( + handle: SoftBodyHandle, + particles: Uint32Array | number[], + bodies: RigidBodySet, + colliders: ColliderSet, + ): number | null { + let cluster = this.raw.addCluster( + handle, + Uint32Array.from(particles), + bodies.raw, + colliders.raw, + ); + bodies.mapNewBodies(colliders); + return cluster === undefined ? null : cluster; + } + + /** + * Removes a cluster with its proxy, colliders and joints. Returns `false` if the cluster + * did not exist. + */ + public removeCluster( + handle: SoftBodyHandle, + cluster: number, + islands: IslandManager, + bodies: RigidBodySet, + colliders: ColliderSet, + impulseJoints: ImpulseJointSet, + multibodyJoints: MultibodyJointSet, + ): boolean { + let removed = this.raw.removeCluster( + handle, + cluster, + islands.raw, + bodies.raw, + colliders.raw, + impulseJoints.raw, + multibodyJoints.raw, + ); + bodies.unmapRemovedBodies(); + colliders.unmapRemovedColliders(); + impulseJoints.unmapRemovedJoints(); + multibodyJoints.unmapRemovedJoints(); + return removed; + } + + /** + * Tears a soft body at once along the given edges and through the given cells, without + * removing material. Pieces disconnected by the tear become soft bodies of their own. + * + * Returns the tear event (which must be freed), or `null` when nothing changed. + */ + public tear( + handle: SoftBodyHandle, + edges: Uint32Array | number[], + cells: Uint32Array | number[], + islands: IslandManager, + bodies: RigidBodySet, + colliders: ColliderSet, + impulseJoints: ImpulseJointSet, + multibodyJoints: MultibodyJointSet, + ): SoftBodyTearEvent | null { + let raw = this.raw.tear( + handle, + Uint32Array.from(edges), + Uint32Array.from(cells), + islands.raw, + bodies.raw, + colliders.raw, + impulseJoints.raw, + multibodyJoints.raw, + ); + return this.finishTopologyChange( + raw, + bodies, + colliders, + impulseJoints, + multibodyJoints, + ); + } + + /** + * Cuts a soft body along a blade: a segment (two points) in 2D, a triangle (three points) + * in 3D. Pieces disconnected by the cut become soft bodies of their own. + * + * Returns the tear event (which must be freed), or `null` when nothing changed. + */ + public cut( + handle: SoftBodyHandle, + blade: Vector[], + islands: IslandManager, + bodies: RigidBodySet, + colliders: ColliderSet, + impulseJoints: ImpulseJointSet, + multibodyJoints: MultibodyJointSet, + ): SoftBodyTearEvent | null { + let flat: number[] = []; + for (let point of blade) { + flat.push(point.x, point.y); + // #if DIM3 + flat.push(point.z); + // #endif + } + let raw = this.raw.cut( + handle, + Float32Array.from(flat), + islands.raw, + bodies.raw, + colliders.raw, + impulseJoints.raw, + multibodyJoints.raw, + ); + return this.finishTopologyChange( + raw, + bodies, + colliders, + impulseJoints, + multibodyJoints, + ); + } + + /** + * Maps the soft bodies, proxies and colliders a topology change created, and unmaps what + * it removed. + */ + private finishTopologyChange( + raw: RawSoftBodyTearEvent | undefined, + bodies: RigidBodySet, + colliders: ColliderSet, + impulseJoints: ImpulseJointSet, + multibodyJoints: MultibodyJointSet, + ): SoftBodyTearEvent | null { + if (!raw) { + return null; + } + this.mapNewSoftBodies(bodies, colliders); + bodies.mapNewBodies(colliders); + colliders.mapNewColliders(bodies); + bodies.unmapRemovedBodies(); + colliders.unmapRemovedColliders(); + impulseJoints.unmapRemovedJoints(); + multibodyJoints.unmapRemovedJoints(); + return new SoftBodyTearEvent(raw); + } + + /** + * Wraps the soft bodies the engine created (the pieces a tear split off) that have no + * JavaScript wrapper yet; the pieces of a tear event are only reachable after this. + */ + public mapNewSoftBodies(bodies: RigidBodySet, colliders: ColliderSet) { + this.raw.forEachSoftBodyHandle((handle: SoftBodyHandle) => { + if (!this.map.get(handle)) { + this.map.set( + handle, + new SoftBody(this.raw, bodies, colliders, handle), + ); + } + }); + } + + /** + * Wakes a soft body and everything it touches up. + * + * @param strong - If `true` the bodies stay awake for a while even if they are at rest. + */ + public wakeUp( + handle: SoftBodyHandle, + bodies: RigidBodySet, + strong: boolean, + ) { + this.raw.wakeUp(handle, bodies.raw, strong); + } + + /** + * The number of soft bodies on this set. + */ + public len(): number { + return this.map.len(); + } + + /** + * Does this set contain a soft body with the given handle? + */ + public contains(handle: SoftBodyHandle): boolean { + return this.get(handle) != null; + } + + /** + * Gets the soft body with the given handle. + */ + public get(handle: SoftBodyHandle): SoftBody | null { + return this.map.get(handle); + } + + /** + * Applies the given closure to each soft body contained by this set. + */ + public forEach(f: (body: SoftBody) => void) { + this.map.forEach(f); + } + + /** + * Gets all soft bodies in the list. + */ + public getAll(): SoftBody[] { + return this.map.getAll(); + } +} diff --git a/typescript/src.ts/geometry/collider.ts b/typescript/src.ts/geometry/collider.ts index 1f1298fdf..daa712c61 100644 --- a/typescript/src.ts/geometry/collider.ts +++ b/typescript/src.ts/geometry/collider.ts @@ -32,7 +32,9 @@ import { ConvexPolyhedron, RoundConvexPolyhedron, HeightFieldFlags, - // #endif + // #endif, + PolylineFlags, + CompoundFlags, } from "./shape"; import {Ray, RayIntersection} from "./ray"; import {PointProjection} from "./point"; @@ -980,6 +982,23 @@ export class Collider { return this._parent; } + /** + * The handle of the soft body whose deformable collision mesh this collider holds, if + * any. + */ + public softBody(): number | null { + let handle = this.colliderSet.raw.coSoftBody(this.handle); + return handle === undefined ? null : handle; + } + + /** + * Does this collider hold a soft body's deformable collision mesh? Its vertices then + * follow the soft body's particles: read them back with `SoftBody.meshVertices`. + */ + public isDeformable(): boolean { + return this.colliderSet.raw.coIsDeformable(this.handle); + } + /** * The friction coefficient of this collider. */ @@ -1512,8 +1531,9 @@ export class ColliderDesc { public static polyline( vertices: Float32Array, indices?: Uint32Array | null, + flags?: PolylineFlags, ): ColliderDesc { - const shape = new Polyline(vertices, indices); + const shape = new Polyline(vertices, indices, flags); return new ColliderDesc(shape); } @@ -1835,13 +1855,15 @@ export class ColliderDesc { * not allowed). * @param positions - The array of positions for each shape (relative to the compound's origin). * @param rotations - The array of rotations for each shape (relative to the compound's orientation). + * @param flags - The compound flags. */ public static compound( shapes: Shape[], positions: Vector[], rotations: Rotation[], + flags?: CompoundFlags, ): ColliderDesc { - const shape = new Compound(shapes, positions, rotations); + const shape = new Compound(shapes, positions, rotations, flags); return new ColliderDesc(shape); } @@ -1852,6 +1874,7 @@ export class ColliderDesc { * @param vertices - The coordinates of the mesh's vertices. * @param indices - The indices of the mesh's triangles (in 3D) or segments (in 2D). * @param params - Optional VHACD parameters to control the decomposition. + * @param flags - The flags of the resulting compound shape. * @returns The collider descriptor, or `null` if the decomposition did not produce * any convex part (e.g. if the input mesh is degenerate). */ @@ -1859,8 +1882,10 @@ export class ColliderDesc { vertices: Float32Array, indices: Uint32Array, params?: VHACDParameters, + flags?: CompoundFlags, ): ColliderDesc | null { let rawShape: RawShape; + const rawFlags = flags ?? 0; if (params) { // Convert TypeScript params to Rust params @@ -1886,10 +1911,15 @@ export class ColliderDesc { vertices, indices, rawParams, + rawFlags, ); rawParams.free(); } else { - rawShape = RawShape.convexDecomposition(vertices, indices); + rawShape = RawShape.convexDecomposition( + vertices, + indices, + rawFlags, + ); } if (!rawShape) { diff --git a/typescript/src.ts/geometry/collider_set.ts b/typescript/src.ts/geometry/collider_set.ts index 832b837a2..f1c7055cf 100644 --- a/typescript/src.ts/geometry/collider_set.ts +++ b/typescript/src.ts/geometry/collider_set.ts @@ -2,8 +2,14 @@ import {RawColliderSet} from "../raw"; import {Coarena} from "../coarena"; import {RotationOps, VectorOps} from "../math"; import {Collider, ColliderDesc, ColliderHandle} from "./collider"; -import {ImpulseJointHandle, IslandManager, RigidBodyHandle} from "../dynamics"; -import {RigidBodySet} from "../dynamics"; +import { + ImpulseJointHandle, + IslandManager, + RigidBodyHandle, + RigidBodySet, + SoftBodySet, + SoftMeshBinding, +} from "../dynamics"; /** * A set of rigid bodies that can be handled by a physics pipeline. @@ -143,6 +149,128 @@ export class ColliderSet { return collider; } + /** + * Creates a collider holding a soft body's deformable collision mesh: a polyline (2D) or a + * triangle mesh (3D) built with the `DEFORMABLE` flag, whose vertices follow the cluster + * of the parent proxy through `binding`. + * + * Returns `null` when the binding fails: the parent is not a live cluster proxy, the shape + * is not a deformable mesh, or a vertex could not be bound. + * + * @param bodies - The set of bodies where the parent proxy can be found. + * @param softBodies - The set of soft bodies owning the cluster. + * @param desc - The collider's description. + * @param binding - How the mesh follows the cluster. + * @param parentHandle - The handle of the cluster proxy (see `SoftBody.rootBody`, + * `SoftBody.clusterProxy`). + */ + public createDeformableCollider( + bodies: RigidBodySet, + softBodies: SoftBodySet, + desc: ColliderDesc, + binding: SoftMeshBinding, + parentHandle: RigidBodyHandle, + ): Collider | null { + let rawShape = desc.shape.intoRaw(); + let rawTra = VectorOps.intoRaw(desc.translation); + let rawRot = RotationOps.intoRaw(desc.rotation); + let rawCom = VectorOps.intoRaw(desc.centerOfMass); + + // #if DIM3 + let rawPrincipalInertia = VectorOps.intoRaw( + desc.principalAngularInertia, + ); + let rawInertiaFrame = RotationOps.intoRaw( + desc.angularInertiaLocalFrame, + ); + // #endif + + let handle = this.raw.createDeformableCollider( + desc.enabled, + rawShape, + rawTra, + rawRot, + desc.massPropsMode, + desc.mass, + rawCom, + // #if DIM2 + desc.principalAngularInertia, + // #endif + // #if DIM3 + rawPrincipalInertia, + rawInertiaFrame, + // #endif + desc.density, + desc.friction, + desc.restitution, + desc.frictionCombineRule, + desc.restitutionCombineRule, + desc.isSensor, + desc.collisionGroups, + desc.solverGroups, + desc.activeCollisionTypes, + desc.activeHooks, + desc.activeEvents, + desc.contactForceEventThreshold, + desc.contactSkin, + binding.rawMode(), + binding.particles, + binding.eps, + binding.selfContacts, + parentHandle, + bodies.raw, + softBodies.raw, + ); + + rawShape.free(); + rawTra.free(); + rawRot.free(); + rawCom.free(); + + // #if DIM3 + rawPrincipalInertia.free(); + rawInertiaFrame.free(); + // #endif + + if (handle === undefined) { + return null; + } + + let parent = bodies.get(parentHandle); + let collider = new Collider(this, handle, parent, desc.shape); + this.map.set(handle, collider); + return collider; + } + + /** + * Wraps the colliders the engine created on its own (the deformable surfaces and particle + * colliders of soft bodies) that have no JavaScript wrapper yet. + */ + public mapNewColliders(bodies: RigidBodySet) { + this.raw.forEachColliderHandle((handle: ColliderHandle) => { + if (!this.map.get(handle)) { + let parentHandle = this.raw.coParent(handle); + let parent = + parentHandle === undefined + ? null + : bodies.get(parentHandle); + this.map.set(handle, new Collider(this, handle, parent)); + } + }); + } + + /** + * Drops the wrappers of the colliders the engine removed on its own (the colliders of + * removed soft bodies and clusters). + */ + public unmapRemovedColliders() { + for (let collider of this.map.getAll()) { + if (!this.raw.contains(collider.handle)) { + this.map.delete(collider.handle); + } + } + } + /** * Remove a collider from this set. * @@ -154,9 +282,16 @@ export class ColliderSet { handle: ColliderHandle, islands: IslandManager, bodies: RigidBodySet, + softBodies: SoftBodySet, wakeUp: boolean, ) { - this.raw.remove(handle, islands.raw, bodies.raw, wakeUp); + this.raw.remove( + handle, + islands.raw, + bodies.raw, + softBodies.raw, + wakeUp, + ); this.unmap(handle); } diff --git a/typescript/src.ts/geometry/shape.ts b/typescript/src.ts/geometry/shape.ts index e74c8e54d..c0257796c 100644 --- a/typescript/src.ts/geometry/shape.ts +++ b/typescript/src.ts/geometry/shape.ts @@ -97,7 +97,11 @@ export abstract class Shape { case RawShapeType.Polyline: vs = rawSet.coVertices(handle); indices = rawSet.coIndices(handle); - return new Polyline(vs, indices); + return new Polyline( + vs, + indices, + rawSet.coPolylineFlags(handle), + ); case RawShapeType.Triangle: vs = rawSet.coVertices(handle); @@ -291,7 +295,7 @@ export abstract class Shape { case RawShapeType.Polyline: vs = rawShape.vertices(); indices = rawShape.indices(); - return new Polyline(vs, indices); + return new Polyline(vs, indices, rawShape.polylineFlags()); case RawShapeType.Triangle: vs = rawShape.vertices(); @@ -821,6 +825,42 @@ export enum HeightFieldFlags { // #endif +// NOTE: this **must** match the PolylineFlags on the rust side. +/** + * Flags controlling the behavior of a polyline. + */ +export enum PolylineFlags { + // #if DIM2 + /** + * The polyline is one-sided: a pseudo-normal is computed at every vertex and contact + * normals are clamped to the outward side, the right of each segment's direction (the + * solid must be wound counter-clockwise). This removes the spurious sideways push a + * body gets at a convex corner of a double-sided polyline. + */ + ORIENTED = 0b0000_0001, + // #endif + /** + * The polyline is deformable: its vertices may be moved after its creation. Required + * for a polyline used as a soft body's collision mesh (see + * `World.createDeformableCollider`). + */ + DEFORMABLE = 0b0000_0010, +} + +// NOTE: this **must** match the CompoundFlags on the rust side. +/** + * Flags controlling the behavior of a compound shape. + */ +export enum CompoundFlags { + /** + * The edges (2D) or faces (3D) where two parts meet are treated as interior to the + * union, and contact normals are clamped to the surviving outline. This removes the + * ledge a body catches on when it slides across the cut between two parts of a convex + * decomposition. Costs a one-off pass over the parts' edges when set. + */ + FIX_INTERNAL_EDGES = 0b0000_0001, +} + // NOTE: this **must** match the TriMeshFlags on the rust side. /** * Flags controlling the behavior of the triangle mesh creation and of some @@ -885,6 +925,18 @@ export enum TriMeshFlags { * /!\ NOT SUPPORTED IN THE 2D VERSION OF RAPIER. */ FIX_INTERNAL_EDGES = 0b1000_0000 | TriMeshFlags.MERGE_DUPLICATE_VERTICES, + /** + * The mesh is deformable: its vertices may be moved after its creation. Required for a + * triangle mesh used as a soft body's collision mesh (see + * `World.createDeformableCollider`). + */ + DEFORMABLE = 0b1_0000_0000, + /** + * Like `FIX_INTERNAL_EDGES`, but a contact coming from the back of a triangle is kept and + * its normal is constrained by the internal edges too. + */ + FIX_INTERNAL_EDGES_TWO_SIDED = 0b10_0000_0000 | + TriMeshFlags.FIX_INTERNAL_EDGES, } /** @@ -1248,21 +1300,32 @@ export class Polyline extends Shape { */ indices: Uint32Array; + /** + * The polyline flags. + */ + flags: PolylineFlags; + /** * Creates a new polyline shape. * * @param vertices - The coordinates of the polyline's vertices. * @param indices - The indices of the polyline's segments. If this is `null` or not provided, then * the vertices are assumed to form a line strip. + * @param flags - The polyline flags. */ - constructor(vertices: Float32Array, indices?: Uint32Array) { + constructor( + vertices: Float32Array, + indices?: Uint32Array, + flags?: PolylineFlags, + ) { super(); this.vertices = vertices; this.indices = indices ?? new Uint32Array(0); + this.flags = flags ?? (0 as PolylineFlags); } public intoRaw(): RawShape { - return RawShape.polyline(this.vertices, this.indices); + return RawShape.polyline(this.vertices, this.indices, this.flags); } } @@ -1336,6 +1399,11 @@ export class Compound extends Shape { */ rotations: Rotation[]; + /** + * The compound flags. + */ + flags: CompoundFlags; + /** * Creates a new compound shape. * @@ -1344,8 +1412,14 @@ export class Compound extends Shape { * are not allowed). * @param positions - The array of positions for each shape. * @param rotations - The array of rotations for each shape. + * @param flags - The compound flags. */ - constructor(shapes: Shape[], positions: Vector[], rotations: Rotation[]) { + constructor( + shapes: Shape[], + positions: Vector[], + rotations: Rotation[], + flags?: CompoundFlags, + ) { super(); if ( @@ -1368,6 +1442,7 @@ export class Compound extends Shape { this.shapes = shapes; this.positions = positions; this.rotations = rotations; + this.flags = flags ?? (0 as CompoundFlags); } /** @@ -1396,7 +1471,12 @@ export class Compound extends Shape { ); } - return new Compound(shapes, positions, rotations); + return new Compound( + shapes, + positions, + rotations, + rawShape.compoundFlags(), + ); } finally { rawShape.free(); } @@ -1435,7 +1515,7 @@ export class Compound extends Shape { }); // #endif - return RawShape.compound(rawShapes, positions, rotations); + return RawShape.compound(rawShapes, positions, rotations, this.flags); } } diff --git a/typescript/src.ts/pipeline/debug_render_pipeline.ts b/typescript/src.ts/pipeline/debug_render_pipeline.ts index a377ef5e4..75a4ff0c5 100644 --- a/typescript/src.ts/pipeline/debug_render_pipeline.ts +++ b/typescript/src.ts/pipeline/debug_render_pipeline.ts @@ -6,6 +6,7 @@ import { ImpulseJointSet, MultibodyJointSet, RigidBodySet, + SoftBodySet, } from "../dynamics"; import {BroadPhase, Collider, ColliderSet, NarrowPhase} from "../geometry"; import {QueryFilterFlags} from "./query_pipeline"; @@ -64,6 +65,7 @@ export class DebugRenderPipeline { public render( bodies: RigidBodySet, colliders: ColliderSet, + soft_bodies: SoftBodySet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, narrow_phase: NarrowPhase, @@ -73,6 +75,7 @@ export class DebugRenderPipeline { this.raw.render( bodies.raw, colliders.raw, + soft_bodies.raw, impulse_joints.raw, multibody_joints.raw, narrow_phase.raw, diff --git a/typescript/src.ts/pipeline/event_queue.ts b/typescript/src.ts/pipeline/event_queue.ts index 1269b7a8f..4d87d978b 100644 --- a/typescript/src.ts/pipeline/event_queue.ts +++ b/typescript/src.ts/pipeline/event_queue.ts @@ -1,5 +1,9 @@ -import {RawContactForceEvent, RawEventQueue} from "../raw"; -import {RigidBodyHandle} from "../dynamics"; +import { + RawContactForceEvent, + RawEventQueue, + RawSoftBodyTearEvent, +} from "../raw"; +import {RigidBodyHandle, SoftBodyTearEvent} from "../dynamics"; import {Collider, ColliderHandle} from "../geometry"; import {Vector, VectorOps, scratchBuffer} from "../math"; @@ -151,6 +155,25 @@ export class EventQueue { }); } + /** + * Applies the given javascript closure on each soft-body tear event of this collector, + * then clears the internal tear event buffer. + * + * The pieces a tear split off are only wrapped once `World.mapNewSoftBodies` (called by + * `World.step` after draining) ran; the event itself is only valid inside the closure. + * + * @param f - JavaScript closure applied to each tear event. The closure must take one + * `SoftBodyTearEvent` argument. + */ + public drainSoftBodyTearEvents(f: (event: SoftBodyTearEvent) => void) { + let event = new SoftBodyTearEvent(); + this.raw.drainSoftBodyTearEvents((raw: RawSoftBodyTearEvent) => { + event.raw = raw; + f(event); + event.free(); + }); + } + /** * Removes all events contained by this collector */ diff --git a/typescript/src.ts/pipeline/physics_pipeline.ts b/typescript/src.ts/pipeline/physics_pipeline.ts index a3319205c..b5854dc15 100644 --- a/typescript/src.ts/pipeline/physics_pipeline.ts +++ b/typescript/src.ts/pipeline/physics_pipeline.ts @@ -8,6 +8,7 @@ import { RigidBodySet, CCDSolver, IslandManager, + SoftBodySet, } from "../dynamics"; import { BroadPhase, @@ -40,6 +41,7 @@ export class PhysicsPipeline { narrowPhase: NarrowPhase, bodies: RigidBodySet, colliders: ColliderSet, + softBodies: SoftBodySet, impulseJoints: ImpulseJointSet, multibodyJoints: MultibodyJointSet, ccdSolver: CCDSolver, @@ -57,6 +59,7 @@ export class PhysicsPipeline { narrowPhase.raw, bodies.raw, colliders.raw, + softBodies.raw, impulseJoints.raw, multibodyJoints.raw, ccdSolver.raw, @@ -74,6 +77,7 @@ export class PhysicsPipeline { narrowPhase.raw, bodies.raw, colliders.raw, + softBodies.raw, impulseJoints.raw, multibodyJoints.raw, ccdSolver.raw, diff --git a/typescript/src.ts/pipeline/serialization_pipeline.ts b/typescript/src.ts/pipeline/serialization_pipeline.ts index 87fde398c..7c42a25bf 100644 --- a/typescript/src.ts/pipeline/serialization_pipeline.ts +++ b/typescript/src.ts/pipeline/serialization_pipeline.ts @@ -6,6 +6,7 @@ import { ImpulseJointSet, MultibodyJointSet, RigidBodySet, + SoftBodySet, } from "../dynamics"; import {BroadPhase, ColliderSet, NarrowPhase} from "../geometry"; import {World} from "./world"; @@ -52,6 +53,7 @@ export class SerializationPipeline { narrowPhase: NarrowPhase, bodies: RigidBodySet, colliders: ColliderSet, + softBodies: SoftBodySet, impulseJoints: ImpulseJointSet, multibodyJoints: MultibodyJointSet, ): Uint8Array { @@ -65,6 +67,7 @@ export class SerializationPipeline { narrowPhase.raw, bodies.raw, colliders.raw, + softBodies.raw, impulseJoints.raw, multibodyJoints.raw, ); diff --git a/typescript/src.ts/pipeline/world.ts b/typescript/src.ts/pipeline/world.ts index 4c919fdb8..63be24b27 100644 --- a/typescript/src.ts/pipeline/world.ts +++ b/typescript/src.ts/pipeline/world.ts @@ -12,6 +12,7 @@ import { RawRigidBodySet, RawSerializationPipeline, RawDebugRenderPipeline, + RawSoftBodySet, } from "../raw"; import { @@ -45,6 +46,12 @@ import { RigidBodyDesc, RigidBodyHandle, RigidBodySet, + SoftBody, + SoftBodyDesc, + SoftBodyHandle, + SoftBodySet, + SoftBodyTearEvent, + SoftMeshBinding, } from "../dynamics"; import {Rotation, Vector, VectorOps} from "../math"; import {PhysicsPipeline} from "./physics_pipeline"; @@ -81,6 +88,7 @@ export class World { colliders: ColliderSet; impulseJoints: ImpulseJointSet; multibodyJoints: MultibodyJointSet; + softBodies: SoftBodySet; ccdSolver: CCDSolver; physicsPipeline: PhysicsPipeline; serializationPipeline: SerializationPipeline; @@ -108,6 +116,7 @@ export class World { this.colliders.free(); this.impulseJoints.free(); this.multibodyJoints.free(); + this.softBodies.free(); this.ccdSolver.free(); this.physicsPipeline.free(); this.serializationPipeline.free(); @@ -128,6 +137,7 @@ export class World { this.ccdSolver = undefined; this.impulseJoints = undefined; this.multibodyJoints = undefined; + this.softBodies = undefined; this.physicsPipeline = undefined; this.serializationPipeline = undefined; this.debugRenderPipeline = undefined; @@ -147,6 +157,7 @@ export class World { rawNarrowPhase?: RawNarrowPhase, rawBodies?: RawRigidBodySet, rawColliders?: RawColliderSet, + rawSoftBodies?: RawSoftBodySet, rawImpulseJoints?: RawImpulseJointSet, rawMultibodyJoints?: RawMultibodyJointSet, rawCCDSolver?: RawCCDSolver, @@ -165,6 +176,7 @@ export class World { this.colliders = new ColliderSet(rawColliders); this.impulseJoints = new ImpulseJointSet(rawImpulseJoints); this.multibodyJoints = new MultibodyJointSet(rawMultibodyJoints); + this.softBodies = new SoftBodySet(rawSoftBodies); this.ccdSolver = new CCDSolver(rawCCDSolver); this.physicsPipeline = new PhysicsPipeline(rawPhysicsPipeline); this.serializationPipeline = new SerializationPipeline( @@ -183,6 +195,7 @@ export class World { this.impulseJoints.finalizeDeserialization(this.bodies); this.bodies.finalizeDeserialization(this.colliders); this.colliders.finalizeDeserialization(this.bodies); + this.softBodies.finalizeDeserialization(this.bodies, this.colliders); } public static fromRaw(raw: RawDeserializedWorld): World { @@ -196,6 +209,7 @@ export class World { raw.takeNarrowPhase(), raw.takeBodies(), raw.takeColliders(), + raw.takeSoftBodies(), raw.takeImpulseJoints(), raw.takeMultibodyJoints(), ); @@ -216,6 +230,7 @@ export class World { this.narrowPhase, this.bodies, this.colliders, + this.softBodies, this.impulseJoints, this.multibodyJoints, ); @@ -245,6 +260,7 @@ export class World { this.debugRenderPipeline.render( this.bodies, this.colliders, + this.softBodies, this.impulseJoints, this.multibodyJoints, this.narrowPhase, @@ -274,12 +290,30 @@ export class World { this.narrowPhase, this.bodies, this.colliders, + this.softBodies, this.impulseJoints, this.multibodyJoints, this.ccdSolver, eventQueue, hooks, ); + // Tears split pieces off into soft bodies of their own, with proxies and colliders. + this.mapNewSoftBodies(); + } + + /** + * Wraps the soft bodies, rigid-body proxies and colliders the engine created on its own + * during the last step (the pieces tears split off), and drops the wrappers of what it + * removed. Called by `World.step`. + */ + public mapNewSoftBodies() { + this.softBodies.mapNewSoftBodies(this.bodies, this.colliders); + this.bodies.mapNewBodies(this.colliders); + this.colliders.mapNewColliders(this.bodies); + this.bodies.unmapRemovedBodies(); + this.colliders.unmapRemovedColliders(); + this.impulseJoints.unmapRemovedJoints(); + this.multibodyJoints.unmapRemovedJoints(); } /** @@ -545,6 +579,130 @@ export class World { return this.colliders.createCollider(this.bodies, desc, parentHandle); } + /** + * Creates a new soft body from the given description, with its hidden root rigid body + * and its colliders (a deformable surface, or one ball per particle). + * + * @param desc - The description of the soft body to create. + */ + public createSoftBody(desc: SoftBodyDesc): SoftBody { + return this.softBodies.createSoftBody( + this.bodies, + this.colliders, + desc, + ); + } + + /** + * Creates a collider holding a soft body's deformable collision mesh (a polyline in 2D or + * a triangle mesh in 3D built with the `DEFORMABLE` flag), bound to the cluster whose + * proxy is `parent` (see `SoftBody.rootBody` and `SoftBody.clusterProxy`). + * + * Returns `null` when the binding fails. + * + * @param desc - The description of the collider; its shape's vertices are given in the + * collider's local frame relative to the proxy. + * @param binding - How the mesh's vertices follow the cluster's particles. + * @param parent - The cluster proxy the collider is attached to. + */ + public createDeformableCollider( + desc: ColliderDesc, + binding: SoftMeshBinding, + parent: RigidBody, + ): Collider | null { + return this.colliders.createDeformableCollider( + this.bodies, + this.softBodies, + desc, + binding, + parent.handle, + ); + } + + /** + * Adds a cluster to a soft body: a rigid proxy over the given particles that joints and + * colliders can attach to. Returns the cluster's index, or `null` if no particle was + * valid. + */ + public addSoftBodyCluster( + body: SoftBody, + particles: Uint32Array | number[], + ): number | null { + return this.softBodies.addCluster( + body.handle, + particles, + this.bodies, + this.colliders, + ); + } + + /** + * Removes a cluster of a soft body, with its proxy, colliders and joints. + */ + public removeSoftBodyCluster(body: SoftBody, cluster: number): boolean { + return this.softBodies.removeCluster( + body.handle, + cluster, + this.islands, + this.bodies, + this.colliders, + this.impulseJoints, + this.multibodyJoints, + ); + } + + /** + * Tears a soft body at once along the given edges and through the given cells (indices + * into `SoftBody.edges` and `SoftBody.cells`), without removing material. Pieces + * disconnected by the tear become soft bodies of their own. + * + * Returns the tear event (to be freed with `.free()`), or `null` when nothing changed. + */ + public tearSoftBody( + body: SoftBody, + edges: Uint32Array | number[], + cells: Uint32Array | number[], + ): SoftBodyTearEvent | null { + return this.softBodies.tear( + body.handle, + edges, + cells, + this.islands, + this.bodies, + this.colliders, + this.impulseJoints, + this.multibodyJoints, + ); + } + + /** + * Cuts a soft body along a blade: a segment (two points) in 2D, a triangle (three points) + * in 3D. Pieces disconnected by the cut become soft bodies of their own. + * + * Returns the tear event (to be freed with `.free()`), or `null` when nothing changed. + */ + public cutSoftBody( + body: SoftBody, + blade: Vector[], + ): SoftBodyTearEvent | null { + return this.softBodies.cut( + body.handle, + blade, + this.islands, + this.bodies, + this.colliders, + this.impulseJoints, + this.multibodyJoints, + ); + } + + /** + * Wakes a soft body and everything it touches up. + */ + public wakeUpSoftBody(body: SoftBody, strong: boolean) { + this.softBodies.wakeUp(body.handle, this.bodies, strong); + } + /** * Creates a new impulse joint from the given joint descriptor. * @@ -626,6 +784,15 @@ export class World { return this.multibodyJoints.get(handle); } + /** + * Retrieves a soft body from its handle. + * + * @param handle - The integer handle of the soft body to retrieve. + */ + public getSoftBody(handle: SoftBodyHandle): SoftBody { + return this.softBodies.get(handle); + } + /** * Removes the given rigid-body from this physics world. * @@ -640,6 +807,26 @@ export class World { body.handle, this.islands, this.colliders, + this.softBodies, + this.impulseJoints, + this.multibodyJoints, + ); + } + } + + /** + * Removes the given soft body from this physics world, with its proxies, colliders and + * attached joints. + * + * @param body - The soft body to remove. + */ + public removeSoftBody(body: SoftBody) { + if (this.softBodies) { + this.softBodies.remove( + body.handle, + this.islands, + this.bodies, + this.colliders, this.impulseJoints, this.multibodyJoints, ); @@ -658,6 +845,7 @@ export class World { collider.handle, this.islands, this.bodies, + this.softBodies, wakeUp, ); } @@ -718,6 +906,15 @@ export class World { this.bodies.forEachActiveRigidBody(this.islands, f); } + /** + * Applies the given closure to each soft body managed by this physics world. + * + * @param f(body) - The function to apply to each soft body managed by this physics world. + */ + public forEachSoftBody(f: (body: SoftBody) => void) { + this.softBodies.forEach(f); + } + /** * Find the closest intersection between a ray and the physics world. * diff --git a/typescript/src/dynamics/integration_parameters.rs b/typescript/src/dynamics/integration_parameters.rs index dde67a6e8..ba4d63f89 100644 --- a/typescript/src/dynamics/integration_parameters.rs +++ b/typescript/src/dynamics/integration_parameters.rs @@ -51,6 +51,36 @@ impl RawIntegrationParameters { self.0.length_unit } + #[wasm_bindgen(getter)] + pub fn softBodiesResweepStrain(&self) -> f32 { + self.0.soft_bodies.resweep_strain + } + + #[wasm_bindgen(setter)] + pub fn set_softBodiesResweepStrain(&mut self, value: f32) { + self.0.soft_bodies.resweep_strain = value; + } + + #[wasm_bindgen(getter)] + pub fn softBodiesMaxExtraSubsteps(&self) -> usize { + self.0.soft_bodies.max_extra_substeps + } + + #[wasm_bindgen(setter)] + pub fn set_softBodiesMaxExtraSubsteps(&mut self, value: usize) { + self.0.soft_bodies.max_extra_substeps = value; + } + + #[wasm_bindgen(getter)] + pub fn softBodiesContactStiffening(&self) -> f32 { + self.0.soft_bodies.contact_stiffening + } + + #[wasm_bindgen(setter)] + pub fn set_softBodiesContactStiffening(&mut self, value: f32) { + self.0.soft_bodies.contact_stiffening = value; + } + #[wasm_bindgen(setter)] pub fn set_dt(&mut self, value: f32) { self.0.dt = value; diff --git a/typescript/src/dynamics/mod.rs b/typescript/src/dynamics/mod.rs index 36cc7eaa6..7318ff8e5 100644 --- a/typescript/src/dynamics/mod.rs +++ b/typescript/src/dynamics/mod.rs @@ -7,6 +7,7 @@ pub use self::island_manager::*; pub use self::joint::*; pub use self::multibody_joint_set::*; pub use self::rigid_body_set::*; +pub use self::soft_body::*; mod ccd_solver; mod impulse_joint; @@ -18,3 +19,4 @@ mod multibody_joint; mod multibody_joint_set; mod rigid_body; mod rigid_body_set; +mod soft_body; diff --git a/typescript/src/dynamics/rigid_body.rs b/typescript/src/dynamics/rigid_body.rs index c497ce90c..3d828ba4a 100644 --- a/typescript/src/dynamics/rigid_body.rs +++ b/typescript/src/dynamics/rigid_body.rs @@ -960,6 +960,29 @@ impl RawRigidBodySet { self.map(handle, |rb| rb.additional_solver_iterations()) } + pub fn rbAdditionalPgsIterations(&self, handle: FlatHandle) -> usize { + self.map(handle, |rb| rb.additional_pgs_iterations()) + } + + pub fn rbSetAdditionalPgsIterations(&mut self, handle: FlatHandle, iters: usize) { + self.map_mut(handle, |rb| rb.set_additional_pgs_iterations(iters)) + } + + /// Is this rigid-body the proxy of a soft-body cluster? + pub fn rbIsSoftFrame(&self, handle: FlatHandle) -> bool { + self.map(handle, |rb| rb.is_soft_frame()) + } + + /// The soft body this rigid-body is a cluster proxy of, if any. + pub fn rbSoftBody(&self, handle: FlatHandle) -> Option { + self.map(handle, |rb| rb.soft_body().map(|h| utils::flat_handle(h.0))) + } + + /// The index of the cluster this proxy stands for in its soft body, if any. + pub fn rbSoftCluster(&self, handle: FlatHandle) -> Option { + self.map(handle, |rb| rb.soft_cluster()) + } + pub fn rbSetAdditionalSolverIterations(&mut self, handle: FlatHandle, iters: usize) { self.map_mut(handle, |rb| { rb.set_additional_solver_iterations(iters as usize); diff --git a/typescript/src/dynamics/rigid_body_set.rs b/typescript/src/dynamics/rigid_body_set.rs index d8027c72c..ee9a332b8 100644 --- a/typescript/src/dynamics/rigid_body_set.rs +++ b/typescript/src/dynamics/rigid_body_set.rs @@ -1,4 +1,4 @@ -use crate::dynamics::{RawImpulseJointSet, RawIslandManager, RawMultibodyJointSet}; +use crate::dynamics::{RawImpulseJointSet, RawIslandManager, RawMultibodyJointSet, RawSoftBodySet}; use crate::geometry::RawColliderSet; use crate::math::{RawRotation, RawVector}; use crate::utils::{self, FlatHandle}; @@ -12,6 +12,7 @@ pub enum RawRigidBodyType { Fixed, KinematicPositionBased, KinematicVelocityBased, + SoftFrame, } impl Into for RawRigidBodyType { @@ -21,6 +22,7 @@ impl Into for RawRigidBodyType { RawRigidBodyType::Fixed => RigidBodyType::Fixed, RawRigidBodyType::KinematicPositionBased => RigidBodyType::KinematicPositionBased, RawRigidBodyType::KinematicVelocityBased => RigidBodyType::KinematicVelocityBased, + RawRigidBodyType::SoftFrame => RigidBodyType::SoftFrame, } } } @@ -32,6 +34,7 @@ impl Into for RigidBodyType { RigidBodyType::Fixed => RawRigidBodyType::Fixed, RigidBodyType::KinematicPositionBased => RawRigidBodyType::KinematicPositionBased, RigidBodyType::KinematicVelocityBased => RawRigidBodyType::KinematicVelocityBased, + RigidBodyType::SoftFrame => RawRigidBodyType::SoftFrame, } } } @@ -95,6 +98,7 @@ impl RawRigidBodySet { ccdEnabled: bool, dominanceGroup: i8, additional_solver_iterations: usize, + additional_pgs_iterations: usize, ) -> FlatHandle { let pos = Pose::from_parts(translation.0, rotation.0); @@ -117,6 +121,7 @@ impl RawRigidBodySet { .ccd_enabled(ccdEnabled) .dominance_group(dominanceGroup) .additional_solver_iterations(additional_solver_iterations) + .additional_pgs_iterations(additional_pgs_iterations) .soft_ccd_prediction(softCcdPrediction); rigid_body = if massOnly { @@ -159,6 +164,7 @@ impl RawRigidBodySet { ccdEnabled: bool, dominanceGroup: i8, additional_solver_iterations: usize, + additional_pgs_iterations: usize, ) -> FlatHandle { let pos = Pose::from_parts(translation.0, rotation.0); let mut rigid_body = RigidBodyBuilder::new(rb_type.into()) @@ -175,6 +181,7 @@ impl RawRigidBodySet { .ccd_enabled(ccdEnabled) .dominance_group(dominanceGroup) .additional_solver_iterations(additional_solver_iterations) + .additional_pgs_iterations(additional_pgs_iterations) .soft_ccd_prediction(softCcdPrediction); rigid_body = if massOnly { @@ -196,6 +203,7 @@ impl RawRigidBodySet { handle: FlatHandle, islands: &mut RawIslandManager, colliders: &mut RawColliderSet, + softBodies: &mut RawSoftBodySet, joints: &mut RawImpulseJointSet, articulations: &mut RawMultibodyJointSet, ) { @@ -206,6 +214,7 @@ impl RawRigidBodySet { &mut colliders.0, &mut joints.0, &mut articulations.0, + &mut softBodies.0, true, ); } diff --git a/typescript/src/dynamics/soft_body.rs b/typescript/src/dynamics/soft_body.rs new file mode 100644 index 000000000..dd3f6546c --- /dev/null +++ b/typescript/src/dynamics/soft_body.rs @@ -0,0 +1,1549 @@ +use crate::dynamics::{ + RawImpulseJointSet, RawIslandManager, RawMultibodyJointSet, RawRigidBodySet, +}; +use crate::geometry::RawColliderSet; +use crate::math::{RawRotation, RawVector}; +use crate::utils::{self, FlatHandle}; +use rapier::dynamics::{ + SoftBody, SoftBodyBuilder, SoftBodyCellModel, SoftBodyMaterial, SoftBodySet, SoftBodyTearEvent, + SoftEdgePlasticFlow, SpringCoefficients, +}; +use rapier::math::{Pose, Vector, DIM}; +use rapier::prelude::ColliderBuilder; +use wasm_bindgen::prelude::*; + +/// Writes a vector into a JS scratch buffer, one float per dimension. +fn write_vector(scratch_buffer: &js_sys::Float32Array, v: Vector) { + for (i, x) in v.to_array().iter().enumerate() { + scratch_buffer.set_index(i as u32, *x); + } +} + +/// Reads `DIM` floats per vector out of a flat array. +fn read_vectors(data: &[f32]) -> Vec { + data.chunks_exact(DIM).map(Vector::from_slice).collect() +} + +/// Reads `N` indices per element out of a flat array. +fn read_elements(data: &[u32]) -> Vec<[u32; N]> { + data.chunks_exact(N) + .map(|e| { + let mut out = [0; N]; + out.copy_from_slice(e); + out + }) + .collect() +} + +/// Flattens elements of `N` indices each. +fn flatten_elements(elements: &[[u32; N]]) -> Vec { + elements.iter().flat_map(|e| e.iter().copied()).collect() +} + +#[wasm_bindgen] +#[derive(Copy, Clone)] +pub enum RawSoftBodyCellModel { + Volume = 0, + Corotational = 1, + NeoHookean = 2, +} + +impl From for SoftBodyCellModel { + fn from(model: RawSoftBodyCellModel) -> Self { + match model { + RawSoftBodyCellModel::Volume => SoftBodyCellModel::Volume, + RawSoftBodyCellModel::Corotational => SoftBodyCellModel::Corotational, + RawSoftBodyCellModel::NeoHookean => SoftBodyCellModel::NeoHookean, + } + } +} + +impl From for RawSoftBodyCellModel { + fn from(model: SoftBodyCellModel) -> Self { + match model { + SoftBodyCellModel::Volume => RawSoftBodyCellModel::Volume, + SoftBodyCellModel::Corotational => RawSoftBodyCellModel::Corotational, + SoftBodyCellModel::NeoHookean => RawSoftBodyCellModel::NeoHookean, + } + } +} + +#[wasm_bindgen] +#[derive(Copy, Clone)] +pub enum RawSoftEdgePlasticFlow { + Both = 0, + Compression = 1, + Tension = 2, +} + +impl From for SoftEdgePlasticFlow { + fn from(flow: RawSoftEdgePlasticFlow) -> Self { + match flow { + RawSoftEdgePlasticFlow::Both => SoftEdgePlasticFlow::Both, + RawSoftEdgePlasticFlow::Compression => SoftEdgePlasticFlow::Compression, + RawSoftEdgePlasticFlow::Tension => SoftEdgePlasticFlow::Tension, + } + } +} + +impl From for RawSoftEdgePlasticFlow { + fn from(flow: SoftEdgePlasticFlow) -> Self { + match flow { + SoftEdgePlasticFlow::Both => RawSoftEdgePlasticFlow::Both, + SoftEdgePlasticFlow::Compression => RawSoftEdgePlasticFlow::Compression, + SoftEdgePlasticFlow::Tension => RawSoftEdgePlasticFlow::Tension, + } + } +} + +/// How a deformable collider's vertices follow the particles of its cluster. +#[wasm_bindgen] +#[derive(Copy, Clone)] +pub enum RawSoftMeshBindingMode { + Direct = 0, + DirectByPosition = 1, + Skinned = 2, +} + +/* + * Material. + */ + +#[wasm_bindgen] +#[derive(Clone)] +pub struct RawSoftBodyMaterial(pub(crate) SoftBodyMaterial); + +#[wasm_bindgen] +impl RawSoftBodyMaterial { + #[wasm_bindgen(constructor)] + pub fn new() -> Self { + RawSoftBodyMaterial(SoftBodyMaterial::default()) + } + + pub fn uniform(natural_frequency: f32, damping_ratio: f32) -> Self { + RawSoftBodyMaterial(SoftBodyMaterial::uniform(SpringCoefficients::new( + natural_frequency, + damping_ratio, + ))) + } + + #[wasm_bindgen(getter)] + pub fn edgeFrequency(&self) -> f32 { + self.0.edge_softness.natural_frequency + } + #[wasm_bindgen(setter)] + pub fn set_edgeFrequency(&mut self, value: f32) { + self.0.edge_softness.natural_frequency = value; + } + #[wasm_bindgen(getter)] + pub fn edgeDampingRatio(&self) -> f32 { + self.0.edge_softness.damping_ratio + } + #[wasm_bindgen(setter)] + pub fn set_edgeDampingRatio(&mut self, value: f32) { + self.0.edge_softness.damping_ratio = value; + } + #[wasm_bindgen(getter)] + pub fn bendFrequency(&self) -> f32 { + self.0.bend_softness.natural_frequency + } + #[wasm_bindgen(setter)] + pub fn set_bendFrequency(&mut self, value: f32) { + self.0.bend_softness.natural_frequency = value; + } + #[wasm_bindgen(getter)] + pub fn bendDampingRatio(&self) -> f32 { + self.0.bend_softness.damping_ratio + } + #[wasm_bindgen(setter)] + pub fn set_bendDampingRatio(&mut self, value: f32) { + self.0.bend_softness.damping_ratio = value; + } + #[wasm_bindgen(getter)] + pub fn volumeFrequency(&self) -> f32 { + self.0.volume_softness.natural_frequency + } + #[wasm_bindgen(setter)] + pub fn set_volumeFrequency(&mut self, value: f32) { + self.0.volume_softness.natural_frequency = value; + } + #[wasm_bindgen(getter)] + pub fn volumeDampingRatio(&self) -> f32 { + self.0.volume_softness.damping_ratio + } + #[wasm_bindgen(setter)] + pub fn set_volumeDampingRatio(&mut self, value: f32) { + self.0.volume_softness.damping_ratio = value; + } + #[wasm_bindgen(getter)] + pub fn shapeMatchingFrequency(&self) -> f32 { + self.0.shape_matching_softness.natural_frequency + } + #[wasm_bindgen(setter)] + pub fn set_shapeMatchingFrequency(&mut self, value: f32) { + self.0.shape_matching_softness.natural_frequency = value; + } + #[wasm_bindgen(getter)] + pub fn shapeMatchingDampingRatio(&self) -> f32 { + self.0.shape_matching_softness.damping_ratio + } + #[wasm_bindgen(setter)] + pub fn set_shapeMatchingDampingRatio(&mut self, value: f32) { + self.0.shape_matching_softness.damping_ratio = value; + } + #[wasm_bindgen(getter)] + pub fn youngModulus(&self) -> f32 { + self.0.young_modulus + } + #[wasm_bindgen(setter)] + pub fn set_youngModulus(&mut self, value: f32) { + self.0.young_modulus = value; + } + #[wasm_bindgen(getter)] + pub fn poissonRatio(&self) -> f32 { + self.0.poisson_ratio + } + #[wasm_bindgen(setter)] + pub fn set_poissonRatio(&mut self, value: f32) { + self.0.poisson_ratio = value; + } + #[wasm_bindgen(getter)] + pub fn elasticDampingRatio(&self) -> f32 { + self.0.elastic_damping_ratio + } + #[wasm_bindgen(setter)] + pub fn set_elasticDampingRatio(&mut self, value: f32) { + self.0.elastic_damping_ratio = value; + } + #[wasm_bindgen(getter)] + pub fn plasticYield(&self) -> f32 { + self.0.plastic_yield + } + #[wasm_bindgen(setter)] + pub fn set_plasticYield(&mut self, value: f32) { + self.0.plastic_yield = value; + } + #[wasm_bindgen(getter)] + pub fn plasticCreep(&self) -> f32 { + self.0.plastic_creep + } + #[wasm_bindgen(setter)] + pub fn set_plasticCreep(&mut self, value: f32) { + self.0.plastic_creep = value; + } + #[wasm_bindgen(getter)] + pub fn plasticMax(&self) -> f32 { + self.0.plastic_max + } + #[wasm_bindgen(setter)] + pub fn set_plasticMax(&mut self, value: f32) { + self.0.plastic_max = value; + } + #[wasm_bindgen(getter)] + pub fn deformationDamping(&self) -> f32 { + self.0.deformation_damping + } + #[wasm_bindgen(setter)] + pub fn set_deformationDamping(&mut self, value: f32) { + self.0.deformation_damping = value; + } + #[wasm_bindgen(getter)] + pub fn edgePlasticYield(&self) -> f32 { + self.0.edge_plastic_yield + } + #[wasm_bindgen(setter)] + pub fn set_edgePlasticYield(&mut self, value: f32) { + self.0.edge_plastic_yield = value; + } + #[wasm_bindgen(getter)] + pub fn edgePlasticCreep(&self) -> f32 { + self.0.edge_plastic_creep + } + #[wasm_bindgen(setter)] + pub fn set_edgePlasticCreep(&mut self, value: f32) { + self.0.edge_plastic_creep = value; + } + #[wasm_bindgen(getter)] + pub fn edgePlasticMax(&self) -> f32 { + self.0.edge_plastic_max + } + #[wasm_bindgen(setter)] + pub fn set_edgePlasticMax(&mut self, value: f32) { + self.0.edge_plastic_max = value; + } + #[wasm_bindgen(getter)] + pub fn tearSmoothing(&self) -> f32 { + self.0.tear_smoothing + } + #[wasm_bindgen(setter)] + pub fn set_tearSmoothing(&mut self, value: f32) { + self.0.tear_smoothing = value; + } + #[wasm_bindgen(getter)] + pub fn interiorStrength(&self) -> f32 { + self.0.interior_strength + } + #[wasm_bindgen(setter)] + pub fn set_interiorStrength(&mut self, value: f32) { + self.0.interior_strength = value; + } + + #[wasm_bindgen(getter)] + pub fn edgePlasticFlow(&self) -> RawSoftEdgePlasticFlow { + self.0.edge_plastic_flow.into() + } + #[wasm_bindgen(setter)] + pub fn set_edgePlasticFlow(&mut self, value: RawSoftEdgePlasticFlow) { + self.0.edge_plastic_flow = value.into(); + } + + #[wasm_bindgen(getter)] + pub fn tearStrain(&self) -> Option { + self.0.tear_strain + } + #[wasm_bindgen(setter)] + pub fn set_tearStrain(&mut self, value: Option) { + self.0.tear_strain = value; + } + + #[wasm_bindgen(getter)] + pub fn tearForce(&self) -> Option { + self.0.tear_force + } + #[wasm_bindgen(setter)] + pub fn set_tearForce(&mut self, value: Option) { + self.0.tear_force = value; + } + + #[wasm_bindgen(getter)] + pub fn maxTearsPerStep(&self) -> u32 { + self.0.max_tears_per_step + } + #[wasm_bindgen(setter)] + pub fn set_maxTearsPerStep(&mut self, value: u32) { + self.0.max_tears_per_step = value; + } + + #[wasm_bindgen(getter)] + pub fn minPiece(&self) -> Option { + self.0.min_piece + } + #[wasm_bindgen(setter)] + pub fn set_minPiece(&mut self, value: Option) { + self.0.min_piece = value; + } + + pub fn tears(&self) -> bool { + self.0.tears() + } +} + +/* + * Builder. + */ + +#[wasm_bindgen] +#[derive(Clone)] +pub struct RawSoftBodyBuilder(pub(crate) SoftBodyBuilder); + +#[wasm_bindgen] +impl RawSoftBodyBuilder { + /// A builder over the given world-space particle positions (`DIM` floats per particle), with + /// no element. + #[wasm_bindgen(constructor)] + pub fn new(positions: Vec) -> Self { + RawSoftBodyBuilder(SoftBodyBuilder::new(read_vectors(&positions))) + } + + pub fn rope(start: &RawVector, end: &RawVector, num_particles: usize) -> Self { + RawSoftBodyBuilder(SoftBodyBuilder::rope(start.0, end.0, num_particles)) + } + + #[cfg(feature = "dim2")] + pub fn polyline(vertices: Vec, indices: Vec) -> Option { + let indices = if indices.is_empty() { + None + } else { + Some(read_elements::<2>(&indices)) + }; + SoftBodyBuilder::polyline(read_vectors(&vertices), indices).map(RawSoftBodyBuilder) + } + + #[cfg(feature = "dim3")] + pub fn trimesh(vertices: Vec, indices: Vec) -> Option { + SoftBodyBuilder::trimesh(read_vectors(&vertices), read_elements::<3>(&indices)) + .map(RawSoftBodyBuilder) + } + + #[cfg(feature = "dim3")] + pub fn cloth(origin: &RawVector, du: &RawVector, dv: &RawVector, nu: usize, nv: usize) -> Self { + RawSoftBodyBuilder(SoftBodyBuilder::cloth(origin.0, du.0, dv.0, nu, nv)) + } + + #[cfg(feature = "dim3")] + pub fn clothTube( + origin: &RawVector, + axis: &RawVector, + radius_start: f32, + radius_end: f32, + num_around: usize, + num_along: usize, + ) -> Self { + RawSoftBodyBuilder(SoftBodyBuilder::cloth_tube( + origin.0, + axis.0, + radius_start, + radius_end, + num_around, + num_along, + )) + } + + #[cfg(feature = "dim3")] + pub fn clothAnisotropic( + origin: &RawVector, + du: &RawVector, + dv: &RawVector, + nu: usize, + nv: usize, + warp_frequency: f32, + warp_damping: f32, + weft_frequency: f32, + weft_damping: f32, + shear_frequency: f32, + shear_damping: f32, + ) -> Self { + RawSoftBodyBuilder(SoftBodyBuilder::cloth_anisotropic( + origin.0, + du.0, + dv.0, + nu, + nv, + SpringCoefficients::new(warp_frequency, warp_damping), + SpringCoefficients::new(weft_frequency, weft_damping), + SpringCoefficients::new(shear_frequency, shear_damping), + )) + } + + #[cfg(feature = "dim3")] + pub fn cuboid( + center: &RawVector, + half_extents: &RawVector, + nx: usize, + ny: usize, + nz: usize, + ) -> Self { + RawSoftBodyBuilder(SoftBodyBuilder::cuboid( + center.0, + half_extents.0, + nx, + ny, + nz, + )) + } + + #[cfg(feature = "dim3")] + pub fn sphere(center: &RawVector, radius: f32, subdivisions: usize) -> Self { + RawSoftBodyBuilder(SoftBodyBuilder::sphere(center.0, radius, subdivisions)) + } + + #[cfg(feature = "dim2")] + pub fn polygon(points: Vec) -> Self { + RawSoftBodyBuilder(SoftBodyBuilder::polygon(read_vectors(&points))) + } + + #[cfg(feature = "dim2")] + pub fn disk(center: &RawVector, radius: f32, num_particles: usize) -> Self { + RawSoftBodyBuilder(SoftBodyBuilder::disk(center.0, radius, num_particles)) + } + + #[cfg(feature = "dim2")] + pub fn grid(center: &RawVector, half_extents: &RawVector, nx: usize, ny: usize) -> Self { + RawSoftBodyBuilder(SoftBodyBuilder::grid(center.0, half_extents.0, nx, ny)) + } + + /// A volumetric body filling the closed surface (segments in 2D, triangles in 3D) with + /// cells of the given size. + pub fn volumetric( + vertices: Vec, + indices: Vec, + cell_size: f32, + skinned: bool, + ) -> Option { + let vertices = read_vectors(&vertices); + let indices = read_elements::(&indices); + if skinned { + SoftBodyBuilder::volumetric_skinned(&vertices, &indices, cell_size) + } else { + SoftBodyBuilder::volumetric(&vertices, &indices, cell_size) + } + .map(RawSoftBodyBuilder) + } + + pub fn numParticles(&self) -> usize { + self.0.positions.len() + } + + pub fn particlePositions(&self) -> Vec { + self.0.positions.iter().flat_map(|p| p.to_array()).collect() + } + + pub fn surfaceEdges(&self) -> Vec { + flatten_elements(&self.0.surface_edges()) + } + + pub fn cellEdges(&self) -> Vec { + flatten_elements(&self.0.cell_edges()) + } + + #[cfg(feature = "dim3")] + pub fn surfaceDihedrals(&self) -> Vec { + flatten_elements(&self.0.surface_dihedrals()) + } + + pub fn append(&mut self, other: &RawSoftBodyBuilder) { + let me = std::mem::take(&mut self.0); + self.0 = me.append(other.0.clone()); + } + + pub fn translated(&mut self, translation: &RawVector) { + let me = std::mem::take(&mut self.0); + self.0 = me.translated(translation.0); + } + + pub fn setPositions(&mut self, positions: Vec) { + self.0.positions = read_vectors(&positions); + } + + pub fn setParticleMass(&mut self, mass: f32) { + self.0.particle_mass = mass; + self.0.masses.clear(); + } + + pub fn setMass(&mut self, mass: f32) { + let me = std::mem::take(&mut self.0); + self.0 = me.mass(mass); + } + + pub fn setMasses(&mut self, masses: Vec) { + self.0.masses = masses; + } + + pub fn setPinnedParticles(&mut self, pinned: Vec) { + self.0.pinned = pinned; + } + + pub fn setEdges(&mut self, edges: Vec) { + self.0.edges = read_elements::<2>(&edges); + } + + pub fn addEdges(&mut self, edges: Vec) { + self.0.edges.extend(read_elements::<2>(&edges)); + } + + pub fn setBendEdges(&mut self, edges: Vec) { + self.0.bend_edges = read_elements::<2>(&edges); + } + + pub fn setTensionOnly(&mut self) { + let me = std::mem::take(&mut self.0); + self.0 = me.tension_only(); + } + + #[cfg(feature = "dim3")] + pub fn setDihedrals(&mut self, dihedrals: Vec) { + self.0.dihedrals = read_elements::<4>(&dihedrals); + } + + pub fn setCells(&mut self, cells: Vec) { + self.0.cells = read_elements::<{ DIM + 1 }>(&cells); + } + + pub fn setSurface(&mut self, surface: Vec) { + self.0.surface = read_elements::(&surface); + } + + pub fn setSkin(&mut self, vertices: Vec, indices: Vec) { + self.0.skin = Some((read_vectors(&vertices), read_elements::(&indices))); + } + + pub fn setSkinCollision(&mut self, enabled: bool) { + self.0.skin_collision = enabled; + } + + #[cfg(feature = "dim3")] + pub fn setWire(&mut self, segments: Vec) { + self.0.wire = read_elements::<2>(&segments); + } + + pub fn setMaterial(&mut self, material: &RawSoftBodyMaterial) { + self.0.material = material.0.clone(); + } + + pub fn material(&self) -> RawSoftBodyMaterial { + RawSoftBodyMaterial(self.0.material.clone()) + } + + pub fn setSoftness(&mut self, natural_frequency: f32, damping_ratio: f32) { + let me = std::mem::take(&mut self.0); + self.0 = me.softness(SpringCoefficients::new(natural_frequency, damping_ratio)); + } + + pub fn setEdgeSoftness(&mut self, edges: Vec, frequencies: Vec, dampings: Vec) { + self.0.edge_softness = edges + .iter() + .zip(frequencies.iter().zip(dampings.iter())) + .map(|(e, (f, d))| (*e, SpringCoefficients::new(*f, *d))) + .collect(); + } + + pub fn setEdgeTearResistance(&mut self, edges: Vec, resistances: Vec) { + self.0.edge_tear_resistance = edges.into_iter().zip(resistances).collect(); + } + + pub fn setCellModel(&mut self, model: RawSoftBodyCellModel) { + self.0.cell_model = model.into(); + } + + pub fn setVolumePreservation(&mut self, enabled: bool) { + self.0.volume_preservation = enabled; + } + + pub fn setVolumeFactor(&mut self, factor: f32) { + let me = std::mem::take(&mut self.0); + self.0 = me.volume_factor(factor); + } + + pub fn setShapeMatching(&mut self, enabled: bool) { + self.0.shape_matching = enabled; + } + + pub fn setSelfContacts(&mut self, enabled: bool) { + self.0.self_contacts = enabled; + } + + pub fn setParticleRadius(&mut self, radius: f32) { + self.0.particle_radius = radius; + } + + /// The template of the body's colliders: its shape is replaced by the deformable surface, + /// or by a ball of `particleRadius` for a body colliding through its particles. + pub fn setSurfaceCollider( + &mut self, + friction: f32, + restitution: f32, + frictionCombineRule: u32, + restitutionCombineRule: u32, + isSensor: bool, + collisionGroups: u32, + solverGroups: u32, + activeCollisionTypes: u16, + activeHooks: u32, + activeEvents: u32, + contactForceEventThreshold: f32, + contactSkin: f32, + ) { + use rapier::geometry::ActiveCollisionTypes; + use rapier::pipeline::{ActiveEvents, ActiveHooks}; + let radius = self.0.particle_radius.max(0.05); + self.0.collider_template = Some( + ColliderBuilder::ball(radius) + .density(0.0) + .friction(friction) + .restitution(restitution) + .friction_combine_rule(crate::geometry::combine_rule_from_u32(frictionCombineRule)) + .restitution_combine_rule(crate::geometry::combine_rule_from_u32( + restitutionCombineRule, + )) + .sensor(isSensor) + .collision_groups(crate::geometry::unpack_interaction_groups(collisionGroups)) + .solver_groups(crate::geometry::unpack_interaction_groups(solverGroups)) + .active_collision_types( + ActiveCollisionTypes::from_bits(activeCollisionTypes) + .unwrap_or(ActiveCollisionTypes::empty()), + ) + .active_hooks(ActiveHooks::from_bits(activeHooks).unwrap_or(ActiveHooks::empty())) + .active_events( + ActiveEvents::from_bits(activeEvents).unwrap_or(ActiveEvents::empty()), + ) + .contact_force_event_threshold(contactForceEventThreshold) + .contact_skin(contactSkin), + ); + } + + pub fn setNoSurfaceCollider(&mut self) { + self.0.collider_template = None; + } + + pub fn setLinearDamping(&mut self, damping: f32) { + self.0.particle_settings.linear_damping = damping; + } + + pub fn setGravityScale(&mut self, scale: f32) { + self.0.particle_settings.gravity_scale = scale; + } + + pub fn setAdditionalSolverIterations(&mut self, iterations: usize) { + self.0.particle_settings.additional_solver_iterations = iterations; + } + + pub fn setAdditionalPgsIterations(&mut self, iterations: usize) { + self.0.particle_settings.additional_pgs_iterations = iterations; + } + + pub fn setCanSleep(&mut self, can_sleep: bool) { + self.0.particle_settings.can_sleep = can_sleep; + } + + pub fn setDominanceGroup(&mut self, group: i8) { + self.0.particle_settings.dominance_group = group; + } +} + +/* + * Tear events. + */ + +#[wasm_bindgen] +pub struct RawSoftBodyTearEvent(pub(crate) SoftBodyTearEvent); + +#[wasm_bindgen] +impl RawSoftBodyTearEvent { + pub fn softBody(&self) -> FlatHandle { + utils::flat_handle(self.0.soft_body.0) + } + + pub fn tornEdges(&self) -> Vec { + flatten_elements(&self.0.torn_edges) + } + + pub fn tornCells(&self) -> Vec { + flatten_elements(&self.0.torn_cells) + } + + pub fn removedEdges(&self) -> Vec { + flatten_elements(&self.0.removed_edges) + } + + pub fn splitParticles(&self) -> Vec { + self.0 + .split_particles + .iter() + .flat_map(|(a, b)| [*a, *b]) + .collect() + } + + pub fn insertedParticles(&self) -> Vec { + self.0.inserted_particles.clone() + } + + pub fn seeds(&self) -> Vec { + flatten_elements(&self.0.seeds()) + } + + pub fn numPieces(&self) -> usize { + self.0.pieces.len() + } + + pub fn pieceSoftBody(&self, i: usize) -> FlatHandle { + utils::flat_handle(self.0.pieces[i].soft_body.0) + } + + pub fn pieceParticles(&self, i: usize) -> Vec { + self.0.pieces[i].particles.clone() + } + + pub fn pieceClusters(&self, i: usize) -> Vec { + flatten_elements(&self.0.pieces[i].clusters) + } + + pub fn numClusterSplits(&self) -> usize { + self.0.clusters.len() + } + + pub fn clusterSplitSource(&self, i: usize) -> u32 { + self.0.clusters[i].source_cluster + } + + pub fn clusterSplitSoftBody(&self, i: usize) -> FlatHandle { + utils::flat_handle(self.0.clusters[i].soft_body.0) + } + + pub fn clusterSplitCluster(&self, i: usize) -> u32 { + self.0.clusters[i].cluster + } + + pub fn clusterSplitProxy(&self, i: usize) -> FlatHandle { + utils::flat_handle(self.0.clusters[i].proxy.0) + } + + pub fn clusterSplitKeepsProxy(&self, i: usize) -> bool { + self.0.clusters[i].keeps_proxy + } + + pub fn numMovedJoints(&self) -> usize { + self.0.moved_joints.len() + } + + pub fn movedJoint(&self, i: usize) -> FlatHandle { + utils::flat_handle(self.0.moved_joints[i].joint.0) + } + + pub fn movedJointFrom(&self, i: usize) -> FlatHandle { + utils::flat_handle(self.0.moved_joints[i].from.0) + } + + pub fn movedJointTo(&self, i: usize) -> FlatHandle { + utils::flat_handle(self.0.moved_joints[i].to.0) + } + + pub fn particleDestinationBody(&self, particle: u32) -> Option { + self.0 + .particle_destination(particle) + .map(|(body, _)| utils::flat_handle(body.0)) + } + + pub fn particleDestinationIndex(&self, particle: u32) -> Option { + self.0.particle_destination(particle).map(|(_, i)| i) + } +} + +/* + * Set. + */ + +#[wasm_bindgen] +pub struct RawSoftBodySet(pub(crate) SoftBodySet); + +impl RawSoftBodySet { + pub(crate) fn map(&self, handle: FlatHandle, f: impl FnOnce(&SoftBody) -> T) -> T { + let body = self + .0 + .get(utils::soft_body_handle(handle)) + .expect("Invalid SoftBody reference. It may have been removed from the physics World."); + f(body) + } + + pub(crate) fn map_mut( + &mut self, + handle: FlatHandle, + f: impl FnOnce(&mut SoftBody) -> T, + ) -> T { + let body = self + .0 + .get_mut(utils::soft_body_handle(handle)) + .expect("Invalid SoftBody reference. It may have been removed from the physics World."); + f(body) + } +} + +#[wasm_bindgen] +impl RawSoftBodySet { + #[wasm_bindgen(constructor)] + pub fn new() -> Self { + RawSoftBodySet(SoftBodySet::new()) + } + + /// Inserts the soft body described by the builder, creating its hidden root rigid body and + /// its colliders. + pub fn insert( + &mut self, + builder: &RawSoftBodyBuilder, + bodies: &mut RawRigidBodySet, + colliders: &mut RawColliderSet, + ) -> FlatHandle { + let handle = self + .0 + .insert(builder.0.clone(), &mut bodies.0, &mut colliders.0); + utils::flat_handle(handle.0) + } + + pub fn remove( + &mut self, + handle: FlatHandle, + islands: &mut RawIslandManager, + bodies: &mut RawRigidBodySet, + colliders: &mut RawColliderSet, + joints: &mut RawImpulseJointSet, + articulations: &mut RawMultibodyJointSet, + ) { + self.0.remove( + utils::soft_body_handle(handle), + &mut islands.0, + &mut bodies.0, + &mut colliders.0, + &mut joints.0, + &mut articulations.0, + ); + } + + /// The number of soft bodies on this set. + pub fn len(&self) -> usize { + self.0.len() + } + + /// Checks if a soft body with the given integer handle exists. + pub fn contains(&self, handle: FlatHandle) -> bool { + self.0.contains(utils::soft_body_handle(handle)) + } + + /// Applies the given JavaScript function to the integer handle of each soft body managed by + /// this set. + pub fn forEachSoftBodyHandle(&self, f: &js_sys::Function) { + let this = JsValue::null(); + for (handle, _) in self.0.iter() { + let _ = f.call1(&this, &JsValue::from(utils::flat_handle(handle.0))); + } + } + + pub fn addCluster( + &mut self, + handle: FlatHandle, + particles: Vec, + bodies: &mut RawRigidBodySet, + colliders: &mut RawColliderSet, + ) -> Option { + self.0.add_cluster( + utils::soft_body_handle(handle), + &particles, + &mut bodies.0, + &mut colliders.0, + ) + } + + pub fn removeCluster( + &mut self, + handle: FlatHandle, + cluster: u32, + islands: &mut RawIslandManager, + bodies: &mut RawRigidBodySet, + colliders: &mut RawColliderSet, + joints: &mut RawImpulseJointSet, + articulations: &mut RawMultibodyJointSet, + ) -> bool { + self.0 + .remove_cluster( + utils::soft_body_handle(handle), + cluster, + &mut islands.0, + &mut bodies.0, + &mut colliders.0, + &mut joints.0, + &mut articulations.0, + ) + .is_some() + } + + pub fn tear( + &mut self, + handle: FlatHandle, + edges: Vec, + cells: Vec, + islands: &mut RawIslandManager, + bodies: &mut RawRigidBodySet, + colliders: &mut RawColliderSet, + joints: &mut RawImpulseJointSet, + articulations: &mut RawMultibodyJointSet, + ) -> Option { + self.0 + .tear( + utils::soft_body_handle(handle), + &edges, + &cells, + &mut islands.0, + &mut bodies.0, + &mut colliders.0, + &mut joints.0, + &mut articulations.0, + ) + .map(RawSoftBodyTearEvent) + } + + /// Cuts a soft body along a blade: a segment (two points) in 2D, a triangle (three points) + /// in 3D, given as `DIM` floats per point. + pub fn cut( + &mut self, + handle: FlatHandle, + blade: Vec, + islands: &mut RawIslandManager, + bodies: &mut RawRigidBodySet, + colliders: &mut RawColliderSet, + joints: &mut RawImpulseJointSet, + articulations: &mut RawMultibodyJointSet, + ) -> Option { + let points = read_vectors(&blade); + if points.len() != DIM { + return None; + } + let mut blade = [Vector::ZERO; DIM]; + blade.copy_from_slice(&points); + self.0 + .cut( + utils::soft_body_handle(handle), + &blade, + &mut islands.0, + &mut bodies.0, + &mut colliders.0, + &mut joints.0, + &mut articulations.0, + ) + .map(RawSoftBodyTearEvent) + } + + pub fn wakeUp(&mut self, handle: FlatHandle, bodies: &mut RawRigidBodySet, strong: bool) { + self.0 + .wake_up(utils::soft_body_handle(handle), &mut bodies.0, strong); + } + + /* + * Particles. + */ + + pub fn sbTopologyVersion(&self, handle: FlatHandle) -> u32 { + self.map(handle, |sb| sb.topology_version()) + } + + pub fn sbNumParticles(&self, handle: FlatHandle) -> usize { + self.map(handle, |sb| sb.num_particles()) + } + + pub fn sbParticlePosition( + &self, + handle: FlatHandle, + i: usize, + scratch_buffer: &js_sys::Float32Array, + ) { + self.map(handle, |sb| { + write_vector(scratch_buffer, sb.particle_position(i)) + }) + } + + pub fn sbParticlePositions(&self, handle: FlatHandle) -> Vec { + self.map(handle, |sb| { + sb.particle_positions().flat_map(|p| p.to_array()).collect() + }) + } + + pub fn sbParticleVelocity( + &self, + handle: FlatHandle, + i: usize, + scratch_buffer: &js_sys::Float32Array, + ) { + self.map(handle, |sb| { + write_vector(scratch_buffer, sb.particle_velocity(i)) + }) + } + + pub fn sbParticleVelocities(&self, handle: FlatHandle) -> Vec { + self.map(handle, |sb| { + sb.particle_velocities() + .flat_map(|v| v.to_array()) + .collect() + }) + } + + pub fn sbParticleRestPosition( + &self, + handle: FlatHandle, + i: usize, + scratch_buffer: &js_sys::Float32Array, + ) { + self.map(handle, |sb| { + write_vector(scratch_buffer, sb.particles()[i].rest_position()) + }) + } + + pub fn sbParticleMass(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.particles()[i].mass()) + } + + pub fn sbIsParticlePinned(&self, handle: FlatHandle, i: usize) -> bool { + self.map(handle, |sb| sb.particles()[i].is_pinned()) + } + + pub fn sbIsParticleOnSurface(&self, handle: FlatHandle, i: usize) -> bool { + self.map(handle, |sb| sb.particles()[i].is_on_surface()) + } + + pub fn sbIsParticleDamaged(&self, handle: FlatHandle, i: usize) -> bool { + self.map(handle, |sb| sb.particles()[i].is_damaged()) + } + + pub fn sbSetParticlePosition(&mut self, handle: FlatHandle, i: usize, position: &RawVector) { + self.map_mut(handle, |sb| sb.set_particle_position(i, position.0)) + } + + pub fn sbSetParticleVelocity(&mut self, handle: FlatHandle, i: usize, velocity: &RawVector) { + self.map_mut(handle, |sb| sb.set_particle_velocity(i, velocity.0)) + } + + pub fn sbSetParticleKinematicTarget( + &mut self, + handle: FlatHandle, + i: usize, + position: &RawVector, + ) { + self.map_mut(handle, |sb| sb.set_particle_kinematic_target(i, position.0)) + } + + pub fn sbSetParticlePinned(&mut self, handle: FlatHandle, i: usize, pinned: bool) { + self.map_mut(handle, |sb| sb.set_particle_pinned(i, pinned)) + } + + pub fn sbAttachParticle( + &mut self, + handle: FlatHandle, + i: usize, + body: FlatHandle, + bodies: &RawRigidBodySet, + ) { + self.map_mut(handle, |sb| { + sb.attach_particle(i, utils::body_handle(body), &bodies.0) + }) + } + + pub fn sbDetachParticle(&mut self, handle: FlatHandle, i: usize) -> bool { + self.map_mut(handle, |sb| sb.detach_particle(i)) + } + + pub fn sbNumAttachments(&self, handle: FlatHandle) -> usize { + self.map(handle, |sb| sb.particle_attachments().len()) + } + + pub fn sbAttachmentParticle(&self, handle: FlatHandle, i: usize) -> u32 { + self.map(handle, |sb| sb.particle_attachments()[i].particle) + } + + pub fn sbAttachmentBody(&self, handle: FlatHandle, i: usize) -> FlatHandle { + self.map(handle, |sb| { + utils::flat_handle(sb.particle_attachments()[i].body.0) + }) + } + + /* + * Elements. + */ + + pub fn sbNumEdges(&self, handle: FlatHandle) -> usize { + self.map(handle, |sb| sb.edges().len()) + } + + /// The particle pairs of every edge, two indices per edge. + pub fn sbEdges(&self, handle: FlatHandle) -> Vec { + self.map(handle, |sb| { + sb.edges().iter().flat_map(|e| e.vertices).collect() + }) + } + + pub fn sbEdgeRestLength(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.edges()[i].rest_length) + } + + pub fn sbEdgeIsBend(&self, handle: FlatHandle, i: usize) -> bool { + self.map(handle, |sb| { + sb.edges()[i].kind == rapier::dynamics::SoftBodyEdgeKind::Bend + }) + } + + pub fn sbEdgeImpulse(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.edges()[i].impulse()) + } + + pub fn sbEdgeStress(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.edges()[i].stress()) + } + + pub fn sbEdgePlasticStrain(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.edges()[i].plastic_strain()) + } + + pub fn sbEdgeTearResistance(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.edges()[i].tear_resistance) + } + + pub fn sbNumCells(&self, handle: FlatHandle) -> usize { + self.map(handle, |sb| sb.cells().len()) + } + + /// The particles of every cell, `DIM + 1` indices per cell. + pub fn sbCells(&self, handle: FlatHandle) -> Vec { + self.map(handle, |sb| { + sb.cells().iter().flat_map(|c| c.vertices).collect() + }) + } + + pub fn sbCellRestVolume(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.cells()[i].rest_volume) + } + + pub fn sbCellStress(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.cells()[i].stress()) + } + + pub fn sbCellStiffnessScale(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.cells()[i].stiffness_scale) + } + + pub fn sbCellTearResistance(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.cells()[i].tear_resistance) + } + + #[cfg(feature = "dim3")] + pub fn sbNumDihedrals(&self, handle: FlatHandle) -> usize { + self.map(handle, |sb| sb.dihedrals().len()) + } + + /// The particles of every dihedral (3D), four indices per dihedral. + #[cfg(feature = "dim3")] + pub fn sbDihedrals(&self, handle: FlatHandle) -> Vec { + self.map(handle, |sb| { + sb.dihedrals().iter().flat_map(|d| d.vertices).collect() + }) + } + + #[cfg(feature = "dim3")] + pub fn sbDihedralRestAngle(&self, handle: FlatHandle, i: usize) -> f32 { + self.map(handle, |sb| sb.dihedrals()[i].rest_angle) + } + + /// The boundary elements of the body (segments in 2D, triangles in 3D), `DIM` particle + /// indices per element. + pub fn sbBoundary(&self, handle: FlatHandle) -> Vec { + self.map(handle, |sb| flatten_elements(sb.boundary())) + } + + /* + * Material and models. + */ + + pub fn sbMaterial(&self, handle: FlatHandle) -> RawSoftBodyMaterial { + self.map(handle, |sb| RawSoftBodyMaterial(sb.material().clone())) + } + + pub fn sbSetMaterial(&mut self, handle: FlatHandle, material: &RawSoftBodyMaterial) { + self.map_mut(handle, |sb| sb.set_material(material.0.clone())) + } + + pub fn sbCellModel(&self, handle: FlatHandle) -> RawSoftBodyCellModel { + self.map(handle, |sb| sb.cell_model().into()) + } + + pub fn sbVolumePreservationEnabled(&self, handle: FlatHandle) -> bool { + self.map(handle, |sb| sb.volume_preservation_enabled()) + } + + pub fn sbEnableVolumePreservation(&mut self, handle: FlatHandle, enabled: bool) { + self.map_mut(handle, |sb| sb.enable_volume_preservation(enabled)) + } + + pub fn sbRestVolume(&self, handle: FlatHandle) -> f32 { + self.map(handle, |sb| sb.rest_volume()) + } + + pub fn sbVolume(&self, handle: FlatHandle) -> f32 { + self.map(handle, |sb| sb.volume()) + } + + pub fn sbVolumeFactor(&self, handle: FlatHandle) -> f32 { + self.map(handle, |sb| sb.volume_factor()) + } + + pub fn sbSetVolumeFactor(&mut self, handle: FlatHandle, factor: f32) { + self.map_mut(handle, |sb| sb.set_volume_factor(factor)) + } + + pub fn sbParticleRadius(&self, handle: FlatHandle) -> f32 { + self.map(handle, |sb| sb.particle_radius()) + } + + pub fn sbResetPlasticity(&mut self, handle: FlatHandle) { + self.map_mut(handle, |sb| sb.reset_plasticity()) + } + + /* + * Whole-body state. + */ + + pub fn sbRootBody(&self, handle: FlatHandle) -> FlatHandle { + self.map(handle, |sb| utils::flat_handle(sb.root_body().0)) + } + + pub fn sbOrigin(&self, handle: FlatHandle) -> Option { + self.map(handle, |sb| sb.origin().map(|h| utils::flat_handle(h.0))) + } + + pub fn sbPieces(&self, handle: FlatHandle) -> Vec { + self.map(handle, |sb| { + sb.pieces() + .iter() + .map(|h| utils::flat_handle(h.0)) + .collect() + }) + } + + pub fn sbCenterOfMass(&self, handle: FlatHandle, scratch_buffer: &js_sys::Float32Array) { + self.map(handle, |sb| { + write_vector(scratch_buffer, sb.center_of_mass()) + }) + } + + pub fn sbMass(&self, handle: FlatHandle) -> f32 { + self.map(handle, |sb| sb.mass()) + } + + pub fn sbIsSleeping(&self, handle: FlatHandle) -> bool { + self.map(handle, |sb| sb.is_sleeping()) + } + + pub fn sbWakeUp(&mut self, handle: FlatHandle) { + self.map_mut(handle, |sb| sb.wake_up()) + } + + pub fn sbIsEnabled(&self, handle: FlatHandle) -> bool { + self.map(handle, |sb| sb.is_enabled()) + } + + pub fn sbSetEnabled(&mut self, handle: FlatHandle, enabled: bool) { + self.map_mut(handle, |sb| sb.set_enabled(enabled)) + } + + pub fn sbSetAdditionalPgsIterations(&mut self, handle: FlatHandle, iterations: usize) { + self.map_mut(handle, |sb| sb.set_additional_pgs_iterations(iterations)) + } + + pub fn sbLinearDamping(&self, handle: FlatHandle) -> f32 { + self.map(handle, |sb| sb.particle_settings().linear_damping) + } + + pub fn sbGravityScale(&self, handle: FlatHandle) -> f32 { + self.map(handle, |sb| sb.particle_settings().gravity_scale) + } + + pub fn sbUserData(&self, handle: FlatHandle) -> f64 { + self.map(handle, |sb| sb.user_data as f64) + } + + pub fn sbSetUserData(&mut self, handle: FlatHandle, data: f64) { + self.map_mut(handle, |sb| sb.user_data = data as u128) + } + + /* + * Forces and impulses. + */ + + pub fn sbAddForce(&mut self, handle: FlatHandle, force: &RawVector, wake_up: bool) { + self.map_mut(handle, |sb| sb.add_force(force.0, wake_up)) + } + + pub fn sbAddParticleForce( + &mut self, + handle: FlatHandle, + i: usize, + force: &RawVector, + wake_up: bool, + ) { + self.map_mut(handle, |sb| sb.add_particle_force(i, force.0, wake_up)) + } + + pub fn sbResetForces(&mut self, handle: FlatHandle, wake_up: bool) { + self.map_mut(handle, |sb| sb.reset_forces(wake_up)) + } + + pub fn sbApplyImpulse(&mut self, handle: FlatHandle, impulse: &RawVector, wake_up: bool) { + self.map_mut(handle, |sb| sb.apply_impulse(impulse.0, wake_up)) + } + + pub fn sbApplyParticleImpulse( + &mut self, + handle: FlatHandle, + i: usize, + impulse: &RawVector, + wake_up: bool, + ) { + self.map_mut(handle, |sb| { + sb.apply_particle_impulse(i, impulse.0, wake_up) + }) + } + + pub fn sbApplyImpulseAtPoint( + &mut self, + handle: FlatHandle, + impulse: &RawVector, + point: &RawVector, + falloff_radius: f32, + wake_up: bool, + ) { + self.map_mut(handle, |sb| { + sb.apply_impulse_at_point(impulse.0, point.0, falloff_radius, wake_up) + }) + } + + pub fn sbApplyRadialImpulse( + &mut self, + handle: FlatHandle, + center: &RawVector, + magnitude: f32, + falloff_radius: f32, + wake_up: bool, + ) { + self.map_mut(handle, |sb| { + sb.apply_radial_impulse(center.0, magnitude, falloff_radius, wake_up) + }) + } + + /* + * Tearing. + */ + + pub fn sbTearEdge(&mut self, handle: FlatHandle, i: usize) { + self.map_mut(handle, |sb| sb.tear_edge(i)) + } + + pub fn sbTearCell(&mut self, handle: FlatHandle, i: usize) { + self.map_mut(handle, |sb| sb.tear_cell(i)) + } + + pub fn sbHasPendingTears(&self, handle: FlatHandle) -> bool { + self.map(handle, |sb| sb.has_pending_tears()) + } + + /* + * Clusters. + */ + + pub fn sbNumClusters(&self, handle: FlatHandle) -> usize { + self.map(handle, |sb| sb.clusters().len()) + } + + pub fn sbIsClusterLive(&self, handle: FlatHandle, i: u32) -> bool { + self.map(handle, |sb| sb.cluster(i).is_some_and(|c| c.is_live())) + } + + pub fn sbClusterProxy(&self, handle: FlatHandle, i: u32) -> Option { + self.map(handle, |sb| { + sb.cluster_proxy(i).map(|h| utils::flat_handle(h.0)) + }) + } + + pub fn sbClusterParticles(&self, handle: FlatHandle, i: u32) -> Vec { + self.map(handle, |sb| { + sb.cluster(i) + .map(|c| c.particles().to_vec()) + .unwrap_or_default() + }) + } + + pub fn sbClusterShapeMatchingEnabled(&self, handle: FlatHandle, i: u32) -> bool { + self.map(handle, |sb| { + sb.cluster(i).is_some_and(|c| c.shape_matching_enabled()) + }) + } + + pub fn sbEnableClusterShapeMatching(&mut self, handle: FlatHandle, i: u32, enabled: bool) { + self.map_mut(handle, |sb| sb.enable_cluster_shape_matching(i, enabled)) + } + + pub fn sbSetClusterStiffnessScale(&mut self, handle: FlatHandle, i: u32, scale: f32) { + self.map_mut(handle, |sb| sb.set_cluster_stiffness_scale(i, scale)) + } + + pub fn sbSetClusterEdgeSoftness( + &mut self, + handle: FlatHandle, + i: u32, + natural_frequency: Option, + damping_ratio: Option, + ) { + let softness = match (natural_frequency, damping_ratio) { + (Some(f), Some(d)) => Some(SpringCoefficients::new(f, d)), + _ => None, + }; + self.map_mut(handle, |sb| sb.set_cluster_edge_softness(i, softness)) + } + + pub fn sbSetClusterTearResistance(&mut self, handle: FlatHandle, i: u32, resistance: f32) { + self.map_mut(handle, |sb| sb.set_cluster_tear_resistance(i, resistance)) + } + + pub fn sbSetClusterPinned(&mut self, handle: FlatHandle, i: u32, pinned: bool) { + self.map_mut(handle, |sb| sb.set_cluster_pinned(i, pinned)) + } + + pub fn sbSetClusterKinematicTarget( + &mut self, + handle: FlatHandle, + i: u32, + translation: &RawVector, + rotation: &RawRotation, + ) { + let pose = Pose::from_parts(translation.0, rotation.0); + self.map_mut(handle, |sb| sb.set_cluster_kinematic_target(i, pose)) + } + + /* + * Collision meshes, indexed in the order the body holds them. + */ + + pub fn sbNumMeshes(&self, handle: FlatHandle) -> usize { + self.map(handle, |sb| sb.meshes().count()) + } + + pub fn sbMeshCluster(&self, handle: FlatHandle, i: usize) -> Option { + self.map(handle, |sb| sb.meshes().nth(i).map(|m| m.id().cluster)) + } + + pub fn sbMeshCollider(&self, handle: FlatHandle, i: usize) -> Option { + self.map(handle, |sb| { + sb.meshes() + .nth(i) + .map(|m| utils::flat_handle(m.collider().0)) + }) + } + + pub fn sbMeshIsSkinned(&self, handle: FlatHandle, i: usize) -> bool { + self.map(handle, |sb| { + sb.meshes().nth(i).is_some_and(|m| m.is_skinned()) + }) + } + + pub fn sbMeshCollisionEnabled(&self, handle: FlatHandle, i: usize) -> bool { + self.map(handle, |sb| { + sb.meshes().nth(i).is_some_and(|m| m.collision_enabled()) + }) + } + + /// The world-space vertex positions of a collision mesh, `DIM` floats per vertex. + pub fn sbMeshVertices(&self, handle: FlatHandle, i: usize) -> Vec { + self.map(handle, |sb| { + sb.meshes() + .nth(i) + .map(|m| m.vertex_positions(sb).flat_map(|v| v.to_array()).collect()) + .unwrap_or_default() + }) + } + + /// The elements of a collision mesh, `DIM` vertex indices per element. + pub fn sbMeshIndices(&self, handle: FlatHandle, i: usize) -> Vec { + self.map(handle, |sb| { + sb.meshes() + .nth(i) + .map(|m| flatten_elements(m.indices())) + .unwrap_or_default() + }) + } + + /// The index, in the body's mesh list, of the mesh a deformable collider holds. + pub fn sbMeshOfCollider(&self, handle: FlatHandle, collider: FlatHandle) -> Option { + let collider = utils::collider_handle(collider); + self.map(handle, |sb| { + sb.meshes().position(|m| m.collider() == collider) + }) + } +} + +impl Default for RawSoftBodySet { + fn default() -> Self { + Self::new() + } +} diff --git a/typescript/src/geometry/collider.rs b/typescript/src/geometry/collider.rs index a7880c7cc..5b3178041 100644 --- a/typescript/src/geometry/collider.rs +++ b/typescript/src/geometry/collider.rs @@ -750,6 +750,22 @@ impl RawColliderSet { }) } + pub fn coCompoundFlags(&self, handle: FlatHandle) -> Option { + self.map(handle, |co| { + co.shape() + .as_compound() + .map(|compound| compound.flags().bits() as u32) + }) + } + + pub fn coPolylineFlags(&self, handle: FlatHandle) -> Option { + self.map(handle, |co| { + co.shape() + .as_polyline() + .map(|polyline| polyline.flags().bits() as u32) + }) + } + pub fn coTriMeshFlags(&self, handle: FlatHandle) -> Option { self.map(handle, |co| { co.shape().as_trimesh().map(|tri| tri.flags().bits() as u32) @@ -848,6 +864,19 @@ impl RawColliderSet { self.map(handle, |co| co.parent().map(|p| utils::flat_handle(p.0))) } + /// The soft body whose deformable collision mesh this collider holds, if any. + pub fn coSoftBody(&self, handle: FlatHandle) -> Option { + self.map(handle, |co| { + co.deformable_mesh_ref() + .map(|mesh| utils::flat_handle(mesh.body.0)) + }) + } + + /// Does this collider hold a soft body's deformable collision mesh? + pub fn coIsDeformable(&self, handle: FlatHandle) -> bool { + self.map(handle, |co| co.deformable_mesh_ref().is_some()) + } + pub fn coSetEnabled(&mut self, handle: FlatHandle, enabled: bool) { self.map_mut(handle, |co| co.set_enabled(enabled)) } diff --git a/typescript/src/geometry/collider_set.rs b/typescript/src/geometry/collider_set.rs index 14e715263..cca36d299 100644 --- a/typescript/src/geometry/collider_set.rs +++ b/typescript/src/geometry/collider_set.rs @@ -1,7 +1,8 @@ -use crate::dynamics::{RawIslandManager, RawRigidBodySet}; +use crate::dynamics::{RawIslandManager, RawRigidBodySet, RawSoftBodySet, RawSoftMeshBindingMode}; use crate::geometry::RawShape; use crate::math::{RawRotation, RawVector}; use crate::utils::{self, FlatHandle}; +use rapier::dynamics::SoftMeshBinding; use rapier::prelude::*; use wasm_bindgen::prelude::*; @@ -82,6 +83,69 @@ impl RawColliderSet { parent: FlatHandle, bodies: &mut RawRigidBodySet, ) -> Option { + let collider = Self::build_collider( + enabled, + shape, + translation, + rotation, + massPropsMode, + mass, + centerOfMass, + principalAngularInertia, + #[cfg(feature = "dim3")] + angularInertiaFrame, + density, + friction, + restitution, + frictionCombineRule, + restitutionCombineRule, + isSensor, + collisionGroups, + solverGroups, + activeCollisionTypes, + activeHooks, + activeEvents, + contactForceEventThreshold, + contactSkin, + ); + + if hasParent { + Some(utils::flat_handle( + self.0 + .insert_with_parent(collider, utils::body_handle(parent), &mut bodies.0) + .0, + )) + } else { + Some(utils::flat_handle(self.0.insert(collider).0)) + } + } + + /// Builds a collider from the flattened description shared by every creation method. + pub fn build_collider( + enabled: bool, + shape: &RawShape, + translation: &RawVector, + rotation: &RawRotation, + massPropsMode: u32, + mass: f32, + centerOfMass: &RawVector, + #[cfg(feature = "dim2")] principalAngularInertia: f32, + #[cfg(feature = "dim3")] principalAngularInertia: &RawVector, + #[cfg(feature = "dim3")] angularInertiaFrame: &RawRotation, + density: f32, + friction: f32, + restitution: f32, + frictionCombineRule: u32, + restitutionCombineRule: u32, + isSensor: bool, + collisionGroups: u32, + solverGroups: u32, + activeCollisionTypes: u16, + activeHooks: u32, + activeEvents: u32, + contactForceEventThreshold: f32, + contactSkin: f32, + ) -> Collider { let pos = Pose::from_parts(translation.0.into(), rotation.0); let mut builder = ColliderBuilder::new(shape.0.clone()) .enabled(enabled) @@ -120,17 +184,89 @@ impl RawColliderSet { builder = builder.mass(mass); }; - let collider = builder.build(); + builder.build() + } - if hasParent { - Some(utils::flat_handle( - self.0 - .insert_with_parent(collider, utils::body_handle(parent), &mut bodies.0) - .0, - )) - } else { - Some(utils::flat_handle(self.0.insert(collider).0)) + /// Inserts a collider holding a soft body's deformable collision mesh, bound to the cluster + /// whose proxy is `parent`. Returns `None` when the binding fails (see + /// `SoftBindingError`). + #[allow(clippy::too_many_arguments)] + pub fn do_create_deformable_collider( + &mut self, + enabled: bool, + shape: &RawShape, + translation: &RawVector, + rotation: &RawRotation, + massPropsMode: u32, + mass: f32, + centerOfMass: &RawVector, + #[cfg(feature = "dim2")] principalAngularInertia: f32, + #[cfg(feature = "dim3")] principalAngularInertia: &RawVector, + #[cfg(feature = "dim3")] angularInertiaFrame: &RawRotation, + density: f32, + friction: f32, + restitution: f32, + frictionCombineRule: u32, + restitutionCombineRule: u32, + isSensor: bool, + collisionGroups: u32, + solverGroups: u32, + activeCollisionTypes: u16, + activeHooks: u32, + activeEvents: u32, + contactForceEventThreshold: f32, + contactSkin: f32, + bindingMode: RawSoftMeshBindingMode, + bindingParticles: Vec, + bindingEps: f32, + bindingSelfContacts: bool, + parent: FlatHandle, + bodies: &mut RawRigidBodySet, + softBodies: &mut RawSoftBodySet, + ) -> Option { + let collider = Self::build_collider( + enabled, + shape, + translation, + rotation, + massPropsMode, + mass, + centerOfMass, + principalAngularInertia, + #[cfg(feature = "dim3")] + angularInertiaFrame, + density, + friction, + restitution, + frictionCombineRule, + restitutionCombineRule, + isSensor, + collisionGroups, + solverGroups, + activeCollisionTypes, + activeHooks, + activeEvents, + contactForceEventThreshold, + contactSkin, + ); + let binding = match bindingMode { + RawSoftMeshBindingMode::Direct => SoftMeshBinding::direct(bindingParticles), + RawSoftMeshBindingMode::DirectByPosition => { + SoftMeshBinding::direct_by_position(bindingEps) + } + RawSoftMeshBindingMode::Skinned => SoftMeshBinding::skinned(), } + .self_contacts(bindingSelfContacts); + self.0 + .insert_deformable( + collider, + binding, + utils::body_handle(parent), + &mut bodies.0, + &mut softBodies.0, + ) + .ok() + .map(|handle| utils::flat_handle(handle.0)) } } @@ -269,10 +405,147 @@ impl RawColliderSet { handle: FlatHandle, islands: &mut RawIslandManager, bodies: &mut RawRigidBodySet, + softBodies: &mut RawSoftBodySet, wakeUp: bool, ) { let handle = utils::collider_handle(handle); - self.0.remove(handle, &mut islands.0, &mut bodies.0, wakeUp); + self.0.remove( + handle, + &mut islands.0, + &mut bodies.0, + &mut softBodies.0, + wakeUp, + ); + } + + #[cfg(feature = "dim2")] + pub fn createDeformableCollider( + &mut self, + enabled: bool, + shape: &RawShape, + translation: &RawVector, + rotation: &RawRotation, + massPropsMode: u32, + mass: f32, + centerOfMass: &RawVector, + principalAngularInertia: f32, + density: f32, + friction: f32, + restitution: f32, + frictionCombineRule: u32, + restitutionCombineRule: u32, + isSensor: bool, + collisionGroups: u32, + solverGroups: u32, + activeCollisionTypes: u16, + activeHooks: u32, + activeEvents: u32, + contactForceEventThreshold: f32, + contactSkin: f32, + bindingMode: RawSoftMeshBindingMode, + bindingParticles: Vec, + bindingEps: f32, + bindingSelfContacts: bool, + parent: FlatHandle, + bodies: &mut RawRigidBodySet, + softBodies: &mut RawSoftBodySet, + ) -> Option { + self.do_create_deformable_collider( + enabled, + shape, + translation, + rotation, + massPropsMode, + mass, + centerOfMass, + principalAngularInertia, + density, + friction, + restitution, + frictionCombineRule, + restitutionCombineRule, + isSensor, + collisionGroups, + solverGroups, + activeCollisionTypes, + activeHooks, + activeEvents, + contactForceEventThreshold, + contactSkin, + bindingMode, + bindingParticles, + bindingEps, + bindingSelfContacts, + parent, + bodies, + softBodies, + ) + } + + #[cfg(feature = "dim3")] + pub fn createDeformableCollider( + &mut self, + enabled: bool, + shape: &RawShape, + translation: &RawVector, + rotation: &RawRotation, + massPropsMode: u32, + mass: f32, + centerOfMass: &RawVector, + principalAngularInertia: &RawVector, + angularInertiaFrame: &RawRotation, + density: f32, + friction: f32, + restitution: f32, + frictionCombineRule: u32, + restitutionCombineRule: u32, + isSensor: bool, + collisionGroups: u32, + solverGroups: u32, + activeCollisionTypes: u16, + activeHooks: u32, + activeEvents: u32, + contactForceEventThreshold: f32, + contactSkin: f32, + bindingMode: RawSoftMeshBindingMode, + bindingParticles: Vec, + bindingEps: f32, + bindingSelfContacts: bool, + parent: FlatHandle, + bodies: &mut RawRigidBodySet, + softBodies: &mut RawSoftBodySet, + ) -> Option { + self.do_create_deformable_collider( + enabled, + shape, + translation, + rotation, + massPropsMode, + mass, + centerOfMass, + principalAngularInertia, + angularInertiaFrame, + density, + friction, + restitution, + frictionCombineRule, + restitutionCombineRule, + isSensor, + collisionGroups, + solverGroups, + activeCollisionTypes, + activeHooks, + activeEvents, + contactForceEventThreshold, + contactSkin, + bindingMode, + bindingParticles, + bindingEps, + bindingSelfContacts, + parent, + bodies, + softBodies, + ) } /// Checks if a collider with the given integer handle exists. diff --git a/typescript/src/geometry/narrow_phase.rs b/typescript/src/geometry/narrow_phase.rs index fc1cc9d55..22d78a5c1 100644 --- a/typescript/src/geometry/narrow_phase.rs +++ b/typescript/src/geometry/narrow_phase.rs @@ -80,11 +80,12 @@ impl RawContactPair { } pub fn numContactManifolds(&self) -> usize { - unsafe { (*self.0).manifolds.len() } + unsafe { (*self.0).manifolds().len() } } pub fn contactManifold(&self, i: usize) -> Option { unsafe { - (&(*self.0).manifolds) + (*self.0) + .manifolds() .get(i) .map(|m| RawContactManifold(m as *const ContactManifold)) } diff --git a/typescript/src/geometry/shape.rs b/typescript/src/geometry/shape.rs index 94c26357b..56aa44cf3 100644 --- a/typescript/src/geometry/shape.rs +++ b/typescript/src/geometry/shape.rs @@ -573,6 +573,18 @@ impl RawShape { }) } + pub fn compoundFlags(&self) -> Option { + self.0 + .as_compound() + .map(|compound| compound.flags().bits() as u32) + } + + pub fn polylineFlags(&self) -> Option { + self.0 + .as_polyline() + .map(|polyline| polyline.flags().bits() as u32) + } + pub fn triMeshFlags(&self) -> Option { self.0 .as_trimesh() @@ -732,17 +744,22 @@ impl RawShape { Self(SharedShape::voxels_from_points(voxel_size.0, &points)) } - pub fn polyline(vertices: Vec, indices: Vec) -> Self { + pub fn polyline(vertices: Vec, indices: Vec, flags: u32) -> Self { + use rapier::parry::shape::{Polyline, PolylineFlags}; + let flags = PolylineFlags::from_bits(flags as u8).unwrap_or_default(); let vertices = vertices .chunks(DIM) .map(|v| Vector::from_slice(v)) .collect(); let indices: Vec<_> = indices.chunks(2).map(|v| [v[0], v[1]]).collect(); - if indices.is_empty() { - Self(SharedShape::polyline(vertices, None)) + let indices = if indices.is_empty() { + None } else { - Self(SharedShape::polyline(vertices, Some(indices))) - } + Some(indices) + }; + Self(SharedShape::new(Polyline::with_flags( + vertices, indices, flags, + ))) } pub fn trimesh(vertices: Vec, indices: Vec, flags: u32) -> Option { @@ -850,7 +867,12 @@ impl RawShape { SharedShape::round_convex_mesh(vertices, &indices, borderRadius).map(|s| Self(s)) } - pub fn compound(shapes: Vec, positions: Vec, rotations: Vec) -> RawShape { + pub fn compound( + shapes: Vec, + positions: Vec, + rotations: Vec, + flags: u32, + ) -> RawShape { let mut compound_parts = Vec::new(); let num_shapes = shapes.len(); @@ -903,17 +925,22 @@ impl RawShape { compound_parts.push((pose, shapes[i].0.clone())); } - Self(SharedShape::compound(compound_parts)) + Self(compound_with_flags(compound_parts, flags)) } - pub fn convexDecomposition(vertices: Vec, indices: Vec) -> Option { - Self::convexDecompositionWithParams(vertices, indices, &RawVHACDParameters::new()) + pub fn convexDecomposition( + vertices: Vec, + indices: Vec, + flags: u32, + ) -> Option { + Self::convexDecompositionWithParams(vertices, indices, &RawVHACDParameters::new(), flags) } pub fn convexDecompositionWithParams( vertices: Vec, indices: Vec, params: &RawVHACDParameters, + flags: u32, ) -> Option { let vertices: Vec<_> = vertices .chunks(DIM) @@ -949,7 +976,7 @@ impl RawShape { return None; } - Some(Self(SharedShape::compound(parts))) + Some(Self(compound_with_flags(parts, flags))) } pub fn castShape( @@ -1313,3 +1340,10 @@ mod tests { assert!(RawShape::convexMesh(roundtrip_vertices, roundtrip_indices).is_some()); } } + +/// A compound shape with the given `CompoundFlags` bits (the default weld tolerance). +fn compound_with_flags(parts: Vec<(Pose, SharedShape)>, flags: u32) -> SharedShape { + use rapier::parry::shape::{Compound, CompoundFlags}; + let flags = CompoundFlags::from_bits(flags as u8).unwrap_or_default(); + SharedShape::new(Compound::with_flags(parts, flags, None)) +} diff --git a/typescript/src/pipeline/debug_render_pipeline.rs b/typescript/src/pipeline/debug_render_pipeline.rs index 690b70bc4..c1ba5df26 100644 --- a/typescript/src/pipeline/debug_render_pipeline.rs +++ b/typescript/src/pipeline/debug_render_pipeline.rs @@ -1,4 +1,4 @@ -use crate::dynamics::{RawImpulseJointSet, RawMultibodyJointSet, RawRigidBodySet}; +use crate::dynamics::{RawImpulseJointSet, RawMultibodyJointSet, RawRigidBodySet, RawSoftBodySet}; use crate::geometry::{RawColliderSet, RawNarrowPhase}; use js_sys::Float32Array; use palette::convert::IntoColorUnclamped; @@ -45,6 +45,7 @@ impl RawDebugRenderPipeline { &mut self, bodies: &RawRigidBodySet, colliders: &RawColliderSet, + soft_bodies: &RawSoftBodySet, impulse_joints: &RawImpulseJointSet, multibody_joints: &RawMultibodyJointSet, narrow_phase: &RawNarrowPhase, @@ -77,6 +78,7 @@ impl RawDebugRenderPipeline { &impulse_joints.0, &multibody_joints.0, &narrow_phase.0, + &soft_bodies.0, ) }) } @@ -126,6 +128,9 @@ impl<'a> DebugRenderBackend for CopyToBuffersBackend<'a> { test_rigid_body(rb) } DebugRenderObject::RigidBody(_, rb) => test_rigid_body(rb), + DebugRenderObject::SoftBody(_, sb) => { + self.bodies.get(sb.root_body()).is_some_and(test_rigid_body) + } } } diff --git a/typescript/src/pipeline/event_queue.rs b/typescript/src/pipeline/event_queue.rs index af61b6273..a49fc146e 100644 --- a/typescript/src/pipeline/event_queue.rs +++ b/typescript/src/pipeline/event_queue.rs @@ -1,5 +1,7 @@ +use crate::dynamics::RawSoftBodyTearEvent; use crate::utils; use crate::utils::FlatHandle; +use rapier::dynamics::SoftBodyTearEvent; use rapier::geometry::{CollisionEvent, ContactForceEvent}; use rapier::pipeline::ChannelEventCollector; use std::sync::mpsc::Receiver; @@ -12,6 +14,7 @@ pub struct RawEventQueue { pub(crate) collector: ChannelEventCollector, collision_events: Receiver, contact_force_events: Receiver, + soft_body_tear_events: Receiver, pub(crate) auto_drain: bool, } @@ -87,12 +90,18 @@ impl RawEventQueue { pub fn new(autoDrain: bool) -> Self { let collision_channel = std::sync::mpsc::channel(); let contact_force_channel = std::sync::mpsc::channel(); - let collector = ChannelEventCollector::new(collision_channel.0, contact_force_channel.0); + let soft_body_tear_channel = std::sync::mpsc::channel(); + let collector = ChannelEventCollector::new( + collision_channel.0, + contact_force_channel.0, + soft_body_tear_channel.0, + ); Self { collector, collision_events: collision_channel.1, contact_force_events: contact_force_channel.1, + soft_body_tear_events: soft_body_tear_channel.1, auto_drain: autoDrain, } } @@ -140,8 +149,19 @@ impl RawEventQueue { } } + /// Applies the given javascript closure on each soft-body tear event of this collector, + /// then clears the internal tear event buffer. + pub fn drainSoftBodyTearEvents(&mut self, f: &js_sys::Function) { + let this = JsValue::null(); + while let Ok(event) = self.soft_body_tear_events.try_recv() { + let _ = f.call1(&this, &JsValue::from(RawSoftBodyTearEvent(event))); + } + } + /// Removes all events contained by this collector. pub fn clear(&self) { while let Ok(_) = self.collision_events.try_recv() {} + while let Ok(_) = self.contact_force_events.try_recv() {} + while let Ok(_) = self.soft_body_tear_events.try_recv() {} } } diff --git a/typescript/src/pipeline/physics_pipeline.rs b/typescript/src/pipeline/physics_pipeline.rs index e844ed995..27b769938 100644 --- a/typescript/src/pipeline/physics_pipeline.rs +++ b/typescript/src/pipeline/physics_pipeline.rs @@ -1,6 +1,6 @@ use crate::dynamics::{ RawCCDSolver, RawImpulseJointSet, RawIntegrationParameters, RawIslandManager, - RawMultibodyJointSet, RawRigidBodySet, + RawMultibodyJointSet, RawRigidBodySet, RawSoftBodySet, }; use crate::geometry::{RawBroadPhase, RawColliderSet, RawNarrowPhase}; use crate::math::RawVector; @@ -105,6 +105,7 @@ impl RawPhysicsPipeline { narrowPhase: &mut RawNarrowPhase, bodies: &mut RawRigidBodySet, colliders: &mut RawColliderSet, + softBodies: &mut RawSoftBodySet, joints: &mut RawImpulseJointSet, articulations: &mut RawMultibodyJointSet, ccd_solver: &mut RawCCDSolver, @@ -119,6 +120,7 @@ impl RawPhysicsPipeline { &mut colliders.0, &mut joints.0, &mut articulations.0, + &mut softBodies.0, &mut ccd_solver.0, &(), &(), @@ -134,6 +136,7 @@ impl RawPhysicsPipeline { narrowPhase: &mut RawNarrowPhase, bodies: &mut RawRigidBodySet, colliders: &mut RawColliderSet, + softBodies: &mut RawSoftBodySet, joints: &mut RawImpulseJointSet, articulations: &mut RawMultibodyJointSet, ccd_solver: &mut RawCCDSolver, @@ -162,6 +165,7 @@ impl RawPhysicsPipeline { &mut colliders.0, &mut joints.0, &mut articulations.0, + &mut softBodies.0, &mut ccd_solver.0, &hooks, &eventQueue.collector, diff --git a/typescript/src/pipeline/serialization_pipeline.rs b/typescript/src/pipeline/serialization_pipeline.rs index 8d2e841ad..16a6ec55f 100644 --- a/typescript/src/pipeline/serialization_pipeline.rs +++ b/typescript/src/pipeline/serialization_pipeline.rs @@ -1,12 +1,13 @@ use crate::dynamics::{ RawImpulseJointSet, RawIntegrationParameters, RawIslandManager, RawMultibodyJointSet, - RawRigidBodySet, + RawRigidBodySet, RawSoftBodySet, }; use crate::geometry::{RawBroadPhase, RawColliderSet, RawNarrowPhase}; use crate::math::RawVector; use js_sys::Uint8Array; use rapier::dynamics::{ ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, RigidBodySet, + SoftBodySet, }; use rapier::geometry::{ColliderSet, DefaultBroadPhase, NarrowPhase}; use rapier::math::Vector; @@ -23,6 +24,7 @@ struct SerializableWorld<'a> { colliders: &'a ColliderSet, impulse_joints: &'a ImpulseJointSet, multibody_joints: &'a MultibodyJointSet, + soft_bodies: &'a SoftBodySet, } #[derive(Deserialize)] @@ -36,6 +38,8 @@ struct DeserializableWorld { colliders: ColliderSet, impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, + #[serde(default)] + soft_bodies: SoftBodySet, } #[wasm_bindgen] @@ -49,6 +53,7 @@ pub struct RawDeserializedWorld { colliders: Option, impulse_joints: Option, multibody_joints: Option, + soft_bodies: Option, } #[wasm_bindgen] @@ -88,6 +93,10 @@ impl RawDeserializedWorld { pub fn takeMultibodyJoints(&mut self) -> Option { self.multibody_joints.take() } + + pub fn takeSoftBodies(&mut self) -> Option { + self.soft_bodies.take() + } } #[wasm_bindgen] @@ -109,6 +118,7 @@ impl RawSerializationPipeline { narrowPhase: &RawNarrowPhase, bodies: &RawRigidBodySet, colliders: &RawColliderSet, + soft_bodies: &RawSoftBodySet, impulse_joints: &RawImpulseJointSet, multibody_joints: &RawMultibodyJointSet, ) -> Option { @@ -122,6 +132,7 @@ impl RawSerializationPipeline { colliders: &colliders.0, impulse_joints: &impulse_joints.0, multibody_joints: &multibody_joints.0, + soft_bodies: &soft_bodies.0, }; let snap = bincode::serialize(&to_serialize).ok()?; Some(Uint8Array::from(&snap[..])) @@ -140,6 +151,7 @@ impl RawSerializationPipeline { colliders: Some(RawColliderSet(d.colliders)), impulse_joints: Some(RawImpulseJointSet(d.impulse_joints)), multibody_joints: Some(RawMultibodyJointSet(d.multibody_joints)), + soft_bodies: Some(RawSoftBodySet(d.soft_bodies)), }) } } diff --git a/typescript/src/utils.rs b/typescript/src/utils.rs index 1b46e7cc8..957b8e6c2 100644 --- a/typescript/src/utils.rs +++ b/typescript/src/utils.rs @@ -1,5 +1,5 @@ use rapier::data::Index; -use rapier::dynamics::{ImpulseJointHandle, MultibodyJointHandle, RigidBodyHandle}; +use rapier::dynamics::{ImpulseJointHandle, MultibodyJointHandle, RigidBodyHandle, SoftBodyHandle}; use rapier::geometry::{Collider, ColliderHandle}; use wasm_bindgen::JsValue; @@ -15,6 +15,11 @@ pub fn body_handle(id: FlatHandle) -> RigidBodyHandle { RigidBodyHandle::from_raw_parts(id.to_bits() as u32, (id.to_bits() >> 32) as u32) } +#[inline(always)] +pub fn soft_body_handle(id: FlatHandle) -> SoftBodyHandle { + SoftBodyHandle::from_raw_parts(id.to_bits() as u32, (id.to_bits() >> 32) as u32) +} + #[inline(always)] pub fn impulse_joint_handle(id: FlatHandle) -> ImpulseJointHandle { ImpulseJointHandle::from_raw_parts(id.to_bits() as u32, (id.to_bits() >> 32) as u32) diff --git a/typescript/testbed2d/src/Graphics.ts b/typescript/testbed2d/src/Graphics.ts index 4f9448f8d..aeb54c173 100644 --- a/typescript/testbed2d/src/Graphics.ts +++ b/typescript/testbed2d/src/Graphics.ts @@ -11,6 +11,7 @@ var kk = 0; export class Graphics { coll2gfx: Map; + sb2gfx: Map; colorIndex: number; colorPalette: Array; renderer: PIXI.Renderer; @@ -24,6 +25,7 @@ export class Graphics { // const pixelRatio = window.devicePixelRatio ? Math.min(window.devicePixelRatio, 1.5) : 1; this.coll2gfx = new Map(); + this.sb2gfx = new Map(); this.colorIndex = 0; this.colorPalette = [0xf3d9b1, 0x98c1d9, 0x053c5e, 0x1f7a8c]; this.renderer = new PIXI.Renderer({ @@ -119,9 +121,49 @@ export class Graphics { } this.updatePositions(world); + this.updateSoftBodies(world); this.renderer.render(this.scene); } + /** + * Draws every soft body as the segments of its boundary, redrawn from its particle + * positions every frame. + */ + updateSoftBodies(world: RAPIER.World) { + let seen = new Set(); + world.forEachSoftBody((body) => { + seen.add(body.handle); + let gfx = this.sb2gfx.get(body.handle); + if (!gfx) { + gfx = new PIXI.Graphics(); + this.viewport.addChild(gfx); + this.sb2gfx.set(body.handle, gfx); + this.colorIndex = + (this.colorIndex + 1) % (this.colorPalette.length - 1); + gfx.tint = this.colorPalette[this.colorIndex + 1]; + } + let positions = body.particlePositions(); + let boundary = body.boundary(); + gfx.clear(); + gfx.lineStyle(0.1, 0xffffff); + for (let i = 0; i < boundary.length; i += 2) { + let a = boundary[i]; + let b = boundary[i + 1]; + gfx.moveTo(positions[a * 2], -positions[a * 2 + 1]); + gfx.lineTo(positions[b * 2], -positions[b * 2 + 1]); + } + }); + + // Soft bodies removed from the world. + this.sb2gfx.forEach((gfx, handle) => { + if (!seen.has(handle)) { + this.viewport.removeChild(gfx); + gfx.destroy(); + this.sb2gfx.delete(handle); + } + }); + } + lookAt(pos: {zoom: number; target: {x: number; y: number}}) { this.viewport.setZoom(pos.zoom); this.viewport.moveCenter(pos.target.x, pos.target.y); @@ -147,6 +189,11 @@ export class Graphics { gfx.destroy(); }); this.coll2gfx = new Map(); + this.sb2gfx.forEach((gfx) => { + this.viewport.removeChild(gfx); + gfx.destroy(); + }); + this.sb2gfx = new Map(); this.colorIndex = 0; } @@ -157,6 +204,11 @@ export class Graphics { ) { let i; let parent = collider.parent(); + // The colliders of a soft body (its deformable surface, or its particles) are drawn + // from the soft body itself. + if (!!parent && parent.isSoftFrame()) { + return; + } let instance; let graphics; let vertices; diff --git a/typescript/testbed2d/src/demos/softBodies.ts b/typescript/testbed2d/src/demos/softBodies.ts new file mode 100644 index 000000000..7b75fa12f --- /dev/null +++ b/typescript/testbed2d/src/demos/softBodies.ts @@ -0,0 +1,111 @@ +import type {Testbed} from "../Testbed"; + +type RAPIER_API = typeof import("@dimforge/rapier2d"); + +export function initWorld(RAPIER: RAPIER_API, testbed: Testbed) { + let gravity = new RAPIER.Vector2(0.0, -9.81); + let world = new RAPIER.World(gravity); + + // Ground and walls. + let walls = [ + {x: 0.0, y: -0.5, hx: 15.0, hy: 0.5}, + {x: -15.0, y: 5.0, hx: 0.5, hy: 5.0}, + {x: 15.0, y: 5.0, hx: 0.5, hy: 5.0}, + ]; + for (let wall of walls) { + let bodyDesc = RAPIER.RigidBodyDesc.fixed().setTranslation( + wall.x, + wall.y, + ); + let body = world.createRigidBody(bodyDesc); + world.createCollider( + RAPIER.ColliderDesc.cuboid(wall.hx, wall.hy), + body, + ); + } + + // Pressurized blobs of various sizes. + for (let i = 0; i < 5; ++i) { + let radius = 0.6 + 0.15 * i; + let blob = RAPIER.SoftBodyDesc.disk( + {x: -10.0 + i * 2.5, y: 2.0 + i}, + radius, + 24, + ) + .setSoftness(20.0, 1.0) + .setVolumeFactor(1.1) + .setSelfContacts(true) + .setParticleMass(0.05); + world.createSoftBody(blob); + } + + // Jelly squares: corotational, Neo-Hookean, and per-cell area constraints. + let jellyMaterial = new RAPIER.SoftBodyMaterial(); + jellyMaterial.youngModulus = 3.0e3; + jellyMaterial.poissonRatio = 0.35; + jellyMaterial.elasticDampingRatio = 0.5; + let models = [ + {x: 2.0, model: RAPIER.SoftBodyCellModel.Corotational}, + {x: 8.0, model: RAPIER.SoftBodyCellModel.NeoHookean}, + ]; + for (let {x, model} of models) { + let jelly = RAPIER.SoftBodyDesc.grid( + {x: x, y: 1.2}, + {x: 1.0, y: 1.0}, + 6, + 6, + ) + .setCellModel(model) + .setMaterial(jellyMaterial) + .setParticleMass(0.2); + world.createSoftBody(jelly); + } + let volumeJelly = RAPIER.SoftBodyDesc.grid( + {x: 5.0, y: 1.2}, + {x: 1.0, y: 1.0}, + 6, + 6, + ) + .setCellModel(RAPIER.SoftBodyCellModel.Volume) + .setSoftness(20.0, 1.0) + .setParticleMass(0.2); + world.createSoftBody(volumeJelly); + + // A rope hanging from a fixed anchor, holding a rigid box. + let ropeDesc = RAPIER.SoftBodyDesc.rope( + {x: 8.0, y: 9.0}, + {x: 12.0, y: 9.0}, + 25, + ) + .setPinnedParticles([0]) + .setSoftness(40.0, 1.0) + .setParticleMass(0.05); + let rope = world.createSoftBody(ropeDesc); + let last = rope.particlePosition(24); + let weightDesc = RAPIER.RigidBodyDesc.dynamic().setTranslation( + last.x, + last.y - 0.4, + ); + let weight = world.createRigidBody(weightDesc); + world.createCollider( + RAPIER.ColliderDesc.cuboid(0.3, 0.3).setDensity(2.0), + weight, + ); + rope.attachParticle(24, weight); + + // A stack of rigid boxes for the blobs to knock down. + for (let i = 0; i < 6; ++i) { + let bodyDesc = RAPIER.RigidBodyDesc.dynamic().setTranslation( + -4.0, + 0.3 + 0.6 * i, + ); + let body = world.createRigidBody(bodyDesc); + world.createCollider(RAPIER.ColliderDesc.cuboid(0.3, 0.3), body); + } + + testbed.setWorld(world); + testbed.lookAt({ + target: {x: 0.0, y: -4.0}, + zoom: 25.0, + }); +} diff --git a/typescript/testbed2d/src/index.ts b/typescript/testbed2d/src/index.ts index 24bd18f43..e291c5f36 100644 --- a/typescript/testbed2d/src/index.ts +++ b/typescript/testbed2d/src/index.ts @@ -10,6 +10,7 @@ import * as ConvexPolygons from "./demos/convexPolygons"; import * as CharacterController from "./demos/characterController"; import * as PidController from "./demos/pidController"; import * as Voxels from "./demos/voxels"; +import * as SoftBodies from "./demos/softBodies"; import("@dimforge/rapier2d").then((RAPIER) => { let builders = new Map([ @@ -23,6 +24,7 @@ import("@dimforge/rapier2d").then((RAPIER) => { ["locked rotations", LockedRotations.initWorld], ["pid controller", PidController.initWorld], ["polyline", Polyline.initWorld], + ["soft bodies", SoftBodies.initWorld], ["voxels", Voxels.initWorld], ]); let testbed = new Testbed(RAPIER, builders); diff --git a/typescript/testbed3d/src/Graphics.ts b/typescript/testbed3d/src/Graphics.ts index f52d6c965..daca7dfc7 100644 --- a/typescript/testbed3d/src/Graphics.ts +++ b/typescript/testbed3d/src/Graphics.ts @@ -118,6 +118,7 @@ export class Graphics { highlightedCollider: null | number; coll2instance: Map; coll2mesh: Map; + sb2mesh: Map; rb2colls: Map>; colorIndex: number; colorPalette: Array; @@ -134,6 +135,7 @@ export class Graphics { this.highlightedCollider = null; this.coll2instance = new Map(); this.coll2mesh = new Map(); + this.sb2mesh = new Map(); this.rb2colls = new Map(); this.colorIndex = 0; this.colorPalette = [0xf3d9b1, 0x98c1d9, 0x053c5e, 0x1f7a8c, 0xff0000]; @@ -277,9 +279,63 @@ export class Graphics { } this.updatePositions(world); + this.updateSoftBodies(world); this.renderer.render(this.scene, this.camera); } + /** + * Draws every soft body as the triangle mesh of its boundary, rebuilt when its topology + * changes (tears, cuts) and refreshed from its particle positions every frame. + */ + updateSoftBodies(world: RAPIER.World) { + let seen = new Set(); + world.forEachSoftBody((body) => { + seen.add(body.handle); + let positions = body.particlePositions(); + let entry = this.sb2mesh.get(body.handle); + let version = body.topologyVersion(); + + if (!entry || entry.topologyVersion != version) { + if (!!entry) { + this.scene.remove(entry.mesh); + } + this.colorIndex = + (this.colorIndex + 1) % (this.colorPalette.length - 2); + let geometry = new THREE.BufferGeometry(); + geometry.setIndex(Array.from(body.boundary())); + geometry.setAttribute( + "position", + new THREE.BufferAttribute(positions, 3), + ); + let material = new THREE.MeshPhongMaterial({ + color: this.colorPalette[this.colorIndex + 1], + side: THREE.DoubleSide, + flatShading: true, + }); + let mesh = new THREE.Mesh(geometry, material); + this.scene.add(mesh); + entry = {mesh, topologyVersion: version}; + this.sb2mesh.set(body.handle, entry); + } else { + let attribute = entry.mesh.geometry.getAttribute( + "position", + ) as THREE.BufferAttribute; + attribute.set(positions); + attribute.needsUpdate = true; + } + entry.mesh.geometry.computeVertexNormals(); + entry.mesh.geometry.computeBoundingSphere(); + }); + + // Soft bodies removed from the world. + this.sb2mesh.forEach((entry, handle) => { + if (!seen.has(handle)) { + this.scene.remove(entry.mesh); + this.sb2mesh.delete(handle); + } + }); + } + rayAtMousePosition(pos: {x: number; y: number}) { this.raycaster.setFromCamera(pos, this.camera); return this.raycaster.ray; @@ -383,8 +439,12 @@ export class Graphics { this.coll2mesh.forEach((mesh) => { this.scene.remove(mesh); }); + this.sb2mesh.forEach((entry) => { + this.scene.remove(entry.mesh); + }); this.coll2instance = new Map(); + this.sb2mesh = new Map(); this.rb2colls = new Map(); this.colorIndex = 0; } @@ -437,9 +497,14 @@ export class Graphics { world: RAPIER.World, collider: RAPIER.Collider, ) { + let parent = collider.parent(); + // The colliders of a soft body (its deformable surface, or its particles) are drawn + // from the soft body itself. + if (!!parent && parent.isSoftFrame()) { + return; + } this.colorIndex = (this.colorIndex + 1) % (this.colorPalette.length - 2); - let parent = collider.parent(); if (!this.rb2colls.get(parent.handle)) { this.rb2colls.set(parent.handle, [collider]); } else { diff --git a/typescript/testbed3d/src/demos/softBodies.ts b/typescript/testbed3d/src/demos/softBodies.ts new file mode 100644 index 000000000..1dcbac353 --- /dev/null +++ b/typescript/testbed3d/src/demos/softBodies.ts @@ -0,0 +1,112 @@ +import type {Testbed} from "../Testbed"; + +type RAPIER_API = typeof import("@dimforge/rapier3d"); + +export function initWorld(RAPIER: RAPIER_API, testbed: Testbed) { + let gravity = new RAPIER.Vector3(0.0, -9.81, 0.0); + let world = new RAPIER.World(gravity); + + // Ground. + let groundDesc = RAPIER.RigidBodyDesc.fixed().setTranslation( + 0.0, + -0.1, + 0.0, + ); + let ground = world.createRigidBody(groundDesc); + world.createCollider(RAPIER.ColliderDesc.cuboid(12.0, 0.1, 12.0), ground); + + // A cloth pinned by its four corners, with a box dropped on it. + let n = 24; + let cloth = RAPIER.SoftBodyDesc.cloth( + {x: -3.5, y: 2.5, z: -1.2}, + {x: 0.1, y: 0.0, z: 0.0}, + {x: 0.0, y: 0.0, z: 0.1}, + n, + n, + ) + .setPinnedParticles([0, n - 1, n * (n - 1), n * n - 1]) + .setSoftness(30.0, 1.0) + .setParticleMass(0.05); + world.createSoftBody(cloth); + let boxDesc = RAPIER.RigidBodyDesc.dynamic().setTranslation( + -2.35, + 4.0, + 0.0, + ); + let box = world.createRigidBody(boxDesc); + world.createCollider( + RAPIER.ColliderDesc.cuboid(0.3, 0.3, 0.3).setDensity(0.5), + box, + ); + + // A balloon: a hollow sphere inflated by volume preservation. + let balloon = RAPIER.SoftBodyDesc.sphere({x: 0.5, y: 3.0, z: 0.0}, 0.8, 2) + .setSoftness(15.0, 1.0) + .setVolumeFactor(1.2) + .setParticleMass(0.05); + world.createSoftBody(balloon); + + // Jelly cubes: corotational, Neo-Hookean, and per-cell volume constraints. + let jellyMaterial = new RAPIER.SoftBodyMaterial(); + jellyMaterial.youngModulus = 2.0e3; + jellyMaterial.poissonRatio = 0.35; + jellyMaterial.elasticDampingRatio = 0.5; + let models = [ + {z: 1.5, model: RAPIER.SoftBodyCellModel.Corotational}, + {z: 4.5, model: RAPIER.SoftBodyCellModel.NeoHookean}, + ]; + for (let {z, model} of models) { + let jelly = RAPIER.SoftBodyDesc.cuboid( + {x: 3.0, y: 1.0, z: z}, + {x: 0.6, y: 0.6, z: 0.6}, + 5, + 5, + 5, + ) + .setCellModel(model) + .setMaterial(jellyMaterial) + .setParticleMass(0.2); + world.createSoftBody(jelly); + } + let volumeJelly = RAPIER.SoftBodyDesc.cuboid( + {x: 3.0, y: 1.0, z: -1.5}, + {x: 0.6, y: 0.6, z: 0.6}, + 5, + 5, + 5, + ) + .setCellModel(RAPIER.SoftBodyCellModel.Volume) + .setSoftness(20.0, 1.0) + .setParticleMass(0.2); + world.createSoftBody(volumeJelly); + + // A rope hanging from a fixed anchor, holding a rigid ball attached by its last + // particle. + let ropeDesc = RAPIER.SoftBodyDesc.rope( + {x: -0.5, y: 5.0, z: 3.0}, + {x: 2.5, y: 5.0, z: 3.0}, + 30, + ) + .setPinnedParticles([0]) + .setSoftness(40.0, 1.0) + .setParticleMass(0.05); + let rope = world.createSoftBody(ropeDesc); + let last = rope.particlePosition(29); + let weightDesc = RAPIER.RigidBodyDesc.dynamic().setTranslation( + last.x, + last.y - 0.3, + last.z, + ); + let weight = world.createRigidBody(weightDesc); + world.createCollider( + RAPIER.ColliderDesc.ball(0.25).setDensity(2.0), + weight, + ); + rope.attachParticle(29, weight); + + testbed.setWorld(world); + testbed.lookAt({ + eye: {x: 9.0, y: 6.0, z: 12.0}, + target: {x: 0.0, y: 1.5, z: 0.0}, + }); +} diff --git a/typescript/testbed3d/src/demos/softTearing.ts b/typescript/testbed3d/src/demos/softTearing.ts new file mode 100644 index 000000000..e9ae29d16 --- /dev/null +++ b/typescript/testbed3d/src/demos/softTearing.ts @@ -0,0 +1,169 @@ +import type {Testbed} from "../Testbed"; + +type RAPIER_API = typeof import("@dimforge/rapier3d"); + +export function initWorld(RAPIER: RAPIER_API, testbed: Testbed) { + let gravity = new RAPIER.Vector3(0.0, -9.81, 0.0); + let world = new RAPIER.World(gravity); + + // Ground. + let groundDesc = RAPIER.RigidBodyDesc.fixed().setTranslation( + 0.0, + -0.1, + 0.0, + ); + let ground = world.createRigidBody(groundDesc); + world.createCollider(RAPIER.ColliderDesc.cuboid(30.0, 0.1, 30.0), ground); + + // Stiff cloth springs (100 Hz) so only impacts pass the tear strain. + let material = RAPIER.SoftBodyMaterial.uniform(100.0, 1.0); + material.bendSoftness = {naturalFrequency: 3.0, dampingRatio: 1.0}; + material.tearStrain = 0.4; + + // A sheet pinned along its border: the heavy ball dropped on it rips through. + let n = 40; + let border = []; + for (let k = 0; k < n * n; ++k) { + let i = Math.floor(k / n); + let j = k % n; + if (i == 0 || j == 0 || i == n - 1 || j == n - 1) { + border.push(k); + } + } + let sheet = RAPIER.SoftBodyDesc.cloth( + {x: -6.0, y: 3.0, z: -2.0}, + {x: 0.1, y: 0.0, z: 0.0}, + {x: 0.0, y: 0.0, z: 0.1}, + n, + n, + ) + .setPinnedParticles(border) + .setMaterial(material) + .setParticleMass(0.02) + .setParticleRadius(0.05) + .setSurfaceCollider(RAPIER.ColliderDesc.ball(0.05).setFriction(0.8)); + world.createSoftBody(sheet); + let ballDesc = RAPIER.RigidBodyDesc.dynamic().setTranslation( + -4.0, + 6.0, + 0.0, + ); + let ball = world.createRigidBody(ballDesc); + world.createCollider(RAPIER.ColliderDesc.ball(0.6).setDensity(30.0), ball); + + // A curtain pinned along its top edge, with a heavy box shot through it. + let curtainMaterial = RAPIER.SoftBodyMaterial.uniform(100.0, 1.0); + curtainMaterial.bendSoftness = {naturalFrequency: 3.0, dampingRatio: 1.0}; + curtainMaterial.tearStrain = 0.2; + let top = []; + for (let i = 0; i < 50; ++i) { + top.push(i * 40); + } + let curtain = RAPIER.SoftBodyDesc.cloth( + {x: 0.0, y: 4.5, z: 4.0}, + {x: 0.1, y: 0.0, z: 0.0}, + {x: 0.0, y: -0.1, z: 0.0}, + 50, + 40, + ) + .setPinnedParticles(top) + .setMaterial(curtainMaterial) + .setParticleMass(0.02); + world.createSoftBody(curtain); + let bulletDesc = RAPIER.RigidBodyDesc.dynamic() + .setTranslation(2.5, 2.5, 12.0) + .setLinvel(0.0, 0.0, -25.0); + let bullet = world.createRigidBody(bulletDesc); + world.createCollider( + RAPIER.ColliderDesc.cuboid(0.3, 0.3, 0.3) + .setRotation({w: 0.8, x: 0.35, y: 0.35, z: 0.35}) + .setDensity(20.0), + bullet, + ); + + // A jelly bar pinned at both ends, torn by pulling its right end away. + let barMaterial = new RAPIER.SoftBodyMaterial(); + barMaterial.youngModulus = 5.0e4; + barMaterial.poissonRatio = 0.3; + barMaterial.elasticDampingRatio = 1.0; + barMaterial.tearStrain = 0.4; + let barDesc = RAPIER.SoftBodyDesc.cuboid( + {x: 3.0, y: 1.0, z: -4.0}, + {x: 2.0, y: 0.4, z: 0.4}, + 21, + 5, + 5, + ) + .setCellModel(RAPIER.SoftBodyCellModel.Corotational) + .setMaterial(barMaterial) + .setParticleMass(0.05) + .setParticleRadius(0.1) + .setSurfaceCollider(RAPIER.ColliderDesc.ball(0.1).setFriction(0.8)); + let bar = world.createSoftBody(barDesc); + type Vector = {x: number; y: number; z: number}; + let ends: Array<{ + body: number; + particle: number; + rest: Vector; + right: boolean; + }> = []; + for (let i = 0; i < bar.numParticles(); ++i) { + let p = bar.particlePosition(i); + if (p.x < 1.01) { + ends.push({ + body: bar.handle, + particle: i, + rest: p as Vector, + right: false, + }); + } else if (p.x > 4.99) { + ends.push({ + body: bar.handle, + particle: i, + rest: p as Vector, + right: true, + }); + } + } + for (let end of ends) { + bar.setParticlePinned(end.particle, true); + } + + // The right end starts moving after a second, at half a meter per second, and stops + // once the bar has doubled its length. Tears move the driven particles to new bodies + // and indices, so the events are followed. + let t = 0.0; + testbed.setpreTimestepAction(() => { + t += world.timestep; + let shift = Math.min(Math.max(t - 1.0, 0.0) * 0.5, 4.0); + for (let end of ends) { + if (end.right) { + let body = world.getSoftBody(end.body); + if (!!body) { + body.setParticleKinematicTarget(end.particle, { + x: end.rest.x + shift, + y: end.rest.y, + z: end.rest.z, + }); + } + } + } + testbed.events.drainSoftBodyTearEvents((event) => { + for (let end of ends) { + if (event.softBody() == end.body) { + let destination = event.particleDestination(end.particle); + if (!!destination) { + end.body = destination.softBody; + end.particle = destination.particle; + } + } + } + }); + }); + + testbed.setWorld(world); + testbed.lookAt({ + eye: {x: 9.0, y: 8.0, z: 16.0}, + target: {x: 0.0, y: 1.5, z: 1.0}, + }); +} diff --git a/typescript/testbed3d/src/index.ts b/typescript/testbed3d/src/index.ts index 8aef2b9e8..466db5e54 100644 --- a/typescript/testbed3d/src/index.ts +++ b/typescript/testbed3d/src/index.ts @@ -18,6 +18,8 @@ import * as glbToTrimesh from "./demos/glbToTrimesh"; import * as glbToConvexHull from "./demos/glbtoConvexHull"; import * as CompoundShapes from "./demos/compoundShapes"; import * as ConvexDecomposition from "./demos/convexDecomposition"; +import * as SoftBodies from "./demos/softBodies"; +import * as SoftTearing from "./demos/softTearing"; import("@dimforge/rapier3d").then((RAPIER) => { let builders = new Map([ @@ -36,6 +38,8 @@ import("@dimforge/rapier3d").then((RAPIER) => { ["pid controller", PidController.initWorld], ["platform", Platform.initWorld], ["pyramid", Pyramid.initWorld], + ["soft bodies", SoftBodies.initWorld], + ["soft tearing", SoftTearing.initWorld], ["triangle mesh", Trimesh.initWorld], ["voxels", Voxels.initWorld], ["GLTF to convexHull", glbToConvexHull.initWorld], From 084e08451c6e38f5847a9dc72192ab4b4477f23c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 18 Sep 2026 00:31:22 +0200 Subject: [PATCH 26/32] feat(python): expose soft bodies in the Python bindings and testbed --- python/docs/api/soft_bodies.rst | 98 + python/docs/changelog.rst | 22 + python/docs/getting_started.rst | 1 + python/docs/index.rst | 1 + python/examples/soft/cloth_drop.py | 54 + python/rapier-py-3d/Cargo.toml | 2 +- .../rapier-py-3d/python/rapier3d/__init__.py | 58 + .../python/rapier3d/_event_handler.py | 14 +- .../python/rapier3d/_rapier3d.pyi | 574 ++++ python/rapier-py-3d/src/controllers.rs | 4 +- python/rapier-py-3d/src/debug_render.rs | 122 +- python/rapier-py-3d/src/dynamics.rs | 60 +- python/rapier-py-3d/src/events_hooks.rs | 92 +- python/rapier-py-3d/src/geometry.rs | 67 +- python/rapier-py-3d/src/joints.rs | 8 +- python/rapier-py-3d/src/lib.rs | 3 + python/rapier-py-3d/src/pipeline.rs | 198 +- python/rapier-py-3d/src/serde_glue.rs | 17 +- python/rapier-py-3d/src/soft_body.rs | 3029 +++++++++++++++++ .../rapier-testbed/rapier_testbed/__init__.py | 2 + .../rapier-testbed/rapier_testbed/_testbed.py | 95 +- .../rapier_testbed/examples3/soft_bodies3.py | 71 + .../rapier_testbed/examples3/soft_tearing3.py | 108 + python/tests/test_examples.py | 6 + python/tests/test_soft_bodies.py | 488 +++ 25 files changed, 5157 insertions(+), 37 deletions(-) create mode 100644 python/docs/api/soft_bodies.rst create mode 100644 python/examples/soft/cloth_drop.py create mode 100644 python/rapier-py-3d/src/soft_body.rs create mode 100644 python/rapier-testbed/rapier_testbed/examples3/soft_bodies3.py create mode 100644 python/rapier-testbed/rapier_testbed/examples3/soft_tearing3.py create mode 100644 python/tests/test_soft_bodies.py diff --git a/python/docs/api/soft_bodies.rst b/python/docs/api/soft_bodies.rst new file mode 100644 index 000000000..f1aaa6f9f --- /dev/null +++ b/python/docs/api/soft_bodies.rst @@ -0,0 +1,98 @@ +Soft bodies +=========== + +Deformable bodies made of particles linked by elastic constraints (edges, bending +constraints and tetrahedral cells), simulated together with the rigid bodies, contacts +and joints of a world. + +.. currentmodule:: rapier3d + +A soft body starts from a :class:`SoftBodyBuilder`, made by one of the generators on +:class:`SoftBody` and inserted with :meth:`PhysicsWorld.add_soft_body`:: + + import rapier3d as rp + + world = rp.PhysicsWorld(gravity=(0, -9.81, 0)) + world.colliders.insert(rp.Collider.cuboid(10, 0.1, 10).build()) + + # A cloth pinned by its four corners. + n = 20 + cloth = world.add_soft_body( + rp.SoftBody.cloth((-1, 2, -1), (0.1, 0, 0), (0, 0, 0.1), n, n) + .pinned_particles([0, n - 1, n * (n - 1), n * n - 1]) + .softness((30.0, 1.0)) + .particle_mass(0.05) + ) + # A jelly cube with corotational elastic cells. + jelly = world.add_soft_body( + rp.SoftBody.cuboid((2, 1, 0), (0.5, 0.5, 0.5), 4, 4, 4, + cell_model=rp.SoftBodyCellModel.COROTATIONAL, + material=rp.SoftBodyMaterial(young_modulus=2e3, poisson_ratio=0.35), + particle_mass=0.2) + ) + + for _ in range(120): + world.step() + + print(world.soft_bodies[cloth].particle_positions.shape) # (400, 3) + +Bodies and sets +--------------- + +.. autoclass:: SoftBody +.. autoclass:: SoftBodyBuilder +.. autoclass:: SoftBodySet +.. autoclass:: SoftBodyHandle + +Material and settings +--------------------- + +.. autoclass:: SoftBodyMaterial +.. autoclass:: SoftBodyCellModel +.. autoclass:: SoftEdgePlasticFlow +.. autoclass:: SoftBodyParticleSettings +.. autoclass:: SoftBodiesSettings +.. autoclass:: SoftRecoverySettings +.. autoclass:: SoftPatchConstraints + +Elements +-------- + +Snapshots of a body's particles and elements, read through :meth:`SoftBody.particle`, +:meth:`SoftBody.edge`, :meth:`SoftBody.cell` and :meth:`SoftBody.dihedral`. + +.. autoclass:: SoftBodyParticle +.. autoclass:: SoftBodyEdge +.. autoclass:: SoftBodyEdgeKind +.. autoclass:: SoftBodyCell +.. autoclass:: SoftBodyDihedral +.. autoclass:: SoftParticleAttachment +.. autoclass:: SoftVolumePiece + +Clusters and deformable colliders +--------------------------------- + +A cluster (:meth:`PhysicsWorld.add_soft_body_cluster`) gives a set of particles a rigid proxy +that joints and colliders attach to. A deformable collider +(:meth:`PhysicsWorld.insert_deformable`) is a triangle mesh built with +``TriMeshFlags.DEFORMABLE`` whose vertices follow a cluster through a :class:`SoftMeshBinding`. + +.. autoclass:: SoftBodyCluster +.. autoclass:: SoftMeshBinding +.. autoclass:: SoftCollisionMesh +.. autoclass:: SoftMeshId +.. autoclass:: SoftMeshRef + +Tearing and cutting +------------------- + +Elements tear on their own past the material's ``tear_strain`` / ``tear_force``, on request +(:meth:`SoftBody.tear_edge`, :meth:`SoftBody.tear_cell`), or at once with +:meth:`PhysicsWorld.tear_soft_body` and :meth:`PhysicsWorld.cut_soft_body`. Pieces a tear +disconnects become soft bodies of their own; every tear is reported as a +:class:`SoftBodyTearEvent` (through the event handler, or returned by the immediate calls). + +.. autoclass:: SoftBodyTearEvent +.. autoclass:: SoftBodyPiece +.. autoclass:: SoftClusterSplit +.. autoclass:: SoftJointMove diff --git a/python/docs/changelog.rst b/python/docs/changelog.rst index ef93c9789..28afd251a 100644 --- a/python/docs/changelog.rst +++ b/python/docs/changelog.rst @@ -10,6 +10,28 @@ release cadence. The authoritative changelog is the Cargo Unreleased ---------- +**Soft bodies.** ``PhysicsWorld.add_soft_body`` inserts a deformable body +(``SoftBody.rope`` / ``cloth`` / ``cuboid`` / ``sphere`` / ``trimesh`` / +``volumetric`` generators, ``SoftBodyBuilder`` setters, ``SoftBodyMaterial``), +simulated together with the rigid bodies, contacts and joints; ``world.soft_bodies`` +is the ``SoftBodySet``. Particles can be pinned, attached to rigid bodies, driven +kinematically or pushed by forces and impulses; clusters +(``add_soft_body_cluster``) give a set of particles a rigid proxy that joints and +colliders attach to; deformable colliders (``insert_deformable``) bind a triangle +mesh to a cluster. Tearing and cutting (``tear_soft_body``, ``cut_soft_body``, +material thresholds) split pieces off into soft bodies of their own and report a +``SoftBodyTearEvent`` (``EventHandler.handle_soft_body_tear_event``, +``ChannelEventCollector.drain_soft_body_tear_events``). See :doc:`api/soft_bodies`. + +Related changes: ``RigidBodyType.SOFT_FRAME`` marks the proxies of soft-body +clusters (``RigidBody.is_soft_frame`` / ``soft_body`` / ``soft_cluster``); +``RigidBody.additional_pgs_iterations``; ``IntegrationParameters.soft_bodies`` +(``SoftBodiesSettings``); ``RigidBodySet.remove``, ``ColliderSet.remove``, +``PhysicsPipeline.step`` and ``DebugRenderPipeline.render`` / ``render_to_arrays`` +take an optional ``soft_bodies`` argument; ``DebugRenderMode.SOFT_BODIES`` and +related modes; snapshots include the soft bodies; ``SolverFlags.COMPUTE_RIGID_IMPULSES`` +is the engine's new name for ``COMPUTE_IMPULSES``. + **Breaking — repackaged as ``rapier3d``.** The single ``rapier`` umbrella package (with ``rapier.dim3`` submodules) has been replaced by the ``rapier3d`` package (3D / f32): diff --git a/python/docs/getting_started.rst b/python/docs/getting_started.rst index 7fde879a0..205bbd36d 100644 --- a/python/docs/getting_started.rst +++ b/python/docs/getting_started.rst @@ -89,6 +89,7 @@ What to read next ----------------- * :doc:`api/dynamics` — rigid bodies, mass, integration. +* :doc:`api/soft_bodies` — cloth, ropes, jellies; tearing, cutting, clusters. * :doc:`api/pipeline` — stepping a world manually & query pipelines. * :doc:`api/joints` — impulse + multibody joints, motors, limits. * :doc:`api/controllers` — kinematic character, PID/PD, ray-cast diff --git a/python/docs/index.rst b/python/docs/index.rst index c52a1ea09..bf4292422 100644 --- a/python/docs/index.rst +++ b/python/docs/index.rst @@ -65,6 +65,7 @@ Contents api/world api/math api/dynamics + api/soft_bodies api/geometry api/joints api/pipeline diff --git a/python/examples/soft/cloth_drop.py b/python/examples/soft/cloth_drop.py new file mode 100644 index 000000000..f39170144 --- /dev/null +++ b/python/examples/soft/cloth_drop.py @@ -0,0 +1,54 @@ +"""A cloth pinned by two corners drops over a ball; a jelly cube lands beside it. + +Shows the soft-body API: generators, pinned particles, materials, particle +positions read back as NumPy arrays, and a tear event. +""" + +from __future__ import annotations + +import rapier3d as rp + +world = rp.PhysicsWorld(gravity=(0, -9.81, 0)) +world.colliders.insert(rp.Collider.cuboid(20, 0.1, 20).build()) +world.colliders.insert(rp.Collider.ball(0.8).translation((0, 0.8, 0)).build()) + +# A cloth pinned by two corners, with a stiff material that tears past 40% strain. +n = 16 +cloth = world.add_soft_body( + rp.SoftBody.cloth((-1.5, 3.0, -1.5), (0.2, 0, 0), (0, 0, 0.2), n, n) + .pinned_particles([0, n * (n - 1)]) + .softness((30.0, 1.0)) + .particle_mass(0.05) +) + +# A jelly cube with corotational elastic cells. +jelly = world.add_soft_body( + rp.SoftBody.cuboid( + (3.0, 1.0, 0.0), + (0.5, 0.5, 0.5), + 4, + 4, + 4, + cell_model=rp.SoftBodyCellModel.COROTATIONAL, + material=rp.SoftBodyMaterial(young_modulus=2.0e3, poisson_ratio=0.35), + particle_mass=0.2, + ) +) + +collector = rp.ChannelEventCollector() +world.event_handler = collector +for _ in range(120): + world.step() + +# Tear one edge by hand: the event reports it at the end of the step. +world.soft_bodies[cloth].tear_edge(20) +world.step() +tears = collector.drain_soft_body_tear_events() + +positions = world.soft_bodies[cloth].particle_positions +lowest = float(positions[:, 1].min()) +jelly_y = world.soft_bodies[jelly].center_of_mass.y +print( + f"soft: cloth particles={positions.shape[0]} lowest_y={lowest:+.2f} " + f"jelly_y={jelly_y:+.2f} tears={len(tears)}" +) diff --git a/python/rapier-py-3d/Cargo.toml b/python/rapier-py-3d/Cargo.toml index 8f654105c..3ab418995 100644 --- a/python/rapier-py-3d/Cargo.toml +++ b/python/rapier-py-3d/Cargo.toml @@ -52,4 +52,4 @@ urdf-rs.workspace = true # Phase 11 — Serialization, snapshots. serde = { workspace = true, features = ["derive", "std"] } bincode = { workspace = true } -serde_json = { workspace = true } +serde_json = { workspace = true, features = ["arbitrary_precision"] } diff --git a/python/rapier-py-3d/python/rapier3d/__init__.py b/python/rapier-py-3d/python/rapier3d/__init__.py index c0df5c38f..9fce97c84 100644 --- a/python/rapier-py-3d/python/rapier3d/__init__.py +++ b/python/rapier-py-3d/python/rapier3d/__init__.py @@ -49,6 +49,35 @@ CCDSolver, ImpulseJointSet, MultibodyJointSet, + # ---- soft bodies ---- + SoftBindingError, + SoftBodyHandle, + SoftBodyCellModel, + SoftEdgePlasticFlow, + SoftBodyEdgeKind, + SoftPatchConstraints, + SoftBodyMaterial, + SoftBodyParticleSettings, + SoftRecoverySettings, + SoftBodiesSettings, + SoftBodyBuilder, + SoftBodyParticle, + SoftBodyEdge, + SoftBodyDihedral, + SoftBodyCell, + SoftParticleAttachment, + SoftVolumePiece, + SoftBodyCluster, + SoftMeshId, + SoftMeshRef, + SoftCollisionMesh, + SoftMeshBinding, + SoftBodyPiece, + SoftClusterSplit, + SoftJointMove, + SoftBodyTearEvent, + SoftBody, + SoftBodySet, # ---- joints ---- JointEnabled, MotorModel, @@ -225,6 +254,35 @@ "CCDSolver", "ImpulseJointSet", "MultibodyJointSet", + # ---- soft bodies ---- + "SoftBindingError", + "SoftBodyHandle", + "SoftBodyCellModel", + "SoftEdgePlasticFlow", + "SoftBodyEdgeKind", + "SoftPatchConstraints", + "SoftBodyMaterial", + "SoftBodyParticleSettings", + "SoftRecoverySettings", + "SoftBodiesSettings", + "SoftBodyBuilder", + "SoftBodyParticle", + "SoftBodyEdge", + "SoftBodyDihedral", + "SoftBodyCell", + "SoftParticleAttachment", + "SoftVolumePiece", + "SoftBodyCluster", + "SoftMeshId", + "SoftMeshRef", + "SoftCollisionMesh", + "SoftMeshBinding", + "SoftBodyPiece", + "SoftClusterSplit", + "SoftJointMove", + "SoftBodyTearEvent", + "SoftBody", + "SoftBodySet", # ---- joints ---- "JointEnabled", "MotorModel", diff --git a/python/rapier-py-3d/python/rapier3d/_event_handler.py b/python/rapier-py-3d/python/rapier3d/_event_handler.py index 4aaedae32..3e68b358d 100644 --- a/python/rapier-py-3d/python/rapier3d/_event_handler.py +++ b/python/rapier-py-3d/python/rapier3d/_event_handler.py @@ -19,7 +19,7 @@ @runtime_checkable class EventHandler(Protocol): - """A receiver of collision / contact-force events. + """A receiver of collision, contact-force and soft-body tear events. Assign an instance of any class implementing this protocol to ``world.event_handler``. The methods are called from the physics solver @@ -67,6 +67,18 @@ def handle_contact_force_event( """ ... + def handle_soft_body_tear_event(self, soft_bodies: Any, event: Any) -> None: + """Called at the end of a step for every soft body that tore during it, + once the topology change is applied. + + ``event`` is a ``SoftBodyTearEvent``: the torn elements, the split + particles and the pieces that became soft bodies of their own. + ``soft_bodies`` is passed as ``None`` (see the note above). Immediate + ``SoftBodySet.tear`` / ``cut`` calls return their event instead. The + method is optional: a handler without it receives no tear events. + """ + ... + @runtime_checkable class PhysicsHooks(Protocol): diff --git a/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi b/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi index eeab78eff..4d28e779c 100644 --- a/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi +++ b/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi @@ -20,6 +20,7 @@ class MjcfError(RapierError): ... class SerializationError(RapierError): ... class QueryFailure(RapierError): ... class MeshLoaderError(RapierError): ... +class SoftBindingError(RapierError): ... # Anything that can be coerced to a 3-vector. VectorLike = Union[ @@ -213,6 +214,7 @@ class RigidBodyType(enum.IntEnum): FIXED: int KINEMATIC_VELOCITY_BASED: int KINEMATIC_POSITION_BASED: int + SOFT_FRAME: int class CoefficientCombineRule(enum.IntEnum): AVERAGE: int @@ -823,6 +825,7 @@ class IntegrationParameters: contact_softness: SpringCoefficients static_contact_softness: SpringCoefficients friction_model: FrictionModel + soft_bodies: SoftBodiesSettings def __init__(self) -> None: ... @staticmethod def default_params() -> IntegrationParameters: ... @@ -890,6 +893,10 @@ class RigidBody: angular_damping: float dominance_group: int additional_solver_iterations: int + additional_pgs_iterations: int + is_soft_frame: bool + soft_body: SoftBodyHandle | None + soft_cluster: int | None locked_axes: LockedAxes enabled_translations: tuple[bool, bool, bool] enabled_rotations: tuple[bool, bool, bool] @@ -968,6 +975,7 @@ class RigidBodySet: impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, remove_attached_colliders: bool = True, + soft_bodies: SoftBodySet | None = None, ) -> RigidBody | None: ... def get(self, handle: RigidBodyHandle) -> RigidBody | None: ... def clear(self) -> None: ... @@ -1484,6 +1492,8 @@ class Collider: def new(shape: SharedShape) -> ColliderBuilder: ... def compute_aabb(self) -> Aabb: ... + deformable_mesh_ref: SoftMeshRef | None + class ColliderSet: def __init__(self) -> None: ... def insert(self, builder: Collider | ColliderBuilder) -> ColliderHandle: ... @@ -1499,7 +1509,16 @@ class ColliderSet: islands: IslandManager, bodies: RigidBodySet, wake_parent: bool = True, + soft_bodies: SoftBodySet | None = None, ) -> Collider | None: ... + def insert_deformable( + self, + builder: Collider | ColliderBuilder, + binding: SoftMeshBinding, + parent: RigidBodyHandle, + bodies: RigidBodySet, + soft_bodies: SoftBodySet, + ) -> ColliderHandle: ... def get(self, handle: ColliderHandle) -> Collider | None: ... def clear(self) -> None: ... def is_empty(self) -> bool: ... @@ -1625,6 +1644,7 @@ class NarrowPhase: class SolverFlags: COMPUTE_IMPULSES: SolverFlags + COMPUTE_RIGID_IMPULSES: SolverFlags EMPTY: SolverFlags def __init__(self, bits: int = 0) -> None: ... @staticmethod @@ -1676,6 +1696,7 @@ class ChannelEventCollector: def __init__(self) -> None: ... def drain_collision_events(self) -> list[CollisionEvent]: ... def drain_contact_force_events(self) -> list[ContactForceEvent]: ... + def drain_soft_body_tear_events(self) -> list[SoftBodyTearEvent]: ... def clear(self) -> None: ... def __len__(self) -> int: ... @@ -1940,6 +1961,7 @@ class DebugRenderObject: IMPULSE_JOINT: DebugRenderObject MULTIBODY_JOINT: DebugRenderObject CONTACT_PAIR: DebugRenderObject + SOFT_BODY: DebugRenderObject class DebugColor: def __init__(self, h: float = 0.0, s: float = 0.0, l: float = 0.0, a: float = 1.0) -> None: ... @@ -1986,6 +2008,10 @@ class DebugRenderMode: def __xor__(self, other: DebugRenderMode) -> DebugRenderMode: ... def __invert__(self) -> DebugRenderMode: ... def __bool__(self) -> bool: ... + SOFT_BODIES: DebugRenderMode + PSEUDO_NORMALS: DebugRenderMode + SOFT_VOLUME_CONTACTS: DebugRenderMode + SOFT_BODY_STRESS: DebugRenderMode class DebugRenderStyle: def __init__(self) -> None: ... @@ -2009,6 +2035,15 @@ class DebugRenderStyle: contact_normal_color: DebugColor contact_normal_length: float collider_aabb_color: DebugColor + soft_body_element_color: DebugColor + soft_body_slack_color: DebugColor + soft_body_loaded_color: DebugColor + soft_body_frame_color: DebugColor + vertex_pseudo_normal_color: DebugColor + edge_pseudo_normal_color: DebugColor + volume_contact_normal_color: DebugColor + volume_gradient_color: DebugColor + pseudo_normal_length: float class DebugLineCollector: def __init__(self) -> None: ... @@ -2035,6 +2070,7 @@ class DebugRenderPipeline: multibody_joints: MultibodyJointSet, narrow_phase: NarrowPhase, backend: Any, + soft_bodies: SoftBodySet | None = None, ) -> None: ... def render_to_arrays( self, @@ -2043,4 +2079,542 @@ class DebugRenderPipeline: impulse_joints: ImpulseJointSet, multibody_joints: MultibodyJointSet, narrow_phase: NarrowPhase, + soft_bodies: SoftBodySet | None = None, ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: ... + +# =========================================================================== +# Soft bodies +# =========================================================================== + +Softness = Union["SpringCoefficients", Tuple[float, float]] +IndicesLike = Union["np.ndarray", list, tuple] + +class SoftBodyHandle: + @staticmethod + def from_raw_parts(index: int, generation: int) -> SoftBodyHandle: ... + @staticmethod + def invalid() -> SoftBodyHandle: ... + def into_raw_parts(self) -> Tuple[int, int]: ... + @property + def index(self) -> int: ... + @property + def generation(self) -> int: ... + def __hash__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class SoftBodyCellModel(enum.IntEnum): + VOLUME: int + COROTATIONAL: int + NEO_HOOKEAN: int + +class SoftEdgePlasticFlow(enum.IntEnum): + BOTH: int + COMPRESSION: int + TENSION: int + +class SoftBodyEdgeKind(enum.IntEnum): + STRUCTURAL: int + BEND: int + +class SoftPatchConstraints(enum.IntEnum): + KEEP: int + STAND_DOWN: int + ALONG_NORMAL: int + +class SoftBodyMaterial: + edge_softness: SpringCoefficients + bend_softness: SpringCoefficients + volume_softness: SpringCoefficients + shape_matching_softness: SpringCoefficients + young_modulus: float + poisson_ratio: float + elastic_damping_ratio: float + plastic_yield: float + plastic_creep: float + plastic_max: float + deformation_damping: float + edge_plastic_yield: float + edge_plastic_creep: float + edge_plastic_max: float + edge_plastic_flow: SoftEdgePlasticFlow + tear_strain: float | None + tear_force: float | None + tear_smoothing: float + interior_strength: float + max_tears_per_step: int + min_piece: int | None + def __init__(self, **kwargs: Any) -> None: ... + @staticmethod + def uniform(softness: Softness) -> SoftBodyMaterial: ... + def tears(self) -> bool: ... + def lame_parameters(self) -> Tuple[float, float]: ... + +class SoftBodyParticleSettings: + linear_damping: float + gravity_scale: float + additional_solver_iterations: int + additional_pgs_iterations: int + can_sleep: bool + dominance_group: int + def __init__(self) -> None: ... + +class SoftRecoverySettings: + authored_velocity_margin: bool + edge_speculation: bool + inverted_cell_detection: bool + self_crossing_detection: bool + detection_motion_gating: bool + cross_body_detection: bool + self_stand_down: bool + cross_body_expel_gate: bool + edge_stand_down: bool + crossing_repulsion: bool + crossing_repulsion_guide: bool + crossing_repulsion_self_guide: bool + recovery_pace: float + overlap_constraints: bool + overlap_rigid: bool + overlap_skip_self_tangled: bool + overlap_edge_stand_down: bool + overlap_constraint_pace: float + overlap_patch_constraints: SoftPatchConstraints + overlap_skin_volume: bool + overlap_kept_depth: float + overlap_self_regions: bool + overlap_normal_push: bool + overlap_multi_volume: bool + overlap_split: int + overlap_patience: int + overlap_progress_margin: float + def __init__(self) -> None: ... + +class SoftBodiesSettings: + recovery: SoftRecoverySettings + resweep_strain: float + max_extra_substeps: int + contact_stiffening: float + def __init__(self) -> None: ... + +class SoftBodyBuilder: + def __init__(self, positions: Any, **kwargs: Any) -> None: ... + @property + def num_particles(self) -> int: ... + @property + def positions(self) -> np.ndarray: ... + @property + def current_edges(self) -> np.ndarray: ... + @property + def current_cells(self) -> np.ndarray: ... + @property + def current_surface(self) -> np.ndarray: ... + @property + def current_material(self) -> SoftBodyMaterial: ... + def surface_edges(self) -> np.ndarray: ... + def cell_edges(self) -> np.ndarray: ... + def surface_dihedrals(self) -> np.ndarray: ... + def set_positions(self, positions: Any) -> SoftBodyBuilder: ... + def particle_mass(self, mass: float) -> SoftBodyBuilder: ... + def mass(self, mass: float) -> SoftBodyBuilder: ... + def masses(self, masses: list[float]) -> SoftBodyBuilder: ... + def pinned_particles(self, pinned: IndicesLike) -> SoftBodyBuilder: ... + def edges(self, edges: IndicesLike) -> SoftBodyBuilder: ... + def add_edges(self, edges: IndicesLike) -> SoftBodyBuilder: ... + def bend_edges(self, edges: IndicesLike) -> SoftBodyBuilder: ... + def tension_only(self) -> SoftBodyBuilder: ... + def dihedrals(self, dihedrals: IndicesLike) -> SoftBodyBuilder: ... + def cells(self, cells: IndicesLike) -> SoftBodyBuilder: ... + def surface(self, surface: IndicesLike) -> SoftBodyBuilder: ... + def skin(self, vertices: Any, indices: IndicesLike) -> SoftBodyBuilder: ... + def skin_collision(self, enabled: bool) -> SoftBodyBuilder: ... + def wire(self, segments: IndicesLike) -> SoftBodyBuilder: ... + def material(self, material: SoftBodyMaterial) -> SoftBodyBuilder: ... + def softness(self, softness: Softness) -> SoftBodyBuilder: ... + def tear_strain(self, strain: float) -> SoftBodyBuilder: ... + def tear_force(self, force: float) -> SoftBodyBuilder: ... + def min_piece(self, elements: int) -> SoftBodyBuilder: ... + def tear_smoothing(self, seconds: float) -> SoftBodyBuilder: ... + def interior_strength(self, strength: float) -> SoftBodyBuilder: ... + def edge_tear_resistance(self, resistances: list[Tuple[int, float]]) -> SoftBodyBuilder: ... + def cell_model(self, model: SoftBodyCellModel) -> SoftBodyBuilder: ... + def volume_preservation(self, enabled: bool) -> SoftBodyBuilder: ... + def volume_factor(self, factor: float) -> SoftBodyBuilder: ... + def shape_matching(self, enabled: bool) -> SoftBodyBuilder: ... + def self_contacts(self, enabled: bool) -> SoftBodyBuilder: ... + def particle_radius(self, radius: float) -> SoftBodyBuilder: ... + def surface_collider(self, collider: ColliderBuilder) -> SoftBodyBuilder: ... + def no_surface_collider(self) -> SoftBodyBuilder: ... + def translated(self, translation: VectorLike) -> SoftBodyBuilder: ... + def particle_settings(self, settings: SoftBodyParticleSettings) -> SoftBodyBuilder: ... + def linear_damping(self, damping: float) -> SoftBodyBuilder: ... + def gravity_scale(self, scale: float) -> SoftBodyBuilder: ... + def additional_solver_iterations(self, iterations: int) -> SoftBodyBuilder: ... + def additional_pgs_iterations(self, iterations: int) -> SoftBodyBuilder: ... + def can_sleep(self, can_sleep: bool) -> SoftBodyBuilder: ... + def user_data(self, data: int) -> SoftBodyBuilder: ... + def append(self, other: SoftBodyBuilder) -> SoftBodyBuilder: ... + +class SoftBodyParticle: + @property + def position(self) -> Vec3: ... + @property + def velocity(self) -> Vec3: ... + @property + def force(self) -> Vec3: ... + @property + def kinematic_target(self) -> Vec3 | None: ... + @property + def rest_position(self) -> Vec3: ... + @property + def initial_rest_position(self) -> Vec3: ... + @property + def mass(self) -> float: ... + @property + def inv_mass(self) -> float: ... + @property + def is_pinned(self) -> bool: ... + @property + def is_damaged(self) -> bool: ... + @property + def is_on_surface(self) -> bool: ... + +class SoftBodyEdge: + @property + def vertices(self) -> Tuple[int, int]: ... + @property + def rest_length(self) -> float: ... + @property + def kind(self) -> SoftBodyEdgeKind: ... + @property + def tension_only(self) -> bool: ... + @property + def softness(self) -> SpringCoefficients | None: ... + @property + def tear_resistance(self) -> float: ... + @property + def impulse(self) -> float: ... + @property + def stress(self) -> float: ... + @property + def plastic_strain(self) -> float: ... + @property + def initial_rest_length(self) -> float: ... + +class SoftBodyDihedral: + @property + def vertices(self) -> Tuple[int, int, int, int]: ... + @property + def rest_angle(self) -> float: ... + @property + def plastic_set(self) -> float: ... + @property + def initial_rest_angle(self) -> float: ... + +class SoftBodyCell: + @property + def vertices(self) -> Tuple[int, int, int, int]: ... + @property + def rest_volume(self) -> float: ... + @property + def stiffness_scale(self) -> float: ... + @property + def tear_resistance(self) -> float: ... + @property + def stress(self) -> float: ... + def plastic_stretch(self) -> np.ndarray: ... + +class SoftParticleAttachment: + @property + def particle(self) -> int: ... + @property + def body(self) -> RigidBodyHandle: ... + @property + def local_anchor(self) -> Vec3: ... + @property + def impulse(self) -> Vec3: ... + +class SoftVolumePiece: + elements: list[int] + particles: list[int] + rest_volume: float + volume: float + +class SoftBodyCluster: + index: int + particles: list[int] + proxy: RigidBodyHandle + is_live: bool + shape_matching_enabled: bool + meshes: list[SoftMeshId] + +class SoftMeshId: + def __init__(self, cluster: int, mesh: int) -> None: ... + @property + def cluster(self) -> int: ... + @property + def mesh(self) -> int: ... + +class SoftMeshRef: + @property + def body(self) -> SoftBodyHandle: ... + @property + def id(self) -> SoftMeshId: ... + +class SoftCollisionMesh: + id: SoftMeshId + collider: ColliderHandle + collision_enabled: bool + is_skinned: bool + is_closed: bool + is_wire: bool + self_contacts_enabled: bool + @property + def vertex_count(self) -> int: ... + @property + def vertices(self) -> np.ndarray: ... + @property + def indices(self) -> np.ndarray: ... + +class SoftMeshBinding: + @staticmethod + def direct(particles: IndicesLike) -> SoftMeshBinding: ... + @staticmethod + def direct_by_position(eps: float) -> SoftMeshBinding: ... + @staticmethod + def skinned() -> SoftMeshBinding: ... + def self_contacts(self, enabled: bool) -> SoftMeshBinding: ... + +class SoftBodyPiece: + @property + def soft_body(self) -> SoftBodyHandle: ... + @property + def particles(self) -> list[int]: ... + @property + def clusters(self) -> list[Tuple[int, int]]: ... + +class SoftClusterSplit: + @property + def source_cluster(self) -> int: ... + @property + def soft_body(self) -> SoftBodyHandle: ... + @property + def cluster(self) -> int: ... + @property + def proxy(self) -> RigidBodyHandle: ... + @property + def keeps_proxy(self) -> bool: ... + +class SoftJointMove: + @property + def joint(self) -> ImpulseJointHandle: ... + @property + def from_body(self) -> RigidBodyHandle: ... + @property + def to_body(self) -> RigidBodyHandle: ... + +class SoftBodyTearEvent: + @property + def soft_body(self) -> SoftBodyHandle: ... + @property + def torn_edges(self) -> np.ndarray: ... + @property + def torn_cells(self) -> np.ndarray: ... + @property + def removed_edges(self) -> np.ndarray: ... + @property + def split_particles(self) -> list[Tuple[int, int]]: ... + @property + def inserted_particles(self) -> list[int]: ... + @property + def pieces(self) -> list[SoftBodyPiece]: ... + @property + def clusters(self) -> list[SoftClusterSplit]: ... + @property + def moved_joints(self) -> list[SoftJointMove]: ... + def bodies(self) -> list[SoftBodyHandle]: ... + def particle_destination(self, particle: int) -> Tuple[SoftBodyHandle, int] | None: ... + def seeds(self) -> np.ndarray: ... + +class SoftBody: + @staticmethod + def rope(start: VectorLike, end: VectorLike, num_particles: int, **kwargs: Any) -> SoftBodyBuilder: ... + @staticmethod + def cloth(origin: VectorLike, du: VectorLike, dv: VectorLike, nu: int, nv: int, **kwargs: Any) -> SoftBodyBuilder: ... + @staticmethod + def cloth_tube(origin: VectorLike, axis: VectorLike, radius_start: float, radius_end: float, num_around: int, num_along: int, **kwargs: Any) -> SoftBodyBuilder: ... + @staticmethod + def cloth_anisotropic(origin: VectorLike, du: VectorLike, dv: VectorLike, nu: int, nv: int, warp: Softness, weft: Softness, shear: Softness, **kwargs: Any) -> SoftBodyBuilder: ... + @staticmethod + def cuboid(center: VectorLike, half_extents: VectorLike, nx: int, ny: int, nz: int, **kwargs: Any) -> SoftBodyBuilder: ... + @staticmethod + def sphere(center: VectorLike, radius: float, subdivisions: int, **kwargs: Any) -> SoftBodyBuilder: ... + @staticmethod + def trimesh(vertices: Any, indices: IndicesLike, **kwargs: Any) -> SoftBodyBuilder: ... + @staticmethod + def volumetric(vertices: Any, indices: IndicesLike, cell_size: float, skinned: bool = False, **kwargs: Any) -> SoftBodyBuilder: ... + # particles + @property + def topology_version(self) -> int: ... + @property + def num_particles(self) -> int: ... + def particle(self, i: int) -> SoftBodyParticle: ... + def particles(self) -> list[SoftBodyParticle]: ... + def particle_position(self, i: int) -> Vec3: ... + @property + def particle_positions(self) -> np.ndarray: ... + def particle_velocity(self, i: int) -> Vec3: ... + @property + def particle_velocities(self) -> np.ndarray: ... + def set_particle_position(self, i: int, position: VectorLike) -> None: ... + def set_particle_velocity(self, i: int, velocity: VectorLike) -> None: ... + def set_particle_kinematic_target(self, i: int, position: VectorLike) -> None: ... + def set_particle_pinned(self, i: int, pinned: bool) -> None: ... + def attach_particle(self, i: int, body: RigidBodyHandle, bodies: RigidBodySet) -> None: ... + def detach_particle(self, i: int) -> bool: ... + @property + def particle_attachments(self) -> list[SoftParticleAttachment]: ... + # elements + @property + def num_edges(self) -> int: ... + def edge(self, i: int) -> SoftBodyEdge: ... + @property + def edges(self) -> np.ndarray: ... + @property + def num_dihedrals(self) -> int: ... + def dihedral(self, i: int) -> SoftBodyDihedral: ... + @property + def dihedrals(self) -> np.ndarray: ... + @property + def num_cells(self) -> int: ... + def cell(self, i: int) -> SoftBodyCell: ... + @property + def cells(self) -> np.ndarray: ... + @property + def boundary(self) -> np.ndarray: ... + # material + material: SoftBodyMaterial + @property + def cell_model(self) -> SoftBodyCellModel: ... + @property + def volume_preservation_enabled(self) -> bool: ... + def enable_volume_preservation(self, enabled: bool) -> None: ... + @property + def volume_pieces(self) -> list[SoftVolumePiece]: ... + @property + def rest_volume(self) -> float: ... + @property + def volume(self) -> float: ... + volume_factor: float + @property + def particle_radius(self) -> float: ... + @property + def particle_settings(self) -> SoftBodyParticleSettings: ... + def reset_plasticity(self) -> None: ... + # whole body + @property + def root_body(self) -> RigidBodyHandle: ... + @property + def origin(self) -> SoftBodyHandle | None: ... + @property + def pieces(self) -> list[SoftBodyHandle]: ... + @property + def center_of_mass(self) -> Vec3: ... + @property + def mass(self) -> float: ... + @property + def is_sleeping(self) -> bool: ... + def wake_up(self) -> None: ... + @property + def is_enabled(self) -> bool: ... + def set_enabled(self, enabled: bool) -> None: ... + def set_additional_pgs_iterations(self, iterations: int) -> None: ... + @property + def num_solver_colors(self) -> int: ... + user_data: int + # forces + def add_force(self, force: VectorLike, wake_up: bool = True) -> None: ... + def add_particle_force(self, i: int, force: VectorLike, wake_up: bool = True) -> None: ... + def reset_forces(self, wake_up: bool = True) -> None: ... + def apply_impulse(self, impulse: VectorLike, wake_up: bool = True) -> None: ... + def apply_particle_impulse(self, i: int, impulse: VectorLike, wake_up: bool = True) -> None: ... + def apply_impulse_at_point(self, impulse: VectorLike, point: VectorLike, falloff_radius: float, wake_up: bool = True) -> None: ... + def apply_radial_impulse(self, center: VectorLike, magnitude: float, falloff_radius: float, wake_up: bool = True) -> None: ... + # tearing + def tear_edge(self, i: int) -> None: ... + def tear_cell(self, i: int) -> None: ... + @property + def has_pending_tears(self) -> bool: ... + def crossing_elements(self, blade: Tuple[VectorLike, VectorLike, VectorLike]) -> Tuple[list[int], list[int]]: ... + # clusters + @property + def num_clusters(self) -> int: ... + @property + def num_live_clusters(self) -> int: ... + def cluster(self, i: int) -> SoftBodyCluster | None: ... + @property + def clusters(self) -> list[SoftBodyCluster]: ... + def cluster_proxy(self, i: int) -> RigidBodyHandle | None: ... + def enable_cluster_shape_matching(self, i: int, enabled: bool) -> None: ... + def set_cluster_stiffness_scale(self, i: int, scale: float) -> None: ... + def set_cluster_edge_softness(self, i: int, softness: Softness | None) -> None: ... + def set_cluster_tear_resistance(self, i: int, resistance: float) -> None: ... + def set_cluster_pinned(self, i: int, pinned: bool) -> None: ... + def set_cluster_kinematic_target(self, i: int, pose: IsometryLike) -> None: ... + # meshes + @property + def meshes(self) -> list[SoftCollisionMesh]: ... + def mesh(self, id: SoftMeshId) -> SoftCollisionMesh | None: ... + def mesh_of(self, collider: ColliderHandle) -> SoftCollisionMesh | None: ... + def collision_mesh(self) -> SoftCollisionMesh | None: ... + +class SoftBodySet: + def __init__(self) -> None: ... + def insert(self, builder: SoftBodyBuilder, bodies: RigidBodySet, colliders: ColliderSet) -> SoftBodyHandle: ... + def remove( + self, + handle: SoftBodyHandle, + islands: IslandManager, + bodies: RigidBodySet, + colliders: ColliderSet, + impulse_joints: ImpulseJointSet, + multibody_joints: MultibodyJointSet, + ) -> SoftBody | None: ... + def add_cluster(self, handle: SoftBodyHandle, particles: IndicesLike, bodies: RigidBodySet, colliders: ColliderSet) -> int | None: ... + def remove_cluster( + self, + handle: SoftBodyHandle, + cluster: int, + islands: IslandManager, + bodies: RigidBodySet, + colliders: ColliderSet, + impulse_joints: ImpulseJointSet, + multibody_joints: MultibodyJointSet, + ) -> bool: ... + def tear( + self, + handle: SoftBodyHandle, + edges: IndicesLike, + cells: IndicesLike, + islands: IslandManager, + bodies: RigidBodySet, + colliders: ColliderSet, + impulse_joints: ImpulseJointSet, + multibody_joints: MultibodyJointSet, + ) -> SoftBodyTearEvent | None: ... + def cut( + self, + handle: SoftBodyHandle, + blade: Tuple[VectorLike, VectorLike, VectorLike], + islands: IslandManager, + bodies: RigidBodySet, + colliders: ColliderSet, + impulse_joints: ImpulseJointSet, + multibody_joints: MultibodyJointSet, + ) -> SoftBodyTearEvent | None: ... + def wake_up(self, handle: SoftBodyHandle, bodies: RigidBodySet, strong: bool = False) -> None: ... + def get(self, handle: SoftBodyHandle) -> SoftBody | None: ... + def is_empty(self) -> bool: ... + def handles(self) -> list[SoftBodyHandle]: ... + def __getitem__(self, handle: SoftBodyHandle) -> SoftBody: ... + def __contains__(self, handle: SoftBodyHandle) -> bool: ... + def __iter__(self) -> Iterator[Tuple[SoftBodyHandle, SoftBody]]: ... + def __len__(self) -> int: ... diff --git a/python/rapier-py-3d/src/controllers.rs b/python/rapier-py-3d/src/controllers.rs index a8374c4a9..cb2e70da6 100644 --- a/python/rapier-py-3d/src/controllers.rs +++ b/python/rapier-py-3d/src/controllers.rs @@ -701,7 +701,7 @@ impl KinematicCharacterController { bp.0.as_query_pipeline(np.0.query_dispatcher(), &bodies.0, &colliders.0, qf); let result = (|qp: rapier::pipeline::QueryPipeline<'_>| { - let mv = inner.move_shape(dt, &qp, &*shape.0.0, &pose, desired, |collision| { + let mv = inner.move_shape(dt, &qp, &*shape.0 .0, &pose, desired, |collision| { if let Some(ref obj) = cb_obj { let py_col = CharacterCollision::from_rapier(collision); if let Err(e) = obj.call1(py, (py_col,)) { @@ -781,7 +781,7 @@ impl KinematicCharacterController { self.0.solve_character_collision_impulses( dt, &mut qpmut, - &*character_shape.0.0, + &*character_shape.0 .0, character_mass, collisions_rs.iter(), ); diff --git a/python/rapier-py-3d/src/debug_render.rs b/python/rapier-py-3d/src/debug_render.rs index 86dd2ec2e..18ee54663 100644 --- a/python/rapier-py-3d/src/debug_render.rs +++ b/python/rapier-py-3d/src/debug_render.rs @@ -79,6 +79,7 @@ const _DRO_COLLIDER_AABB: u32 = 2; const _DRO_IMPULSE_JOINT: u32 = 3; const _DRO_MULTIBODY_JOINT: u32 = 4; const _DRO_CONTACT_PAIR: u32 = 5; +const _DRO_SOFT_BODY: u32 = 6; #[pymethods] impl DebugRenderObject { @@ -106,6 +107,10 @@ impl DebugRenderObject { const CONTACT_PAIR: DebugRenderObject = DebugRenderObject { kind: _DRO_CONTACT_PAIR, }; + #[classattr] + const SOFT_BODY: DebugRenderObject = DebugRenderObject { + kind: _DRO_SOFT_BODY, + }; /// Return ``DebugRenderObject.`` repr. fn __repr__(&self) -> String { @@ -116,6 +121,7 @@ impl DebugRenderObject { 3 => "IMPULSE_JOINT", 4 => "MULTIBODY_JOINT", 5 => "CONTACT_PAIR", + 6 => "SOFT_BODY", _ => "UNKNOWN", }; format!("DebugRenderObject.{s}") @@ -145,6 +151,7 @@ fn _dro_kind(obj: rapier::pipeline::DebugRenderObject<'_>) -> u32 { rapier::pipeline::DebugRenderObject::ImpulseJoint(..) => _DRO_IMPULSE_JOINT, rapier::pipeline::DebugRenderObject::MultibodyJoint(..) => _DRO_MULTIBODY_JOINT, rapier::pipeline::DebugRenderObject::ContactPair(..) => _DRO_CONTACT_PAIR, + rapier::pipeline::DebugRenderObject::SoftBody(..) => _DRO_SOFT_BODY, } } @@ -376,6 +383,18 @@ impl DebugRenderMode { const COLLIDER_AABBS: DebugRenderMode = DebugRenderMode(rapier::pipeline::DebugRenderMode::COLLIDER_AABBS); #[classattr] + const SOFT_BODIES: DebugRenderMode = + DebugRenderMode(rapier::pipeline::DebugRenderMode::SOFT_BODIES); + #[classattr] + const PSEUDO_NORMALS: DebugRenderMode = + DebugRenderMode(rapier::pipeline::DebugRenderMode::PSEUDO_NORMALS); + #[classattr] + const SOFT_VOLUME_CONTACTS: DebugRenderMode = + DebugRenderMode(rapier::pipeline::DebugRenderMode::SOFT_VOLUME_CONTACTS); + #[classattr] + const SOFT_BODY_STRESS: DebugRenderMode = + DebugRenderMode(rapier::pipeline::DebugRenderMode::SOFT_BODY_STRESS); + #[classattr] const EMPTY: DebugRenderMode = DebugRenderMode(rapier::pipeline::DebugRenderMode::empty()); /// Raw bits as an unsigned int. @@ -488,6 +507,95 @@ impl DebugRenderStyle { self.0.border_subdivisions = v; } + /// Debug color ``soft_body_element_color`` (see the soft-body debug-render modes). + #[getter] + fn soft_body_element_color(&self) -> DebugColor { + DebugColor(self.0.soft_body_element_color) + } + #[setter] + fn set_soft_body_element_color(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.soft_body_element_color = DebugColor::extract_from(v)?; + Ok(()) + } + /// Debug color ``soft_body_slack_color`` (see the soft-body debug-render modes). + #[getter] + fn soft_body_slack_color(&self) -> DebugColor { + DebugColor(self.0.soft_body_slack_color) + } + #[setter] + fn set_soft_body_slack_color(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.soft_body_slack_color = DebugColor::extract_from(v)?; + Ok(()) + } + /// Debug color ``soft_body_loaded_color`` (see the soft-body debug-render modes). + #[getter] + fn soft_body_loaded_color(&self) -> DebugColor { + DebugColor(self.0.soft_body_loaded_color) + } + #[setter] + fn set_soft_body_loaded_color(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.soft_body_loaded_color = DebugColor::extract_from(v)?; + Ok(()) + } + /// Debug color ``soft_body_frame_color`` (see the soft-body debug-render modes). + #[getter] + fn soft_body_frame_color(&self) -> DebugColor { + DebugColor(self.0.soft_body_frame_color) + } + #[setter] + fn set_soft_body_frame_color(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.soft_body_frame_color = DebugColor::extract_from(v)?; + Ok(()) + } + /// Debug color ``vertex_pseudo_normal_color`` (see the soft-body debug-render modes). + #[getter] + fn vertex_pseudo_normal_color(&self) -> DebugColor { + DebugColor(self.0.vertex_pseudo_normal_color) + } + #[setter] + fn set_vertex_pseudo_normal_color(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.vertex_pseudo_normal_color = DebugColor::extract_from(v)?; + Ok(()) + } + /// Debug color ``edge_pseudo_normal_color`` (see the soft-body debug-render modes). + #[getter] + fn edge_pseudo_normal_color(&self) -> DebugColor { + DebugColor(self.0.edge_pseudo_normal_color) + } + #[setter] + fn set_edge_pseudo_normal_color(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.edge_pseudo_normal_color = DebugColor::extract_from(v)?; + Ok(()) + } + /// Debug color ``volume_contact_normal_color`` (see the soft-body debug-render modes). + #[getter] + fn volume_contact_normal_color(&self) -> DebugColor { + DebugColor(self.0.volume_contact_normal_color) + } + #[setter] + fn set_volume_contact_normal_color(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.volume_contact_normal_color = DebugColor::extract_from(v)?; + Ok(()) + } + /// Debug color ``volume_gradient_color`` (see the soft-body debug-render modes). + #[getter] + fn volume_gradient_color(&self) -> DebugColor { + DebugColor(self.0.volume_gradient_color) + } + #[setter] + fn set_volume_gradient_color(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.volume_gradient_color = DebugColor::extract_from(v)?; + Ok(()) + } + /// Length of the pseudo-normals drawn by ``DebugRenderMode.PSEUDO_NORMALS``. + #[getter] + fn pseudo_normal_length(&self) -> Real { + self.0.pseudo_normal_length + } + #[setter] + fn set_pseudo_normal_length(&mut self, v: Real) { + self.0.pseudo_normal_length = v; + } /// Color for colliders attached to a dynamic rigid body. #[getter] fn collider_dynamic_color(&self) -> DebugColor { @@ -950,8 +1058,11 @@ impl DebugRenderPipeline { impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, narrow_phase: &NarrowPhase, + soft_bodies: Option<&crate::soft_body::SoftBodySet>, backend: &mut dyn rapier::pipeline::DebugRenderBackend, ) { + let scratch = rapier::dynamics::SoftBodySet::new(); + let soft_bodies = soft_bodies.map_or(&scratch, |s| &s.0); // The trait method `render` takes `&mut impl DebugRenderBackend` // which is not directly object-safe via `&mut dyn`. Wrap it. struct DynWrap<'a> { @@ -976,6 +1087,7 @@ impl DebugRenderPipeline { &impulse_joints.0, &multibody_joints.0, &narrow_phase.0, + soft_bodies, ); } } @@ -1037,7 +1149,7 @@ impl DebugRenderPipeline { /// :param multibody_joints: :class:`MultibodyJointSet`. /// :param narrow_phase: :class:`NarrowPhase`. /// :param backend: Collector or Python ``DebugRenderBackend``. - #[pyo3(signature = (bodies, colliders, impulse_joints, multibody_joints, narrow_phase, backend))] + #[pyo3(signature = (bodies, colliders, impulse_joints, multibody_joints, narrow_phase, backend, soft_bodies=None))] #[allow(clippy::too_many_arguments)] fn render( &mut self, @@ -1048,6 +1160,7 @@ impl DebugRenderPipeline { multibody_joints: &MultibodyJointSet, narrow_phase: &NarrowPhase, backend: &Bound<'_, PyAny>, + soft_bodies: Option<&crate::soft_body::SoftBodySet>, ) -> PyResult<()> { // Fast path: DebugLineCollector. We bypass the Python callback // dispatch entirely. @@ -1061,6 +1174,7 @@ impl DebugRenderPipeline { impulse_joints, multibody_joints, narrow_phase, + soft_bodies, &mut adapter, ); return Ok(()); @@ -1079,6 +1193,7 @@ impl DebugRenderPipeline { impulse_joints, multibody_joints, narrow_phase, + soft_bodies, &mut adapter, ); drop(adapter); @@ -1100,12 +1215,13 @@ impl DebugRenderPipeline { /// :param impulse_joints: :class:`ImpulseJointSet`. /// :param multibody_joints: :class:`MultibodyJointSet`. /// :param narrow_phase: :class:`NarrowPhase`. + /// :param soft_bodies: :class:`SoftBodySet`, optional. /// :returns: A ``(lines, colors, objects)`` tuple where /// ``lines`` is ``(N, 2, D)`` float, ``colors`` is /// ``(N, 4)`` float32 RGBA in ``[0, 1]``, and ``objects`` /// is ``(N,)`` uint32 of /// :class:`DebugRenderObject` discriminants. - #[pyo3(signature = (bodies, colliders, impulse_joints, multibody_joints, narrow_phase))] + #[pyo3(signature = (bodies, colliders, impulse_joints, multibody_joints, narrow_phase, soft_bodies=None))] #[allow(clippy::too_many_arguments)] fn render_to_arrays<'py>( &mut self, @@ -1115,6 +1231,7 @@ impl DebugRenderPipeline { impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, narrow_phase: &NarrowPhase, + soft_bodies: Option<&crate::soft_body::SoftBodySet>, ) -> PyResult<( Bound<'py, crate::numpy::PyArray3>, Bound<'py, crate::numpy::PyArray2>, @@ -1133,6 +1250,7 @@ impl DebugRenderPipeline { impulse_joints, multibody_joints, narrow_phase, + soft_bodies, &mut adapter, ); } diff --git a/python/rapier-py-3d/src/dynamics.rs b/python/rapier-py-3d/src/dynamics.rs index 3ed77a4c2..54bd46462 100644 --- a/python/rapier-py-3d/src/dynamics.rs +++ b/python/rapier-py-3d/src/dynamics.rs @@ -156,6 +156,9 @@ pub enum RigidBodyType { KINEMATIC_VELOCITY_BASED, /// Animated by writing to ``position`` each step. KINEMATIC_POSITION_BASED, + /// The proxy of a soft-body cluster: stands for the cluster in joints and islands, holds + /// its colliders, and is moved by the cluster's particles. + SOFT_FRAME, } impl RigidBodyType { @@ -170,6 +173,7 @@ impl RigidBodyType { Self::KINEMATIC_POSITION_BASED => { rapier::dynamics::RigidBodyType::KinematicPositionBased } + Self::SOFT_FRAME => rapier::dynamics::RigidBodyType::SoftFrame, } } #[inline] @@ -183,6 +187,7 @@ impl RigidBodyType { rapier::dynamics::RigidBodyType::KinematicPositionBased => { Self::KINEMATIC_POSITION_BASED } + rapier::dynamics::RigidBodyType::SoftFrame => Self::SOFT_FRAME, } } } @@ -1212,6 +1217,15 @@ impl IntegrationParameters { fn set_dt(&mut self, v: Real) { self.0.dt = v; } + /// The settings shared by every soft body (a copy: assign it back to apply changes). + #[getter] + fn soft_bodies(&self) -> crate::soft_body::SoftBodiesSettings { + crate::soft_body::SoftBodiesSettings(self.0.soft_bodies) + } + #[setter] + fn set_soft_bodies(&mut self, v: &crate::soft_body::SoftBodiesSettings) { + self.0.soft_bodies = v.0; + } /// Minimum substep length used by CCD, in seconds. #[getter] fn min_ccd_dt(&self) -> Real { @@ -1725,6 +1739,31 @@ impl RigidBody { fn additional_solver_iterations(&self) -> usize { self.with_ref(|b| b.additional_solver_iterations()) } + /// Extra internal PGS iterations run per substep for the island component containing + /// this body (default ``0``); the component runs the largest request. + #[getter] + fn additional_pgs_iterations(&self) -> usize { + self.with_ref(|b| b.additional_pgs_iterations()) + } + #[setter] + fn set_additional_pgs_iterations(&mut self, v: usize) { + self.with_mut(|b| b.set_additional_pgs_iterations(v)) + } + /// Is this body the proxy of a soft-body cluster? + #[getter] + fn is_soft_frame(&self) -> bool { + self.with_ref(|b| b.is_soft_frame()) + } + /// The soft body this body is a cluster proxy of, if any. + #[getter] + fn soft_body(&self) -> Option { + self.with_ref(|b| b.soft_body().map(crate::soft_body::SoftBodyHandle)) + } + /// The index of the cluster this proxy stands for in its soft body, if any. + #[getter] + fn soft_cluster(&self) -> Option { + self.with_ref(|b| b.soft_cluster()) + } /// Set the number of additional solver iterations. #[setter] fn set_additional_solver_iterations(&mut self, v: usize) { @@ -2127,6 +2166,10 @@ impl RigidBodyBuilder { let n: usize = v.extract()?; self.builder = self.builder.clone().additional_solver_iterations(n); } + "additional_pgs_iterations" => { + let n: usize = v.extract()?; + self.builder = self.builder.clone().additional_pgs_iterations(n); + } "user_data" => { let d: u128 = v.extract()?; self.builder = self.builder.clone().user_data(d); @@ -2273,6 +2316,12 @@ impl RigidBodyBuilder { builder: self.builder.clone().additional_solver_iterations(n), } } + /// Extra internal PGS iterations run per substep for the island component of this body. + fn additional_pgs_iterations(&self, n: usize) -> Self { + Self { + builder: self.builder.clone().additional_pgs_iterations(n), + } + } /// Attach an application-defined ``u128`` payload to the body. fn user_data(&self, d: u128) -> Self { Self { @@ -2646,7 +2695,12 @@ impl RigidBodySet { /// /// :returns: the removed body, or ``None`` if no body matched /// ``handle``. - #[pyo3(signature = (handle, islands, colliders, impulse_joints, multibody_joints, remove_attached_colliders=true))] + /// + /// ``soft_bodies`` is the world's :class:`SoftBodySet`: removing the proxy of a + /// soft-body cluster removes that cluster. It can be left out for a world without soft + /// bodies. + #[pyo3(signature = (handle, islands, colliders, impulse_joints, multibody_joints, remove_attached_colliders=true, soft_bodies=None))] + #[allow(clippy::too_many_arguments)] fn remove( &mut self, handle: &RigidBodyHandle, @@ -2655,7 +2709,10 @@ impl RigidBodySet { impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, remove_attached_colliders: bool, + soft_bodies: Option<&mut crate::soft_body::SoftBodySet>, ) -> Option { + let mut scratch = rapier::dynamics::SoftBodySet::new(); + let soft_bodies = soft_bodies.map_or(&mut scratch, |s| &mut s.0); self.0 .remove( handle.0, @@ -2663,6 +2720,7 @@ impl RigidBodySet { &mut colliders.0, &mut impulse_joints.0, &mut multibody_joints.0, + soft_bodies, remove_attached_colliders, ) .map(RigidBody::new_owned) diff --git a/python/rapier-py-3d/src/events_hooks.rs b/python/rapier-py-3d/src/events_hooks.rs index 4d7b625da..1c684db91 100644 --- a/python/rapier-py-3d/src/events_hooks.rs +++ b/python/rapier-py-3d/src/events_hooks.rs @@ -106,7 +106,12 @@ impl SolverFlags { /// Compute impulses for this contact pair (i.e. solve the contact). #[classattr] const COMPUTE_IMPULSES: SolverFlags = - SolverFlags(rapier::geometry::SolverFlags::COMPUTE_IMPULSES); + SolverFlags(rapier::geometry::SolverFlags::COMPUTE_RIGID_IMPULSES); + /// Compute impulses between two rigid colliders (the name the engine uses for + /// ``COMPUTE_IMPULSES``). + #[classattr] + const COMPUTE_RIGID_IMPULSES: SolverFlags = + SolverFlags(rapier::geometry::SolverFlags::COMPUTE_RIGID_IMPULSES); /// Empty flag set — equivalent to :meth:`empty`. #[classattr] const EMPTY: SolverFlags = SolverFlags(rapier::geometry::SolverFlags::empty()); @@ -587,6 +592,40 @@ impl rapier::pipeline::EventHandler for PyEventHandler { }); } + fn handle_soft_body_tear_event( + &self, + _soft_bodies: &rapier::dynamics::SoftBodySet, + event: &rapier::dynamics::SoftBodyTearEvent, + ) { + { + let slot = self.err_slot.lock().unwrap(); + if slot.aborted { + return; + } + } + Python::with_gil(|py| { + let bound = self.obj.bind(py); + // The method is optional: handlers written before soft bodies existed keep working. + if !bound + .hasattr("handle_soft_body_tear_event") + .unwrap_or(false) + { + return; + } + let py_event = crate::soft_body::SoftBodyTearEvent(event.clone()); + let res = bound.call_method1("handle_soft_body_tear_event", (py.None(), py_event)); + if let Err(e) = res { + let mut s = self.err_slot.lock().unwrap(); + if s.err.is_none() { + s.err = Some(e); + } + if s.policy_strict { + s.aborted = true; + } + } + }); + } + fn handle_contact_force_event( &self, dt: Real, @@ -658,6 +697,7 @@ impl rapier::pipeline::EventHandler for PyEventHandler { pub struct ChannelEventCollector { collisions: Arc>>, forces: Arc>>, + tears: Arc>>, } #[pymethods] @@ -668,9 +708,17 @@ impl ChannelEventCollector { Self { collisions: Arc::new(Mutex::new(Vec::new())), forces: Arc::new(Mutex::new(Vec::new())), + tears: Arc::new(Mutex::new(Vec::new())), } } + /// Consume and return every queued :class:`SoftBodyTearEvent`. + /// + /// After this call the internal tear buffer is empty. + fn drain_soft_body_tear_events(&self) -> Vec { + std::mem::take(&mut *self.tears.lock().unwrap()) + } + /// Consume and return every queued :class:`CollisionEvent`. /// /// After this call the internal collision buffer is empty. @@ -691,15 +739,18 @@ impl ChannelEventCollector { std::mem::take(&mut *self.forces.lock().unwrap()) } - /// Drop every queued collision and contact-force event. + /// Drop every queued collision, contact-force and tear event. fn clear(&self) { self.collisions.lock().unwrap().clear(); self.forces.lock().unwrap().clear(); + self.tears.lock().unwrap().clear(); } - /// Total queued events (collisions + contact-force). + /// Total queued events (collisions + contact-force + tears). fn __len__(&self) -> usize { - self.collisions.lock().unwrap().len() + self.forces.lock().unwrap().len() + self.collisions.lock().unwrap().len() + + self.forces.lock().unwrap().len() + + self.tears.lock().unwrap().len() } /// Debug repr — shows the per-buffer queue lengths. @@ -718,6 +769,7 @@ impl ChannelEventCollector { ChannelEventCollectorAdapter { collisions: Arc::clone(&self.collisions), forces: Arc::clone(&self.forces), + tears: Arc::clone(&self.tears), } } } @@ -728,9 +780,21 @@ impl ChannelEventCollector { pub struct ChannelEventCollectorAdapter { collisions: Arc>>, forces: Arc>>, + tears: Arc>>, } impl rapier::pipeline::EventHandler for ChannelEventCollectorAdapter { + fn handle_soft_body_tear_event( + &self, + _soft_bodies: &rapier::dynamics::SoftBodySet, + event: &rapier::dynamics::SoftBodyTearEvent, + ) { + self.tears + .lock() + .unwrap() + .push(crate::soft_body::SoftBodyTearEvent(event.clone())); + } + fn handle_collision_event( &self, _bodies: &rapier::dynamics::RigidBodySet, @@ -906,14 +970,20 @@ impl rapier::pipeline::PhysicsHooks for PyPhysicsHooks { return; } } + // The contacts of two soft surfaces are candidates rather than a manifold: the Python + // hook only sees rigid manifolds. + let manifold = match &mut context.contacts { + rapier::pipeline::ModifiableContacts::Rigid(manifold) => manifold, + rapier::pipeline::ModifiableContacts::Soft(_) => return, + }; Python::with_gil(|py| { - let manifold_local_n1 = context.manifold.local_n1; - let manifold_local_n2 = context.manifold.local_n2; - let normal_ptr: *mut rapier::math::Vector = context.normal; - let sc_ptr: *mut Vec = context.solver_contacts; - let friction_ptr: *mut Real = context.friction; - let restitution_ptr: *mut Real = context.restitution; - let ud_ptr: *mut u32 = context.user_data; + let manifold_local_n1 = manifold.manifold.local_n1; + let manifold_local_n2 = manifold.manifold.local_n2; + let normal_ptr: *mut rapier::math::Vector = manifold.normal; + let sc_ptr: *mut Vec = manifold.solver_contacts; + let friction_ptr: *mut Real = manifold.friction; + let restitution_ptr: *mut Real = manifold.restitution; + let ud_ptr: *mut u32 = manifold.user_data; let ctx_py = match Py::new( py, ContactModificationContext { diff --git a/python/rapier-py-3d/src/geometry.rs b/python/rapier-py-3d/src/geometry.rs index ae1ff76f9..19e3f0f65 100644 --- a/python/rapier-py-3d/src/geometry.rs +++ b/python/rapier-py-3d/src/geometry.rs @@ -1023,6 +1023,16 @@ impl TriMeshFlags { #[classattr] const FIX_INTERNAL_EDGES: TriMeshFlags = TriMeshFlags(rapier::parry::shape::TriMeshFlags::FIX_INTERNAL_EDGES); + /// The mesh is deformable: its vertices may be moved after its creation. Required for a + /// triangle mesh used as a soft body's collision mesh (see + /// :meth:`ColliderSet.insert_deformable`). + #[classattr] + const DEFORMABLE: TriMeshFlags = TriMeshFlags(rapier::parry::shape::TriMeshFlags::DEFORMABLE); + /// Like ``FIX_INTERNAL_EDGES``, but a contact coming from the back of a triangle is kept + /// and its normal is constrained by the internal edges too. + #[classattr] + const FIX_INTERNAL_EDGES_TWO_SIDED: TriMeshFlags = + TriMeshFlags(rapier::parry::shape::TriMeshFlags::FIX_INTERNAL_EDGES_TWO_SIDED); /// Raw bit pattern of the flag set. #[getter] @@ -2618,7 +2628,7 @@ impl ContactPair { #[getter] fn manifolds(&self) -> Vec { self.0 - .manifolds + .manifolds() .iter() .map(|m| { let normal_v: crate::na::SVector = m.data.normal.into(); @@ -3066,6 +3076,12 @@ impl Collider { self.with_ref(|c| c.parent()).map(RigidBodyHandle) } + /// The soft-body collision mesh this collider holds, if it is a deformable collider. + #[getter] + fn deformable_mesh_ref(&self) -> Option { + self.with_ref(|c| c.deformable_mesh_ref().map(crate::soft_body::SoftMeshRef)) + } + // ---- position / translation / rotation ---- /// World-space pose of the collider. @@ -4023,19 +4039,64 @@ impl ColliderSet { /// :param wake_parent: If True, wakes the parent rigid-body so /// islands re-evaluate. Defaults to True. /// :returns: The removed `Collider`, or `None` if `handle` is unknown. - #[pyo3(signature = (handle, islands, bodies, wake_parent=true))] + #[pyo3(signature = (handle, islands, bodies, wake_parent=true, soft_bodies=None))] fn remove( &mut self, handle: &ColliderHandle, islands: &mut IslandManager, bodies: &mut RigidBodySet, wake_parent: bool, + soft_bodies: Option<&mut crate::soft_body::SoftBodySet>, ) -> Option { + let mut scratch = rapier::dynamics::SoftBodySet::new(); + let soft_bodies = soft_bodies.map_or(&mut scratch, |s| &mut s.0); self.0 - .remove(handle.0, &mut islands.0, &mut bodies.0, wake_parent) + .remove( + handle.0, + &mut islands.0, + &mut bodies.0, + soft_bodies, + wake_parent, + ) .map(Collider::new_owned) } + /// Insert a collider holding a soft body's deformable collision mesh: a triangle mesh + /// built with ``TriMeshFlags.DEFORMABLE`` whose vertices follow the cluster of ``parent`` + /// (a cluster proxy, see :attr:`SoftBody.root_body` and :meth:`SoftBody.cluster_proxy`) + /// through ``binding``. + /// + /// :raises SoftBindingError: if the parent is not a live cluster proxy, the shape is not a + /// deformable mesh, or a vertex could not be bound. + pub fn insert_deformable( + &mut self, + builder: &Bound<'_, PyAny>, + binding: &crate::soft_body::SoftMeshBinding, + parent: &RigidBodyHandle, + bodies: &mut RigidBodySet, + soft_bodies: &mut crate::soft_body::SoftBodySet, + ) -> PyResult { + let coll = if let Ok(b) = builder.extract::>() { + b.builder.clone().build() + } else if let Ok(c) = builder.extract::>() { + c.to_owned_collider() + } else { + return Err(PyTypeError::new_err( + "ColliderSet.insert_deformable expects a Collider or ColliderBuilder", + )); + }; + self.0 + .insert_deformable( + coll, + binding.0.clone(), + parent.0, + &mut bodies.0, + &mut soft_bodies.0, + ) + .map(ColliderHandle) + .map_err(|e| crate::soft_body::SoftBindingError::new_err(e.to_string())) + } + /// Return a live **view** of the collider for `handle`, or `None` /// if the handle is unknown. Reads and writes go straight through /// to the set with no copy. diff --git a/python/rapier-py-3d/src/joints.rs b/python/rapier-py-3d/src/joints.rs index 8a1db4cff..a78b82ff9 100644 --- a/python/rapier-py-3d/src/joints.rs +++ b/python/rapier-py-3d/src/joints.rs @@ -637,16 +637,16 @@ impl MultibodyIndex { /// Slot index of this multibody. #[getter] fn index(&self) -> u32 { - self.0.0.into_raw_parts().0 + self.0 .0.into_raw_parts().0 } /// Generation counter, incremented when a slot is reused. #[getter] fn generation(&self) -> u32 { - self.0.0.into_raw_parts().1 + self.0 .0.into_raw_parts().1 } /// Hash matching equality semantics. fn __hash__(&self) -> u64 { - let (i, g) = self.0.0.into_raw_parts(); + let (i, g) = self.0 .0.into_raw_parts(); ((i as u64) << 32) | (g as u64) } /// Equality and inequality comparisons; other operators raise. @@ -661,7 +661,7 @@ impl MultibodyIndex { } /// Return a developer-readable representation. fn __repr__(&self) -> String { - let (i, g) = self.0.0.into_raw_parts(); + let (i, g) = self.0 .0.into_raw_parts(); format!("MultibodyIndex(index={}, generation={})", i, g) } } diff --git a/python/rapier-py-3d/src/lib.rs b/python/rapier-py-3d/src/lib.rs index bb1d741e5..e24b73b44 100644 --- a/python/rapier-py-3d/src/lib.rs +++ b/python/rapier-py-3d/src/lib.rs @@ -50,6 +50,7 @@ pub mod loaders; pub mod math; pub mod pipeline; pub mod serde_glue; +pub mod soft_body; pub use controllers::*; pub use conv::*; @@ -62,6 +63,7 @@ pub use joints::*; pub use loaders::*; pub use math::*; pub use pipeline::*; +pub use soft_body::*; use pyo3::prelude::*; @@ -79,6 +81,7 @@ fn _rapier3d(py: Python<'_>, m: &Bound<'_, PyModule>) -> PyResult<()> { register_loaders(py, m)?; register_controllers(py, m)?; register_debug_render(py, m)?; + register_soft_bodies(py, m)?; m.add("__version__", RAPIER_PY_VERSION)?; Ok(()) } diff --git a/python/rapier-py-3d/src/pipeline.rs b/python/rapier-py-3d/src/pipeline.rs index dc5d158b4..4c818037d 100644 --- a/python/rapier-py-3d/src/pipeline.rs +++ b/python/rapier-py-3d/src/pipeline.rs @@ -1727,7 +1727,7 @@ impl QueryPipeline { let result = (|qp: rapier::pipeline::QueryPipeline<'_>| { Ok(qp - .cast_shape(&pose, vel, &*shape.0.0, options.0) + .cast_shape(&pose, vel, &*shape.0 .0, options.0) .map(|(h, hit)| (ColliderHandle(h), ShapeCastHit::from_parry(hit)))) })(qp); if let Some(e) = pred_err.into_inner() { @@ -1823,7 +1823,7 @@ impl QueryPipeline { Ok(qp .cast_shape_nonlinear( &motion.0, - &*shape.0.0, + &*shape.0 .0, start_time, end_time, options.0.stop_at_penetration, @@ -1913,7 +1913,7 @@ impl QueryPipeline { bp.0.as_query_pipeline(np.0.query_dispatcher(), &bodies.0, &colliders.0, qf); let result = (|qp: rapier::pipeline::QueryPipeline<'_>| { - for (h, _co) in qp.intersect_shape(pose, &*shape.0.0) { + for (h, _co) in qp.intersect_shape(pose, &*shape.0 .0) { let res = callback.call1(py, (ColliderHandle(h),))?; if let Ok(b) = res.extract::(py) { if !b { @@ -2466,6 +2466,7 @@ impl PhysicsPipeline { ccd_solver, hooks=None, events=None, + soft_bodies=None, ))] #[allow(clippy::too_many_arguments)] fn step( @@ -2483,6 +2484,7 @@ impl PhysicsPipeline { ccd_solver: &mut CCDSolver, hooks: Option<&Bound<'_, PyAny>>, events: Option<&Bound<'_, PyAny>>, + soft_bodies: Option<&mut crate::soft_body::SoftBodySet>, ) -> PyResult<()> { let hooks_obj: Option> = hooks.and_then(|h| { if h.is_none() { @@ -2503,6 +2505,8 @@ impl PhysicsPipeline { let events_box = build_event_handler(py, events_obj.as_ref(), err_slot.clone()); let g: rapier::math::Vector = gravity.0.into(); + let mut scratch_soft_bodies = rapier::dynamics::SoftBodySet::new(); + let sb_inner = soft_bodies.map_or(&mut scratch_soft_bodies, |s| &mut s.0); let pp_inner = &mut self.0; let ip_inner = &integration_parameters.0; let islands_inner = &mut islands.0; @@ -2532,6 +2536,7 @@ impl PhysicsPipeline { colliders_inner, ij_inner, mj_inner, + sb_inner, ccd_inner, hooks_ref, events_ref, @@ -2696,6 +2701,7 @@ impl CollisionPipeline { pub struct PhysicsWorld { pub bodies: Py, pub colliders: Py, + pub soft_bodies: Py, pub impulse_joints: Py, pub multibody_joints: Py, pub broad_phase: Py, @@ -2734,6 +2740,10 @@ impl PhysicsWorld { let bodies = Py::new(py, RigidBodySet(rapier::dynamics::RigidBodySet::new()))?; let colliders = Py::new(py, ColliderSet(rapier::geometry::ColliderSet::new()))?; + let soft_bodies = Py::new( + py, + crate::soft_body::SoftBodySet(rapier::dynamics::SoftBodySet::new()), + )?; let impulse_joints = Py::new( py, ImpulseJointSet(rapier::dynamics::ImpulseJointSet::new()), @@ -2768,6 +2778,7 @@ impl PhysicsWorld { Ok(Self { bodies, colliders, + soft_bodies, impulse_joints, multibody_joints, broad_phase, @@ -2808,6 +2819,11 @@ impl PhysicsWorld { fn multibody_joints(&self, py: Python<'_>) -> Py { self.multibody_joints.clone_ref(py) } + /// The world's :class:`SoftBodySet` (stable across calls). + #[getter] + fn soft_bodies(&self, py: Python<'_>) -> Py { + self.soft_bodies.clone_ref(py) + } /// The world's :class:`BroadPhaseBvh` (stable across calls). #[getter] fn broad_phase(&self, py: Python<'_>) -> Py { @@ -2997,6 +3013,7 @@ impl PhysicsWorld { let mut colliders = self.colliders.borrow_mut(py); let mut ij = self.impulse_joints.borrow_mut(py); let mut mj = self.multibody_joints.borrow_mut(py); + let mut sb = self.soft_bodies.borrow_mut(py); let mut ccd = self.ccd_solver.borrow_mut(py); let g_engine: rapier::math::Vector = g.into(); // Borrow the inner rapier values explicitly so the @@ -3011,6 +3028,7 @@ impl PhysicsWorld { let colliders_inner = &mut colliders.0; let ij_inner = &mut ij.0; let mj_inner = &mut mj.0; + let sb_inner = &mut sb.0; let ccd_inner = &mut ccd.0; let hooks_ref: &dyn rapier::pipeline::PhysicsHooks = match hooks_box.as_deref() { Some(h) => h, @@ -3031,6 +3049,7 @@ impl PhysicsWorld { colliders_inner, ij_inner, mj_inner, + sb_inner, ccd_inner, hooks_ref, events_ref, @@ -3245,6 +3264,7 @@ impl PhysicsWorld { let mut colliders = self.colliders.borrow_mut(py); let mut ij = self.impulse_joints.borrow_mut(py); let mut mj = self.multibody_joints.borrow_mut(py); + let mut sb = self.soft_bodies.borrow_mut(py); Ok(bodies .0 .remove( @@ -3253,6 +3273,7 @@ impl PhysicsWorld { &mut colliders.0, &mut ij.0, &mut mj.0, + &mut sb.0, true, ) .map(RigidBody::new_owned)) @@ -3271,17 +3292,182 @@ impl PhysicsWorld { let mut cset = self.colliders.borrow_mut(py); let mut bset = self.bodies.borrow_mut(py); let mut islands = self.islands.borrow_mut(py); + let mut sb = self.soft_bodies.borrow_mut(py); Ok(cset .0 - .remove(handle.0, &mut islands.0, &mut bset.0, true) + .remove(handle.0, &mut islands.0, &mut bset.0, &mut sb.0, true) .map(Collider::new_owned)) } - /// Debug repr — shows body and collider counts. + // ---- soft bodies ---- + + /// Insert a soft body and return its handle; this creates its hidden root rigid body and + /// its colliders, which :meth:`remove_soft_body` removes. + /// + /// :param builder: A :class:`SoftBodyBuilder` (see :meth:`SoftBody.cloth`, + /// :meth:`SoftBody.cuboid`, ...). + fn add_soft_body( + &self, + py: Python<'_>, + builder: &crate::soft_body::SoftBodyBuilder, + ) -> crate::soft_body::SoftBodyHandle { + let mut sb = self.soft_bodies.borrow_mut(py); + let mut bodies = self.bodies.borrow_mut(py); + let mut colliders = self.colliders.borrow_mut(py); + crate::soft_body::SoftBodyHandle(sb.0.insert( + builder.builder.clone(), + &mut bodies.0, + &mut colliders.0, + )) + } + + /// Remove a soft body with its proxies, colliders and attached joints. + /// + /// :returns: the removed :class:`SoftBody`, or ``None`` if the handle matched nothing. + fn remove_soft_body( + &self, + py: Python<'_>, + handle: &crate::soft_body::SoftBodyHandle, + ) -> Option { + let mut sb = self.soft_bodies.borrow_mut(py); + let mut islands = self.islands.borrow_mut(py); + let mut bodies = self.bodies.borrow_mut(py); + let mut colliders = self.colliders.borrow_mut(py); + let mut ij = self.impulse_joints.borrow_mut(py); + let mut mj = self.multibody_joints.borrow_mut(py); + sb.0.remove( + handle.0, + &mut islands.0, + &mut bodies.0, + &mut colliders.0, + &mut ij.0, + &mut mj.0, + ) + .map(crate::soft_body::SoftBody::new_owned) + } + + /// Insert a collider holding a soft body's deformable collision mesh, bound to the cluster + /// of ``parent`` (a cluster proxy; see :meth:`ColliderSet.insert_deformable`). + /// + /// :raises SoftBindingError: if the mesh cannot be bound. + fn insert_deformable( + &self, + py: Python<'_>, + builder: &Bound<'_, PyAny>, + binding: &crate::soft_body::SoftMeshBinding, + parent: &RigidBodyHandle, + ) -> PyResult { + let mut colliders = self.colliders.borrow_mut(py); + let mut bodies = self.bodies.borrow_mut(py); + let mut sb = self.soft_bodies.borrow_mut(py); + colliders.insert_deformable(builder, binding, parent, &mut bodies, &mut sb) + } + + /// Add a cluster to a soft body: a rigid proxy over the given particles that joints and + /// colliders can attach to. Returns the cluster's index, or ``None`` if no particle was + /// valid. + fn add_soft_body_cluster( + &self, + py: Python<'_>, + handle: &crate::soft_body::SoftBodyHandle, + particles: &Bound<'_, PyAny>, + ) -> PyResult> { + let mut sb = self.soft_bodies.borrow_mut(py); + let mut bodies = self.bodies.borrow_mut(py); + let mut colliders = self.colliders.borrow_mut(py); + sb.add_cluster(handle, particles, &mut bodies, &mut colliders) + } + + /// Remove a cluster of a soft body with its proxy, colliders and joints; ``False`` if it + /// did not exist. + fn remove_soft_body_cluster( + &self, + py: Python<'_>, + handle: &crate::soft_body::SoftBodyHandle, + cluster: u32, + ) -> bool { + let mut sb = self.soft_bodies.borrow_mut(py); + let mut islands = self.islands.borrow_mut(py); + let mut bodies = self.bodies.borrow_mut(py); + let mut colliders = self.colliders.borrow_mut(py); + let mut ij = self.impulse_joints.borrow_mut(py); + let mut mj = self.multibody_joints.borrow_mut(py); + sb.remove_cluster( + handle, + cluster, + &mut islands, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + ) + } + + /// Tear a soft body at once along the given edges and through the given cells, without + /// removing material (see :meth:`SoftBodySet.tear`). + /// + /// :returns: the :class:`SoftBodyTearEvent`, or ``None`` when nothing changed. + fn tear_soft_body( + &self, + py: Python<'_>, + handle: &crate::soft_body::SoftBodyHandle, + edges: &Bound<'_, PyAny>, + cells: &Bound<'_, PyAny>, + ) -> PyResult> { + let mut sb = self.soft_bodies.borrow_mut(py); + let mut islands = self.islands.borrow_mut(py); + let mut bodies = self.bodies.borrow_mut(py); + let mut colliders = self.colliders.borrow_mut(py); + let mut ij = self.impulse_joints.borrow_mut(py); + let mut mj = self.multibody_joints.borrow_mut(py); + sb.tear( + handle, + edges, + cells, + &mut islands, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + ) + } + + /// Cut a soft body along a blade (a world-space triangle given as three points) at once, + /// without removing material (see :meth:`SoftBodySet.cut`). + /// + /// :returns: the :class:`SoftBodyTearEvent`, or ``None`` when nothing changed. + fn cut_soft_body( + &self, + py: Python<'_>, + handle: &crate::soft_body::SoftBodyHandle, + blade: (PyVector, PyVector, PyVector), + ) -> Option { + let mut sb = self.soft_bodies.borrow_mut(py); + let mut islands = self.islands.borrow_mut(py); + let mut bodies = self.bodies.borrow_mut(py); + let mut colliders = self.colliders.borrow_mut(py); + let mut ij = self.impulse_joints.borrow_mut(py); + let mut mj = self.multibody_joints.borrow_mut(py); + sb.cut( + handle, + blade, + &mut islands, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + ) + } + + /// Debug repr — shows body, collider and soft-body counts. fn __repr__(&self, py: Python<'_>) -> String { let nb = self.bodies.borrow(py).0.len(); let nc = self.colliders.borrow(py).0.len(); - format!("PhysicsWorld(bodies={}, colliders={})", nb, nc) + let ns = self.soft_bodies.borrow(py).0.len(); + format!( + "PhysicsWorld(bodies={}, colliders={}, soft_bodies={})", + nb, nc, ns + ) } } diff --git a/python/rapier-py-3d/src/serde_glue.rs b/python/rapier-py-3d/src/serde_glue.rs index 3df1ff45b..3753e4512 100644 --- a/python/rapier-py-3d/src/serde_glue.rs +++ b/python/rapier-py-3d/src/serde_glue.rs @@ -89,11 +89,11 @@ impl MultibodyIndex { )) } fn into_raw_parts(&self) -> (u32, u32) { - self.0.0.into_raw_parts() + self.0 .0.into_raw_parts() } fn __reduce__(slf: PyRef<'_, Self>) -> PyResult<(Py, (u32, u32))> { let py = slf.py(); - let (i, g) = slf.0.0.into_raw_parts(); + let (i, g) = slf.0 .0.into_raw_parts(); let ctor: Py = py .get_type_bound::() .getattr("from_raw_parts")? @@ -1527,6 +1527,7 @@ type _PhysicsWorldOwned = ( rapier::dynamics::CCDSolver, rapier::dynamics::IntegrationParameters, crate::na::SVector, + rapier::dynamics::SoftBodySet, ); #[pymethods] @@ -1550,6 +1551,7 @@ impl PhysicsWorld { let islands = self.islands.borrow(py); let ccd = self.ccd_solver.borrow(py); let ip = self.integration_parameters.borrow(py); + let sb = self.soft_bodies.borrow(py); let tup = ( &bodies.0, &colliders.0, @@ -1561,6 +1563,7 @@ impl PhysicsWorld { &ccd.0, &ip.0, &self.gravity.0, + &sb.0, ); let payload = crate::bincode::serialize(&tup).map_err(crate::serde_io::bincode_err)?; Ok(crate::serde_io::bytes_to_py( @@ -1576,9 +1579,10 @@ impl PhysicsWorld { let body = crate::serde_io::unwrap_bincode(buf)?; let owned: _PhysicsWorldOwned = crate::bincode::deserialize(body).map_err(crate::serde_io::bincode_err)?; - let (bs, cs, ijs, mjs, bps, nps, isl, ccd, ip, g) = owned; + let (bs, cs, ijs, mjs, bps, nps, isl, ccd, ip, g, sbs) = owned; let bodies = Py::new(py, RigidBodySet(bs))?; let colliders = Py::new(py, ColliderSet(cs))?; + let soft_bodies = Py::new(py, crate::soft_body::SoftBodySet(sbs))?; let impulse_joints = Py::new(py, ImpulseJointSet(ijs))?; let multibody_joints = Py::new(py, MultibodyJointSet(mjs))?; let broad_phase = Py::new(py, BroadPhaseBvh(bps))?; @@ -1604,6 +1608,7 @@ impl PhysicsWorld { PhysicsWorld { bodies, colliders, + soft_bodies, impulse_joints, multibody_joints, broad_phase, @@ -1636,6 +1641,7 @@ impl PhysicsWorld { let islands = self.islands.borrow(py); let ccd = self.ccd_solver.borrow(py); let ip = self.integration_parameters.borrow(py); + let sb = self.soft_bodies.borrow(py); let tup = ( &bodies.0, &colliders.0, @@ -1647,6 +1653,7 @@ impl PhysicsWorld { &ccd.0, &ip.0, &self.gravity.0, + &sb.0, ); let payload = crate::serde_json::to_value(tup).map_err(crate::serde_io::json_err)?; let env = crate::serde_io::wrap_json(payload); @@ -1661,9 +1668,10 @@ impl PhysicsWorld { let payload = crate::serde_io::unwrap_json(env)?; let owned: _PhysicsWorldOwned = crate::serde_json::from_value(payload).map_err(crate::serde_io::json_err)?; - let (bs, cs, ijs, mjs, bps, nps, isl, ccd, ip, g) = owned; + let (bs, cs, ijs, mjs, bps, nps, isl, ccd, ip, g, sbs) = owned; let bodies = Py::new(py, RigidBodySet(bs))?; let colliders = Py::new(py, ColliderSet(cs))?; + let soft_bodies = Py::new(py, crate::soft_body::SoftBodySet(sbs))?; let impulse_joints = Py::new(py, ImpulseJointSet(ijs))?; let multibody_joints = Py::new(py, MultibodyJointSet(mjs))?; let broad_phase = Py::new(py, BroadPhaseBvh(bps))?; @@ -1689,6 +1697,7 @@ impl PhysicsWorld { PhysicsWorld { bodies, colliders, + soft_bodies, impulse_joints, multibody_joints, broad_phase, diff --git a/python/rapier-py-3d/src/soft_body.rs b/python/rapier-py-3d/src/soft_body.rs new file mode 100644 index 000000000..bffbb2d29 --- /dev/null +++ b/python/rapier-py-3d/src/soft_body.rs @@ -0,0 +1,3029 @@ +//! Soft bodies: deformable bodies made of particles linked by elastic constraints, simulated +//! together with the rigid bodies, contacts and joints of a world. + +use crate::conv::{PyIsometry, PyVector, Real}; +use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet, SpringCoefficients}; +use crate::geometry::{ColliderBuilder, ColliderHandle, ColliderSet}; +use crate::joints::{ImpulseJointHandle, ImpulseJointSet, MultibodyJointSet}; +use crate::math::Vec3; +use crate::numpy::{PyArray2, PyReadonlyArray2, PyUntypedArrayMethods}; +use crate::pyo3::create_exception; +use crate::pyo3::exceptions::{PyIndexError, PyTypeError, PyValueError}; +use crate::pyo3::prelude::*; +use crate::pyo3::types::PyDict; +use rapier3d as rapier; + +create_exception!( + rapier, + SoftBindingError, + crate::errors::RapierError, + "A deformable collider could not be bound to a soft-body cluster." +); + +// ---------------------------------------------------------------------- +// Array helpers. +// ---------------------------------------------------------------------- + +/// An `(N, 3)` float ndarray from vectors. +fn vectors_to_array<'py>( + py: Python<'py>, + it: impl Iterator, +) -> Bound<'py, PyArray2> { + let rows: Vec> = it.map(|v| v.to_array().to_vec()).collect(); + if rows.is_empty() { + PyArray2::zeros_bound(py, [0, 3], false) + } else { + PyArray2::from_vec2_bound(py, &rows).expect("contiguous ndarray") + } +} + +/// An `(M, N)` uint32 ndarray from elements of `N` indices. +fn elements_to_array<'py, const N: usize>( + py: Python<'py>, + elements: &[[u32; N]], +) -> Bound<'py, PyArray2> { + if elements.is_empty() { + PyArray2::zeros_bound(py, [0, N], false) + } else { + let rows: Vec> = elements.iter().map(|e| e.to_vec()).collect(); + PyArray2::from_vec2_bound(py, &rows).expect("contiguous ndarray") + } +} + +/// Elements of `N` indices each, from an `(M, N)` integer ndarray or a sequence of +/// `N`-sequences. +fn extract_elements(obj: &Bound<'_, PyAny>) -> PyResult> { + if let Ok(arr) = obj.extract::>() { + let (nrows, ncols) = (arr.shape()[0], arr.shape()[1]); + if ncols != N { + return Err(PyValueError::new_err(format!( + "expected an integer ndarray with shape (M, {N}); got (M, {ncols})" + ))); + } + let slice = arr + .as_slice() + .map_err(|_| PyValueError::new_err("ndarray must be contiguous"))?; + let mut out = Vec::with_capacity(nrows); + for chunk in slice.chunks_exact(N) { + let mut e = [0u32; N]; + e.copy_from_slice(chunk); + out.push(e); + } + return Ok(out); + } + let rows: Vec> = obj.extract().map_err(|_| { + PyTypeError::new_err(format!( + "expected an (M, {N}) integer ndarray or a sequence of {N}-tuples of ints" + )) + })?; + rows.into_iter() + .map(|r| { + <[u32; N]>::try_from(r).map_err(|r| { + PyValueError::new_err(format!("expected {N} indices per element; got {}", r.len())) + }) + }) + .collect() +} + +/// A flat list of particle indices, from a 1D integer ndarray or a sequence of ints. +fn extract_indices_1d(obj: &Bound<'_, PyAny>) -> PyResult> { + if let Ok(arr) = obj.extract::>() { + return Ok(arr.as_slice().map(|s| s.to_vec()).unwrap_or_default()); + } + obj.extract::>() + .map_err(|_| PyTypeError::new_err("expected a 1D integer ndarray or a sequence of ints")) +} + +fn vec3(v: rapier::math::Vector) -> Vec3 { + let nav: crate::na::SVector = v.into(); + Vec3(nav) +} + +fn spring(obj: &Bound<'_, PyAny>) -> PyResult> { + if let Ok(s) = obj.extract::>() { + return Ok(s.0); + } + let (f, d): (Real, Real) = obj.extract().map_err(|_| { + PyTypeError::new_err( + "expected a SpringCoefficients or a (natural_frequency, damping_ratio) tuple", + ) + })?; + Ok(rapier::dynamics::SpringCoefficients::new(f, d)) +} + +// ---------------------------------------------------------------------- +// Handle. +// ---------------------------------------------------------------------- + +/// Opaque identifier of a soft body stored in a :class:`SoftBodySet`. +#[pyclass(name = "SoftBodyHandle", module = "rapier", frozen)] +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub struct SoftBodyHandle(pub rapier::dynamics::SoftBodyHandle); + +#[pymethods] +impl SoftBodyHandle { + /// Rebuild a handle from its ``(index, generation)`` parts. + #[staticmethod] + fn from_raw_parts(index: u32, generation: u32) -> Self { + Self(rapier::dynamics::SoftBodyHandle::from_raw_parts( + index, generation, + )) + } + /// The ``(index, generation)`` parts of the handle. + fn into_raw_parts(&self) -> (u32, u32) { + self.0.into_raw_parts() + } + /// Arena slot index. + #[getter] + fn index(&self) -> u32 { + self.0.into_raw_parts().0 + } + /// Arena slot generation. + #[getter] + fn generation(&self) -> u32 { + self.0.into_raw_parts().1 + } + /// An invalid handle, matching no soft body. + #[staticmethod] + fn invalid() -> Self { + Self(rapier::dynamics::SoftBodyHandle::invalid()) + } + fn __repr__(&self) -> String { + let (i, g) = self.0.into_raw_parts(); + format!("SoftBodyHandle(index={i}, generation={g})") + } + fn __hash__(&self) -> u64 { + let (i, g) = self.0.into_raw_parts(); + ((g as u64) << 32) | i as u64 + } + fn __eq__(&self, other: &Self) -> bool { + self.0 == other.0 + } + /// Pickle support. + fn __reduce__(slf: PyRef<'_, Self>) -> PyResult<(Py, (u32, u32))> { + let py = slf.py(); + let cls = py.get_type_bound::(); + let ctor = cls.getattr("from_raw_parts")?; + Ok((ctor.unbind(), slf.0.into_raw_parts())) + } +} + +// ---------------------------------------------------------------------- +// Enums. +// ---------------------------------------------------------------------- + +/// The constitutive model of a soft body's cells (tetrahedra). +#[pyclass(name = "SoftBodyCellModel", module = "rapier", eq, eq_int)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum SoftBodyCellModel { + /// Per-cell volume preservation constraints; the shape is held by the edges. + VOLUME, + /// Corotational linear elasticity: a linear material in the cell's rotated frame. + COROTATIONAL, + /// Neo-Hookean hyperelasticity, which resists inversion and large compressions. + NEO_HOOKEAN, +} + +impl SoftBodyCellModel { + pub(crate) fn to_rapier(self) -> rapier::dynamics::SoftBodyCellModel { + match self { + Self::VOLUME => rapier::dynamics::SoftBodyCellModel::Volume, + Self::COROTATIONAL => rapier::dynamics::SoftBodyCellModel::Corotational, + Self::NEO_HOOKEAN => rapier::dynamics::SoftBodyCellModel::NeoHookean, + } + } + pub(crate) fn from_rapier(m: rapier::dynamics::SoftBodyCellModel) -> Self { + match m { + rapier::dynamics::SoftBodyCellModel::Volume => Self::VOLUME, + rapier::dynamics::SoftBodyCellModel::Corotational => Self::COROTATIONAL, + rapier::dynamics::SoftBodyCellModel::NeoHookean => Self::NEO_HOOKEAN, + } + } +} + +/// Which strains make a soft body's edge rest lengths flow plastically. +#[pyclass(name = "SoftEdgePlasticFlow", module = "rapier", eq, eq_int)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum SoftEdgePlasticFlow { + BOTH, + COMPRESSION, + TENSION, +} + +impl SoftEdgePlasticFlow { + pub(crate) fn to_rapier(self) -> rapier::dynamics::SoftEdgePlasticFlow { + match self { + Self::BOTH => rapier::dynamics::SoftEdgePlasticFlow::Both, + Self::COMPRESSION => rapier::dynamics::SoftEdgePlasticFlow::Compression, + Self::TENSION => rapier::dynamics::SoftEdgePlasticFlow::Tension, + } + } + pub(crate) fn from_rapier(f: rapier::dynamics::SoftEdgePlasticFlow) -> Self { + match f { + rapier::dynamics::SoftEdgePlasticFlow::Both => Self::BOTH, + rapier::dynamics::SoftEdgePlasticFlow::Compression => Self::COMPRESSION, + rapier::dynamics::SoftEdgePlasticFlow::Tension => Self::TENSION, + } + } +} + +/// The kind of a soft body's edge. +#[pyclass(name = "SoftBodyEdgeKind", module = "rapier", eq, eq_int)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum SoftBodyEdgeKind { + /// A structural edge holding the distance between two particles. + STRUCTURAL, + /// A bending edge across two elements, resisting folds. + BEND, +} + +/// What the soft-body tangle recovery does with the contact patches of crossed surfaces. +#[pyclass(name = "SoftPatchConstraints", module = "rapier", eq, eq_int)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum SoftPatchConstraints { + KEEP, + STAND_DOWN, + ALONG_NORMAL, +} + +impl SoftPatchConstraints { + fn to_rapier(self) -> rapier::dynamics::SoftPatchConstraints { + match self { + Self::KEEP => rapier::dynamics::SoftPatchConstraints::Keep, + Self::STAND_DOWN => rapier::dynamics::SoftPatchConstraints::StandDown, + Self::ALONG_NORMAL => rapier::dynamics::SoftPatchConstraints::AlongNormal, + } + } + fn from_rapier(c: rapier::dynamics::SoftPatchConstraints) -> Self { + match c { + rapier::dynamics::SoftPatchConstraints::Keep => Self::KEEP, + rapier::dynamics::SoftPatchConstraints::StandDown => Self::STAND_DOWN, + rapier::dynamics::SoftPatchConstraints::AlongNormal => Self::ALONG_NORMAL, + } + } +} + +// ---------------------------------------------------------------------- +// Material. +// ---------------------------------------------------------------------- + +/// The material of a soft body: the softness of its constraints, the elasticity of its +/// cells, its plasticity and its tearing thresholds. +/// +/// Every field can be passed as a keyword to the constructor:: +/// +/// material = SoftBodyMaterial(young_modulus=2.0e3, poisson_ratio=0.35, tear_strain=0.4) +#[pyclass(name = "SoftBodyMaterial", module = "rapier")] +#[derive(Debug, Clone)] +pub struct SoftBodyMaterial(pub rapier::dynamics::SoftBodyMaterial); + +impl SoftBodyMaterial { + fn apply_kwarg(&mut self, key: &str, v: &Bound<'_, PyAny>) -> PyResult<()> { + let m = &mut self.0; + match key { + "edge_softness" => m.edge_softness = spring(v)?, + "bend_softness" => m.bend_softness = spring(v)?, + "volume_softness" => m.volume_softness = spring(v)?, + "shape_matching_softness" => m.shape_matching_softness = spring(v)?, + "young_modulus" => m.young_modulus = v.extract()?, + "poisson_ratio" => m.poisson_ratio = v.extract()?, + "elastic_damping_ratio" => m.elastic_damping_ratio = v.extract()?, + "plastic_yield" => m.plastic_yield = v.extract()?, + "plastic_creep" => m.plastic_creep = v.extract()?, + "plastic_max" => m.plastic_max = v.extract()?, + "deformation_damping" => m.deformation_damping = v.extract()?, + "edge_plastic_yield" => m.edge_plastic_yield = v.extract()?, + "edge_plastic_creep" => m.edge_plastic_creep = v.extract()?, + "edge_plastic_max" => m.edge_plastic_max = v.extract()?, + "edge_plastic_flow" => { + m.edge_plastic_flow = v.extract::()?.to_rapier() + } + "tear_strain" => m.tear_strain = v.extract()?, + "tear_force" => m.tear_force = v.extract()?, + "tear_smoothing" => m.tear_smoothing = v.extract()?, + "interior_strength" => m.interior_strength = v.extract()?, + "max_tears_per_step" => m.max_tears_per_step = v.extract()?, + "min_piece" => m.min_piece = v.extract()?, + other => { + return Err(PyTypeError::new_err(format!( + "SoftBodyMaterial: unknown keyword argument '{other}'" + ))) + } + } + Ok(()) + } +} + +#[pymethods] +impl SoftBodyMaterial { + /// The default material, with every field given as a keyword overridden. + #[new] + #[pyo3(signature = (**kwargs))] + fn new(kwargs: Option<&Bound<'_, PyDict>>) -> PyResult { + let mut me = Self(rapier::dynamics::SoftBodyMaterial::default()); + if let Some(kw) = kwargs { + for (k, v) in kw.iter() { + let key: String = k.extract()?; + me.apply_kwarg(&key, &v)?; + } + } + Ok(me) + } + + /// A material whose edge, bend, volume and shape-matching softness all take the same + /// value (a :class:`SpringCoefficients` or a ``(frequency, damping)`` tuple). + #[staticmethod] + fn uniform(softness: &Bound<'_, PyAny>) -> PyResult { + Ok(Self(rapier::dynamics::SoftBodyMaterial::uniform(spring( + softness, + )?))) + } + + /// Does the material tear (a strain or force threshold is set)? + fn tears(&self) -> bool { + self.0.tears() + } + + /// The Lamé parameters ``(lambda, mu)`` of the elastic cells. + fn lame_parameters(&self) -> (Real, Real) { + self.0.lame_parameters() + } + + /// Softness of the structural edges. + #[getter] + fn edge_softness(&self) -> SpringCoefficients { + SpringCoefficients(self.0.edge_softness) + } + #[setter] + fn set_edge_softness(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.edge_softness = spring(v)?; + Ok(()) + } + /// Softness of the bending constraints (bend edges and dihedrals). + #[getter] + fn bend_softness(&self) -> SpringCoefficients { + SpringCoefficients(self.0.bend_softness) + } + #[setter] + fn set_bend_softness(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.bend_softness = spring(v)?; + Ok(()) + } + /// Softness of the volume preservation constraints. + #[getter] + fn volume_softness(&self) -> SpringCoefficients { + SpringCoefficients(self.0.volume_softness) + } + #[setter] + fn set_volume_softness(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.volume_softness = spring(v)?; + Ok(()) + } + /// Softness of the shape-matching constraints. + #[getter] + fn shape_matching_softness(&self) -> SpringCoefficients { + SpringCoefficients(self.0.shape_matching_softness) + } + #[setter] + fn set_shape_matching_softness(&mut self, v: &Bound<'_, PyAny>) -> PyResult<()> { + self.0.shape_matching_softness = spring(v)?; + Ok(()) + } + /// Young's modulus of the elastic cells (``COROTATIONAL`` and ``NEO_HOOKEAN`` models). + #[getter] + fn young_modulus(&self) -> Real { + self.0.young_modulus + } + #[setter] + fn set_young_modulus(&mut self, v: Real) { + self.0.young_modulus = v; + } + /// Poisson's ratio of the elastic cells. + #[getter] + fn poisson_ratio(&self) -> Real { + self.0.poisson_ratio + } + #[setter] + fn set_poisson_ratio(&mut self, v: Real) { + self.0.poisson_ratio = v; + } + /// Damping ratio of the elastic cells. + #[getter] + fn elastic_damping_ratio(&self) -> Real { + self.0.elastic_damping_ratio + } + #[setter] + fn set_elastic_damping_ratio(&mut self, v: Real) { + self.0.elastic_damping_ratio = v; + } + /// Strain beyond which the rest shape of an elastic cell flows (``0`` disables it). + #[getter] + fn plastic_yield(&self) -> Real { + self.0.plastic_yield + } + #[setter] + fn set_plastic_yield(&mut self, v: Real) { + self.0.plastic_yield = v; + } + /// Rate (per second) at which a cell's rest shape follows its deformation past the yield. + #[getter] + fn plastic_creep(&self) -> Real { + self.0.plastic_creep + } + #[setter] + fn set_plastic_creep(&mut self, v: Real) { + self.0.plastic_creep = v; + } + /// Largest accumulated plastic deformation of a cell. + #[getter] + fn plastic_max(&self) -> Real { + self.0.plastic_max + } + #[setter] + fn set_plastic_max(&mut self, v: Real) { + self.0.plastic_max = v; + } + /// Damping of the deformation velocity of the elastic cells. + #[getter] + fn deformation_damping(&self) -> Real { + self.0.deformation_damping + } + #[setter] + fn set_deformation_damping(&mut self, v: Real) { + self.0.deformation_damping = v; + } + /// Strain beyond which an edge's rest length flows (``0`` disables it). + #[getter] + fn edge_plastic_yield(&self) -> Real { + self.0.edge_plastic_yield + } + #[setter] + fn set_edge_plastic_yield(&mut self, v: Real) { + self.0.edge_plastic_yield = v; + } + /// Rate (per second) at which an edge's rest length follows its stretch past the yield. + #[getter] + fn edge_plastic_creep(&self) -> Real { + self.0.edge_plastic_creep + } + #[setter] + fn set_edge_plastic_creep(&mut self, v: Real) { + self.0.edge_plastic_creep = v; + } + /// Largest relative change of an edge's rest length. + #[getter] + fn edge_plastic_max(&self) -> Real { + self.0.edge_plastic_max + } + #[setter] + fn set_edge_plastic_max(&mut self, v: Real) { + self.0.edge_plastic_max = v; + } + /// Which strains make the edge rest lengths flow. + #[getter] + fn edge_plastic_flow(&self) -> SoftEdgePlasticFlow { + SoftEdgePlasticFlow::from_rapier(self.0.edge_plastic_flow) + } + #[setter] + fn set_edge_plastic_flow(&mut self, v: SoftEdgePlasticFlow) { + self.0.edge_plastic_flow = v.to_rapier(); + } + /// Strain beyond which an element tears, or ``None``. + #[getter] + fn tear_strain(&self) -> Option { + self.0.tear_strain + } + #[setter] + fn set_tear_strain(&mut self, v: Option) { + self.0.tear_strain = v; + } + /// Force beyond which an element tears, or ``None``. + #[getter] + fn tear_force(&self) -> Option { + self.0.tear_force + } + #[setter] + fn set_tear_force(&mut self, v: Option) { + self.0.tear_force = v; + } + /// Time constant (seconds) of the load smoothing compared against ``tear_force``. + #[getter] + fn tear_smoothing(&self) -> Real { + self.0.tear_smoothing + } + #[setter] + fn set_tear_smoothing(&mut self, v: Real) { + self.0.tear_smoothing = v; + } + /// Multiplier of the tear thresholds of the interior elements. + #[getter] + fn interior_strength(&self) -> Real { + self.0.interior_strength + } + #[setter] + fn set_interior_strength(&mut self, v: Real) { + self.0.interior_strength = v; + } + /// Largest number of elements torn per step. + #[getter] + fn max_tears_per_step(&self) -> u32 { + self.0.max_tears_per_step + } + #[setter] + fn set_max_tears_per_step(&mut self, v: u32) { + self.0.max_tears_per_step = v; + } + /// Smallest piece (in elements) a tear may split off, or ``None`` for the default. + #[getter] + fn min_piece(&self) -> Option { + self.0.min_piece + } + #[setter] + fn set_min_piece(&mut self, v: Option) { + self.0.min_piece = v; + } + + fn __repr__(&self) -> String { + format!( + "SoftBodyMaterial(young_modulus={}, poisson_ratio={}, tear_strain={:?}, tear_force={:?})", + self.0.young_modulus, self.0.poisson_ratio, self.0.tear_strain, self.0.tear_force + ) + } +} + +// ---------------------------------------------------------------------- +// Settings. +// ---------------------------------------------------------------------- + +/// Dynamics settings of a soft body's particles. +#[pyclass(name = "SoftBodyParticleSettings", module = "rapier")] +#[derive(Debug, Clone, Copy)] +pub struct SoftBodyParticleSettings(pub rapier::dynamics::SoftBodyParticleSettings); + +#[pymethods] +impl SoftBodyParticleSettings { + #[new] + fn new() -> Self { + Self(rapier::dynamics::SoftBodyParticleSettings::default()) + } + /// Linear damping of the particles. + #[getter] + fn linear_damping(&self) -> Real { + self.0.linear_damping + } + #[setter] + fn set_linear_damping(&mut self, v: Real) { + self.0.linear_damping = v; + } + /// Gravity scale of the particles. + #[getter] + fn gravity_scale(&self) -> Real { + self.0.gravity_scale + } + #[setter] + fn set_gravity_scale(&mut self, v: Real) { + self.0.gravity_scale = v; + } + /// Extra solver substeps requested for the particles and everything they touch. + #[getter] + fn additional_solver_iterations(&self) -> usize { + self.0.additional_solver_iterations + } + #[setter] + fn set_additional_solver_iterations(&mut self, v: usize) { + self.0.additional_solver_iterations = v; + } + /// Extra internal PGS iterations per substep for the particles and everything they touch + /// (default ``3``). + #[getter] + fn additional_pgs_iterations(&self) -> usize { + self.0.additional_pgs_iterations + } + #[setter] + fn set_additional_pgs_iterations(&mut self, v: usize) { + self.0.additional_pgs_iterations = v; + } + /// Whether the soft body may fall asleep. + #[getter] + fn can_sleep(&self) -> bool { + self.0.can_sleep + } + #[setter] + fn set_can_sleep(&mut self, v: bool) { + self.0.can_sleep = v; + } + /// Dominance group of the soft body. + #[getter] + fn dominance_group(&self) -> i8 { + self.0.dominance_group + } + #[setter] + fn set_dominance_group(&mut self, v: i8) { + self.0.dominance_group = v; + } +} + +/// Runtime toggles and tuning of the soft-body tangle detection and recovery stack. Every +/// mechanism can be switched off individually. +#[pyclass(name = "SoftRecoverySettings", module = "rapier")] +#[derive(Debug, Clone, Copy)] +pub struct SoftRecoverySettings(pub rapier::dynamics::SoftRecoverySettings); + +#[pymethods] +impl SoftRecoverySettings { + #[new] + fn new() -> Self { + Self(rapier::dynamics::SoftRecoverySettings::default()) + } + #[getter] + fn authored_velocity_margin(&self) -> bool { + self.0.authored_velocity_margin + } + #[setter] + fn set_authored_velocity_margin(&mut self, v: bool) { + self.0.authored_velocity_margin = v; + } + #[getter] + fn edge_speculation(&self) -> bool { + self.0.edge_speculation + } + #[setter] + fn set_edge_speculation(&mut self, v: bool) { + self.0.edge_speculation = v; + } + #[getter] + fn inverted_cell_detection(&self) -> bool { + self.0.inverted_cell_detection + } + #[setter] + fn set_inverted_cell_detection(&mut self, v: bool) { + self.0.inverted_cell_detection = v; + } + #[getter] + fn self_crossing_detection(&self) -> bool { + self.0.self_crossing_detection + } + #[setter] + fn set_self_crossing_detection(&mut self, v: bool) { + self.0.self_crossing_detection = v; + } + #[getter] + fn detection_motion_gating(&self) -> bool { + self.0.detection_motion_gating + } + #[setter] + fn set_detection_motion_gating(&mut self, v: bool) { + self.0.detection_motion_gating = v; + } + #[getter] + fn cross_body_detection(&self) -> bool { + self.0.cross_body_detection + } + #[setter] + fn set_cross_body_detection(&mut self, v: bool) { + self.0.cross_body_detection = v; + } + #[getter] + fn self_stand_down(&self) -> bool { + self.0.self_stand_down + } + #[setter] + fn set_self_stand_down(&mut self, v: bool) { + self.0.self_stand_down = v; + } + #[getter] + fn cross_body_expel_gate(&self) -> bool { + self.0.cross_body_expel_gate + } + #[setter] + fn set_cross_body_expel_gate(&mut self, v: bool) { + self.0.cross_body_expel_gate = v; + } + #[getter] + fn edge_stand_down(&self) -> bool { + self.0.edge_stand_down + } + #[setter] + fn set_edge_stand_down(&mut self, v: bool) { + self.0.edge_stand_down = v; + } + #[getter] + fn crossing_repulsion(&self) -> bool { + self.0.crossing_repulsion + } + #[setter] + fn set_crossing_repulsion(&mut self, v: bool) { + self.0.crossing_repulsion = v; + } + #[getter] + fn crossing_repulsion_guide(&self) -> bool { + self.0.crossing_repulsion_guide + } + #[setter] + fn set_crossing_repulsion_guide(&mut self, v: bool) { + self.0.crossing_repulsion_guide = v; + } + #[getter] + fn crossing_repulsion_self_guide(&self) -> bool { + self.0.crossing_repulsion_self_guide + } + #[setter] + fn set_crossing_repulsion_self_guide(&mut self, v: bool) { + self.0.crossing_repulsion_self_guide = v; + } + #[getter] + fn recovery_pace(&self) -> Real { + self.0.recovery_pace + } + #[setter] + fn set_recovery_pace(&mut self, v: Real) { + self.0.recovery_pace = v; + } + #[getter] + fn overlap_constraints(&self) -> bool { + self.0.overlap_constraints + } + #[setter] + fn set_overlap_constraints(&mut self, v: bool) { + self.0.overlap_constraints = v; + } + #[getter] + fn overlap_rigid(&self) -> bool { + self.0.overlap_rigid + } + #[setter] + fn set_overlap_rigid(&mut self, v: bool) { + self.0.overlap_rigid = v; + } + #[getter] + fn overlap_skip_self_tangled(&self) -> bool { + self.0.overlap_skip_self_tangled + } + #[setter] + fn set_overlap_skip_self_tangled(&mut self, v: bool) { + self.0.overlap_skip_self_tangled = v; + } + #[getter] + fn overlap_edge_stand_down(&self) -> bool { + self.0.overlap_edge_stand_down + } + #[setter] + fn set_overlap_edge_stand_down(&mut self, v: bool) { + self.0.overlap_edge_stand_down = v; + } + #[getter] + fn overlap_constraint_pace(&self) -> Real { + self.0.overlap_constraint_pace + } + #[setter] + fn set_overlap_constraint_pace(&mut self, v: Real) { + self.0.overlap_constraint_pace = v; + } + #[getter] + fn overlap_skin_volume(&self) -> bool { + self.0.overlap_skin_volume + } + #[setter] + fn set_overlap_skin_volume(&mut self, v: bool) { + self.0.overlap_skin_volume = v; + } + #[getter] + fn overlap_kept_depth(&self) -> Real { + self.0.overlap_kept_depth + } + #[setter] + fn set_overlap_kept_depth(&mut self, v: Real) { + self.0.overlap_kept_depth = v; + } + #[getter] + fn overlap_self_regions(&self) -> bool { + self.0.overlap_self_regions + } + #[setter] + fn set_overlap_self_regions(&mut self, v: bool) { + self.0.overlap_self_regions = v; + } + #[getter] + fn overlap_normal_push(&self) -> bool { + self.0.overlap_normal_push + } + #[setter] + fn set_overlap_normal_push(&mut self, v: bool) { + self.0.overlap_normal_push = v; + } + #[getter] + fn overlap_multi_volume(&self) -> bool { + self.0.overlap_multi_volume + } + #[setter] + fn set_overlap_multi_volume(&mut self, v: bool) { + self.0.overlap_multi_volume = v; + } + #[getter] + fn overlap_split(&self) -> u32 { + self.0.overlap_split + } + #[setter] + fn set_overlap_split(&mut self, v: u32) { + self.0.overlap_split = v; + } + #[getter] + fn overlap_patience(&self) -> u32 { + self.0.overlap_patience + } + #[setter] + fn set_overlap_patience(&mut self, v: u32) { + self.0.overlap_patience = v; + } + #[getter] + fn overlap_progress_margin(&self) -> Real { + self.0.overlap_progress_margin + } + #[setter] + fn set_overlap_progress_margin(&mut self, v: Real) { + self.0.overlap_progress_margin = v; + } + /// What the recovery does with the contact patches of crossed surfaces. + #[getter] + fn overlap_patch_constraints(&self) -> SoftPatchConstraints { + SoftPatchConstraints::from_rapier(self.0.overlap_patch_constraints) + } + #[setter] + fn set_overlap_patch_constraints(&mut self, v: SoftPatchConstraints) { + self.0.overlap_patch_constraints = v.to_rapier(); + } +} + +/// Simulation settings shared by every soft body of a world; lives on +/// :attr:`IntegrationParameters.soft_bodies`. +#[pyclass(name = "SoftBodiesSettings", module = "rapier")] +#[derive(Debug, Clone, Copy)] +pub struct SoftBodiesSettings(pub rapier::dynamics::SoftBodiesSettings); + +#[pymethods] +impl SoftBodiesSettings { + #[new] + fn new() -> Self { + Self(rapier::dynamics::SoftBodiesSettings::default()) + } + /// The tangle detection and recovery settings (a copy: assign it back to apply). + #[getter] + fn recovery(&self) -> SoftRecoverySettings { + SoftRecoverySettings(self.0.recovery) + } + #[setter] + fn set_recovery(&mut self, v: &SoftRecoverySettings) { + self.0.recovery = v.0; + } + /// Strain beyond which a soft-body constraint is re-solved after the contacts inside every + /// substep (default ``0.75``). + #[getter] + fn resweep_strain(&self) -> Real { + self.0.resweep_strain + } + #[setter] + fn set_resweep_strain(&mut self, v: Real) { + self.0.resweep_strain = v; + } + /// Maximum number of extra substeps a soft body requests while it is hit fast (default + /// ``4``; ``0`` disables them). + #[getter] + fn max_extra_substeps(&self) -> usize { + self.0.max_extra_substeps + } + #[setter] + fn set_max_extra_substeps(&mut self, v: usize) { + self.0.max_extra_substeps = v; + } + /// Factor applied to the contact softness natural frequencies for the soft-body contacts + /// (default ``4.0``). + #[getter] + fn contact_stiffening(&self) -> Real { + self.0.contact_stiffening + } + #[setter] + fn set_contact_stiffening(&mut self, v: Real) { + self.0.contact_stiffening = v; + } +} + +// ---------------------------------------------------------------------- +// Builder. +// ---------------------------------------------------------------------- + +/// Builder of a :class:`SoftBody`: particles in world space, and elements referencing them by +/// index. Start from a generator (:meth:`SoftBody.rope`, :meth:`SoftBody.cloth`, +/// :meth:`SoftBody.cuboid`, :meth:`SoftBody.sphere`, :meth:`SoftBody.trimesh`, +/// :meth:`SoftBody.volumetric`) or from raw positions, then chain the setters, and insert +/// the result with :meth:`SoftBodySet.insert` or :meth:`PhysicsWorld.add_soft_body`. +#[pyclass(name = "SoftBodyBuilder", module = "rapier")] +#[derive(Debug, Clone)] +pub struct SoftBodyBuilder { + pub builder: rapier::dynamics::SoftBodyBuilder, +} + +impl SoftBodyBuilder { + pub fn from_kwargs( + base: rapier::dynamics::SoftBodyBuilder, + kwargs: Option<&Bound<'_, PyDict>>, + ) -> PyResult { + let mut me = Self { builder: base }; + if let Some(kw) = kwargs { + for (k, v) in kw.iter() { + let key: String = k.extract()?; + me.apply_kwarg(&key, &v)?; + } + } + Ok(me) + } + + fn apply_kwarg(&mut self, key: &str, v: &Bound<'_, PyAny>) -> PyResult<()> { + let b = std::mem::take(&mut self.builder); + self.builder = match key { + "particle_mass" => b.particle_mass(v.extract()?), + "mass" => b.mass(v.extract()?), + "masses" => b.masses(v.extract()?), + "pinned_particles" => b.pinned_particles(extract_indices_1d(v)?), + "softness" => b.softness(spring(v)?), + "material" => b.material(v.extract::>()?.0.clone()), + "tear_strain" => b.tear_strain(v.extract()?), + "tear_force" => b.tear_force(v.extract()?), + "min_piece" => b.min_piece(v.extract()?), + "tear_smoothing" => b.tear_smoothing(v.extract()?), + "interior_strength" => b.interior_strength(v.extract()?), + "cell_model" => b.cell_model(v.extract::()?.to_rapier()), + "volume_preservation" => b.volume_preservation(v.extract()?), + "volume_factor" => b.volume_factor(v.extract()?), + "shape_matching" => b.shape_matching(v.extract()?), + "self_contacts" => b.self_contacts(v.extract()?), + "particle_radius" => b.particle_radius(v.extract()?), + "surface_collider" => { + b.surface_collider(v.extract::>()?.builder.clone()) + } + "skin_collision" => b.skin_collision(v.extract()?), + "translation" => b.translated(v.extract::()?.0.into()), + "linear_damping" => b.linear_damping(v.extract()?), + "gravity_scale" => b.gravity_scale(v.extract()?), + "additional_solver_iterations" => b.additional_solver_iterations(v.extract()?), + "additional_pgs_iterations" => b.additional_pgs_iterations(v.extract()?), + "can_sleep" => b.can_sleep(v.extract()?), + "user_data" => b.user_data(v.extract()?), + other => { + self.builder = b; + return Err(PyTypeError::new_err(format!( + "SoftBodyBuilder: unknown keyword argument '{other}'" + ))); + } + }; + Ok(()) + } + + fn chained( + &self, + f: impl FnOnce(rapier::dynamics::SoftBodyBuilder) -> rapier::dynamics::SoftBodyBuilder, + ) -> Self { + Self { + builder: f(self.builder.clone()), + } + } +} + +#[pymethods] +impl SoftBodyBuilder { + /// A builder over raw world-space particle positions (an ``(N, 3)`` ndarray or a + /// sequence of 3-tuples), with no element yet; the setters add them. + #[new] + #[pyo3(signature = (positions, **kwargs))] + fn new(positions: &Bound<'_, PyAny>, kwargs: Option<&Bound<'_, PyDict>>) -> PyResult { + let positions = crate::geometry::extract_verts_for_dim(positions)?; + Self::from_kwargs(rapier::dynamics::SoftBodyBuilder::new(positions), kwargs) + } + + /// The number of particles the builder holds. + #[getter] + fn num_particles(&self) -> usize { + self.builder.positions.len() + } + + /// The world-space particle positions as an ``(N, 3)`` ndarray. + #[getter] + fn positions<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + vectors_to_array(py, self.builder.positions.iter().copied()) + } + + /// The structural edges as an ``(E, 2)`` ndarray. + #[getter] + fn current_edges<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + elements_to_array(py, &self.builder.edges) + } + + /// The cells (tetrahedra) as a ``(C, 4)`` ndarray. + #[getter] + fn current_cells<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + elements_to_array(py, &self.builder.cells) + } + + /// The boundary triangles as a ``(B, 3)`` ndarray (empty when derived from the cells at + /// build time). + #[getter] + fn current_surface<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + elements_to_array(py, &self.builder.surface) + } + + /// The material (a copy). + #[getter] + fn current_material(&self) -> SoftBodyMaterial { + SoftBodyMaterial(self.builder.material.clone()) + } + + /// The edges of the surface elements, deduplicated, as an ``(E, 2)`` ndarray. + fn surface_edges<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + elements_to_array(py, &self.builder.surface_edges()) + } + + /// The edges of the cells, deduplicated, as an ``(E, 2)`` ndarray. + fn cell_edges<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + elements_to_array(py, &self.builder.cell_edges()) + } + + /// The dihedrals across the surface's interior edges, as a ``(D, 4)`` ndarray. + fn surface_dihedrals<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + elements_to_array(py, &self.builder.surface_dihedrals()) + } + + /// Replace the particle positions. + fn set_positions(&self, positions: &Bound<'_, PyAny>) -> PyResult { + let positions = crate::geometry::extract_verts_for_dim(positions)?; + Ok(self.chained(|b| b.positions(positions))) + } + /// Set the uniform mass of the particles. + fn particle_mass(&self, mass: Real) -> Self { + self.chained(|b| b.particle_mass(mass)) + } + /// Set the total mass of the body, spread over its particles. + fn mass(&self, mass: Real) -> Self { + self.chained(|b| b.mass(mass)) + } + /// Set per-particle masses. + fn masses(&self, masses: Vec) -> Self { + self.chained(|b| b.masses(masses)) + } + /// Pin the given particles in place. + fn pinned_particles(&self, pinned: &Bound<'_, PyAny>) -> PyResult { + let pinned = extract_indices_1d(pinned)?; + Ok(self.chained(|b| b.pinned_particles(pinned))) + } + /// Replace the structural edges (an ``(E, 2)`` ndarray or a sequence of pairs). + fn edges(&self, edges: &Bound<'_, PyAny>) -> PyResult { + let edges = extract_elements::<2>(edges)?; + Ok(self.chained(|b| b.edges(edges))) + } + /// Add structural edges. + fn add_edges(&self, edges: &Bound<'_, PyAny>) -> PyResult { + let edges = extract_elements::<2>(edges)?; + Ok(self.chained(|b| b.add_edges(edges))) + } + /// Replace the bending edges. + fn bend_edges(&self, edges: &Bound<'_, PyAny>) -> PyResult { + let edges = extract_elements::<2>(edges)?; + Ok(self.chained(|b| b.bend_edges(edges))) + } + /// Make every edge resist stretching only (a rope or a net that folds freely). + fn tension_only(&self) -> Self { + self.chained(|b| b.tension_only()) + } + /// Replace the dihedral bending constraints (``(D, 4)``: the shared edge, then the two + /// opposite vertices). + fn dihedrals(&self, dihedrals: &Bound<'_, PyAny>) -> PyResult { + let dihedrals = extract_elements::<4>(dihedrals)?; + Ok(self.chained(|b| b.dihedrals(dihedrals))) + } + /// Replace the cells (``(C, 4)`` tetrahedra). + fn cells(&self, cells: &Bound<'_, PyAny>) -> PyResult { + let cells = extract_elements::<4>(cells)?; + Ok(self.chained(|b| b.cells(cells))) + } + /// Replace the boundary triangles (``(B, 3)``), oriented outward. + fn surface(&self, surface: &Bound<'_, PyAny>) -> PyResult { + let surface = extract_elements::<3>(surface)?; + Ok(self.chained(|b| b.surface(surface))) + } + /// Set a skin: a finer mesh (world-space vertices and triangles) embedded in the cells. + fn skin(&self, vertices: &Bound<'_, PyAny>, indices: &Bound<'_, PyAny>) -> PyResult { + let vertices = crate::geometry::extract_verts_for_dim(vertices)?; + let indices = extract_elements::<3>(indices)?; + Ok(self.chained(|b| b.skin(vertices, indices))) + } + /// Make the body collide through its skin rather than through its cells' boundary. + fn skin_collision(&self, enabled: bool) -> Self { + self.chained(|b| b.skin_collision(enabled)) + } + /// Set the segments a body without surface collides through (a wire). + fn wire(&self, segments: &Bound<'_, PyAny>) -> PyResult { + let segments = extract_elements::<2>(segments)?; + Ok(self.chained(|b| b.wire(segments))) + } + /// Set the material. + fn material(&self, material: &SoftBodyMaterial) -> Self { + let m = material.0.clone(); + self.chained(|b| b.material(m)) + } + /// Set a uniform softness (a :class:`SpringCoefficients` or a ``(frequency, damping)`` + /// tuple) for every constraint of the material. + fn softness(&self, softness: &Bound<'_, PyAny>) -> PyResult { + let s = spring(softness)?; + Ok(self.chained(|b| b.softness(s))) + } + /// Strain beyond which an element tears. + fn tear_strain(&self, strain: Real) -> Self { + self.chained(|b| b.tear_strain(strain)) + } + /// Force beyond which an element tears. + fn tear_force(&self, force: Real) -> Self { + self.chained(|b| b.tear_force(force)) + } + /// Smallest piece (in elements) a tear may split off. + fn min_piece(&self, elements: u32) -> Self { + self.chained(|b| b.min_piece(elements)) + } + /// Time constant (seconds) of the load smoothing compared against the tear force. + fn tear_smoothing(&self, seconds: Real) -> Self { + self.chained(|b| b.tear_smoothing(seconds)) + } + /// Multiplier of the tear thresholds of the interior elements. + fn interior_strength(&self, strength: Real) -> Self { + self.chained(|b| b.interior_strength(strength)) + } + /// Multiply the tear thresholds of the given edges: ``resistances`` maps edge indices to + /// multipliers. + fn edge_tear_resistance(&self, resistances: Vec<(u32, Real)>) -> Self { + self.chained(|b| b.edge_tear_resistance(resistances)) + } + /// Set the constitutive model of the cells. + fn cell_model(&self, model: SoftBodyCellModel) -> Self { + self.chained(|b| b.cell_model(model.to_rapier())) + } + /// Enable the preservation of the volume enclosed by the body's closed surfaces. + fn volume_preservation(&self, enabled: bool) -> Self { + self.chained(|b| b.volume_preservation(enabled)) + } + /// Set the target volume multiplier (``> 1`` inflates the body); this also enables the + /// volume preservation. + fn volume_factor(&self, factor: Real) -> Self { + self.chained(|b| b.volume_factor(factor)) + } + /// Hold the body's shape by shape matching. + fn shape_matching(&self, enabled: bool) -> Self { + self.chained(|b| b.shape_matching(enabled)) + } + /// Make the body's surface collide with itself. + fn self_contacts(&self, enabled: bool) -> Self { + self.chained(|b| b.self_contacts(enabled)) + } + /// Set the thickness of the particles. + fn particle_radius(&self, radius: Real) -> Self { + self.chained(|b| b.particle_radius(radius)) + } + /// Set the template of the body's colliders: its friction, groups, events and other + /// settings are kept, its shape is replaced by the body's deformable surface. + fn surface_collider(&self, collider: &ColliderBuilder) -> Self { + let c = collider.builder.clone(); + self.chained(|b| b.surface_collider(c)) + } + /// Remove the body's colliders: it will not collide with anything. + fn no_surface_collider(&self) -> Self { + self.chained(|b| b.no_surface_collider()) + } + /// Translate every particle. + fn translated(&self, translation: PyVector) -> Self { + let t: rapier::math::Vector = translation.0.into(); + self.chained(|b| b.translated(t)) + } + /// Set the dynamics settings of the particles. + fn particle_settings(&self, settings: &SoftBodyParticleSettings) -> Self { + let s = settings.0; + self.chained(|b| b.particle_settings(s)) + } + /// Set the linear damping of the particles. + fn linear_damping(&self, damping: Real) -> Self { + self.chained(|b| b.linear_damping(damping)) + } + /// Set the gravity scale of the particles. + fn gravity_scale(&self, scale: Real) -> Self { + self.chained(|b| b.gravity_scale(scale)) + } + /// Extra solver substeps requested for the body and everything it touches. + fn additional_solver_iterations(&self, iterations: usize) -> Self { + self.chained(|b| b.additional_solver_iterations(iterations)) + } + /// Extra internal PGS iterations per substep for the body and everything it touches + /// (default ``3``). + fn additional_pgs_iterations(&self, iterations: usize) -> Self { + self.chained(|b| b.additional_pgs_iterations(iterations)) + } + /// Whether the body may fall asleep. + fn can_sleep(&self, can_sleep: bool) -> Self { + self.chained(|b| b.can_sleep(can_sleep)) + } + /// Arbitrary integer user data. + fn user_data(&self, data: u128) -> Self { + self.chained(|b| b.user_data(data)) + } + /// Append the particles and elements of another builder. + fn append(&self, other: &SoftBodyBuilder) -> Self { + let o = other.builder.clone(); + self.chained(|b| b.append(o)) + } + + fn __repr__(&self) -> String { + format!( + "SoftBodyBuilder(particles={}, edges={}, cells={})", + self.builder.positions.len(), + self.builder.edges.len(), + self.builder.cells.len() + ) + } +} + +// ---------------------------------------------------------------------- +// Element snapshots. +// ---------------------------------------------------------------------- + +/// A snapshot of one particle of a soft body. +#[pyclass(name = "SoftBodyParticle", module = "rapier", frozen)] +#[derive(Debug, Clone, Copy)] +pub struct SoftBodyParticle(pub rapier::dynamics::SoftBodyParticle); + +#[pymethods] +impl SoftBodyParticle { + /// World-space position. + #[getter] + fn position(&self) -> Vec3 { + vec3(self.0.position()) + } + /// Velocity. + #[getter] + fn velocity(&self) -> Vec3 { + vec3(self.0.velocity()) + } + /// User force accumulated for the next step. + #[getter] + fn force(&self) -> Vec3 { + vec3(self.0.force()) + } + /// The position the particle is driven to over the next step, if any. + #[getter] + fn kinematic_target(&self) -> Option { + self.0.kinematic_target().map(vec3) + } + /// The rest position (the position its constraints hold it at). + #[getter] + fn rest_position(&self) -> Vec3 { + vec3(self.0.rest_position()) + } + /// The rest position before any plastic flow. + #[getter] + fn initial_rest_position(&self) -> Vec3 { + vec3(self.0.initial_rest_position()) + } + /// Mass. + #[getter] + fn mass(&self) -> Real { + self.0.mass() + } + /// Inverse mass (``0`` for a pinned particle). + #[getter] + fn inv_mass(&self) -> Real { + self.0.inv_mass() + } + /// Is the particle pinned? + #[getter] + fn is_pinned(&self) -> bool { + self.0.is_pinned() + } + /// Has an element of the particle torn? + #[getter] + fn is_damaged(&self) -> bool { + self.0.is_damaged() + } + /// Does the particle lie on the body's surface? + #[getter] + fn is_on_surface(&self) -> bool { + self.0.is_on_surface() + } + fn __repr__(&self) -> String { + let p = self.0.position(); + format!( + "SoftBodyParticle(position=({}, {}, {}), mass={})", + p.x, + p.y, + p.z, + self.0.mass() + ) + } +} + +/// A snapshot of one edge of a soft body. +#[pyclass(name = "SoftBodyEdge", module = "rapier", frozen)] +#[derive(Debug, Clone, Copy)] +pub struct SoftBodyEdge(pub rapier::dynamics::SoftBodyEdge); + +#[pymethods] +impl SoftBodyEdge { + /// The two particles. + #[getter] + fn vertices(&self) -> (u32, u32) { + (self.0.vertices[0], self.0.vertices[1]) + } + /// Rest length. + #[getter] + fn rest_length(&self) -> Real { + self.0.rest_length + } + /// Structural or bending. + #[getter] + fn kind(&self) -> SoftBodyEdgeKind { + match self.0.kind { + rapier::dynamics::SoftBodyEdgeKind::Structural => SoftBodyEdgeKind::STRUCTURAL, + rapier::dynamics::SoftBodyEdgeKind::Bend => SoftBodyEdgeKind::BEND, + } + } + /// Does the edge resist stretching only? + #[getter] + fn tension_only(&self) -> bool { + self.0.tension_only + } + /// Per-edge softness override, if any. + #[getter] + fn softness(&self) -> Option { + self.0.softness.map(SpringCoefficients) + } + /// Multiplier of the tear thresholds. + #[getter] + fn tear_resistance(&self) -> Real { + self.0.tear_resistance + } + /// Impulse applied during the last step. + #[getter] + fn impulse(&self) -> Real { + self.0.impulse() + } + /// Load relative to the tear threshold (``1`` tears). + #[getter] + fn stress(&self) -> Real { + self.0.stress() + } + /// Relative change of the rest length due to plastic flow. + #[getter] + fn plastic_strain(&self) -> Real { + self.0.plastic_strain() + } + /// The rest length before any plastic flow. + #[getter] + fn initial_rest_length(&self) -> Real { + self.0.initial_rest_length() + } + fn __repr__(&self) -> String { + format!( + "SoftBodyEdge(vertices=({}, {}), rest_length={})", + self.0.vertices[0], self.0.vertices[1], self.0.rest_length + ) + } +} + +/// A snapshot of one dihedral bending constraint of a soft body. +#[pyclass(name = "SoftBodyDihedral", module = "rapier", frozen)] +#[derive(Debug, Clone, Copy)] +pub struct SoftBodyDihedral(pub rapier::dynamics::SoftBodyDihedral); + +#[pymethods] +impl SoftBodyDihedral { + /// The four particles: the shared edge, then the two opposite vertices. + #[getter] + fn vertices(&self) -> (u32, u32, u32, u32) { + let v = self.0.vertices; + (v[0], v[1], v[2], v[3]) + } + /// The rest angle between the two triangle normals. + #[getter] + fn rest_angle(&self) -> Real { + self.0.rest_angle + } + /// The permanent set of the rest angle due to plastic flow, in radians. + #[getter] + fn plastic_set(&self) -> Real { + self.0.plastic_set() + } + /// The rest angle before any plastic flow. + #[getter] + fn initial_rest_angle(&self) -> Real { + self.0.initial_rest_angle() + } +} + +/// A snapshot of one cell (tetrahedron) of a soft body. +#[pyclass(name = "SoftBodyCell", module = "rapier", frozen)] +#[derive(Debug, Clone, Copy)] +pub struct SoftBodyCell(pub rapier::dynamics::SoftBodyCell); + +#[pymethods] +impl SoftBodyCell { + /// The four particles. + #[getter] + fn vertices(&self) -> (u32, u32, u32, u32) { + let v = self.0.vertices; + (v[0], v[1], v[2], v[3]) + } + /// Rest volume. + #[getter] + fn rest_volume(&self) -> Real { + self.0.rest_volume + } + /// Stiffness multiplier. + #[getter] + fn stiffness_scale(&self) -> Real { + self.0.stiffness_scale + } + /// Multiplier of the tear thresholds. + #[getter] + fn tear_resistance(&self) -> Real { + self.0.tear_resistance + } + /// Load relative to the tear threshold (``1`` tears). + #[getter] + fn stress(&self) -> Real { + self.0.stress() + } + /// The accumulated plastic stretch of the rest shape, as a ``(3, 3)`` ndarray. + fn plastic_stretch<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + let m = self.0.plastic_stretch(); + let rows: Vec> = (0..3) + .map(|i| (0..3).map(|j| m.col(j)[i]).collect()) + .collect(); + PyArray2::from_vec2_bound(py, &rows).expect("contiguous ndarray") + } +} + +/// A particle attached to a rigid body. +#[pyclass(name = "SoftParticleAttachment", module = "rapier", frozen)] +#[derive(Debug, Clone, Copy)] +pub struct SoftParticleAttachment(pub rapier::dynamics::SoftParticleAttachment); + +#[pymethods] +impl SoftParticleAttachment { + /// The attached particle. + #[getter] + fn particle(&self) -> u32 { + self.0.particle + } + /// The rigid body it follows. + #[getter] + fn body(&self) -> RigidBodyHandle { + RigidBodyHandle(self.0.body) + } + /// The attachment point in the body's local frame. + #[getter] + fn local_anchor(&self) -> Vec3 { + vec3(self.0.local_anchor) + } + /// The impulse the attachment applied during the last step. + #[getter] + fn impulse(&self) -> Vec3 { + vec3(self.0.impulse()) + } +} + +/// A piece of material enclosed by a closed surface, whose volume is preserved. +#[pyclass(name = "SoftVolumePiece", module = "rapier", frozen)] +#[derive(Debug, Clone)] +pub struct SoftVolumePiece { + #[pyo3(get)] + elements: Vec, + #[pyo3(get)] + particles: Vec, + #[pyo3(get)] + rest_volume: Real, + #[pyo3(get)] + volume: Real, +} + +// ---------------------------------------------------------------------- +// Clusters and collision meshes. +// ---------------------------------------------------------------------- + +/// A snapshot of a soft body's cluster: a set of particles with a rigid proxy that joints and +/// colliders attach to. +#[pyclass(name = "SoftBodyCluster", module = "rapier", frozen)] +#[derive(Debug, Clone)] +pub struct SoftBodyCluster { + /// The cluster's index in its soft body. + #[pyo3(get)] + index: u32, + /// The particles of the cluster. + #[pyo3(get)] + particles: Vec, + /// The rigid-body proxy standing for the cluster. + #[pyo3(get)] + proxy: RigidBodyHandle, + /// Does the cluster still exist? + #[pyo3(get)] + is_live: bool, + /// Does shape matching hold the cluster's shape? + #[pyo3(get)] + shape_matching_enabled: bool, + /// The identifiers of the collision meshes the cluster holds. + #[pyo3(get)] + meshes: Vec, +} + +#[pymethods] +impl SoftBodyCluster { + fn __repr__(&self) -> String { + format!( + "SoftBodyCluster(index={}, particles={}, live={})", + self.index, + self.particles.len(), + self.is_live + ) + } +} + +/// The identifier of a collision mesh within its soft body: the cluster holding it and its +/// index in that cluster. +#[pyclass(name = "SoftMeshId", module = "rapier", frozen)] +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub struct SoftMeshId(pub rapier::dynamics::SoftMeshId); + +#[pymethods] +impl SoftMeshId { + #[new] + fn new(cluster: u32, mesh: u32) -> Self { + Self(rapier::dynamics::SoftMeshId { cluster, mesh }) + } + #[getter] + fn cluster(&self) -> u32 { + self.0.cluster + } + #[getter] + fn mesh(&self) -> u32 { + self.0.mesh + } + fn __repr__(&self) -> String { + format!( + "SoftMeshId(cluster={}, mesh={})", + self.0.cluster, self.0.mesh + ) + } + fn __eq__(&self, other: &Self) -> bool { + self.0 == other.0 + } + fn __hash__(&self) -> u64 { + ((self.0.cluster as u64) << 32) | self.0.mesh as u64 + } +} + +/// A collision mesh of a soft body (a deformable collider), by body and identifier. +#[pyclass(name = "SoftMeshRef", module = "rapier", frozen)] +#[derive(Debug, Clone, Copy)] +pub struct SoftMeshRef(pub rapier::dynamics::SoftMeshRef); + +#[pymethods] +impl SoftMeshRef { + /// The soft body holding the mesh. + #[getter] + fn body(&self) -> SoftBodyHandle { + SoftBodyHandle(self.0.body) + } + /// The mesh's identifier within the body. + #[getter] + fn id(&self) -> SoftMeshId { + SoftMeshId(self.0.id) + } + fn __repr__(&self) -> String { + format!( + "SoftMeshRef(body={:?}, cluster={}, mesh={})", + self.0.body.into_raw_parts(), + self.0.id.cluster, + self.0.id.mesh + ) + } +} + +/// A snapshot of a soft body's collision mesh: the triangles a cluster collides through, with +/// their current world-space vertex positions. +#[pyclass(name = "SoftCollisionMesh", module = "rapier", frozen)] +#[derive(Debug, Clone)] +pub struct SoftCollisionMesh { + /// The mesh's identifier within its body. + #[pyo3(get)] + id: SoftMeshId, + /// The collider holding the mesh. + #[pyo3(get)] + collider: ColliderHandle, + /// Does the mesh collide? + #[pyo3(get)] + collision_enabled: bool, + /// Is the mesh embedded in the cells (rather than bound vertex-to-particle)? + #[pyo3(get)] + is_skinned: bool, + /// Is the surface closed? + #[pyo3(get)] + is_closed: bool, + /// Is the mesh made of segments (a wire)? + #[pyo3(get)] + is_wire: bool, + /// Does the mesh collide with itself? + #[pyo3(get)] + self_contacts_enabled: bool, + vertices: Vec, + indices: Vec>, +} + +#[pymethods] +impl SoftCollisionMesh { + /// The number of vertices. + #[getter] + fn vertex_count(&self) -> usize { + self.vertices.len() + } + /// The world-space vertex positions as an ``(N, 3)`` ndarray. + #[getter] + fn vertices<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + vectors_to_array(py, self.vertices.iter().copied()) + } + /// The elements as an ``(M, 3)`` ndarray (``(M, 2)`` for a wire). + #[getter] + fn indices<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + let cols = self.indices.first().map_or(3, |e| e.len()); + if self.indices.is_empty() { + PyArray2::zeros_bound(py, [0, cols], false) + } else { + PyArray2::from_vec2_bound(py, &self.indices).expect("contiguous ndarray") + } + } + fn __repr__(&self) -> String { + format!( + "SoftCollisionMesh(id={:?}, vertices={}, elements={})", + (self.id.0.cluster, self.id.0.mesh), + self.vertices.len(), + self.indices.len() + ) + } +} + +impl SoftCollisionMesh { + fn from_rapier( + body: &rapier::dynamics::SoftBody, + mesh: &rapier::dynamics::SoftCollisionMesh, + ) -> Self { + let indices = if mesh.is_wire() { + (0..mesh.indices().len()) + .map(|i| mesh.element(i).to_vec()) + .collect() + } else { + mesh.indices().iter().map(|e| e.to_vec()).collect() + }; + Self { + id: SoftMeshId(mesh.id()), + collider: ColliderHandle(mesh.collider()), + collision_enabled: mesh.collision_enabled(), + is_skinned: mesh.is_skinned(), + is_closed: mesh.is_closed(), + is_wire: mesh.is_wire(), + self_contacts_enabled: mesh.self_contacts_enabled(), + vertices: mesh.vertex_positions(body).collect(), + indices, + } + } +} + +/// How a deformable collider's vertices follow the particles of its cluster (see +/// :meth:`ColliderSet.insert_deformable`). +#[pyclass(name = "SoftMeshBinding", module = "rapier")] +#[derive(Debug, Clone)] +pub struct SoftMeshBinding(pub rapier::dynamics::SoftMeshBinding); + +#[pymethods] +impl SoftMeshBinding { + /// Bind vertex ``i`` of the mesh to ``particles[i]``, which must belong to the cluster. + #[staticmethod] + fn direct(particles: &Bound<'_, PyAny>) -> PyResult { + Ok(Self(rapier::dynamics::SoftMeshBinding::direct( + extract_indices_1d(particles)?, + ))) + } + /// Bind every vertex to the particle of the cluster closest to it, within ``eps``. + #[staticmethod] + fn direct_by_position(eps: Real) -> Self { + Self(rapier::dynamics::SoftMeshBinding::direct_by_position(eps)) + } + /// Bind every vertex to the cell of the cluster holding it (cage simulation). + #[staticmethod] + fn skinned() -> Self { + Self(rapier::dynamics::SoftMeshBinding::skinned()) + } + /// Enable collisions of the mesh with itself. + fn self_contacts(&self, enabled: bool) -> Self { + Self(self.0.clone().self_contacts(enabled)) + } +} + +// ---------------------------------------------------------------------- +// Tear events. +// ---------------------------------------------------------------------- + +/// A piece a tear split off into a soft body of its own. +#[pyclass(name = "SoftBodyPiece", module = "rapier", frozen)] +#[derive(Debug, Clone)] +pub struct SoftBodyPiece(pub rapier::dynamics::SoftBodyPiece); + +#[pymethods] +impl SoftBodyPiece { + /// The soft body the piece became. + #[getter] + fn soft_body(&self) -> SoftBodyHandle { + SoftBodyHandle(self.0.soft_body) + } + /// The particles (indices in the torn body) that went into the piece. + #[getter] + fn particles(&self) -> Vec { + self.0.particles.clone() + } + /// The clusters that moved into the piece, as ``(source, destination)`` index pairs. + #[getter] + fn clusters(&self) -> Vec<(u32, u32)> { + self.0.clusters.iter().map(|c| (c[0], c[1])).collect() + } +} + +/// A cluster split by a tear. +#[pyclass(name = "SoftClusterSplit", module = "rapier", frozen)] +#[derive(Debug, Clone, Copy)] +pub struct SoftClusterSplit(pub rapier::dynamics::SoftClusterSplit); + +#[pymethods] +impl SoftClusterSplit { + /// The cluster of the torn body the split came from. + #[getter] + fn source_cluster(&self) -> u32 { + self.0.source_cluster + } + /// The soft body holding the new cluster. + #[getter] + fn soft_body(&self) -> SoftBodyHandle { + SoftBodyHandle(self.0.soft_body) + } + /// The index of the new cluster. + #[getter] + fn cluster(&self) -> u32 { + self.0.cluster + } + /// The proxy of the new cluster. + #[getter] + fn proxy(&self) -> RigidBodyHandle { + RigidBodyHandle(self.0.proxy) + } + /// Does the new cluster keep the source cluster's proxy? + #[getter] + fn keeps_proxy(&self) -> bool { + self.0.keeps_proxy + } +} + +/// An impulse joint a tear moved to another proxy. +#[pyclass(name = "SoftJointMove", module = "rapier", frozen)] +#[derive(Debug, Clone, Copy)] +pub struct SoftJointMove(pub rapier::dynamics::SoftJointMove); + +#[pymethods] +impl SoftJointMove { + #[getter] + fn joint(&self) -> ImpulseJointHandle { + ImpulseJointHandle(self.0.joint) + } + #[getter] + fn from_body(&self) -> RigidBodyHandle { + RigidBodyHandle(self.0.from) + } + #[getter] + fn to_body(&self) -> RigidBodyHandle { + RigidBodyHandle(self.0.to) + } +} + +/// The record of a soft body tearing: the elements it lost, the particles the tear split, +/// the pieces that became soft bodies of their own, and the clusters and joints that moved +/// with them. +#[pyclass(name = "SoftBodyTearEvent", module = "rapier", frozen)] +#[derive(Debug, Clone)] +pub struct SoftBodyTearEvent(pub rapier::dynamics::SoftBodyTearEvent); + +#[pymethods] +impl SoftBodyTearEvent { + /// The soft body that tore. + #[getter] + fn soft_body(&self) -> SoftBodyHandle { + SoftBodyHandle(self.0.soft_body) + } + /// The particle pairs of the edges that tore, as an ``(E, 2)`` ndarray. + #[getter] + fn torn_edges<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + elements_to_array(py, &self.0.torn_edges) + } + /// The particles of the cells that tore, as a ``(C, 4)`` ndarray. + #[getter] + fn torn_cells<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + elements_to_array(py, &self.0.torn_cells) + } + /// The particle pairs of the edges the tear removed, as an ``(E, 2)`` ndarray. + #[getter] + fn removed_edges<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + elements_to_array(py, &self.0.removed_edges) + } + /// The particles the tear split, as ``(original, copy)`` pairs. + #[getter] + fn split_particles(&self) -> Vec<(u32, u32)> { + self.0.split_particles.clone() + } + /// The particles the tear inserted. + #[getter] + fn inserted_particles(&self) -> Vec { + self.0.inserted_particles.clone() + } + /// The pieces the tear split off into soft bodies of their own. + #[getter] + fn pieces(&self) -> Vec { + self.0.pieces.iter().cloned().map(SoftBodyPiece).collect() + } + /// The clusters the tear split. + #[getter] + fn clusters(&self) -> Vec { + self.0 + .clusters + .iter() + .copied() + .map(SoftClusterSplit) + .collect() + } + /// The impulse joints the tear moved to another proxy. + #[getter] + fn moved_joints(&self) -> Vec { + self.0 + .moved_joints + .iter() + .copied() + .map(SoftJointMove) + .collect() + } + /// The soft bodies touched by the tear: the torn body and its new pieces. + fn bodies(&self) -> Vec { + self.0.bodies().map(SoftBodyHandle).collect() + } + /// Where a particle of the torn body is after the tear: ``(soft_body, index)``, or + /// ``None`` if it was removed. + fn particle_destination(&self, particle: u32) -> Option<(SoftBodyHandle, u32)> { + self.0 + .particle_destination(particle) + .map(|(h, i)| (SoftBodyHandle(h), i)) + } + /// The particle pairs the tear started from, as an ``(S, 2)`` ndarray. + fn seeds<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + elements_to_array(py, &self.0.seeds()) + } + fn __repr__(&self) -> String { + format!( + "SoftBodyTearEvent(soft_body={:?}, torn_edges={}, torn_cells={}, pieces={})", + self.0.soft_body.into_raw_parts(), + self.0.torn_edges.len(), + self.0.torn_cells.len(), + self.0.pieces.len() + ) + } +} + +// ---------------------------------------------------------------------- +// SoftBody. +// ---------------------------------------------------------------------- + +/// Storage backing a `SoftBody`: an owned body (removed from a set) or a handle-backed view +/// into a `SoftBodySet`. +#[derive(Debug)] +pub enum SoftBodyBacking { + Owned(Box), + InSet { + set: Py, + handle: rapier::dynamics::SoftBodyHandle, + }, +} + +impl Clone for SoftBodyBacking { + fn clone(&self) -> Self { + match self { + SoftBodyBacking::Owned(b) => SoftBodyBacking::Owned(b.clone()), + SoftBodyBacking::InSet { set, handle } => { + Python::with_gil(|py| SoftBodyBacking::InSet { + set: set.clone_ref(py), + handle: *handle, + }) + } + } + } +} + +/// A soft body: particles linked by edges, bending constraints and cells, simulated together +/// with the rigid bodies, contacts and joints of a world. +/// +/// Instances returned by :class:`SoftBodySet` are live **views**: reads and writes go straight +/// through to the set. The static constructors (:meth:`rope`, :meth:`cloth`, ...) return a +/// :class:`SoftBodyBuilder` to insert with :meth:`SoftBodySet.insert` or +/// :meth:`PhysicsWorld.add_soft_body`. +#[pyclass(name = "SoftBody", module = "rapier")] +#[derive(Debug, Clone)] +pub struct SoftBody { + pub backing: SoftBodyBacking, +} + +impl SoftBody { + pub(crate) fn new_owned(body: rapier::dynamics::SoftBody) -> Self { + SoftBody { + backing: SoftBodyBacking::Owned(Box::new(body)), + } + } + + fn with_ref(&self, f: impl FnOnce(&rapier::dynamics::SoftBody) -> R) -> R { + match &self.backing { + SoftBodyBacking::Owned(b) => f(b), + SoftBodyBacking::InSet { set, handle } => Python::with_gil(|py| { + let set = set.bind(py).borrow(); + let body = set + .0 + .get(*handle) + .expect("SoftBody refers to a body that was removed from its set"); + f(body) + }), + } + } + + fn with_mut(&mut self, f: impl FnOnce(&mut rapier::dynamics::SoftBody) -> R) -> R { + match &mut self.backing { + SoftBodyBacking::Owned(b) => f(b), + SoftBodyBacking::InSet { set, handle } => Python::with_gil(|py| { + let mut set = set.bind(py).borrow_mut(); + let body = set + .0 + .get_mut(*handle) + .expect("SoftBody refers to a body that was removed from its set"); + f(body) + }), + } + } + + fn check_particle(&self, i: usize) -> PyResult<()> { + let n = self.with_ref(|b| b.num_particles()); + if i < n { + Ok(()) + } else { + Err(PyIndexError::new_err(format!( + "particle index {i} out of range (the body has {n} particles)" + ))) + } + } +} + +#[pymethods] +impl SoftBody { + /* + * Generators: each returns a builder, with the builder's keywords accepted. + */ + + /// A rope of ``num_particles`` particles from ``start`` to ``end``. + #[staticmethod] + #[pyo3(signature = (start, end, num_particles, **kwargs))] + fn rope( + start: PyVector, + end: PyVector, + num_particles: usize, + kwargs: Option<&Bound<'_, PyDict>>, + ) -> PyResult { + SoftBodyBuilder::from_kwargs( + rapier::dynamics::SoftBodyBuilder::rope(start.0.into(), end.0.into(), num_particles), + kwargs, + ) + } + + /// A cloth of ``nu`` by ``nv`` particles: particle ``(i, j)`` is at + /// ``origin + i * du + j * dv``. + #[staticmethod] + #[pyo3(signature = (origin, du, dv, nu, nv, **kwargs))] + fn cloth( + origin: PyVector, + du: PyVector, + dv: PyVector, + nu: usize, + nv: usize, + kwargs: Option<&Bound<'_, PyDict>>, + ) -> PyResult { + SoftBodyBuilder::from_kwargs( + rapier::dynamics::SoftBodyBuilder::cloth( + origin.0.into(), + du.0.into(), + dv.0.into(), + nu, + nv, + ), + kwargs, + ) + } + + /// A tube of cloth around ``axis``, from ``radius_start`` at ``origin`` to ``radius_end`` + /// at its other end, with ``num_around`` particles per ring and ``num_along`` rings. + #[staticmethod] + #[pyo3(signature = (origin, axis, radius_start, radius_end, num_around, num_along, **kwargs))] + #[allow(clippy::too_many_arguments)] + fn cloth_tube( + origin: PyVector, + axis: PyVector, + radius_start: Real, + radius_end: Real, + num_around: usize, + num_along: usize, + kwargs: Option<&Bound<'_, PyDict>>, + ) -> PyResult { + SoftBodyBuilder::from_kwargs( + rapier::dynamics::SoftBodyBuilder::cloth_tube( + origin.0.into(), + axis.0.into(), + radius_start, + radius_end, + num_around, + num_along, + ), + kwargs, + ) + } + + /// A cloth with different softness along ``du`` (warp), along ``dv`` (weft) and across the + /// diagonals (shear); each softness is a :class:`SpringCoefficients` or a + /// ``(frequency, damping)`` tuple. + #[staticmethod] + #[pyo3(signature = (origin, du, dv, nu, nv, warp, weft, shear, **kwargs))] + #[allow(clippy::too_many_arguments)] + fn cloth_anisotropic( + origin: PyVector, + du: PyVector, + dv: PyVector, + nu: usize, + nv: usize, + warp: &Bound<'_, PyAny>, + weft: &Bound<'_, PyAny>, + shear: &Bound<'_, PyAny>, + kwargs: Option<&Bound<'_, PyDict>>, + ) -> PyResult { + SoftBodyBuilder::from_kwargs( + rapier::dynamics::SoftBodyBuilder::cloth_anisotropic( + origin.0.into(), + du.0.into(), + dv.0.into(), + nu, + nv, + spring(warp)?, + spring(weft)?, + spring(shear)?, + ), + kwargs, + ) + } + + /// A box of ``nx`` by ``ny`` by ``nz`` particles filled with tetrahedral cells. + #[staticmethod] + #[pyo3(signature = (center, half_extents, nx, ny, nz, **kwargs))] + fn cuboid( + center: PyVector, + half_extents: PyVector, + nx: usize, + ny: usize, + nz: usize, + kwargs: Option<&Bound<'_, PyDict>>, + ) -> PyResult { + SoftBodyBuilder::from_kwargs( + rapier::dynamics::SoftBodyBuilder::cuboid( + center.0.into(), + half_extents.0.into(), + nx, + ny, + nz, + ), + kwargs, + ) + } + + /// A hollow sphere: an icosphere surface with ``subdivisions`` refinement levels, holding + /// its volume (a balloon). + #[staticmethod] + #[pyo3(signature = (center, radius, subdivisions, **kwargs))] + fn sphere( + center: PyVector, + radius: Real, + subdivisions: usize, + kwargs: Option<&Bound<'_, PyDict>>, + ) -> PyResult { + SoftBodyBuilder::from_kwargs( + rapier::dynamics::SoftBodyBuilder::sphere(center.0.into(), radius, subdivisions), + kwargs, + ) + } + + /// A triangle-mesh body without cells: the vertices are particles, the triangles the + /// surface, held by dihedral bending and shape matching. + /// + /// :raises MeshConversionError: if the mesh is empty. + #[staticmethod] + #[pyo3(signature = (vertices, indices, **kwargs))] + fn trimesh( + vertices: &Bound<'_, PyAny>, + indices: &Bound<'_, PyAny>, + kwargs: Option<&Bound<'_, PyDict>>, + ) -> PyResult { + let vertices = crate::geometry::extract_verts_for_dim(vertices)?; + let indices = extract_elements::<3>(indices)?; + let builder = rapier::dynamics::SoftBodyBuilder::trimesh(vertices, indices) + .ok_or_else(|| crate::errors::MeshConversionError::new_err("empty triangle mesh"))?; + SoftBodyBuilder::from_kwargs(builder, kwargs) + } + + /// A body filling the closed triangle surface with tetrahedral cells of the given size; + /// ``skinned`` keeps the surface as a skin embedded in the cells. + /// + /// :raises MeshConversionError: if the surface cannot be meshed. + #[staticmethod] + #[pyo3(signature = (vertices, indices, cell_size, skinned=false, **kwargs))] + fn volumetric( + vertices: &Bound<'_, PyAny>, + indices: &Bound<'_, PyAny>, + cell_size: Real, + skinned: bool, + kwargs: Option<&Bound<'_, PyDict>>, + ) -> PyResult { + let vertices = crate::geometry::extract_verts_for_dim(vertices)?; + let indices = extract_elements::<3>(indices)?; + let builder = if skinned { + rapier::dynamics::SoftBodyBuilder::volumetric_skinned(&vertices, &indices, cell_size) + } else { + rapier::dynamics::SoftBodyBuilder::volumetric(&vertices, &indices, cell_size) + } + .ok_or_else(|| { + crate::errors::MeshConversionError::new_err( + "the surface could not be filled with cells (is it closed?)", + ) + })?; + SoftBodyBuilder::from_kwargs(builder, kwargs) + } + + /* + * Particles. + */ + + /// A counter incremented by every change of the body's topology (tears, cuts). + #[getter] + fn topology_version(&self) -> u32 { + self.with_ref(|b| b.topology_version()) + } + /// The number of particles. + #[getter] + fn num_particles(&self) -> usize { + self.with_ref(|b| b.num_particles()) + } + /// A snapshot of the ``i``-th particle. + fn particle(&self, i: usize) -> PyResult { + self.check_particle(i)?; + Ok(self.with_ref(|b| SoftBodyParticle(b.particles()[i]))) + } + /// Snapshots of every particle. + fn particles(&self) -> Vec { + self.with_ref(|b| { + b.particles() + .iter() + .copied() + .map(SoftBodyParticle) + .collect() + }) + } + /// The world-space position of the ``i``-th particle. + fn particle_position(&self, i: usize) -> PyResult { + self.check_particle(i)?; + Ok(self.with_ref(|b| vec3(b.particle_position(i)))) + } + /// The world-space positions of every particle as an ``(N, 3)`` ndarray. + #[getter] + fn particle_positions<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + self.with_ref(|b| vectors_to_array(py, b.particle_positions())) + } + /// The velocity of the ``i``-th particle. + fn particle_velocity(&self, i: usize) -> PyResult { + self.check_particle(i)?; + Ok(self.with_ref(|b| vec3(b.particle_velocity(i)))) + } + /// The velocities of every particle as an ``(N, 3)`` ndarray. + #[getter] + fn particle_velocities<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + self.with_ref(|b| vectors_to_array(py, b.particle_velocities())) + } + /// Set the world-space position of the ``i``-th particle. + fn set_particle_position(&mut self, i: usize, position: PyVector) -> PyResult<()> { + self.check_particle(i)?; + self.with_mut(|b| b.set_particle_position(i, position.0.into())); + Ok(()) + } + /// Set the velocity of the ``i``-th particle. + fn set_particle_velocity(&mut self, i: usize, velocity: PyVector) -> PyResult<()> { + self.check_particle(i)?; + self.with_mut(|b| b.set_particle_velocity(i, velocity.0.into())); + Ok(()) + } + /// Move the pinned ``i``-th particle to ``position`` over the next step, like a + /// position-based kinematic body (it pushes what it meets), then hold it there. Ignored for + /// a free particle. + fn set_particle_kinematic_target(&mut self, i: usize, position: PyVector) -> PyResult<()> { + self.check_particle(i)?; + self.with_mut(|b| b.set_particle_kinematic_target(i, position.0.into())); + Ok(()) + } + /// Pin (or release) the ``i``-th particle. + fn set_particle_pinned(&mut self, i: usize, pinned: bool) -> PyResult<()> { + self.check_particle(i)?; + self.with_mut(|b| b.set_particle_pinned(i, pinned)); + Ok(()) + } + /// Attach the ``i``-th particle to a rigid body, at the particle's current position. + fn attach_particle( + &mut self, + i: usize, + body: &RigidBodyHandle, + bodies: &RigidBodySet, + ) -> PyResult<()> { + self.check_particle(i)?; + self.with_mut(|b| b.attach_particle(i, body.0, &bodies.0)); + Ok(()) + } + /// Detach the ``i``-th particle from the rigid body it follows; ``False`` if it was not + /// attached. + fn detach_particle(&mut self, i: usize) -> bool { + self.with_mut(|b| b.detach_particle(i)) + } + /// The particles attached to rigid bodies. + #[getter] + fn particle_attachments(&self) -> Vec { + self.with_ref(|b| { + b.particle_attachments() + .iter() + .copied() + .map(SoftParticleAttachment) + .collect() + }) + } + + /* + * Elements. + */ + + /// The number of edges (structural and bending). + #[getter] + fn num_edges(&self) -> usize { + self.with_ref(|b| b.edges().len()) + } + /// A snapshot of the ``i``-th edge. + fn edge(&self, i: usize) -> PyResult { + self.with_ref(|b| { + b.edges() + .get(i) + .copied() + .map(SoftBodyEdge) + .ok_or_else(|| PyIndexError::new_err(format!("edge index {i} out of range"))) + }) + } + /// The particle pairs of every edge as an ``(E, 2)`` ndarray. + #[getter] + fn edges<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + self.with_ref(|b| { + let e: Vec<[u32; 2]> = b.edges().iter().map(|e| e.vertices).collect(); + elements_to_array(py, &e) + }) + } + /// The number of dihedral bending constraints. + #[getter] + fn num_dihedrals(&self) -> usize { + self.with_ref(|b| b.dihedrals().len()) + } + /// A snapshot of the ``i``-th dihedral. + fn dihedral(&self, i: usize) -> PyResult { + self.with_ref(|b| { + b.dihedrals() + .get(i) + .copied() + .map(SoftBodyDihedral) + .ok_or_else(|| PyIndexError::new_err(format!("dihedral index {i} out of range"))) + }) + } + /// The particles of every dihedral as a ``(D, 4)`` ndarray. + #[getter] + fn dihedrals<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + self.with_ref(|b| { + let d: Vec<[u32; 4]> = b.dihedrals().iter().map(|d| d.vertices).collect(); + elements_to_array(py, &d) + }) + } + /// The number of cells (tetrahedra). + #[getter] + fn num_cells(&self) -> usize { + self.with_ref(|b| b.cells().len()) + } + /// A snapshot of the ``i``-th cell. + fn cell(&self, i: usize) -> PyResult { + self.with_ref(|b| { + b.cells() + .get(i) + .copied() + .map(SoftBodyCell) + .ok_or_else(|| PyIndexError::new_err(format!("cell index {i} out of range"))) + }) + } + /// The particles of every cell as a ``(C, 4)`` ndarray. + #[getter] + fn cells<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + self.with_ref(|b| { + let c: Vec<[u32; 4]> = b.cells().iter().map(|c| c.vertices).collect(); + elements_to_array(py, &c) + }) + } + /// The boundary triangles, oriented outward, as a ``(B, 3)`` ndarray. + #[getter] + fn boundary<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + self.with_ref(|b| elements_to_array(py, b.boundary())) + } + + /* + * Material and models. + */ + + /// The material (a copy: assign it back or use :meth:`set_material` to apply changes). + #[getter] + fn material(&self) -> SoftBodyMaterial { + self.with_ref(|b| SoftBodyMaterial(b.material().clone())) + } + #[setter] + fn set_material(&mut self, material: &SoftBodyMaterial) { + let m = material.0.clone(); + self.with_mut(|b| b.set_material(m)); + } + /// The constitutive model of the cells. + #[getter] + fn cell_model(&self) -> SoftBodyCellModel { + self.with_ref(|b| SoftBodyCellModel::from_rapier(b.cell_model())) + } + /// Is the volume enclosed by the body's closed surfaces preserved? + #[getter] + fn volume_preservation_enabled(&self) -> bool { + self.with_ref(|b| b.volume_preservation_enabled()) + } + /// Enable or disable the preservation of the enclosed volume. + fn enable_volume_preservation(&mut self, enabled: bool) { + self.with_mut(|b| b.enable_volume_preservation(enabled)) + } + /// The pieces of material whose volume is preserved. + #[getter] + fn volume_pieces(&self) -> Vec { + self.with_ref(|b| { + b.volume_pieces() + .iter() + .map(|p| SoftVolumePiece { + elements: p.elements().to_vec(), + particles: p.particles().to_vec(), + rest_volume: p.rest_volume(), + volume: p.volume(b), + }) + .collect() + }) + } + /// The rest volume enclosed by the closed surfaces. + #[getter] + fn rest_volume(&self) -> Real { + self.with_ref(|b| b.rest_volume()) + } + /// The current volume enclosed by the closed surfaces. + #[getter] + fn volume(&self) -> Real { + self.with_ref(|b| b.volume()) + } + /// The target volume multiplier (``> 1`` inflates the body). + #[getter] + fn volume_factor(&self) -> Real { + self.with_ref(|b| b.volume_factor()) + } + #[setter] + fn set_volume_factor(&mut self, factor: Real) { + self.with_mut(|b| b.set_volume_factor(factor)) + } + /// The thickness of the particles. + #[getter] + fn particle_radius(&self) -> Real { + self.with_ref(|b| b.particle_radius()) + } + /// The dynamics settings of the particles (a copy). + #[getter] + fn particle_settings(&self) -> SoftBodyParticleSettings { + self.with_ref(|b| SoftBodyParticleSettings(*b.particle_settings())) + } + /// Forget every plastic deformation: the rest shapes return to their initial values. + fn reset_plasticity(&mut self) { + self.with_mut(|b| b.reset_plasticity()) + } + + /* + * Whole-body state. + */ + + /// The hidden rigid body standing for the whole soft body in joints and islands. + #[getter] + fn root_body(&self) -> RigidBodyHandle { + self.with_ref(|b| RigidBodyHandle(b.root_body())) + } + /// The soft body this one was split off from by a tear, if any. + #[getter] + fn origin(&self) -> Option { + self.with_ref(|b| b.origin().map(SoftBodyHandle)) + } + /// The soft bodies that tears split off from this one. + #[getter] + fn pieces(&self) -> Vec { + self.with_ref(|b| b.pieces().iter().copied().map(SoftBodyHandle).collect()) + } + /// The center of mass of the particles. + #[getter] + fn center_of_mass(&self) -> Vec3 { + self.with_ref(|b| vec3(b.center_of_mass())) + } + /// The total mass of the particles. + #[getter] + fn mass(&self) -> Real { + self.with_ref(|b| b.mass()) + } + /// Is the soft body sleeping? + #[getter] + fn is_sleeping(&self) -> bool { + self.with_ref(|b| b.is_sleeping()) + } + /// Wake the soft body up. + fn wake_up(&mut self) { + self.with_mut(|b| b.wake_up()) + } + /// Is the soft body enabled (simulated)? + #[getter] + fn is_enabled(&self) -> bool { + self.with_ref(|b| b.is_enabled()) + } + /// Enable or disable the soft body. + fn set_enabled(&mut self, enabled: bool) { + self.with_mut(|b| b.set_enabled(enabled)) + } + /// Set the extra internal PGS iterations run per substep for this body and everything it + /// touches. + fn set_additional_pgs_iterations(&mut self, iterations: usize) { + self.with_mut(|b| b.set_additional_pgs_iterations(iterations)) + } + /// The number of colors the parallel solver splits the body's constraints into. + #[getter] + fn num_solver_colors(&self) -> usize { + self.with_ref(|b| b.num_solver_colors()) + } + /// Arbitrary integer user data. + #[getter] + fn user_data(&self) -> u128 { + self.with_ref(|b| b.user_data) + } + #[setter] + fn set_user_data(&mut self, data: u128) { + self.with_mut(|b| b.user_data = data) + } + + /* + * Forces and impulses. + */ + + /// Add a force to every particle (spread by mass). + #[pyo3(signature = (force, wake_up=true))] + fn add_force(&mut self, force: PyVector, wake_up: bool) { + self.with_mut(|b| b.add_force(force.0.into(), wake_up)) + } + /// Add a force to the ``i``-th particle. + #[pyo3(signature = (i, force, wake_up=true))] + fn add_particle_force(&mut self, i: usize, force: PyVector, wake_up: bool) -> PyResult<()> { + self.check_particle(i)?; + self.with_mut(|b| b.add_particle_force(i, force.0.into(), wake_up)); + Ok(()) + } + /// Reset the user forces applied to the particles. + #[pyo3(signature = (wake_up=true))] + fn reset_forces(&mut self, wake_up: bool) { + self.with_mut(|b| b.reset_forces(wake_up)) + } + /// Apply an impulse to every particle (spread by mass). + #[pyo3(signature = (impulse, wake_up=true))] + fn apply_impulse(&mut self, impulse: PyVector, wake_up: bool) { + self.with_mut(|b| b.apply_impulse(impulse.0.into(), wake_up)) + } + /// Apply an impulse to the ``i``-th particle. + #[pyo3(signature = (i, impulse, wake_up=true))] + fn apply_particle_impulse( + &mut self, + i: usize, + impulse: PyVector, + wake_up: bool, + ) -> PyResult<()> { + self.check_particle(i)?; + self.with_mut(|b| b.apply_particle_impulse(i, impulse.0.into(), wake_up)); + Ok(()) + } + /// Apply an impulse to the particles within ``falloff_radius`` of ``point``, scaled down + /// linearly with their distance to it (``falloff_radius <= 0`` applies it everywhere). + #[pyo3(signature = (impulse, point, falloff_radius, wake_up=true))] + fn apply_impulse_at_point( + &mut self, + impulse: PyVector, + point: PyVector, + falloff_radius: Real, + wake_up: bool, + ) { + self.with_mut(|b| { + b.apply_impulse_at_point(impulse.0.into(), point.0.into(), falloff_radius, wake_up) + }) + } + /// Apply an impulse of the given magnitude pushing the particles away from ``center`` (a + /// blast), scaled down linearly up to ``falloff_radius``. + #[pyo3(signature = (center, magnitude, falloff_radius, wake_up=true))] + fn apply_radial_impulse( + &mut self, + center: PyVector, + magnitude: Real, + falloff_radius: Real, + wake_up: bool, + ) { + self.with_mut(|b| { + b.apply_radial_impulse(center.0.into(), magnitude, falloff_radius, wake_up) + }) + } + + /* + * Tearing. + */ + + /// Request the ``i``-th edge to tear at the end of the next step. + fn tear_edge(&mut self, i: usize) { + self.with_mut(|b| b.tear_edge(i)) + } + /// Request the ``i``-th cell to tear at the end of the next step. + fn tear_cell(&mut self, i: usize) { + self.with_mut(|b| b.tear_cell(i)) + } + /// Are there tears requested for the next step? + #[getter] + fn has_pending_tears(&self) -> bool { + self.with_ref(|b| b.has_pending_tears()) + } + /// The ``(edges, cells)`` a blade (a world-space triangle given as three points) crosses, + /// as would be torn by :meth:`SoftBodySet.cut`. + fn crossing_elements(&self, blade: (PyVector, PyVector, PyVector)) -> (Vec, Vec) { + let blade = [blade.0 .0.into(), blade.1 .0.into(), blade.2 .0.into()]; + self.with_ref(|b| b.crossing_elements(&blade)) + } + + /* + * Clusters. + */ + + /// The number of cluster slots (some may have been removed: see + /// :attr:`SoftBodyCluster.is_live`). + #[getter] + fn num_clusters(&self) -> usize { + self.with_ref(|b| b.clusters().len()) + } + /// The number of live clusters. + #[getter] + fn num_live_clusters(&self) -> usize { + self.with_ref(|b| b.num_live_clusters()) + } + /// A snapshot of the ``i``-th cluster, or ``None``. + fn cluster(&self, i: u32) -> Option { + self.with_ref(|b| { + b.cluster(i).map(|c| SoftBodyCluster { + index: i, + particles: c.particles().to_vec(), + proxy: RigidBodyHandle(c.proxy()), + is_live: c.is_live(), + shape_matching_enabled: c.shape_matching_enabled(), + meshes: c.meshes().map(|m| SoftMeshId(m.id())).collect(), + }) + }) + } + /// Snapshots of every live cluster. + #[getter] + fn clusters(&self) -> Vec { + self.with_ref(|b| { + b.live_clusters() + .map(|(i, c)| SoftBodyCluster { + index: i, + particles: c.particles().to_vec(), + proxy: RigidBodyHandle(c.proxy()), + is_live: true, + shape_matching_enabled: c.shape_matching_enabled(), + meshes: c.meshes().map(|m| SoftMeshId(m.id())).collect(), + }) + .collect() + }) + } + /// The rigid-body proxy of the ``i``-th cluster: joints and colliders attach to it. + fn cluster_proxy(&self, i: u32) -> Option { + self.with_ref(|b| b.cluster_proxy(i).map(RigidBodyHandle)) + } + /// Enable or disable shape matching on the ``i``-th cluster. + fn enable_cluster_shape_matching(&mut self, i: u32, enabled: bool) { + self.with_mut(|b| b.enable_cluster_shape_matching(i, enabled)) + } + /// Scale the stiffness of the elements of the ``i``-th cluster. + fn set_cluster_stiffness_scale(&mut self, i: u32, scale: Real) { + self.with_mut(|b| b.set_cluster_stiffness_scale(i, scale)) + } + /// Override the softness of the edges of the ``i``-th cluster (``None`` restores the + /// material's). + #[pyo3(signature = (i, softness))] + fn set_cluster_edge_softness( + &mut self, + i: u32, + softness: Option<&Bound<'_, PyAny>>, + ) -> PyResult<()> { + let s = softness.map(spring).transpose()?; + self.with_mut(|b| b.set_cluster_edge_softness(i, s)); + Ok(()) + } + /// Scale the tear thresholds of the elements of the ``i``-th cluster. + fn set_cluster_tear_resistance(&mut self, i: u32, resistance: Real) { + self.with_mut(|b| b.set_cluster_tear_resistance(i, resistance)) + } + /// Pin (or release) every particle of the ``i``-th cluster. + fn set_cluster_pinned(&mut self, i: u32, pinned: bool) { + self.with_mut(|b| b.set_cluster_pinned(i, pinned)) + } + /// Move the ``i``-th cluster rigidly to the given pose over the next step. + fn set_cluster_kinematic_target(&mut self, i: u32, pose: PyIsometry) { + let p: rapier::math::Pose = pose.0.into(); + self.with_mut(|b| b.set_cluster_kinematic_target(i, p)) + } + + /* + * Collision meshes. + */ + + /// Snapshots of every collision mesh held by the clusters (the body's own deformable + /// surface included), with their current world-space vertices. + #[getter] + fn meshes(&self) -> Vec { + self.with_ref(|b| { + b.meshes() + .map(|m| SoftCollisionMesh::from_rapier(b, m)) + .collect() + }) + } + /// A snapshot of the collision mesh with the given identifier, or ``None``. + fn mesh(&self, id: &SoftMeshId) -> Option { + self.with_ref(|b| b.mesh(id.0).map(|m| SoftCollisionMesh::from_rapier(b, m))) + } + /// A snapshot of the collision mesh a deformable collider holds, or ``None``. + fn mesh_of(&self, collider: &ColliderHandle) -> Option { + self.with_ref(|b| { + b.mesh_of(collider.0) + .map(|m| SoftCollisionMesh::from_rapier(b, m)) + }) + } + /// A snapshot of the body's own deformable surface mesh, or ``None`` when the body + /// collides through its particles. + fn collision_mesh(&self) -> Option { + self.with_ref(|b| { + b.collision_mesh() + .map(|m| SoftCollisionMesh::from_rapier(b, m)) + }) + } + + fn __repr__(&self) -> String { + self.with_ref(|b| { + format!( + "SoftBody(particles={}, edges={}, cells={}, mass={})", + b.num_particles(), + b.edges().len(), + b.cells().len(), + b.mass() + ) + }) + } +} + +// ---------------------------------------------------------------------- +// SoftBodySet. +// ---------------------------------------------------------------------- + +/// Container holding every :class:`SoftBody` of a physics world. +/// +/// Bodies are addressed by :class:`SoftBodyHandle` and live in the set until removed. The +/// set supports ``len()``, ``in``, iteration (yielding ``(handle, body)`` pairs) and +/// ``[handle]`` lookup; the bodies it returns are live views. +#[pyclass(name = "SoftBodySet", module = "rapier", unsendable)] +pub struct SoftBodySet(pub rapier::dynamics::SoftBodySet); + +#[pymethods] +impl SoftBodySet { + /// Build an empty soft-body set. + #[new] + fn new() -> Self { + Self(rapier::dynamics::SoftBodySet::new()) + } + + /// Insert the soft body described by ``builder``, creating its hidden root rigid body in + /// ``bodies`` and its colliders in ``colliders``. + fn insert( + &mut self, + builder: &SoftBodyBuilder, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) -> SoftBodyHandle { + SoftBodyHandle( + self.0 + .insert(builder.builder.clone(), &mut bodies.0, &mut colliders.0), + ) + } + + /// Remove a soft body with its proxies, colliders and attached joints. + /// + /// :returns: the removed body, or ``None`` if the handle matched nothing. + #[allow(clippy::too_many_arguments)] + fn remove( + &mut self, + handle: &SoftBodyHandle, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + ) -> Option { + self.0 + .remove( + handle.0, + &mut islands.0, + &mut bodies.0, + &mut colliders.0, + &mut impulse_joints.0, + &mut multibody_joints.0, + ) + .map(SoftBody::new_owned) + } + + /// Add a cluster over the given particles: a rigid proxy in ``bodies`` that joints and + /// colliders can attach to. Returns the cluster's index, or ``None`` if no particle was + /// valid. + pub fn add_cluster( + &mut self, + handle: &SoftBodyHandle, + particles: &Bound<'_, PyAny>, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) -> PyResult> { + let particles = extract_indices_1d(particles)?; + Ok(self + .0 + .add_cluster(handle.0, &particles, &mut bodies.0, &mut colliders.0)) + } + + /// Remove a cluster with its proxy, colliders and joints; ``False`` if it did not exist. + #[allow(clippy::too_many_arguments)] + pub fn remove_cluster( + &mut self, + handle: &SoftBodyHandle, + cluster: u32, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + ) -> bool { + self.0 + .remove_cluster( + handle.0, + cluster, + &mut islands.0, + &mut bodies.0, + &mut colliders.0, + &mut impulse_joints.0, + &mut multibody_joints.0, + ) + .is_some() + } + + /// Tear a soft body at once along the given edges and through the given cells (indices + /// into its edge and cell lists), without removing material; pieces disconnected by the + /// tear become soft bodies of their own. + /// + /// :returns: the :class:`SoftBodyTearEvent`, or ``None`` when nothing changed. + #[allow(clippy::too_many_arguments)] + pub fn tear( + &mut self, + handle: &SoftBodyHandle, + edges: &Bound<'_, PyAny>, + cells: &Bound<'_, PyAny>, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + ) -> PyResult> { + let edges = extract_indices_1d(edges)?; + let cells = extract_indices_1d(cells)?; + Ok(self + .0 + .tear( + handle.0, + &edges, + &cells, + &mut islands.0, + &mut bodies.0, + &mut colliders.0, + &mut impulse_joints.0, + &mut multibody_joints.0, + ) + .map(SoftBodyTearEvent)) + } + + /// Cut a soft body along a blade (a world-space triangle given as three points) at once, + /// without removing material; pieces disconnected by the cut become soft bodies of their + /// own. + /// + /// :returns: the :class:`SoftBodyTearEvent`, or ``None`` when nothing changed. + #[allow(clippy::too_many_arguments)] + pub fn cut( + &mut self, + handle: &SoftBodyHandle, + blade: (PyVector, PyVector, PyVector), + islands: &mut IslandManager, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + impulse_joints: &mut ImpulseJointSet, + multibody_joints: &mut MultibodyJointSet, + ) -> Option { + let blade = [blade.0 .0.into(), blade.1 .0.into(), blade.2 .0.into()]; + self.0 + .cut( + handle.0, + &blade, + &mut islands.0, + &mut bodies.0, + &mut colliders.0, + &mut impulse_joints.0, + &mut multibody_joints.0, + ) + .map(SoftBodyTearEvent) + } + + /// Wake a soft body and everything it touches up; ``strong`` keeps them awake for a while + /// even at rest. + #[pyo3(signature = (handle, bodies, strong=false))] + fn wake_up(&mut self, handle: &SoftBodyHandle, bodies: &mut RigidBodySet, strong: bool) { + self.0.wake_up(handle.0, &mut bodies.0, strong) + } + + /// A live view of the body for ``handle``, or ``None``. + fn get(slf: &Bound<'_, Self>, handle: &SoftBodyHandle) -> Option { + slf.borrow().0.get(handle.0)?; + Some(SoftBody { + backing: SoftBodyBacking::InSet { + set: slf.clone().unbind(), + handle: handle.0, + }, + }) + } + + /// Indexing form of ``get``. + /// + /// :raises InvalidHandle: if ``handle`` matches no body. + fn __getitem__(slf: &Bound<'_, Self>, handle: &SoftBodyHandle) -> PyResult { + if slf.borrow().0.get(handle.0).is_none() { + return Err(crate::errors::InvalidHandle::new_err(format!( + "no soft body for {:?}", + handle.0.into_raw_parts() + ))); + } + Ok(SoftBody { + backing: SoftBodyBacking::InSet { + set: slf.clone().unbind(), + handle: handle.0, + }, + }) + } + + fn __contains__(&self, handle: &SoftBodyHandle) -> bool { + self.0.contains(handle.0) + } + + fn __len__(&self) -> usize { + self.0.len() + } + + /// Is the set empty? + fn is_empty(&self) -> bool { + self.0.is_empty() + } + + /// Iterate over ``(handle, body)`` pairs; each body is a live view. + fn __iter__(slf: &Bound<'_, Self>) -> PyResult> { + let handles: Vec = + slf.borrow().0.iter().map(|(h, _)| h).collect(); + Py::new( + slf.py(), + SoftBodySetIter { + set: slf.clone().unbind(), + handles, + i: 0, + }, + ) + } + + /// The handles of every body in the set. + fn handles(&self) -> Vec { + self.0.iter().map(|(h, _)| SoftBodyHandle(h)).collect() + } + + fn __repr__(&self) -> String { + format!("SoftBodySet(len={})", self.0.len()) + } +} + +/// Iterator yielding ``(SoftBodyHandle, SoftBody)`` pairs from a :class:`SoftBodySet`. +#[pyclass] +pub struct SoftBodySetIter { + set: Py, + handles: Vec, + i: usize, +} + +#[pymethods] +impl SoftBodySetIter { + fn __iter__(slf: PyRef<'_, Self>) -> PyRef<'_, Self> { + slf + } + fn __next__(mut slf: PyRefMut<'_, Self>) -> Option<(SoftBodyHandle, SoftBody)> { + if slf.i >= slf.handles.len() { + return None; + } + let py = slf.py(); + let handle = slf.handles[slf.i]; + slf.i += 1; + let set = slf.set.clone_ref(py); + Some(( + SoftBodyHandle(handle), + SoftBody { + backing: SoftBodyBacking::InSet { set, handle }, + }, + )) + } +} + +pub fn register_soft_bodies( + py: Python<'_>, + m: &Bound<'_, crate::pyo3::types::PyModule>, +) -> PyResult<()> { + m.add("SoftBindingError", py.get_type_bound::())?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + Ok(()) +} diff --git a/python/rapier-testbed/rapier_testbed/__init__.py b/python/rapier-testbed/rapier_testbed/__init__.py index f35062a92..c065ceca8 100644 --- a/python/rapier-testbed/rapier_testbed/__init__.py +++ b/python/rapier-testbed/rapier_testbed/__init__.py @@ -59,6 +59,8 @@ def _autoload_bundled_examples() -> None: # New examples ported from ``examples3d/`` should extend this list. _bundled = ( "rapier_testbed.examples3.primitives3", + "rapier_testbed.examples3.soft_bodies3", + "rapier_testbed.examples3.soft_tearing3", # ── 3D examples (first half) ──────────────────────────────── "rapier_testbed.examples3.ccd3", "rapier_testbed.examples3.character_controller3", diff --git a/python/rapier-testbed/rapier_testbed/_testbed.py b/python/rapier-testbed/rapier_testbed/_testbed.py index f052305a7..114eddb26 100644 --- a/python/rapier-testbed/rapier_testbed/_testbed.py +++ b/python/rapier-testbed/rapier_testbed/_testbed.py @@ -263,6 +263,7 @@ def __init__( self.colliders = ns.ColliderSet() self.impulse_joints = ns.ImpulseJointSet() self.multibody_joints = ns.MultibodyJointSet() + self.soft_bodies = ns.SoftBodySet() self.gravity = _DEFAULT_GRAVITY_3D self._hooks: Optional[Any] = None self._event_handler: Optional[Any] = None @@ -316,6 +317,9 @@ def __init__( self._on_escape: Optional[Callable[[], None]] = on_escape self._known_colliders: set = set() # ColliderHandle.index seen self._show_wireframe: bool = False # debug-line overlay toggle + # One NodePath per soft body, rebuilt on topology changes and refreshed from the + # particle positions every frame: ``SoftBodyHandle.index`` → dict. + self._soft_nodes: dict = {} if not self._headless: _ensure_panda_config() @@ -344,15 +348,19 @@ def set_world( colliders: Any, impulse_joints: Any, multibody_joints: Any, + soft_bodies: Optional[Any] = None, ) -> None: """Bind freshly-built sets as the active simulation state. Mirrors :meth:`rapier_testbed::Testbed::set_world`. Uses the testbed's current :attr:`gravity` (defaulting to ``(0, -9.81, 0)`` - if the example doesn't override it). + if the example doesn't override it). ``soft_bodies`` is the + :class:`rapier3d.SoftBodySet` of the scene, or ``None`` for a scene + without soft bodies. """ self.set_world_with_params( - bodies, colliders, impulse_joints, multibody_joints, self.gravity + bodies, colliders, impulse_joints, multibody_joints, self.gravity, + soft_bodies=soft_bodies, ) def set_world_with_params( @@ -363,12 +371,14 @@ def set_world_with_params( multibody_joints: Any, gravity: Any, hooks: Optional[Any] = None, + soft_bodies: Optional[Any] = None, ) -> None: """Bind sets + override gravity and (optionally) physics hooks.""" self.bodies = bodies self.colliders = colliders self.impulse_joints = impulse_joints self.multibody_joints = multibody_joints + self.soft_bodies = soft_bodies if soft_bodies is not None else self._ns.SoftBodySet() self.gravity = gravity self._hooks = hooks self._step_count = 0 @@ -474,6 +484,7 @@ def step_once(self) -> None: self._ccd_solver, self._hooks, self._event_handler, + soft_bodies=self.soft_bodies, ) self._step_count += 1 for cb in self._callbacks: @@ -497,6 +508,7 @@ def _render_frame(self) -> None: return self._sync_mesh_nodes() self._update_transforms() + self._sync_soft_body_nodes() if self._show_wireframe: lines, colors, _objects = self._debug_pipeline.render_to_arrays( self.bodies, @@ -504,6 +516,7 @@ def _render_frame(self) -> None: self.impulse_joints, self.multibody_joints, self._narrow_phase, + self.soft_bodies, ) self._upload_lines(lines, colors) elif self._line_node is not None: @@ -525,6 +538,12 @@ def _clear_mesh_cache(self) -> None: pass self._mesh_nodes.clear() self._known_colliders.clear() + for entry in self._soft_nodes.values(): + try: + entry["node"].removeNode() + except Exception: + pass + self._soft_nodes.clear() def _sync_mesh_nodes(self) -> None: """Insert nodes for new colliders; drop nodes for removed ones.""" @@ -554,6 +573,12 @@ def _sync_mesh_nodes(self) -> None: for idx, (handle, col) in current.items(): if idx in self._known_colliders: continue + # The colliders of a soft body (its deformable surface, or its particles) are + # drawn from the soft body itself (see ``_sync_soft_body_nodes``). + parent = col.parent + if parent is not None and parent in self.bodies and self.bodies[parent].is_soft_frame: + self._known_colliders.add(idx) + continue color = self._color_for_collider(col) try: geoms = _meshes.shape_to_geoms(col.shape, color_rgba=color) @@ -608,6 +633,71 @@ def _update_transforms(self) -> None: # Panda3D LQuaternion takes (w, x, y, z) — matches Rapier. np_group.setQuat(LQuaternion(float(r.w), float(r.i), float(r.j), float(r.k))) + def _sync_soft_body_nodes(self) -> None: + """Draw every soft body as the flat-shaded triangles of its boundary. + + A body's Geom is rebuilt when its topology changes (tears, cuts) and its + vertex buffer is rewritten from the particle positions every frame. + """ + if self._mesh_root is None: + return + from panda3d.core import Geom, GeomNode, GeomTriangles, GeomVertexData, GeomVertexFormat + + from . import _meshes + + seen = set() + for handle, body in self.soft_bodies: + idx = int(handle.index) + seen.add(idx) + boundary = body.boundary + positions = body.particle_positions + version = body.topology_version + entry = self._soft_nodes.get(idx) + if entry is None or entry["version"] != version or entry["triangles"] != boundary.shape[0]: + if entry is not None: + entry["node"].removeNode() + rgba = _meshes.color_for_body(idx + 7919, self._ns.RigidBodyType.DYNAMIC) + color_u8 = np.array([max(0, min(255, int(c * 255.0))) for c in rgba], dtype=np.uint8) + fmt = GeomVertexFormat.getV3n3c4() + nv = int(boundary.shape[0] * 3) + vdata = GeomVertexData(f"soft-{idx}", fmt, Geom.UHDynamic) + vdata.unclean_set_num_rows(nv) + prim = GeomTriangles(Geom.UHStatic) + if nv > 0: + prim.setIndexType(Geom.NTUint32) + prim.reserveNumVertices(nv) + for t in range(boundary.shape[0]): + prim.addVertices(3 * t, 3 * t + 1, 3 * t + 2) + prim.closePrimitive() + geom = Geom(vdata) + geom.addPrimitive(prim) + node = GeomNode(f"soft-{idx}") + node.addGeom(geom) + np_node = self._mesh_root.attachNewNode(node) + np_node.setTwoSided(True) + np_node.setMaterialOff(1) + entry = { + "node": np_node, + "vdata": vdata, + "version": version, + "triangles": int(boundary.shape[0]), + "color": color_u8, + } + self._soft_nodes[idx] = entry + if boundary.shape[0] == 0: + continue + pos, nrm, _ = _meshes._trimesh_mesh(positions, boundary) + buf = np.empty(pos.shape[0], dtype=np.dtype([("pos", np.float32, 3), ("nrm", np.float32, 3), ("color", np.uint8, 4)])) + buf["pos"] = pos + buf["nrm"] = nrm + buf["color"] = entry["color"] + entry["vdata"].modifyArrayHandle(0).copyDataFrom(buf.tobytes()) + + # Soft bodies removed from the world. + for idx in list(self._soft_nodes): + if idx not in seen: + self._soft_nodes.pop(idx)["node"].removeNode() + def _apply_local_pose(self, np_node: Any, iso: Any) -> None: """Apply a sub-shape's local pose to a Compound-child NodePath.""" from panda3d.core import LQuaternion @@ -770,6 +860,7 @@ def _cleanup_scene(self) -> None: # Drop per-collider node caches so the next scenario rebuilds. self._mesh_nodes.clear() self._known_colliders.clear() + self._soft_nodes.clear() self._mesh_root = None # Reset arcball bindings. self._arcball = None diff --git a/python/rapier-testbed/rapier_testbed/examples3/soft_bodies3.py b/python/rapier-testbed/rapier_testbed/examples3/soft_bodies3.py new file mode 100644 index 000000000..105d4a1e0 --- /dev/null +++ b/python/rapier-testbed/rapier_testbed/examples3/soft_bodies3.py @@ -0,0 +1,71 @@ +"""Port of examples3d/soft_bodies3.rs: a pinned cloth catching a box, a pressurized +balloon, jelly cubes (corotational, Neo-Hookean, volume constraints) and a rope holding a +rigid weight.""" +from __future__ import annotations + +import rapier3d as rp +from .._registry import register + +CATEGORY = "Soft bodies" +NAME = "Showcase" + + +def init_world(testbed) -> None: + bodies = rp.RigidBodySet() + colliders = rp.ColliderSet() + impulse_joints = rp.ImpulseJointSet() + multibody_joints = rp.MultibodyJointSet() + soft_bodies = rp.SoftBodySet() + + # Ground. + ground = bodies.insert(rp.RigidBody.fixed(translation=(0.0, -0.1, 0.0))) + colliders.insert_with_parent(rp.Collider.cuboid(12.0, 0.1, 12.0), ground, bodies) + + # A cloth pinned by its four corners, with a box dropped on it. + n = 24 + cloth = rp.SoftBody.cloth( + (-3.5, 2.5, -1.2), (0.1, 0.0, 0.0), (0.0, 0.0, 0.1), n, n, + pinned_particles=[0, n - 1, n * (n - 1), n * n - 1], + softness=(30.0, 1.0), + particle_mass=0.05, + ) + soft_bodies.insert(cloth, bodies, colliders) + box = bodies.insert(rp.RigidBody.dynamic(translation=(-2.35, 4.0, 0.0))) + colliders.insert_with_parent(rp.Collider.cuboid(0.3, 0.3, 0.3).density(0.5), box, bodies) + + # A balloon: a hollow sphere inflated by volume preservation. + balloon = rp.SoftBody.sphere((0.5, 3.0, 0.0), 0.8, 2, softness=(15.0, 1.0), volume_factor=1.2, + particle_mass=0.05) + soft_bodies.insert(balloon, bodies, colliders) + + # Jelly cubes: corotational, Neo-Hookean, and per-cell volume constraints. + jelly_material = rp.SoftBodyMaterial(young_modulus=2.0e3, poisson_ratio=0.35, elastic_damping_ratio=0.5) + for z, model in [(1.5, rp.SoftBodyCellModel.COROTATIONAL), (4.5, rp.SoftBodyCellModel.NEO_HOOKEAN)]: + jelly = rp.SoftBody.cuboid((3.0, 1.0, z), (0.6, 0.6, 0.6), 5, 5, 5, cell_model=model, + material=jelly_material, particle_mass=0.2) + soft_bodies.insert(jelly, bodies, colliders) + volume_jelly = rp.SoftBody.cuboid((3.0, 1.0, -1.5), (0.6, 0.6, 0.6), 5, 5, 5, + cell_model=rp.SoftBodyCellModel.VOLUME, softness=(20.0, 1.0), + particle_mass=0.2) + soft_bodies.insert(volume_jelly, bodies, colliders) + + # A rope hanging from a fixed anchor, holding a rigid ball attached by its last particle. + rope = rp.SoftBody.rope((-0.5, 5.0, 3.0), (2.5, 5.0, 3.0), 30, pinned_particles=[0], + softness=(40.0, 1.0), particle_mass=0.05) + rope_handle = soft_bodies.insert(rope, bodies, colliders) + last = soft_bodies[rope_handle].particle_position(29) + weight = bodies.insert(rp.RigidBody.dynamic(translation=(last.x, last.y - 0.3, last.z))) + colliders.insert_with_parent(rp.Collider.ball(0.25).density(2.0), weight, bodies) + soft_bodies[rope_handle].attach_particle(29, weight, bodies) + + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints, soft_bodies) + testbed.look_at((9.0, 6.0, 12.0), (0.0, 1.5, 0.0)) + + +register(CATEGORY, NAME, init_world, dim=3) + + +if __name__ == "__main__": + from .._picker import run + + run(initial=f"{CATEGORY} / {NAME}") diff --git a/python/rapier-testbed/rapier_testbed/examples3/soft_tearing3.py b/python/rapier-testbed/rapier_testbed/examples3/soft_tearing3.py new file mode 100644 index 000000000..f7ac1421d --- /dev/null +++ b/python/rapier-testbed/rapier_testbed/examples3/soft_tearing3.py @@ -0,0 +1,108 @@ +"""Port of examples3d/soft_tearing3.rs: a pinned sheet ripped by a dropped ball, a curtain +shot through by a box, and a jelly bar pulled apart by its kinematic ends. Tear events move +the driven particles to their new bodies and indices.""" +from __future__ import annotations + +import rapier3d as rp +from .._registry import register + +CATEGORY = "Soft bodies" +NAME = "Tearing" + + +def init_world(testbed) -> None: + bodies = rp.RigidBodySet() + colliders = rp.ColliderSet() + impulse_joints = rp.ImpulseJointSet() + multibody_joints = rp.MultibodyJointSet() + soft_bodies = rp.SoftBodySet() + + ground = bodies.insert(rp.RigidBody.fixed(translation=(0.0, -0.1, 0.0))) + colliders.insert_with_parent(rp.Collider.cuboid(30.0, 0.1, 30.0), ground, bodies) + + # Stiff cloth springs (100 Hz) so only impacts pass the tear strain. + def cloth_material(tear_strain): + m = rp.SoftBodyMaterial.uniform((100.0, 1.0)) + m.bend_softness = (3.0, 1.0) + m.tear_strain = tear_strain + return m + + # A sheet pinned along its border: the heavy ball dropped on it rips through. + n = 40 + border = [k for k in range(n * n) if k // n in (0, n - 1) or k % n in (0, n - 1)] + sheet = rp.SoftBody.cloth( + (-6.0, 3.0, -2.0), (0.1, 0.0, 0.0), (0.0, 0.0, 0.1), n, n, + pinned_particles=border, material=cloth_material(0.4), particle_mass=0.02, + particle_radius=0.05, surface_collider=rp.Collider.ball(0.05).friction(0.8), + ) + soft_bodies.insert(sheet, bodies, colliders) + ball = bodies.insert(rp.RigidBody.dynamic(translation=(-4.0, 6.0, 0.0))) + colliders.insert_with_parent(rp.Collider.ball(0.6).density(30.0), ball, bodies) + + # A curtain pinned along its top edge, with a heavy box shot through it. + curtain = rp.SoftBody.cloth( + (0.0, 4.5, 4.0), (0.1, 0.0, 0.0), (0.0, -0.1, 0.0), 50, 40, + pinned_particles=[i * 40 for i in range(50)], material=cloth_material(0.2), + particle_mass=0.02, + ) + soft_bodies.insert(curtain, bodies, colliders) + bullet = bodies.insert(rp.RigidBody.dynamic(translation=(2.5, 2.5, 12.0), linvel=(0.0, 0.0, -25.0))) + colliders.insert_with_parent( + rp.Collider.cuboid(0.3, 0.3, 0.3).rotation((0.5, 0.5, 0.5)).density(20.0), bullet, bodies + ) + + # A jelly bar pinned at both ends, torn by pulling its right end away. + bar_material = rp.SoftBodyMaterial(young_modulus=5.0e4, poisson_ratio=0.3, elastic_damping_ratio=1.0, + tear_strain=0.4) + bar = rp.SoftBody.cuboid( + (3.0, 1.0, -4.0), (2.0, 0.4, 0.4), 21, 5, 5, + cell_model=rp.SoftBodyCellModel.COROTATIONAL, material=bar_material, particle_mass=0.05, + particle_radius=0.1, surface_collider=rp.Collider.ball(0.1).friction(0.8), + ) + bar_handle = soft_bodies.insert(bar, bodies, colliders) + sb = soft_bodies[bar_handle] + ends = [] + for i in range(sb.num_particles): + p = sb.particle_position(i) + if p.x < 1.01: + ends.append({"body": bar_handle, "particle": i, "rest": (p.x, p.y, p.z), "right": False}) + elif p.x > 4.99: + ends.append({"body": bar_handle, "particle": i, "rest": (p.x, p.y, p.z), "right": True}) + for end in ends: + sb.set_particle_pinned(end["particle"], True) + + # The tears of every step, to follow the driven particles through them. + collector = rp.ChannelEventCollector() + testbed.set_event_handler(collector) + state = {"t": 0.0} + + def drive(tb) -> None: + state["t"] += tb._integration_parameters.dt + # The right end starts moving after a second, at half a meter per second, and stops + # once the bar has doubled its length. + shift = min(max(state["t"] - 1.0, 0.0) * 0.5, 4.0) + for end in ends: + if end["right"] and end["body"] in tb.soft_bodies: + rest = end["rest"] + tb.soft_bodies[end["body"]].set_particle_kinematic_target( + end["particle"], (rest[0] + shift, rest[1], rest[2]) + ) + for event in collector.drain_soft_body_tear_events(): + for end in ends: + if event.soft_body == end["body"]: + destination = event.particle_destination(end["particle"]) + if destination is not None: + end["body"], end["particle"] = destination + + testbed.add_callback(drive) + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints, soft_bodies) + testbed.look_at((9.0, 8.0, 16.0), (0.0, 1.5, 1.0)) + + +register(CATEGORY, NAME, init_world, dim=3) + + +if __name__ == "__main__": + from .._picker import run + + run(initial=f"{CATEGORY} / {NAME}") diff --git a/python/tests/test_examples.py b/python/tests/test_examples.py index 530719a8e..757dd88d2 100644 --- a/python/tests/test_examples.py +++ b/python/tests/test_examples.py @@ -50,6 +50,12 @@ # perf/many_bodies prints a timing-dependent ms/frame value; we only # assert that it ran with the expected shape. "perf/many_bodies.py": "perf: bodies=100 frames=240 ms_per_frame_present=True", + # Soft-body positions depend on floating-point details; assert the shape of the + # result: the cloth hangs over the ball, the jelly rests on the ground, and the + # requested tear was reported (it split one particle: 256 -> 257). + "soft/cloth_drop.py": re.compile( + r"^soft: cloth particles=257 lowest_y=\+0\.[3-9]\d jelly_y=\+0\.[4-9]\d tears=1$" + ), "parity/balls3.py": ( "parity: (1, 1, 1)=(-1.0,+1.5,-1.0) (2, 2, 2)=(-0.0,+2.5,-0.0) " "(2, 3, 2)=(-0.0,+3.5,-0.0)" diff --git a/python/tests/test_soft_bodies.py b/python/tests/test_soft_bodies.py new file mode 100644 index 000000000..83866cfb5 --- /dev/null +++ b/python/tests/test_soft_bodies.py @@ -0,0 +1,488 @@ +"""Soft-body tests (3D, f32): builders, the set, particles, clusters, deformable +colliders, tearing and cutting, events, settings and snapshots.""" + +from __future__ import annotations + +import pickle + +import numpy as np +import pytest + +import rapier3d as rp + + +def _ground_world(): + w = rp.PhysicsWorld(gravity=(0, -9.81, 0)) + w.colliders.insert(rp.Collider.cuboid(20, 0.1, 20).build()) + return w + + +def _cloth(n=8, **kwargs): + return rp.SoftBody.cloth((-1, 2, -1), (0.25, 0, 0), (0, 0, 0.25), n, n, **kwargs) + + +# ---- builders -------------------------------------------------------------- + + +def test_generators_and_kwargs(): + cloth = _cloth(pinned_particles=[0, 3], softness=(30.0, 1.0), particle_mass=0.05) + assert isinstance(cloth, rp.SoftBodyBuilder) + assert cloth.num_particles == 64 + assert cloth.positions.shape == (64, 3) + assert cloth.current_edges.shape[1] == 2 + rope = rp.SoftBody.rope((0, 1, 0), (1, 1, 0), 10) + assert rope.num_particles == 10 + cube = rp.SoftBody.cuboid((0, 1, 0), (0.5, 0.5, 0.5), 3, 3, 3) + assert cube.num_particles == 27 + assert cube.current_cells.shape[1] == 4 + sphere = rp.SoftBody.sphere((0, 1, 0), 0.5, 1) + assert sphere.num_particles > 12 + with pytest.raises(TypeError): + rp.SoftBody.rope((0, 1, 0), (1, 1, 0), 10, not_a_kwarg=3) + + +def test_builder_chaining_returns_new_builder(): + base = _cloth() + heavier = base.particle_mass(2.0).softness(rp.SpringCoefficients(10.0, 1.0)) + assert isinstance(heavier, rp.SoftBodyBuilder) + assert heavier is not base + pinned = base.pinned_particles(np.array([0, 1], dtype=np.uint32)) + assert isinstance(pinned, rp.SoftBodyBuilder) + + +def test_material_kwargs_and_fields(): + m = rp.SoftBodyMaterial(young_modulus=2.0e3, poisson_ratio=0.3, tear_strain=0.4) + assert m.young_modulus == pytest.approx(2.0e3) + assert m.tear_strain == pytest.approx(0.4) + assert m.tears() + m.tear_strain = None + assert m.tear_strain is None + assert not m.tears() + m.edge_softness = (12.0, 0.5) + assert m.edge_softness.natural_frequency == pytest.approx(12.0) + u = rp.SoftBodyMaterial.uniform((20.0, 1.0)) + assert u.bend_softness.natural_frequency == pytest.approx(20.0) + lam, mu = m.lame_parameters() + assert lam > 0 and mu > 0 + with pytest.raises(TypeError): + rp.SoftBodyMaterial(nope=1) + + +def test_custom_positions_builder(): + positions = np.array([[0, 1, 0], [1, 1, 0], [0, 1, 1]], dtype=np.float32) + b = rp.SoftBodyBuilder(positions).edges([[0, 1], [1, 2]]).cells(np.zeros((0, 4), dtype=np.uint32)) + assert b.num_particles == 3 + assert b.current_edges.shape == (2, 2) + + +# ---- set and views ----------------------------------------------------------- + + +def test_insert_creates_root_body_and_colliders(): + w = _ground_world() + nb, nc = len(w.rigid_bodies), len(w.colliders) + h = w.add_soft_body(_cloth(pinned_particles=[0])) + assert h in w.soft_bodies + assert len(w.soft_bodies) == 1 + sb = w.soft_bodies[h] + assert sb.num_particles == 64 + root = w.rigid_bodies[sb.root_body] + assert root.is_soft_frame + assert root.body_type == rp.RigidBodyType.SOFT_FRAME + assert root.soft_body == h + assert root.soft_cluster == 0 + assert len(w.rigid_bodies) == nb + 1 + assert len(w.colliders) == nc + 1 + surface = w.colliders[sb.collision_mesh().collider] + assert surface.deformable_mesh_ref.body == h + assert sb.particle_positions.shape == (64, 3) + assert sb.boundary.shape[1] == 3 + assert sb.particle(0).is_pinned + assert not sb.particle(1).is_pinned + for handle, body in w.soft_bodies: + assert handle == h and body.num_particles == 64 + assert w.soft_bodies.handles() == [h] + assert w.soft_bodies.get(rp.SoftBodyHandle.invalid()) is None + with pytest.raises(rp.InvalidHandle): + w.soft_bodies[rp.SoftBodyHandle.invalid()] + + +def test_cloth_falls_and_pinned_corner_stays(): + w = _ground_world() + h = w.add_soft_body(_cloth(pinned_particles=[0], softness=(30.0, 1.0))) + sb = w.soft_bodies[h] + p0 = sb.particle_position(0) + before = sb.center_of_mass.y + for _ in range(60): + w.step() + assert sb.center_of_mass.y < before - 0.2 + after = sb.particle_position(0) + assert abs(after.x - p0.x) < 1e-5 and abs(after.y - p0.y) < 1e-5 + assert sb.particle_velocities.shape == (64, 3) + assert sb.topology_version == 0 + + +def test_particle_setters_and_impulses(): + w = rp.PhysicsWorld(gravity=(0, 0, 0)) + h = w.add_soft_body(rp.SoftBody.rope((0, 1, 0), (1, 1, 0), 5)) + sb = w.soft_bodies[h] + sb.set_particle_position(2, (0.5, 3.0, 0.0)) + assert sb.particle_position(2).y == pytest.approx(3.0) + sb.set_particle_velocity(2, (0, 1, 0)) + assert sb.particle_velocity(2).y == pytest.approx(1.0) + sb.set_particle_pinned(0, True) + assert sb.particle(0).is_pinned + sb.apply_impulse((0, 1, 0)) + sb.apply_particle_impulse(1, (0, 1, 0)) + sb.apply_impulse_at_point((0, 1, 0), (0.5, 1, 0), 1.0) + sb.apply_radial_impulse((0.5, 1, 0), 1.0, 2.0) + sb.add_force((0, 1, 0)) + sb.add_particle_force(1, (0, 1, 0)) + sb.reset_forces() + # Kinematic targets drive pinned particles only. + sb.set_particle_pinned(4, True) + sb.set_particle_kinematic_target(4, (1, 2, 0)) + w.step() + assert sb.particle_position(4).y > 1.5 + with pytest.raises(IndexError): + sb.set_particle_position(99, (0, 0, 0)) + + +def test_attach_particle_to_rigid_body(): + w = rp.PhysicsWorld(gravity=(0, -9.81, 0)) + h = w.add_soft_body(rp.SoftBody.rope((0, 3, 0), (2, 3, 0), 10, pinned_particles=[0], softness=(40.0, 1.0))) + sb = w.soft_bodies[h] + last = sb.particle_position(9) + weight = w.add_body(rp.RigidBody.dynamic(translation=(last.x, last.y - 0.3, last.z)), + colliders=[rp.Collider.ball(0.2).density(2.0)]) + sb.attach_particle(9, weight, w.rigid_bodies) + assert len(sb.particle_attachments) == 1 + assert sb.particle_attachments[0].body == weight + for _ in range(60): + w.step() + # The weight hangs from the rope: it stays near the rope's last particle. + tip = sb.particle_position(9) + wp = w.rigid_bodies[weight].translation + assert abs(wp.x - tip.x) < 0.6 and abs(wp.y - tip.y) < 0.6 + assert sb.detach_particle(9) + assert not sb.detach_particle(9) + + +def test_material_and_volume_views(): + w = rp.PhysicsWorld(gravity=(0, 0, 0)) + h = w.add_soft_body(rp.SoftBody.sphere((0, 1, 0), 0.5, 1, softness=(20.0, 1.0), volume_factor=1.2)) + sb = w.soft_bodies[h] + assert sb.volume_preservation_enabled + assert sb.volume_factor == pytest.approx(1.2) + assert sb.volume == pytest.approx(sb.rest_volume) + assert len(sb.volume_pieces) >= 1 + m = sb.material + m.edge_softness = (5.0, 1.0) + sb.material = m + assert sb.material.edge_softness.natural_frequency == pytest.approx(5.0) + sb.volume_factor = 1.5 + assert sb.volume_factor == pytest.approx(1.5) + sb.enable_volume_preservation(False) + assert not sb.volume_preservation_enabled + jelly = w.add_soft_body(rp.SoftBody.cuboid((3, 1, 0), (0.5, 0.5, 0.5), 3, 3, 3, + cell_model=rp.SoftBodyCellModel.COROTATIONAL)) + jb = w.soft_bodies[jelly] + assert jb.cell_model == rp.SoftBodyCellModel.COROTATIONAL + assert jb.cells.shape == (jb.num_cells, 4) + assert jb.cell(0).rest_volume > 0 + assert jb.cell(0).plastic_stretch().shape == (3, 3) + assert jb.mass == pytest.approx(27.0) + jb.reset_plasticity() + jb.user_data = 42 + assert jb.user_data == 42 + + +# ---- clusters and deformable colliders ----------------------------------------- + + +def _jelly_world(): + w = _ground_world() + h = w.add_soft_body(rp.SoftBody.cuboid((0, 1.2, 0), (0.6, 0.6, 0.6), 4, 4, 4, + cell_model=rp.SoftBodyCellModel.COROTATIONAL, + material=rp.SoftBodyMaterial(young_modulus=4e3, poisson_ratio=0.4), + particle_mass=0.1)) + return w, h + + +def test_cluster_proxy_joint_and_removal(): + w, h = _jelly_world() + sb = w.soft_bodies[h] + top = [i for i in range(sb.num_particles) if sb.particle_position(i).y > 1.5] + assert len(top) == 16 + cluster = w.add_soft_body_cluster(h, top) + assert cluster == 1 + info = sb.cluster(cluster) + assert info.is_live and sorted(info.particles) == sorted(top) + proxy = sb.cluster_proxy(cluster) + assert w.rigid_bodies[proxy].soft_cluster == cluster + assert sb.num_live_clusters == 2 + plate = w.add_body(rp.RigidBody.dynamic(translation=(0, 1.9, 0)), + colliders=[rp.Collider.cuboid(0.7, 0.05, 0.7).density(0.4)]) + joint = w.impulse_joints.insert(plate, proxy, rp.FixedJointBuilder().local_anchor1((0, -0.1, 0)).build(), True) + sb.set_cluster_stiffness_scale(cluster, 2.0) + sb.set_cluster_edge_softness(cluster, (50.0, 1.0)) + sb.set_cluster_edge_softness(cluster, None) + sb.set_cluster_tear_resistance(cluster, 2.0) + sb.enable_cluster_shape_matching(cluster, True) + for _ in range(30): + w.step() + assert w.rigid_bodies[plate].translation.y > 1.0 + assert w.remove_soft_body_cluster(h, cluster) + assert joint not in w.impulse_joints + assert proxy not in w.rigid_bodies + assert not w.remove_soft_body_cluster(h, cluster) + assert w.add_soft_body_cluster(h, [999]) is None + + +def test_cluster_pinned_and_kinematic_target(): + w, h = _jelly_world() + sb = w.soft_bodies[h] + bottom = [i for i in range(sb.num_particles) if sb.particle_position(i).y < 0.7] + cluster = w.add_soft_body_cluster(h, bottom) + sb.set_cluster_pinned(cluster, True) + assert sb.particle(bottom[0]).is_pinned + sb.set_cluster_kinematic_target(cluster, rp.Isometry3(translation=(0.5, 0.6, 0.0))) + w.step() + assert sb.particle_position(bottom[0]).x > -0.6 + + +def test_deformable_collider_binding(): + w, h = _jelly_world() + sb = w.soft_bodies[h] + root = sb.root_body + origin = w.rigid_bodies[root].translation + c = sb.center_of_mass + r = 1.0 + verts = np.array([[c.x + r, c.y, c.z], [c.x - r, c.y, c.z], [c.x, c.y + r, c.z], + [c.x, c.y - r, c.z], [c.x, c.y, c.z + r], [c.x, c.y, c.z - r]], dtype=np.float32) + verts -= np.array([origin.x, origin.y, origin.z], dtype=np.float32) + idx = np.array([[0, 2, 4], [2, 1, 4], [1, 3, 4], [3, 0, 4], [2, 0, 5], [1, 2, 5], [3, 1, 5], [0, 3, 5]], dtype=np.uint32) + skin = w.insert_deformable(rp.Collider.trimesh(verts, idx, rp.TriMeshFlags.DEFORMABLE).sensor(True), + rp.SoftMeshBinding.skinned(), root) + ref = w.colliders[skin].deformable_mesh_ref + assert ref.body == h and ref.id.cluster == 0 + meshes = sb.meshes + assert len(meshes) == 2 + mesh = sb.mesh_of(skin) + assert mesh.is_skinned and mesh.vertex_count == 6 and mesh.indices.shape == (8, 3) + assert sb.mesh(ref.id).collider == skin + top_before = mesh.vertices[2, 1] + for _ in range(30): + w.step() + assert sb.mesh_of(skin).vertices[2, 1] < top_before + # A plain (non-deformable) mesh cannot be bound. + with pytest.raises(rp.SoftBindingError): + w.insert_deformable(rp.Collider.trimesh(verts, idx), rp.SoftMeshBinding.skinned(), root) + # Neither can a parent that is not a cluster proxy. + ball = w.add_body(rp.RigidBody.dynamic(translation=(5, 5, 5))) + with pytest.raises(rp.SoftBindingError): + w.insert_deformable(rp.Collider.trimesh(verts, idx, rp.TriMeshFlags.DEFORMABLE), + rp.SoftMeshBinding.skinned(), ball) + w.remove_collider(skin) + assert len(sb.meshes) == 1 + direct = rp.SoftMeshBinding.direct([0, 1, 2]).self_contacts(True) + assert isinstance(direct, rp.SoftMeshBinding) + assert isinstance(rp.SoftMeshBinding.direct_by_position(0.1), rp.SoftMeshBinding) + + +# ---- tearing and cutting --------------------------------------------------------- + + +def test_cut_splits_cloth_into_pieces(): + w = _ground_world() + n = 8 + h = w.add_soft_body(_cloth(n, pinned_particles=[0, n * (n - 1)], softness=(30.0, 1.0))) + sb = w.soft_bodies[h] + ev = w.cut_soft_body(h, ((-0.1, -10, -10), (-0.1, 10, 0), (-0.1, -10, 10))) + assert ev is not None + assert ev.soft_body == h + assert ev.torn_edges.shape[1] == 2 and ev.torn_edges.shape[0] > 0 + assert len(ev.pieces) == 2 + assert len(w.soft_bodies) == 2 + piece = [p for p in ev.pieces if p.soft_body != h][0] + assert w.soft_bodies[piece.soft_body].origin == h + assert sb.pieces == [piece.soft_body] + dest = ev.particle_destination(n * n - 1) + assert dest is not None and dest[0] in w.soft_bodies + assert sb.topology_version >= 1 + assert ev.seeds().shape[1] == 2 + assert len(ev.bodies()) == 2 + for _ in range(10): + w.step() + # Nothing changes when the blade misses. + assert w.cut_soft_body(h, ((50, 0, 0), (50, 1, 0), (50, 0, 1))) is None + + +def test_tear_requests_and_events(): + w = _ground_world() + h = w.add_soft_body(_cloth(6, pinned_particles=[0], softness=(30.0, 1.0))) + sb = w.soft_bodies[h] + collector = rp.ChannelEventCollector() + w.event_handler = collector + sb.tear_edge(3) + assert sb.has_pending_tears + w.step() + events = collector.drain_soft_body_tear_events() + assert len(events) == 1 and events[0].soft_body == h and events[0].torn_edges.shape[0] == 1 + assert not sb.has_pending_tears + + class Handler: + def __init__(self): + self.events = [] + + def handle_collision_event(self, *args): + pass + + def handle_contact_force_event(self, *args): + pass + + def handle_soft_body_tear_event(self, soft_bodies, event): + self.events.append(event) + + handler = Handler() + w.event_handler = handler + ev = w.tear_soft_body(h, [10], []) + assert ev is not None and ev.torn_edges.shape[0] == 1 + assert handler.events == [] # immediate tears return their event instead + jelly = w.add_soft_body(rp.SoftBody.cuboid((3, 1, 0), (0.5, 0.5, 0.5), 3, 3, 3, + cell_model=rp.SoftBodyCellModel.COROTATIONAL)) + w.soft_bodies[jelly].tear_cell(0) + w.step() + assert len(handler.events) == 1 and handler.events[0].soft_body == jelly + assert handler.events[0].torn_cells.shape[0] >= 1 + + +def test_strain_tearing_splits_a_stretched_rope(): + w = rp.PhysicsWorld(gravity=(0, 0, 0)) + h = w.add_soft_body(rp.SoftBody.rope((0, 1, 0), (1, 1, 0), 6, pinned_particles=[0, 5], + softness=(60.0, 1.0), tear_strain=0.3, min_piece=1)) + collector = rp.ChannelEventCollector() + w.event_handler = collector + events = [] + # Pull the pinned end away until an edge passes the tear strain. + for k in range(60): + w.soft_bodies[h].set_particle_kinematic_target(5, (1.0 + 0.05 * (k + 1), 1, 0)) + w.step() + events += collector.drain_soft_body_tear_events() + if events: + break + assert events, "the stretched rope should have torn" + assert events[0].soft_body == h + assert len(w.soft_bodies) == 2 + + +# ---- settings, events, snapshots ------------------------------------------------ + + +def test_integration_parameters_soft_settings(): + ip = rp.IntegrationParameters() + s = ip.soft_bodies + assert isinstance(s, rp.SoftBodiesSettings) + assert s.resweep_strain == pytest.approx(0.75) + assert s.max_extra_substeps == 4 + s.max_extra_substeps = 2 + s.contact_stiffening = 3.0 + rec = s.recovery + rec.crossing_repulsion = False + rec.overlap_patch_constraints = rp.SoftPatchConstraints.KEEP + s.recovery = rec + ip.soft_bodies = s + assert ip.soft_bodies.max_extra_substeps == 2 + assert ip.soft_bodies.contact_stiffening == pytest.approx(3.0) + assert not ip.soft_bodies.recovery.crossing_repulsion + ps = rp.SoftBodyParticleSettings() + assert ps.additional_pgs_iterations == 3 + b = rp.RigidBody.dynamic(additional_pgs_iterations=2).build() + assert b.additional_pgs_iterations == 2 + + +def test_snapshot_round_trip_keeps_soft_bodies(): + w = _ground_world() + h = w.add_soft_body(_cloth(6, pinned_particles=[0])) + for _ in range(10): + w.step() + snap = w.snapshot() + w2 = rp.PhysicsWorld.restore(snap) + assert len(w2.soft_bodies) == 1 + a = w.soft_bodies[h].particle_positions + b = w2.soft_bodies[h].particle_positions + assert np.allclose(a, b) + assert w2.rigid_bodies[w2.soft_bodies[h].root_body].is_soft_frame + w2.step() + w3 = rp.PhysicsWorld.restore_json(w.snapshot_json()) + assert len(w3.soft_bodies) == 1 + assert np.allclose(w3.soft_bodies[h].particle_positions, a) + assert pickle.loads(pickle.dumps(h)) == h + assert hash(h) == hash(rp.SoftBodyHandle.from_raw_parts(*h.into_raw_parts())) + + +def test_removals(): + w = _ground_world() + h = w.add_soft_body(_cloth(6)) + sb = w.soft_bodies[h] + root = sb.root_body + nb, nc = len(w.rigid_bodies), len(w.colliders) + removed = w.remove_soft_body(h) + assert isinstance(removed, rp.SoftBody) and removed.num_particles == 36 + assert h not in w.soft_bodies + assert root not in w.rigid_bodies + assert len(w.rigid_bodies) == nb - 1 and len(w.colliders) == nc - 1 + assert w.remove_soft_body(h) is None + # Removing the root proxy through the rigid-body set removes the soft body. + h2 = w.add_soft_body(_cloth(6)) + root2 = w.soft_bodies[h2].root_body + w.remove_body(root2) + assert h2 not in w.soft_bodies + # The low-level sets accept an explicit soft-body set. + h3 = w.add_soft_body(_cloth(6)) + surface = w.soft_bodies[h3].collision_mesh().collider + w.colliders.remove(surface, w.islands, w.rigid_bodies, soft_bodies=w.soft_bodies) + assert w.soft_bodies[h3].collision_mesh() is None + w.step() + + +def test_debug_render_soft_bodies(): + w = _ground_world() + w.add_soft_body(_cloth(6)) + pipeline = rp.DebugRenderPipeline(rp.DebugRenderMode.SOFT_BODIES) + lines, colors, objects = pipeline.render_to_arrays( + w.rigid_bodies, w.colliders, w.impulse_joints, w.multibody_joints, w.narrow_phase, w.soft_bodies + ) + assert lines.shape[0] > 0 and lines.shape[1:] == (2, 3) + assert rp.DebugRenderObject.SOFT_BODY.kind in set(objects.tolist()) + # Without the soft bodies, nothing is drawn for them. + lines2, _, objects2 = pipeline.render_to_arrays( + w.rigid_bodies, w.colliders, w.impulse_joints, w.multibody_joints, w.narrow_phase + ) + assert rp.DebugRenderObject.SOFT_BODY.kind not in set(objects2.tolist()) + style = rp.DebugRenderStyle() + assert isinstance(style.soft_body_element_color, rp.DebugColor) + + +def test_low_level_pipeline_step_with_soft_bodies(): + bodies = rp.RigidBodySet() + colliders = rp.ColliderSet() + soft = rp.SoftBodySet() + h = soft.insert(_cloth(6, pinned_particles=[0]), bodies, colliders) + assert h in soft and len(soft) == 1 and not soft.is_empty() + islands = rp.IslandManager() + bp = rp.BroadPhaseBvh() + np_ = rp.NarrowPhase() + ij = rp.ImpulseJointSet() + mj = rp.MultibodyJointSet() + ccd = rp.CCDSolver() + params = rp.IntegrationParameters() + pipeline = rp.PhysicsPipeline() + before = soft[h].center_of_mass.y + for _ in range(20): + pipeline.step((0, -9.81, 0), params, islands, bp, np_, bodies, colliders, ij, mj, ccd, + soft_bodies=soft) + assert soft[h].center_of_mass.y < before + soft.wake_up(h, bodies, True) + removed = soft.remove(h, islands, bodies, colliders, ij, mj) + assert removed is not None and len(soft) == 0 From 6f8759500283ed53baa77385f737c29a4a528b18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 18 Sep 2026 00:38:14 +0200 Subject: [PATCH 27/32] docs(website): soft-body examples for the Rust and JS snippets, and the changed pipeline signatures --- .../docs-examples/2d/javascript/src/index.ts | 1 + .../2d/javascript/src/snippets/soft_bodies.ts | 222 +++++++++++++ .../rs_advanced_collision_detection2.rs | 45 ++- .../2d/rust/examples/rs_basic_sim2.rs | 2 + .../rust/examples/rs_character_controller2.rs | 2 + .../2d/rust/examples/rs_scene_queries2.rs | 2 + .../2d/rust/examples/rs_serialization.rs | 4 + .../examples/rs_simulation_structures2.rs | 2 + .../2d/rust/examples/rs_soft_bodies2.rs | 264 ++++++++++++++++ .../docs-examples/3d/javascript/src/index.ts | 1 + .../3d/javascript/src/snippets/soft_bodies.ts | 231 ++++++++++++++ .../3d/rust/examples/rs_basic_sim3.rs | 2 + .../rust/examples/rs_character_controller3.rs | 2 + .../3d/rust/examples/rs_scene_queries3.rs | 2 + .../3d/rust/examples/rs_soft_bodies3.rs | 299 ++++++++++++++++++ website/docs-examples/Cargo.toml | 6 + 16 files changed, 1071 insertions(+), 16 deletions(-) create mode 100644 website/docs-examples/2d/javascript/src/snippets/soft_bodies.ts create mode 100644 website/docs-examples/2d/rust/examples/rs_soft_bodies2.rs create mode 100644 website/docs-examples/3d/javascript/src/snippets/soft_bodies.ts create mode 100644 website/docs-examples/3d/rust/examples/rs_soft_bodies3.rs diff --git a/website/docs-examples/2d/javascript/src/index.ts b/website/docs-examples/2d/javascript/src/index.ts index a2fc34c98..6a0a7a7fe 100644 --- a/website/docs-examples/2d/javascript/src/index.ts +++ b/website/docs-examples/2d/javascript/src/index.ts @@ -6,4 +6,5 @@ require('./snippets/getting_started') require('./snippets/joints') require('./snippets/rigid_bodies') require('./snippets/scene_queries') +require('./snippets/soft_bodies') require('./snippets/standalone') diff --git a/website/docs-examples/2d/javascript/src/snippets/soft_bodies.ts b/website/docs-examples/2d/javascript/src/snippets/soft_bodies.ts new file mode 100644 index 000000000..1f10ca08e --- /dev/null +++ b/website/docs-examples/2d/javascript/src/snippets/soft_bodies.ts @@ -0,0 +1,222 @@ +import RAPIER from '@dimforge/rapier2d'; + +{ + // DOCUSAURUS: Creation start + // The world that will contain our soft bodies. + let world = new RAPIER.World({ x: 0.0, y: -9.81 }); + world.createCollider(RAPIER.ColliderDesc.cuboid(10.0, 0.1)); + + // Description of a rope of 20 particles between two points. + let example1 = RAPIER.SoftBodyDesc.rope({ x: 0.0, y: 3.0 }, { x: 2.0, y: 3.0 }, 20); + // Description of a grid of `nx` by `ny` particles filled with triangle cells. + let example2 = RAPIER.SoftBodyDesc.grid({ x: 3.0, y: 1.0 }, { x: 1.0, y: 1.0 }, 6, 6); + // Description of a disk: a ring of particles holding its area (a pressurized blob). + let example3 = RAPIER.SoftBodyDesc.disk({ x: 0.0, y: 3.0 }, 0.8, 24); + // Description of a closed polygon of particles holding its area. + let example4 = RAPIER.SoftBodyDesc.polygon([5.0, 4.0, 7.0, 4.0, 7.0, 6.0, 5.0, 6.0]); + // Description over raw particle positions; the elements are added by the setters. + let example5 = new RAPIER.SoftBodyDesc([0.0, 1.0, 1.0, 1.0]).setEdges([0, 1]); + + let n = 20; + let sheetDesc = RAPIER.SoftBodyDesc.grid({ x: -3.0, y: 3.0 }, { x: 1.0, y: 1.0 }, n, n) + // Particles held in place. + .setPinnedParticles([0, n - 1]) + // A uniform softness (natural frequency in Hz, damping ratio) for every constraint. + .setSoftness(30.0, 1.0) + // The mass of each particle. + // Default: 1.0 + .setParticleMass(0.05) + // The thickness of the particles, for collisions. + // Default: 0.01 + .setParticleRadius(0.05) + // The template of the body's colliders: its shape is replaced by the deformable surface. + .setSurfaceCollider(RAPIER.ColliderDesc.ball(0.05).setFriction(0.8)) + // Whether the body may fall asleep. + // Default: true + .setCanSleep(true); + // Create the soft body: this creates its hidden root rigid body and its colliders. + let sheet = world.createSoftBody(sheetDesc); + // The integer handle of the soft body can be read from the `handle` field. + let sheetHandle = sheet.handle; + // DOCUSAURUS: Creation stop + + // DOCUSAURUS: Material start + // Elastic cells: a jelly square with corotational linear elasticity. + let material = new RAPIER.SoftBodyMaterial(); + // Stiffness of the elastic cells. + material.youngModulus = 3.0e3; + material.poissonRatio = 0.35; + material.elasticDampingRatio = 0.5; + // Plasticity: the rest shape flows past 5% strain, at 20 per second. + material.plasticYield = 0.05; + material.plasticCreep = 20.0; + // Tearing: an element past 40% strain tears. + material.tearStrain = 0.4; + let jellyDesc = RAPIER.SoftBodyDesc.grid({ x: 3.0, y: 1.2 }, { x: 1.0, y: 1.0 }, 6, 6) + // The constitutive model of the cells: `Volume` (per-cell area constraints, + // the shape is held by the edges), `Corotational` or `NeoHookean`. + .setCellModel(RAPIER.SoftBodyCellModel.Corotational) + .setMaterial(material) + .setParticleMass(0.2); + let jelly = world.createSoftBody(jellyDesc); + + // A pressurized blob: a ring of particles inflated by area preservation. + let blobDesc = RAPIER.SoftBodyDesc.disk({ x: 0.0, y: 3.0 }, 0.8, 24) + .setSoftness(20.0, 1.0) + // Target area multiplier (`> 1` inflates the body); enables area preservation. + .setVolumeFactor(1.1) + .setSelfContacts(true); + let blob = world.createSoftBody(blobDesc); + // DOCUSAURUS: Material stop + + // DOCUSAURUS: Particles start + // Read the particles. + let position = sheet.particlePosition(0); + let velocity = sheet.particleVelocity(0); + let positions: Float32Array = sheet.particlePositions(); // Two floats per particle. + console.log("The sheet has", sheet.numParticles(), "particles;", positions.length / 2); + // Move a particle. + sheet.setParticlePosition(1, { x: position.x, y: position.y + 0.1 }); + sheet.setParticleVelocity(1, velocity); + // Pin (or release) a particle; a pinned particle can be driven like a kinematic body. + sheet.setParticlePinned(2, true); + sheet.setParticleKinematicTarget(2, { x: -3.5, y: 3.5 }); + // The elements: edges (two indices each), cells (three) and the boundary segments (two). + let edges: Uint32Array = sheet.edges(); + let cells: Uint32Array = sheet.cells(); + let boundary: Uint32Array = sheet.boundary(); + console.log(edges.length / 2, "edges,", cells.length / 3, "cells,", boundary.length / 2, "segments"); + // DOCUSAURUS: Particles stop + + // DOCUSAURUS: Forces start + // The `true` argument makes sure the soft body is awake. + sheet.resetForces(true); // Reset the forces to zero. + sheet.addForce({ x: 0.0, y: 1.0 }, true); // Spread over the particles by mass. + sheet.addParticleForce(3, { x: 0.0, y: 1.0 }, true); + sheet.applyImpulse({ x: 0.0, y: 0.1 }, true); + sheet.applyParticleImpulse(3, { x: 0.0, y: 0.1 }, true); + // An impulse on the particles within 0.5 of a point, scaled down with the distance. + sheet.applyImpulseAtPoint({ x: 0.0, y: 0.1 }, { x: -3.0, y: 3.0 }, 0.5, true); + // A blast pushing the particles away from a center. + sheet.applyRadialImpulse({ x: -3.0, y: 3.0 }, 0.1, 1.0, true); + // DOCUSAURUS: Forces stop + + // DOCUSAURUS: Attachments start + // Attach the last particle of a rope to a rigid box, at the particle's position. + let ropeDesc = RAPIER.SoftBodyDesc.rope({ x: 8.0, y: 9.0 }, { x: 12.0, y: 9.0 }, 25) + .setPinnedParticles([0]) + .setSoftness(40.0, 1.0); + let rope = world.createSoftBody(ropeDesc); + let last = rope.particlePosition(24); + let weight = world.createRigidBody(RAPIER.RigidBodyDesc.dynamic().setTranslation(last.x, last.y - 0.4)); + world.createCollider(RAPIER.ColliderDesc.cuboid(0.3, 0.3).setDensity(2.0), weight); + rope.attachParticle(24, weight); + // The hidden rigid body standing for the whole soft body in joints and islands. + let rootBody = rope.rootBody(); + console.log("root body is a soft frame:", rootBody.isSoftFrame()); + // DOCUSAURUS: Attachments stop + + // DOCUSAURUS: Clusters start + // A cluster over the top particles of the jelly: a rigid proxy that joints and + // colliders can attach to. + let top = []; + for (let i = 0; i < jelly.numParticles(); ++i) { + if (jelly.particlePosition(i).y > 2.0) { + top.push(i); + } + } + let cluster = world.addSoftBodyCluster(jelly, top); + let proxy = jelly.clusterProxy(cluster); + // A rigid plate welded onto the cluster. + let plate = world.createRigidBody(RAPIER.RigidBodyDesc.dynamic().setTranslation(3.0, 2.4)); + world.createCollider(RAPIER.ColliderDesc.cuboid(1.2, 0.05).setDensity(0.4), plate); + let weld = RAPIER.JointData.fixed({ x: 0.0, y: -0.1 }, 0.0, { x: 0.0, y: 0.0 }, 0.0); + world.createImpulseJoint(weld, plate, proxy, true); + // A cluster can be pinned, driven or tuned as a whole. + jelly.setClusterStiffnessScale(cluster, 2.0); + jelly.enableClusterShapeMatching(cluster, true); + // DOCUSAURUS: Clusters stop + + // DOCUSAURUS: DeformableColliders start + // A deformable polyline bound to the blob: each vertex follows one particle (`direct`), + // or is embedded in the cell holding it (`skinned`). The polyline is given in the frame + // of the proxy it is attached to. + let root = blob.rootBody(); + let origin = root.translation(); + let num = blob.numParticles(); + let vertices = blob.particlePositions(); + for (let i = 0; i < num; ++i) { + vertices[i * 2] -= origin.x; + vertices[i * 2 + 1] -= origin.y; + } + let indices = new Uint32Array(num * 2); + let particles = []; + for (let i = 0; i < num; ++i) { + indices[i * 2] = i; + indices[i * 2 + 1] = (i + 1) % num; + particles.push(i); + } + let outlineDesc = RAPIER.ColliderDesc.polyline(vertices, indices, RAPIER.PolylineFlags.DEFORMABLE).setSensor(true); + let outline = world.createDeformableCollider(outlineDesc, RAPIER.SoftMeshBinding.direct(particles), root); + // The polyline follows the particles: read its current vertices back. + let meshIndex = blob.meshOfCollider(outline); + let outlineVertices: Float32Array = blob.meshVertices(meshIndex); + console.log("The outline has", outlineVertices.length / 2, "vertices"); + // DOCUSAURUS: DeformableColliders stop + + // DOCUSAURUS: Tearing start + // Elements tear on their own past the material's thresholds; a tear can also be requested. + sheet.tearEdge(10); // Applied at the end of the next step. + // Tear at once along edges and through cells; pieces the tear disconnects become soft + // bodies of their own. + let tear = world.tearSoftBody(sheet, [11, 12], []); + if (tear) { + console.log(tear.tornEdges().length / 2, "edges torn"); + tear.free(); + } + // Cut along a blade (a segment in 2D), without removing material. + let cut = world.cutSoftBody(sheet, [{ x: -3.0, y: -10.0 }, { x: -3.0, y: 10.0 }]); + if (cut) { + for (let i = 0; i < cut.numPieces(); ++i) { + let piece = world.getSoftBody(cut.pieceSoftBody(i)); + console.log("piece", i, "has", piece.numParticles(), "particles"); + } + // Where a particle of the torn body went. + let destination = cut.particleDestination(n * n - 1); + if (destination) { + console.log("particle", n * n - 1, "is now particle", destination.particle, "of", destination.softBody); + } + cut.free(); + } + // DOCUSAURUS: Tearing stop + + // DOCUSAURUS: Events start + // Tears applied during a step are reported through the event queue. + let eventQueue = new RAPIER.EventQueue(true); + world.step(eventQueue); + eventQueue.drainSoftBodyTearEvents((event) => { + console.log("Soft body", event.softBody(), "tore:", event.numPieces(), "pieces split off"); + }); + // DOCUSAURUS: Events stop + + // DOCUSAURUS: Settings start + // Settings shared by every soft body of the world. + // Strain beyond which a constraint is re-solved after the contacts of every substep. + // Default: 0.75 + world.integrationParameters.softBodiesResweepStrain = 0.75; + // Extra substeps a soft body requests while it is hit fast; 0 disables them. + // Default: 4 + world.integrationParameters.softBodiesMaxExtraSubsteps = 4; + // Stiffening of the soft-body contacts relative to the rigid ones. + // Default: 4.0 + world.integrationParameters.softBodiesContactStiffening = 4.0; + // DOCUSAURUS: Settings stop + + // DOCUSAURUS: Removal start + // Removing a soft body removes its root body, its proxies, its colliders and the joints + // attached to them. + world.removeSoftBody(rope); + // A cluster can be removed on its own. + world.removeSoftBodyCluster(jelly, cluster); + // DOCUSAURUS: Removal stop +} diff --git a/website/docs-examples/2d/rust/examples/rs_advanced_collision_detection2.rs b/website/docs-examples/2d/rust/examples/rs_advanced_collision_detection2.rs index 02620f35d..dedaae8af 100644 --- a/website/docs-examples/2d/rust/examples/rs_advanced_collision_detection2.rs +++ b/website/docs-examples/2d/rust/examples/rs_advanced_collision_detection2.rs @@ -26,6 +26,7 @@ fn main() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); let mut multibody_joint_set = MultibodyJointSet::new(); + let mut soft_body_set = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let physics_hooks = (); let event_handler = (); @@ -34,7 +35,9 @@ fn main() { // Initialize the event collector. let (collision_send, collision_recv) = std::sync::mpsc::channel(); let (contact_force_send, contact_force_recv) = std::sync::mpsc::channel(); - let event_handler = ChannelEventCollector::new(collision_send, contact_force_send); + let (soft_body_tear_send, soft_body_tear_recv) = std::sync::mpsc::channel(); + let event_handler = + ChannelEventCollector::new(collision_send, contact_force_send, soft_body_tear_send); physics_pipeline.step( gravity, @@ -46,6 +49,7 @@ fn main() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -60,6 +64,11 @@ fn main() { // Handle the contact force event. println!("Received contact force event: {:?}", contact_force_event); } + + while let Ok(tear_event) = soft_body_tear_recv.try_recv() { + // Handle the soft-body tear event. + println!("Received soft-body tear event: {:?}", tear_event); + } // DOCUSAURUS: Events stop // DOCUSAURUS: ContactGraph1 start @@ -72,7 +81,7 @@ fn main() { } // We may also read the contact manifolds to access the contact geometry. - for manifold in &contact_pair.manifolds { + for manifold in contact_pair.manifolds() { println!("Local-space contact normal: {}", manifold.local_n1); println!("Local-space contact normal: {}", manifold.local_n2); println!("World-space contact normal: {}", manifold.data.normal); @@ -94,10 +103,9 @@ fn main() { // Solver contacts are anchored in the local-space of the body they touch, so // they ride rigidly with it. Resolve them through the bodies' current poses to // get the world-space contact point on each body's surface. - let (point1, point2) = - manifold - .data - .solver_contact_world_points(solver_contact, &rigid_body_set); + let (point1, point2) = manifold + .data + .solver_contact_world_points(solver_contact, &rigid_body_set); println!("Found solver contact points: {point1:?}, {point2:?}"); // The solver contact distance is negative if there is a penetration. println!("Found solver contact distance: {:?}", solver_contact.dist); @@ -158,7 +166,7 @@ fn main() { let user_data2 = context.colliders[context.collider2].user_data; if user_data1 % 2 == 0 && user_data2 % 2 == 0 { - Some(SolverFlags::COMPUTE_IMPULSES) + Some(SolverFlags::COMPUTE_RIGID_IMPULSES) } else if user_data1 == user_data2 { Some(SolverFlags::empty()) } else { @@ -191,28 +199,33 @@ fn main() { // - Set the friction coefficient to 0.3 // - Set the restitution coefficient to 0.4 // - Set the tangent velocities to X * 10.0 - *context.normal = -*context.normal; - - if !context.solver_contacts.is_empty() { - context.solver_contacts.swap_remove(0); + // The contacts of two soft surfaces are candidates rather than a manifold: + // only the manifolds of rigid pairs are modified here. + let ModifiableContacts::Rigid(manifold) = &mut context.contacts else { + return; + }; + *manifold.normal = -*manifold.normal; + + if !manifold.solver_contacts.is_empty() { + manifold.solver_contacts.swap_remove(0); } // Friction and restitution are combined once per manifold, so they are set // for the whole manifold rather than per solver contact. - *context.friction = 0.3; - *context.restitution = 0.4; + *manifold.friction = 0.3; + *manifold.restitution = 0.4; - for solver_contact in &mut *context.solver_contacts { + for solver_contact in &mut *manifold.solver_contacts { solver_contact.tangent_velocity.x = 10.0; } // Use the persistent user-data to count the number of times // contact modification was called for this contact manifold // since its creation. - *context.user_data += 1; + *manifold.user_data += 1; println!( "Contact manifold has been modified {} times since its creation.", - *context.user_data + *manifold.user_data ); } } diff --git a/website/docs-examples/2d/rust/examples/rs_basic_sim2.rs b/website/docs-examples/2d/rust/examples/rs_basic_sim2.rs index cd3d39e1e..c0f7ef459 100644 --- a/website/docs-examples/2d/rust/examples/rs_basic_sim2.rs +++ b/website/docs-examples/2d/rust/examples/rs_basic_sim2.rs @@ -26,6 +26,7 @@ fn main() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); let mut multibody_joint_set = MultibodyJointSet::new(); + let mut soft_body_set = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let physics_hooks = (); let event_handler = (); @@ -42,6 +43,7 @@ fn main() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, diff --git a/website/docs-examples/2d/rust/examples/rs_character_controller2.rs b/website/docs-examples/2d/rust/examples/rs_character_controller2.rs index fccdcde49..e80b0c9e7 100644 --- a/website/docs-examples/2d/rust/examples/rs_character_controller2.rs +++ b/website/docs-examples/2d/rust/examples/rs_character_controller2.rs @@ -28,6 +28,7 @@ fn main() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); let mut multibody_joint_set = MultibodyJointSet::new(); + let mut soft_body_set = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let physics_hooks = (); let event_handler = (); @@ -75,6 +76,7 @@ fn main() { &mut colliders, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, diff --git a/website/docs-examples/2d/rust/examples/rs_scene_queries2.rs b/website/docs-examples/2d/rust/examples/rs_scene_queries2.rs index 0fa178695..b05518a38 100644 --- a/website/docs-examples/2d/rust/examples/rs_scene_queries2.rs +++ b/website/docs-examples/2d/rust/examples/rs_scene_queries2.rs @@ -20,6 +20,7 @@ fn main() { let integration_parameters = IntegrationParameters::default(); let mut physics_pipeline = PhysicsPipeline::new(); let mut island_manager = IslandManager::new(); + let mut soft_body_set = SoftBodySet::new(); let mut broad_phase = DefaultBroadPhase::new(); let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); @@ -41,6 +42,7 @@ fn main() { handle_to_remove, &mut island_manager, &mut rigid_body_set, + &mut soft_body_set, true, ); let collider = collider_set.get_mut(handle1).unwrap(); diff --git a/website/docs-examples/2d/rust/examples/rs_serialization.rs b/website/docs-examples/2d/rust/examples/rs_serialization.rs index 84af147f2..c0f69be33 100644 --- a/website/docs-examples/2d/rust/examples/rs_serialization.rs +++ b/website/docs-examples/2d/rust/examples/rs_serialization.rs @@ -13,6 +13,7 @@ struct PhysicsState { pub colliders: ColliderSet, pub impulse_joints: ImpulseJointSet, pub multibody_joints: MultibodyJointSet, + pub soft_bodies: SoftBodySet, pub ccd_solver: CCDSolver, pub integration_parameters: IntegrationParameters, pub gravity: Vector, @@ -59,6 +60,7 @@ fn setup_physics_scene() -> PhysicsState { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); let mut multibody_joint_set = MultibodyJointSet::new(); + let mut soft_body_set = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let physics_hooks = (); let event_handler = (); @@ -75,6 +77,7 @@ fn setup_physics_scene() -> PhysicsState { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, @@ -91,6 +94,7 @@ fn setup_physics_scene() -> PhysicsState { colliders: collider_set, impulse_joints: impulse_joint_set, multibody_joints: multibody_joint_set, + soft_bodies: soft_body_set, ccd_solver, integration_parameters, gravity, diff --git a/website/docs-examples/2d/rust/examples/rs_simulation_structures2.rs b/website/docs-examples/2d/rust/examples/rs_simulation_structures2.rs index 1cbfed100..c82b36408 100644 --- a/website/docs-examples/2d/rust/examples/rs_simulation_structures2.rs +++ b/website/docs-examples/2d/rust/examples/rs_simulation_structures2.rs @@ -25,6 +25,7 @@ fn main() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); let mut multibody_joint_set = MultibodyJointSet::new(); + let mut soft_body_set = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let physics_hooks = (); let event_handler = (); @@ -41,6 +42,7 @@ fn main() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, diff --git a/website/docs-examples/2d/rust/examples/rs_soft_bodies2.rs b/website/docs-examples/2d/rust/examples/rs_soft_bodies2.rs new file mode 100644 index 000000000..9d8d9f4ab --- /dev/null +++ b/website/docs-examples/2d/rust/examples/rs_soft_bodies2.rs @@ -0,0 +1,264 @@ +use rapier2d::prelude::*; + +fn main() { + // DOCUSAURUS: Creation start + // A world with a ground. + let mut world = PhysicsWorld::new(); + world.insert_collider(ColliderBuilder::cuboid(10.0, 0.1), None); + + // Builder for a rope of 20 particles between two points. + let _ = SoftBodyBuilder::rope(Vector::new(0.0, 3.0), Vector::new(2.0, 3.0), 20); + // Builder for a grid of `nx` by `ny` particles filled with triangle cells. + let _ = SoftBodyBuilder::grid(Vector::new(3.0, 1.0), Vector::new(1.0, 1.0), 6, 6); + // Builder for a disk: a ring of particles holding its area (a pressurized blob). + let _ = SoftBodyBuilder::disk(Vector::new(0.0, 3.0), 0.8, 24); + // Builder for a closed polygon of particles holding its area. + let _ = SoftBodyBuilder::polygon(vec![ + Vector::new(5.0, 4.0), + Vector::new(7.0, 4.0), + Vector::new(7.0, 6.0), + Vector::new(5.0, 6.0), + ]); + let n = 20; + let sheet = SoftBodyBuilder::grid(Vector::new(-3.0, 3.0), Vector::new(1.0, 1.0), n, n) + // Particles held in place. + .pinned_particles([0, (n - 1) as u32]) + // A uniform softness (natural frequency in Hz, damping ratio) for every constraint. + .softness(SpringCoefficients::new(30.0, 1.0)) + // The mass of each particle. + // Default: 1.0 + .particle_mass(0.05) + // The thickness of the particles, for collisions. + // Default: 0.01 + .particle_radius(0.05) + // The template of the body's colliders: its shape is replaced by the deformable surface. + .surface_collider(ColliderBuilder::ball(0.05).friction(0.8)) + // Whether the body may fall asleep. + // Default: true + .can_sleep(true); + // Insert the soft body: this creates its hidden root rigid body and its colliders. + let sheet_handle = world.insert_soft_body(sheet); + // DOCUSAURUS: Creation stop + + // DOCUSAURUS: Sets start + // The sets can also be used directly, without the `PhysicsWorld` façade. + let mut soft_body_set = SoftBodySet::new(); + let mut rigid_body_set = RigidBodySet::new(); + let mut collider_set = ColliderSet::new(); + let rope = SoftBodyBuilder::rope(Vector::new(0.0, 3.0), Vector::new(2.0, 3.0), 20); + let rope_handle = soft_body_set.insert(rope, &mut rigid_body_set, &mut collider_set); + let soft_body = &soft_body_set[rope_handle]; + assert_eq!(soft_body.num_particles(), 20); + // DOCUSAURUS: Sets stop + + // DOCUSAURUS: Material start + // Elastic cells: a jelly square with corotational linear elasticity. + let jelly = SoftBodyBuilder::grid(Vector::new(3.0, 1.2), Vector::splat(1.0), 6, 6) + // The constitutive model of the cells: `Volume` (per-cell area constraints, + // the shape is held by the edges), `Corotational` or `NeoHookean`. + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + // Stiffness of the elastic cells. + young_modulus: 3.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.5, + // Plasticity: the rest shape flows past 5% strain, at 20 per second. + plastic_yield: 0.05, + plastic_creep: 20.0, + // Tearing: an element past 40% strain tears. + tear_strain: Some(0.4), + ..Default::default() + }) + .particle_mass(0.2); + let jelly_handle = world.insert_soft_body(jelly); + + // A pressurized blob: a ring of particles inflated by area preservation. + let blob = SoftBodyBuilder::disk(Vector::new(0.0, 3.0), 0.8, 24) + .softness(SpringCoefficients::new(20.0, 1.0)) + // Target area multiplier (`> 1` inflates the body); enables area preservation. + .volume_factor(1.1) + .self_contacts(true); + let blob_handle = world.insert_soft_body(blob); + // DOCUSAURUS: Material stop + + // DOCUSAURUS: Particles start + let soft_body = &mut world.soft_bodies[sheet_handle]; + // Read the particles. + let position = soft_body.particle_position(0); + let velocity = soft_body.particle_velocity(0); + let positions: Vec = soft_body.particle_positions().collect(); + assert_eq!(positions.len(), soft_body.num_particles()); + // Move a particle. + soft_body.set_particle_position(1, position + Vector::new(0.0, 0.1)); + soft_body.set_particle_velocity(1, velocity); + // Pin (or release) a particle; a pinned particle can be driven like a kinematic body. + soft_body.set_particle_pinned(2, true); + soft_body.set_particle_kinematic_target(2, Vector::new(-3.5, 3.5)); + // The elements: edges, cells and the boundary segments. + let num_edges = soft_body.edges().len(); + let num_cells = soft_body.cells().len(); + let boundary: &[[u32; 2]] = soft_body.boundary(); + assert!(num_edges > 0 && num_cells > 0 && !boundary.is_empty()); + // DOCUSAURUS: Particles stop + + // DOCUSAURUS: Forces start + let soft_body = &mut world.soft_bodies[sheet_handle]; + // The `true` argument makes sure the soft body is awake. + soft_body.reset_forces(true); // Reset the forces to zero. + soft_body.add_force(Vector::new(0.0, 1.0), true); // Spread over the particles by mass. + soft_body.add_particle_force(3, Vector::new(0.0, 1.0), true); + soft_body.apply_impulse(Vector::new(0.0, 0.1), true); + soft_body.apply_particle_impulse(3, Vector::new(0.0, 0.1), true); + // An impulse on the particles within 0.5 of a point, scaled down with the distance. + soft_body.apply_impulse_at_point(Vector::new(0.0, 0.1), Vector::new(-3.0, 3.0), 0.5, true); + // A blast pushing the particles away from a center. + soft_body.apply_radial_impulse(Vector::new(-3.0, 3.0), 0.1, 1.0, true); + // DOCUSAURUS: Forces stop + + // DOCUSAURUS: Attachments start + // Attach the last particle of a rope to a rigid box, at the particle's position. + let rope = SoftBodyBuilder::rope(Vector::new(8.0, 9.0), Vector::new(12.0, 9.0), 25) + .pinned_particles([0]) + .softness(SpringCoefficients::new(40.0, 1.0)); + let rope_handle = world.insert_soft_body(rope); + let last_position = world.soft_bodies[rope_handle].particle_position(24); + let (weight, _) = world.insert( + RigidBodyBuilder::dynamic().translation(last_position - Vector::new(0.0, 0.4)), + ColliderBuilder::cuboid(0.3, 0.3).density(2.0), + ); + world.soft_bodies[rope_handle].attach_particle(24, weight, &world.bodies); + // The hidden rigid body standing for the whole soft body in joints and islands. + let root_body: RigidBodyHandle = world.soft_bodies[rope_handle].root_body(); + assert!(world.bodies[root_body].is_soft_frame()); + // DOCUSAURUS: Attachments stop + + // DOCUSAURUS: Clusters start + // A cluster over the top particles of the jelly: a rigid proxy that joints and + // colliders can attach to. + let top: Vec = { + let jelly = &world.soft_bodies[jelly_handle]; + (0..jelly.num_particles() as u32) + .filter(|&i| jelly.particle_position(i as usize).y > 2.0) + .collect() + }; + let cluster = world + .add_soft_body_cluster(jelly_handle, &top) + .expect("at least one valid particle"); + let proxy: RigidBodyHandle = world.soft_bodies[jelly_handle] + .cluster_proxy(cluster) + .unwrap(); + // A rigid plate welded onto the cluster. + let (plate, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(3.0, 2.4)), + ColliderBuilder::cuboid(1.2, 0.05).density(0.4), + ); + world.insert_impulse_joint( + plate, + proxy, + FixedJointBuilder::new().local_anchor1(Vector::new(0.0, -0.1)), + ); + // A cluster can be pinned, driven or tuned as a whole. + let jelly = &mut world.soft_bodies[jelly_handle]; + jelly.set_cluster_stiffness_scale(cluster, 2.0); + jelly.enable_cluster_shape_matching(cluster, true); + // DOCUSAURUS: Clusters stop + + // DOCUSAURUS: DeformableColliders start + // A deformable polyline bound to the blob: each vertex follows one particle (`direct`), + // or is embedded in the cell holding it (`skinned`). The polyline is given in the frame + // of the proxy it is attached to. + let root = world.soft_bodies[blob_handle].root_body(); + let root_pose = *world.bodies[root].position(); + let blob = &world.soft_bodies[blob_handle]; + let num = blob.num_particles(); + let vertices: Vec = blob + .particle_positions() + .map(|p| root_pose.inverse() * p) + .collect(); + let indices: Vec<[u32; 2]> = (0..num as u32).map(|i| [i, (i + 1) % num as u32]).collect(); + let particles: Vec = (0..num as u32).collect(); + let outline = + ColliderBuilder::polyline_with_flags(vertices, Some(indices), PolylineFlags::DEFORMABLE) + .sensor(true); + let outline_handle = world + .insert_deformable(outline, SoftMeshBinding::direct(particles), root) + .expect("a deformable polyline bound to a cluster proxy"); + // The polyline follows the particles: read its current vertices back. + let blob = &world.soft_bodies[blob_handle]; + let mesh = blob.mesh_of(outline_handle).unwrap(); + let outline_vertices: Vec = mesh.vertex_positions(blob).collect(); + assert_eq!(outline_vertices.len(), num); + // DOCUSAURUS: DeformableColliders stop + + // DOCUSAURUS: Tearing start + // Elements tear on their own past the material's thresholds; a tear can also be requested. + world.soft_bodies[sheet_handle].tear_edge(10); // Applied at the end of the next step. + // Tear at once along edges and through cells; pieces the tear disconnects become soft + // bodies of their own. + let event = world.tear_soft_body(sheet_handle, &[11, 12], &[]); + if let Some(event) = event { + println!("{} edges torn", event.torn_edges.len()); + } + // Cut along a blade (a segment in 2D), without removing material. + let blade = [Vector::new(-3.0, -10.0), Vector::new(-3.0, 10.0)]; + if let Some(event) = world.cut_soft_body(sheet_handle, &blade) { + for piece in &event.pieces { + println!( + "piece {:?} has {} particles", + piece.soft_body, + piece.particles.len() + ); + } + // Where a particle of the torn body went. + if let Some((body, index)) = event.particle_destination(n as u32 * n as u32 - 1) { + println!( + "particle {} is now particle {} of {:?}", + n * n - 1, + index, + body + ); + } + } + // DOCUSAURUS: Tearing stop + + // DOCUSAURUS: Events start + // Tears applied during a step are reported through the event handler. + let (collision_send, _collision_recv) = std::sync::mpsc::channel(); + let (contact_force_send, _contact_force_recv) = std::sync::mpsc::channel(); + let (soft_body_tear_send, soft_body_tear_recv) = std::sync::mpsc::channel(); + let event_handler = + ChannelEventCollector::new(collision_send, contact_force_send, soft_body_tear_send); + world.step_with_events(&(), &event_handler); + while let Ok(tear_event) = soft_body_tear_recv.try_recv() { + println!("Soft body {:?} tore", tear_event.soft_body); + } + // DOCUSAURUS: Events stop + + // DOCUSAURUS: Settings start + // Settings shared by every soft body of the world. + let settings = &mut world.integration_parameters.soft_bodies; + // Strain beyond which a constraint is re-solved after the contacts of every substep. + // Default: 0.75 + settings.resweep_strain = 0.75; + // Extra substeps a soft body requests while it is hit fast; 0 disables them. + // Default: 4 + settings.max_extra_substeps = 4; + // Stiffening of the soft-body contacts relative to the rigid ones. + // Default: 4.0 + settings.contact_stiffening = 4.0; + // The tangle detection and recovery stack can be switched off mechanism by mechanism. + settings.recovery.crossing_repulsion = true; + // DOCUSAURUS: Settings stop + + // DOCUSAURUS: Removal start + // Removing a soft body removes its root body, its proxies, its colliders and the joints + // attached to them. + world.remove_soft_body(rope_handle); + // A cluster can be removed on its own. + world.remove_soft_body_cluster(jelly_handle, cluster); + // DOCUSAURUS: Removal stop + + for _ in 0..10 { + world.step(); + } +} diff --git a/website/docs-examples/3d/javascript/src/index.ts b/website/docs-examples/3d/javascript/src/index.ts index 5324086ac..973204eb7 100644 --- a/website/docs-examples/3d/javascript/src/index.ts +++ b/website/docs-examples/3d/javascript/src/index.ts @@ -4,4 +4,5 @@ require('./snippets/getting_started') require('./snippets/joints') require('./snippets/rigid_bodies') require('./snippets/scene_queries') +require('./snippets/soft_bodies') require('./snippets/standalone') diff --git a/website/docs-examples/3d/javascript/src/snippets/soft_bodies.ts b/website/docs-examples/3d/javascript/src/snippets/soft_bodies.ts new file mode 100644 index 000000000..026acc5d6 --- /dev/null +++ b/website/docs-examples/3d/javascript/src/snippets/soft_bodies.ts @@ -0,0 +1,231 @@ +import RAPIER from '@dimforge/rapier3d'; + +{ + // DOCUSAURUS: Creation start + // The world that will contain our soft bodies. + let world = new RAPIER.World({ x: 0.0, y: -9.81, z: 0.0 }); + world.createCollider(RAPIER.ColliderDesc.cuboid(10.0, 0.1, 10.0)); + + // Description of a rope of 20 particles between two points. + let example1 = RAPIER.SoftBodyDesc.rope({ x: 0.0, y: 3.0, z: 0.0 }, { x: 2.0, y: 3.0, z: 0.0 }, 20); + // Description of a cloth: `nu` by `nv` particles, particle `(i, j)` at `origin + i * du + j * dv`. + let example2 = RAPIER.SoftBodyDesc.cloth( + { x: -1.0, y: 2.0, z: -1.0 }, { x: 0.1, y: 0.0, z: 0.0 }, { x: 0.0, y: 0.0, z: 0.1 }, 20, 20, + ); + // Description of a box of `nx * ny * nz` particles filled with tetrahedral cells. + let example3 = RAPIER.SoftBodyDesc.cuboid({ x: 3.0, y: 1.0, z: 0.0 }, { x: 0.5, y: 0.5, z: 0.5 }, 4, 4, 4); + // Description of a hollow sphere holding its volume (a balloon). + let example4 = RAPIER.SoftBodyDesc.sphere({ x: 0.0, y: 3.0, z: 3.0 }, 0.8, 2); + // Description over raw particle positions; the elements are added by the setters. + let example5 = new RAPIER.SoftBodyDesc([0.0, 1.0, 0.0, 1.0, 1.0, 0.0]).setEdges([0, 1]); + + let n = 20; + let clothDesc = RAPIER.SoftBodyDesc.cloth( + { x: -1.0, y: 2.0, z: -1.0 }, { x: 0.1, y: 0.0, z: 0.0 }, { x: 0.0, y: 0.0, z: 0.1 }, n, n, + ) + // Particles held in place. + .setPinnedParticles([0, n - 1, n * (n - 1), n * n - 1]) + // A uniform softness (natural frequency in Hz, damping ratio) for every constraint. + .setSoftness(30.0, 1.0) + // The mass of each particle. + // Default: 1.0 + .setParticleMass(0.05) + // The thickness of the particles, for collisions. + // Default: 0.01 + .setParticleRadius(0.02) + // The template of the body's colliders: its shape is replaced by the deformable surface. + .setSurfaceCollider(RAPIER.ColliderDesc.ball(0.05).setFriction(0.8)) + // Whether the surface collides with itself. + // Default: false + .setSelfContacts(true) + // Whether the body may fall asleep. + // Default: true + .setCanSleep(true); + // Create the soft body: this creates its hidden root rigid body and its colliders. + let cloth = world.createSoftBody(clothDesc); + // The integer handle of the soft body can be read from the `handle` field. + let clothHandle = cloth.handle; + // DOCUSAURUS: Creation stop + + // DOCUSAURUS: Material start + // Elastic cells: a jelly cube with corotational linear elasticity. + let material = new RAPIER.SoftBodyMaterial(); + // Stiffness of the elastic cells. + material.youngModulus = 2.0e3; + material.poissonRatio = 0.35; + material.elasticDampingRatio = 0.5; + // Plasticity: the rest shape flows past 5% strain, at 20 per second. + material.plasticYield = 0.05; + material.plasticCreep = 20.0; + // Tearing: an element past 40% strain tears. + material.tearStrain = 0.4; + let jellyDesc = RAPIER.SoftBodyDesc.cuboid({ x: 3.0, y: 1.0, z: 0.0 }, { x: 0.5, y: 0.5, z: 0.5 }, 4, 4, 4) + // The constitutive model of the cells: `Volume` (per-cell volume constraints, + // the shape is held by the edges), `Corotational` or `NeoHookean`. + .setCellModel(RAPIER.SoftBodyCellModel.Corotational) + .setMaterial(material) + .setParticleMass(0.2); + let jelly = world.createSoftBody(jellyDesc); + + // A material shared by the edges, bending constraints and volume constraints. + let clothMaterial = RAPIER.SoftBodyMaterial.uniform(30.0, 1.0); + // Softness of the bending constraints, on top of the uniform 30 Hz softness. + clothMaterial.bendSoftness = { naturalFrequency: 3.0, dampingRatio: 1.0 }; + cloth.setMaterial(clothMaterial); + // DOCUSAURUS: Material stop + + // DOCUSAURUS: Particles start + // Read the particles. + let position = cloth.particlePosition(0); + let velocity = cloth.particleVelocity(0); + let positions: Float32Array = cloth.particlePositions(); // Three floats per particle. + console.log("The cloth has", cloth.numParticles(), "particles;", positions.length / 3); + // Move a particle. + cloth.setParticlePosition(1, { x: position.x, y: position.y + 0.1, z: position.z }); + cloth.setParticleVelocity(1, velocity); + // Pin (or release) a particle; a pinned particle can be driven like a kinematic body. + cloth.setParticlePinned(2, true); + cloth.setParticleKinematicTarget(2, { x: -1.0, y: 2.5, z: -0.8 }); + // The elements: edges (two indices each), cells (four) and the boundary triangles (three). + let edges: Uint32Array = cloth.edges(); + let cells: Uint32Array = cloth.cells(); + let boundary: Uint32Array = cloth.boundary(); + console.log(edges.length / 2, "edges,", cells.length / 4, "cells,", boundary.length / 3, "triangles"); + // DOCUSAURUS: Particles stop + + // DOCUSAURUS: Forces start + // The `true` argument makes sure the soft body is awake. + cloth.resetForces(true); // Reset the forces to zero. + cloth.addForce({ x: 0.0, y: 1.0, z: 0.0 }, true); // Spread over the particles by mass. + cloth.addParticleForce(3, { x: 0.0, y: 1.0, z: 0.0 }, true); + cloth.applyImpulse({ x: 0.0, y: 0.1, z: 0.0 }, true); + cloth.applyParticleImpulse(3, { x: 0.0, y: 0.1, z: 0.0 }, true); + // An impulse on the particles within 0.5 of a point, scaled down with the distance. + cloth.applyImpulseAtPoint({ x: 0.0, y: 0.1, z: 0.0 }, { x: 0.0, y: 2.0, z: 0.0 }, 0.5, true); + // A blast pushing the particles away from a center. + cloth.applyRadialImpulse({ x: 0.0, y: 2.0, z: 0.0 }, 0.1, 1.0, true); + // DOCUSAURUS: Forces stop + + // DOCUSAURUS: Attachments start + // Attach the last particle of a rope to a rigid ball, at the particle's position. + let ropeDesc = RAPIER.SoftBodyDesc.rope({ x: -0.5, y: 5.0, z: 3.0 }, { x: 2.5, y: 5.0, z: 3.0 }, 30) + .setPinnedParticles([0]) + .setSoftness(40.0, 1.0); + let rope = world.createSoftBody(ropeDesc); + let last = rope.particlePosition(29); + let ball = world.createRigidBody(RAPIER.RigidBodyDesc.dynamic().setTranslation(last.x, last.y - 0.3, last.z)); + world.createCollider(RAPIER.ColliderDesc.ball(0.25).setDensity(2.0), ball); + rope.attachParticle(29, ball); + // The hidden rigid body standing for the whole soft body in joints and islands. + let rootBody = rope.rootBody(); + console.log("root body is a soft frame:", rootBody.isSoftFrame()); + // DOCUSAURUS: Attachments stop + + // DOCUSAURUS: Clusters start + // A cluster over the top particles of the jelly: a rigid proxy that joints and + // colliders can attach to. + let top = []; + for (let i = 0; i < jelly.numParticles(); ++i) { + if (jelly.particlePosition(i).y > 1.3) { + top.push(i); + } + } + let cluster = world.addSoftBodyCluster(jelly, top); + let proxy = jelly.clusterProxy(cluster); + // A rigid plate welded onto the cluster. + let plate = world.createRigidBody(RAPIER.RigidBodyDesc.dynamic().setTranslation(3.0, 1.9, 0.0)); + world.createCollider(RAPIER.ColliderDesc.cuboid(0.7, 0.05, 0.7).setDensity(0.4), plate); + let weld = RAPIER.JointData.fixed( + { x: 0.0, y: -0.1, z: 0.0 }, { w: 1.0, x: 0.0, y: 0.0, z: 0.0 }, + { x: 0.0, y: 0.0, z: 0.0 }, { w: 1.0, x: 0.0, y: 0.0, z: 0.0 }, + ); + world.createImpulseJoint(weld, plate, proxy, true); + // A cluster can be pinned, driven or tuned as a whole. + jelly.setClusterStiffnessScale(cluster, 2.0); + jelly.enableClusterShapeMatching(cluster, true); + // DOCUSAURUS: Clusters stop + + // DOCUSAURUS: DeformableColliders start + // A deformable triangle mesh bound to the jelly: each vertex is embedded in the cell + // holding it (`skinned`), or follows one particle (`direct`). The mesh is given in the + // frame of the proxy it is attached to. + let root = jelly.rootBody(); + let origin = root.translation(); + let c = jelly.centerOfMass(); + let r = 1.0; + let vertices = new Float32Array([ + c.x + r, c.y, c.z, c.x - r, c.y, c.z, c.x, c.y + r, c.z, + c.x, c.y - r, c.z, c.x, c.y, c.z + r, c.x, c.y, c.z - r, + ]); + for (let i = 0; i < 6; ++i) { + vertices[i * 3] -= origin.x; + vertices[i * 3 + 1] -= origin.y; + vertices[i * 3 + 2] -= origin.z; + } + let indices = new Uint32Array([0, 2, 4, 2, 1, 4, 1, 3, 4, 3, 0, 4, 2, 0, 5, 1, 2, 5, 3, 1, 5, 0, 3, 5]); + let skinDesc = RAPIER.ColliderDesc.trimesh(vertices, indices, RAPIER.TriMeshFlags.DEFORMABLE).setSensor(true); + let skin = world.createDeformableCollider(skinDesc, RAPIER.SoftMeshBinding.skinned(), root); + // The mesh follows the particles: read its current vertices back. + let meshIndex = jelly.meshOfCollider(skin); + let skinVertices: Float32Array = jelly.meshVertices(meshIndex); + console.log("The skin has", skinVertices.length / 3, "vertices"); + // DOCUSAURUS: DeformableColliders stop + + // DOCUSAURUS: Tearing start + // Elements tear on their own past the material's thresholds; a tear can also be requested. + cloth.tearEdge(10); // Applied at the end of the next step. + // Tear at once along edges and through cells; pieces the tear disconnects become soft + // bodies of their own. + let tear = world.tearSoftBody(cloth, [11, 12], []); + if (tear) { + console.log(tear.tornEdges().length / 2, "edges torn"); + tear.free(); + } + // Cut along a blade (a triangle in 3D), without removing material. + let cut = world.cutSoftBody(cloth, [ + { x: -0.1, y: -10.0, z: -10.0 }, { x: -0.1, y: 10.0, z: 0.0 }, { x: -0.1, y: -10.0, z: 10.0 }, + ]); + if (cut) { + for (let i = 0; i < cut.numPieces(); ++i) { + let piece = world.getSoftBody(cut.pieceSoftBody(i)); + console.log("piece", i, "has", piece.numParticles(), "particles"); + } + // Where a particle of the torn body went. + let destination = cut.particleDestination(n * n - 1); + if (destination) { + console.log("particle", n * n - 1, "is now particle", destination.particle, "of", destination.softBody); + } + cut.free(); + } + // DOCUSAURUS: Tearing stop + + // DOCUSAURUS: Events start + // Tears applied during a step are reported through the event queue. + let eventQueue = new RAPIER.EventQueue(true); + world.step(eventQueue); + eventQueue.drainSoftBodyTearEvents((event) => { + console.log("Soft body", event.softBody(), "tore:", event.numPieces(), "pieces split off"); + }); + // DOCUSAURUS: Events stop + + // DOCUSAURUS: Settings start + // Settings shared by every soft body of the world. + // Strain beyond which a constraint is re-solved after the contacts of every substep. + // Default: 0.75 + world.integrationParameters.softBodiesResweepStrain = 0.75; + // Extra substeps a soft body requests while it is hit fast; 0 disables them. + // Default: 4 + world.integrationParameters.softBodiesMaxExtraSubsteps = 4; + // Stiffening of the soft-body contacts relative to the rigid ones. + // Default: 4.0 + world.integrationParameters.softBodiesContactStiffening = 4.0; + // DOCUSAURUS: Settings stop + + // DOCUSAURUS: Removal start + // Removing a soft body removes its root body, its proxies, its colliders and the joints + // attached to them. + world.removeSoftBody(rope); + // A cluster can be removed on its own. + world.removeSoftBodyCluster(jelly, cluster); + // DOCUSAURUS: Removal stop +} diff --git a/website/docs-examples/3d/rust/examples/rs_basic_sim3.rs b/website/docs-examples/3d/rust/examples/rs_basic_sim3.rs index 342033796..44729cb12 100644 --- a/website/docs-examples/3d/rust/examples/rs_basic_sim3.rs +++ b/website/docs-examples/3d/rust/examples/rs_basic_sim3.rs @@ -26,6 +26,7 @@ fn main() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); let mut multibody_joint_set = MultibodyJointSet::new(); + let mut soft_body_set = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let physics_hooks = (); let event_handler = (); @@ -42,6 +43,7 @@ fn main() { &mut collider_set, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, diff --git a/website/docs-examples/3d/rust/examples/rs_character_controller3.rs b/website/docs-examples/3d/rust/examples/rs_character_controller3.rs index 6bcd30630..4851002a8 100644 --- a/website/docs-examples/3d/rust/examples/rs_character_controller3.rs +++ b/website/docs-examples/3d/rust/examples/rs_character_controller3.rs @@ -25,6 +25,7 @@ fn main() { let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); let mut multibody_joint_set = MultibodyJointSet::new(); + let mut soft_body_set = SoftBodySet::new(); let mut ccd_solver = CCDSolver::new(); let physics_hooks = (); let event_handler = (); @@ -73,6 +74,7 @@ fn main() { &mut colliders, &mut impulse_joint_set, &mut multibody_joint_set, + &mut soft_body_set, &mut ccd_solver, &physics_hooks, &event_handler, diff --git a/website/docs-examples/3d/rust/examples/rs_scene_queries3.rs b/website/docs-examples/3d/rust/examples/rs_scene_queries3.rs index 89aab4bff..3e86904f3 100644 --- a/website/docs-examples/3d/rust/examples/rs_scene_queries3.rs +++ b/website/docs-examples/3d/rust/examples/rs_scene_queries3.rs @@ -20,6 +20,7 @@ fn main() { let integration_parameters = IntegrationParameters::default(); let mut physics_pipeline = PhysicsPipeline::new(); let mut island_manager = IslandManager::new(); + let mut soft_body_set = SoftBodySet::new(); let mut broad_phase = DefaultBroadPhase::new(); let mut narrow_phase = NarrowPhase::new(); let mut impulse_joint_set = ImpulseJointSet::new(); @@ -41,6 +42,7 @@ fn main() { handle_to_remove, &mut island_manager, &mut rigid_body_set, + &mut soft_body_set, true, ); let collider = collider_set.get_mut(handle1).unwrap(); diff --git a/website/docs-examples/3d/rust/examples/rs_soft_bodies3.rs b/website/docs-examples/3d/rust/examples/rs_soft_bodies3.rs new file mode 100644 index 000000000..0bb3e29a1 --- /dev/null +++ b/website/docs-examples/3d/rust/examples/rs_soft_bodies3.rs @@ -0,0 +1,299 @@ +use rapier3d::prelude::*; + +fn main() { + // DOCUSAURUS: Creation start + // A world with a ground. + let mut world = PhysicsWorld::new(); + world.insert_collider(ColliderBuilder::cuboid(10.0, 0.1, 10.0), None); + + // Builder for a rope of 20 particles between two points. + let _ = SoftBodyBuilder::rope(Vector::new(0.0, 3.0, 0.0), Vector::new(2.0, 3.0, 0.0), 20); + // Builder for a cloth: `nu` by `nv` particles, particle `(i, j)` at `origin + i * du + j * dv`. + let _ = SoftBodyBuilder::cloth( + Vector::new(-1.0, 2.0, -1.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + 20, + 20, + ); + // Builder for a box of `nx * ny * nz` particles filled with tetrahedral cells. + let _ = SoftBodyBuilder::cuboid(Vector::new(3.0, 1.0, 0.0), Vector::splat(0.5), 4, 4, 4); + // Builder for a hollow sphere holding its volume (a balloon). + let _ = SoftBodyBuilder::sphere(Vector::new(0.0, 3.0, 3.0), 0.8, 2); + // Builder over raw particle positions; the elements are added by the setters. + let n = 20; + let cloth = SoftBodyBuilder::cloth( + Vector::new(-1.0, 2.0, -1.0), + Vector::new(0.1, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.1), + n, + n, + ) + // Particles held in place. + .pinned_particles([0, (n - 1) as u32, (n * (n - 1)) as u32, (n * n - 1) as u32]) + // A uniform softness (natural frequency in Hz, damping ratio) for every constraint. + .softness(SpringCoefficients::new(30.0, 1.0)) + // The mass of each particle. + // Default: 1.0 + .particle_mass(0.05) + // The thickness of the particles, for collisions. + // Default: 0.01 + .particle_radius(0.02) + // The template of the body's colliders: its shape is replaced by the deformable surface. + .surface_collider(ColliderBuilder::ball(0.05).friction(0.8)) + // Whether the surface collides with itself. + // Default: false + .self_contacts(true) + // Whether the body may fall asleep. + // Default: true + .can_sleep(true); + // Insert the soft body: this creates its hidden root rigid body and its colliders. + let cloth_handle = world.insert_soft_body(cloth); + // DOCUSAURUS: Creation stop + + // DOCUSAURUS: Sets start + // The sets can also be used directly, without the `PhysicsWorld` façade. + let mut soft_body_set = SoftBodySet::new(); + let mut rigid_body_set = RigidBodySet::new(); + let mut collider_set = ColliderSet::new(); + let rope = SoftBodyBuilder::rope(Vector::new(0.0, 3.0, 0.0), Vector::new(2.0, 3.0, 0.0), 20); + let rope_handle = soft_body_set.insert(rope, &mut rigid_body_set, &mut collider_set); + let soft_body = &soft_body_set[rope_handle]; + assert_eq!(soft_body.num_particles(), 20); + // DOCUSAURUS: Sets stop + + // DOCUSAURUS: Material start + // Elastic cells: a jelly cube with corotational linear elasticity. + let jelly = SoftBodyBuilder::cuboid(Vector::new(3.0, 1.0, 0.0), Vector::splat(0.5), 4, 4, 4) + // The constitutive model of the cells: `Volume` (per-cell volume constraints, + // the shape is held by the edges), `Corotational` or `NeoHookean`. + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + // Stiffness of the elastic cells. + young_modulus: 2.0e3, + poisson_ratio: 0.35, + elastic_damping_ratio: 0.5, + // Plasticity: the rest shape flows past 5% strain, at 20 per second. + plastic_yield: 0.05, + plastic_creep: 20.0, + // Tearing: an element past 40% strain tears. + tear_strain: Some(0.4), + ..Default::default() + }) + .particle_mass(0.2); + let jelly_handle = world.insert_soft_body(jelly); + + // A material shared by the edges, bending constraints and volume constraints. + let material = SoftBodyMaterial { + // Softness of the bending constraints, on top of a uniform 30 Hz softness. + bend_softness: SpringCoefficients::new(3.0, 1.0), + ..SoftBodyMaterial::uniform(SpringCoefficients::new(30.0, 1.0)) + }; + world.soft_bodies[cloth_handle].set_material(material); + // DOCUSAURUS: Material stop + + // DOCUSAURUS: Particles start + let soft_body = &mut world.soft_bodies[cloth_handle]; + // Read the particles. + let position = soft_body.particle_position(0); + let velocity = soft_body.particle_velocity(0); + let positions: Vec = soft_body.particle_positions().collect(); + assert_eq!(positions.len(), soft_body.num_particles()); + // Move a particle. + soft_body.set_particle_position(1, position + Vector::new(0.0, 0.1, 0.0)); + soft_body.set_particle_velocity(1, velocity); + // Pin (or release) a particle; a pinned particle can be driven like a kinematic body. + soft_body.set_particle_pinned(2, true); + soft_body.set_particle_kinematic_target(2, Vector::new(-1.0, 2.5, -0.8)); + // The elements: edges, cells and the boundary triangles. + let num_edges = soft_body.edges().len(); + let num_cells = soft_body.cells().len(); + let boundary: &[[u32; 3]] = soft_body.boundary(); + assert!(num_edges > 0 && num_cells == 0 && !boundary.is_empty()); + // DOCUSAURUS: Particles stop + + // DOCUSAURUS: Forces start + let soft_body = &mut world.soft_bodies[cloth_handle]; + // The `true` argument makes sure the soft body is awake. + soft_body.reset_forces(true); // Reset the forces to zero. + soft_body.add_force(Vector::new(0.0, 1.0, 0.0), true); // Spread over the particles by mass. + soft_body.add_particle_force(3, Vector::new(0.0, 1.0, 0.0), true); + soft_body.apply_impulse(Vector::new(0.0, 0.1, 0.0), true); + soft_body.apply_particle_impulse(3, Vector::new(0.0, 0.1, 0.0), true); + // An impulse on the particles within 0.5 of a point, scaled down with the distance. + soft_body.apply_impulse_at_point( + Vector::new(0.0, 0.1, 0.0), + Vector::new(0.0, 2.0, 0.0), + 0.5, + true, + ); + // A blast pushing the particles away from a center. + soft_body.apply_radial_impulse(Vector::new(0.0, 2.0, 0.0), 0.1, 1.0, true); + // DOCUSAURUS: Forces stop + + // DOCUSAURUS: Attachments start + // Attach the last particle of a rope to a rigid ball, at the particle's position. + let rope = SoftBodyBuilder::rope(Vector::new(-0.5, 5.0, 3.0), Vector::new(2.5, 5.0, 3.0), 30) + .pinned_particles([0]) + .softness(SpringCoefficients::new(40.0, 1.0)); + let rope_handle = world.insert_soft_body(rope); + let last_position = world.soft_bodies[rope_handle].particle_position(29); + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(last_position - Vector::new(0.0, 0.3, 0.0)), + ColliderBuilder::ball(0.25).density(2.0), + ); + world.soft_bodies[rope_handle].attach_particle(29, ball, &world.bodies); + // The hidden rigid body standing for the whole soft body in joints and islands. + let root_body: RigidBodyHandle = world.soft_bodies[rope_handle].root_body(); + assert!(world.bodies[root_body].is_soft_frame()); + // DOCUSAURUS: Attachments stop + + // DOCUSAURUS: Clusters start + // A cluster over the top particles of the jelly: a rigid proxy that joints and + // colliders can attach to. + let top: Vec = { + let jelly = &world.soft_bodies[jelly_handle]; + (0..jelly.num_particles() as u32) + .filter(|&i| jelly.particle_position(i as usize).y > 1.3) + .collect() + }; + let cluster = world + .add_soft_body_cluster(jelly_handle, &top) + .expect("at least one valid particle"); + let proxy: RigidBodyHandle = world.soft_bodies[jelly_handle] + .cluster_proxy(cluster) + .unwrap(); + // A rigid plate welded onto the cluster. + let (plate, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(3.0, 1.9, 0.0)), + ColliderBuilder::cuboid(0.7, 0.05, 0.7).density(0.4), + ); + world.insert_impulse_joint( + plate, + proxy, + FixedJointBuilder::new().local_anchor1(Vector::new(0.0, -0.1, 0.0)), + ); + // A cluster can be pinned, driven or tuned as a whole. + let jelly = &mut world.soft_bodies[jelly_handle]; + jelly.set_cluster_stiffness_scale(cluster, 2.0); + jelly.enable_cluster_shape_matching(cluster, true); + // DOCUSAURUS: Clusters stop + + // DOCUSAURUS: DeformableColliders start + // A deformable triangle mesh bound to the jelly: each vertex is embedded in the cell + // holding it (`skinned`), or follows one particle (`direct`). The mesh is given in the + // frame of the proxy it is attached to. + let root = world.soft_bodies[jelly_handle].root_body(); + let root_pose = *world.bodies[root].position(); + let center = world.soft_bodies[jelly_handle].center_of_mass(); + let r = 1.0; + let vertices: Vec = [ + Vector::new(r, 0.0, 0.0), + Vector::new(-r, 0.0, 0.0), + Vector::new(0.0, r, 0.0), + Vector::new(0.0, -r, 0.0), + Vector::new(0.0, 0.0, r), + Vector::new(0.0, 0.0, -r), + ] + .iter() + .map(|v| root_pose.inverse() * (center + *v)) + .collect(); + let indices = vec![ + [0, 2, 4], + [2, 1, 4], + [1, 3, 4], + [3, 0, 4], + [2, 0, 5], + [1, 2, 5], + [3, 1, 5], + [0, 3, 5], + ]; + let skin = ColliderBuilder::trimesh_with_flags(vertices, indices, TriMeshFlags::DEFORMABLE) + .unwrap() + .sensor(true); + let skin_handle = world + .insert_deformable(skin, SoftMeshBinding::skinned(), root) + .expect("a deformable mesh bound to a cluster proxy"); + // The mesh follows the particles: read its current vertices back. + let jelly = &world.soft_bodies[jelly_handle]; + let mesh = jelly.mesh_of(skin_handle).unwrap(); + let skin_vertices: Vec = mesh.vertex_positions(jelly).collect(); + assert_eq!(skin_vertices.len(), 6); + // DOCUSAURUS: DeformableColliders stop + + // DOCUSAURUS: Tearing start + // Elements tear on their own past the material's thresholds; a tear can also be requested. + world.soft_bodies[cloth_handle].tear_edge(10); // Applied at the end of the next step. + // Tear at once along edges and through cells; pieces the tear disconnects become soft + // bodies of their own. + let event = world.tear_soft_body(cloth_handle, &[11, 12], &[]); + if let Some(event) = event { + println!("{} edges torn", event.torn_edges.len()); + } + // Cut along a blade (a triangle in 3D), without removing material. + let blade = [ + Vector::new(-0.1, -10.0, -10.0), + Vector::new(-0.1, 10.0, 0.0), + Vector::new(-0.1, -10.0, 10.0), + ]; + if let Some(event) = world.cut_soft_body(cloth_handle, &blade) { + for piece in &event.pieces { + println!( + "piece {:?} has {} particles", + piece.soft_body, + piece.particles.len() + ); + } + // Where a particle of the torn body went. + if let Some((body, index)) = event.particle_destination(n as u32 * n as u32 - 1) { + println!( + "particle {} is now particle {} of {:?}", + n * n - 1, + index, + body + ); + } + } + // DOCUSAURUS: Tearing stop + + // DOCUSAURUS: Events start + // Tears applied during a step are reported through the event handler. + let (collision_send, _collision_recv) = std::sync::mpsc::channel(); + let (contact_force_send, _contact_force_recv) = std::sync::mpsc::channel(); + let (soft_body_tear_send, soft_body_tear_recv) = std::sync::mpsc::channel(); + let event_handler = + ChannelEventCollector::new(collision_send, contact_force_send, soft_body_tear_send); + world.step_with_events(&(), &event_handler); + while let Ok(tear_event) = soft_body_tear_recv.try_recv() { + println!("Soft body {:?} tore", tear_event.soft_body); + } + // DOCUSAURUS: Events stop + + // DOCUSAURUS: Settings start + // Settings shared by every soft body of the world. + let settings = &mut world.integration_parameters.soft_bodies; + // Strain beyond which a constraint is re-solved after the contacts of every substep. + // Default: 0.75 + settings.resweep_strain = 0.75; + // Extra substeps a soft body requests while it is hit fast; 0 disables them. + // Default: 4 + settings.max_extra_substeps = 4; + // Stiffening of the soft-body contacts relative to the rigid ones. + // Default: 4.0 + settings.contact_stiffening = 4.0; + // The tangle detection and recovery stack can be switched off mechanism by mechanism. + settings.recovery.crossing_repulsion = true; + // DOCUSAURUS: Settings stop + + // DOCUSAURUS: Removal start + // Removing a soft body removes its root body, its proxies, its colliders and the joints + // attached to them. + world.remove_soft_body(rope_handle); + // A cluster can be removed on its own. + world.remove_soft_body_cluster(jelly_handle, cluster); + // DOCUSAURUS: Removal stop + + for _ in 0..10 { + world.step(); + } +} diff --git a/website/docs-examples/Cargo.toml b/website/docs-examples/Cargo.toml index efb32bf5d..7e8b128a9 100644 --- a/website/docs-examples/Cargo.toml +++ b/website/docs-examples/Cargo.toml @@ -1,3 +1,9 @@ [workspace] members = ["2d/bevy", "2d/rust", "3d/bevy", "3d/rust", "inject_file"] resolver = "2" + +[patch.crates-io] +# Temporary: the deformable-mesh API (parry branch `soft-bodies`), to be replaced by a +# version bump when parry is released. Mirrors the patch of the root workspace. +parry2d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } +parry3d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } From cc74509efad29d1a853016cc9210f9a25b20d13d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 18 Sep 2026 17:41:42 +0200 Subject: [PATCH 28/32] chore: switch to parry 0.31 --- Cargo.toml | 18 +- crates/rapier2d/tests/overlap_constraints.rs | 255 ------ crates/rapier2d/tests/self_intersect_probe.rs | 428 ---------- crates/rapier2d/tests/soft_bodies.rs | 7 +- crates/rapier2d/tests/soft_recovery.rs | 756 ++++++++++++++++++ crates/rapier2d/tests/soft_stack_probe.rs | 520 ------------ crates/rapier2d/tests/volume_contact.rs | 332 -------- crates/rapier3d/tests/overlap_constraints3.rs | 108 --- .../tests/query_pipeline_subshapes.rs | 120 +++ .../rapier3d/tests/self_intersect_probe3.rs | 406 ---------- crates/rapier3d/tests/soft_bodies.rs | 88 +- crates/rapier3d/tests/soft_body_clusters.rs | 13 +- crates/rapier3d/tests/soft_recovery3.rs | 590 ++++++++++++++ crates/rapier3d/tests/soft_stack_probe3.rs | 445 ----------- python/rapier-py-3d/src/controllers.rs | 2 + src/dynamics/ccd/ccd_solver.rs | 4 +- .../collision_mesh/collision_mesh_topology.rs | 26 +- .../soft_body_set/soft_body_set_clusters.rs | 6 +- src/dynamics/soft_body/soft_body_settings.rs | 2 +- .../soft_body/soft_recovery_settings.rs | 6 +- .../tearing/tearing_particle_split.rs | 2 +- .../soft_constraint_plasticity.rs | 2 +- .../soft_element_constraint_assembly.rs | 2 +- .../solver/soft_fem/soft_fem_sparse.rs | 6 +- src/geometry/narrow_phase/intersections.rs | 2 +- src/pipeline/physics_world.rs | 5 +- src/pipeline/query_pipeline.rs | 57 +- typescript/Cargo.toml | 18 +- .../prepare_builds/templates/Cargo.toml.tera | 2 +- .../src/control/character_controller.rs | 2 + typescript/src/geometry/shape.rs | 3 +- website/docs-examples/Cargo.toml | 9 +- 32 files changed, 1638 insertions(+), 2604 deletions(-) delete mode 100644 crates/rapier2d/tests/overlap_constraints.rs delete mode 100644 crates/rapier2d/tests/self_intersect_probe.rs create mode 100644 crates/rapier2d/tests/soft_recovery.rs delete mode 100644 crates/rapier2d/tests/soft_stack_probe.rs delete mode 100644 crates/rapier2d/tests/volume_contact.rs delete mode 100644 crates/rapier3d/tests/overlap_constraints3.rs create mode 100644 crates/rapier3d/tests/query_pipeline_subshapes.rs delete mode 100644 crates/rapier3d/tests/self_intersect_probe3.rs create mode 100644 crates/rapier3d/tests/soft_recovery3.rs delete mode 100644 crates/rapier3d/tests/soft_stack_probe3.rs diff --git a/Cargo.toml b/Cargo.toml index a5ffa6101..6b54e9610 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -72,10 +72,10 @@ num-traits = { version = "0.2", default-features = false } approx = { version = "0.5", default-features = false } # Parry (each crate picks its own variant) -parry2d = { version = "0.30.2", default-features = false, features = ["required-features"] } -parry3d = { version = "0.30.2", default-features = false, features = ["required-features"] } -parry2d-f64 = { version = "0.30.2", default-features = false, features = ["required-features"] } -parry3d-f64 = { version = "0.30.2", default-features = false, features = ["required-features"] } +parry2d = { version = "0.31.1", default-features = false, features = ["required-features"] } +parry3d = { version = "0.31.1", default-features = false, features = ["required-features"] } +parry2d-f64 = { version = "0.31.1", default-features = false, features = ["required-features"] } +parry3d-f64 = { version = "0.31.1", default-features = false, features = ["required-features"] } # Utilities arrayvec = { version = "0.7", default-features = false } @@ -148,8 +148,6 @@ tempfile = "3" #simba = { path = "../simba" } #kiss3d = { path = "../kiss3d" } -# Temporary: the deformable-mesh API (parry branch `soft-bodies`), to be replaced by a -# version bump when parry is released. #parry2d = { path = "../parry/crates/parry2d" } #parry3d = { path = "../parry/crates/parry3d" } #parry2d-f64 = { path = "../parry/crates/parry2d-f64" } @@ -162,10 +160,10 @@ tempfile = "3" #kiss3d = { git = "https://github.com/sebcrozet/kiss3d", branch = "render-2d" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } #glamx = { git = "https://github.com/dimforge/glamx", branch = "fix-lerp2" } -parry2d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } -parry3d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } -parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } -parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } +#parry2d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } +#parry3d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } +#parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } +#parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } # See https://github.com/EmbarkStudios/puffin/pull/234 puffin_egui = { git = "https://github.com/tedsteen/puffin.git", rev = "11771ebe00fd257aedbb545df3339ad597b1cc34" } diff --git a/crates/rapier2d/tests/overlap_constraints.rs b/crates/rapier2d/tests/overlap_constraints.rs deleted file mode 100644 index b90470427..000000000 --- a/crates/rapier2d/tests/overlap_constraints.rs +++ /dev/null @@ -1,255 +0,0 @@ -//! Intersection-volume constraints (see docs/soft-depenetration-literature-plan.md, phase 3): -//! the part of a closed boundary inside another, or a self-crossed loop's mirrored lobe, -//! retracts along its own area gradient. Every other active recovery is off in these scenes. - -use rapier2d::prelude::*; - -fn segments_cross(a0: Vector, a1: Vector, b0: Vector, b1: Vector) -> bool { - let d1 = a1 - a0; - let d2 = b1 - b0; - let denom = d1.perp_dot(d2); - if denom.abs() < 1.0e-12 { - return false; - } - let t = (b0 - a0).perp_dot(d2) / denom; - let u = (b0 - a0).perp_dot(d1) / denom; - (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) -} - -fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { - let mut parity = false; - for e in mesh.indices() { - let (p0, p1) = ( - mesh.vertex(body, e[0] as usize), - mesh.vertex(body, e[1] as usize), - ); - if (p0.y > point.y) != (p1.y > point.y) { - let t = (point.y - p0.y) / (p1.y - p0.y); - if p0.x + t * (p1.x - p0.x) > point.x { - parity = !parity; - } - } - } - parity -} - -fn overlap(world: &PhysicsWorld, a: SoftBodyHandle, b: SoftBodyHandle) -> (usize, usize, usize) { - let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); - let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); - let pa = |v: u32| ma.vertex(sa, v as usize); - let pb = |v: u32| mb.vertex(sb, v as usize); - let mut crossings = 0; - for ea in ma.indices() { - for eb in mb.indices() { - if segments_cross(pa(ea[0]), pa(ea[1]), pb(eb[0]), pb(eb[1])) { - crossings += 1; - } - } - } - let a_in_b = (0..ma.vertex_count()) - .filter(|&v| inside(pa(v as u32), mb, sb)) - .count(); - let b_in_a = (0..mb.vertex_count()) - .filter(|&v| inside(pb(v as u32), ma, sa)) - .count(); - (crossings, a_in_b, b_in_a) -} - -fn self_crossings(world: &PhysicsWorld, h: SoftBodyHandle) -> usize { - let sb = &world.soft_bodies[h]; - let mesh = sb.meshes().next().unwrap(); - let idx = mesh.indices(); - let p = |v: u32| mesh.vertex(sb, v as usize); - let mut crossings = 0; - for i in 0..idx.len() { - for j in i + 1..idx.len() { - let (ei, ej) = (idx[i], idx[j]); - if ei.iter().any(|v| ej.contains(v)) { - continue; - } - if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { - crossings += 1; - } - } - } - crossings -} - -/// Overlap constraints on, every other active recovery off (`SOFT_DEPEN_PRESET=defaults` keeps -/// the defaults for A/B runs). -fn overlap_only(world: &mut PhysicsWorld) { - if std::env::var("SOFT_DEPEN_PRESET").as_deref() == Ok("defaults") { - return; - } - // The reference settings: prevention without edge speculation, detection and the - // stand-down, the constraints on. - let r = &mut world.integration_parameters.soft_bodies.recovery; - r.overlap_constraints = true; - r.edge_speculation = false; - // Diagnostic: the closed-closed edge constraints off, or the overlap constraints off (the reference - // settings minus the constraints). - if std::env::var("SOFT_DEPEN_NO_EDGE").is_ok() { - r.edge_speculation = false; - } - if std::env::var("SOFT_OVERLAP_OFF").is_ok() { - r.overlap_constraints = false; - } - if let Some(pace) = std::env::var("SOFT_OVERLAP_CONSTRAINT_PACE").ok().and_then(|v| v.parse().ok()) { - r.overlap_constraint_pace = pace; - } -} - -fn floor(world: &mut PhysicsWorld) { - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), - ColliderBuilder::cuboid(6.0, 0.5), - ); -} - -fn blob(center: Vector, radius: Real, n: usize) -> SoftBodyBuilder { - SoftBodyBuilder::disk(center, radius, n) - .softness(SpringCoefficients::new(4.0, 1.0)) - .self_contacts(true) - .particle_mass(0.05) -} - -fn make_gerono_eight(world: &mut PhysicsWorld, h: SoftBodyHandle, center: Vector, r: Real) { - let n = world.soft_bodies[h].particles().len(); - for i in 0..n { - let t = core::f32::consts::TAU as Real * i as Real / n as Real; - let p = center + Vector::new(r * t.cos(), r * t.sin() * t.cos()); - world.soft_bodies[h].set_particle_position(i, p); - } -} - -/// Two cell-less blobs inserted overlapping by a third of a radius: each one's intruding -/// arc retracts into its own body, and the pair de-overlaps with no expulsion. -#[test] -fn overlapping_blobs_deoverlap() { - let mut world = PhysicsWorld::new(); - floor(&mut world); - let a = world.insert_soft_body(blob(Vector::new(-0.42, 0.6), 0.6, 24)); - let b = world.insert_soft_body(blob(Vector::new(0.42, 0.6), 0.6, 24)); - overlap_only(&mut world); - for step in 0..=900 { - if step % 100 == 0 { - let (c, ab, ba) = overlap(&world, a, b); - println!("step {step:4}: crossings={c} a_in_b={ab} b_in_a={ba}"); - } - world.step(); - } - let (c, ab, ba) = overlap(&world, a, b); - assert_eq!((c, ab, ba), (0, 0, 0), "the blobs still overlap"); -} - -/// Checks that a balanced figure-eight's mirrored lobe deflates along its area gradient until -/// the crossing annihilates and the loop re-inflates. Ignored: without the active untangler's -/// endgame pull the balanced eight stalls as a sliver crossing. -#[test] -#[ignore] -fn balanced_eight_recovers() { - let mut world = PhysicsWorld::new(); - floor(&mut world); - let a = world.insert_soft_body(blob(Vector::new(0.0, 0.65), 0.6, 24)); - make_gerono_eight(&mut world, a, Vector::new(0.0, 0.65), 0.6); - overlap_only(&mut world); - for step in 0..=1200 { - if step % 100 == 0 { - println!( - "step {step:4}: crossings={} area={:.3}", - self_crossings(&world, a), - world.soft_bodies[a].volume() - ); - } - world.step(); - } - assert_eq!( - self_crossings(&world, a), - 0, - "the balanced eight never recovered" - ); - let area = world.soft_bodies[a].volume(); - let rest = world.soft_bodies[a].rest_volume(); - assert!( - area > 0.8 * rest, - "the loop did not re-inflate: {area:.3} of {rest:.3}" - ); -} - -/// A dynamic rigid box inserted inside a hollow blob, with expulsion off: the blob's patch -/// inside the box and the box itself share one constraint, and the pair separates without the box -/// being thrown (a box, not a ball: a ball keeps rolling with whatever it was handed). -#[test] -fn rigid_ball_inside_blob_separates() { - let mut world = PhysicsWorld::new(); - floor(&mut world); - let a = world.insert_soft_body(blob(Vector::new(0.0, 0.65), 0.6, 24)); - let (ball, _) = world.insert( - RigidBodyBuilder::dynamic().translation(Vector::new(0.3, 0.65)), - ColliderBuilder::cuboid(0.3, 0.3).density(1.0), - ); - overlap_only(&mut world); - let inside_count = |world: &PhysicsWorld| { - let sb = &world.soft_bodies[a]; - let c = world.bodies[ball].center_of_mass(); - sb.particle_positions() - .filter(|p| (p.x - c.x).abs() < 0.3 && (p.y - c.y).abs() < 0.3) - .count() - }; - for step in 0..=600 { - if step % 100 == 0 { - println!( - "step {step:4}: vertices inside ball={} ball=({:+.2},{:+.2}) blob=({:+.2},{:+.2})", - inside_count(&world), - world.bodies[ball].center_of_mass().x, - world.bodies[ball].center_of_mass().y, - world.soft_bodies[a].center_of_mass().x, - world.soft_bodies[a].center_of_mass().y, - ); - } - world.step(); - } - assert_eq!( - inside_count(&world), - 0, - "the blob still has vertices inside the ball" - ); - let speed = world.bodies[ball].linvel().length(); - assert!(speed < 1.0, "the ball was thrown: {speed} u/s"); -} - -/// A hollow blob dropped onto a thin fixed pin that ends up inside it (the pile's impaled -/// blobs): the rigid overlap constraint lifts the blob off the pin, expulsion off. -#[test] -fn impaled_blob_recovers() { - let mut world = PhysicsWorld::new(); - floor(&mut world); - let pin_friction: Real = std::env::var("SOFT_PIN_FRICTION") - .ok() - .and_then(|v| v.parse().ok()) - .unwrap_or(0.5); - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(0.0, 1.5)), - ColliderBuilder::capsule_x(0.6, 0.1).friction(pin_friction), - ); - let a = world.insert_soft_body(blob(Vector::new(0.0, 1.5), 0.6, 24)); - overlap_only(&mut world); - let pin_inside = |world: &PhysicsWorld| { - let sb = &world.soft_bodies[a]; - let mesh = sb.meshes().next().unwrap(); - inside(Vector::new(0.0, 1.5), mesh, sb) - }; - println!("start: pin inside={}", pin_inside(&world)); - for step in 0..=600 { - if step % 100 == 0 { - println!( - "step {step:4}: pin inside={} blob=({:+.2},{:+.2})", - pin_inside(&world), - world.soft_bodies[a].center_of_mass().x, - world.soft_bodies[a].center_of_mass().y, - ); - } - world.step(); - } - assert!(!pin_inside(&world), "the pin is still inside the blob"); -} diff --git a/crates/rapier2d/tests/self_intersect_probe.rs b/crates/rapier2d/tests/self_intersect_probe.rs deleted file mode 100644 index e5be86564..000000000 --- a/crates/rapier2d/tests/self_intersect_probe.rs +++ /dev/null @@ -1,428 +0,0 @@ -//! Headless probes of the `debug_self_intersect2` demo family: surfaces that start -//! self-intersecting must recover instead of being held tangled by their own self-contact -//! constraints (docs/self-intersection-recovery-plan.md); the 8-shaped blobs probe open problems. - -use rapier2d::prelude::*; - -fn segments_cross(a0: Vector, a1: Vector, b0: Vector, b1: Vector) -> bool { - let d1 = a1 - a0; - let d2 = b1 - b0; - let denom = d1.perp_dot(d2); - if denom.abs() < 1.0e-12 { - return false; - } - let t = (b0 - a0).perp_dot(d2) / denom; - let u = (b0 - a0).perp_dot(d1) / denom; - (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) -} - -fn probe(world: &PhysicsWorld, h: SoftBodyHandle) -> (usize, usize, Real) { - let sb = &world.soft_bodies[h]; - // Boundary self-crossings among non-adjacent surface segments. - let mesh = sb.meshes().next().unwrap(); - let indices = mesh.indices(); - let mut crossings = 0; - for i in 0..indices.len() { - for j in i + 1..indices.len() { - let (ei, ej) = (indices[i], indices[j]); - if ei.iter().any(|v| ej.contains(v)) { - continue; - } - let p = |v: u32| mesh.vertex(sb, v as usize); - if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { - crossings += 1; - } - } - } - // Inverted cells. - let mut inverted = 0; - let mut min_ratio: Real = Real::MAX; - for cell in sb.cells() { - let x: [Vector; 3] = core::array::from_fn(|k| sb.particle_position(cell.vertices[k] as usize)); - let vol = (x[1] - x[0]).perp_dot(x[2] - x[0]) * 0.5; - let ratio = vol / cell.rest_volume; - min_ratio = min_ratio.min(ratio); - if ratio <= 0.0 { - inverted += 1; - } - } - (crossings, inverted, min_ratio) -} - -const SELF_CONTACTS: bool = option_env!("NO_SELF_CONTACTS").is_none(); -const BLOB_HZ: Real = if option_env!("STIFF_BLOB").is_some() { 20.0 } else { 4.0 }; - -/// A closed blob (no cells: edges + boundary-volume) whose top vertex starts pushed down -/// through the bottom wall: does the cell-less capture recover? -#[test] -fn blob_vertex_capture() { - let mut world = PhysicsWorld::new(); - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(0.0, -1.0)), - ColliderBuilder::cuboid(4.0, 0.5), - ); - let h = world.insert_soft_body( - SoftBodyBuilder::disk(Vector::new(0.0, 0.0), 0.5, 16) - .softness(SpringCoefficients::new(BLOB_HZ, 1.0)) - .self_contacts(SELF_CONTACTS) - .particle_mass(0.05), - ); - // Vertex 4 sits at the top of the disk (angle 90°); park it below the bottom wall. - world.soft_bodies[h].set_particle_position(4, Vector::new(0.0, -0.8)); - - for step in 0..=600 { - if step % 60 == 0 { - let (crossings, _, _) = probe(&world, h); - let p4 = world.soft_bodies[h].particle_position(4); - println!( - "step {step:4}: crossings={crossings} p4=({:+.3},{:+.3})", - p4.x, p4.y - ); - } - world.step(); - } - let (crossings, _, _) = probe(&world, h); - println!("final: crossings={crossings}"); - assert_eq!(crossings, 0, "the blob's boundary is still self-crossed"); -} - -#[test] -fn self_intersect_recovery() { - let mut world = PhysicsWorld::new(); - let strip = SoftBodyBuilder::grid(Vector::ZERO, Vector::new(3.0, 0.15), 3, 2) - .pinned_particles([0, 1, 4, 5]) - .softness(SpringCoefficients::new(3.0, 1.0)) - .self_contacts(SELF_CONTACTS); - let h = world.insert_soft_body(strip); - world.soft_bodies[h].set_particle_position(3, Vector::new(1.0, -0.4)); - - for step in 0..=600 { - if step % 60 == 0 { - let (crossings, inverted, min_ratio) = probe(&world, h); - let p2 = world.soft_bodies[h].particle_position(2); - let p3 = world.soft_bodies[h].particle_position(3); - println!( - "step {step:4}: crossings={crossings} inverted_cells={inverted} \ - min_vol_ratio={min_ratio:+.3} p2=({:+.3},{:+.3}) p3=({:+.3},{:+.3})", - p2.x, p2.y, p3.x, p3.y - ); - } - world.step(); - } - let (crossings, inverted, _) = probe(&world, h); - println!("final: crossings={crossings} inverted_cells={inverted}"); - assert_eq!(crossings, 0, "the strip's boundary is still self-crossed"); - assert_eq!(inverted, 0, "the strip still has inverted cells"); -} - - -/// Boundary crossings between two bodies, and how many of each body's vertices lie inside -/// the other (even-odd parity along +x). -fn pair_overlap( - world: &PhysicsWorld, - a: SoftBodyHandle, - b: SoftBodyHandle, -) -> (usize, usize, usize) { - let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); - let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); - let pa = |v: u32| ma.vertex(sa, v as usize); - let pb = |v: u32| mb.vertex(sb, v as usize); - let mut crossings = 0; - for ea in ma.indices() { - for eb in mb.indices() { - if segments_cross(pa(ea[0]), pa(ea[1]), pb(eb[0]), pb(eb[1])) { - crossings += 1; - } - } - } - let inside = |point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody| { - let mut parity = false; - for e in mesh.indices() { - let (p0, p1) = ( - mesh.vertex(body, e[0] as usize), - mesh.vertex(body, e[1] as usize), - ); - if (p0.y > point.y) != (p1.y > point.y) { - let t = (point.y - p0.y) / (p1.y - p0.y); - if p0.x + t * (p1.x - p0.x) > point.x { - parity = !parity; - } - } - } - parity - }; - let a_in_b = (0..ma.vertex_count()) - .filter(|&v| inside(pa(v as u32), mb, sb)) - .count(); - let b_in_a = (0..mb.vertex_count()) - .filter(|&v| inside(pb(v as u32), ma, sa)) - .count(); - (crossings, a_in_b, b_in_a) -} - -fn eight_blob(center: Vector, radius: Real, n: usize) -> SoftBodyBuilder { - SoftBodyBuilder::disk(center, radius, n) - .softness(SpringCoefficients::new(4.0, 1.0)) - .self_contacts(true) - .particle_mass(0.05) -} - -/// Reshapes a disk blob into an asymmetric 8: a big CCW lobe and a small mirrored (CW) lobe, -/// two circles joined near `center`. The signed area stays clearly positive, but the small -/// lobe's segments are wound backward. -fn make_asym_eight( - world: &mut PhysicsWorld, - h: SoftBodyHandle, - center: Vector, - rb: Real, - rs: Real, -) { - let n = world.soft_bodies[h].particles().len(); - let n_big = (n as Real * rb / (rb + rs)) as usize; - let tau = core::f32::consts::TAU as Real; - for i in 0..n { - let p = if i < n_big { - let t = tau * i as Real / n_big as Real; - center + Vector::new(-rb + rb * t.cos(), rb * t.sin()) - } else { - let t = tau * (i - n_big) as Real / (n - n_big) as Real; - center + Vector::new(rs - rs * t.cos(), -rs * t.sin()) - }; - world.soft_bodies[h].set_particle_position(i, p); - } -} - -fn eight_floor(world: &mut PhysicsWorld) { - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), - ColliderBuilder::cuboid(6.0, 0.5), - ); -} - -/// An 8 with a clearly smaller mirrored lobe recovers to an O: the area deficit gives the -/// volume constraint a strong gradient, and the stand-down lets the lobe flip back through. -#[test] -fn asymmetric_eight_recovers() { - let mut world = PhysicsWorld::new(); - eight_floor(&mut world); - let a = world.insert_soft_body(eight_blob(Vector::new(0.0, 0.65), 0.6, 24)); - make_asym_eight(&mut world, a, Vector::new(0.1, 0.65), 0.55, 0.3); - for _ in 0..600 { - world.step(); - } - let (crossings, _, _) = probe(&world, a); - assert_eq!(crossings, 0, "the asymmetric 8 never recovered"); -} - -/// An 8 whose mirrored lobe starts swallowed inside a bigger blob still recovers, and the -/// pair de-overlaps. -#[test] -fn swallowed_lobe_recovers() { - let mut world = PhysicsWorld::new(); - eight_floor(&mut world); - let a = world.insert_soft_body(eight_blob(Vector::new(-0.5, 0.65), 0.6, 24)); - make_asym_eight(&mut world, a, Vector::new(-0.7, 0.65), 0.55, 0.3); - let b = world.insert_soft_body(eight_blob(Vector::new(0.15, 0.65), 0.55, 20)); - for _ in 0..600 { - world.step(); - } - let (sa, _, _) = probe(&world, a); - let (sb, _, _) = probe(&world, b); - let (cross, a_in_b, b_in_a) = pair_overlap(&world, a, b); - assert_eq!((sa, sb), (0, 0), "a blob is still self-crossed"); - assert_eq!((cross, a_in_b, b_in_a), (0, 0, 0), "the blobs still overlap"); -} - -/// The demo capture with a heavy ball parked on top of the tangled region: the load opposes -/// the elastic recovery, and the stand-down must still resolve the crossing (the probed -/// candidate trigger for active untangling; the elasticity won). -#[test] -fn loaded_tangle_recovers() { - let mut world = PhysicsWorld::new(); - let strip = SoftBodyBuilder::grid(Vector::ZERO, Vector::new(3.0, 0.15), 3, 2) - .pinned_particles([0, 1, 4, 5]) - .softness(SpringCoefficients::new(3.0, 1.0)) - .self_contacts(true); - let h = world.insert_soft_body(strip); - world.soft_bodies[h].set_particle_position(3, Vector::new(1.0, -0.4)); - // A ball ~20x the strip's mass resting right above the tangle. - world.insert( - RigidBodyBuilder::dynamic().translation(Vector::new(0.3, 0.8)), - ColliderBuilder::ball(0.45).density(20.0), - ); - - for step in 0..=900 { - if step % 60 == 0 { - let (crossings, inverted, min_ratio) = probe(&world, h); - let p3 = world.soft_bodies[h].particle_position(3); - println!( - "step {step:4}: crossings={crossings} inverted={inverted} \ - min_ratio={min_ratio:+.3} p3=({:+.3},{:+.3})", - p3.x, p3.y - ); - } - world.step(); - } - let (crossings, inverted, _) = probe(&world, h); - assert_eq!(crossings, 0, "the loaded tangle never resolved"); - assert_eq!(inverted, 0, "cells stayed inverted under the load"); -} - -/// Rough step-time probe: a pile of resting self-colliding blobs (no tangles), to price the -/// per-step crossing sweep. -#[test] -#[ignore] -fn pile_timing() { - let mut world = PhysicsWorld::new(); - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(0.0, -1.0)), - ColliderBuilder::cuboid(6.0, 0.5), - ); - for j in 0..4 { - for i in 0..6 { - let x = -3.0 + i as Real * 1.2 + (j % 2) as Real * 0.3; - let y = 0.2 + j as Real * 1.1; - world.insert_soft_body( - SoftBodyBuilder::disk(Vector::new(x, y), 0.5, 24) - .softness(SpringCoefficients::new(20.0, 1.0)) - .self_contacts(true) - .particle_mass(0.05), - ); - } - } - for _ in 0..120 { - world.step(); - } - let start = std::time::Instant::now(); - for _ in 0..2000 { - world.step(); - } - println!("pile: {:.3} ms/step", start.elapsed().as_secs_f64() * 1000.0 / 2000.0); -} - -/// A rope threaded through a pinned rope slides free under gravity instead of hanging frozen: -/// edge constraints touching the mutual crossing stand down. -#[test] -fn rope_through_rope_slides_free() { - let mut world = PhysicsWorld::new(); - let horizontal = world.insert_soft_body( - SoftBodyBuilder::rope(Vector::new(-1.5, 0.0), Vector::new(1.5, 0.0), 16) - .pinned_particles([0, 15]) - .particle_mass(0.05) - .particle_radius(0.05), - ); - let vertical = world.insert_soft_body( - SoftBodyBuilder::rope(Vector::new(0.02, -1.0), Vector::new(0.02, 1.0), 12) - .particle_mass(0.05) - .particle_radius(0.05), - ); - let crossings = |world: &PhysicsWorld| { - let (cross, _, _) = pair_overlap(world, horizontal, vertical); - cross - }; - assert!(crossings(&world) > 0, "the ropes were authored crossed"); - for _ in 0..300 { - world.step(); - } - let com = world.soft_bodies[vertical].center_of_mass(); - println!("crossings {} com_y {:+.3}", crossings(&world), com.y); - assert_eq!(crossings(&world), 0, "the ropes are still crossed"); - assert!(com.y < -1.5, "the vertical rope hangs frozen at {:+.3}", com.y); -} - -/// With the whole `soft_bodies.recovery` stack disabled a self-intersecting strip stays -/// tangled, and with the defaults it recovers: every toggle is read. -#[test] -fn recovery_toggles_are_wired() { - let build = || { - let mut world = PhysicsWorld::new(); - let strip = SoftBodyBuilder::grid(Vector::ZERO, Vector::new(3.0, 0.15), 3, 2) - .pinned_particles([0, 1, 4, 5]) - .softness(SpringCoefficients::new(3.0, 1.0)) - .self_contacts(true); - let h = world.insert_soft_body(strip); - world.soft_bodies[h].set_particle_position(3, Vector::new(1.0, -0.4)); - (world, h) - }; - - // Defaults: recovers. - let (mut world, h) = build(); - for _ in 0..600 { - world.step(); - } - assert_eq!(probe(&world, h).0, 0, "defaults did not recover the strip"); - - // Everything off: stays tangled, like the pre-recovery baseline. - let (mut world, h) = build(); - world.integration_parameters.soft_bodies.recovery = SoftRecoverySettings { - authored_velocity_margin: false, - edge_speculation: false, - inverted_cell_detection: false, - self_crossing_detection: false, - detection_motion_gating: false, - cross_body_detection: false, - self_stand_down: false, - cross_body_expel_gate: false, - edge_stand_down: false, - crossing_repulsion: false, - crossing_repulsion_guide: false, - crossing_repulsion_self_guide: false, - recovery_pace: 0.5, - overlap_constraints: false, - overlap_rigid: true, - overlap_skip_self_tangled: true, - overlap_edge_stand_down: true, - overlap_constraint_pace: 1.0, - overlap_patch_constraints: SoftPatchConstraints::Keep, - overlap_skin_volume: false, - overlap_kept_depth: 0.0, - overlap_normal_push: false, - overlap_self_regions: false, - overlap_multi_volume: false, - overlap_split: 3, - overlap_patience: 240, - overlap_progress_margin: 0.02, - }; - for _ in 0..600 { - world.step(); - } - assert!( - probe(&world, h).0 > 0, - "disabling every recovery toggle still recovered the strip: a toggle is unwired" - ); -} - -/// Crossing repulsion versus the freeze: with the self stand-down off, the self contacts -/// of a strip vertex flicked through its bottom edge hold the crossing forever; with -/// repulsion the same constraints push the vertex back to the side its neighbors are on. -#[test] -fn crossing_repulsion_recovers_flick() { - for repulsion in [true, false] { - let mut world = PhysicsWorld::new(); - world.gravity = Vector::ZERO; - let r = &mut world.integration_parameters.soft_bodies.recovery; - r.authored_velocity_margin = false; - r.self_stand_down = false; - r.crossing_repulsion = repulsion; - let strip = SoftBodyBuilder::grid(Vector::ZERO, Vector::new(3.0, 0.15), 3, 2) - .pinned_particles([0, 1, 4, 5]) - .softness(SpringCoefficients::new(3.0, 1.0)) - .particle_radius(0.02) - .self_contacts(true); - let h = world.insert_soft_body(strip); - for _ in 0..5 { - world.step(); - } - world.soft_bodies[h].set_particle_velocity(3, Vector::new(0.0, -600.0)); - for _ in 0..120 { - world.step(); - } - let end = probe(&world, h).0; - println!("repulsion={repulsion}: end={end}"); - if repulsion { - assert_eq!(end, 0, "crossing repulsion did not push the vertex back"); - } else { - assert!(end > 0, "the frozen constraints did not keep the strip crossed"); - } - } -} diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index 73b5d077a..85aaf434f 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -1,5 +1,6 @@ -//! Soft-body regression tests (2D): substep invariance, ropes, blobs (area preservation), -//! triangulated bodies, shape matching, two-way coupling, sleeping and removal. +//! Soft-body regression tests (2D): substep invariance, materials (springs, corotational, +//! Neo-Hookean, FEM), contacts, sleeping and removal, snapshots, tearing and cutting, plasticity, +//! impulses, hooks and clusters. use rapier2d::prelude::*; @@ -1578,6 +1579,8 @@ fn fem_deflection_is_substep_invariant() { let settle = |substeps: usize| -> Real { let mut world = PhysicsWorld::new(); world.integration_parameters.num_solver_iterations = substeps; + // The invariance is a property of the converged linear solve: lift the iteration cap. + world.integration_parameters.soft_bodies.fem.max_linear_iterations = 256; let builder = SoftBodyBuilder::grid( Vector::new(length * 0.5, 0.0), Vector::new(length * 0.5, thickness * 0.5), diff --git a/crates/rapier2d/tests/soft_recovery.rs b/crates/rapier2d/tests/soft_recovery.rs new file mode 100644 index 000000000..c82896090 --- /dev/null +++ b/crates/rapier2d/tests/soft_recovery.rs @@ -0,0 +1,756 @@ +//! Soft-body recovery scenes: stacks and pressed piles that must stay uncrossed and uninverted, +//! self-intersecting surfaces that must untangle, and overlapping pairs that must de-overlap +//! through the intersection-volume constraints. Every scene runs the default recovery settings +//! unless it tests an opt-in toggle. + +use rapier2d::prelude::*; + +fn segments_cross(a0: Vector, a1: Vector, b0: Vector, b1: Vector) -> bool { + let d1 = a1 - a0; + let d2 = b1 - b0; + let denom = d1.perp_dot(d2); + if denom.abs() < 1.0e-12 { + return false; + } + let t = (b0 - a0).perp_dot(d2) / denom; + let u = (b0 - a0).perp_dot(d1) / denom; + (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) +} + +/// Even-odd containment of `point` in a closed polyline mesh (ray along +x). +fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { + let mut parity = false; + for e in mesh.indices() { + let (p0, p1) = ( + mesh.vertex(body, e[0] as usize), + mesh.vertex(body, e[1] as usize), + ); + if (p0.y > point.y) != (p1.y > point.y) { + let t = (point.y - p0.y) / (p1.y - p0.y); + if p0.x + t * (p1.x - p0.x) > point.x { + parity = !parity; + } + } + } + parity +} + +/// Boundary crossings among the non-adjacent surface segments of a body. +fn self_crossings(world: &PhysicsWorld, h: SoftBodyHandle) -> usize { + let sb = &world.soft_bodies[h]; + let mesh = sb.meshes().next().unwrap(); + let idx = mesh.indices(); + let p = |v: u32| mesh.vertex(sb, v as usize); + let mut crossings = 0; + for i in 0..idx.len() { + for j in i + 1..idx.len() { + let (ei, ej) = (idx[i], idx[j]); + if ei.iter().any(|v| ej.contains(v)) { + continue; + } + if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { + crossings += 1; + } + } + } + crossings +} + +/// The body's cells with a non-positive area. +fn inverted_cells(world: &PhysicsWorld, h: SoftBodyHandle) -> usize { + let sb = &world.soft_bodies[h]; + sb.cells() + .iter() + .filter(|cell| { + let x: [Vector; 3] = + core::array::from_fn(|k| sb.particle_position(cell.vertices[k] as usize)); + (x[1] - x[0]).perp_dot(x[2] - x[0]) * 0.5 / cell.rest_volume <= 0.0 + }) + .count() +} + +/// Boundary crossings between two bodies, and how many of each body's vertices lie inside +/// the other. +fn pair_overlap( + world: &PhysicsWorld, + a: SoftBodyHandle, + b: SoftBodyHandle, +) -> (usize, usize, usize) { + let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); + let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); + let pa = |v: u32| ma.vertex(sa, v as usize); + let pb = |v: u32| mb.vertex(sb, v as usize); + let mut crossings = 0; + for ea in ma.indices() { + for eb in mb.indices() { + if segments_cross(pa(ea[0]), pa(ea[1]), pb(eb[0]), pb(eb[1])) { + crossings += 1; + } + } + } + let a_in_b = (0..ma.vertex_count()) + .filter(|&v| inside(pa(v as u32), mb, sb)) + .count(); + let b_in_a = (0..mb.vertex_count()) + .filter(|&v| inside(pb(v as u32), ma, sa)) + .count(); + (crossings, a_in_b, b_in_a) +} + +/// The crossings within and between the bodies of a stack, and their inverted cells. +#[derive(Clone, Copy, Debug, Default)] +struct Metrics { + self_crossings: usize, + cross_crossings: usize, + inverted_cells: usize, +} + +fn measure(world: &PhysicsWorld, handles: &[SoftBodyHandle]) -> Metrics { + let mut m = Metrics::default(); + for (i, &a) in handles.iter().enumerate() { + m.self_crossings += self_crossings(world, a); + m.inverted_cells += inverted_cells(world, a); + for &b in &handles[i + 1..] { + m.cross_crossings += pair_overlap(world, a, b).0; + } + } + m +} + +/// Runs `steps` steps, `hook` before each of them (kinematic drivers), and measures the end state. +fn run( + world: &mut PhysicsWorld, + handles: &[SoftBodyHandle], + steps: usize, + mut hook: impl FnMut(&mut PhysicsWorld, usize), +) -> Metrics { + for step in 0..steps { + hook(world, step); + world.step(); + } + measure(world, handles) +} + +fn floor(world: &mut PhysicsWorld, half_width: Real, friction: Real) { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), + ColliderBuilder::cuboid(half_width, 0.5).friction(friction), + ); +} + +/// A floor with two walls: the bin of the pressed piles. +fn bin(world: &mut PhysicsWorld) { + floor(world, 3.5, 0.5); + for x in [-3.5, 3.5] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 4.0)), + ColliderBuilder::cuboid(0.5, 5.0), + ); + } +} + +/// A cell-less loop stiff enough to bear another body. +fn stiff_blob(center: Vector, radius: Real, n: usize) -> SoftBodyBuilder { + SoftBodyBuilder::disk(center, radius, n) + .softness(SpringCoefficients::new(20.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) +} + +/// A soft cell-less loop that folds easily. +fn soft_blob(center: Vector, radius: Real, n: usize) -> SoftBodyBuilder { + SoftBodyBuilder::disk(center, radius, n) + .softness(SpringCoefficients::new(4.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) +} + +/// A solid corotational square (its flat faces neither roll nor tip, unlike a blob). +fn jelly(center: Vector, half: Real) -> SoftBodyBuilder { + SoftBodyBuilder::grid(center, Vector::splat(half), 4, 4) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 3.0e3, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .self_contacts(true) + .particle_mass(0.1) + .particle_radius(0.08) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) +} + +/// A soft strip pinned at both ends, from the `debug_self_intersect2` demo. +fn strip() -> SoftBodyBuilder { + SoftBodyBuilder::grid(Vector::ZERO, Vector::new(3.0, 0.15), 3, 2) + .pinned_particles([0, 1, 4, 5]) + .softness(SpringCoefficients::new(3.0, 1.0)) + .self_contacts(true) +} + +/// The strip with its bottom-middle vertex pushed through its bottom edge. +fn tangled_strip(world: &mut PhysicsWorld) -> SoftBodyHandle { + let h = world.insert_soft_body(strip()); + world.soft_bodies[h].set_particle_position(3, Vector::new(1.0, -0.4)); + h +} + +/// Reshapes a disk blob into an asymmetric 8: a big CCW lobe and a small mirrored (CW) lobe, +/// two circles joined near `center`. The signed area stays clearly positive, but the small +/// lobe's segments are wound backward. +fn make_asym_eight( + world: &mut PhysicsWorld, + h: SoftBodyHandle, + center: Vector, + rb: Real, + rs: Real, +) { + let n = world.soft_bodies[h].particles().len(); + let n_big = (n as Real * rb / (rb + rs)) as usize; + let tau = core::f32::consts::TAU as Real; + for i in 0..n { + let p = if i < n_big { + let t = tau * i as Real / n_big as Real; + center + Vector::new(-rb + rb * t.cos(), rb * t.sin()) + } else { + let t = tau * (i - n_big) as Real / (n - n_big) as Real; + center + Vector::new(rs - rs * t.cos(), -rs * t.sin()) + }; + world.soft_bodies[h].set_particle_position(i, p); + } +} + +/// A column of twelve cell-less blobs dropped one on the other: the bottom blobs bear the +/// whole column's weight and must neither be pushed through the floor nor into each other. +#[test] +fn blob_column() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 4.0, 0.5); + let handles: Vec<_> = (0..12) + .map(|i| { + let x = (i % 2) as Real * 0.05; + world.insert_soft_body(stiff_blob(Vector::new(x, 0.55 + i as Real * 1.05), 0.5, 20)) + }) + .collect(); + // A heavy box lands on the column once it has settled (twenty times a blob's mass). + let m = run(&mut world, &handles, 1200, |world, step| { + if step == 300 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 14.0)), + ColliderBuilder::cuboid(0.6, 0.4).density(10.0), + ); + } + }); + assert_eq!( + m.cross_crossings, 0, + "blobs of the column penetrate each other" + ); + assert_eq!(m.self_crossings, 0, "a blob of the column is self-crossed"); +} + +/// A column of eight solid jelly squares: the cells of the bottom ones compress under the +/// load but must never invert, and the boundaries must not cross. +#[test] +fn jelly_column() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 4.0, 0.5); + let handles: Vec<_> = (0..8) + .map(|i| { + let x = (i % 2) as Real * 0.05; + world.insert_soft_body(jelly(Vector::new(x, 0.55 + i as Real * 1.1), 0.5)) + }) + .collect(); + let m = run(&mut world, &handles, 1200, |world, step| { + if step == 300 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 10.0)), + ColliderBuilder::cuboid(0.6, 0.4).density(10.0), + ); + } + }); + assert_eq!( + m.cross_crossings, 0, + "squares of the column penetrate each other" + ); + assert_eq!( + m.inverted_cells, 0, + "a square of the column has inverted cells" + ); +} + +/// A bin of blobs pressed from above by a kinematic plate: the plate descends slowly to two +/// thirds of the pile's height and holds there. Nothing in the pile may end up crossed or +/// inside a neighbor, whatever the load. +#[test] +fn pressed_blob_pile() { + let mut world = PhysicsWorld::new(); + bin(&mut world); + let mut handles = Vec::new(); + for j in 0..4 { + for i in 0..5 { + let x = -2.4 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.55 + j as Real * 1.05; + handles.push(world.insert_soft_body(stiff_blob(Vector::new(x, y), 0.5, 20))); + } + } + let plate = world.insert_body( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 5.0)), + ); + world.insert_collider(ColliderBuilder::cuboid(2.9, 0.25), Some(plate)); + // Settle, press down at 0.5 u/s to y = 3.0 (the pile starts ~4.3 high), then hold. + let m = run(&mut world, &handles, 1500, |world, step| { + if step >= 300 { + let y = (5.0 - (step - 300) as Real * 0.5 / 60.0).max(3.0); + world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y)); + } + }); + assert_eq!( + m.cross_crossings, 0, + "blobs of the pressed pile penetrate each other" + ); + assert_eq!( + m.self_crossings, 0, + "a blob of the pressed pile is self-crossed" + ); +} + +/// A bin of jelly squares pressed the same way, to four fifths of the pile's height: cells +/// compress, none may invert. +#[test] +fn pressed_jelly_pile() { + let mut world = PhysicsWorld::new(); + bin(&mut world); + let mut handles = Vec::new(); + for j in 0..3 { + for i in 0..5 { + let x = -2.4 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.55 + j as Real * 1.1; + handles.push(world.insert_soft_body(jelly(Vector::new(x, y), 0.5))); + } + } + let plate = world.insert_body( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 4.0)), + ); + world.insert_collider(ColliderBuilder::cuboid(2.9, 0.25), Some(plate)); + let m = run(&mut world, &handles, 1500, |world, step| { + if step >= 300 { + let y = (4.0 - (step - 300) as Real * 0.5 / 60.0).max(2.7); + world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y)); + } + }); + assert_eq!( + m.cross_crossings, 0, + "squares of the pressed pile penetrate each other" + ); + assert_eq!( + m.inverted_cells, 0, + "a square of the pressed pile has inverted cells" + ); +} + +/// A closed blob (no cells: edges + boundary-volume) whose top vertex starts pushed down +/// through the bottom wall recovers. +#[test] +fn blob_vertex_capture() { + let mut world = PhysicsWorld::new(); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -1.0)), + ColliderBuilder::cuboid(4.0, 0.5), + ); + let h = world.insert_soft_body(soft_blob(Vector::new(0.0, 0.0), 0.5, 16)); + // Vertex 4 sits at the top of the disk (angle 90°); park it below the bottom wall. + world.soft_bodies[h].set_particle_position(4, Vector::new(0.0, -0.8)); + for _ in 0..600 { + world.step(); + } + assert_eq!( + self_crossings(&world, h), + 0, + "the blob's boundary is still self-crossed" + ); +} + +/// An 8 with a clearly smaller mirrored lobe recovers to an O: the area deficit gives the +/// volume constraint a strong gradient, and the stand-down lets the lobe flip back through. +#[test] +fn asymmetric_eight_recovers() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 6.0, 0.5); + let a = world.insert_soft_body(soft_blob(Vector::new(0.0, 0.65), 0.6, 24)); + make_asym_eight(&mut world, a, Vector::new(0.1, 0.65), 0.55, 0.3); + for _ in 0..600 { + world.step(); + } + assert_eq!( + self_crossings(&world, a), + 0, + "the asymmetric 8 never recovered" + ); +} + +/// An 8 whose mirrored lobe starts swallowed inside a bigger blob still recovers, and the +/// pair de-overlaps. +#[test] +fn swallowed_lobe_recovers() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 6.0, 0.5); + let a = world.insert_soft_body(soft_blob(Vector::new(-0.5, 0.65), 0.6, 24)); + make_asym_eight(&mut world, a, Vector::new(-0.7, 0.65), 0.55, 0.3); + let b = world.insert_soft_body(soft_blob(Vector::new(0.15, 0.65), 0.55, 20)); + for _ in 0..600 { + world.step(); + } + let (sa, sb) = (self_crossings(&world, a), self_crossings(&world, b)); + assert_eq!((sa, sb), (0, 0), "a blob is still self-crossed"); + assert_eq!( + pair_overlap(&world, a, b), + (0, 0, 0), + "the blobs still overlap" + ); +} + +/// The tangled strip with a heavy ball parked on top of the tangled region: the load opposes +/// the elastic recovery, and the stand-down must still resolve the crossing. +#[test] +fn loaded_tangle_recovers() { + let mut world = PhysicsWorld::new(); + let h = tangled_strip(&mut world); + // A ball ~20x the strip's mass resting right above the tangle. + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.3, 0.8)), + ColliderBuilder::ball(0.45).density(20.0), + ); + for _ in 0..900 { + world.step(); + } + assert_eq!( + self_crossings(&world, h), + 0, + "the loaded tangle never resolved" + ); + assert_eq!( + inverted_cells(&world, h), + 0, + "cells stayed inverted under the load" + ); +} + +/// A rope threaded through a pinned rope slides free under gravity instead of hanging frozen: +/// edge constraints touching the mutual crossing stand down. +#[test] +fn rope_through_rope_slides_free() { + let mut world = PhysicsWorld::new(); + let horizontal = world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(-1.5, 0.0), Vector::new(1.5, 0.0), 16) + .pinned_particles([0, 15]) + .particle_mass(0.05) + .particle_radius(0.05), + ); + let vertical = world.insert_soft_body( + SoftBodyBuilder::rope(Vector::new(0.02, -1.0), Vector::new(0.02, 1.0), 12) + .particle_mass(0.05) + .particle_radius(0.05), + ); + let crossings = |world: &PhysicsWorld| pair_overlap(world, horizontal, vertical).0; + assert!(crossings(&world) > 0, "the ropes were authored crossed"); + for _ in 0..300 { + world.step(); + } + let com = world.soft_bodies[vertical].center_of_mass(); + assert_eq!(crossings(&world), 0, "the ropes are still crossed"); + assert!( + com.y < -1.5, + "the vertical rope hangs frozen at {:+.3}", + com.y + ); +} + +/// With the whole `soft_bodies.recovery` stack disabled the tangled strip stays tangled, and +/// with the defaults it recovers: every toggle is read. +#[test] +fn recovery_toggles_are_wired() { + let mut world = PhysicsWorld::new(); + let h = tangled_strip(&mut world); + for _ in 0..600 { + world.step(); + } + assert_eq!( + self_crossings(&world, h), + 0, + "defaults did not recover the strip" + ); + + let mut world = PhysicsWorld::new(); + let h = tangled_strip(&mut world); + world.integration_parameters.soft_bodies.recovery = SoftRecoverySettings { + authored_velocity_margin: false, + edge_speculation: false, + inverted_cell_detection: false, + self_crossing_detection: false, + detection_motion_gating: false, + cross_body_detection: false, + self_stand_down: false, + cross_body_expel_gate: false, + edge_stand_down: false, + crossing_repulsion: false, + crossing_repulsion_guide: false, + crossing_repulsion_self_guide: false, + recovery_pace: 0.5, + overlap_constraints: false, + overlap_rigid: true, + overlap_skip_self_tangled: true, + overlap_edge_stand_down: true, + overlap_constraint_pace: 1.0, + overlap_patch_constraints: SoftPatchConstraints::Keep, + overlap_skin_volume: false, + overlap_kept_depth: 0.0, + overlap_normal_push: false, + overlap_self_regions: false, + overlap_multi_volume: false, + overlap_split: 3, + overlap_patience: 240, + overlap_progress_margin: 0.02, + }; + for _ in 0..600 { + world.step(); + } + assert!( + self_crossings(&world, h) > 0, + "disabling every recovery toggle still recovered the strip: a toggle is unwired" + ); +} + +/// Crossing repulsion versus the freeze: with the self stand-down off, the self contacts +/// of a strip vertex flicked through its bottom edge hold the crossing forever; with +/// repulsion the same constraints push the vertex back to the side its neighbors are on. +#[test] +fn crossing_repulsion_recovers_flick() { + for repulsion in [true, false] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + let r = &mut world.integration_parameters.soft_bodies.recovery; + r.authored_velocity_margin = false; + r.self_stand_down = false; + r.crossing_repulsion = repulsion; + let h = world.insert_soft_body(strip().particle_radius(0.02)); + for _ in 0..5 { + world.step(); + } + world.soft_bodies[h].set_particle_velocity(3, Vector::new(0.0, -600.0)); + for _ in 0..120 { + world.step(); + } + let end = self_crossings(&world, h); + if repulsion { + assert_eq!(end, 0, "crossing repulsion did not push the vertex back"); + } else { + assert!( + end > 0, + "the frozen constraints did not keep the strip crossed" + ); + } + } +} + +/// A blob whose top vertex was pushed through its own bottom (two crossings and a mirrored tip), +/// with the crossing repulsion and no volume constraint: guided by the fold's volume normal, the +/// tip is pulled back onto the pierced surface until the crossings annihilate. +#[test] +fn self_guided_repulsion_recovers_pierced_tip() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 6.0, 0.6); + let a = world.insert_soft_body(stiff_blob(Vector::new(0.0, 0.56), 0.5, 24)); + // Vertex 6 is the top of the loop (angle 90 degrees): pushed below the bottom. + world.soft_bodies[a].set_particle_position(6, Vector::new(0.0, 0.56 - 0.5 - 0.15)); + let r = &mut world.integration_parameters.soft_bodies.recovery; + r.overlap_constraints = false; + r.crossing_repulsion = true; + r.crossing_repulsion_self_guide = true; + for _ in 0..900 { + world.step(); + } + assert_eq!( + self_crossings(&world, a), + 0, + "the guided repulsion never pulled the tip back" + ); +} + +/// A blob on the floor with two bottom vertices tunneled through it, manifold constraints kept: +/// the volume constraint pulls the vertices back while the manifold constraints of the same +/// features may fight it. Every `overlap_patch_constraints` policy must recover the vertices. +#[test] +fn patch_constraints_tunneled_vertices_recover() { + let mut failed = Vec::new(); + for policy in [ + SoftPatchConstraints::Keep, + SoftPatchConstraints::StandDown, + SoftPatchConstraints::AlongNormal, + ] { + let mut world = PhysicsWorld::new(); + floor(&mut world, 6.0, 0.6); + let a = world.insert_soft_body(stiff_blob(Vector::new(0.0, 0.56), 0.5, 24)); + // Vertices 17, 18, 19 are the bottom of the loop (angles around 270 degrees). + for v in [17usize, 18, 19] { + let p = world.soft_bodies[a].particle_position(v); + world.soft_bodies[a].set_particle_position(v, Vector::new(p.x, p.y - 0.2)); + } + world + .integration_parameters + .soft_bodies + .recovery + .overlap_patch_constraints = policy; + for _ in 0..900 { + world.step(); + } + let below = world.soft_bodies[a] + .particle_positions() + .filter(|p| p.y < 0.0) + .count(); + if below != 0 { + failed.push(policy); + } + } + assert!( + failed.is_empty(), + "{failed:?} left vertices under the floor" + ); +} + +/// A concave "U" whose legs are pressed into each other (a self-contact between distinct regions +/// of one surface): the self-region constraints de-overlap the legs like a pair of bodies, and +/// without them no volume constraint appears. +#[test] +fn self_regions_deoverlap_legs() { + for regions in [false, true] { + let mut world = PhysicsWorld::new(); + floor(&mut world, 6.0, 0.6); + // A U, counter-clockwise, densified to 0.2 segments. + let corners = [ + Vector::new(0.0, 0.0), + Vector::new(1.4, 0.0), + Vector::new(1.4, 1.2), + Vector::new(1.0, 1.2), + Vector::new(1.0, 0.4), + Vector::new(0.4, 0.4), + Vector::new(0.4, 1.2), + Vector::new(0.0, 1.2), + ]; + let mut points = Vec::new(); + for i in 0..corners.len() { + let (a, b) = (corners[i], corners[(i + 1) % corners.len()]); + let n = ((b - a).length() / 0.2).round().max(1.0) as usize; + for k in 0..n { + points.push(a + (b - a) * (k as Real / n as Real)); + } + } + let lift = Vector::new(-0.7, 0.1); + let points: Vec = points.iter().map(|p| *p + lift).collect(); + let h = world.insert_soft_body( + SoftBodyBuilder::polygon(points.clone()) + .softness(SpringCoefficients::new(20.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)), + ); + // The right leg pushed into the left one (their gap is 0.6). + for (i, p) in points.iter().enumerate() { + if p.x - lift.x > 0.9 && p.y - lift.y > 0.4 { + world.soft_bodies[h].set_particle_position(i, *p - Vector::new(0.75, 0.0)); + } + } + world + .integration_parameters + .soft_bodies + .recovery + .overlap_self_regions = regions; + world.step(); + let constraints = world.soft_bodies[h].volume_contacts().count(); + assert_eq!( + constraints > 0, + regions, + "the self-region constraints are {}", + if regions { "missing" } else { "unexpected" } + ); + if !regions { + continue; + } + for _ in 0..900 { + world.step(); + } + assert_eq!( + self_crossings(&world, h), + 0, + "the self-region constraints never de-overlapped the legs" + ); + } +} + +/// Two cell-less blobs inserted overlapping by a third of a radius: each one's intruding +/// arc retracts into its own body, and the pair de-overlaps with no expulsion. +#[test] +fn overlapping_blobs_deoverlap() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 6.0, 0.5); + let a = world.insert_soft_body(soft_blob(Vector::new(-0.42, 0.6), 0.6, 24)); + let b = world.insert_soft_body(soft_blob(Vector::new(0.42, 0.6), 0.6, 24)); + for _ in 0..900 { + world.step(); + } + assert_eq!( + pair_overlap(&world, a, b), + (0, 0, 0), + "the blobs still overlap" + ); +} + +/// A dynamic rigid box inserted inside a hollow blob: the blob's patch inside the box and the +/// box itself share one constraint, and the pair separates without the box being thrown (a box, +/// not a ball: a ball keeps rolling with whatever it was handed). +#[test] +fn rigid_box_inside_blob_separates() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 6.0, 0.5); + let a = world.insert_soft_body(soft_blob(Vector::new(0.0, 0.65), 0.6, 24)); + let (rb, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.3, 0.65)), + ColliderBuilder::cuboid(0.3, 0.3).density(1.0), + ); + for _ in 0..600 { + world.step(); + } + let c = world.bodies[rb].center_of_mass(); + let inside_box = world.soft_bodies[a] + .particle_positions() + .filter(|p| (p.x - c.x).abs() < 0.3 && (p.y - c.y).abs() < 0.3) + .count(); + assert_eq!(inside_box, 0, "the blob still has vertices inside the box"); + let speed = world.bodies[rb].linvel().length(); + assert!(speed < 1.0, "the box was thrown: {speed} u/s"); +} + +/// A hollow blob dropped onto a thin fixed pin that ends up inside it (the pile's impaled +/// blobs): the rigid overlap constraint lifts the blob off the pin. +#[test] +fn impaled_blob_recovers() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 6.0, 0.5); + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, 1.5)), + ColliderBuilder::capsule_x(0.6, 0.1).friction(0.5), + ); + let a = world.insert_soft_body(soft_blob(Vector::new(0.0, 1.5), 0.6, 24)); + let pin_inside = |world: &PhysicsWorld| { + let sb = &world.soft_bodies[a]; + inside(Vector::new(0.0, 1.5), sb.meshes().next().unwrap(), sb) + }; + assert!(pin_inside(&world), "the pin was authored inside the blob"); + for _ in 0..600 { + world.step(); + } + assert!(!pin_inside(&world), "the pin is still inside the blob"); +} diff --git a/crates/rapier2d/tests/soft_stack_probe.rs b/crates/rapier2d/tests/soft_stack_probe.rs deleted file mode 100644 index bfb56bf85..000000000 --- a/crates/rapier2d/tests/soft_stack_probe.rs +++ /dev/null @@ -1,520 +0,0 @@ -//! Stack scenes for the soft-body depenetration work (columns and a pressed pile of soft bodies) -//! with their gating metrics (boundary crossings, vertices inside another body, cell compression, -//! step time); the `SOFT_STACK_PRESET` env variable (`defaults`, `minimal`) picks the preset. - -use rapier2d::prelude::*; - -fn segments_cross(a0: Vector, a1: Vector, b0: Vector, b1: Vector) -> bool { - let d1 = a1 - a0; - let d2 = b1 - b0; - let denom = d1.perp_dot(d2); - if denom.abs() < 1.0e-12 { - return false; - } - let t = (b0 - a0).perp_dot(d2) / denom; - let u = (b0 - a0).perp_dot(d1) / denom; - (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) -} - -/// Even-odd containment of `point` in a closed polyline mesh (ray along +x). -fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { - let mut parity = false; - for e in mesh.indices() { - let (p0, p1) = ( - mesh.vertex(body, e[0] as usize), - mesh.vertex(body, e[1] as usize), - ); - if (p0.y > point.y) != (p1.y > point.y) { - let t = (point.y - p0.y) / (p1.y - p0.y); - if p0.x + t * (p1.x - p0.x) > point.x { - parity = !parity; - } - } - } - parity -} - -/// Distance from `point` to the closest element of the mesh. -fn surface_distance(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> Real { - let mut best: Real = Real::MAX; - for e in mesh.indices() { - let (p0, p1) = ( - mesh.vertex(body, e[0] as usize), - mesh.vertex(body, e[1] as usize), - ); - let d = p1 - p0; - let len2 = d.length_squared(); - let t = if len2 > 0.0 { - ((point - p0).dot(d) / len2).clamp(0.0, 1.0) - } else { - 0.0 - }; - best = best.min((point - (p0 + d * t)).length()); - } - best -} - -/// The stack metrics of one probe. -#[derive(Clone, Copy, Debug, Default)] -struct Metrics { - /// Boundary self-crossings, summed over the bodies. - self_crossings: usize, - /// Boundary crossings between distinct bodies, summed over the pairs. - cross_crossings: usize, - /// Vertices lying inside another body's closed boundary. - inside: usize, - /// The deepest such vertex (distance to that body's boundary). - max_depth: Real, - /// The most compressed cell (current over rest area), 1.0 without cells. - min_cell_ratio: Real, - /// The lowest inverted-cell count over the bodies, summed. - inverted_cells: usize, -} - -fn measure(world: &PhysicsWorld, handles: &[SoftBodyHandle]) -> Metrics { - let mut m = Metrics { - min_cell_ratio: 1.0, - ..Default::default() - }; - let bodies: Vec<(&SoftBody, &SoftCollisionMesh)> = handles - .iter() - .map(|&h| { - let sb = &world.soft_bodies[h]; - (sb, sb.meshes().next().unwrap()) - }) - .collect(); - for (sb, mesh) in &bodies { - let idx = mesh.indices(); - let p = |v: u32| mesh.vertex(sb, v as usize); - for i in 0..idx.len() { - for j in i + 1..idx.len() { - let (ei, ej) = (idx[i], idx[j]); - if ei.iter().any(|v| ej.contains(v)) { - continue; - } - if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { - m.self_crossings += 1; - } - } - } - for cell in sb.cells() { - let x: [Vector; 3] = - core::array::from_fn(|k| sb.particle_position(cell.vertices[k] as usize)); - let vol = (x[1] - x[0]).perp_dot(x[2] - x[0]) * 0.5; - let ratio = vol / cell.rest_volume; - m.min_cell_ratio = m.min_cell_ratio.min(ratio); - if ratio <= 0.0 { - m.inverted_cells += 1; - } - } - } - for a in 0..bodies.len() { - for b in 0..bodies.len() { - if a == b { - continue; - } - let (sa, ma) = bodies[a]; - let (sb, mb) = bodies[b]; - let pa = |v: u32| ma.vertex(sa, v as usize); - let pb = |v: u32| mb.vertex(sb, v as usize); - if a < b { - for ea in ma.indices() { - for eb in mb.indices() { - if segments_cross(pa(ea[0]), pa(ea[1]), pb(eb[0]), pb(eb[1])) { - m.cross_crossings += 1; - } - } - } - } - if !mb.is_closed() { - continue; - } - for v in 0..ma.vertex_count() { - let x = pa(v as u32); - if inside(x, mb, sb) { - m.inside += 1; - m.max_depth = m.max_depth.max(surface_distance(x, mb, sb)); - } - } - } - } - m -} - -/// The recovery preset under test (`SOFT_STACK_PRESET`). -fn apply_preset(world: &mut PhysicsWorld) -> String { - let preset = std::env::var("SOFT_STACK_PRESET").unwrap_or_else(|_| "defaults".into()); - let r = &mut world.integration_parameters.soft_bodies.recovery; - *r = Default::default(); - // Per-feature policy of the volume-patch constraints (`SoftPatchConstraints`) on any preset: - // `SOFT_PATCH_CONSTRAINTS=stand|normal`. - match std::env::var("SOFT_PATCH_CONSTRAINTS").as_deref() { - Ok("stand") => r.overlap_patch_constraints = SoftPatchConstraints::StandDown, - Ok("normal") => r.overlap_patch_constraints = SoftPatchConstraints::AlongNormal, - _ => {} - } - match preset.as_str() { - "defaults" => {} - // The user's reference settings (2026-09-04): prevention without edge speculation, - // detection + stand-down, the intersection-volume constraints on. - "reference" => { - r.edge_speculation = false; - r.overlap_constraints = true; - } - // The reference settings with the crossing repulsion, unguided or guided by the - // pair's (and the fold's) volume normal (see `crossing_repulsion_guide`). - "reference+repel" | "reference+repel-guide" => { - r.edge_speculation = false; - r.overlap_constraints = true; - r.crossing_repulsion = true; - r.crossing_repulsion_guide = preset.ends_with("-guide"); - r.crossing_repulsion_self_guide = preset.ends_with("-guide"); - } - // The intersection-volume constraints on the defaults. - "overlap" => r.overlap_constraints = true, - other => panic!("unknown SOFT_STACK_PRESET `{other}`"), - } - preset -} - -fn floor(world: &mut PhysicsWorld, half_width: Real) { - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), - ColliderBuilder::cuboid(half_width, 0.5), - ); -} - -fn blob(center: Vector, radius: Real) -> SoftBodyBuilder { - SoftBodyBuilder::disk(center, radius, 20) - .softness(SpringCoefficients::new(20.0, 1.0)) - .self_contacts(true) - .particle_mass(0.05) - .particle_radius(0.06) - .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) -} - -fn jelly(center: Vector, half: Real) -> SoftBodyBuilder { - SoftBodyBuilder::grid(center, Vector::splat(half), 4, 4) - .cell_model(SoftBodyCellModel::Corotational) - .material(SoftBodyMaterial { - young_modulus: 3.0e3, - poisson_ratio: 0.4, - elastic_damping_ratio: 0.5, - ..Default::default() - }) - .self_contacts(true) - .particle_mass(0.1) - .particle_radius(0.08) - .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) -} - -/// Runs `steps` steps, printing the metrics every `every` steps (and the step time at the end); -/// `hook` runs before every step (kinematic drivers). -fn run( - name: &str, - world: &mut PhysicsWorld, - handles: &[SoftBodyHandle], - steps: usize, - every: usize, - mut hook: impl FnMut(&mut PhysicsWorld, usize), -) -> Metrics { - let preset = apply_preset(world); - let mut worst = Metrics::default(); - let mut stepping = std::time::Duration::ZERO; - for step in 0..=steps { - if step % every == 0 { - let m = measure(world, handles); - println!( - "{name} [{preset}] step {step:4}: self={} cross={} inside={} depth={:.3} \ - min_cell={:+.3} inverted={}", - m.self_crossings, - m.cross_crossings, - m.inside, - m.max_depth, - m.min_cell_ratio, - m.inverted_cells - ); - worst.self_crossings = worst.self_crossings.max(m.self_crossings); - worst.cross_crossings = worst.cross_crossings.max(m.cross_crossings); - worst.inside = worst.inside.max(m.inside); - worst.max_depth = worst.max_depth.max(m.max_depth); - worst.min_cell_ratio = worst.min_cell_ratio.min(m.min_cell_ratio); - worst.inverted_cells = worst.inverted_cells.max(m.inverted_cells); - } - if step == steps { - break; - } - hook(world, step); - let start = std::time::Instant::now(); - world.step(); - stepping += start.elapsed(); - } - let ms = stepping.as_secs_f64() * 1000.0 / steps as f64; - let last = measure(world, handles); - println!( - "{name} [{preset}] final: self={} cross={} inside={} depth={:.3} min_cell={:+.3} \ - inverted={} ({ms:.3} ms/step); worst: cross={} inside={} depth={:.3} min_cell={:+.3}", - last.self_crossings, - last.cross_crossings, - last.inside, - last.max_depth, - last.min_cell_ratio, - last.inverted_cells, - worst.cross_crossings, - worst.inside, - worst.max_depth, - worst.min_cell_ratio - ); - last -} - -/// A column of twelve cell-less blobs dropped one on the other: the bottom blobs bear the -/// whole column's weight and must neither be pushed through the floor nor into each other. -#[test] -fn blob_column() { - let mut world = PhysicsWorld::new(); - floor(&mut world, 4.0); - let handles: Vec<_> = (0..12) - .map(|i| { - let x = (i % 2) as Real * 0.05; - world.insert_soft_body(blob(Vector::new(x, 0.55 + i as Real * 1.05), 0.5)) - }) - .collect(); - // A heavy box lands on the column once it has settled (twenty times a blob's mass). - let m = run( - "blob_column", - &mut world, - &handles, - 1200, - 100, - |world, step| { - if step == 300 { - world.insert( - RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 14.0)), - ColliderBuilder::cuboid(0.6, 0.4).density(10.0), - ); - } - }, - ); - assert_eq!( - m.cross_crossings, 0, - "blobs of the column penetrate each other" - ); - assert_eq!(m.self_crossings, 0, "a blob of the column is self-crossed"); -} - -/// A column of eight solid jelly squares: the cells of the bottom ones compress under the -/// load but must never invert, and the boundaries must not cross. -#[test] -fn jelly_column() { - let mut world = PhysicsWorld::new(); - floor(&mut world, 4.0); - let handles: Vec<_> = (0..8) - .map(|i| { - let x = (i % 2) as Real * 0.05; - world.insert_soft_body(jelly(Vector::new(x, 0.55 + i as Real * 1.1), 0.5)) - }) - .collect(); - let m = run( - "jelly_column", - &mut world, - &handles, - 1200, - 100, - |world, step| { - if step == 300 { - world.insert( - RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 10.0)), - ColliderBuilder::cuboid(0.6, 0.4).density(10.0), - ); - } - }, - ); - assert_eq!( - m.cross_crossings, 0, - "squares of the column penetrate each other" - ); - assert_eq!( - m.inverted_cells, 0, - "a square of the column has inverted cells" - ); -} - -/// A bin of blobs pressed from above by a kinematic plate: the plate descends slowly to two -/// thirds of the pile's height and holds there. Nothing in the pile may end up crossed or -/// inside a neighbor, whatever the load. -#[test] -fn pressed_blob_pile() { - let mut world = PhysicsWorld::new(); - floor(&mut world, 3.5); - for x in [-3.5, 3.5] { - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(x, 4.0)), - ColliderBuilder::cuboid(0.5, 5.0), - ); - } - let mut handles = Vec::new(); - for j in 0..4 { - for i in 0..5 { - let x = -2.4 + i as Real * 1.2 + (j % 2) as Real * 0.3; - let y = 0.55 + j as Real * 1.05; - handles.push(world.insert_soft_body(blob(Vector::new(x, y), 0.5))); - } - } - let plate = world.insert_body( - RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 5.0)), - ); - world.insert_collider(ColliderBuilder::cuboid(2.9, 0.25), Some(plate)); - // Settle, press down at 0.5 u/s to y = 3.0 (the pile starts ~4.3 high), then hold. - let m = run( - "pressed_blob_pile", - &mut world, - &handles, - 1500, - 150, - |world, step| { - if step >= 300 { - let y = (5.0 - (step - 300) as Real * 0.5 / 60.0).max(3.0); - world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y)); - } - }, - ); - assert_eq!( - m.cross_crossings, 0, - "blobs of the pressed pile penetrate each other" - ); - assert_eq!( - m.self_crossings, 0, - "a blob of the pressed pile is self-crossed" - ); -} - -/// A bin of jelly squares pressed the same way, to four fifths of the pile's height: cells -/// compress, none may invert. -#[test] -fn pressed_jelly_pile() { - let mut world = PhysicsWorld::new(); - floor(&mut world, 3.5); - for x in [-3.5, 3.5] { - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(x, 4.0)), - ColliderBuilder::cuboid(0.5, 5.0), - ); - } - let mut handles = Vec::new(); - for j in 0..3 { - for i in 0..5 { - let x = -2.4 + i as Real * 1.2 + (j % 2) as Real * 0.3; - let y = 0.55 + j as Real * 1.1; - handles.push(world.insert_soft_body(jelly(Vector::new(x, y), 0.5))); - } - } - let plate = world.insert_body( - RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 4.0)), - ); - world.insert_collider(ColliderBuilder::cuboid(2.9, 0.25), Some(plate)); - let m = run( - "pressed_jelly_pile", - &mut world, - &handles, - 1500, - 150, - |world, step| { - if step >= 300 { - let y = (4.0 - (step - 300) as Real * 0.5 / 60.0).max(2.7); - world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y)); - } - }, - ); - assert_eq!( - m.cross_crossings, 0, - "squares of the pressed pile penetrate each other" - ); - assert_eq!( - m.inverted_cells, 0, - "a square of the pressed pile has inverted cells" - ); -} - -/// Checks that the jelly pile crushed to two thirds of its height keeps its cells uninverted and -/// its boundaries uncrossed. Ignored until a phase passes it. -#[test] -#[ignore] -fn crushed_jelly_pile() { - let mut world = PhysicsWorld::new(); - floor(&mut world, 3.5); - for x in [-3.5, 3.5] { - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(x, 4.0)), - ColliderBuilder::cuboid(0.5, 5.0), - ); - } - let mut handles = Vec::new(); - for j in 0..3 { - for i in 0..5 { - let x = -2.4 + i as Real * 1.2 + (j % 2) as Real * 0.3; - let y = 0.55 + j as Real * 1.1; - handles.push(world.insert_soft_body(jelly(Vector::new(x, y), 0.5))); - } - } - let plate = world.insert_body( - RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 4.0)), - ); - world.insert_collider(ColliderBuilder::cuboid(2.9, 0.25), Some(plate)); - let m = run( - "crushed_jelly_pile", - &mut world, - &handles, - 1500, - 150, - |world, step| { - if step >= 300 { - let y = (4.0 - (step - 300) as Real * 0.5 / 60.0).max(2.4); - world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y)); - } - }, - ); - assert_eq!( - m.cross_crossings, 0, - "squares of the crushed pile penetrate each other" - ); - assert_eq!( - m.self_crossings, 0, - "a square of the crushed pile is self-crossed" - ); - assert_eq!( - m.inverted_cells, 0, - "a square of the crushed pile has inverted cells" - ); -} - -/// Step-time reference: the 24-blob pile of the recovery probes, resting. -#[test] -#[ignore] -fn pile_timing() { - let mut world = PhysicsWorld::new(); - floor(&mut world, 6.0); - let handles: Vec<_> = (0..4) - .flat_map(|j| (0..6).map(move |i| (i, j))) - .map(|(i, j)| { - let x = -3.0 + i as Real * 1.2 + (j % 2) as Real * 0.3; - let y = 0.7 + j as Real * 1.1; - world.insert_soft_body(blob(Vector::new(x, y), 0.5)) - }) - .collect(); - apply_preset(&mut world); - for _ in 0..120 { - world.step(); - } - let start = std::time::Instant::now(); - for _ in 0..2000 { - world.step(); - } - let m = measure(&world, &handles); - println!( - "pile: {:.3} ms/step (cross={} inside={})", - start.elapsed().as_secs_f64() * 1000.0 / 2000.0, - m.cross_crossings, - m.inside - ); -} diff --git a/crates/rapier2d/tests/volume_contact.rs b/crates/rapier2d/tests/volume_contact.rs deleted file mode 100644 index 49322c684..000000000 --- a/crates/rapier2d/tests/volume_contact.rs +++ /dev/null @@ -1,332 +0,0 @@ -//! The intersection-volume constraints as contacts (`SoftRecoverySettings::overlap_constraints`) -//! under the reference settings (`SOFT_VOLUME_MONO`: no multi-volume grid, `SOFT_NORMAL_PUSH`: -//! push along the normal): patch policies, self-guided repulsion, self-region overlaps. - -use rapier2d::prelude::*; - -fn segments_cross(a0: Vector, a1: Vector, b0: Vector, b1: Vector) -> bool { - let d1 = a1 - a0; - let d2 = b1 - b0; - let denom = d1.perp_dot(d2); - if denom.abs() < 1.0e-12 { - return false; - } - let t = (b0 - a0).perp_dot(d2) / denom; - let u = (b0 - a0).perp_dot(d1) / denom; - (0.0..=1.0).contains(&t) && (0.0..=1.0).contains(&u) -} - -fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { - let mut parity = false; - for e in mesh.indices() { - let (p0, p1) = ( - mesh.vertex(body, e[0] as usize), - mesh.vertex(body, e[1] as usize), - ); - if (p0.y > point.y) != (p1.y > point.y) { - let t = (point.y - p0.y) / (p1.y - p0.y); - if p0.x + t * (p1.x - p0.x) > point.x { - parity = !parity; - } - } - } - parity -} - -fn overlap(world: &PhysicsWorld, a: SoftBodyHandle, b: SoftBodyHandle) -> (usize, usize, usize) { - let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); - let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); - let pa = |v: u32| ma.vertex(sa, v as usize); - let pb = |v: u32| mb.vertex(sb, v as usize); - let mut crossings = 0; - for ea in ma.indices() { - for eb in mb.indices() { - if segments_cross(pa(ea[0]), pa(ea[1]), pb(eb[0]), pb(eb[1])) { - crossings += 1; - } - } - } - let a_in_b = (0..ma.vertex_count()) - .filter(|&v| inside(pa(v as u32), mb, sb)) - .count(); - let b_in_a = (0..mb.vertex_count()) - .filter(|&v| inside(pb(v as u32), ma, sa)) - .count(); - (crossings, a_in_b, b_in_a) -} - -fn max_speed(world: &PhysicsWorld, h: SoftBodyHandle) -> Real { - world.soft_bodies[h] - .particle_velocities() - .map(|v| v.length()) - .fold(0.0, Real::max) -} - -/// The reference settings with the crossing repulsion guides and the volume constraints on (see -/// the module docs). -fn configure(world: &mut PhysicsWorld) { - let r = &mut world.integration_parameters.soft_bodies.recovery; - r.overlap_constraints = true; - r.edge_speculation = false; - // The mono-volume variant (a single multiplier per pair: blocks tip on slopes). - if std::env::var("SOFT_VOLUME_MONO").is_ok() { - r.overlap_multi_volume = false; - } - if std::env::var("SOFT_NORMAL_PUSH").is_ok() { - r.overlap_normal_push = true; - } -} - -fn floor(world: &mut PhysicsWorld) { - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5)), - ColliderBuilder::cuboid(6.0, 0.5).friction(0.6), - ); -} - -/// Steps to run (`SOFT_STEPS` shortens a scene for tracing). -fn steps() -> usize { - std::env::var("SOFT_STEPS") - .ok() - .and_then(|v| v.parse().ok()) - .unwrap_or(900) -} - -/// The stack suite's blob: a cell-less loop stiff enough to support another. -fn blob(center: Vector, radius: Real, n: usize) -> SoftBodyBuilder { - SoftBodyBuilder::disk(center, radius, n) - .softness(SpringCoefficients::new(20.0, 1.0)) - .self_contacts(true) - .particle_mass(0.05) - .particle_radius(0.06) - .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) -} - -/// The stack suite's jelly square: a solid corotational grid (its flat faces neither roll -/// nor tip, unlike a blob). -fn jelly(center: Vector, half: Real) -> SoftBodyBuilder { - SoftBodyBuilder::grid(center, Vector::splat(half), 4, 4) - .cell_model(SoftBodyCellModel::Corotational) - .material(SoftBodyMaterial { - young_modulus: 3.0e3, - poisson_ratio: 0.4, - elastic_damping_ratio: 0.5, - ..Default::default() - }) - .self_contacts(true) - .particle_mass(0.1) - .particle_radius(0.08) - .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) -} - -/// A blob whose top vertex was pushed through its own bottom (two crossings and a mirrored tip), -/// with the crossing repulsion and no volume constraint: guided by the fold's volume normal, the -/// tip is pulled back onto the pierced surface until the crossings annihilate. -#[test] -fn self_guided_repulsion_recovers_pierced_tip() { - let run = |guided: bool| -> usize { - let mut world = PhysicsWorld::new(); - floor(&mut world); - let a = world.insert_soft_body(blob(Vector::new(0.0, 0.56), 0.5, 24)); - // Vertex 6 is the top of the loop (angle 90 degrees): pushed below the bottom. - world.soft_bodies[a].set_particle_position(6, Vector::new(0.0, 0.56 - 0.5 - 0.15)); - configure(&mut world); - let r = &mut world.integration_parameters.soft_bodies.recovery; - r.overlap_constraints = false; - r.crossing_repulsion = true; - r.crossing_repulsion_self_guide = guided; - let self_crossings = |world: &PhysicsWorld| { - let sb = &world.soft_bodies[a]; - let mesh = sb.meshes().next().unwrap(); - let idx = mesh.indices(); - let p = |v: u32| mesh.vertex(sb, v as usize); - let mut crossings = 0; - for i in 0..idx.len() { - for j in i + 1..idx.len() { - let (ei, ej) = (idx[i], idx[j]); - if ei.iter().any(|v| ej.contains(v)) { - continue; - } - if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { - crossings += 1; - } - } - } - crossings - }; - for step in 0..=steps() { - if step % 100 == 0 { - println!( - "guided {guided} step {step:4}: crossings={} area={:+.3}", - self_crossings(&world), - world.soft_bodies[a].volume() - ); - } - world.step(); - } - self_crossings(&world) - }; - let unguided = run(false); - let guided = run(true); - println!("end crossings: unguided {unguided}, guided {guided}"); - assert_eq!(guided, 0, "the guided repulsion never pulled the tip back"); -} - -/// A blob on the floor with two bottom vertices tunneled through it, manifold constraints kept: -/// the volume constraint pulls the vertices back while the manifold constraints of the same -/// features may fight it. Every `overlap_patch_constraints` policy must recover the vertices. -#[test] -fn patch_constraints_tunneled_vertices_recover() { - let run = |policy: SoftPatchConstraints| -> (usize, Real) { - let mut world = PhysicsWorld::new(); - floor(&mut world); - let a = world.insert_soft_body(blob(Vector::new(0.0, 0.56), 0.5, 24)); - // Vertices 17, 18, 19 are the bottom of the loop (angles around 270 degrees). - for v in [17usize, 18, 19] { - let p = world.soft_bodies[a].particle_position(v); - world.soft_bodies[a].set_particle_position(v, Vector::new(p.x, p.y - 0.2)); - } - configure(&mut world); - world - .integration_parameters - .soft_bodies - .recovery - .overlap_patch_constraints = policy; - let below = |world: &PhysicsWorld| { - world.soft_bodies[a] - .particle_positions() - .filter(|p| p.y < 0.0) - .count() - }; - for step in 0..=steps() { - if step % 100 == 0 { - println!( - "{policy:?} step {step:4}: below floor={} speed={:.4}", - below(&world), - max_speed(&world, a) - ); - } - world.step(); - } - (below(&world), max_speed(&world, a)) - }; - let mut failed = Vec::new(); - for policy in [ - SoftPatchConstraints::Keep, - SoftPatchConstraints::StandDown, - SoftPatchConstraints::AlongNormal, - ] { - let (below, speed) = run(policy); - println!("{policy:?}: below floor {below}, speed {speed:.4}"); - if below != 0 { - failed.push(policy); - } - } - assert!( - failed.is_empty(), - "{failed:?} left vertices under the floor" - ); -} - -/// A concave "U" whose legs are pressed into each other (a self-contact between distinct regions -/// of one surface): the self-region constraints de-overlap the legs like a pair of bodies. -#[test] -fn self_regions_deoverlap_legs() { - let run = |regions: bool| -> usize { - let mut world = PhysicsWorld::new(); - floor(&mut world); - // A U, counter-clockwise, densified to 0.2 segments. - let corners = [ - Vector::new(0.0, 0.0), - Vector::new(1.4, 0.0), - Vector::new(1.4, 1.2), - Vector::new(1.0, 1.2), - Vector::new(1.0, 0.4), - Vector::new(0.4, 0.4), - Vector::new(0.4, 1.2), - Vector::new(0.0, 1.2), - ]; - let mut points = Vec::new(); - for i in 0..corners.len() { - let (a, b) = (corners[i], corners[(i + 1) % corners.len()]); - let n = ((b - a).length() / 0.2).round().max(1.0) as usize; - for k in 0..n { - points.push(a + (b - a) * (k as Real / n as Real)); - } - } - let lift = Vector::new(-0.7, 0.1); - let points: Vec = points.iter().map(|p| *p + lift).collect(); - let h = world.insert_soft_body( - SoftBodyBuilder::polygon(points.clone()) - .softness(SpringCoefficients::new(20.0, 1.0)) - .self_contacts(true) - .particle_mass(0.05) - .particle_radius(0.06) - .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)), - ); - // The right leg pushed into the left one (their gap is 0.6): by 0.15 by default, - // `SOFT_LEG_PUSH` sets the shift. - let shift: Real = std::env::var("SOFT_LEG_PUSH") - .ok() - .and_then(|v| v.parse().ok()) - .unwrap_or(0.75); - for (i, p) in points.iter().enumerate() { - if p.x - lift.x > 0.9 && p.y - lift.y > 0.4 { - world.soft_bodies[h].set_particle_position(i, *p - Vector::new(shift, 0.0)); - } - } - configure(&mut world); - world - .integration_parameters - .soft_bodies - .recovery - .overlap_self_regions = regions; - world.step(); - let constraints = world.soft_bodies[h].volume_contacts().count(); - println!("regions {regions}: {constraints} volume constraints after the first step"); - assert_eq!( - constraints > 0, - regions, - "the self-region constraints are {}", - if regions { "missing" } else { "unexpected" } - ); - let self_crossings = |world: &PhysicsWorld| { - let sb = &world.soft_bodies[h]; - let mesh = sb.meshes().next().unwrap(); - let idx = mesh.indices(); - let p = |v: u32| mesh.vertex(sb, v as usize); - let mut crossings = 0; - for i in 0..idx.len() { - for j in i + 1..idx.len() { - let (ei, ej) = (idx[i], idx[j]); - if ei.iter().any(|v| ej.contains(v)) { - continue; - } - if segments_cross(p(ei[0]), p(ei[1]), p(ej[0]), p(ej[1])) { - crossings += 1; - } - } - } - crossings - }; - for step in 0..=steps() { - if step % 100 == 0 { - println!( - "regions {regions} step {step:4}: crossings={} area={:+.3} speed={:.3}", - self_crossings(&world), - world.soft_bodies[h].volume(), - max_speed(&world, h) - ); - } - world.step(); - } - self_crossings(&world) - }; - let plain = run(false); - let regions = run(true); - println!("end crossings: plain {plain}, regions {regions}"); - assert_eq!( - regions, 0, - "the self-region constraints never de-overlapped the legs" - ); -} diff --git a/crates/rapier3d/tests/overlap_constraints3.rs b/crates/rapier3d/tests/overlap_constraints3.rs deleted file mode 100644 index 296113d9b..000000000 --- a/crates/rapier3d/tests/overlap_constraints3.rs +++ /dev/null @@ -1,108 +0,0 @@ -//! 3D intersection-volume constraints (see the 2D `overlap_constraints`): two closed cell-less balloons -//! inserted overlapping de-overlap, each retracting its own intruding patch. - -use rapier3d::prelude::*; - -fn segment_crosses_triangle(p: Vector, q: Vector, tri: &[Vector; 3]) -> bool { - let n = (tri[1] - tri[0]).cross(tri[2] - tri[0]); - let dp = (p - tri[0]).dot(n); - let dq = (q - tri[0]).dot(n); - if dp * dq >= 0.0 { - return false; - } - let pq = q - p; - let d = [ - pq.dot((tri[0] - p).cross(tri[1] - p)), - pq.dot((tri[1] - p).cross(tri[2] - p)), - pq.dot((tri[2] - p).cross(tri[0] - p)), - ]; - d.iter().all(|x| *x >= 0.0) || d.iter().all(|x| *x <= 0.0) -} - -fn triangles_cross(a: &[Vector; 3], b: &[Vector; 3]) -> bool { - (0..3).any(|k| segment_crosses_triangle(a[k], a[(k + 1) % 3], b)) - || (0..3).any(|k| segment_crosses_triangle(b[k], b[(k + 1) % 3], a)) -} - -fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { - let q = point + Vector::new(0.5341, 0.6432, 0.5487) * 100.0; - let mut crossings = 0; - for e in mesh.indices() { - let tri: [Vector; 3] = core::array::from_fn(|k| mesh.vertex(body, e[k] as usize)); - if segment_crosses_triangle(point, q, &tri) { - crossings += 1; - } - } - crossings % 2 == 1 -} - -fn overlap(world: &PhysicsWorld, a: SoftBodyHandle, b: SoftBodyHandle) -> (usize, usize, usize) { - let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); - let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); - let tris = |sb: &SoftBody, mesh: &SoftCollisionMesh| -> Vec<[Vector; 3]> { - mesh.indices() - .iter() - .map(|e| core::array::from_fn(|k| mesh.vertex(sb, e[k] as usize))) - .collect() - }; - let (ta, tb) = (tris(sa, ma), tris(sb, mb)); - let mut crossings = 0; - for x in &ta { - for y in &tb { - if triangles_cross(x, y) { - crossings += 1; - } - } - } - let a_in_b = (0..ma.vertex_count()) - .filter(|&v| inside(ma.vertex(sa, v), mb, sb)) - .count(); - let b_in_a = (0..mb.vertex_count()) - .filter(|&v| inside(mb.vertex(sb, v), ma, sa)) - .count(); - (crossings, a_in_b, b_in_a) -} - -fn overlap_only(world: &mut PhysicsWorld) { - if std::env::var("SOFT_DEPEN_PRESET").as_deref() == Ok("defaults") { - return; - } - // The reference settings: prevention without edge speculation, detection and the - // stand-down, the constraints on. - let r = &mut world.integration_parameters.soft_bodies.recovery; - r.overlap_constraints = true; - r.edge_speculation = false; - // Diagnostic: the closed-closed edge constraints off. - if std::env::var("SOFT_DEPEN_NO_EDGE").is_ok() { - r.edge_speculation = false; - } -} - -fn balloon(center: Vector, radius: Real) -> SoftBodyBuilder { - SoftBodyBuilder::sphere(center, radius, 1) - .softness(SpringCoefficients::new(20.0, 1.0)) - .self_contacts(true) - .particle_mass(0.05) - .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) -} - -#[test] -fn overlapping_balloons_deoverlap() { - let mut world = PhysicsWorld::new(); - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), - ColliderBuilder::cuboid(6.0, 0.5, 6.0), - ); - let a = world.insert_soft_body(balloon(Vector::new(-0.35, 0.55, 0.0), 0.5)); - let b = world.insert_soft_body(balloon(Vector::new(0.35, 0.55, 0.0), 0.5)); - overlap_only(&mut world); - for step in 0..=900 { - if step % 100 == 0 { - let (c, ab, ba) = overlap(&world, a, b); - println!("step {step:4}: crossings={c} a_in_b={ab} b_in_a={ba}"); - } - world.step(); - } - let (c, ab, ba) = overlap(&world, a, b); - assert_eq!((c, ab, ba), (0, 0, 0), "the balloons still overlap"); -} diff --git a/crates/rapier3d/tests/query_pipeline_subshapes.rs b/crates/rapier3d/tests/query_pipeline_subshapes.rs new file mode 100644 index 000000000..a252429de --- /dev/null +++ b/crates/rapier3d/tests/query_pipeline_subshapes.rs @@ -0,0 +1,120 @@ +//! The query pipeline's hits keep the hit collider's own sub-shape (a trimesh's triangle), and +//! the query filter still applies. + +use rapier3d::parry::query::{NonlinearRigidMotion, ShapeCastOptions}; +use rapier3d::prelude::*; + +/// A ground trimesh of two triangles (`z <= x` is triangle 0, `z >= x` triangle 1) at +/// `x = 10`, facing +y, over a cuboid at `y = -5`. +fn scene() -> (PhysicsWorld, ColliderHandle, ColliderHandle) { + let mut world = PhysicsWorld::new(); + let vertices = vec![ + Vector::new(-1.0, 0.0, -1.0), + Vector::new(1.0, 0.0, -1.0), + Vector::new(1.0, 0.0, 1.0), + Vector::new(-1.0, 0.0, 1.0), + ]; + let (_, mesh) = world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(10.0, 0.0, 0.0)), + ColliderBuilder::trimesh(vertices, vec![[0, 2, 1], [0, 3, 2]]).unwrap(), + ); + let (_, floor) = world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(10.0, -5.0, 0.0)), + ColliderBuilder::cuboid(5.0, 0.5, 5.0), + ); + // The broad phase indexes the colliders on the first step. + world.step(); + (world, mesh, floor) +} + +#[test] +fn ray_hits_report_the_triangle() { + let (world, mesh, floor) = scene(); + let pipeline = world.query_pipeline(); + let down = Vector::new(0.0, -1.0, 0.0); + for (x, z, triangle) in [(0.5, -0.5, 0), (-0.5, 0.5, 1)] { + let ray = Ray::new(Vector::new(10.0 + x, 5.0, z), down); + let (handle, hit) = pipeline + .cast_ray_and_get_normal(&ray, Real::MAX, true) + .expect("the ray hits the mesh"); + assert_eq!(handle, mesh); + assert_eq!(hit.subshape, triangle, "wrong triangle"); + assert!((hit.time_of_impact - 5.0).abs() < 1.0e-4); + let trimesh = world.colliders[mesh].shape().as_trimesh().unwrap(); + assert!(!trimesh.is_backface(hit.feature)); + } + // From below, the same triangle is hit on its back face. + let ray = Ray::new(Vector::new(10.5, -2.0, -0.5), -down); + let (handle, hit) = pipeline + .cast_ray_and_get_normal(&ray, Real::MAX, true) + .unwrap(); + assert_eq!(handle, mesh); + assert_eq!(hit.subshape, 0); + let trimesh = world.colliders[mesh].shape().as_trimesh().unwrap(); + assert!(trimesh.is_backface(hit.feature)); + // The filter still excludes colliders: the ray passes on to the floor. + let filter = QueryFilter::default().exclude_collider(mesh); + let ray = Ray::new(Vector::new(10.5, 5.0, -0.5), down); + let (handle, hit) = world + .query_pipeline_with_filter(filter) + .cast_ray_and_get_normal(&ray, Real::MAX, true) + .unwrap(); + assert_eq!(handle, floor); + assert_eq!(hit.subshape, 0); + assert!((hit.time_of_impact - 9.5).abs() < 1.0e-4); +} + +#[test] +fn point_projections_report_the_triangle() { + let (world, mesh, _) = scene(); + let pipeline = world.query_pipeline(); + for (x, z, triangle) in [(0.5, -0.5, 0), (-0.5, 0.5, 1)] { + let point = Vector::new(10.0 + x, 0.3, z); + let (handle, proj) = pipeline + .project_point(point, Real::MAX, true) + .expect("the mesh is the closest collider"); + assert_eq!(handle, mesh); + assert_eq!(proj.subshape, triangle, "wrong triangle"); + assert!((proj.point - Vector::new(10.0 + x, 0.0, z)).length() < 1.0e-4); + let (handle, proj, feature) = pipeline + .project_point_and_get_feature(point, Real::MAX) + .unwrap(); + assert_eq!(handle, mesh); + assert_eq!(proj.subshape, triangle); + assert_eq!(feature, FeatureId::Face(0), "the triangle's own feature"); + } +} + +#[test] +fn shape_casts_report_the_triangle() { + let (world, mesh, _) = scene(); + let pipeline = world.query_pipeline(); + let ball = Ball::new(0.25); + for (x, z, triangle) in [(0.5, -0.5, 0), (-0.5, 0.5, 1)] { + let start = Pose::from_translation(Vector::new(10.0 + x, 5.0, z)); + let (handle, hit) = pipeline + .cast_shape( + &start, + Vector::new(0.0, -1.0, 0.0), + &ball, + ShapeCastOptions::default(), + ) + .expect("the ball hits the mesh"); + assert_eq!(handle, mesh); + assert_eq!((hit.subshape1, hit.subshape2), (triangle, 0)); + assert!((hit.time_of_impact - 4.75).abs() < 1.0e-3); + + let motion = NonlinearRigidMotion::new( + start, + Vector::ZERO, + Vector::new(0.0, -1.0, 0.0), + Vector::ZERO, + ); + let (handle, hit) = pipeline + .cast_shape_nonlinear(&motion, &ball, 0.0, 10.0, true) + .expect("the ball hits the mesh"); + assert_eq!(handle, mesh); + assert_eq!((hit.subshape1, hit.subshape2), (triangle, 0)); + assert!((hit.time_of_impact - 4.75).abs() < 1.0e-2); + } +} diff --git a/crates/rapier3d/tests/self_intersect_probe3.rs b/crates/rapier3d/tests/self_intersect_probe3.rs deleted file mode 100644 index 6ea155a6d..000000000 --- a/crates/rapier3d/tests/self_intersect_probe3.rs +++ /dev/null @@ -1,406 +0,0 @@ -//! Headless probes of 3D self-intersection recovery: surfaces that start in a self-crossed state -//! must recover instead of being held tangled by their own self-contact constraints (mechanism in -//! the 2D `self_intersect_probe`). Env toggle `NO_SELF_CONTACTS` runs the baseline without them. - -use rapier3d::prelude::*; - -const SELF_CONTACTS: bool = option_env!("NO_SELF_CONTACTS").is_none(); - -/// Whether the segment `[p, q]` crosses the triangle's interior transversally. -fn segment_crosses_triangle(p: Vector, q: Vector, tri: &[Vector; 3]) -> bool { - let n = (tri[1] - tri[0]).cross(tri[2] - tri[0]); - let dp = (p - tri[0]).dot(n); - let dq = (q - tri[0]).dot(n); - if dp * dq >= 0.0 { - return false; - } - let pq = q - p; - let d = [ - pq.dot((tri[0] - p).cross(tri[1] - p)), - pq.dot((tri[1] - p).cross(tri[2] - p)), - pq.dot((tri[2] - p).cross(tri[0] - p)), - ]; - d.iter().all(|x| *x >= 0.0) || d.iter().all(|x| *x <= 0.0) -} - -/// The number of edge-vs-triangle self-crossings of the body's surface, and its inverted cells. -fn probe(world: &PhysicsWorld, h: SoftBodyHandle) -> (usize, usize) { - let sb = &world.soft_bodies[h]; - let mesh = sb.meshes().next().unwrap(); - let indices = mesh.indices(); - let p = |v: u32| mesh.vertex(sb, v as usize); - let mut crossings = 0; - for i in 0..indices.len() { - for j in 0..indices.len() { - let (ei, ej) = (indices[i], indices[j]); - if i == j || ei.iter().any(|v| ej.contains(v)) { - continue; - } - let tri = [p(ej[0]), p(ej[1]), p(ej[2])]; - for k in 0..3 { - let (a, b) = (ei[k], ei[(k + 1) % 3]); - if a < b && segment_crosses_triangle(p(a), p(b), &tri) { - crossings += 1; - } - } - } - } - let mut inverted = 0; - for cell in sb.cells() { - let x: [Vector; 4] = - core::array::from_fn(|k| sb.particle_position(cell.vertices[k] as usize)); - let vol = (x[1] - x[0]).cross(x[2] - x[0]).dot(x[3] - x[0]); - if vol * cell.rest_volume <= 0.0 { - inverted += 1; - } - } - (crossings, inverted) -} - -/// Rough step-time probe: a resting self-colliding cloth (no tangles), to price the per-step -/// edge-vs-triangle crossing sweep. -#[test] -#[ignore] -fn cloth_timing() { - let mut world = PhysicsWorld::new(); - let n = 20; - let edge = |i: usize, j: usize| i == 0 || j == 0 || i == n - 1 || j == n - 1; - let pinned = (0..n * n) - .filter(|&id| edge(id / n, id % n)) - .map(|id| id as u32); - let extent = 0.12 * (n - 1) as Real / 2.0; - world.insert_soft_body( - SoftBodyBuilder::cloth( - Vector::new(-extent, 2.0, -extent), - Vector::new(0.12, 0.0, 0.0), - Vector::new(0.0, 0.0, 0.12), - n, - n, - ) - .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) - .softness(SpringCoefficients::new(40.0, 1.0)) - .particle_mass(0.02) - .particle_radius(0.05) - .pinned_particles(pinned) - .self_contacts(true) - ); - for _ in 0..120 { - world.step(); - } - let start = std::time::Instant::now(); - for _ in 0..1000 { - world.step(); - } - println!( - "cloth: {:.3} ms/step", - start.elapsed().as_secs_f64() * 1000.0 / 1000.0 - ); -} - -/// A thin volumetric slab pinned at both end faces, with a top vertex parked below the bottom -/// face: cells invert and the boundary trimesh self-crosses. The stand-down (inverted cells -/// and edge-vs-triangle crossings) must let the elasticity pull it back out. -#[test] -fn slab_vertex_capture() { - let mut world = PhysicsWorld::new(); - let builder = SoftBodyBuilder::cuboid( - Vector::new(0.0, 0.15, 0.0), - Vector::new(1.5, 0.15, 0.15), - 3, - 2, - 2, - ) - .softness(SpringCoefficients::new(5.0, 1.0)); - let pinned: Vec = builder - .particle_positions() - .iter() - .enumerate() - .filter(|(_, p)| p.x.abs() > 1.4) - .map(|(i, _)| i as u32) - .collect(); - let target = Vector::new(0.0, 0.3, 0.15); - let captured = builder - .particle_positions() - .iter() - .enumerate() - .min_by(|(_, a), (_, b)| { - (**a - target) - .length() - .partial_cmp(&(**b - target).length()) - .unwrap() - }) - .unwrap() - .0; - let h = world.insert_soft_body(builder.pinned_particles(pinned).self_contacts(SELF_CONTACTS)); - world.soft_bodies[h].set_particle_position(captured, Vector::new(0.3, -0.4, 0.0)); - - for step in 0..=600 { - if step % 60 == 0 { - let (crossings, inverted) = probe(&world, h); - let p = world.soft_bodies[h].particle_position(captured); - println!( - "step {step:4}: crossings={crossings} inverted_cells={inverted} \ - captured=({:+.3},{:+.3},{:+.3})", - p.x, p.y, p.z - ); - } - world.step(); - } - let (crossings, inverted) = probe(&world, h); - println!("final: crossings={crossings} inverted_cells={inverted}"); - assert_eq!(crossings, 0, "the slab's boundary is still self-crossed"); - assert_eq!(inverted, 0, "the slab still has inverted cells"); -} - -/// A cloth strip folded into two layers (no cells at all), with one top-layer vertex parked -/// below the bottom layer: its incident triangles pierce the bottom layer. Only the crossing -/// signal can see this; the stand-down must let the elasticity pull the vertex back through. -#[test] -fn folded_cloth_vertex_capture() { - let mut world = PhysicsWorld::new(); - let (nu, nv) = (8usize, 3usize); - let builder = SoftBodyBuilder::cloth( - Vector::ZERO, - Vector::new(0.25, 0.0, 0.0), - Vector::new(0.0, 0.0, 0.25), - nu, - nv, - ) - .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) - .softness(SpringCoefficients::new(40.0, 1.0)) - .particle_mass(0.02) - .particle_radius(0.05) - // Both ends of the strip pinned: the outer edge of the bottom layer at its rest place, - // the free edge of the folded-back top layer above it (positions set below). - .pinned_particles((0..nv as u32).chain((nu as u32 - 1) * nv as u32..(nu * nv) as u32)) - .self_contacts(SELF_CONTACTS); - let h = world.insert_soft_body(builder); - // Fold columns 4..8 back over the first four, one gap above them. - let idx = |i: usize, j: usize| i * nv + j; - for i in 4..nu { - for j in 0..nv { - let x = 0.25 * (7 - i) as Real; - world.soft_bodies[h] - .set_particle_position(idx(i, j), Vector::new(x, 0.15, 0.25 * j as Real)); - } - } - // Park a middle vertex of the top layer below the bottom layer. - let captured = idx(5, 1); - world.soft_bodies[h].set_particle_position(captured, Vector::new(0.5, -0.15, 0.25)); - - for step in 0..=600 { - if step % 60 == 0 { - let (crossings, _) = probe(&world, h); - let p = world.soft_bodies[h].particle_position(captured); - println!( - "step {step:4}: crossings={crossings} captured=({:+.3},{:+.3},{:+.3})", - p.x, p.y, p.z - ); - } - world.step(); - } - let (crossings, _) = probe(&world, h); - println!("final: crossings={crossings}"); - assert_eq!(crossings, 0, "the cloth is still self-crossed"); -} - -/// The number of edge-vs-triangle crossings between two bodies' surfaces (both directions). -fn cross_crossings(world: &PhysicsWorld, a: SoftBodyHandle, b: SoftBodyHandle) -> usize { - let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); - let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); - let pa = |v: u32| ma.vertex(sa, v as usize); - let pb = |v: u32| mb.vertex(sb, v as usize); - let mut crossings = 0; - let mut count = |ea: &[u32; 3], tri: &[Vector; 3], p: &dyn Fn(u32) -> Vector| { - for k in 0..3 { - let (u, v) = (ea[k], ea[(k + 1) % 3]); - if u < v && segment_crosses_triangle(p(u), p(v), tri) { - crossings += 1; - } - } - }; - for ea in ma.indices() { - for eb in mb.indices() { - let tb = [pb(eb[0]), pb(eb[1]), pb(eb[2])]; - count(ea, &tb, &pa); - let ta = [pa(ea[0]), pa(ea[1]), pa(ea[2])]; - count(eb, &ta, &pb); - } - } - crossings -} - -/// A cloth strip threaded through a pinned strip slides free under gravity instead of -/// hanging frozen on it: edge constraints touching the mutual crossing stand down (thin open -/// bodies meet through their edge constraints, so the vertex-pass gate alone cannot free them). -#[test] -fn ribbon_through_ribbon_slides_free() { - let mut world = PhysicsWorld::new(); - let strip = |origin: Vector, du: Vector, dv: Vector| { - SoftBodyBuilder::cloth(origin, du, dv, 12, 2) - .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) - .softness(SpringCoefficients::new(40.0, 1.0)) - .particle_mass(0.02) - .particle_radius(0.04) - }; - let horizontal = world.insert_soft_body( - strip( - Vector::new(-1.1, 0.0, -0.1), - Vector::new(0.2, 0.0, 0.0), - Vector::new(0.0, 0.0, 0.2), - ) - .pinned_particles([0, 1, 22, 23]), - ); - // Vertical, in the xz-plane of the pinned strip's middle, crossing its surface. - let vertical = world.insert_soft_body(strip( - Vector::new(0.03, -1.1, -0.1), - Vector::new(0.0, 0.2, 0.0), - Vector::new(0.0, 0.0, 0.2), - )); - let crossings = |world: &PhysicsWorld| cross_crossings(world, horizontal, vertical); - assert!(crossings(&world) > 0, "the strips were authored crossed"); - for _ in 0..300 { - world.step(); - } - let com = world.soft_bodies[vertical].center_of_mass(); - println!("crossings {} com_y {:+.3}", crossings(&world), com.y); - assert_eq!(crossings(&world), 0, "the strips are still crossed"); - assert!(com.y < -1.5, "the vertical strip hangs frozen at {:+.3}", com.y); -} - -/// Two closed slim bars crossing edge-first collide instead of passing through: closed-vs-closed -/// pairs emit edge-vs-edge constraints in 3D with the vertex pass's speculative reach. -#[test] -fn edge_leading_bars_collide() { - for speed in [5.0, 20.0] { - let mut world = PhysicsWorld::new(); - world.gravity = Vector::ZERO; - // The edge constraints under test are behind the (default-off) edge speculation. - world.integration_parameters.soft_bodies.recovery.edge_speculation = true; - let bar = |center: Vector, along_x: bool| { - let he = if along_x { - Vector::new(1.5, 0.15, 0.15) - } else { - Vector::new(0.15, 0.15, 1.5) - }; - SoftBodyBuilder::cuboid( - center, - he, - if along_x { 6 } else { 2 }, - 2, - if along_x { 2 } else { 6 }, - ) - .softness(SpringCoefficients::new(20.0, 1.0)) - .particle_mass(0.1) - .particle_radius(0.02) - }; - let c = core::f32::consts::FRAC_1_SQRT_2; - let a = world.insert_soft_body(bar(Vector::new(0.0, 0.0, 0.0), true)); - for i in 0..world.soft_bodies[a].particles().len() { - let p = world.soft_bodies[a].particle_position(i); - let (y, z) = (p.y, p.z); - world.soft_bodies[a] - .set_particle_position(i, Vector::new(p.x, c * (y - z), c * (y + z))); - } - let b = world.insert_soft_body(bar(Vector::new(0.0, 1.2, 0.0), false)); - for i in 0..world.soft_bodies[b].particles().len() { - let p = world.soft_bodies[b].particle_position(i); - let (x, y) = (p.x, p.y - 1.2); - world.soft_bodies[b].set_particle_position( - i, - Vector::new(c * (x + y), 1.2 + c * (y - x), p.z), - ); - } - for i in 0..world.soft_bodies[b].particles().len() { - world.soft_bodies[b].set_particle_velocity(i, Vector::new(0.0, -speed, 0.0)); - } - let mut max_cross = 0; - for _ in 0..90 { - world.step(); - max_cross = max_cross.max(cross_crossings(&world, a, b)); - } - let com = world.soft_bodies[b].center_of_mass(); - println!("speed {speed:5}: max_cross {max_cross} b_com_y {:+.3}", com.y); - assert_eq!(max_cross, 0, "speed {speed}: the bars crossed"); - } -} - -/// A frictionless rope threaded through a closed soft ball slides out under gravity: the crossing -/// stand-down frees the piercing, so nothing wrong-sided holds the thread. -#[test] -fn rope_through_ball_slides_out() { - let mut world = PhysicsWorld::new(); - world.insert_soft_body( - SoftBodyBuilder::sphere(Vector::new(0.0, 0.0, 0.0), 0.4, 1) - .softness(SpringCoefficients::new(20.0, 1.0)) - .particle_mass(0.1) - .surface_collider(ColliderBuilder::ball(0.05).friction(0.0)) - .pinned_particles([0, 3, 11]), - ); - let rope = world.insert_soft_body( - SoftBodyBuilder::rope(Vector::new(0.0, -1.0, 0.02), Vector::new(0.0, 1.0, 0.02), 16) - .particle_mass(0.02) - .particle_radius(0.03) - .surface_collider(ColliderBuilder::ball(0.03).friction(0.0)), - ); - for _ in 0..400 { - world.step(); - } - let com = world.soft_bodies[rope].center_of_mass(); - let inside = (0..16) - .filter(|&i| world.soft_bodies[rope].particle_position(i).length() < 0.4) - .count(); - println!("rope com_y {:+.3}, particles inside ball: {inside}", com.y); - assert_eq!(inside, 0, "the rope still threads the ball"); - assert!(com.y < -1.0, "the rope hangs frozen at {:+.3}", com.y); -} - -/// Crossing repulsion clears a static crossing nothing else resolves: an unstrained horizontal -/// strip whose free end pokes through a vertical strip. With the crossing constraints dropped -/// nothing moves; with repulsion the end vertices are pushed back to their neighbors' side. -#[test] -fn crossing_repulsion_clears_static_poke() { - for repulsion in [true, false] { - let mut world = PhysicsWorld::new(); - world.gravity = Vector::ZERO; - let r = &mut world.integration_parameters.soft_bodies.recovery; - r.crossing_repulsion = repulsion; - let strip = |origin: Vector, du: Vector, dv: Vector| { - SoftBodyBuilder::cloth(origin, du, dv, 12, 2) - .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) - .softness(SpringCoefficients::new(40.0, 1.0)) - .particle_mass(0.02) - .particle_radius(0.02) - }; - // Horizontal, pinned at its left end, its right end (x = 1.2) 0.05 past the vertical - // strip's plane (x = 1.15). - let horizontal = world.insert_soft_body( - strip( - Vector::new(-1.0, 0.0, -0.1), - Vector::new(0.2, 0.0, 0.0), - Vector::new(0.0, 0.0, 0.2), - ) - .pinned_particles([0, 1]), - ); - let vertical = world.insert_soft_body( - strip( - Vector::new(1.15, -1.1, -0.1), - Vector::new(0.0, 0.2, 0.0), - Vector::new(0.0, 0.0, 0.2), - ) - .pinned_particles([0, 1, 22, 23]), - ); - let start = cross_crossings(&world, horizontal, vertical); - assert!(start > 0, "the strips were not authored crossed"); - for _ in 0..120 { - world.step(); - } - let end = cross_crossings(&world, horizontal, vertical); - println!("repulsion={repulsion}: {start} -> {end}"); - if repulsion { - assert_eq!(end, 0, "crossing repulsion did not clear the poke"); - } else { - assert!(end > 0, "the dropped constraints resolved the poke (the scene proves nothing)"); - } - } -} diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index 05027bdcd..3b813030c 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -1651,8 +1651,8 @@ fn creeping_body_stays_awake() { let mut handles = vec![]; for (i, plastic_yield) in [0.0, 0.05].into_iter().enumerate() { // Resting on the ground rather than authored into it, and the elastic twin squat enough - // not to buckle: overlap recovery is paced (see SoftPairBudget) and this test is about - // sleep gating. The creeping twin stays slender (it collapses either way). + // not to buckle: this test is about sleep gating, not overlap recovery. The creeping + // twin stays slender (it collapses either way). let width = if plastic_yield > 0.0 { 0.3 } else { 0.45 }; let column = SoftBodyBuilder::cuboid( Vector::new(i as Real * 3.0, 1.3, 0.0), @@ -2933,9 +2933,8 @@ fn skinned_bodies_collide_with_each_other() { ); } -/// Reproducer for a known bug (see `docs/soft-soft-overlap-bug.md`): two soft bodies inserted -/// with overlapping contact skins are thrown apart instead of settling. Ignored because it -/// fails (the per-pair de-overlap budget that fixed it lives on the experimental branch). +/// Reproducer for a known bug: two soft bodies inserted with overlapping contact skins are +/// thrown apart instead of settling. Ignored because it still fails. #[test] #[ignore = "known bug: soft-vs-soft bodies inserted within each other's contact skins explode"] fn soft_bodies_inserted_overlapping_settle() { @@ -3234,6 +3233,8 @@ fn fem_deflection_is_substep_invariant() { let settle = |solver: SoftBodySolver, substeps: usize, young: Real| -> Real { let mut world = PhysicsWorld::new(); world.integration_parameters.num_solver_iterations = substeps; + // The invariance is a property of the converged linear solve: lift the iteration cap. + world.integration_parameters.soft_bodies.fem.max_linear_iterations = 256; let (builder, tip) = fem_cantilever(solver, young, length, thickness, 9, 3); let handle = world.insert_soft_body(builder.mass(mass)); for _ in 0..1500 { @@ -3590,9 +3591,11 @@ fn fem_edge_period_is_substep_invariant() { #[test] fn fem_rope_hangs_from_anchor() { let mut world = PhysicsWorld::new(); + // Damped, so the released rope stops swinging and hangs within the run. let rope = SoftBodyBuilder::rope(Vector::ZERO, Vector::new(2.0, 0.0, 0.0), 21) .pinned_particles([0]) .softness(SpringCoefficients::new(300.0, 1.0)) + .linear_damping(2.0) .solver(SoftBodySolver::Fem) .no_surface_collider() .can_sleep(false); @@ -3821,8 +3824,9 @@ fn fem_plastic_cells_keep_their_deformation() { ); } -/// Tearing on the FEM path: a pinned FEM cloth pulled apart tears, and the topology change is -/// picked up by the solver (the sparsity pattern is rebuilt). +/// Tearing on the FEM path: a pinned FEM cloth pulled apart tears (the pieces become bodies of +/// their own), and the topology change is picked up by the solver (the sparsity pattern is +/// rebuilt). #[cfg(feature = "fem")] #[test] fn fem_cloth_tears_when_pulled_apart() { @@ -3842,44 +3846,58 @@ fn fem_cloth_tears_when_pulled_apart() { .no_surface_collider() .can_sleep(false); let handle = world.insert_soft_body(cloth); - let left: Vec = (0..n).map(|k| k * n).collect(); - let right: Vec = (0..n).map(|k| k * n + n - 1).collect(); - for &i in left.iter().chain(right.iter()) { - world.soft_bodies[handle].set_particle_pinned(i, true); + // The two side columns, driven apart; they follow the tears into the split-off pieces. + let mut left: Vec<(SoftBodyHandle, u32)> = (0..n).map(|k| (handle, (k * n) as u32)).collect(); + let mut right: Vec<(SoftBodyHandle, u32)> = + (0..n).map(|k| (handle, (k * n + n - 1) as u32)).collect(); + for &(_, i) in left.iter().chain(right.iter()) { + world.soft_bodies[handle].set_particle_pinned(i as usize, true); } - let rest_left: Vec = left - .iter() - .map(|&i| world.soft_bodies[handle].particle_position(i)) - .collect(); - let rest_right: Vec = right - .iter() - .map(|&i| world.soft_bodies[handle].particle_position(i)) - .collect(); + let rest = |driven: &[(SoftBodyHandle, u32)]| -> Vec { + driven + .iter() + .map(|&(_, i)| world.soft_bodies[handle].particle_position(i as usize)) + .collect() + }; + let (rest_left, rest_right) = (rest(&left), rest(&right)); let particles_before = world.soft_bodies[handle].num_particles(); let triangles_before = world.soft_bodies[handle].boundary().len(); let version_before = world.soft_bodies[handle].topology_version(); + let log = TearLog::default(); for step in 0..400 { let pull = 0.004 * step as Real; - for (&i, p) in left.iter().zip(&rest_left) { - world.soft_bodies[handle] - .set_particle_kinematic_target(i, *p - Vector::new(pull, 0.0, 0.0)); + for (&(body, i), p) in left.iter().zip(&rest_left) { + world.soft_bodies[body] + .set_particle_kinematic_target(i as usize, *p - Vector::new(pull, 0.0, 0.0)); } - for (&i, p) in right.iter().zip(&rest_right) { - world.soft_bodies[handle] - .set_particle_kinematic_target(i, *p + Vector::new(pull, 0.0, 0.0)); + for (&(body, i), p) in right.iter().zip(&rest_right) { + world.soft_bodies[body] + .set_particle_kinematic_target(i as usize, *p + Vector::new(pull, 0.0, 0.0)); + } + world.step_with_events(&(), &log); + let events = log.drain(); + for r in left.iter_mut().chain(right.iter_mut()) { + *r = follow(&events, r.0, r.1); } - world.step(); } - assert_finite(&world, handle); - let sb = &world.soft_bodies[handle]; - assert!( - sb.num_particles() > particles_before, - "the FEM cloth did not tear: no particle split" - ); - assert_eq!(sb.boundary().len(), triangles_before, "the torn FEM cloth lost triangles"); - sb.validate_topology().unwrap(); + let bodies = family(&world, handle); + assert!(bodies.len() >= 2, "the FEM cloth did not tear in two"); + let particles: usize = bodies + .iter() + .map(|&h| world.soft_bodies[h].num_particles()) + .sum(); + assert!(particles > particles_before, "the crack did not split any particle"); + let triangles: usize = bodies + .iter() + .map(|&h| world.soft_bodies[h].boundary().len()) + .sum(); + assert_eq!(triangles, triangles_before, "the torn FEM cloth lost triangles"); + for &h in &bodies { + assert_finite(&world, h); + world.soft_bodies[h].validate_topology().unwrap(); + } assert!( - sb.topology_version() > version_before, + world.soft_bodies[handle].topology_version() > version_before, "the topology version was not bumped" ); } diff --git a/crates/rapier3d/tests/soft_body_clusters.rs b/crates/rapier3d/tests/soft_body_clusters.rs index a4f045acf..03c9d5885 100644 --- a/crates/rapier3d/tests/soft_body_clusters.rs +++ b/crates/rapier3d/tests/soft_body_clusters.rs @@ -248,7 +248,6 @@ fn tear_duplicates_join_their_source_clusters() { .tear_strain(0.2), ); world.step(); - let before = world.soft_bodies[h].num_particles(); // Tear a middle edge immediately. let torn = world.tear_soft_body(h, &[30], &[]); assert!(torn.is_some()); @@ -257,7 +256,6 @@ fn tear_duplicates_join_their_source_clusters() { // Whatever was duplicated must be covered by cluster 0 (refcounts stay consistent). assert_eq!(sb.cluster(0).unwrap().particles().len(), after); sb.validate_topology().unwrap(); - let _ = before; for _ in 0..5 { world.step(); } @@ -402,7 +400,7 @@ fn cluster_stiffness_scale_stiffens_a_region() { SoftBodyBuilder::cuboid(Vector::new(0.0, 0.5, 0.0), Vector::splat(0.5), 3, 3, 3) .cell_model(SoftBodyCellModel::Corotational) .material(SoftBodyMaterial { - young_modulus: 5.0e2, + young_modulus: 2.0e2, poisson_ratio: 0.3, elastic_damping_ratio: 1.0, ..Default::default() @@ -417,7 +415,7 @@ fn cluster_stiffness_scale_stiffens_a_region() { SoftBodyBuilder::cuboid(Vector::new(0.0, 0.5, 0.0), Vector::splat(0.5), 3, 3, 3) .cell_model(SoftBodyCellModel::Corotational) .material(SoftBodyMaterial { - young_modulus: 5.0e2, + young_modulus: 2.0e2, poisson_ratio: 0.3, elastic_damping_ratio: 1.0, ..Default::default() @@ -427,7 +425,7 @@ fn cluster_stiffness_scale_stiffens_a_region() { .surface_collider(ColliderBuilder::ball(0.05)), ); // Stiffen the whole body through the whole-body cluster: it must sag less than the soft - // reference under its own weight. + // reference under its own weight (a few centimeters at this modulus). world.soft_bodies[h].set_cluster_stiffness_scale(0, 50.0); for _ in 0..300 { world.step(); @@ -590,12 +588,13 @@ fn a_rigid_collider_on_a_sub_cluster_does_not_fight_its_own_body() { #[test] fn a_rigid_collider_on_a_cluster_brings_the_world_to_the_particles() { let mut world = world_with_ground(); - // A soft jelly, so the load's deflection is visible. + // A soft jelly, so the load's deflection is visible: the box's weight over the top face + // compresses it by about a twelfth of its height at this modulus. let h = world.insert_soft_body( SoftBodyBuilder::cuboid(Vector::new(0.0, 0.6, 0.0), Vector::splat(0.5), 3, 3, 3) .cell_model(SoftBodyCellModel::Corotational) .material(SoftBodyMaterial { - young_modulus: 2.0e3, + young_modulus: 5.0e2, poisson_ratio: 0.3, elastic_damping_ratio: 1.0, ..Default::default() diff --git a/crates/rapier3d/tests/soft_recovery3.rs b/crates/rapier3d/tests/soft_recovery3.rs new file mode 100644 index 000000000..3784778a5 --- /dev/null +++ b/crates/rapier3d/tests/soft_recovery3.rs @@ -0,0 +1,590 @@ +//! 3D soft-body recovery scenes (mechanisms in the 2D `soft_recovery`): balloon and jelly +//! columns under a heavy weight, a pressed cube pile, self-crossed surfaces that must untangle, +//! and overlapping balloons that must de-overlap. Every scene runs the default recovery +//! settings unless it tests an opt-in toggle. + +use rapier3d::prelude::*; + +/// Whether the segment `[p, q]` crosses the triangle's interior transversally. +fn segment_crosses_triangle(p: Vector, q: Vector, tri: &[Vector; 3]) -> bool { + let n = (tri[1] - tri[0]).cross(tri[2] - tri[0]); + let dp = (p - tri[0]).dot(n); + let dq = (q - tri[0]).dot(n); + if dp * dq >= 0.0 { + return false; + } + let pq = q - p; + let d = [ + pq.dot((tri[0] - p).cross(tri[1] - p)), + pq.dot((tri[1] - p).cross(tri[2] - p)), + pq.dot((tri[2] - p).cross(tri[0] - p)), + ]; + d.iter().all(|x| *x >= 0.0) || d.iter().all(|x| *x <= 0.0) +} + +/// Whether two triangles cross (an edge of one pierces the other). +fn triangles_cross(a: &[Vector; 3], b: &[Vector; 3]) -> bool { + (0..3).any(|k| segment_crosses_triangle(a[k], a[(k + 1) % 3], b)) + || (0..3).any(|k| segment_crosses_triangle(b[k], b[(k + 1) % 3], a)) +} + +/// Ray-parity containment of `point` in a closed triangle mesh. +fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { + let q = point + Vector::new(0.5341, 0.6432, 0.5487) * 100.0; + let mut crossings = 0; + for e in mesh.indices() { + let tri: [Vector; 3] = core::array::from_fn(|k| mesh.vertex(body, e[k] as usize)); + if segment_crosses_triangle(point, q, &tri) { + crossings += 1; + } + } + crossings % 2 == 1 +} + +/// The world-space triangles of a body's surface mesh. +fn triangles(body: &SoftBody, mesh: &SoftCollisionMesh) -> Vec<[Vector; 3]> { + mesh.indices() + .iter() + .map(|e| core::array::from_fn(|k| mesh.vertex(body, e[k] as usize))) + .collect() +} + +/// Crossings among the non-adjacent surface triangles of a body. +fn self_crossings(world: &PhysicsWorld, h: SoftBodyHandle) -> usize { + let sb = &world.soft_bodies[h]; + let mesh = sb.meshes().next().unwrap(); + let idx = mesh.indices(); + let tris = triangles(sb, mesh); + let mut crossings = 0; + for i in 0..idx.len() { + for j in i + 1..idx.len() { + if idx[i].iter().any(|v| idx[j].contains(v)) { + continue; + } + if triangles_cross(&tris[i], &tris[j]) { + crossings += 1; + } + } + } + crossings +} + +/// The body's cells with a non-positive volume. +fn inverted_cells(world: &PhysicsWorld, h: SoftBodyHandle) -> usize { + let sb = &world.soft_bodies[h]; + sb.cells() + .iter() + .filter(|cell| { + let x: [Vector; 4] = + core::array::from_fn(|k| sb.particle_position(cell.vertices[k] as usize)); + let vol = (x[1] - x[0]).cross(x[2] - x[0]).dot(x[3] - x[0]); + vol * cell.rest_volume <= 0.0 + }) + .count() +} + +/// Surface crossings between two bodies, and how many of each body's vertices lie inside +/// the other (0 for an open surface). +fn pair_overlap( + world: &PhysicsWorld, + a: SoftBodyHandle, + b: SoftBodyHandle, +) -> (usize, usize, usize) { + let (sa, sb) = (&world.soft_bodies[a], &world.soft_bodies[b]); + let (ma, mb) = (sa.meshes().next().unwrap(), sb.meshes().next().unwrap()); + let (ta, tb) = (triangles(sa, ma), triangles(sb, mb)); + let mut crossings = 0; + for x in &ta { + for y in &tb { + if triangles_cross(x, y) { + crossings += 1; + } + } + } + let count_inside = |m1: &SoftCollisionMesh, s1: &SoftBody, m2: &SoftCollisionMesh, s2| { + if !m2.is_closed() { + return 0; + } + (0..m1.vertex_count()) + .filter(|&v| inside(m1.vertex(s1, v), m2, s2)) + .count() + }; + ( + crossings, + count_inside(ma, sa, mb, sb), + count_inside(mb, sb, ma, sa), + ) +} + +/// The crossings within and between the bodies of a stack, and their inverted cells. +#[derive(Clone, Copy, Debug, Default)] +struct Metrics { + self_crossings: usize, + cross_crossings: usize, + inverted_cells: usize, +} + +fn measure(world: &PhysicsWorld, handles: &[SoftBodyHandle]) -> Metrics { + let mut m = Metrics::default(); + for (i, &a) in handles.iter().enumerate() { + m.self_crossings += self_crossings(world, a); + m.inverted_cells += inverted_cells(world, a); + for &b in &handles[i + 1..] { + m.cross_crossings += pair_overlap(world, a, b).0; + } + } + m +} + +/// Runs `steps` steps, `hook` before each of them (kinematic drivers), and measures the end state. +fn run( + world: &mut PhysicsWorld, + handles: &[SoftBodyHandle], + steps: usize, + mut hook: impl FnMut(&mut PhysicsWorld, usize), +) -> Metrics { + for step in 0..steps { + hook(world, step); + world.step(); + } + measure(world, handles) +} + +fn floor(world: &mut PhysicsWorld, half: Real) { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), + ColliderBuilder::cuboid(half, 0.5, half), + ); +} + +/// A closed cell-less sphere stiff enough to bear another body. +fn balloon(center: Vector, radius: Real) -> SoftBodyBuilder { + SoftBodyBuilder::sphere(center, radius, 1) + .softness(SpringCoefficients::new(20.0, 1.0)) + .self_contacts(true) + .particle_mass(0.05) + .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) +} + +/// A solid corotational cube. +fn jelly(center: Vector, half: Real) -> SoftBodyBuilder { + SoftBodyBuilder::cuboid(center, Vector::splat(half), 3, 3, 3) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 3.0e3, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .self_contacts(true) + .particle_mass(0.1) + .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) +} + +/// A stiff cloth strip of twelve columns and two rows. +fn strip(origin: Vector, du: Vector, dv: Vector, radius: Real) -> SoftBodyBuilder { + SoftBodyBuilder::cloth(origin, du, dv, 12, 2) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new( + 40.0, 1.0, + ))) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.02) + .particle_radius(radius) +} + +/// A column of six balloons with a heavy box landing on top once it has settled. +#[test] +fn balloon_column() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 4.0); + let handles: Vec<_> = (0..6) + .map(|i| { + let x = (i % 2) as Real * 0.05; + world.insert_soft_body(balloon(Vector::new(x, 0.55 + i as Real * 1.05, 0.0), 0.5)) + }) + .collect(); + let m = run(&mut world, &handles, 900, |world, step| { + if step == 300 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 8.0, 0.0)), + ColliderBuilder::cuboid(0.5, 0.3, 0.5).density(10.0), + ); + } + }); + assert_eq!( + m.cross_crossings, 0, + "balloons of the column penetrate each other" + ); + assert_eq!( + m.self_crossings, 0, + "a balloon of the column is self-crossed" + ); +} + +/// A column of five jelly cubes under the same weight: cells compress, none may invert. +#[test] +fn jelly_column() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 4.0); + let handles: Vec<_> = (0..5) + .map(|i| { + let x = (i % 2) as Real * 0.05; + world.insert_soft_body(jelly(Vector::new(x, 0.55 + i as Real * 1.1, 0.0), 0.5)) + }) + .collect(); + let m = run(&mut world, &handles, 900, |world, step| { + if step == 300 { + world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 7.0, 0.0)), + ColliderBuilder::cuboid(0.5, 0.3, 0.5).density(10.0), + ); + } + }); + assert_eq!( + m.cross_crossings, 0, + "cubes of the column penetrate each other" + ); + assert_eq!( + m.inverted_cells, 0, + "a cube of the column has inverted cells" + ); +} + +/// Two layers of jelly cubes in a bin, pressed by a kinematic plate to four fifths of +/// their height and held there. +#[test] +fn pressed_jelly_pile() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 3.0); + for (x, z) in [(-2.6, 0.0), (2.6, 0.0), (0.0, -2.6), (0.0, 2.6)] { + world.insert( + RigidBodyBuilder::fixed().translation(Vector::new(x, 3.0, z)), + ColliderBuilder::cuboid( + if z == 0.0 { 0.3 } else { 2.9 }, + 3.5, + if z == 0.0 { 2.9 } else { 0.3 }, + ), + ); + } + let mut handles = Vec::new(); + for j in 0..2 { + for i in 0..2 { + for k in 0..2 { + let x = -0.6 + i as Real * 1.2 + (j % 2) as Real * 0.3; + let z = -0.6 + k as Real * 1.2 + (j % 2) as Real * 0.3; + let y = 0.55 + j as Real * 1.1; + handles.push(world.insert_soft_body(jelly(Vector::new(x, y, z), 0.5))); + } + } + } + let plate = world.insert_body( + RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 3.0, 0.0)), + ); + world.insert_collider(ColliderBuilder::cuboid(2.2, 0.25, 2.2), Some(plate)); + let m = run(&mut world, &handles, 1200, |world, step| { + if step >= 300 { + let y = (3.0 - (step - 300) as Real * 0.5 / 60.0).max(1.95); + world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y, 0.0)); + } + }); + assert_eq!( + m.cross_crossings, 0, + "cubes of the pressed pile penetrate each other" + ); + assert_eq!( + m.inverted_cells, 0, + "a cube of the pressed pile has inverted cells" + ); +} + +/// A thin volumetric slab pinned at both end faces, with a top vertex parked below the bottom +/// face: cells invert and the boundary trimesh self-crosses. The stand-down (inverted cells +/// and edge-vs-triangle crossings) must let the elasticity pull it back out. +#[test] +fn slab_vertex_capture() { + let mut world = PhysicsWorld::new(); + let builder = SoftBodyBuilder::cuboid( + Vector::new(0.0, 0.15, 0.0), + Vector::new(1.5, 0.15, 0.15), + 3, + 2, + 2, + ) + .softness(SpringCoefficients::new(5.0, 1.0)); + let pinned: Vec = builder + .particle_positions() + .iter() + .enumerate() + .filter(|(_, p)| p.x.abs() > 1.4) + .map(|(i, _)| i as u32) + .collect(); + let target = Vector::new(0.0, 0.3, 0.15); + let captured = builder + .particle_positions() + .iter() + .enumerate() + .min_by(|(_, a), (_, b)| { + (**a - target) + .length() + .partial_cmp(&(**b - target).length()) + .unwrap() + }) + .unwrap() + .0; + let h = world.insert_soft_body(builder.pinned_particles(pinned).self_contacts(true)); + world.soft_bodies[h].set_particle_position(captured, Vector::new(0.3, -0.4, 0.0)); + for _ in 0..600 { + world.step(); + } + assert_eq!( + self_crossings(&world, h), + 0, + "the slab's boundary is still self-crossed" + ); + assert_eq!( + inverted_cells(&world, h), + 0, + "the slab still has inverted cells" + ); +} + +/// A cloth strip folded into two layers (no cells at all), with one top-layer vertex parked +/// below the bottom layer: its incident triangles pierce the bottom layer. Only the crossing +/// signal can see this; the stand-down must let the elasticity pull the vertex back through. +#[test] +fn folded_cloth_vertex_capture() { + let mut world = PhysicsWorld::new(); + let (nu, nv) = (8usize, 3usize); + let builder = SoftBodyBuilder::cloth( + Vector::ZERO, + Vector::new(0.25, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.25), + nu, + nv, + ) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new( + 40.0, 1.0, + ))) + .softness(SpringCoefficients::new(40.0, 1.0)) + .particle_mass(0.02) + .particle_radius(0.05) + // Both ends of the strip pinned: the outer edge of the bottom layer at its rest place, + // the free edge of the folded-back top layer above it (positions set below). + .pinned_particles((0..nv as u32).chain((nu as u32 - 1) * nv as u32..(nu * nv) as u32)) + .self_contacts(true); + let h = world.insert_soft_body(builder); + // Fold columns 4..8 back over the first four, one gap above them. + let idx = |i: usize, j: usize| i * nv + j; + for i in 4..nu { + for j in 0..nv { + let x = 0.25 * (7 - i) as Real; + world.soft_bodies[h] + .set_particle_position(idx(i, j), Vector::new(x, 0.15, 0.25 * j as Real)); + } + } + // Park a middle vertex of the top layer below the bottom layer. + let captured = idx(5, 1); + world.soft_bodies[h].set_particle_position(captured, Vector::new(0.5, -0.15, 0.25)); + for _ in 0..600 { + world.step(); + } + assert_eq!( + self_crossings(&world, h), + 0, + "the cloth is still self-crossed" + ); +} + +/// A cloth strip threaded through a pinned strip slides free under gravity instead of +/// hanging frozen on it: edge constraints touching the mutual crossing stand down (thin open +/// bodies meet through their edge constraints, so the vertex-pass gate alone cannot free them). +#[test] +fn ribbon_through_ribbon_slides_free() { + let mut world = PhysicsWorld::new(); + let horizontal = world.insert_soft_body( + strip( + Vector::new(-1.1, 0.0, -0.1), + Vector::new(0.2, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.2), + 0.04, + ) + .pinned_particles([0, 1, 22, 23]), + ); + // Vertical, in the xz-plane of the pinned strip's middle, crossing its surface. + let vertical = world.insert_soft_body(strip( + Vector::new(0.03, -1.1, -0.1), + Vector::new(0.0, 0.2, 0.0), + Vector::new(0.0, 0.0, 0.2), + 0.04, + )); + let crossings = |world: &PhysicsWorld| pair_overlap(world, horizontal, vertical).0; + assert!(crossings(&world) > 0, "the strips were authored crossed"); + for _ in 0..300 { + world.step(); + } + let com = world.soft_bodies[vertical].center_of_mass(); + assert_eq!(crossings(&world), 0, "the strips are still crossed"); + assert!( + com.y < -1.5, + "the vertical strip hangs frozen at {:+.3}", + com.y + ); +} + +/// Two closed slim bars crossing edge-first collide instead of passing through: closed-vs-closed +/// pairs emit edge-vs-edge constraints in 3D with the vertex pass's speculative reach. +#[test] +fn edge_leading_bars_collide() { + for speed in [5.0, 20.0] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + // The edge constraints under test are behind the (default-off) edge speculation. + world + .integration_parameters + .soft_bodies + .recovery + .edge_speculation = true; + let bar = |center: Vector, along_x: bool| { + let he = if along_x { + Vector::new(1.5, 0.15, 0.15) + } else { + Vector::new(0.15, 0.15, 1.5) + }; + SoftBodyBuilder::cuboid( + center, + he, + if along_x { 6 } else { 2 }, + 2, + if along_x { 2 } else { 6 }, + ) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.1) + .particle_radius(0.02) + }; + let c = core::f32::consts::FRAC_1_SQRT_2; + let a = world.insert_soft_body(bar(Vector::new(0.0, 0.0, 0.0), true)); + for i in 0..world.soft_bodies[a].particles().len() { + let p = world.soft_bodies[a].particle_position(i); + let (y, z) = (p.y, p.z); + world.soft_bodies[a] + .set_particle_position(i, Vector::new(p.x, c * (y - z), c * (y + z))); + } + let b = world.insert_soft_body(bar(Vector::new(0.0, 1.2, 0.0), false)); + for i in 0..world.soft_bodies[b].particles().len() { + let p = world.soft_bodies[b].particle_position(i); + let (x, y) = (p.x, p.y - 1.2); + world.soft_bodies[b] + .set_particle_position(i, Vector::new(c * (x + y), 1.2 + c * (y - x), p.z)); + } + for i in 0..world.soft_bodies[b].particles().len() { + world.soft_bodies[b].set_particle_velocity(i, Vector::new(0.0, -speed, 0.0)); + } + let mut max_cross = 0; + for _ in 0..90 { + world.step(); + max_cross = max_cross.max(pair_overlap(&world, a, b).0); + } + assert_eq!(max_cross, 0, "speed {speed}: the bars crossed"); + } +} + +/// A frictionless rope threaded through a closed soft ball slides out under gravity: the crossing +/// stand-down frees the piercing, so nothing wrong-sided holds the thread. +#[test] +fn rope_through_ball_slides_out() { + let mut world = PhysicsWorld::new(); + world.insert_soft_body( + SoftBodyBuilder::sphere(Vector::new(0.0, 0.0, 0.0), 0.4, 1) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.1) + .surface_collider(ColliderBuilder::ball(0.05).friction(0.0)) + .pinned_particles([0, 3, 11]), + ); + let rope = world.insert_soft_body( + SoftBodyBuilder::rope( + Vector::new(0.0, -1.0, 0.02), + Vector::new(0.0, 1.0, 0.02), + 16, + ) + .particle_mass(0.02) + .particle_radius(0.03) + .surface_collider(ColliderBuilder::ball(0.03).friction(0.0)), + ); + for _ in 0..400 { + world.step(); + } + let com = world.soft_bodies[rope].center_of_mass(); + let inside = (0..16) + .filter(|&i| world.soft_bodies[rope].particle_position(i).length() < 0.4) + .count(); + assert_eq!(inside, 0, "the rope still threads the ball"); + assert!(com.y < -1.0, "the rope hangs frozen at {:+.3}", com.y); +} + +/// Crossing repulsion clears a static crossing nothing else resolves: an unstrained horizontal +/// strip whose free end pokes through a vertical strip. With the crossing constraints dropped +/// nothing moves; with repulsion the end vertices are pushed back to their neighbors' side. +#[test] +fn crossing_repulsion_clears_static_poke() { + for repulsion in [true, false] { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world + .integration_parameters + .soft_bodies + .recovery + .crossing_repulsion = repulsion; + // Horizontal, pinned at its left end, its right end (x = 1.2) 0.05 past the vertical + // strip's plane (x = 1.15). + let horizontal = world.insert_soft_body( + strip( + Vector::new(-1.0, 0.0, -0.1), + Vector::new(0.2, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.2), + 0.02, + ) + .pinned_particles([0, 1]), + ); + let vertical = world.insert_soft_body( + strip( + Vector::new(1.15, -1.1, -0.1), + Vector::new(0.0, 0.2, 0.0), + Vector::new(0.0, 0.0, 0.2), + 0.02, + ) + .pinned_particles([0, 1, 22, 23]), + ); + let start = pair_overlap(&world, horizontal, vertical).0; + assert!(start > 0, "the strips were not authored crossed"); + for _ in 0..120 { + world.step(); + } + let end = pair_overlap(&world, horizontal, vertical).0; + if repulsion { + assert_eq!(end, 0, "crossing repulsion did not clear the poke"); + } else { + assert!( + end > 0, + "the dropped constraints resolved the poke (the scene proves nothing)" + ); + } + } +} + +/// Two closed cell-less balloons inserted overlapping de-overlap through the intersection-volume +/// constraints, each retracting its own intruding patch. +#[test] +fn overlapping_balloons_deoverlap() { + let mut world = PhysicsWorld::new(); + floor(&mut world, 6.0); + let a = world.insert_soft_body(balloon(Vector::new(-0.35, 0.55, 0.0), 0.5)); + let b = world.insert_soft_body(balloon(Vector::new(0.35, 0.55, 0.0), 0.5)); + for _ in 0..900 { + world.step(); + } + assert_eq!( + pair_overlap(&world, a, b), + (0, 0, 0), + "the balloons still overlap" + ); +} diff --git a/crates/rapier3d/tests/soft_stack_probe3.rs b/crates/rapier3d/tests/soft_stack_probe3.rs deleted file mode 100644 index a77a06d00..000000000 --- a/crates/rapier3d/tests/soft_stack_probe3.rs +++ /dev/null @@ -1,445 +0,0 @@ -//! 3D stack scenes for the soft-body depenetration work (metrics in the 2D `soft_stack_probe`): -//! balloon and jelly-cube columns under a heavy weight, and a cube pile pressed by a kinematic -//! plate. `SOFT_STACK_PRESET` selects the recovery preset (`defaults`, `minimal`). - -use rapier3d::parry::query::PointQuery; -use rapier3d::prelude::*; - -/// Whether the segment `[p, q]` crosses the triangle's interior transversally. -fn segment_crosses_triangle(p: Vector, q: Vector, tri: &[Vector; 3]) -> bool { - let n = (tri[1] - tri[0]).cross(tri[2] - tri[0]); - let dp = (p - tri[0]).dot(n); - let dq = (q - tri[0]).dot(n); - if dp * dq >= 0.0 { - return false; - } - let pq = q - p; - let d = [ - pq.dot((tri[0] - p).cross(tri[1] - p)), - pq.dot((tri[1] - p).cross(tri[2] - p)), - pq.dot((tri[2] - p).cross(tri[0] - p)), - ]; - d.iter().all(|x| *x >= 0.0) || d.iter().all(|x| *x <= 0.0) -} - -/// Whether two triangles cross (an edge of one pierces the other). -fn triangles_cross(a: &[Vector; 3], b: &[Vector; 3]) -> bool { - (0..3).any(|k| segment_crosses_triangle(a[k], a[(k + 1) % 3], b)) - || (0..3).any(|k| segment_crosses_triangle(b[k], b[(k + 1) % 3], a)) -} - -/// Ray-parity containment of `point` in a closed triangle mesh. -fn inside(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> bool { - let q = point + Vector::new(0.5341, 0.6432, 0.5487) * 100.0; - let mut crossings = 0; - for e in mesh.indices() { - let tri: [Vector; 3] = core::array::from_fn(|k| mesh.vertex(body, e[k] as usize)); - if segment_crosses_triangle(point, q, &tri) { - crossings += 1; - } - } - crossings % 2 == 1 -} - -/// Distance from `point` to the closest triangle of the mesh. -fn surface_distance(point: Vector, mesh: &SoftCollisionMesh, body: &SoftBody) -> Real { - let mut best: Real = Real::MAX; - for e in mesh.indices() { - let tri = rapier3d::parry::shape::Triangle::new( - mesh.vertex(body, e[0] as usize), - mesh.vertex(body, e[1] as usize), - mesh.vertex(body, e[2] as usize), - ); - best = best.min(tri.distance_to_local_point(point, true)); - } - best -} - -#[derive(Clone, Copy, Debug, Default)] -struct Metrics { - self_crossings: usize, - cross_crossings: usize, - inside: usize, - max_depth: Real, - min_cell_ratio: Real, - inverted_cells: usize, -} - -fn measure(world: &PhysicsWorld, handles: &[SoftBodyHandle]) -> Metrics { - let mut m = Metrics { - min_cell_ratio: 1.0, - ..Default::default() - }; - let bodies: Vec<(&SoftBody, &SoftCollisionMesh)> = handles - .iter() - .map(|&h| { - let sb = &world.soft_bodies[h]; - (sb, sb.meshes().next().unwrap()) - }) - .collect(); - let tris = |sb: &SoftBody, mesh: &SoftCollisionMesh| -> Vec<[Vector; 3]> { - mesh.indices() - .iter() - .map(|e| core::array::from_fn(|k| mesh.vertex(sb, e[k] as usize))) - .collect() - }; - let all: Vec> = bodies.iter().map(|(sb, mesh)| tris(sb, mesh)).collect(); - for (bi, (sb, mesh)) in bodies.iter().enumerate() { - let idx = mesh.indices(); - let t = &all[bi]; - for i in 0..idx.len() { - for j in i + 1..idx.len() { - if idx[i].iter().any(|v| idx[j].contains(v)) { - continue; - } - if triangles_cross(&t[i], &t[j]) { - m.self_crossings += 1; - } - } - } - for cell in sb.cells() { - let x: [Vector; 4] = - core::array::from_fn(|k| sb.particle_position(cell.vertices[k] as usize)); - let vol = (x[1] - x[0]).cross(x[2] - x[0]).dot(x[3] - x[0]) / 6.0; - let ratio = vol / cell.rest_volume; - m.min_cell_ratio = m.min_cell_ratio.min(ratio); - if ratio <= 0.0 { - m.inverted_cells += 1; - } - } - } - for a in 0..bodies.len() { - for b in 0..bodies.len() { - if a == b { - continue; - } - let (sa, ma) = bodies[a]; - let (sb, mb) = bodies[b]; - if a < b { - for ta in &all[a] { - for tb in &all[b] { - if triangles_cross(ta, tb) { - m.cross_crossings += 1; - } - } - } - } - if !mb.is_closed() { - continue; - } - for v in 0..ma.vertex_count() { - let x = ma.vertex(sa, v); - if inside(x, mb, sb) { - m.inside += 1; - m.max_depth = m.max_depth.max(surface_distance(x, mb, sb)); - } - } - } - } - m -} - -fn apply_preset(world: &mut PhysicsWorld) -> String { - let preset = std::env::var("SOFT_STACK_PRESET").unwrap_or_else(|_| "defaults".into()); - let r = &mut world.integration_parameters.soft_bodies.recovery; - *r = Default::default(); - // Per-feature policy of the volume-patch constraints (`SoftPatchConstraints`), on any preset: - // `SOFT_PATCH_CONSTRAINTS=stand|normal`. - match std::env::var("SOFT_PATCH_CONSTRAINTS").as_deref() { - Ok("stand") => r.overlap_patch_constraints = SoftPatchConstraints::StandDown, - Ok("normal") => r.overlap_patch_constraints = SoftPatchConstraints::AlongNormal, - _ => {} - } - match preset.as_str() { - "defaults" => {} - // The user's reference settings (2026-09-04): prevention without edge speculation, - // detection + stand-down, the intersection-volume constraints on. - "reference" => { - r.edge_speculation = false; - r.overlap_constraints = true; - } - // The reference settings with the crossing repulsion, unguided or guided by the - // pair's (and the fold's) volume normal (see `crossing_repulsion_guide`). - "reference+repel" | "reference+repel-guide" => { - r.edge_speculation = false; - r.overlap_constraints = true; - r.crossing_repulsion = true; - r.crossing_repulsion_guide = preset.ends_with("-guide"); - r.crossing_repulsion_self_guide = preset.ends_with("-guide"); - } - // The intersection-volume constraints on the defaults. - "overlap" => r.overlap_constraints = true, - other => panic!("unknown SOFT_STACK_PRESET `{other}`"), - } - preset -} - -fn floor(world: &mut PhysicsWorld, half: Real) { - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(0.0, -0.5, 0.0)), - ColliderBuilder::cuboid(half, 0.5, half), - ); -} - -fn balloon(center: Vector, radius: Real) -> SoftBodyBuilder { - SoftBodyBuilder::sphere(center, radius, 1) - .softness(SpringCoefficients::new(20.0, 1.0)) - .self_contacts(true) - .particle_mass(0.05) - .surface_collider(ColliderBuilder::ball(0.06).friction(0.6)) -} - -fn jelly(center: Vector, half: Real) -> SoftBodyBuilder { - SoftBodyBuilder::cuboid(center, Vector::splat(half), 3, 3, 3) - .cell_model(SoftBodyCellModel::Corotational) - .material(SoftBodyMaterial { - young_modulus: 3.0e3, - poisson_ratio: 0.4, - elastic_damping_ratio: 0.5, - ..Default::default() - }) - .self_contacts(true) - .particle_mass(0.1) - .surface_collider(ColliderBuilder::ball(0.08).friction(0.7)) -} - -fn run( - name: &str, - world: &mut PhysicsWorld, - handles: &[SoftBodyHandle], - steps: usize, - every: usize, - mut hook: impl FnMut(&mut PhysicsWorld, usize), -) -> Metrics { - let preset = apply_preset(world); - let mut worst = Metrics::default(); - let mut stepping = std::time::Duration::ZERO; - for step in 0..=steps { - if step % every == 0 { - let m = measure(world, handles); - println!( - "{name} [{preset}] step {step:4}: self={} cross={} inside={} depth={:.3} \ - min_cell={:+.3} inverted={}", - m.self_crossings, - m.cross_crossings, - m.inside, - m.max_depth, - m.min_cell_ratio, - m.inverted_cells - ); - worst.cross_crossings = worst.cross_crossings.max(m.cross_crossings); - worst.inside = worst.inside.max(m.inside); - worst.max_depth = worst.max_depth.max(m.max_depth); - worst.min_cell_ratio = worst.min_cell_ratio.min(m.min_cell_ratio); - } - if step == steps { - break; - } - hook(world, step); - let start = std::time::Instant::now(); - world.step(); - stepping += start.elapsed(); - } - let ms = stepping.as_secs_f64() * 1000.0 / steps as f64; - let last = measure(world, handles); - println!( - "{name} [{preset}] final: self={} cross={} inside={} depth={:.3} min_cell={:+.3} \ - inverted={} ({ms:.3} ms/step); worst: cross={} inside={} depth={:.3} min_cell={:+.3}", - last.self_crossings, - last.cross_crossings, - last.inside, - last.max_depth, - last.min_cell_ratio, - last.inverted_cells, - worst.cross_crossings, - worst.inside, - worst.max_depth, - worst.min_cell_ratio - ); - last -} - -/// A column of six balloons with a heavy box landing on top once it has settled. -#[test] -fn balloon_column() { - let mut world = PhysicsWorld::new(); - floor(&mut world, 4.0); - let handles: Vec<_> = (0..6) - .map(|i| { - let x = (i % 2) as Real * 0.05; - world.insert_soft_body(balloon(Vector::new(x, 0.55 + i as Real * 1.05, 0.0), 0.5)) - }) - .collect(); - let m = run( - "balloon_column", - &mut world, - &handles, - 900, - 100, - |world, step| { - if step == 300 { - world.insert( - RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 8.0, 0.0)), - ColliderBuilder::cuboid(0.5, 0.3, 0.5).density(10.0), - ); - } - }, - ); - assert_eq!( - m.cross_crossings, 0, - "balloons of the column penetrate each other" - ); - assert_eq!( - m.self_crossings, 0, - "a balloon of the column is self-crossed" - ); -} - -/// A column of five jelly cubes under the same weight: cells compress, none may invert. -#[test] -fn jelly_column() { - let mut world = PhysicsWorld::new(); - floor(&mut world, 4.0); - let handles: Vec<_> = (0..5) - .map(|i| { - let x = (i % 2) as Real * 0.05; - world.insert_soft_body(jelly(Vector::new(x, 0.55 + i as Real * 1.1, 0.0), 0.5)) - }) - .collect(); - let m = run( - "jelly_column", - &mut world, - &handles, - 900, - 100, - |world, step| { - if step == 300 { - world.insert( - RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 7.0, 0.0)), - ColliderBuilder::cuboid(0.5, 0.3, 0.5).density(10.0), - ); - } - }, - ); - assert_eq!( - m.cross_crossings, 0, - "cubes of the column penetrate each other" - ); - assert_eq!( - m.inverted_cells, 0, - "a cube of the column has inverted cells" - ); -} - -/// Two layers of jelly cubes in a bin, pressed by a kinematic plate to four fifths of -/// their height and held there. -#[test] -fn pressed_jelly_pile() { - let mut world = PhysicsWorld::new(); - floor(&mut world, 3.0); - for (x, z) in [(-2.6, 0.0), (2.6, 0.0), (0.0, -2.6), (0.0, 2.6)] { - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(x, 3.0, z)), - ColliderBuilder::cuboid( - if z == 0.0 { 0.3 } else { 2.9 }, - 3.5, - if z == 0.0 { 2.9 } else { 0.3 }, - ), - ); - } - let mut handles = Vec::new(); - for j in 0..2 { - for i in 0..2 { - for k in 0..2 { - let x = -0.6 + i as Real * 1.2 + (j % 2) as Real * 0.3; - let z = -0.6 + k as Real * 1.2 + (j % 2) as Real * 0.3; - let y = 0.55 + j as Real * 1.1; - handles.push(world.insert_soft_body(jelly(Vector::new(x, y, z), 0.5))); - } - } - } - let plate = world.insert_body( - RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 3.0, 0.0)), - ); - world.insert_collider(ColliderBuilder::cuboid(2.2, 0.25, 2.2), Some(plate)); - let m = run( - "pressed_jelly_pile", - &mut world, - &handles, - 1200, - 150, - |world, step| { - if step >= 300 { - let y = (3.0 - (step - 300) as Real * 0.5 / 60.0).max(1.95); - world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y, 0.0)); - } - }, - ); - assert_eq!( - m.cross_crossings, 0, - "cubes of the pressed pile penetrate each other" - ); - assert_eq!( - m.inverted_cells, 0, - "a cube of the pressed pile has inverted cells" - ); -} - -/// The 3D trigger scene: the same pile crushed to two thirds of its height. Ignored until a -/// phase passes it; see the 2D `crushed_jelly_pile`. -#[test] -#[ignore] -fn crushed_jelly_pile() { - let mut world = PhysicsWorld::new(); - floor(&mut world, 3.0); - for (x, z) in [(-2.6, 0.0), (2.6, 0.0), (0.0, -2.6), (0.0, 2.6)] { - world.insert( - RigidBodyBuilder::fixed().translation(Vector::new(x, 3.0, z)), - ColliderBuilder::cuboid( - if z == 0.0 { 0.3 } else { 2.9 }, - 3.5, - if z == 0.0 { 2.9 } else { 0.3 }, - ), - ); - } - let mut handles = Vec::new(); - for j in 0..2 { - for i in 0..2 { - for k in 0..2 { - let x = -0.6 + i as Real * 1.2 + (j % 2) as Real * 0.3; - let z = -0.6 + k as Real * 1.2 + (j % 2) as Real * 0.3; - let y = 0.55 + j as Real * 1.1; - handles.push(world.insert_soft_body(jelly(Vector::new(x, y, z), 0.5))); - } - } - } - let plate = world.insert_body( - RigidBodyBuilder::kinematic_position_based().translation(Vector::new(0.0, 3.0, 0.0)), - ); - world.insert_collider(ColliderBuilder::cuboid(2.2, 0.25, 2.2), Some(plate)); - let m = run( - "crushed_jelly_pile", - &mut world, - &handles, - 1200, - 150, - |world, step| { - if step >= 300 { - let y = (3.0 - (step - 300) as Real * 0.5 / 60.0).max(1.6); - world.bodies[plate].set_next_kinematic_translation(Vector::new(0.0, y, 0.0)); - } - }, - ); - assert_eq!( - m.cross_crossings, 0, - "cubes of the crushed pile penetrate each other" - ); - assert_eq!( - m.self_crossings, 0, - "a cube of the crushed pile is self-crossed" - ); - assert_eq!( - m.inverted_cells, 0, - "a cube of the crushed pile has inverted cells" - ); -} diff --git a/python/rapier-py-3d/src/controllers.rs b/python/rapier-py-3d/src/controllers.rs index cb2e70da6..3033046a8 100644 --- a/python/rapier-py-3d/src/controllers.rs +++ b/python/rapier-py-3d/src/controllers.rs @@ -349,6 +349,8 @@ impl CharacterCollision { translation_remaining: tr, hit: rapier::parry::query::ShapeCastHit { time_of_impact: self.toi.time_of_impact, + subshape1: 0, + subshape2: 0, witness1: self.toi.witness1.0.coords.into(), witness2: self.toi.witness2.0.coords.into(), normal1: crate::na::Unit::new_unchecked(self.toi.normal1.0) diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 32676dee6..f89daf01b 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -300,10 +300,10 @@ impl CCDSolver { let dispatcher = narrow_phase.query_dispatcher(); let intersect_before = dispatcher .intersection_test(&prev_pos12, co1.shape.as_ref(), co2.shape.as_ref()) - .unwrap_or(false); + .is_ok_and(|hit| hit.intersecting); let intersect_after = dispatcher .intersection_test(&next_pos12, co1.shape.as_ref(), co2.shape.as_ref()) - .unwrap_or(false); + .is_ok_and(|hit| hit.intersecting); if !intersect_before && !intersect_after diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs index 2ab30ead7..272223b07 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs @@ -234,7 +234,8 @@ impl SoftCollisionMesh { } /// Moves the vertices of this mesh's collider shape to the current positions, in place: - /// parry refits the shape's BVH. + /// parry refits the shape's BVH. A closed 2D polyline is one-sided only while its winding + /// can be trusted (see [`Self::winding_trustworthy`]). pub(crate) fn deform_shape( &self, body: &SoftBody, @@ -249,6 +250,16 @@ impl SoftCollisionMesh { }; // A polyline in either dimension (a 2D surface, or a wire in 3D), a trimesh in 3D. if let Some(polyline) = shape.as_polyline_mut() { + #[cfg(feature = "dim2")] + { + use parry::shape::PolylineFlags; + let oriented = self.closed && self.winding_trustworthy(body); + let mut flags = polyline.flags(); + if flags.contains(PolylineFlags::ORIENTED) != oriented { + flags.set(PolylineFlags::ORIENTED, oriented); + polyline.set_flags(flags); + } + } polyline.update_vertices(write); return; } @@ -276,6 +287,19 @@ impl SoftCollisionMesh { } } + /// Whether every element's winding says where the outside is: not for a mesh turned inside + /// out or hovering around a zero volume, nor while a cell backing an element is inverted. + /// The rigid contacts of a closed mesh are one-sided only then; otherwise they are two-sided + /// and the contact assembly orients the reversed ones (see `element_outward_normal`). + #[cfg(feature = "dim2")] + pub(crate) fn winding_trustworthy(&self, body: &SoftBody) -> bool { + if self.inverted || self.orientation_unreliable { + return false; + } + body.cells.is_empty() + || !(0..self.indices.len()).any(|e| self.element_cell_inverted(body, e)) + } + /// Updates the inside-out state of a closed mesh from the current vertex positions. pub(crate) fn update_orientation(&mut self, body: &SoftBody) { if !self.closed || self.rest_signed_volume == 0.0 { diff --git a/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs b/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs index ecb8357cb..d54315871 100644 --- a/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs @@ -108,7 +108,7 @@ impl SoftBodySet { ) -> Option<(SoftClusterRemoval, RigidBodyHandle)> { // First pass, on the soft body alone: tombstone the cluster and remove the particles // only it covered (with their elements and attachments). - let (mut counts, remap, dead, proxy, root, last) = { + let (mut counts, proxy, root, last) = { let sb = self.bodies.get_mut(handle.0)?; let c = sb.clusters.get_mut(cluster as usize)?; if !c.is_live() { @@ -126,7 +126,7 @@ impl SoftBodySet { } } } - let (counts, remap) = sb.remove_dead_particles(&dead); + let (counts, _) = sb.remove_dead_particles(&dead); // The root body follows the first live cluster when cluster 0 goes. if sb.root_body == proxy { @@ -138,7 +138,7 @@ impl SoftBodySet { sb.root_body = next_root; } let last = sb.num_live_clusters() == 0; - (counts, remap, dead, proxy, sb.root_body, last) + (counts, proxy, sb.root_body, last) }; counts.soft_body_removed = last; diff --git a/src/dynamics/soft_body/soft_body_settings.rs b/src/dynamics/soft_body/soft_body_settings.rs index b76634486..b394efae4 100644 --- a/src/dynamics/soft_body/soft_body_settings.rs +++ b/src/dynamics/soft_body/soft_body_settings.rs @@ -57,7 +57,7 @@ impl Default for SoftBodiesSettings { pub struct SoftFemParameters { /// Relative residual at which the conjugate gradient stops (default: `1.0e-5`). pub linear_tolerance: Real, - /// Hard cap on the conjugate-gradient iterations, whatever the residual (default: `256`). + /// Hard cap on the conjugate-gradient iterations, whatever the residual (default: `20`). /// /// A very stiff, finely meshed body can need several hundred iterations to converge; the /// truncated step it gets instead is under-relaxed (safe), only slower to settle. diff --git a/src/dynamics/soft_body/soft_recovery_settings.rs b/src/dynamics/soft_body/soft_recovery_settings.rs index 223011097..ca7b61ff3 100644 --- a/src/dynamics/soft_body/soft_recovery_settings.rs +++ b/src/dynamics/soft_body/soft_recovery_settings.rs @@ -39,8 +39,8 @@ pub struct SoftRecoverySettings { /// Detect inverted cells (material locally inside out) each step; feeds the self stand-down. /// (default: `true`) pub inverted_cell_detection: bool, - /// Detect surface self-crossings each step (the BVTT sweep); feeds the self stand-down and the - /// self untangler. (default: `true`) + /// Detect surface self-crossings each step (the BVTT sweep); feeds the self stand-down. + /// (default: `true`) pub self_crossing_detection: bool, /// Skip the self-crossing sweep while the surface's accumulated travel could not have bridged /// half a skin (crossings only form through motion). Off: sweep every step. (default: `true`) @@ -106,7 +106,7 @@ pub struct SoftRecoverySettings { pub overlap_patch_constraints: SoftPatchConstraints, /// Measure the intersection volume on the contact skins (surfaces dilated by their skin), not /// the geometric surfaces: a resting pair then has a hard volume constraint with speculative - /// slack before the skins meet. Implied by [`SoftContactModel::VolumeOnly`]. (default: `false`) + /// slack before the skins meet. (default: `false`) pub overlap_skin_volume: bool, /// The skin overlap kept at rest, as a fraction of the pair's skins (the paper's maintained /// interpenetration layer): the correction removes only the volume beyond it, holding the diff --git a/src/dynamics/soft_body/tearing/tearing_particle_split.rs b/src/dynamics/soft_body/tearing/tearing_particle_split.rs index f3469169b..7fde623d0 100644 --- a/src/dynamics/soft_body/tearing/tearing_particle_split.rs +++ b/src/dynamics/soft_body/tearing/tearing_particle_split.rs @@ -10,7 +10,7 @@ use parry::utils::hashmap::HashMap; use simba::scalar::{ComplexField as _, RealField as _}; use super::super::soft_body_builder::cell_faces; -use super::super::{SoftBody, SoftBodyEdgeKind, SoftBodyMaterial, SoftBodyParticle}; +use super::super::{SoftBody, SoftBodyEdgeKind, SoftBodyParticle}; use super::tearing::shared_others; /// The elements carrying a body's rest measure (see [`SoftBody::rest_measure`]). diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs index d55759137..afa2d236e 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs @@ -158,7 +158,7 @@ mod tests { fn inverted_cells_do_not_flow() { let (mut cell, rest0) = unit_cell(); let before = cell; - // A reflection about an oblique plane: no diagonal strain reaches the old `-0.9` guard. + // A reflection about an oblique plane: no diagonal strain reveals the inversion. let mut strain = StrainVector::zeros(); for k in 0..DIM { strain[k] = -2.0 / DIM as Real; diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs index d7854eb9b..2f357fa2b 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs @@ -25,7 +25,7 @@ use crate::dynamics::{ RigidBodyHandle, RigidBodySet, SoftBody, SoftBodyCellModel, SoftBodyEdgeKind, SoftBodyHandle, SoftBodySet, SpringCoefficients, }; -use crate::math::{AngVector, DIM, Matrix, Real, Vector}; +use crate::math::{DIM, Matrix, Real, Vector}; use crate::utils::RotationOps; use na::SimdRealField; use super::soft_constraints_set::AwakeCluster; diff --git a/src/dynamics/solver/soft_fem/soft_fem_sparse.rs b/src/dynamics/solver/soft_fem/soft_fem_sparse.rs index b48bd62b9..f19c9f234 100644 --- a/src/dynamics/solver/soft_fem/soft_fem_sparse.rs +++ b/src/dynamics/solver/soft_fem/soft_fem_sparse.rs @@ -306,10 +306,9 @@ mod tests { use super::*; use crate::dynamics::solver::soft_constraint::soft_element_constraint::outer_product; - /// A small SPD block system solved by the conjugate gradient must match a dense reference - /// solve. + /// A small SPD block system solved by the conjugate gradient leaves a negligible residual. #[test] - fn conjugate_gradient_matches_dense_solve() { + fn conjugate_gradient_solves_the_system() { let n = 12; // A chain: every particle couples with its two neighbours. let pairs: Vec<(u32, u32)> = (0..n as u32 - 1).map(|i| (i, i + 1)).collect(); @@ -347,7 +346,6 @@ mod tests { cg.update_preconditioner(&matrix); cg.solve(&matrix, &rhs, &mut x, 1.0e-10, 1000); - // Residual check (the dense reference here is the matrix itself). let mut ax = vec![Vector::ZERO; n]; matrix.mul(&x, &mut ax); for k in 0..n { diff --git a/src/geometry/narrow_phase/intersections.rs b/src/geometry/narrow_phase/intersections.rs index 94c7d04f5..eb7ec59d8 100644 --- a/src/geometry/narrow_phase/intersections.rs +++ b/src/geometry/narrow_phase/intersections.rs @@ -128,7 +128,7 @@ impl NarrowPhase { let pos12 = co1.pos.inv_mul(&co2.pos); edge.weight.intersecting = query_dispatcher .intersection_test(&pos12, &*co1.shape, &*co2.shape) - .unwrap_or(false); + .is_ok_and(|hit| hit.intersecting); } let active_events = co1.flags.active_events | co2.flags.active_events; diff --git a/src/pipeline/physics_world.rs b/src/pipeline/physics_world.rs index 3f6110374..b335a2db8 100644 --- a/src/pipeline/physics_world.rs +++ b/src/pipeline/physics_world.rs @@ -712,7 +712,10 @@ impl PhysicsWorld { let (co, co_handle) = colliders.get_unknown_gen(leaf)?; if filter.test(bodies, co_handle, co) { let pos12 = shape_pos.inv_mul(co.position()); - if dispatcher.intersection_test(&pos12, shape, co.shape()) == Ok(true) { + if dispatcher + .intersection_test(&pos12, shape, co.shape()) + .is_ok_and(|hit| hit.intersecting) + { return Some((co_handle, co)); } } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index d752617bb..5c7270793 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -171,6 +171,11 @@ impl<'a> QueryPipeline<'a> { self.colliders.get_unknown_gen(id).map(|(_, h)| (h, data)) } + /// The handle of the collider parry reported as the part of the collider set that answered. + fn handle_of(&self, id: u32) -> Option { + self.colliders.get_unknown_gen(id).map(|(_, h)| h) + } + /// Replaces [`Self::filter`] with different filtering rules. pub fn with_filter(self, filter: QueryFilter<'a>) -> Self { Self { filter, ..self } @@ -254,9 +259,9 @@ impl<'a> QueryPipeline<'a> { max_toi: Real, solid: bool, ) -> Option<(ColliderHandle, RayIntersection)> { - CompositeShapeRef(self) - .cast_local_ray_and_get_normal(ray, max_toi, solid) - .and_then(|hit| self.id_to_handle(hit)) + let (id, hit) = + CompositeShapeRef(self).cast_local_ray_and_get_normal(ray, max_toi, solid)?; + Some((self.handle_of(id)?, hit)) } /// Returns ALL colliders that a ray passes through (not just the first). @@ -349,7 +354,8 @@ impl<'a> QueryPipeline<'a> { max_dist: Real, solid: bool, ) -> Option<(ColliderHandle, PointProjection)> { - self.id_to_handle(CompositeShapeRef(self).project_local_point(point, max_dist, solid)?) + let (id, proj) = CompositeShapeRef(self).project_local_point(point, max_dist, solid)?; + Some((self.handle_of(id)?, proj)) } /// Returns ALL colliders that contain the given point. @@ -406,10 +412,9 @@ impl<'a> QueryPipeline<'a> { point: Vector, max_dist: Real, ) -> Option<(ColliderHandle, PointProjection, FeatureId)> { - let (id, (proj, feat)) = + let (id, proj, feat) = CompositeShapeRef(self).project_local_point_and_get_feature(point, max_dist)?; - let handle = self.colliders.get_unknown_gen(id)?.1; - Some((handle, proj, feat)) + Some((self.handle_of(id)?, proj, feat)) } /// Finds all handles of all the colliders with an [`Aabb`] intersecting the given [`Aabb`]. @@ -488,9 +493,14 @@ impl<'a> QueryPipeline<'a> { shape: &dyn Shape, options: ShapeCastOptions, ) -> Option<(ColliderHandle, ShapeCastHit)> { - CompositeShapeRef(self) - .cast_shape(self.dispatcher, shape_pos, shape_vel, shape, options) - .and_then(|hit| self.id_to_handle(hit)) + let (id, hit) = CompositeShapeRef(self).cast_shape( + self.dispatcher, + shape_pos, + shape_vel, + shape, + options, + )?; + Some((self.handle_of(id)?, hit)) } /// Casts a shape with an arbitrary continuous motion and retrieve the first collider it hits. @@ -520,17 +530,16 @@ impl<'a> QueryPipeline<'a> { end_time: Real, stop_at_penetration: bool, ) -> Option<(ColliderHandle, ShapeCastHit)> { - CompositeShapeRef(self) - .cast_shape_nonlinear( - self.dispatcher, - &NonlinearRigidMotion::identity(), - shape_motion, - shape, - start_time, - end_time, - stop_at_penetration, - ) - .and_then(|hit| self.id_to_handle(hit)) + let (id, hit) = CompositeShapeRef(self).cast_shape_nonlinear( + self.dispatcher, + &NonlinearRigidMotion::identity(), + shape_motion, + shape, + start_time, + end_time, + stop_at_penetration, + )?; + Some((self.handle_of(id)?, hit)) } /// Retrieve all the colliders intersecting the given shape. @@ -552,7 +561,11 @@ impl<'a> QueryPipeline<'a> { let (co, co_handle) = self.colliders.get_unknown_gen(leaf)?; if self.filter.test(self.bodies, co_handle, co) { let pos12 = shape_pos.inv_mul(co.position()); - if self.dispatcher.intersection_test(&pos12, shape, co.shape()) == Ok(true) { + if self + .dispatcher + .intersection_test(&pos12, shape, co.shape()) + .is_ok_and(|hit| hit.intersecting) + { return Some((co_handle, co)); } } diff --git a/typescript/Cargo.toml b/typescript/Cargo.toml index 349abf9b3..70f4fde22 100644 --- a/typescript/Cargo.toml +++ b/typescript/Cargo.toml @@ -16,15 +16,15 @@ strip = true # Workaround for https://github.com/bevyengine/bevy/issues/16030 (t # must stay semver-compatible with the in-repo crate version for this to apply. rapier2d = { path = "../crates/rapier2d" } rapier3d = { path = "../crates/rapier3d" } -# Temporary: the deformable-mesh API (parry branch `soft-bodies`), to be replaced by a -# version bump when parry is released. Mirrors the patch of the root workspace. -parry2d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } -parry3d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } -# parry, nalgebra and simba are still pulled from crates.io, matching the -# versions the in-repo rapier crates depend on. Uncomment to build against local -# checkouts placed next to this repository during development. -#parry2d = { path = "../../parry/crates/parry2d" } -#parry3d = { path = "../../parry/crates/parry3d" } +# Temporary: the composite-shape queries returning the part id alongside the result (parry +# branch `composite-subshape-ids`), to be replaced by a version bump when parry is released. +# Mirrors the patch of the root workspace. +parry2d = { path = "../../parry/crates/parry2d" } +parry3d = { path = "../../parry/crates/parry3d" } + +# nalgebra and simba are still pulled from crates.io, matching the versions the +# in-repo rapier crates depend on. Uncomment to build against local checkouts +# placed next to this repository during development. #nalgebra = { path = "../../nalgebra" } #simba = { path = "../../simba" } diff --git a/typescript/builds/prepare_builds/templates/Cargo.toml.tera b/typescript/builds/prepare_builds/templates/Cargo.toml.tera index 68bd7273b..d6fd9db9c 100644 --- a/typescript/builds/prepare_builds/templates/Cargo.toml.tera +++ b/typescript/builds/prepare_builds/templates/Cargo.toml.tera @@ -40,7 +40,7 @@ rapier{{ dimension }}d = { version = "0.35.0", features = [ ] } # The explicit dependency to parry only needed to pin the version; it must match # the version (and math-backend feature set) the in-repo rapier crate depends on. -parry{{ dimension }}d = { version = "0.30", default-features = false, features = [ +parry{{ dimension }}d = { version = "0.31", default-features = false, features = [ "required-features", ] } ref-cast = "1" diff --git a/typescript/src/control/character_controller.rs b/typescript/src/control/character_controller.rs index 8456624ea..f7d93dca4 100644 --- a/typescript/src/control/character_controller.rs +++ b/typescript/src/control/character_controller.rs @@ -256,6 +256,8 @@ impl RawCharacterCollision { translation_applied: Vector::ZERO, translation_remaining: Vector::ZERO, hit: ShapeCastHit { + subshape1: 0, + subshape2: 0, time_of_impact: 0.0, witness1: Vector::ZERO, witness2: Vector::ZERO, diff --git a/typescript/src/geometry/shape.rs b/typescript/src/geometry/shape.rs index 56aa44cf3..7f5f1d1b4 100644 --- a/typescript/src/geometry/shape.rs +++ b/typescript/src/geometry/shape.rs @@ -104,7 +104,8 @@ impl SharedShapeUtility for SharedShape { } fn intersectsShape(&self, shapePos1: &Pose, shape2: &dyn Shape, shapePos2: &Pose) -> bool { - query::intersection_test(shapePos1, &*self.0, shapePos2, shape2).unwrap_or(false) + query::intersection_test(shapePos1, &*self.0, shapePos2, shape2) + .is_ok_and(|hit| hit.intersecting) } fn contactShape( diff --git a/website/docs-examples/Cargo.toml b/website/docs-examples/Cargo.toml index 7e8b128a9..eb98b34cd 100644 --- a/website/docs-examples/Cargo.toml +++ b/website/docs-examples/Cargo.toml @@ -3,7 +3,8 @@ members = ["2d/bevy", "2d/rust", "3d/bevy", "3d/rust", "inject_file"] resolver = "2" [patch.crates-io] -# Temporary: the deformable-mesh API (parry branch `soft-bodies`), to be replaced by a -# version bump when parry is released. Mirrors the patch of the root workspace. -parry2d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } -parry3d = { git = "https://github.com/dimforge/parry", branch = "soft-bodies" } +# Temporary: the composite-shape queries returning the part id alongside the result (parry +# branch `composite-subshape-ids`), to be replaced by a version bump when parry is released. +# Mirrors the patch of the root workspace. +parry2d = { path = "../../../parry/crates/parry2d" } +parry3d = { path = "../../../parry/crates/parry3d" } From 6ed93a3880160c21a7acdacefdb202dd1adc2fea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 18 Sep 2026 21:52:50 +0200 Subject: [PATCH 29/32] feat: improve handling of the ORIENTED flag and oriented/shell soft-bodies --- crates/rapier2d/tests/soft_bodies.rs | 177 +++++++++--------- crates/rapier3d/tests/soft_bodies.rs | 101 ++++++++++ .../python/rapier3d/_rapier3d.pyi | 2 + python/rapier-py-3d/src/soft_body.rs | 13 ++ .../collision_mesh/collision_mesh.rs | 7 +- .../collision_mesh_accessors.rs | 30 ++- .../collision_mesh/collision_mesh_geometry.rs | 6 +- .../collision_mesh/collision_mesh_topology.rs | 29 +-- .../soft_body_builder/soft_body_build.rs | 12 +- .../soft_body_builder/soft_body_builder.rs | 3 + .../soft_body_builder_settings.rs | 11 ++ .../soft_body_builder_shapes.rs | 3 +- .../soft_body_set/soft_body_set_proxies.rs | 63 ++++--- .../soft_body_set/soft_body_set_step_sync.rs | 13 +- .../soft_body/soft_recovery_settings.rs | 10 +- .../soft_contact_assembly_body_contacts.rs | 14 +- .../soft_contact_assembly_vertex_contacts.rs | 8 +- src/geometry/collider_set.rs | 12 +- .../soft_contacts/soft_contacts_pairs.rs | 4 +- .../soft_contacts/soft_contacts_types.rs | 4 + .../soft_contacts_vertex_pass.rs | 14 +- .../soft_contacts/soft_contacts_volume.rs | 7 +- .../soft_contacts/soft_self_contacts.rs | 2 +- typescript/CHANGELOG.md | 4 + typescript/src.ts/dynamics/soft_body.ts | 62 ++++-- typescript/src/dynamics/soft_body.rs | 8 + 26 files changed, 425 insertions(+), 194 deletions(-) diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index 85aaf434f..f10b80aee 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -346,6 +346,13 @@ fn stiff_neo_hookean_square_is_stable() { #[test] fn neo_hookean_square_survives_large_compression() { let mut world = world_with_ground(); + // The plate swallows the square's top vertices once it is squashed, which makes a volume + // patch: the contacts must keep holding it (not bent along the patch normal), and the patch + // needs its per-cell multipliers to follow a kinematic press. + let recovery = &mut world.integration_parameters.soft_bodies.recovery; + recovery.overlap_patch_constraints = SoftPatchConstraints::Keep; + recovery.overlap_normal_push = false; + recovery.overlap_multi_volume = true; let square = SoftBodyBuilder::grid(Vector::new(0.0, 0.5), Vector::splat(0.5), 5, 5) .cell_model(SoftBodyCellModel::NeoHookean) .material(SoftBodyMaterial { @@ -502,9 +509,9 @@ fn neo_hookean_response_is_substep_invariant() { ); } -/// A closed surface expels bodies that cross it: a small rigid ball spawned straddling a soft -/// square's edge from inside (and a particle of a second soft body likewise) come out instead -/// of being trapped, and the square itself stays whole. +/// An oriented surface holds nothing inside: a small rigid ball straddling a soft square's +/// edge from inside and moving outward crosses it freely (and a particle of a second soft body +/// likewise comes out) instead of being trapped, and the square itself stays whole. #[test] fn closed_surface_expels_intruders() { let mut world = world_with_ground(); @@ -521,7 +528,9 @@ fn closed_surface_expels_intruders() { .particle_radius(0.05); let square = world.insert_soft_body(square); let (ball, _) = world.insert( - RigidBodyBuilder::dynamic().translation(Vector::new(0.1, 3.68)), + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.1, 3.68)) + .linvel(Vector::new(0.0, 1.0)), ColliderBuilder::ball(0.1), ); let intruder = SoftBodyBuilder::new(vec![ @@ -544,8 +553,8 @@ fn closed_surface_expels_intruders() { }; let ball_pos = world.bodies[ball].translation(); assert!( - !inside(ball_pos), - "rigid ball still inside the square: {ball_pos:?}" + !inside(ball_pos) && ball_pos.y > 4.0, + "the rigid ball was held inside the square: {ball_pos:?}" ); for p in world.soft_bodies[intruder].particles() { assert!( @@ -986,100 +995,84 @@ fn torn_cell_recovers_without_snapping() { assert!(max_speed < 60.0, "recovery snapped at {max_speed} m/s"); } -/// A closed surface only expels intruders through elements whose winding can be trusted: an -/// element belonging to an inverted cell has a mirrored winding, and a ball resting on it must -/// not be "expelled" through the body. +/// An unoriented blob is a shell: a rigid ball spawned straddling its wall from inside is pushed back +/// in and rests on its inner wall, while an oriented (solid) blob expels it. #[test] -fn inverted_cell_elements_do_not_expel() { - let mut world = world_with_ground(); - let square = SoftBodyBuilder::grid(Vector::new(0.0, 0.75), Vector::splat(0.75), 4, 4) - .cell_model(SoftBodyCellModel::Corotational) - .material(SoftBodyMaterial { - young_modulus: 1.0e4, - poisson_ratio: 0.4, - ..Default::default() - }) - .particle_mass(0.1) - .particle_radius(0.05); - let handle = world.insert_soft_body(square); - // Pin the top edge, its two middle particles swapped and raised: the segment between them - // runs backwards (its cell is inverted), so its winding normal points into the square, and - // it stands alone above the rest of the top edge. - let sb = &world.soft_bodies[handle]; - let n = sb.num_particles(); - let mut top: Vec = (0..n) - .filter(|&i| sb.particle_position(i).y > 1.49) - .collect(); - top.sort_by(|&a, &b| { - sb.particle_position(a) - .x - .partial_cmp(&sb.particle_position(b).x) - .unwrap() - }); - let (a, b) = (top[1], top[2]); - for &i in &top { - world.soft_bodies[handle].set_particle_pinned(i, true); - } - for (i, x) in [(a, 0.6), (b, -0.6)] { - world.soft_bodies[handle].set_particle_position(i, Vector::new(x, 1.9)); - } - // A ball resting on that segment only. - let (ball, _) = world.insert( - RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 1.9 + 0.05 + 0.15)), - ColliderBuilder::ball(0.15), +fn unoriented_blob_holds_a_ball_inside() { + let run = |oriented: bool| -> (Vector, Real) { + let mut world = world_with_ground(); + let blob = SoftBodyBuilder::disk(Vector::new(0.0, 1.0), 0.8, 32) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.05) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05)) + .oriented(oriented); + let handle = world.insert_soft_body(blob); + // Straddling the wall on the right, its center inside. + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.75, 1.0)), + ColliderBuilder::ball(0.1), + ); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let lowest = sb.particle_positions().map(|p| p.y).fold(Real::MAX, Real::min); + (world.bodies[ball].translation() - sb.center_of_mass(), lowest) + }; + let (offset, lowest) = run(false); + assert!( + offset.length() < 0.8 && offset.y + 1.0 > lowest + 0.1, + "the ball left the unoriented blob: offset {offset:?}, lowest particle {lowest}" ); - for _ in 0..120 { - world.step(); - } - assert_finite(&world, handle); - // Resting on the segment: 1.9 + skin 0.05 + radius 0.15 (a mirrored-winding expulsion - // pushed it down to 1.93). - let y = world.bodies[ball].translation().y; + let (offset, _) = run(true); assert!( - y > 2.05, - "the ball was pushed into the square through a mirrored element: y = {y}" + offset.length() > 0.8, + "the ball was not expelled from the oriented blob: offset {offset:?}" ); } -/// A pressurized body turned inside out is accepted as mirrored: its area target follows its -/// orientation and its outward normals flip with it, so it stays a blob and supports a box. +/// The builder orients a closed surface unless told otherwise, and after insertion the +/// collider's shape is the authority: clearing its `ORIENTED` flag makes the surface a shell. #[test] -fn inverted_blob_is_accepted_as_mirrored() { +fn the_collider_shape_decides_the_orientation() { + use rapier2d::parry::shape::PolylineFlags; let mut world = world_with_ground(); - let blob = SoftBodyBuilder::disk(Vector::new(0.0, 0.7), 0.6, 20) - .softness(SpringCoefficients::new(20.0, 1.0)) - .volume_factor(1.1) - .particle_mass(0.05) - .particle_radius(0.06); - let handle = world.insert_soft_body(blob); - // Mirror it (same shape, reversed winding). - { - let sb = &mut world.soft_bodies[handle]; - for i in 0..sb.num_particles() { - let p = sb.particle_position(i); - sb.set_particle_position(i, Vector::new(-p.x, p.y)); - } - } - let (block, _) = world.insert( - RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 1.8)), - ColliderBuilder::cuboid(0.3, 0.2).density(0.3), - ); - for _ in 0..240 { - world.step(); - } - assert_finite(&world, handle); - let sb = &world.soft_bodies[handle]; - let area = sb.volume(); - assert!( - (area.abs() - 1.1 * sb.rest_volume()).abs() < 0.15 * sb.rest_volume(), - "mirrored blob area {area} vs target {}", - 1.1 * sb.rest_volume() - ); - let y = world.bodies[block].translation().y; - assert!( - y > 1.0, - "the block did not rest on the mirrored blob: y = {y}" - ); + let blob = |center: Vector| { + SoftBodyBuilder::disk(center, 0.8, 32) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.05) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05)) + }; + let closed = world.insert_soft_body(blob(Vector::new(0.0, 1.0))); + let shell = world.insert_soft_body(blob(Vector::new(4.0, 1.0)).oriented(false)); + let rope = world.insert_soft_body(SoftBodyBuilder::rope( + Vector::new(-4.0, 3.0), + Vector::new(-2.0, 3.0), + 8, + )); + let flag = |world: &PhysicsWorld, h: SoftBodyHandle| { + let co = world.soft_bodies[h].collision_mesh().unwrap().collider(); + let polyline = world.colliders[co].shape().as_polyline().unwrap(); + polyline.flags().contains(PolylineFlags::ORIENTED) + }; + assert!(flag(&world, closed), "a closed surface is oriented by default"); + assert!(!flag(&world, shell), "an explicit `oriented(false)` was ignored"); + assert!(!flag(&world, rope), "an open surface cannot be oriented"); + world.step(); + assert!(world.soft_bodies[closed].collision_mesh().unwrap().is_solid()); + assert!(!world.soft_bodies[shell].collision_mesh().unwrap().is_solid()); + + // Cleared on the collider's shape: the mesh follows at the next step. + let co = world.soft_bodies[closed].collision_mesh().unwrap().collider(); + let polyline = world.colliders[co].shape_mut().as_polyline_mut().unwrap(); + polyline.set_flags(polyline.flags() & !PolylineFlags::ORIENTED); + world.step(); + let mesh = world.soft_bodies[closed].collision_mesh().unwrap(); + assert!(!mesh.is_oriented() && !mesh.is_solid()); + assert!(!flag(&world, closed), "the step put the flag back"); } /// A fixed thin pin inside a blob pressed on the ground by a plate cannot be expelled: the diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index 3b813030c..cc05de001 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -1643,6 +1643,107 @@ fn plastic_flow_is_bounded() { ); } +/// An oriented surface holds nothing inside: a small rigid ball straddling a soft cube's +/// face from inside and moving outward crosses it freely (and a particle of a second soft body +/// likewise comes out) instead of being trapped, and the cube itself stays whole. +#[test] +fn closed_surface_expels_intruders() { + let mut world = world_with_ground(); + world.gravity = Vector::ZERO; + let cube = SoftBodyBuilder::cuboid(Vector::new(0.0, 3.0, 0.0), Vector::splat(0.75), 5, 5, 5) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 1.0e4, + poisson_ratio: 0.4, + elastic_damping_ratio: 0.5, + ..Default::default() + }) + .particle_mass(0.1) + .particle_radius(0.05); + let cube = world.insert_soft_body(cube); + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(0.1, 3.68, 0.1)) + .linvel(Vector::new(0.0, 1.0, 0.0)), + ColliderBuilder::ball(0.1), + ); + let intruder = SoftBodyBuilder::new(vec![ + Vector::new(0.72, 3.1, 0.0), + Vector::new(1.2, 3.0, 0.1), + Vector::new(1.0, 3.5, -0.1), + Vector::new(1.0, 3.2, 0.4), + ]) + .cells(vec![[0, 1, 2, 3]]) + .particle_radius(0.05) + .particle_mass(0.1); + let intruder = world.insert_soft_body(intruder); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, cube); + let sb = &world.soft_bodies[cube]; + let inside = |p: Vector| { + let com = sb.center_of_mass(); + (p - com).abs().max_element() < 0.75 - 0.05 + }; + let ball_pos = world.bodies[ball].translation(); + assert!( + !inside(ball_pos) && ball_pos.y > 4.0, + "the rigid ball was held inside the cube: {ball_pos:?}" + ); + for p in world.soft_bodies[intruder].particles() { + assert!( + !inside(p.position()), + "intruder particle still inside: {:?}", + p.position() + ); + } + let volume = sb.volume(); + assert!( + (volume - sb.rest_volume()).abs() < 0.1 * sb.rest_volume(), + "cube volume {volume} vs rest {}", + sb.rest_volume() + ); +} + +/// An unoriented balloon is a shell: a rigid ball spawned straddling its wall from inside is pushed +/// back in and rests on its inner wall, while an oriented (solid) balloon expels it. +#[test] +fn unoriented_balloon_holds_a_ball_inside() { + let run = |oriented: bool| -> (Vector, Real) { + let mut world = world_with_ground(); + let balloon = SoftBodyBuilder::sphere(Vector::new(0.0, 1.0, 0.0), 0.8, 2) + .softness(SpringCoefficients::new(20.0, 1.0)) + .particle_mass(0.05) + .particle_radius(0.05) + .surface_collider(ColliderBuilder::ball(0.05)) + .oriented(oriented); + let handle = world.insert_soft_body(balloon); + // Straddling the wall on the right, its center inside. + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.75, 1.0, 0.0)), + ColliderBuilder::ball(0.1), + ); + for _ in 0..300 { + world.step(); + } + assert_finite(&world, handle); + let sb = &world.soft_bodies[handle]; + let lowest = sb.particle_positions().map(|p| p.y).fold(Real::MAX, Real::min); + (world.bodies[ball].translation() - sb.center_of_mass(), lowest) + }; + let (offset, lowest) = run(false); + assert!( + offset.length() < 0.8 && offset.y + 1.0 > lowest + 0.1, + "the ball left the unoriented balloon: offset {offset:?}, lowest particle {lowest}" + ); + let (offset, _) = run(true); + assert!( + offset.length() > 0.8, + "the ball was not expelled from the oriented balloon: offset {offset:?}" + ); +} + /// A body whose cells keep flowing is kept awake: a plastic column creeping under its own /// weight must not fall asleep mid-creep (its elastic twin, at rest, does sleep). #[test] diff --git a/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi b/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi index 4d28e779c..400b8619a 100644 --- a/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi +++ b/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi @@ -2240,6 +2240,7 @@ class SoftBodyBuilder: def volume_factor(self, factor: float) -> SoftBodyBuilder: ... def shape_matching(self, enabled: bool) -> SoftBodyBuilder: ... def self_contacts(self, enabled: bool) -> SoftBodyBuilder: ... + def oriented(self, oriented: bool) -> SoftBodyBuilder: ... def particle_radius(self, radius: float) -> SoftBodyBuilder: ... def surface_collider(self, collider: ColliderBuilder) -> SoftBodyBuilder: ... def no_surface_collider(self) -> SoftBodyBuilder: ... @@ -2367,6 +2368,7 @@ class SoftCollisionMesh: is_closed: bool is_wire: bool self_contacts_enabled: bool + is_oriented: bool @property def vertex_count(self) -> int: ... @property diff --git a/python/rapier-py-3d/src/soft_body.rs b/python/rapier-py-3d/src/soft_body.rs index bffbb2d29..1ea05b679 100644 --- a/python/rapier-py-3d/src/soft_body.rs +++ b/python/rapier-py-3d/src/soft_body.rs @@ -956,6 +956,7 @@ impl SoftBodyBuilder { "volume_factor" => b.volume_factor(v.extract()?), "shape_matching" => b.shape_matching(v.extract()?), "self_contacts" => b.self_contacts(v.extract()?), + "oriented" => b.oriented(v.extract()?), "particle_radius" => b.particle_radius(v.extract()?), "surface_collider" => { b.surface_collider(v.extract::>()?.builder.clone()) @@ -1180,6 +1181,13 @@ impl SoftBodyBuilder { fn self_contacts(&self, enabled: bool) -> Self { self.chained(|b| b.self_contacts(enabled)) } + /// Set whether the shape of the body's collision surface is built with the ``ORIENTED`` + /// flag, like a polyline or mesh. Left unset, it is whenever the surface is closed, which is + /// what a solid body wants: an oriented closed surface encloses matter, so nothing is held + /// inside it. Set it to ``False`` for a shell, whose inner side holds the bodies inside it. + fn oriented(&self, oriented: bool) -> Self { + self.chained(|b| b.oriented(oriented)) + } /// Set the thickness of the particles. fn particle_radius(&self, radius: Real) -> Self { self.chained(|b| b.particle_radius(radius)) @@ -1635,6 +1643,10 @@ pub struct SoftCollisionMesh { /// Does the mesh collide with itself? #[pyo3(get)] self_contacts_enabled: bool, + /// Does the mesh's collider shape carry the ``ORIENTED`` flag (see + /// :meth:`SoftBodyBuilder.oriented`)? + #[pyo3(get)] + is_oriented: bool, vertices: Vec, indices: Vec>, } @@ -1691,6 +1703,7 @@ impl SoftCollisionMesh { is_closed: mesh.is_closed(), is_wire: mesh.is_wire(), self_contacts_enabled: mesh.self_contacts_enabled(), + is_oriented: mesh.is_oriented(), vertices: mesh.vertex_positions(body).collect(), indices, } diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh.rs index d7104d5dc..c132603f5 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh.rs @@ -90,8 +90,13 @@ pub struct SoftCollisionMesh { #[cfg(feature = "dim3")] pub(crate) edge_owners: Vec, /// Whether the mesh is closed (every segment vertex / triangle edge shared by exactly two - /// elements): its contacts with other bodies are then oriented outward. + /// elements): its contacts with other bodies are then oriented outward, if it is oriented. pub(crate) closed: bool, + /// Whether the collider's shape carries parry's `ORIENTED` flag, read back from it before + /// every step: closed, the mesh then encloses solid matter. Otherwise it is a shell: its + /// contacts are two-sided, a body inside it rests on its inner side, and it takes no patch. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) oriented: bool, /// Signed area/volume enclosed by the mesh at rest: with `inverted`, it says which way the /// winding points. pub(crate) rest_signed_volume: Real, diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs index 28a25c970..44f90dbdf 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs @@ -97,11 +97,39 @@ impl SoftCollisionMesh { matches!(self.binding, SoftMeshMapping::Skinned { .. }) } - /// Whether this mesh is closed (its contacts with other bodies are then oriented outward). + /// Whether this mesh is closed (every segment vertex / triangle edge shared by exactly two + /// elements). pub fn is_closed(&self) -> bool { self.closed } + /// Whether this mesh's collider shape carries parry's `ORIENTED` flag, as of the last step. + pub fn is_oriented(&self) -> bool { + self.oriented + } + + /// Whether this mesh encloses solid matter: closed and oriented. Its contacts with other + /// bodies are then oriented outward and nothing is held inside it; otherwise it is two-sided. + pub fn is_solid(&self) -> bool { + self.closed && self.oriented + } + + /// Reads the orientation back from the collider's shape, which is the authority on it. + pub(crate) fn read_orientation(&mut self, colliders: &crate::geometry::ColliderSet) { + let Some(co) = colliders.get(self.collider) else { + return; + }; + #[cfg(feature = "dim2")] + let oriented = co.shape().as_polyline().is_some_and(|polyline| { + polyline.flags().contains(parry::shape::PolylineFlags::ORIENTED) + }); + #[cfg(feature = "dim3")] + let oriented = co.shape().as_trimesh().is_some_and(|trimesh| { + trimesh.flags().contains(parry::shape::TriMeshFlags::ORIENTED) + }); + self.oriented = oriented; + } + /// Whether this mesh collides with itself. pub fn self_contacts_enabled(&self) -> bool { self.self_contacts diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs index 1e534ea7a..29a6b4278 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs @@ -278,14 +278,14 @@ impl SoftCollisionMesh { } /// Outward normal (not normalized) of an element from the cached vertex positions, only - /// meaningful for a closed mesh (`None` otherwise, and for an element whose cell is - /// currently inverted: its winding is mirrored, so it cannot orient its contacts). + /// meaningful for a solid mesh (`None` for an open or unoriented one, and for an element whose + /// cell is currently inverted: its winding is mirrored, so it cannot orient its contacts). pub(crate) fn element_outward_normal( &self, body: &SoftBody, element_id: usize, ) -> Option { - if !self.closed { + if !self.is_solid() { return None; } // A likely self-crossed loop (an O squeezed into an 8) has part of its winding mirrored; diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs index 272223b07..112d31704 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs @@ -60,6 +60,7 @@ impl SoftCollisionMesh { edge_owners, inverted: false, orientation_unreliable: false, + oriented: false, collision_enabled, collider: ColliderHandle::invalid(), self_contacts, @@ -128,6 +129,7 @@ impl SoftCollisionMesh { edge_owners, inverted: false, orientation_unreliable: false, + oriented: false, collision_enabled: true, collider: ColliderHandle::invalid(), self_contacts, @@ -209,6 +211,7 @@ impl SoftCollisionMesh { edge_owners, inverted: false, orientation_unreliable: false, + oriented: false, collision_enabled, collider: ColliderHandle::invalid(), self_contacts, @@ -234,8 +237,7 @@ impl SoftCollisionMesh { } /// Moves the vertices of this mesh's collider shape to the current positions, in place: - /// parry refits the shape's BVH. A closed 2D polyline is one-sided only while its winding - /// can be trusted (see [`Self::winding_trustworthy`]). + /// parry refits the shape's BVH. pub(crate) fn deform_shape( &self, body: &SoftBody, @@ -250,16 +252,6 @@ impl SoftCollisionMesh { }; // A polyline in either dimension (a 2D surface, or a wire in 3D), a trimesh in 3D. if let Some(polyline) = shape.as_polyline_mut() { - #[cfg(feature = "dim2")] - { - use parry::shape::PolylineFlags; - let oriented = self.closed && self.winding_trustworthy(body); - let mut flags = polyline.flags(); - if flags.contains(PolylineFlags::ORIENTED) != oriented { - flags.set(PolylineFlags::ORIENTED, oriented); - polyline.set_flags(flags); - } - } polyline.update_vertices(write); return; } @@ -287,19 +279,6 @@ impl SoftCollisionMesh { } } - /// Whether every element's winding says where the outside is: not for a mesh turned inside - /// out or hovering around a zero volume, nor while a cell backing an element is inverted. - /// The rigid contacts of a closed mesh are one-sided only then; otherwise they are two-sided - /// and the contact assembly orients the reversed ones (see `element_outward_normal`). - #[cfg(feature = "dim2")] - pub(crate) fn winding_trustworthy(&self, body: &SoftBody) -> bool { - if self.inverted || self.orientation_unreliable { - return false; - } - body.cells.is_empty() - || !(0..self.indices.len()).any(|e| self.element_cell_inverted(body, e)) - } - /// Updates the inside-out state of a closed mesh from the current vertex positions. pub(crate) fn update_orientation(&mut self, body: &SoftBody) { if !self.closed || self.rest_signed_volume == 0.0 { 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 af840d587..f74efe629 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 @@ -277,22 +277,28 @@ impl SoftBodyBuilder { } // A wire replaces the boundary mesh: a body made of segments has no surface. #[cfg(feature = "dim3")] - if !skin_collides && !self.wire.is_empty() { + let wire = !skin_collides && !self.wire.is_empty(); + #[cfg(feature = "dim2")] + let wire = false; + #[cfg(feature = "dim3")] + if wire { meshes.push(SoftCollisionMesh::wire( body, &self.wire, self.self_contacts, has_collider, )); - return meshes; } - if !skin_collides { + if !skin_collides && !wire { meshes.push(SoftCollisionMesh::boundary( body, self.self_contacts, has_collider, )); } + for mesh in &mut meshes { + mesh.oriented = self.oriented.unwrap_or(mesh.closed); + } meshes } } diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs index d6bda9435..160af2f2b 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs @@ -93,6 +93,9 @@ pub struct SoftBodyBuilder { pub shape_matching: bool, /// Whether the body's surface collides with itself. pub self_contacts: bool, + /// Whether the default collision surface's shape is built with parry's `ORIENTED` flag + /// (see [`Self::oriented`]); `None`: when the surface is closed. + pub oriented: Option, /// Thickness of the particles: the radius of their ball colliders (bodies colliding through /// their particles), the surface collider's contact skin otherwise. pub particle_radius: Real, diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs index 33df49e92..0205cb14d 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs @@ -334,6 +334,17 @@ impl SoftBodyBuilder { self } + /// Whether the shape of the body's default collision surface is built with parry's + /// `ORIENTED` flag, like a rigid polyline or mesh. Left unset, it is whenever the surface is + /// closed, which is what a solid body wants: an oriented closed surface encloses matter, so + /// nothing is held inside it. Set it to `false` for a shell, whose inner side holds the bodies + /// inside it. After insertion the collider's shape is the authority: change its flags there, + /// like for any collider. Independent of the material inside (cells, pressure). + pub fn oriented(mut self, oriented: bool) -> Self { + self.oriented = Some(oriented); + self + } + /// Sets the thickness of the particles (the radius of their ball colliders for a body /// colliding through its particles, the surface collider's contact skin otherwise). pub fn particle_radius(mut self, radius: Real) -> Self { diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs index 81baa8e4f..6a8219332 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs @@ -42,6 +42,7 @@ impl SoftBodyBuilder { volume_factor: 1.0, shape_matching: false, self_contacts: false, + oriented: None, particle_radius: 0.01, skin: None, #[cfg(feature = "dim3")] @@ -125,7 +126,7 @@ impl SoftBodyBuilder { .edges(edges) .surface(boundary) .shape_matching(true) - .particle_radius(mean_edge * 0.5), + .particle_radius(mean_edge * 0.5), ) } } 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 6955dcda5..bdf0f6eb4 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 @@ -8,32 +8,44 @@ use simba::scalar::{ComplexField as _, RealField as _}; use crate::dynamics::soft_body::{SoftBody, SoftBodyHandle, SoftMeshRef}; use crate::math::{DIM, Pose, Real, Vector}; -/// The deformable surface shape (a 2D polyline or 3D triangle mesh over `vertices`), flagged -/// deformable so the narrow phase never trusts contact points cached across a vertex update. -/// A closed 2D polyline is one-sided (pushing outward); an open one (a rope) is two-sided. +/// The deformable surface shape of `mesh` (a 2D polyline or 3D triangle mesh over `vertices`), +/// flagged deformable so the narrow phase never trusts contact points cached across a vertex +/// update, and oriented when the mesh is solid (see `SoftCollisionMesh::is_solid`): one-sided, +/// pushing outward. An open mesh (a rope, a cloth) and a shell are two-sided. pub(crate) fn surface_shape( vertices: Vec, - surface: &[[u32; DIM]], - closed: bool, + mesh: &super::SoftCollisionMesh, ) -> Option { + let surface = mesh.indices(); if surface.is_empty() || vertices.is_empty() { return None; } + let solid = mesh.is_solid(); + // Parry reads the outside from the winding: a mesh wound inward gets its elements reversed + // (their ids, which the contacts report, stay the same). + let outward = |surface: &[[u32; DIM]]| -> Vec<[u32; DIM]> { + let mut elements = surface.to_vec(); + if mesh.rest_signed_volume < 0.0 { + for e in &mut elements { + e.swap(DIM - 2, DIM - 1); + } + } + elements + }; #[cfg(feature = "dim2")] { use parry::shape::{Polyline, PolylineFlags}; let mut flags = PolylineFlags::DEFORMABLE; - flags.set(PolylineFlags::ORIENTED, closed); + flags.set(PolylineFlags::ORIENTED, solid); Some(SharedShape::new(Polyline::with_flags( vertices, - Some(surface.to_vec()), + Some(outward(surface)), flags, ))) } #[cfg(feature = "dim3")] { use parry::shape::{Polyline, PolylineFlags, TriMeshFlags}; - let _ = closed; // A wire's elements are segments (`u32::MAX` fills the unused slot): it collides as a // polyline, which parry supports in 3D too. if super::soft_body_builder::element_vertices(&surface[0]).len() < DIM { @@ -44,40 +56,44 @@ pub(crate) fn surface_shape( PolylineFlags::DEFORMABLE, ))); } - SharedShape::trimesh_with_flags(vertices, surface.to_vec(), TriMeshFlags::DEFORMABLE).ok() + let mut flags = TriMeshFlags::DEFORMABLE; + flags.set(TriMeshFlags::ORIENTED, solid); + SharedShape::trimesh_with_flags(vertices, outward(surface), flags).ok() } } /// Rebuilds a surface shape whose topology changed (a tear), keeping the replaced shape's flags -/// except those that would edit the vertex or index buffers; a 2D polyline's orientation -/// follows `closed` (a torn open loop is two-sided). +/// except those that would edit the vertex or index buffers; its orientation follows the mesh (a +/// torn open loop is two-sided). pub(super) fn rebuilt_surface_shape( prev: &dyn parry::shape::Shape, vertices: Vec, - surface: &[[u32; DIM]], - closed: bool, + mesh: &super::SoftCollisionMesh, ) -> Option { - let mut shape = surface_shape(vertices, surface, closed)?; + let mut shape = surface_shape(vertices, mesh)?; let rebuilt = shape.make_mut(); if let (Some(prev), Some(rebuilt)) = (prev.as_polyline(), rebuilt.as_polyline_mut()) { #[allow(unused_mut)] let mut flags = prev.flags(); #[cfg(feature = "dim2")] - flags.set(parry::shape::PolylineFlags::ORIENTED, closed); + flags.set(parry::shape::PolylineFlags::ORIENTED, mesh.is_solid()); rebuilt.set_flags(flags); } #[cfg(feature = "dim3")] if let (Some(prev), Some(rebuilt)) = (prev.as_trimesh(), rebuilt.as_trimesh_mut()) { use parry::shape::TriMeshFlags; - let flags = prev.flags() + let mut flags = prev.flags() & !(TriMeshFlags::MERGE_DUPLICATE_VERTICES | TriMeshFlags::DELETE_DEGENERATE_TRIANGLES | TriMeshFlags::DELETE_DUPLICATE_TRIANGLES | TriMeshFlags::DELETE_BAD_TOPOLOGY_TRIANGLES); + flags.set(TriMeshFlags::ORIENTED, mesh.is_solid()); // A torn mesh may no longer satisfy a topology flag: it falls back to the bare // deformable shape. if rebuilt.set_flags(flags).is_err() { - let _ = rebuilt.set_flags(TriMeshFlags::DEFORMABLE); + let mut bare = TriMeshFlags::DEFORMABLE; + bare.set(TriMeshFlags::ORIENTED, mesh.is_solid()); + let _ = rebuilt.set_flags(bare); } } Some(shape) @@ -97,11 +113,7 @@ pub(super) fn spawn_mesh_collider( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, ) -> Option { - let shape = surface_shape( - mesh.local_vertices(body, frame), - mesh.indices(), - mesh.is_closed(), - )?; + let shape = surface_shape(mesh.local_vertices(body, frame), mesh)?; let mut collider = template .clone() .density(0.0) @@ -136,12 +148,7 @@ pub(super) fn clone_mesh_collider( colliders: &mut ColliderSet, ) -> Option { let mut collider = colliders.get(source)?.clone(); - let shape = rebuilt_surface_shape( - collider.shape(), - mesh.local_vertices(body, frame), - mesh.indices(), - mesh.is_closed(), - )?; + let shape = rebuilt_surface_shape(collider.shape(), mesh.local_vertices(body, frame), mesh)?; collider.set_shape(shape); collider.set_position(Pose::IDENTITY); collider.set_enabled(true); 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 f656717e3..aa42e6db3 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 @@ -25,6 +25,12 @@ impl SoftBodySet { params: &IntegrationParameters, quarantined: &mut Vec, ) { + // The colliders' shapes say which surfaces are oriented (the user may have changed them). + for (_, sb) in self.bodies.iter_mut() { + for mesh in sb.meshes_mut() { + mesh.read_orientation(colliders); + } + } let mut relink = false; let handles: Vec = self .bodies @@ -166,12 +172,7 @@ impl SoftBodySet { co.deform_pose(frame); let pose = *co.position(); if rebuild { - match rebuilt_surface_shape( - co.shape(), - mesh.local_vertices(sb, &pose), - mesh.indices(), - mesh.is_closed(), - ) { + match rebuilt_surface_shape(co.shape(), mesh.local_vertices(sb, &pose), mesh) { Some(shape) => co.replace_deformed_shape(shape), None => co.set_enabled(false), } diff --git a/src/dynamics/soft_body/soft_recovery_settings.rs b/src/dynamics/soft_body/soft_recovery_settings.rs index ca7b61ff3..7d82c1a4c 100644 --- a/src/dynamics/soft_body/soft_recovery_settings.rs +++ b/src/dynamics/soft_body/soft_recovery_settings.rs @@ -102,7 +102,7 @@ pub struct SoftRecoverySettings { pub overlap_constraint_pace: Real, /// What the per-point constraints (manifold constraints against rigid colliders, vertex /// constraints against soft surfaces) of the features inside a volume constraint's patch do, - /// feature by feature (see [`SoftPatchConstraints`]). (default: `Keep`) + /// feature by feature (see [`SoftPatchConstraints`]). (default: `AlongNormal`) pub overlap_patch_constraints: SoftPatchConstraints, /// Measure the intersection volume on the contact skins (surfaces dilated by their skin), not /// the geometric surfaces: a resting pair then has a hard volume constraint with speculative @@ -118,7 +118,7 @@ pub struct SoftRecoverySettings { pub overlap_self_regions: bool, /// Push along each constraint's normal instead of the volume gradients: every entry keeps its /// gradient's magnitude but takes the constraint's mean direction, so the whole patch - /// separates along one axis. (default: `false`) + /// separates along one axis. (default: `true`) pub overlap_normal_push: bool, /// Multi-volume grid (the paper's section 5): each pair's patch is split into a regular grid of /// cells along its tangent axes, each with its own constraint and multiplier, so the pressure @@ -157,12 +157,12 @@ impl Default for SoftRecoverySettings { overlap_skip_self_tangled: true, overlap_edge_stand_down: true, overlap_constraint_pace: 1.0, - overlap_patch_constraints: SoftPatchConstraints::Keep, + overlap_patch_constraints: SoftPatchConstraints::AlongNormal, overlap_skin_volume: false, overlap_kept_depth: 0.0, - overlap_normal_push: false, + overlap_normal_push: true, overlap_self_regions: false, - overlap_multi_volume: true, + overlap_multi_volume: false, overlap_split: 3, overlap_patience: 240, overlap_progress_margin: 0.02, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs index 65afe1f8e..77575dc19 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs @@ -284,11 +284,19 @@ impl SoftConstraintsSet { (v_other - v_surface).gdot(dir).max(0.0) }; - // A closed surface's reversed interior contacts (an intruder seen from inside, + // A solid surface's reversed interior contacts (an intruder seen from inside, // a fold's far layer) are dropped: the volume constraints and the elasticity - // resolve them. The rule reads the cells' winding, so a skin collides plainly. + // resolve them. The rule reads the cells' winding, so a skin collides plainly, + // and a shell (an unoriented surface) holds every body on the side it is on. let skinned = mesh.is_skinned(); - let closed = mesh.is_closed() && self_particle.is_none() && !skinned; + let closed = mesh.is_solid() && self_particle.is_none() && !skinned; + // A body centered inside a solid surface is not held in it. In 2D parry's + // oriented polyline gives it no contact at all; a 3D mesh's contacts stay + // two-sided whatever its `ORIENTED` flag, so they are dropped here. + #[cfg(feature = "dim3")] + if closed && mesh.contains_point_parity(sb, other_co.translation()) { + continue; + } // The features a volume constraint acts on (see `overlap_patch_constraints`). let patch_policy = params.soft_bodies.recovery.overlap_patch_constraints; diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs index 8b9b9990a..a6c6cccce 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs @@ -320,8 +320,12 @@ impl SoftConstraintsSet { if patch_guide.is_some() && patch_policy == SoftPatchConstraints::StandDown { continue; } - let along_patch_normal = - patch_guide.is_some() && patch_policy == SoftPatchConstraints::AlongNormal; + // A constraint measured from a rest gap (a crack between two pieces of one torn + // body) holds that rest geometry: bending it along the patch normal would treat + // the crack as a penetration by the skins and push the pieces apart. + let along_patch_normal = patch_guide.is_some() + && patch_policy == SoftPatchConstraints::AlongNormal + && !c.rest_gap; let repelled = if repel && flagged || along_patch_normal { // The nearest guiding cell's normal, if the guide is on. let guide = out diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 30b92d11e..1f42ebe4f 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -281,7 +281,17 @@ impl ColliderSet { .map_or(coll.pos.0, |parent| parent.pos_wrt_parent); let pose = frame * pos_wrt_parent; let vertices: Vec = vertices.iter().map(|v| pose * *v).collect(); - let mesh = sb.bind_mesh(cluster_id, &binding, vertices, indices)?; + let mut mesh = sb.bind_mesh(cluster_id, &binding, vertices, indices)?; + // The shape's `ORIENTED` flag says whether the closed mesh encloses solid matter. + #[cfg(feature = "dim2")] + let oriented = coll.shape().as_polyline().is_some_and(|polyline| { + polyline.flags().contains(parry::shape::PolylineFlags::ORIENTED) + }); + #[cfg(feature = "dim3")] + let oriented = coll.shape().as_trimesh().is_some_and(|trimesh| { + trimesh.flags().contains(parry::shape::TriMeshFlags::ORIENTED) + }); + mesh.oriented = oriented; coll.set_density(0.0); // The contact skin is the thickness of the mesh's vertices in the soft contact // passes: a collider left without one gets the body's particle radius. diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs index e87b5701b..67f87a891 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs @@ -255,8 +255,8 @@ fn detect_soft_pair( let owner_pass = if own.2 == h1 { &pass1 } else { &pass2 }; let recovery = &ctx.params.soft_bodies.recovery; let stood_down = cfg!(feature = "dim3") - && mesh1.is_closed() - && mesh2.is_closed() + && mesh1.is_solid() + && mesh2.is_solid() && recovery.overlap_constraints && recovery.overlap_edge_stand_down && !owner_pass.cross_tangled_elements.is_empty(); 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 e24c9152e..b4c376387 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs @@ -67,6 +67,10 @@ pub struct SoftVertexCandidate { pub interior: bool, /// Whether the solver builds a constraint for it (the hook clears it to drop the contact). pub enabled: bool, + /// The two surfaces are pieces of one torn body, their separations measured from the + /// features' rest gaps (see `rest_gap_skins`): the constraint holds a crack's rest geometry, + /// and the along-normal volume-patch policy leaves it as it is. + pub rest_gap: bool, /// The normal impulse the solver applied through it at the last step. pub impulse: Real, /// The world-space friction impulse the solver applied through it at the last step. diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs index 02ca033c3..89182f06b 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs @@ -157,8 +157,9 @@ pub(crate) fn detect_vertex_pass( let skins = vb_co.contact_skin() + eb_co.contact_skin(); let prediction = params.prediction_distance(); let reach = prediction + skins + ctx.motion_margin(eb_co) + ctx.motion_margin(vb_co); - // A closed surface's reversed sightings mark a foreign vertex inside it. - let closed = eb_mesh.is_closed() && !is_self; + // A solid surface's reversed sightings mark a foreign vertex inside it (a shell holds it on + // its inner side). + let closed = eb_mesh.is_solid() && !is_self; // Crossing repulsion (see `repel_constraint`): the detected crossing pairs also get their own // constraints (the piercing element's vertices against the pierced element), so an edge-first @@ -205,8 +206,8 @@ pub(crate) fn detect_vertex_pass( if ((repel && params.soft_bodies.recovery.crossing_repulsion_guide) || patch_policy != crate::dynamics::SoftPatchConstraints::Keep) && !is_self - && vb_mesh.is_closed() - && eb_mesh.is_closed() + && vb_mesh.is_solid() + && eb_mesh.is_solid() && !out.cross_pairs.is_empty() { let mut inside_vb = Vec::new(); @@ -262,7 +263,7 @@ pub(crate) fn detect_vertex_pass( let self_guided = repel && params.soft_bodies.recovery.crossing_repulsion_self_guide && is_self - && vb_mesh.is_closed() + && vb_mesh.is_solid() && !crossings.is_empty(); if self_guided { let n_v = vb_mesh.vertex_count(); @@ -570,6 +571,7 @@ impl VertexScan<'_> { outward: Some(outward), interior: false, enabled: true, + rest_gap: true, impulse: 0.0, tangent_impulse: Vector::ZERO, }); @@ -641,6 +643,7 @@ impl VertexScan<'_> { outward, interior: weights.iter().all(|w| *w > 0.02), enabled: true, + rest_gap: self.rest_gaps, impulse: 0.0, tangent_impulse: Vector::ZERO, }); @@ -671,6 +674,7 @@ impl VertexScan<'_> { outward: None, interior: false, enabled: true, + rest_gap: false, impulse: 0.0, tangent_impulse: Vector::ZERO, }); diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs index 559254ec4..54161b60d 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs @@ -63,7 +63,7 @@ pub(super) fn detect_volume_patch( ) -> Option { let params = ctx.params; let recovery = ¶ms.soft_bodies.recovery; - let closed_pair = vb_mesh.is_closed() && eb_mesh.is_closed(); + let closed_pair = vb_mesh.is_solid() && eb_mesh.is_solid(); let crossed = !pass.cross_pairs.is_empty() && !pass.cross_tangled_vb_elements.is_empty(); if !recovery.overlap_constraints || !closed_pair @@ -171,8 +171,9 @@ pub(super) fn detect_rigid_patch( { return None; } - // A self-crossed mesh sits out: part of its winding is mirrored. - if !mesh.is_closed() + // A shell (unoriented mesh) holds intruders instead; a self-crossed mesh sits out: part of + // its winding is mirrored. + if !mesh.is_solid() || (recovery.overlap_skip_self_tangled && (mesh.orientation_unreliable || mesh.crossed_partners.contains(&handle))) { diff --git a/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs b/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs index 69e1171ab..85a7c3dc9 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs @@ -396,7 +396,7 @@ fn detect_self_regions( out.region_bins.clear(); if !recovery.overlap_constraints || !recovery.overlap_self_regions - || !mesh.is_closed() + || !mesh.is_solid() || mesh.orientation_unreliable || out.crossings.is_empty() { diff --git a/typescript/CHANGELOG.md b/typescript/CHANGELOG.md index dbcc4073c..37d3236dd 100644 --- a/typescript/CHANGELOG.md +++ b/typescript/CHANGELOG.md @@ -15,6 +15,10 @@ ### Added +- `SoftBodyDesc.setOriented` and `SoftBody.isMeshOriented`: whether the shape of a soft body's + collision surface carries the `ORIENTED` flag. Left unset, a closed surface is oriented (it + encloses solid matter, so nothing is held inside it); `setOriented(false)` makes it a shell + whose inner side holds the bodies inside it. A deformable collider follows its own shape's flag. - Soft bodies: `World.createSoftBody(desc)` inserts a deformable body made of particles linked by edges, bending constraints and cells, simulated together with the rigid bodies, contacts and joints. `SoftBodyDesc` offers the generators `rope`, `cloth`, `clothTube`, diff --git a/typescript/src.ts/dynamics/soft_body.ts b/typescript/src.ts/dynamics/soft_body.ts index 54b302176..04b8cbb12 100644 --- a/typescript/src.ts/dynamics/soft_body.ts +++ b/typescript/src.ts/dynamics/soft_body.ts @@ -1113,6 +1113,14 @@ export class SoftBody { return this.rawSet.sbMeshCollisionEnabled(this.handle, i); } + /** + * Does the shape of the `i`-th collision mesh carry the `ORIENTED` flag (see + * `SoftBodyDesc.setOriented`)? + */ + public isMeshOriented(i: number): boolean { + return this.rawSet.sbMeshIsOriented(this.handle, i); + } + /** * The world-space vertex positions of the `i`-th collision mesh, flattened. */ @@ -1178,25 +1186,32 @@ export class SoftBodyDesc { */ cellModel: SoftBodyCellModel; /** - * Whether the area/volume enclosed by the body's closed surfaces is preserved. + * Whether the area/volume enclosed by the body's closed surfaces is preserved (`null`: + * what the generator chose, off for raw positions). */ - volumePreservation: boolean; + volumePreservation: boolean | null; /** * The target volume multiplier of the volume preservation (`> 1` inflates the body). */ volumeFactor: number; /** - * Whether shape matching holds the body's shape. + * Whether shape matching holds the body's shape (`null`: what the generator chose, off for + * raw positions). */ - shapeMatching: boolean; + shapeMatching: boolean | null; /** * Whether the body's surface collides with itself. */ selfContacts: boolean; /** - * The thickness of the particles. + * Whether the shape of the body's collision surface is built with the `ORIENTED` flag + * (`null`: whenever the surface is closed). */ - particleRadius: number; + oriented: boolean | null; + /** + * The thickness of the particles (`null`: what the generator chose). + */ + particleRadius: number | null; /** * The template of the body's colliders (their shape is replaced by the body's deformable * surface), or `null` for a body without collisions. @@ -1256,11 +1271,12 @@ export class SoftBodyDesc { this.masses = null; this.pinnedParticles = new Uint32Array(0); this.cellModel = SoftBodyCellModel.Volume; - this.volumePreservation = false; + this.volumePreservation = null; this.volumeFactor = 1.0; - this.shapeMatching = false; + this.shapeMatching = null; this.selfContacts = false; - this.particleRadius = 0.01; + this.oriented = null; + this.particleRadius = null; this.surfaceCollider = ColliderDesc.ball(0.05); this.skinCollision = false; this.translation = null; @@ -1758,6 +1774,18 @@ export class SoftBodyDesc { return this; } + /** + * Sets whether the shape of the body's collision surface is built with the `ORIENTED` + * flag, like a polyline or mesh. Left unset, it is whenever the surface is closed, which + * is what a solid body wants: an oriented closed surface encloses matter, so nothing is + * held inside it. Set it to `false` for a shell, whose inner side holds the bodies inside + * it. + */ + public setOriented(oriented: boolean): SoftBodyDesc { + this.oriented = oriented; + return this; + } + /** * Sets the thickness of the particles. */ @@ -1890,11 +1918,21 @@ export class SoftBodyDesc { } raw.setPinnedParticles(this.pinnedParticles); raw.setCellModel(this.cellModel as number as RawSoftBodyCellModel); - raw.setVolumePreservation(this.volumePreservation); + // The settings a generator may have chosen are only overridden when set here. + if (this.volumePreservation !== null) { + raw.setVolumePreservation(this.volumePreservation); + } raw.setVolumeFactor(this.volumeFactor); - raw.setShapeMatching(this.shapeMatching); + if (this.shapeMatching !== null) { + raw.setShapeMatching(this.shapeMatching); + } raw.setSelfContacts(this.selfContacts); - raw.setParticleRadius(this.particleRadius); + if (this.oriented !== null) { + raw.setOriented(this.oriented); + } + if (this.particleRadius !== null) { + raw.setParticleRadius(this.particleRadius); + } raw.setSkinCollision(this.skinCollision); if (this.surfaceCollider) { let c = this.surfaceCollider; diff --git a/typescript/src/dynamics/soft_body.rs b/typescript/src/dynamics/soft_body.rs index dd3f6546c..9d62b70de 100644 --- a/typescript/src/dynamics/soft_body.rs +++ b/typescript/src/dynamics/soft_body.rs @@ -622,6 +622,10 @@ impl RawSoftBodyBuilder { self.0.self_contacts = enabled; } + pub fn setOriented(&mut self, oriented: bool) { + self.0.oriented = Some(oriented); + } + pub fn setParticleRadius(&mut self, radius: f32) { self.0.particle_radius = radius; } @@ -1513,6 +1517,10 @@ impl RawSoftBodySet { }) } + pub fn sbMeshIsOriented(&self, handle: FlatHandle, i: usize) -> bool { + self.map(handle, |sb| sb.meshes().nth(i).is_some_and(|m| m.is_oriented())) + } + /// The world-space vertex positions of a collision mesh, `DIM` floats per vertex. pub fn sbMeshVertices(&self, handle: FlatHandle, i: usize) -> Vec { self.map(handle, |sb| { From c234c125cf461a905982a53be2f1c5143f99cbab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 19 Sep 2026 17:54:56 +0200 Subject: [PATCH 30/32] chore: CI fixes --- crates/rapier2d/tests/snapshot_portability.rs | 2 +- crates/rapier2d/tests/soft_bodies.rs | 342 ++++++++++++++---- crates/rapier2d/tests/soft_body_clusters.rs | 19 +- .../tests/soft_contact_debug_render.rs | 17 +- crates/rapier3d/tests/parallel_path_parity.rs | 2 +- crates/rapier3d/tests/snapshot_portability.rs | 2 +- crates/rapier3d/tests/soft_bodies.rs | 142 ++++++-- crates/rapier3d/tests/soft_body_clusters.rs | 31 +- crates/rapier3d/tests/soft_body_joints.rs | 16 +- crates/rapier3d/tests/soft_body_meshes.rs | 39 +- crates/rapier3d/tests/soft_wires.rs | 6 +- examples2d/all_examples2.rs | 2 +- examples2d/debug_self_intersect2.rs | 37 +- examples2d/soft_cutting2.rs | 7 +- examples2d/soft_force_tearing2.rs | 18 +- examples2d/soft_joints2.rs | 17 +- examples2d/soft_stress2.rs | 2 +- examples2d/soft_tearing2.rs | 11 +- examples3d/all_examples3.rs | 2 +- examples3d/debug_dynamic_collider_add3.rs | 16 +- examples3d/debug_self_intersect3.rs | 27 +- examples3d/soft_joints3.rs | 42 ++- examples3d/soft_meshes3.rs | 7 +- examples3d/soft_tearing3.rs | 11 +- python/rapier-py-3d/src/controllers.rs | 4 +- python/rapier-py-3d/src/joints.rs | 8 +- python/rapier-py-3d/src/pipeline.rs | 6 +- python/rapier-py-3d/src/serde_glue.rs | 4 +- python/rapier-py-3d/src/soft_body.rs | 6 +- src/dynamics/integration_parameters.rs | 7 +- src/dynamics/island_manager/global_split.rs | 4 +- .../joint/multibody_joint/multibody.rs | 2 +- src/dynamics/mod.rs | 2 +- .../collision_mesh_accessors.rs | 28 +- .../collision_mesh/collision_mesh_geometry.rs | 5 +- .../collision_mesh/collision_mesh_topology.rs | 60 ++- src/dynamics/soft_body/collision_mesh/mod.rs | 6 +- src/dynamics/soft_body/mod.rs | 18 +- src/dynamics/soft_body/soft_body.rs | 14 +- src/dynamics/soft_body/soft_body_accessors.rs | 18 +- .../soft_body/soft_body_builder/mod.rs | 4 +- .../soft_body_builder/soft_body_build.rs | 6 +- .../soft_body_builder/soft_body_builder.rs | 2 +- .../soft_body_builder_mesh_helpers.rs | 6 +- .../soft_body_builder_settings.rs | 2 +- .../soft_body_builder_shapes.rs | 4 +- src/dynamics/soft_body/soft_body_cluster.rs | 30 +- src/dynamics/soft_body/soft_body_coloring.rs | 2 +- src/dynamics/soft_body/soft_body_contacts.rs | 9 +- src/dynamics/soft_body/soft_body_material.rs | 2 +- src/dynamics/soft_body/soft_body_meshes.rs | 24 +- .../soft_body/soft_body_plasticity.rs | 13 +- src/dynamics/soft_body/soft_body_set/mod.rs | 4 +- .../soft_body_set/soft_body_set_clusters.rs | 10 +- .../soft_body_set_insert_remove.rs | 28 +- .../soft_body_set/soft_body_set_proxies.rs | 4 +- .../soft_body_set/soft_body_set_split.rs | 28 +- .../soft_body_set/soft_body_set_step_sync.rs | 38 +- src/dynamics/soft_body/soft_body_volume.rs | 4 +- .../soft_body/soft_recovery_settings.rs | 4 +- src/dynamics/soft_body/tearing/tearing.rs | 9 +- .../soft_body/tearing/tearing_crack.rs | 18 +- src/dynamics/soft_body/tearing/tearing_cut.rs | 1 + .../soft_body/tearing/tearing_helpers.rs | 7 +- .../tearing/tearing_particle_split.rs | 12 +- .../soft_body/tearing/tearing_validate.rs | 19 +- src/dynamics/soft_body/tearing/tests.rs | 30 +- .../soft_constraints_set/mod.rs | 8 +- .../soft_constraint_plasticity.rs | 15 +- .../soft_constraint_prepare.rs | 17 +- .../soft_constraint_solve.rs | 23 +- .../soft_constraint_writeback.rs | 6 +- .../soft_constraints_set/soft_constraints.rs | 3 +- .../soft_constraints_set.rs | 1 - .../solver/soft_constraint/soft_contact.rs | 9 +- .../soft_contact_assembly.rs | 13 +- .../soft_contact_assembly_body_contacts.rs | 98 ++--- .../soft_contact_assembly_edge_contacts.rs | 18 +- .../soft_contact_assembly_vertex_contacts.rs | 34 +- ...oft_contact_assembly_volume_constraints.rs | 22 +- .../soft_contact_assembly_workspace.rs | 8 +- .../soft_contact_assembly_writeback.rs | 9 +- .../soft_constraint/soft_contact_chunks.rs | 23 +- .../soft_elastic_constraint.rs | 7 +- .../soft_neo_hookean.rs | 5 +- .../soft_element_constraint/test.rs | 38 +- .../soft_element_constraint_assembly.rs | 102 ++++-- src/dynamics/solver/soft_fem/soft_fem_set.rs | 23 +- .../solver/soft_fem/soft_fem_skyline.rs | 14 +- .../system/soft_fem_system_assemble.rs | 9 +- .../system/soft_fem_system_prepare.rs | 4 +- .../system/soft_fem_system_response.rs | 2 +- src/dynamics/solver/soft_fem/system/tests.rs | 19 +- .../solver/staged_island_solver/init.rs | 11 +- .../solver/staged_island_solver/joints.rs | 20 +- .../solver/staged_island_solver/solve.rs | 19 +- src/geometry/broad_phase_bvh/update.rs | 8 +- src/geometry/collider.rs | 19 +- src/geometry/collider_components.rs | 12 +- src/geometry/collider_set.rs | 16 +- src/geometry/contact_pair.rs | 11 +- src/geometry/mod.rs | 7 +- src/geometry/narrow_phase/contacts.rs | 2 +- src/geometry/narrow_phase/pair_update.rs | 24 +- .../narrow_phase/soft_contacts/mod.rs | 10 +- .../soft_contacts/soft_contacts_driver.rs | 12 +- .../soft_contacts/soft_contacts_edge_pass.rs | 29 +- .../soft_contacts/soft_contacts_pairs.rs | 10 +- .../soft_contacts/soft_contacts_types.rs | 20 +- .../soft_contacts_vertex_pass.rs | 46 ++- .../soft_contacts/soft_contacts_volume.rs | 16 +- .../soft_contacts/soft_self_contacts.rs | 26 +- .../debug_render_pipeline.rs | 11 +- src/pipeline/physics_hooks.rs | 2 +- src/pipeline/physics_pipeline/substep.rs | 6 +- src/pipeline/physics_world.rs | 27 +- src_testbed/grab.rs | 24 +- src_testbed/graphics.rs | 2 +- src_testbed/graphics/graphics_polyline.rs | 18 +- src_testbed/graphics/graphics_soft_bodies.rs | 7 +- src_testbed/ui.rs | 83 +++-- src_testbed/viewer.rs | 4 +- 122 files changed, 1603 insertions(+), 758 deletions(-) diff --git a/crates/rapier2d/tests/snapshot_portability.rs b/crates/rapier2d/tests/snapshot_portability.rs index 1c4cc6d0f..9b5489664 100644 --- a/crates/rapier2d/tests/snapshot_portability.rs +++ b/crates/rapier2d/tests/snapshot_portability.rs @@ -31,7 +31,7 @@ use rapier2d::prelude::*; /// Snapshot size in bytes, and its FNV-1a digest. Both, because the size alone localizes a /// failure: a differing size means a container's *encoding* changed, an equal size with a /// differing digest means the values did. -const GOLDEN: (usize, u64) = (90_574, 0xf017_bf38_b19d_4eab); +const GOLDEN: (usize, u64) = (90_574, 0x3019_f8f0_67c5_e643); const STEPS: usize = 60; diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index f10b80aee..dbbdb5b8d 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -1018,8 +1018,14 @@ fn unoriented_blob_holds_a_ball_inside() { } assert_finite(&world, handle); let sb = &world.soft_bodies[handle]; - let lowest = sb.particle_positions().map(|p| p.y).fold(Real::MAX, Real::min); - (world.bodies[ball].translation() - sb.center_of_mass(), lowest) + let lowest = sb + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + ( + world.bodies[ball].translation() - sb.center_of_mass(), + lowest, + ) }; let (offset, lowest) = run(false); assert!( @@ -1058,15 +1064,34 @@ fn the_collider_shape_decides_the_orientation() { let polyline = world.colliders[co].shape().as_polyline().unwrap(); polyline.flags().contains(PolylineFlags::ORIENTED) }; - assert!(flag(&world, closed), "a closed surface is oriented by default"); - assert!(!flag(&world, shell), "an explicit `oriented(false)` was ignored"); + assert!( + flag(&world, closed), + "a closed surface is oriented by default" + ); + assert!( + !flag(&world, shell), + "an explicit `oriented(false)` was ignored" + ); assert!(!flag(&world, rope), "an open surface cannot be oriented"); world.step(); - assert!(world.soft_bodies[closed].collision_mesh().unwrap().is_solid()); - assert!(!world.soft_bodies[shell].collision_mesh().unwrap().is_solid()); + assert!( + world.soft_bodies[closed] + .collision_mesh() + .unwrap() + .is_solid() + ); + assert!( + !world.soft_bodies[shell] + .collision_mesh() + .unwrap() + .is_solid() + ); // Cleared on the collider's shape: the mesh follows at the next step. - let co = world.soft_bodies[closed].collision_mesh().unwrap().collider(); + let co = world.soft_bodies[closed] + .collision_mesh() + .unwrap() + .collider(); let polyline = world.colliders[co].shape_mut().as_polyline_mut().unwrap(); polyline.set_flags(polyline.flags() & !PolylineFlags::ORIENTED); world.step(); @@ -1263,7 +1288,10 @@ fn strip_tears_when_pulled_apart() { } } let bodies = family(&world, handle); - assert!(bodies.len() >= 2, "{model:?}: the strip is still in one piece"); + assert!( + bodies.len() >= 2, + "{model:?}: the strip is still in one piece" + ); let sum = |f: &dyn Fn(&SoftBody) -> Real| -> Real { bodies.iter().map(|&h| f(&world.soft_bodies[h])).sum() }; @@ -1329,7 +1357,8 @@ fn tearing_splits_the_particle_two_fans_share() { .expect("the bottom edge exists") as u32; let num_particles = sb.num_particles() as u32; let (num_cells, mass, measure) = (sb.cells().len(), sb.mass(), sb.rest_measure()); - let event = world.tear_soft_body(handle, &[edge], &[]) + let event = world + .tear_soft_body(handle, &[edge], &[]) .expect("nothing tore"); world.soft_bodies[handle].validate_topology().unwrap(); assert_eq!(event.soft_body, handle); @@ -1573,7 +1602,11 @@ fn fem_deflection_is_substep_invariant() { let mut world = PhysicsWorld::new(); world.integration_parameters.num_solver_iterations = substeps; // The invariance is a property of the converged linear solve: lift the iteration cap. - world.integration_parameters.soft_bodies.fem.max_linear_iterations = 256; + world + .integration_parameters + .soft_bodies + .fem + .max_linear_iterations = 256; let builder = SoftBodyBuilder::grid( Vector::new(length * 0.5, 0.0), Vector::new(length * 0.5, thickness * 0.5), @@ -1672,7 +1705,10 @@ fn fem_stiff_stack_rests_without_sinking() { let (half, radius) = (3.0, 0.15); let mut handles = Vec::new(); for row in 0..3 { - let center = Vector::new(0.0, ground_top + half + 0.5 + row as Real * (2.0 * half + 1.0)); + let center = Vector::new( + 0.0, + ground_top + half + 0.5 + row as Real * (2.0 * half + 1.0), + ); let square = SoftBodyBuilder::grid(center, Vector::splat(half), 4, 4) .cell_model(SoftBodyCellModel::Corotational) .material(SoftBodyMaterial { @@ -1696,11 +1732,26 @@ fn fem_stiff_stack_rests_without_sinking() { for &h in &handles { let sb = &world.soft_bodies[h]; assert_finite(&world, h); - let min_y = sb.particles().iter().map(|p| p.position().y).fold(Real::MAX, Real::min); - let max_y = sb.particles().iter().map(|p| p.position().y).fold(Real::MIN, Real::max); - let max_vel = sb.particles().iter().map(|p| p.velocity().length()).fold(0.0, Real::max); + let min_y = sb + .particles() + .iter() + .map(|p| p.position().y) + .fold(Real::MAX, Real::min); + let max_y = sb + .particles() + .iter() + .map(|p| p.position().y) + .fold(Real::MIN, Real::max); + let max_vel = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); let penetration = previous_top + radius - min_y; - assert!(penetration < 0.02, "a body sank by {penetration} into the one below"); + assert!( + penetration < 0.02, + "a body sank by {penetration} into the one below" + ); assert!(max_vel < 0.1, "the stack did not settle: {max_vel} m/s"); previous_top = max_y + radius; } @@ -1777,7 +1828,10 @@ fn fem_thin_feature_self_contacts_stay_calm() { .map(|(a, b)| (*a - *b).length()) .fold(0.0, Real::max); let min_y = settled.iter().map(|p| p.y).fold(Real::MAX, Real::min); - assert!(drift < 1.0e-3, "the bar keeps moving: drift {drift} over 800 steps"); + assert!( + drift < 1.0e-3, + "the bar keeps moving: drift {drift} over 800 steps" + ); assert!(min_y > 0.3 - 0.02, "the bar sank into the ground: {min_y}"); } @@ -1891,8 +1945,7 @@ fn self_contact_blob_survives_grab_crush() { let anchor = world.soft_bodies[victim].particle_position(grabbed as usize); let cluster = world.add_soft_body_cluster(victim, &[grabbed]).unwrap(); let proxy = world.soft_bodies[victim].cluster_proxy(cluster).unwrap(); - let mouse = - world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); + let mouse = world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); let joint: GenericJoint = GenericJointBuilder::new(JointAxesMask::empty()) .motor_position(JointAxis::LinX, 0.0, 1000.0, 50.0) .motor_position(JointAxis::LinY, 0.0, 1000.0, 50.0) @@ -1900,10 +1953,8 @@ fn self_contact_blob_survives_grab_crush() { world.insert_impulse_joint(mouse, proxy, joint); for step in 0..900 { let tt = step as Real / 60.0; - world.bodies[mouse].set_next_kinematic_translation(Vector::new( - anchor.x + 1.5 * (3.0 * tt).sin(), - -8.0, - )); + world.bodies[mouse] + .set_next_kinematic_translation(Vector::new(anchor.x + 1.5 * (3.0 * tt).sin(), -8.0)); world.bodies[proxy].wake_up(true); world.step(); } @@ -1970,7 +2021,10 @@ fn stiff_edge_tears_under_force_not_strain() { } let sb = &world.soft_bodies[handle]; let tested = sb.edges().iter().find(|e| e.vertices == [3, 4]); - let particles: usize = bodies.iter().map(|&h| world.soft_bodies[h].num_particles()).sum(); + let particles: usize = bodies + .iter() + .map(|&h| world.soft_bodies[h].num_particles()) + .sum(); ( particles, bodies.len(), @@ -1980,7 +2034,11 @@ fn stiff_edge_tears_under_force_not_strain() { }; // The load is the weight, 9.81 N. let (particles, pieces, stress, edges) = hang(30.0); - assert_eq!((particles, pieces, edges), (8, 1, 7), "the edge tore under a third of its threshold"); + assert_eq!( + (particles, pieces, edges), + (8, 1, 7), + "the edge tore under a third of its threshold" + ); let stress = stress.unwrap(); assert!( (stress - 9.81 / 30.0).abs() < 0.08, @@ -1990,7 +2048,11 @@ fn stiff_edge_tears_under_force_not_strain() { // splits, its copy stays on the bar as a stub and the weight falls as its own body. Edge // `[3, 4]` now bears the stub alone (three quarters of the split particle's mass). let (particles, pieces, stress, edges) = hang(6.0); - assert_eq!((particles, pieces, edges), (9, 2, 4), "the edge held a load above its tear force"); + assert_eq!( + (particles, pieces, edges), + (9, 2, 4), + "the edge held a load above its tear force" + ); let stress = stress.unwrap(); assert!( (stress - 0.1875 * 9.81 / 6.0).abs() < 0.03, @@ -2018,7 +2080,11 @@ fn tear_smoothing_shrugs_off_a_spike() { family(&world, handle).len() }; assert_eq!(jerk(0.0), 2, "without smoothing the spike snaps the edge"); - assert_eq!(jerk(1.0), 1, "smoothed over a second, the spike is shrugged off"); + assert_eq!( + jerk(1.0), + 1, + "smoothed over a second, the spike is shrugged off" + ); } /// A per-edge tear resistance multiplies the edge's threshold: of two edges bearing the same @@ -2033,14 +2099,32 @@ fn tear_resistance_scales_the_threshold() { positions.extend([Vector::new(0.0, y), Vector::new(1.0, y)]); } let builder = SoftBodyBuilder::new(positions) - .edges(vec![[0, 1], [0, 2], [1, 3], [2, 4], [3, 5], [4, 6], [5, 7], [6, 8], [7, 9]]) - .edge_tear_resistance([(2, 3.0), (3, 100.0), (4, 100.0), (5, 100.0), (6, 100.0), (7, 100.0), (8, 100.0)]) - .pinned_particles([0, 1]) - .particle_mass(0.25) - .softness(SpringCoefficients::new(120.0, 1.0)) - .tear_force(6.0) - .no_surface_collider() - .can_sleep(false); + .edges(vec![ + [0, 1], + [0, 2], + [1, 3], + [2, 4], + [3, 5], + [4, 6], + [5, 7], + [6, 8], + [7, 9], + ]) + .edge_tear_resistance([ + (2, 3.0), + (3, 100.0), + (4, 100.0), + (5, 100.0), + (6, 100.0), + (7, 100.0), + (8, 100.0), + ]) + .pinned_particles([0, 1]) + .particle_mass(0.25) + .softness(SpringCoefficients::new(120.0, 1.0)) + .tear_force(6.0) + .no_surface_collider() + .can_sleep(false); let handle = world.insert_soft_body(builder); for _ in 0..60 { world.step(); @@ -2062,7 +2146,10 @@ fn tear_resistance_scales_the_threshold() { let sb = &world.soft_bodies[handle]; let reinforced = sb.edges().iter().find(|e| e.tear_resistance == 3.0); let stress = reinforced.expect("the reinforced edge tore").stress(); - assert!((stress - 9.81 / 18.0).abs() < 0.08, "stress {stress} ignores the resistance"); + assert!( + (stress - 9.81 / 18.0).abs() < 0.08, + "stress {stress} ignores the resistance" + ); } /// An edge whose particles are all interior and undamaged bears its load against @@ -2128,12 +2215,24 @@ fn interior_strength_shields_undamaged_interior_edges() { }; let plain = run(1.0); let tough = run(4.0); - assert!(plain.0 > 0.05 && plain.1 > 0.02, "the stretched grid reads a load: {plain:?}"); + assert!( + plain.0 > 0.05 && plain.1 > 0.02, + "the stretched grid reads a load: {plain:?}" + ); let close = |a: Real, b: Real| (a - b).abs() <= 1.0e-5 * a.abs().max(1.0); assert!(close(plain.0, tough.0), "a surface edge is not shielded"); - assert!(close(plain.1, 4.0 * tough.1), "an interior edge bears 4x the threshold"); - assert!(close(plain.2, tough.2), "an edge next to damage lost its shield"); - assert!(close(plain.3, 4.0 * tough.3), "an edge far from damage keeps its shield"); + assert!( + close(plain.1, 4.0 * tough.1), + "an interior edge bears 4x the threshold" + ); + assert!( + close(plain.2, tough.2), + "an edge next to damage lost its shield" + ); + assert!( + close(plain.3, 4.0 * tough.3), + "an edge far from damage keeps its shield" + ); } /// A squeezed edge past its yield takes a permanent set (only the excess strain flows), bounded @@ -2159,7 +2258,16 @@ fn edge_plasticity_keeps_a_dent() { Vector::new(0.0, 3.0), Vector::new(1.0, 3.0), ]) - .edges(vec![[0, 1], [2, 3], [0, 4], [1, 5], [4, 6], [5, 7], [6, 8], [7, 9]]) + .edges(vec![ + [0, 1], + [2, 3], + [0, 4], + [1, 5], + [4, 6], + [5, 7], + [6, 8], + [7, 9], + ]) .pinned_particles([0, 1, 3, 4, 5, 6, 7, 8, 9]) .particle_mass(1.0) .material(material) @@ -2187,7 +2295,11 @@ fn edge_plasticity_keeps_a_dent() { let (mut world, handle) = squeeze(clay(SoftEdgePlasticFlow::Both, 1.0)); let sb = &world.soft_bodies[handle]; let e = &sb.edges()[0]; - assert!((e.rest_length - 0.7 / 0.9).abs() < 1.0e-4, "rest length {}", e.rest_length); + assert!( + (e.rest_length - 0.7 / 0.9).abs() < 1.0e-4, + "rest length {}", + e.rest_length + ); assert!((e.initial_rest_length() - 1.0).abs() < 1.0e-4); assert!((e.plastic_strain() - (0.7 / 0.9 - 1.0)).abs() < 1.0e-4); // The rest positions followed the flow: the edge is as long at rest as its rest length. @@ -2212,12 +2324,20 @@ fn edge_plasticity_keeps_a_dent() { // The plastic maximum bounds the set. let (world, handle) = squeeze(clay(SoftEdgePlasticFlow::Both, 0.15)); let e = &world.soft_bodies[handle].edges()[0]; - assert!((e.rest_length - 0.85).abs() < 1.0e-4, "rest length {}", e.rest_length); + assert!( + (e.rest_length - 0.85).abs() < 1.0e-4, + "rest length {}", + e.rest_length + ); // Tension-only clay does not dent. let (world, handle) = squeeze(clay(SoftEdgePlasticFlow::Tension, 1.0)); let e = &world.soft_bodies[handle].edges()[0]; - assert!((e.rest_length - 1.0).abs() < 1.0e-6, "rest length {}", e.rest_length); + assert!( + (e.rest_length - 1.0).abs() < 1.0e-6, + "rest length {}", + e.rest_length + ); // Compression-only clay dents, then springs back from a stretch (the rest stays where it // flowed to), and the tear strain is measured against the initial length: 20% past it is @@ -2229,7 +2349,11 @@ fn edge_plasticity_keeps_a_dent() { } let sb = &world.soft_bodies[handle]; let e = &sb.edges()[0]; - assert!((e.rest_length - 0.7 / 0.9).abs() < 1.0e-4, "a stretch flowed: {}", e.rest_length); + assert!( + (e.rest_length - 0.7 / 0.9).abs() < 1.0e-4, + "a stretch flowed: {}", + e.rest_length + ); let stress = e.stress(); assert!( (stress - 0.2 / 0.25).abs() < 0.02, @@ -2242,7 +2366,11 @@ fn edge_plasticity_keeps_a_dent() { } let sb = &world.soft_bodies[handle]; sb.validate_topology().unwrap(); - assert_eq!(sb.num_particles(), 10, "an edge between two pinned particles tore"); + assert_eq!( + sb.num_particles(), + 10, + "an edge between two pinned particles tore" + ); let stress = sb.edges()[0].stress(); assert!( (stress - 0.3 / 0.25).abs() < 0.02, @@ -2300,7 +2428,10 @@ fn tear_events_and_rope_ends() { fallen.validate_topology().unwrap(); assert_eq!(fallen.origin(), Some(handle)); assert_eq!((fallen.num_particles(), fallen.edges().len()), (4, 3)); - assert_eq!(world.soft_bodies[handle].pieces(), &[event.pieces[1].soft_body]); + assert_eq!( + world.soft_bodies[handle].pieces(), + &[event.pieces[1].soft_body] + ); } let sb = &world.soft_bodies[handle]; sb.validate_topology().unwrap(); @@ -2309,8 +2440,7 @@ fn tear_events_and_rope_ends() { // The pinned piece is three segments: any split would leave a side below the minimum. let pinned: Vec = (0..sb.edges().len() as u32).collect(); assert!( - world.tear_soft_body(handle, &pinned, &[]) - .is_none(), + world.tear_soft_body(handle, &pinned, &[]).is_none(), "a piece of three segments tore" ); assert_eq!(world.soft_bodies[handle].num_particles(), 4); @@ -2358,7 +2488,8 @@ fn torn_builder_rope_separates() { .iter() .position(|e| e.vertices == [4, 5] && e.kind == SoftBodyEdgeKind::Structural) .unwrap() as u32; - let event = world.tear_soft_body(handle, &[torn], &[]) + let event = world + .tear_soft_body(handle, &[torn], &[]) .expect("the rope tore"); // Particle 4 splits (the lower index), and the bending edge (3, 5) over it goes. assert_eq!(event.split_particles, vec![(9, 4)]); @@ -2378,9 +2509,15 @@ fn torn_builder_rope_separates() { let measure = sb.rest_measure() + lb.rest_measure(); assert!((mass - before.mass()).abs() <= 1.0e-5 * before.mass()); assert!((measure - before.rest_measure()).abs() <= 1.0e-5 * before.rest_measure()); - assert_eq!(sb.edges().len() + lb.edges().len(), before.edges().len() - 1); + assert_eq!( + sb.edges().len() + lb.edges().len(), + before.edges().len() - 1 + ); assert_eq!(num_bends(sb) + num_bends(lb), num_bends(&before) - 1); - assert_eq!(sb.boundary().len() + lb.boundary().len(), before.boundary().len()); + assert_eq!( + sb.boundary().len() + lb.boundary().len(), + before.boundary().len() + ); assert_eq!(sb.connected_pieces().len(), 1); assert_eq!(lb.connected_pieces().len(), 1); @@ -2430,7 +2567,8 @@ fn tears_keep_every_particle_in_an_element() { world.tear_soft_body(grid, &corner, &[]); let ring = edges_at(&world.soft_bodies[disk], Vector::new(5.6, 2.0), true); assert_eq!(ring.len(), 2); - world.tear_soft_body(disk, &ring, &[]) + world + .tear_soft_body(disk, &ring, &[]) .expect("the ring tore"); let all_edges: Vec = (0..world.soft_bodies[rope].edges().len() as u32).collect(); world.tear_soft_body(rope, &all_edges, &[]); @@ -2438,7 +2576,10 @@ fn tears_keep_every_particle_in_an_element() { for _ in 0..60 { world.step(); } - for handle in [grid, disk, rope].into_iter().flat_map(|h| family(&world, h)) { + for handle in [grid, disk, rope] + .into_iter() + .flat_map(|h| family(&world, h)) + { let sb = &world.soft_bodies[handle]; sb.validate_topology().unwrap(); assert!(sb.particles().iter().all(|p| p.position().is_finite())); @@ -2485,8 +2626,8 @@ fn cuts_keep_every_particle_in_an_element() { fn cut_splits_a_grid_along_a_segment() { let mut world = world_with_ground(); let n = 5usize; - let grid = SoftBodyBuilder::grid(Vector::new(0.0, 2.0), Vector::splat(1.0), n, n) - .particle_mass(0.2); + let grid = + SoftBodyBuilder::grid(Vector::new(0.0, 2.0), Vector::splat(1.0), n, n).particle_mass(0.2); let handle = world.insert_soft_body(grid); world.step(); let blade = [Vector::new(0.25, -5.0), Vector::new(0.25, 10.0)]; @@ -2646,11 +2787,23 @@ fn impulses_fall_off_with_the_distance() { let sb = &mut world.soft_bodies[handle]; sb.apply_impulse_at_point(Vector::new(0.0, 4.0), Vector::ZERO, 2.0, true); let v = |i: usize| sb.particle_velocity(i); - assert!((v(0) - Vector::new(0.0, 4.0)).length() < 1.0e-6, "on the point: {:?}", v(0)); - assert!((v(1) - Vector::new(0.0, 1.0)).length() < 1.0e-6, "halfway, twice the mass: {:?}", v(1)); + assert!( + (v(0) - Vector::new(0.0, 4.0)).length() < 1.0e-6, + "on the point: {:?}", + v(0) + ); + assert!( + (v(1) - Vector::new(0.0, 1.0)).length() < 1.0e-6, + "halfway, twice the mass: {:?}", + v(1) + ); assert_eq!(v(2), Vector::ZERO, "at the radius"); assert_eq!(v(3), Vector::ZERO, "pinned"); - assert!((v(4) - Vector::new(0.0, 2.0)).length() < 1.0e-6, "halfway: {:?}", v(4)); + assert!( + (v(4) - Vector::new(0.0, 2.0)).length() < 1.0e-6, + "halfway: {:?}", + v(4) + ); for i in 0..sb.num_particles() { sb.set_particle_velocity(i, Vector::ZERO); @@ -2658,10 +2811,18 @@ fn impulses_fall_off_with_the_distance() { sb.apply_radial_impulse(Vector::ZERO, 4.0, 2.0, true); let v = |i: usize| sb.particle_velocity(i); assert_eq!(v(0), Vector::ZERO, "on the center: no direction"); - assert!((v(1) - Vector::new(1.0, 0.0)).length() < 1.0e-6, "pushed right: {:?}", v(1)); + assert!( + (v(1) - Vector::new(1.0, 0.0)).length() < 1.0e-6, + "pushed right: {:?}", + v(1) + ); assert_eq!(v(2), Vector::ZERO, "at the radius"); assert_eq!(v(3), Vector::ZERO, "pinned"); - assert!((v(4) - Vector::new(-2.0, 0.0)).length() < 1.0e-6, "pushed left: {:?}", v(4)); + assert!( + (v(4) - Vector::new(-2.0, 0.0)).length() < 1.0e-6, + "pushed left: {:?}", + v(4) + ); // Without a falloff radius every free particle gets the whole impulse. for i in 0..sb.num_particles() { @@ -2725,8 +2886,14 @@ fn modify_solver_contacts_edits_soft_soft_contacts() { }; let resting = scene(false); let dropped = scene(true); - assert!(resting > 1.1, "the blob did not rest on the rope: y = {resting}"); - assert!(dropped < 0.6, "the hook did not drop the soft-soft contacts: y = {dropped}"); + assert!( + resting > 1.1, + "the blob did not rest on the rope: y = {resting}" + ); + assert!( + dropped < 0.6, + "the hook did not drop the soft-soft contacts: y = {dropped}" + ); } /// A rope shot tip first at a fixed wall stops at the wall: an open polyline collides from @@ -2761,7 +2928,10 @@ fn a_rope_shot_at_a_wall_stops_tip_first() { assert_finite(&world, handle); // The wall's face is at x = 2.5; the rope's skin is half a segment thick. assert!(max_x < 2.5, "the rope went into the wall: max x = {max_x}"); - assert!(max_x > 2.3, "the rope never reached the wall: max x = {max_x}"); + assert!( + max_x > 2.3, + "the rope never reached the wall: max x = {max_x}" + ); } /// A rope released hanging over a small fixed ball rests on it: the segments beside the ball @@ -3049,7 +3219,10 @@ mod tear_and_cut_conserve_mass_and_measure { #[test] fn grid_cut() { - check(grid(), cut([Vector::new(0.25, -5.0), Vector::new(0.25, 5.0)])); + check( + grid(), + cut([Vector::new(0.25, -5.0), Vector::new(0.25, 5.0)]), + ); } #[test] @@ -3064,12 +3237,17 @@ mod tear_and_cut_conserve_mass_and_measure { #[test] fn polyline_edge_tear() { - check(strip(), |w, h| tear_edge_near(w, h, Vector::new(0.875, 0.0))); + check(strip(), |w, h| { + tear_edge_near(w, h, Vector::new(0.875, 0.0)) + }); } #[test] fn polyline_cut() { - check(strip(), cut([Vector::new(0.8, -1.0), Vector::new(0.8, 1.0)])); + check( + strip(), + cut([Vector::new(0.8, -1.0), Vector::new(0.8, 1.0)]), + ); } #[test] @@ -3165,7 +3343,10 @@ fn tangles(sb: &SoftBody) -> (usize, usize) { .iter() .filter(|c| { let now = area(c.vertices.map(|v| sb.particle_position(v as usize))); - let rest = area(c.vertices.map(|v| sb.particles()[v as usize].rest_position())); + let rest = area( + c.vertices + .map(|v| sb.particles()[v as usize].rest_position()), + ); now * rest <= 0.0 }) .count(); @@ -3250,7 +3431,11 @@ fn crack_faces_collide_when_squeezed_shut() { .split_particles .iter() .map(|&(copy, source)| { - let (l, r) = if left[source as usize] { (source, copy) } else { (copy, source) }; + let (l, r) = if left[source as usize] { + (source, copy) + } else { + (copy, source) + }; sb.particle_position(r as usize).x - sb.particle_position(l as usize).x }) .fold(Real::MAX, Real::min) @@ -3259,7 +3444,11 @@ fn crack_faces_collide_when_squeezed_shut() { for _ in 0..60 { world.step(); } - assert!(gap(&world) > 0.05, "the crack did not open: {}", gap(&world)); + assert!( + gap(&world) > 0.05, + "the crack did not open: {}", + gap(&world) + ); assert_eq!(tangles(&world.soft_bodies[handle]), (0, 0)); push(&mut world, 4.0); @@ -3304,7 +3493,10 @@ fn cut_halves_keep_their_own_areas() { assert_eq!(sb.volume_pieces().len(), 1); rest.push(sb.volume_pieces()[0].rest_volume()); } - assert!(rest.iter().all(|r| *r > 0.5), "a half is too small: {rest:?}"); + assert!( + rest.iter().all(|r| *r > 0.5), + "a half is too small: {rest:?}" + ); assert!((rest[0] + rest[1] - rest_volume).abs() < 1.0e-4 * rest_volume); // A heavy lid presses both halves into the ground. @@ -3350,7 +3542,10 @@ fn cut_halves_rest_as_cut_and_collide_when_shoved() { world.step(); for &h in &halves { let sb = &world.soft_bodies[h]; - let speed = sb.particle_velocities().map(|v| v.length()).fold(0.0, Real::max); + let speed = sb + .particle_velocities() + .map(|v| v.length()) + .fold(0.0, Real::max); assert!(speed < 1.0e-3, "the halves moved on their own: {speed} m/s"); } } @@ -3371,5 +3566,8 @@ fn cut_halves_rest_as_cut_and_collide_when_shoved() { } } let pushed_back = (offset(&world) - rest_offset) * sign; - assert!(pushed_back > -0.05, "the shoved halves stayed interlocked: {pushed_back}"); + assert!( + pushed_back > -0.05, + "the shoved halves stayed interlocked: {pushed_back}" + ); } diff --git a/crates/rapier2d/tests/soft_body_clusters.rs b/crates/rapier2d/tests/soft_body_clusters.rs index 74bcc7ef2..e52496c6e 100644 --- a/crates/rapier2d/tests/soft_body_clusters.rs +++ b/crates/rapier2d/tests/soft_body_clusters.rs @@ -19,8 +19,8 @@ fn torn_cluster_splits_and_its_joint_follows_the_source() { let h = world.insert_soft_body(builder); let cluster = world.add_soft_body_cluster(h, &[4]).unwrap(); let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); - let anchor = - world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(Vector::X * 4.0)); + let anchor = world + .insert_body(RigidBodyBuilder::kinematic_position_based().translation(Vector::X * 4.0)); let joint = world.insert_impulse_joint( anchor, proxy, @@ -53,13 +53,19 @@ fn torn_cluster_splits_and_its_joint_follows_the_source() { assert_eq!(splits.len(), 2, "{:?}", event.clusters); let kept = splits.iter().find(|c| c.keeps_proxy).unwrap(); let fresh = splits.iter().find(|c| !c.keeps_proxy).unwrap(); - assert_eq!((kept.soft_body, kept.cluster, kept.proxy), (h, cluster, proxy)); + assert_eq!( + (kept.soft_body, kept.cluster, kept.proxy), + (h, cluster, proxy) + ); assert_eq!(fresh.soft_body, lh); let (rb, lb) = (&world.soft_bodies[h], &world.soft_bodies[lh]); assert_eq!(rb.cluster(kept.cluster).unwrap().particles(), &[4]); assert_eq!(lb.cluster(fresh.cluster).unwrap().particles(), &[4]); assert_eq!(world.bodies[fresh.proxy].soft_body(), Some(lh)); - assert_eq!(world.bodies[fresh.proxy].soft_cluster(), Some(fresh.cluster)); + assert_eq!( + world.bodies[fresh.proxy].soft_cluster(), + Some(fresh.cluster) + ); assert!(rb.cluster(kept.cluster).unwrap().meshes().next().is_none()); // The left body took the fresh pieces of both clusters (made in cluster order). assert_eq!(left.clusters, vec![[2, 0], [3, 1]]); @@ -69,7 +75,10 @@ fn torn_cluster_splits_and_its_joint_follows_the_source() { // source's wins the tie, and the anchor stays where the particle is. assert_eq!(event.moved_joints.len(), 1); let moved = event.moved_joints[0]; - assert_eq!((moved.joint, moved.from, moved.to), (joint, proxy, fresh.proxy)); + assert_eq!( + (moved.joint, moved.from, moved.to), + (joint, proxy, fresh.proxy) + ); let j = world.impulse_joints.get(joint).unwrap(); assert_eq!((j.body1(), j.body2()), (anchor, fresh.proxy)); let world_anchor = *world.bodies[fresh.proxy].position() * j.data.local_frame2.translation; diff --git a/crates/rapier2d/tests/soft_contact_debug_render.rs b/crates/rapier2d/tests/soft_contact_debug_render.rs index 9493fc5c8..af5842af4 100644 --- a/crates/rapier2d/tests/soft_contact_debug_render.rs +++ b/crates/rapier2d/tests/soft_contact_debug_render.rs @@ -151,7 +151,10 @@ fn soft_elements_are_colored_by_their_load() { } let sb = &world.soft_bodies[handle]; let loaded = sb.edges()[0].stress(); - assert!((loaded - 0.5).abs() < 0.05, "edge 0 bears {loaded} of its threshold"); + assert!( + (loaded - 0.5).abs() < 0.05, + "edge 0 bears {loaded} of its threshold" + ); assert!(sb.edges()[1].stress().abs() < 1.0e-6); let style = DebugRenderStyle::default(); @@ -164,14 +167,22 @@ fn soft_elements_are_colored_by_their_load() { // The lines come out sorted by particle pair: edge 0 first. let plain = render(DebugRenderMode::SOFT_BODIES); assert_eq!(plain.len(), 2); - assert!(plain.iter().all(|(.., c)| *c == style.soft_body_element_color)); + assert!( + plain + .iter() + .all(|(.., c)| *c == style.soft_body_element_color) + ); let by_load = render(DebugRenderMode::SOFT_BODIES | DebugRenderMode::SOFT_BODY_STRESS); assert_eq!(by_load.len(), 2); let (slack, full) = (style.soft_body_slack_color, style.soft_body_loaded_color); let expected: DebugColor = core::array::from_fn(|k| slack[k] + (full[k] - slack[k]) * loaded); for k in 0..4 { - assert!((by_load[0].2[k] - expected[k]).abs() < 1.0e-4, "loaded edge color {:?}", by_load[0].2); + assert!( + (by_load[0].2[k] - expected[k]).abs() < 1.0e-4, + "loaded edge color {:?}", + by_load[0].2 + ); } assert_eq!(by_load[1].2, slack); } diff --git a/crates/rapier3d/tests/parallel_path_parity.rs b/crates/rapier3d/tests/parallel_path_parity.rs index 66b12cd5b..ecf81018a 100644 --- a/crates/rapier3d/tests/parallel_path_parity.rs +++ b/crates/rapier3d/tests/parallel_path_parity.rs @@ -26,7 +26,7 @@ use rapier3d::prelude::*; /// Golden hash of [`run`]. Identical in every build; re-mint (with a note saying why) /// only when a change is *meant* to alter the simulation. -const GOLDEN: u64 = 0x7c57_2d38_0b87_2d4c; +const GOLDEN: u64 = 0xcc9e_c93f_2c15_65cc; /// FNV-1a. struct Fnv(u64); diff --git a/crates/rapier3d/tests/snapshot_portability.rs b/crates/rapier3d/tests/snapshot_portability.rs index 23d8b1c71..df1f3876d 100644 --- a/crates/rapier3d/tests/snapshot_portability.rs +++ b/crates/rapier3d/tests/snapshot_portability.rs @@ -31,7 +31,7 @@ use rapier3d::prelude::*; /// Snapshot size in bytes, and its FNV-1a digest. Both, because the size alone localizes a /// failure: a differing size means a container's *encoding* changed, an equal size with a /// differing digest means the values did. -const GOLDEN: (usize, u64) = (488_334, 0x50d4_0720_b93e_831b); +const GOLDEN: (usize, u64) = (488_334, 0x3f5e_ac53_ad07_5243); const STEPS: usize = 60; diff --git a/crates/rapier3d/tests/soft_bodies.rs b/crates/rapier3d/tests/soft_bodies.rs index cc05de001..177648447 100644 --- a/crates/rapier3d/tests/soft_bodies.rs +++ b/crates/rapier3d/tests/soft_bodies.rs @@ -911,7 +911,10 @@ fn box_rests_on_surface_and_nothing_creeps() { } assert!(world.bodies[rb].linvel().length() < 0.01); // The soft body's surface contacts report their force through the manifolds. - let surface = world.soft_bodies[handle].collision_mesh().unwrap().collider(); + let surface = world.soft_bodies[handle] + .collision_mesh() + .unwrap() + .collider(); let total_impulse: Real = world .narrow_phase .contact_pairs_with(surface) @@ -1484,7 +1487,9 @@ fn plastic_cells_keep_their_deformation() { .filter(|&i| sb.particles()[i].initial_rest_position().x > 0.74) .collect(); let mean = |set: &[usize]| { - set.iter().map(|&i| sb.particles()[i].rest_position()).sum::() + set.iter() + .map(|&i| sb.particles()[i].rest_position()) + .sum::() / set.len() as Real }; let vertical = mean(&top) - mean(&bottom); @@ -1729,8 +1734,14 @@ fn unoriented_balloon_holds_a_ball_inside() { } assert_finite(&world, handle); let sb = &world.soft_bodies[handle]; - let lowest = sb.particle_positions().map(|p| p.y).fold(Real::MAX, Real::min); - (world.bodies[ball].translation() - sb.center_of_mass(), lowest) + let lowest = sb + .particle_positions() + .map(|p| p.y) + .fold(Real::MAX, Real::min); + ( + world.bodies[ball].translation() - sb.center_of_mass(), + lowest, + ) }; let (offset, lowest) = run(false); assert!( @@ -2108,9 +2119,15 @@ fn cloth_tears_when_pulled_apart() { } let bodies = family(&world, handle); assert!(bodies.len() >= 2, "the cloth is still in one piece"); - let surface: usize = bodies.iter().map(|&h| world.soft_bodies[h].boundary().len()).sum(); + let surface: usize = bodies + .iter() + .map(|&h| world.soft_bodies[h].boundary().len()) + .sum(); assert_eq!(surface, num_surface, "the torn cloth lost triangles"); - let particles: usize = bodies.iter().map(|&h| world.soft_bodies[h].num_particles()).sum(); + let particles: usize = bodies + .iter() + .map(|&h| world.soft_bodies[h].num_particles()) + .sum(); assert!(particles > nu * nv, "the crack did not split any particle"); // The two pinned columns are now more than twice the rest width apart: what still ties // them would be strained way past the threshold, so nothing does. The middle sagged under @@ -2171,7 +2188,9 @@ fn tearing_splits_the_particles_a_crack_passes_through() { .map(|(i, _)| i as u32) .collect(); assert_eq!(warp.len(), n); - let event = world.tear_soft_body(handle, &warp, &[]).expect("nothing tore"); + let event = world + .tear_soft_body(handle, &warp, &[]) + .expect("nothing tore"); // One split per torn edge (its first endpoint, `(col, j)`), the triangles all still there, // and the cloth in two bodies: the wider left piece keeps the handle. assert_eq!(event.pieces.len(), 2); @@ -2254,8 +2273,7 @@ fn jelly_bar_tears_when_pulled_apart() { .map(|&i| world.soft_bodies[handle].particle_position(i)) .collect(); // The driven particles follow the tears: the piece they are in becomes its own body. - let mut right: Vec<(SoftBodyHandle, u32)> = - right.iter().map(|&i| (handle, i as u32)).collect(); + let mut right: Vec<(SoftBodyHandle, u32)> = right.iter().map(|&i| (handle, i as u32)).collect(); let log = TearLog::default(); for k in 0..240 { let shift = 2.4 * (k as Real / 180.0).min(1.0); @@ -2275,9 +2293,15 @@ fn jelly_bar_tears_when_pulled_apart() { } let bodies = family(&world, handle); assert!(bodies.len() >= 2, "the bar is still in one piece"); - let cells: usize = bodies.iter().map(|&h| world.soft_bodies[h].cells().len()).sum(); + let cells: usize = bodies + .iter() + .map(|&h| world.soft_bodies[h].cells().len()) + .sum(); assert_eq!(cells, num_cells, "a tear removed cells"); - let particles: usize = bodies.iter().map(|&h| world.soft_bodies[h].num_particles()).sum(); + let particles: usize = bodies + .iter() + .map(|&h| world.soft_bodies[h].num_particles()) + .sum(); assert!(particles > num_particles, "no particle split"); // The bar is now torn: no remaining cell is stretched past twice the tear strain. let mut max_stretch: Real = 0.0; @@ -2335,10 +2359,7 @@ fn torn_soft_bodies_serialize() { }) .map(|(i, _)| i as u32) .collect(); - assert!( - world.tear_soft_body(handle, &diagonal, &[]) - .is_some() - ); + assert!(world.tear_soft_body(handle, &diagonal, &[]).is_some()); for _ in 0..30 { world.step(); } @@ -2994,9 +3015,7 @@ fn skinned_bodies_collide_with_each_other() { let cube = world.insert_soft_body(cube); let sphere = world.insert_soft_body(sphere); assert_eq!( - world.soft_bodies[cube] - .meshes() - .any(|m| m.is_skinned()), + world.soft_bodies[cube].meshes().any(|m| m.is_skinned()), skinned ); for _ in 0..400 { @@ -3335,7 +3354,11 @@ fn fem_deflection_is_substep_invariant() { let mut world = PhysicsWorld::new(); world.integration_parameters.num_solver_iterations = substeps; // The invariance is a property of the converged linear solve: lift the iteration cap. - world.integration_parameters.soft_bodies.fem.max_linear_iterations = 256; + world + .integration_parameters + .soft_bodies + .fem + .max_linear_iterations = 256; let (builder, tip) = fem_cantilever(solver, young, length, thickness, 9, 3); let handle = world.insert_soft_body(builder.mass(mass)); for _ in 0..1500 { @@ -3624,11 +3647,26 @@ fn fem_stiff_stack_rests_without_sinking() { for &h in &handles { let sb = &world.soft_bodies[h]; assert_finite(&world, h); - let min_y = sb.particles().iter().map(|p| p.position().y).fold(Real::MAX, Real::min); - let max_y = sb.particles().iter().map(|p| p.position().y).fold(Real::MIN, Real::max); - let max_vel = sb.particles().iter().map(|p| p.velocity().length()).fold(0.0, Real::max); + let min_y = sb + .particles() + .iter() + .map(|p| p.position().y) + .fold(Real::MAX, Real::min); + let max_y = sb + .particles() + .iter() + .map(|p| p.position().y) + .fold(Real::MIN, Real::max); + let max_vel = sb + .particles() + .iter() + .map(|p| p.velocity().length()) + .fold(0.0, Real::max); let penetration = previous_top + radius - min_y; - assert!(penetration < 0.02, "a cube sank by {penetration} into the one below"); + assert!( + penetration < 0.02, + "a cube sank by {penetration} into the one below" + ); assert!(max_vel < 0.1, "the stack did not settle: {max_vel} m/s"); previous_top = max_y + radius; } @@ -3987,12 +4025,18 @@ fn fem_cloth_tears_when_pulled_apart() { .iter() .map(|&h| world.soft_bodies[h].num_particles()) .sum(); - assert!(particles > particles_before, "the crack did not split any particle"); + assert!( + particles > particles_before, + "the crack did not split any particle" + ); let triangles: usize = bodies .iter() .map(|&h| world.soft_bodies[h].boundary().len()) .sum(); - assert_eq!(triangles, triangles_before, "the torn FEM cloth lost triangles"); + assert_eq!( + triangles, triangles_before, + "the torn FEM cloth lost triangles" + ); for &h in &bodies { assert_finite(&world, h); world.soft_bodies[h].validate_topology().unwrap(); @@ -4178,8 +4222,14 @@ fn tears_keep_every_particle_in_an_element() { .pinned_particles([5]), ); let torn_box = world.insert_soft_body( - SoftBodyBuilder::cuboid(Vector::new(4.0, 2.0, 0.0), Vector::new(1.0, 0.5, 0.5), 5, 3, 3) - .particle_mass(0.2), + SoftBodyBuilder::cuboid( + Vector::new(4.0, 2.0, 0.0), + Vector::new(1.0, 0.5, 0.5), + 5, + 3, + 3, + ) + .particle_mass(0.2), ); let rope = world.insert_soft_body( SoftBodyBuilder::rope(Vector::new(0.0, 3.0, 4.0), Vector::new(2.0, 3.0, 4.0), 6) @@ -4194,7 +4244,8 @@ fn tears_keep_every_particle_in_an_element() { }; let sb = &world.soft_bodies[cloth]; let corner_edges = touching(sb.edges().iter().map(|e| e.vertices.contains(&0)).collect()); - let event = world.tear_soft_body(cloth, &corner_edges, &[]) + let event = world + .tear_soft_body(cloth, &corner_edges, &[]) .expect("the corner edges tore"); // The torn edges reported are the ones a crack opened across, before the tear. assert!(!event.torn_edges.is_empty() && event.torn_edges.iter().all(|e| e.contains(&0))); @@ -4220,8 +4271,14 @@ fn tears_keep_every_particle_in_an_element() { fn cuts_keep_every_particle_in_an_element() { let mut world = world_with_ground(); let cut_box = world.insert_soft_body( - SoftBodyBuilder::cuboid(Vector::new(-4.0, 2.0, 0.0), Vector::new(1.0, 0.5, 0.5), 5, 3, 3) - .particle_mass(0.2), + SoftBodyBuilder::cuboid( + Vector::new(-4.0, 2.0, 0.0), + Vector::new(1.0, 0.5, 0.5), + 5, + 3, + 3, + ) + .particle_mass(0.2), ); world.step(); // A triangle in the plane through the middle of the corner cube of the grid, across the @@ -4301,8 +4358,14 @@ fn modify_solver_contacts_edits_soft_soft_contacts() { }; let resting = scene(false); let dropped = scene(true); - assert!(resting > 1.1, "the jelly did not rest on the cloth: y = {resting}"); - assert!(dropped < 0.5, "the hook did not drop the soft-soft contacts: y = {dropped}"); + assert!( + resting > 1.1, + "the jelly did not rest on the cloth: y = {resting}" + ); + assert!( + dropped < 0.5, + "the hook did not drop the soft-soft contacts: y = {dropped}" + ); } /// A cloth draped over a rigid ball much larger than its elements rests on it: the ball's @@ -4401,7 +4464,10 @@ fn small_ball_slides_over_cloth_without_ghost_bumps() { "the ball bumps on the cloth's vertices: height range {}, vertical speed {max_vy}", max_y - min_y ); - assert!(x > 0.6, "the cloth's vertices slowed the ball down: x = {x}"); + assert!( + x > 0.6, + "the cloth's vertices slowed the ball down: x = {x}" + ); } /// A tear or a cut never deletes material: the mass and the rest measure (cell volumes, surface @@ -4693,7 +4759,10 @@ fn cut_halves_rest_as_cut_and_collide_when_shoved() { world.step(); for &h in &halves { let sb = &world.soft_bodies[h]; - let speed = sb.particle_velocities().map(|v| v.length()).fold(0.0, Real::max); + let speed = sb + .particle_velocities() + .map(|v| v.length()) + .fold(0.0, Real::max); assert!(speed < 1.0e-3, "the halves moved on their own: {speed} m/s"); } } @@ -4714,5 +4783,8 @@ fn cut_halves_rest_as_cut_and_collide_when_shoved() { } } let pushed_back = (offset(&world) - rest_offset) * sign; - assert!(pushed_back > -0.05, "the shoved halves stayed interlocked: {pushed_back}"); + assert!( + pushed_back > -0.05, + "the shoved halves stayed interlocked: {pushed_back}" + ); } diff --git a/crates/rapier3d/tests/soft_body_clusters.rs b/crates/rapier3d/tests/soft_body_clusters.rs index 03c9d5885..59a6d3d18 100644 --- a/crates/rapier3d/tests/soft_body_clusters.rs +++ b/crates/rapier3d/tests/soft_body_clusters.rs @@ -38,10 +38,7 @@ fn root_body_is_the_whole_body_cluster_proxy() { let sb = &world.soft_bodies[h]; assert_eq!(sb.num_live_clusters(), 1); assert_eq!(sb.cluster_proxy(0), Some(sb.root_body())); - assert_eq!( - sb.cluster(0).unwrap().particles().len(), - sb.num_particles() - ); + assert_eq!(sb.cluster(0).unwrap().particles().len(), sb.num_particles()); let rb = &world.bodies[sb.root_body()]; assert_eq!(rb.body_type(), RigidBodyType::SoftFrame); assert!(rb.is_soft_frame()); @@ -342,7 +339,7 @@ fn user_collider_on_a_proxy_rides_the_frame() { /// Per-cluster shape matching: two clusters of one cloth are held rigid independently while the /// rest of the cloth stays soft. #[test] -fn per_cluster_shape_matching_holds_two_regions() { +fn per_cluster_shape_matching_holds_two_regions() { let mut world = world_with_ground(); let h = world.insert_soft_body( SoftBodyBuilder::cloth( @@ -511,7 +508,10 @@ fn pinned_cluster_drives_its_region_kinematically() { ); // The free part of the cloth was dragged along. let bottom = world.soft_bodies[h].particle_position(32); - assert!(bottom.x > 0.7, "the cloth did not follow its grip ({bottom:?})"); + assert!( + bottom.x > 0.7, + "the cloth did not follow its grip ({bottom:?})" + ); for p in world.soft_bodies[h].particles() { assert!(p.position().is_finite()); } @@ -657,8 +657,8 @@ fn torn_cluster_splits_and_its_joint_follows_the_source() { let h = world.insert_soft_body(builder); let cluster = world.add_soft_body_cluster(h, &[4]).unwrap(); let proxy = world.soft_bodies[h].cluster_proxy(cluster).unwrap(); - let anchor = - world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(Vector::X * 4.0)); + let anchor = world + .insert_body(RigidBodyBuilder::kinematic_position_based().translation(Vector::X * 4.0)); let joint = world.insert_impulse_joint( anchor, proxy, @@ -691,13 +691,19 @@ fn torn_cluster_splits_and_its_joint_follows_the_source() { assert_eq!(splits.len(), 2, "{:?}", event.clusters); let kept = splits.iter().find(|c| c.keeps_proxy).unwrap(); let fresh = splits.iter().find(|c| !c.keeps_proxy).unwrap(); - assert_eq!((kept.soft_body, kept.cluster, kept.proxy), (h, cluster, proxy)); + assert_eq!( + (kept.soft_body, kept.cluster, kept.proxy), + (h, cluster, proxy) + ); assert_eq!(fresh.soft_body, lh); let (rb, lb) = (&world.soft_bodies[h], &world.soft_bodies[lh]); assert_eq!(rb.cluster(kept.cluster).unwrap().particles(), &[4]); assert_eq!(lb.cluster(fresh.cluster).unwrap().particles(), &[4]); assert_eq!(world.bodies[fresh.proxy].soft_body(), Some(lh)); - assert_eq!(world.bodies[fresh.proxy].soft_cluster(), Some(fresh.cluster)); + assert_eq!( + world.bodies[fresh.proxy].soft_cluster(), + Some(fresh.cluster) + ); assert!(rb.cluster(kept.cluster).unwrap().meshes().next().is_none()); // The left body took the fresh pieces of both clusters (made in cluster order). assert_eq!(left.clusters, vec![[2, 0], [3, 1]]); @@ -707,7 +713,10 @@ fn torn_cluster_splits_and_its_joint_follows_the_source() { // source's wins the tie, and the anchor stays where the particle is. assert_eq!(event.moved_joints.len(), 1); let moved = event.moved_joints[0]; - assert_eq!((moved.joint, moved.from, moved.to), (joint, proxy, fresh.proxy)); + assert_eq!( + (moved.joint, moved.from, moved.to), + (joint, proxy, fresh.proxy) + ); let j = world.impulse_joints.get(joint).unwrap(); assert_eq!((j.body1(), j.body2()), (anchor, fresh.proxy)); let world_anchor = *world.bodies[fresh.proxy].position() * j.data.local_frame2.translation; diff --git a/crates/rapier3d/tests/soft_body_joints.rs b/crates/rapier3d/tests/soft_body_joints.rs index 4b4118f7c..39ac1271a 100644 --- a/crates/rapier3d/tests/soft_body_joints.rs +++ b/crates/rapier3d/tests/soft_body_joints.rs @@ -85,7 +85,8 @@ fn rope_joint_limits_a_jelly_fall() { let mut world = PhysicsWorld::new(); let h = jelly_at(&mut world, Vector::new(0.0, 0.0, 0.0)); let root = world.soft_bodies[h].root_body(); - let anchor = world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(0.0, 2.0, 0.0))); + let anchor = + world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(0.0, 2.0, 0.0))); let joint = RopeJointBuilder::new(3.0) .local_anchor1(Vector::ZERO) .local_anchor2(Vector::ZERO); @@ -299,10 +300,7 @@ fn soft_cluster_jointed_to_a_multibody() { } // The jelly hangs from the arm instead of free-falling. let com = world.soft_bodies[h].center_of_mass(); - assert!( - com.y > 0.5, - "the jelly fell as if unjointed (com {com:?})" - ); + assert!(com.y > 0.5, "the jelly fell as if unjointed (com {com:?})"); assert!(com.is_finite()); } @@ -327,7 +325,8 @@ fn joint_on_a_fem_body_holds() { .surface_collider(ColliderBuilder::ball(0.05)), ); let root = world.soft_bodies[h].root_body(); - let anchor = world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(0.0, 2.0, 0.0))); + let anchor = + world.insert_body(RigidBodyBuilder::fixed().translation(Vector::new(0.0, 2.0, 0.0))); let joint = RopeJointBuilder::new(3.0) .local_anchor1(Vector::ZERO) .local_anchor2(Vector::ZERO); @@ -360,7 +359,10 @@ fn quarantine_contains_a_nan_on_a_jointed_cluster() { for _ in 0..10 { world.step(); } - assert!(!world.soft_bodies[h].is_enabled(), "the NaN body must be quarantined"); + assert!( + !world.soft_bodies[h].is_enabled(), + "the NaN body must be quarantined" + ); assert!(world.bodies[partner].position().translation.is_finite()); } diff --git a/crates/rapier3d/tests/soft_body_meshes.rs b/crates/rapier3d/tests/soft_body_meshes.rs index 9b31e20b3..7187a6996 100644 --- a/crates/rapier3d/tests/soft_body_meshes.rs +++ b/crates/rapier3d/tests/soft_body_meshes.rs @@ -170,7 +170,14 @@ fn a_mesh_bound_outside_its_cluster_is_rejected() { matches!(err, SoftBindingError::UnmatchedVertex { .. }), "{err}" ); - assert_eq!(world.soft_bodies[handle].cluster(cluster).unwrap().meshes().count(), 0); + assert_eq!( + world.soft_bodies[handle] + .cluster(cluster) + .unwrap() + .meshes() + .count(), + 0 + ); } #[test] @@ -321,14 +328,13 @@ fn a_mesh_on_a_sub_cluster_binds_to_its_own_particles() { // A quad over those four particles. let sb = &world.soft_bodies[handle]; - let vertices: Vec = lower.iter().map(|&i| sb.particle_position(i as usize)).collect(); + let vertices: Vec = lower + .iter() + .map(|&i| sb.particle_position(i as usize)) + .collect(); let patch = world_mesh(&world, vertices, vec![[0, 1, 2]], proxy); let collider = world - .insert_deformable( - patch, - SoftMeshBinding::direct_by_position(1.0e-4), - proxy, - ) + .insert_deformable(patch, SoftMeshBinding::direct_by_position(1.0e-4), proxy) .expect("the patch binds to the lower-face cluster"); let mesh = world.soft_bodies[handle].mesh_of(collider).unwrap(); @@ -387,7 +393,9 @@ fn a_skinned_mesh_follows_its_cells_through_a_tear() { ); let sb = &world.soft_bodies[handle]; - let mesh = sb.collision_mesh().expect("the skin is still the collision mesh"); + let mesh = sb + .collision_mesh() + .expect("the skin is still the collision mesh"); // Every vertex still rides a live cell. match mesh.binding() { SoftMeshMapping::Skinned { bindings } => { @@ -512,7 +520,10 @@ fn a_skinned_mesh_comes_apart_with_its_cells() { ); world.step(); let skins = |sb: &SoftBody| -> Vec { - sb.meshes().filter(|mesh| mesh.is_skinned()).cloned().collect() + sb.meshes() + .filter(|mesh| mesh.is_skinned()) + .cloned() + .collect() }; let vertex_count = skins(&world.soft_bodies[handle])[0].vertex_count(); @@ -553,7 +564,10 @@ fn a_skinned_mesh_comes_apart_with_its_cells() { } positions.push(mesh.vertex_positions(sb).collect::>()); } - assert!(vertices > vertex_count, "no skin vertex was duplicated across the cut"); + assert!( + vertices > vertex_count, + "no skin vertex was duplicated across the cut" + ); // The duplicated vertices start where their source was: nothing jumps. world.step(); let mut moved: Real = 0.0; @@ -563,7 +577,10 @@ fn a_skinned_mesh_comes_apart_with_its_cells() { moved = moved.max((now - *then).length()); } } - assert!(moved < 0.1, "a skin vertex jumped {moved} through the cut (collides: {collides})"); + assert!( + moved < 0.1, + "a skin vertex jumped {moved} through the cut (collides: {collides})" + ); for _ in 0..120 { world.step(); diff --git a/crates/rapier3d/tests/soft_wires.rs b/crates/rapier3d/tests/soft_wires.rs index 6991aa748..246903c42 100644 --- a/crates/rapier3d/tests/soft_wires.rs +++ b/crates/rapier3d/tests/soft_wires.rs @@ -231,10 +231,12 @@ fn a_rope_draped_over_a_rod_rests() { } assert!(on_rod > 0, "the rope slid off the rod"); let low = lowest(&world, handle); - assert!(low > 2.0 && low < 3.0, "the rope is not hanging from the rod: {low}"); + assert!( + low > 2.0 && low < 3.0, + "the rope is not hanging from the rod: {low}" + ); assert!( max_speed < 0.02, "the rope jitters on the rod: max speed {max_speed}" ); } - diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index b98674ebf..bee3cec9a 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -26,6 +26,7 @@ mod debug_box_ball2; mod debug_compression2; mod debug_intersection2; mod debug_many_colliders2; +mod debug_self_intersect2; mod debug_total_overlap2; mod debug_vertical_column2; mod drum2; @@ -42,7 +43,6 @@ mod platform2; mod polyline2; mod pyramid2; mod restitution2; -mod debug_self_intersect2; mod rope_joints2; mod s2d_arch; mod s2d_ball_and_chain; diff --git a/examples2d/debug_self_intersect2.rs b/examples2d/debug_self_intersect2.rs index 4b43cb4a5..329ad99d7 100644 --- a/examples2d/debug_self_intersect2.rs +++ b/examples2d/debug_self_intersect2.rs @@ -58,17 +58,25 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { ); let bottom_row = (0..7).map(|i| i * 3); let ground_strip = world.insert_soft_body( - SoftBodyBuilder::grid(grab_origin + Vector::new(0.0, 0.15), Vector::new(1.5, 0.15), 7, 3) - .pinned_particles(bottom_row) - .softness(SpringCoefficients::new(5.0, 1.0)) - .self_contacts(true), + SoftBodyBuilder::grid( + grab_origin + Vector::new(0.0, 0.15), + Vector::new(1.5, 0.15), + 7, + 3, + ) + .pinned_particles(bottom_row) + .softness(SpringCoefficients::new(5.0, 1.0)) + .self_contacts(true), ); let grabbed = 11u32; let anchor = world.soft_bodies[ground_strip].particle_position(grabbed as usize); - let cluster = world.add_soft_body_cluster(ground_strip, &[grabbed]).unwrap(); - let proxy = world.soft_bodies[ground_strip].cluster_proxy(cluster).unwrap(); - let mouse = - world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); + let cluster = world + .add_soft_body_cluster(ground_strip, &[grabbed]) + .unwrap(); + let proxy = world.soft_bodies[ground_strip] + .cluster_proxy(cluster) + .unwrap(); + let mouse = world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); let joint: GenericJoint = GenericJointBuilder::new(JointAxesMask::empty()) .motor_position(JointAxis::LinX, 0.0, 1000.0, 50.0) .motor_position(JointAxis::LinY, 0.0, 1000.0, 50.0) @@ -94,10 +102,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let reload = |world: &mut PhysicsWorld| { let n = world.soft_bodies[bullet].particles().len(); for i in 0..n { - let angle = core::f32::consts::TAU as Real * i as Real / n as Real; - let p = cannon_origin - + Vector::new(0.0, 2.5) - + Vector::new(angle.cos(), angle.sin()) * 0.3; + let angle = core::f32::consts::TAU * i as Real / n as Real; + let p = + cannon_origin + Vector::new(0.0, 2.5) + Vector::new(angle.cos(), angle.sin()) * 0.3; world.soft_bodies[bullet].set_particle_position(i, p); world.soft_bodies[bullet].set_particle_velocity(i, Vector::new(0.0, -150.0)); } @@ -119,7 +126,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let gerono_eight = |world: &mut PhysicsWorld, h: SoftBodyHandle, center: Vector, r: Real| { let n = world.soft_bodies[h].particles().len(); for i in 0..n { - let t = core::f32::consts::TAU as Real * i as Real / n as Real; + let t = core::f32::consts::TAU * i as Real / n as Real; let p = center + Vector::new(r * t.cos(), r * t.sin() * t.cos()); world.soft_bodies[h].set_particle_position(i, p); } @@ -128,7 +135,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { |world: &mut PhysicsWorld, h: SoftBodyHandle, center: Vector, rb: Real, rs: Real| { let n = world.soft_bodies[h].particles().len(); let n_big = (n as Real * rb / (rb + rs)) as usize; - let tau = core::f32::consts::TAU as Real; + let tau = core::f32::consts::TAU; for i in 0..n { let p = if i < n_big { let t = tau * i as Real / n_big as Real; @@ -196,7 +203,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let target = if cycle < 1.0 { Vector::new(0.0, 0.5 - 0.48 * cycle) } else if cycle < 4.0 { - let w = core::f32::consts::TAU as Real * (cycle - 1.0); + let w = core::f32::consts::TAU * (cycle - 1.0); Vector::new(0.3 * w.sin(), 0.02 + 0.03 * (1.0 - (2.0 * w).cos())) } else if cycle < 5.0 { Vector::new(0.0, 0.02 + 0.48 * (cycle - 4.0)) diff --git a/examples2d/soft_cutting2.rs b/examples2d/soft_cutting2.rs index 811ec892e..4c591c993 100644 --- a/examples2d/soft_cutting2.rs +++ b/examples2d/soft_cutting2.rs @@ -3,8 +3,8 @@ //! suspended slab. Cuts remove no material and separated pieces become soft bodies. use kiss3d::color::Color; -use rapier2d::prelude::*; use rapier_testbed2d::{Key, TestbedViewer}; +use rapier2d::prelude::*; pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let mut world = PhysicsWorld::new(); @@ -74,9 +74,8 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { // The saw: a kinematic blade drawn as a thin plate, a sensor so it pushes nothing (the // cut does the work). let saw_start = Vector::new(10.0, 1.5); - let saw = world.insert_body( - RigidBodyBuilder::kinematic_position_based().translation(saw_start), - ); + let saw = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(saw_start)); world.insert_collider(ColliderBuilder::cuboid(0.05, 1.0).sensor(true), Some(saw)); viewer.set_world(&mut world); diff --git a/examples2d/soft_force_tearing2.rs b/examples2d/soft_force_tearing2.rs index 498216c6a..99ff4a9e8 100644 --- a/examples2d/soft_force_tearing2.rs +++ b/examples2d/soft_force_tearing2.rs @@ -2,9 +2,9 @@ //! bars hanging a crate (the strain criterion never fires, the force one tears), a notched slab //! pulled apart with a tougher interior, and crate bars struck by a disk, one of them smoothed. +use rapier_testbed2d::TestbedViewer; use rapier2d::pipeline::DebugRenderMode; use rapier2d::prelude::*; -use rapier_testbed2d::TestbedViewer; pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let settings = viewer.example_settings_mut(); @@ -114,7 +114,12 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { } let mut right_end: Vec<((SoftBodyHandle, u32), Vector)> = (0..sy) .flat_map(|j| [sidx(sx - 2, j), sidx(sx - 1, j)]) - .map(|i| ((slab, i), world.soft_bodies[slab].particle_position(i as usize))) + .map(|i| { + ( + (slab, i), + world.soft_bodies[slab].particle_position(i as usize), + ) + }) .collect(); /* @@ -196,11 +201,10 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { /// particle's body and index follow the events. fn follow_tears(events: &[SoftBodyTearEvent], particle: &mut (SoftBodyHandle, u32)) { for event in events { - if event.soft_body == particle.0 { - if let Some(destination) = event.particle_destination(particle.1) { - *particle = destination; - } + if event.soft_body == particle.0 + && let Some(destination) = event.particle_destination(particle.1) + { + *particle = destination; } } } - diff --git a/examples2d/soft_joints2.rs b/examples2d/soft_joints2.rs index 11c69f00c..f889fa283 100644 --- a/examples2d/soft_joints2.rs +++ b/examples2d/soft_joints2.rs @@ -59,7 +59,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let top_cluster = world .add_soft_body_cluster(wobbler, &top_edge) .expect("top-edge cluster"); - let top_proxy = world.soft_bodies[wobbler].cluster_proxy(top_cluster).unwrap(); + let top_proxy = world.soft_bodies[wobbler] + .cluster_proxy(top_cluster) + .unwrap(); let top_pos = world.bodies[top_proxy].position().translation; let (plate, _) = world.insert( RigidBodyBuilder::dynamic().translation(top_pos + Vector::new(0.0, 0.12)), @@ -99,7 +101,8 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { RigidBodyBuilder::fixed().translation(Vector::new(1.5, 1.0)), ColliderBuilder::cuboid(1.2, 1.0), ); - let anchor_jelly = world.insert_soft_body(jelly(Vector::new(1.5, 2.6), Vector::splat(0.5), 1.2e4)); + let anchor_jelly = + world.insert_soft_body(jelly(Vector::new(1.5, 2.6), Vector::splat(0.5), 1.2e4)); let hanging_jelly = world.insert_soft_body(jelly(Vector::new(3.8, 2.6), Vector::splat(0.5), 1.2e4)); world.insert_impulse_joint( @@ -129,7 +132,11 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { RigidBodyBuilder::fixed().translation(pole_pos), ColliderBuilder::cuboid(0.05, 2.5).collision_groups(InteractionGroups::none()), ); - let bead = world.insert_soft_body(jelly(pole_pos + Vector::new(0.0, 1.7), Vector::splat(0.35), 8.0e3)); + let bead = world.insert_soft_body(jelly( + pole_pos + Vector::new(0.0, 1.7), + Vector::splat(0.35), + 8.0e3, + )); let pole = world.insert_body(RigidBodyBuilder::fixed().translation(pole_pos)); world.insert_impulse_joint( pole, @@ -168,7 +175,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { } (left, right) }; - let left_cluster = world.add_soft_body_cluster(bar, &left_half).expect("left half"); + let left_cluster = world + .add_soft_body_cluster(bar, &left_half) + .expect("left half"); let right_cluster = world .add_soft_body_cluster(bar, &right_half) .expect("right half"); diff --git a/examples2d/soft_stress2.rs b/examples2d/soft_stress2.rs index d793a82f2..c82c95959 100644 --- a/examples2d/soft_stress2.rs +++ b/examples2d/soft_stress2.rs @@ -2,9 +2,9 @@ //! `SoftBodyEdge::stress`, blue when slack, red at the tear threshold; nothing tears. Scenes: a //! bridge, a bar and a block, each under a crate, and a blob colored by its stretch instead. +use rapier_testbed2d::TestbedViewer; use rapier2d::pipeline::DebugRenderMode; use rapier2d::prelude::*; -use rapier_testbed2d::TestbedViewer; pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let mut world = PhysicsWorld::new(); diff --git a/examples2d/soft_tearing2.rs b/examples2d/soft_tearing2.rs index 40c81f4ec..7ceb6d718 100644 --- a/examples2d/soft_tearing2.rs +++ b/examples2d/soft_tearing2.rs @@ -2,11 +2,11 @@ //! hanging strip shot through by a fast disk, and a jelly bar pulled apart by its kinematic ends. //! Cracks split particles and shed pieces as new soft bodies; the panel sets `min_piece`. -use rapier2d::prelude::*; use rapier_testbed2d::{ TestbedViewer, egui::{Align2, Slider, Window}, }; +use rapier2d::prelude::*; pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let mut world = PhysicsWorld::new(); @@ -156,11 +156,10 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { /// particle's body and index follow the events. fn follow_tears(events: &[SoftBodyTearEvent], particle: &mut (SoftBodyHandle, u32)) { for event in events { - if event.soft_body == particle.0 { - if let Some(destination) = event.particle_destination(particle.1) { - *particle = destination; - } + if event.soft_body == particle.0 + && let Some(destination) = event.particle_destination(particle.1) + { + *particle = destination; } } } - diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 56e63a481..fb113ac63 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -46,9 +46,9 @@ mod debug_long_chain3; mod debug_multi_collider_body3; mod debug_multibody_ang_motor_pos3; mod debug_pop3; -mod debug_self_intersect3; mod debug_prismatic3; mod debug_rollback3; +mod debug_self_intersect3; mod debug_shape_modification3; mod debug_sleeping_kinematic3; mod debug_thin_cube_on_mesh3; diff --git a/examples3d/debug_dynamic_collider_add3.rs b/examples3d/debug_dynamic_collider_add3.rs index b7f38abca..36a7c5e7e 100644 --- a/examples3d/debug_dynamic_collider_add3.rs +++ b/examples3d/debug_dynamic_collider_add3.rs @@ -78,15 +78,13 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { for handle in &extra_colliders { viewer.remove_collider(*handle); - world - .colliders - .remove( - *handle, - &mut world.islands, - &mut world.bodies, - &mut world.soft_bodies, - true, - ); + world.colliders.remove( + *handle, + &mut world.islands, + &mut world.bodies, + &mut world.soft_bodies, + true, + ); } extra_colliders.clear(); diff --git a/examples3d/debug_self_intersect3.rs b/examples3d/debug_self_intersect3.rs index b8c003270..385c8a3a9 100644 --- a/examples3d/debug_self_intersect3.rs +++ b/examples3d/debug_self_intersect3.rs @@ -49,8 +49,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { }) .unwrap() .0; - let slab = - world.insert_soft_body(builder.pinned_particles(pinned).self_contacts(true)); + let slab = world.insert_soft_body(builder.pinned_particles(pinned).self_contacts(true)); world.soft_bodies[slab] .set_particle_position(captured, slab_origin + Vector::new(0.3, -0.4, 0.0)); @@ -68,7 +67,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { nu, nv, ) - .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new( + 40.0, 1.0, + ))) .softness(SpringCoefficients::new(40.0, 1.0)) .particle_mass(0.02) .particle_radius(0.05) @@ -105,7 +106,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { nu, nv, ) - .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new( + 40.0, 1.0, + ))) .softness(SpringCoefficients::new(40.0, 1.0)) .particle_mass(0.02) .particle_radius(0.05) @@ -142,7 +145,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { n, n, ) - .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new( + 40.0, 1.0, + ))) .softness(SpringCoefficients::new(40.0, 1.0)) .particle_mass(0.02) .particle_radius(0.05) @@ -163,7 +168,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let strips_origin = Vector::new(8.5, 0.0, 0.0); let strip = |origin: Vector, du: Vector, dv: Vector| { SoftBodyBuilder::cloth(origin, du, dv, 12, 2) - .material(SoftBodyMaterial::uniform(SpringCoefficients::new(40.0, 1.0))) + .material(SoftBodyMaterial::uniform(SpringCoefficients::new( + 40.0, 1.0, + ))) .softness(SpringCoefficients::new(40.0, 1.0)) .particle_mass(0.02) .particle_radius(0.05) @@ -202,8 +209,12 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { Vector::ZERO } }); - reset(world, bullet, &bullet_rest, &|_| Vector::new(0.0, -60.0, 0.0)); - reset(world, top_strip, &top_rest, &|_| Vector::new(0.0, -30.0, 0.0)); + reset(world, bullet, &bullet_rest, &|_| { + Vector::new(0.0, -60.0, 0.0) + }); + reset(world, top_strip, &top_rest, &|_| { + Vector::new(0.0, -30.0, 0.0) + }); }; refire(&mut world); diff --git a/examples3d/soft_joints3.rs b/examples3d/soft_joints3.rs index 848d6ac4d..f127ec171 100644 --- a/examples3d/soft_joints3.rs +++ b/examples3d/soft_joints3.rs @@ -87,7 +87,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let top_cluster = world .add_soft_body_cluster(wobbler, &top_face) .expect("top-face cluster"); - let top_proxy = world.soft_bodies[wobbler].cluster_proxy(top_cluster).unwrap(); + let top_proxy = world.soft_bodies[wobbler] + .cluster_proxy(top_cluster) + .unwrap(); let top_pos = world.bodies[top_proxy].position().translation; let (plate, _) = world.insert( RigidBodyBuilder::dynamic().translation(top_pos + Vector::new(0.0, 0.12, 0.0)), @@ -194,17 +196,23 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { * motorized revolute at its middle, so the body folds and flaps at its own hinge. */ let bar = world.insert_soft_body( - SoftBodyBuilder::cuboid(Vector::new(2.5, 2.2, 5.5), Vector::new(1.0, 0.22, 0.4), 7, 3, 3) - .cell_model(SoftBodyCellModel::Corotational) - .material(SoftBodyMaterial { - young_modulus: 2.0e4, - poisson_ratio: 0.35, - elastic_damping_ratio: 1.0, - ..Default::default() - }) - .particle_mass(0.08) - .particle_radius(0.06) - .surface_collider(ColliderBuilder::ball(0.06)), + SoftBodyBuilder::cuboid( + Vector::new(2.5, 2.2, 5.5), + Vector::new(1.0, 0.22, 0.4), + 7, + 3, + 3, + ) + .cell_model(SoftBodyCellModel::Corotational) + .material(SoftBodyMaterial { + young_modulus: 2.0e4, + poisson_ratio: 0.35, + elastic_damping_ratio: 1.0, + ..Default::default() + }) + .particle_mass(0.08) + .particle_radius(0.06) + .surface_collider(ColliderBuilder::ball(0.06)), ); let (left_half, right_half): (Vec, Vec) = { let sb = &world.soft_bodies[bar]; @@ -220,7 +228,9 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { } (left, right) }; - let left_cluster = world.add_soft_body_cluster(bar, &left_half).expect("left half"); + let left_cluster = world + .add_soft_body_cluster(bar, &left_half) + .expect("left half"); let right_cluster = world .add_soft_body_cluster(bar, &right_half) .expect("right half"); @@ -294,11 +304,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { RigidBodyBuilder::dynamic().translation(Vector::new(10.2, 0.3, 6.0)), ColliderBuilder::cuboid(0.3, 0.3, 0.3).density(0.5), ); - world.insert_impulse_joint( - pendulum_root, - crate_body, - RopeJointBuilder::new(3.2), - ); + world.insert_impulse_joint(pendulum_root, crate_body, RopeJointBuilder::new(3.2)); /* * Kinematic cluster (no joint): a banner whose pinned top-edge cluster is waved rigidly. diff --git a/examples3d/soft_meshes3.rs b/examples3d/soft_meshes3.rs index 96bcae0df..333bb0ee3 100644 --- a/examples3d/soft_meshes3.rs +++ b/examples3d/soft_meshes3.rs @@ -204,8 +204,11 @@ mod tests { fn the_split_jelly_halves_are_closed() { let (world, [_, _, split]) = build_world().expect("the demo builds"); let sb = &world.soft_bodies[split]; - let on_boundary = - |particle: u32| sb.boundary().iter().any(|element| element.contains(&particle)); + let on_boundary = |particle: u32| { + sb.boundary() + .iter() + .any(|element| element.contains(&particle)) + }; let halves: Vec<_> = sb.meshes().filter(|mesh| mesh.id().cluster != 0).collect(); assert_eq!(halves.len(), 2); diff --git a/examples3d/soft_tearing3.rs b/examples3d/soft_tearing3.rs index 51d3f8192..5760f79dc 100644 --- a/examples3d/soft_tearing3.rs +++ b/examples3d/soft_tearing3.rs @@ -2,11 +2,11 @@ //! shot through by a ball, and a jelly bar pulled apart by its kinematic ends. Cloth springs are //! stiff (100 Hz) so only impacts pass the 0.4 tear strain; the panel sets `min_piece`. -use rapier3d::prelude::*; use rapier_testbed3d::{ TestbedViewer, egui::{Align2, Slider, Window}, }; +use rapier3d::prelude::*; pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { let mut world = PhysicsWorld::new(); @@ -177,11 +177,10 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { /// particle's body and index follow the events. fn follow_tears(events: &[SoftBodyTearEvent], particle: &mut (SoftBodyHandle, u32)) { for event in events { - if event.soft_body == particle.0 { - if let Some(destination) = event.particle_destination(particle.1) { - *particle = destination; - } + if event.soft_body == particle.0 + && let Some(destination) = event.particle_destination(particle.1) + { + *particle = destination; } } } - diff --git a/python/rapier-py-3d/src/controllers.rs b/python/rapier-py-3d/src/controllers.rs index 3033046a8..474b9e335 100644 --- a/python/rapier-py-3d/src/controllers.rs +++ b/python/rapier-py-3d/src/controllers.rs @@ -703,7 +703,7 @@ impl KinematicCharacterController { bp.0.as_query_pipeline(np.0.query_dispatcher(), &bodies.0, &colliders.0, qf); let result = (|qp: rapier::pipeline::QueryPipeline<'_>| { - let mv = inner.move_shape(dt, &qp, &*shape.0 .0, &pose, desired, |collision| { + let mv = inner.move_shape(dt, &qp, &*shape.0.0, &pose, desired, |collision| { if let Some(ref obj) = cb_obj { let py_col = CharacterCollision::from_rapier(collision); if let Err(e) = obj.call1(py, (py_col,)) { @@ -783,7 +783,7 @@ impl KinematicCharacterController { self.0.solve_character_collision_impulses( dt, &mut qpmut, - &*character_shape.0 .0, + &*character_shape.0.0, character_mass, collisions_rs.iter(), ); diff --git a/python/rapier-py-3d/src/joints.rs b/python/rapier-py-3d/src/joints.rs index a78b82ff9..8a1db4cff 100644 --- a/python/rapier-py-3d/src/joints.rs +++ b/python/rapier-py-3d/src/joints.rs @@ -637,16 +637,16 @@ impl MultibodyIndex { /// Slot index of this multibody. #[getter] fn index(&self) -> u32 { - self.0 .0.into_raw_parts().0 + self.0.0.into_raw_parts().0 } /// Generation counter, incremented when a slot is reused. #[getter] fn generation(&self) -> u32 { - self.0 .0.into_raw_parts().1 + self.0.0.into_raw_parts().1 } /// Hash matching equality semantics. fn __hash__(&self) -> u64 { - let (i, g) = self.0 .0.into_raw_parts(); + let (i, g) = self.0.0.into_raw_parts(); ((i as u64) << 32) | (g as u64) } /// Equality and inequality comparisons; other operators raise. @@ -661,7 +661,7 @@ impl MultibodyIndex { } /// Return a developer-readable representation. fn __repr__(&self) -> String { - let (i, g) = self.0 .0.into_raw_parts(); + let (i, g) = self.0.0.into_raw_parts(); format!("MultibodyIndex(index={}, generation={})", i, g) } } diff --git a/python/rapier-py-3d/src/pipeline.rs b/python/rapier-py-3d/src/pipeline.rs index 4c818037d..0b73e906f 100644 --- a/python/rapier-py-3d/src/pipeline.rs +++ b/python/rapier-py-3d/src/pipeline.rs @@ -1727,7 +1727,7 @@ impl QueryPipeline { let result = (|qp: rapier::pipeline::QueryPipeline<'_>| { Ok(qp - .cast_shape(&pose, vel, &*shape.0 .0, options.0) + .cast_shape(&pose, vel, &*shape.0.0, options.0) .map(|(h, hit)| (ColliderHandle(h), ShapeCastHit::from_parry(hit)))) })(qp); if let Some(e) = pred_err.into_inner() { @@ -1823,7 +1823,7 @@ impl QueryPipeline { Ok(qp .cast_shape_nonlinear( &motion.0, - &*shape.0 .0, + &*shape.0.0, start_time, end_time, options.0.stop_at_penetration, @@ -1913,7 +1913,7 @@ impl QueryPipeline { bp.0.as_query_pipeline(np.0.query_dispatcher(), &bodies.0, &colliders.0, qf); let result = (|qp: rapier::pipeline::QueryPipeline<'_>| { - for (h, _co) in qp.intersect_shape(pose, &*shape.0 .0) { + for (h, _co) in qp.intersect_shape(pose, &*shape.0.0) { let res = callback.call1(py, (ColliderHandle(h),))?; if let Ok(b) = res.extract::(py) { if !b { diff --git a/python/rapier-py-3d/src/serde_glue.rs b/python/rapier-py-3d/src/serde_glue.rs index 3753e4512..21f805a1a 100644 --- a/python/rapier-py-3d/src/serde_glue.rs +++ b/python/rapier-py-3d/src/serde_glue.rs @@ -89,11 +89,11 @@ impl MultibodyIndex { )) } fn into_raw_parts(&self) -> (u32, u32) { - self.0 .0.into_raw_parts() + self.0.0.into_raw_parts() } fn __reduce__(slf: PyRef<'_, Self>) -> PyResult<(Py, (u32, u32))> { let py = slf.py(); - let (i, g) = slf.0 .0.into_raw_parts(); + let (i, g) = slf.0.0.into_raw_parts(); let ctor: Py = py .get_type_bound::() .getattr("from_raw_parts")? diff --git a/python/rapier-py-3d/src/soft_body.rs b/python/rapier-py-3d/src/soft_body.rs index 1ea05b679..8e22e2be6 100644 --- a/python/rapier-py-3d/src/soft_body.rs +++ b/python/rapier-py-3d/src/soft_body.rs @@ -307,7 +307,7 @@ impl SoftBodyMaterial { other => { return Err(PyTypeError::new_err(format!( "SoftBodyMaterial: unknown keyword argument '{other}'" - ))) + ))); } } Ok(()) @@ -2605,7 +2605,7 @@ impl SoftBody { /// The ``(edges, cells)`` a blade (a world-space triangle given as three points) crosses, /// as would be torn by :meth:`SoftBodySet.cut`. fn crossing_elements(&self, blade: (PyVector, PyVector, PyVector)) -> (Vec, Vec) { - let blade = [blade.0 .0.into(), blade.1 .0.into(), blade.2 .0.into()]; + let blade = [blade.0.0.into(), blade.1.0.into(), blade.2.0.into()]; self.with_ref(|b| b.crossing_elements(&blade)) } @@ -2888,7 +2888,7 @@ impl SoftBodySet { impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, ) -> Option { - let blade = [blade.0 .0.into(), blade.1 .0.into(), blade.2 .0.into()]; + let blade = [blade.0.0.into(), blade.1.0.into(), blade.2.0.into()]; self.0 .cut( handle.0, diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 5e12aeee3..53b6acebe 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -119,7 +119,10 @@ impl + Copy> SpringCoefficients { // let cfm = 1.0 / (dt * dt * stiffness + dt * damping); // NOTE: This simplifies to cfm = cfm_coeff / projected_mass: inv_erp_minus_one * inv_erp_minus_one - / ((one + inv_erp_minus_one) * N::splat(4.0) * self.damping_ratio * self.damping_ratio) + / ((one + inv_erp_minus_one) + * N::splat(4.0) + * self.damping_ratio + * self.damping_ratio) }; let undamped = { // Undamped version if the damping ratio is zero. @@ -291,7 +294,7 @@ pub struct IntegrationParameters { /// into one "cluster" manifold before constraint generation (default: `true`, 3D only), so at /// most 4 contact points are solved per contact plane — a large solver win on composite shapes /// (meshes, heightfields, compounds, voxels) that emit one manifold per subshape. When clustering - /// applies, read solver contacts/impulses from [`crate::geometry::ContactPair::solver_clusters`], + /// applies, read solver contacts/impulses from [`crate::geometry::RigidPairContacts::solver_clusters`], /// not [`crate::geometry::ContactPair::manifolds`]. pub contact_clustering: bool, /// If enabled, a contact pair whose relative pose moved less than [`Self::contact_recycle_distance`] diff --git a/src/dynamics/island_manager/global_split.rs b/src/dynamics/island_manager/global_split.rs index 8ffad0781..7e929a57c 100644 --- a/src/dynamics/island_manager/global_split.rs +++ b/src/dynamics/island_manager/global_split.rs @@ -204,7 +204,9 @@ impl PersistentIslands { let new_id = self.alloc_island(); let island = &mut self.islands[new_id as usize]; island.sleeping = base_sleeping; - island.bodies.reserve(workspace.uf.size(root as u32) as usize); + island + .bodies + .reserve(workspace.uf.size(root as u32) as usize); island .contact_links .reserve(workspace.contact_counts[root] as usize); diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 53f36807d..6fc587f9d 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1811,7 +1811,7 @@ mod test { use crate::math::{Real, SPATIAL_DIM}; use crate::prelude::{ ColliderSet, MultibodyJointHandle, MultibodyJointSet, RevoluteJoint, RigidBodyBuilder, - RigidBodySet, SoftBodySet + RigidBodySet, SoftBodySet, }; use na::{DVector, RowDVector}; diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 47396fadf..2b082b4a7 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -29,9 +29,9 @@ pub use parry::mass_properties::MassProperties; pub use self::rigid_body::{RigidBody, RigidBodyBuilder}; #[cfg(feature = "alloc")] pub use self::rigid_body_set::{BodyPair, RigidBodySet}; +pub(crate) use self::soft_body::soft_body_crossing_tests; #[cfg(feature = "alloc")] pub use self::soft_body::*; -pub(crate) use self::soft_body::soft_body_crossing_tests; #[cfg(feature = "alloc")] mod ccd; diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs index 44f90dbdf..478d31450 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_accessors.rs @@ -43,9 +43,9 @@ impl SoftCollisionMesh { /// mesh in 2D), `DIM` for a surface. #[inline] pub fn arity(&self) -> usize { - self.indices.first().map_or(DIM, |e| { - super::soft_body_builder::element_vertices(e).len() - }) + self.indices + .first() + .map_or(DIM, |e| super::soft_body_builder::element_vertices(e).len()) } /// Whether this mesh is a wire (segments in 3D): a curve, with no inside and no facets. @@ -121,11 +121,15 @@ impl SoftCollisionMesh { }; #[cfg(feature = "dim2")] let oriented = co.shape().as_polyline().is_some_and(|polyline| { - polyline.flags().contains(parry::shape::PolylineFlags::ORIENTED) + polyline + .flags() + .contains(parry::shape::PolylineFlags::ORIENTED) }); #[cfg(feature = "dim3")] let oriented = co.shape().as_trimesh().is_some_and(|trimesh| { - trimesh.flags().contains(parry::shape::TriMeshFlags::ORIENTED) + trimesh + .flags() + .contains(parry::shape::TriMeshFlags::ORIENTED) }); self.oriented = oriented; } @@ -160,9 +164,7 @@ impl SoftCollisionMesh { /// The world position of the `i`-th vertex. pub fn vertex(&self, body: &SoftBody, i: usize) -> Vector { match &self.binding { - SoftMeshMapping::Direct { particles } => { - body.particles[particles[i] as usize].position - } + SoftMeshMapping::Direct { particles } => body.particles[particles[i] as usize].position, SoftMeshMapping::Skinned { .. } => self.vertices[i], } } @@ -192,9 +194,7 @@ impl SoftCollisionMesh { /// The world velocity of the `i`-th vertex: a skinned vertex moves with the cell holding it. pub fn vertex_velocity(&self, body: &SoftBody, i: usize) -> Vector { match &self.binding { - SoftMeshMapping::Direct { particles } => { - body.particles[particles[i] as usize].velocity - } + SoftMeshMapping::Direct { particles } => body.particles[particles[i] as usize].velocity, SoftMeshMapping::Skinned { bindings } => { let binding = &bindings[i]; let Some(cell) = body.cells.get(binding.cell as usize) else { @@ -223,11 +223,7 @@ impl SoftCollisionMesh { /// The particles a contact at the `i`-th vertex acts through, and their weights: the /// vertex's own particle for a direct mesh, the particles of the cell holding it for a /// skinned one. - pub fn vertex_anchors( - &self, - body: &SoftBody, - i: usize, - ) -> ([u32; DIM + 1], [Real; DIM + 1]) { + pub fn vertex_anchors(&self, body: &SoftBody, i: usize) -> ([u32; DIM + 1], [Real; DIM + 1]) { match &self.binding { SoftMeshMapping::Direct { particles } => { let mut ids = [u32::MAX; DIM + 1]; diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs index 29a6b4278..298239cfa 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_geometry.rs @@ -40,7 +40,10 @@ impl SoftCollisionMesh { ); if a.is_finite() && b.is_finite() - && crate::dynamics::soft_body::soft_body_crossing_tests::segments_cross([p, q], [a, b]) + && crate::dynamics::soft_body::soft_body_crossing_tests::segments_cross( + [p, q], + [a, b], + ) { crossings += 1; } diff --git a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs index 112d31704..aa6f45904 100644 --- a/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs +++ b/src/dynamics/soft_body/collision_mesh/collision_mesh_topology.rs @@ -13,19 +13,15 @@ use super::collision_mesh_binding::{bind_to_cells, closest_cell_binding}; use super::soft_body_builder::{element_vertices, element_vertices_mut}; use super::{ SoftBody, SoftBodyCell, SoftCollisionMesh, SoftMeshCellBinding, SoftMeshId, SoftMeshMapping, - SoftTopologyRemap, - surface_element_cells, surface_is_closed, surface_rings, surface_vertex_elements, + SoftTopologyRemap, surface_element_cells, surface_is_closed, surface_rings, + surface_vertex_elements, }; impl SoftCollisionMesh { /// The mesh a body collides through when it collides through its own boundary: one vertex /// per particle (interior particles included, so vertex indices are particle indices), the /// cells' boundary as elements. - pub(crate) fn boundary( - body: &SoftBody, - self_contacts: bool, - collision_enabled: bool, - ) -> Self { + pub(crate) fn boundary(body: &SoftBody, self_contacts: bool, collision_enabled: bool) -> Self { let num_vertices = body.particles.len(); let indices = body.boundary.clone(); let (ring_offsets, ring) = surface_rings(num_vertices, &indices); @@ -39,7 +35,10 @@ impl SoftCollisionMesh { binding: SoftMeshMapping::Direct { particles: (0..num_vertices as u32).collect(), }, - id: SoftMeshId { cluster: 0, mesh: 0 }, + id: SoftMeshId { + cluster: 0, + mesh: 0, + }, follows_boundary: true, closed: surface_is_closed(&indices), rest_signed_volume: SoftBody::boundary_volume(&indices, |i| { @@ -188,11 +187,15 @@ impl SoftCollisionMesh { let (vertex_elements_offsets, vertex_elements) = surface_vertex_elements(vertices.len(), indices); #[cfg(feature = "dim3")] - let (edges, element_edges, edge_owners) = super::soft_body_builder::surface_edge_table(indices); + let (edges, element_edges, edge_owners) = + super::soft_body_builder::surface_edge_table(indices); Some(Self { binding: SoftMeshMapping::Skinned { bindings }, - id: SoftMeshId { cluster: 0, mesh: 0 }, + id: SoftMeshId { + cluster: 0, + mesh: 0, + }, follows_boundary: false, closed: surface_is_closed(indices), rest_signed_volume: SoftBody::boundary_volume(indices, |i| vertices[i as usize]), @@ -250,14 +253,14 @@ impl SoftCollisionMesh { *v = pose.inverse_transform_point(self.vertex(body, i)); } }; - // A polyline in either dimension (a 2D surface, or a wire in 3D), a trimesh in 3D. - if let Some(polyline) = shape.as_polyline_mut() { - polyline.update_vertices(write); - return; - } #[cfg(feature = "dim3")] if let Some(trimesh) = shape.as_trimesh_mut() { trimesh.update_vertices(write); + return; + } + // A polyline in either dimension: a 2D surface, or a wire in 3D. + if let Some(polyline) = shape.as_polyline_mut() { + polyline.update_vertices(write); } } @@ -478,14 +481,21 @@ impl SoftCollisionMesh { /// Follows the particles a cut inserted into segments (`inserted`: `[a, b, p, q]`, segment `(a, b)` /// became `(a, p)` and `(q, b)`) in a direct mesh (`particles`: the particle of each vertex): a /// mesh segment over `a` and `b` is split the same way, with a new vertex per inserted particle. -fn follow_insertions(particles: &mut Vec, indices: &mut Vec<[u32; DIM]>, inserted: &[[u32; 4]]) { +fn follow_insertions( + particles: &mut Vec, + indices: &mut Vec<[u32; DIM]>, + inserted: &[[u32; 4]], +) { for &[a, b, p, q] in inserted { for i in 0..indices.len() { let element = indices[i]; if element_vertices(&element).len() != 2 { continue; } - let ends = [particles[element[0] as usize], particles[element[1] as usize]]; + let ends = [ + particles[element[0] as usize], + particles[element[1] as usize], + ]; let (first, second) = if ends == [a, b] { (p, q) } else if ends == [b, a] { @@ -533,7 +543,11 @@ fn follow_pieces( let used = element_vertices(element).len(); let piece_of: Vec = element[..used] .iter() - .map(|&w| bindings.get(w as usize).map_or(u32::MAX, |b| cell_piece(b.cell))) + .map(|&w| { + bindings + .get(w as usize) + .map_or(u32::MAX, |b| cell_piece(b.cell)) + }) .collect(); // The most frequent piece, ties going to the one met first. let mut majority = (u32::MAX, 0); @@ -544,7 +558,10 @@ fn follow_pieces( } } let majority = majority.0; - if piece_of.iter().all(|&piece| piece == majority || piece == u32::MAX) { + if piece_of + .iter() + .all(|&piece| piece == majority || piece == u32::MAX) + { continue; } for k in 0..used { @@ -591,7 +608,10 @@ fn follow_splits( for &(copy, source) in split { let first = root.get(&source).copied().unwrap_or(source); root.insert(copy, first); - families.entry(first).or_insert_with(|| vec![first]).push(copy); + families + .entry(first) + .or_insert_with(|| vec![first]) + .push(copy); } let mut vertex_of: HashMap = HashMap::default(); for (vertex, &particle) in particles.iter().enumerate() { diff --git a/src/dynamics/soft_body/collision_mesh/mod.rs b/src/dynamics/soft_body/collision_mesh/mod.rs index 8a2534518..84c6e13da 100644 --- a/src/dynamics/soft_body/collision_mesh/mod.rs +++ b/src/dynamics/soft_body/collision_mesh/mod.rs @@ -8,12 +8,12 @@ mod collision_mesh_binding; mod collision_mesh_geometry; mod collision_mesh_topology; -pub use self::collision_mesh_binding::{SoftBindingError, SoftMeshBinding, SoftMeshBindingMode}; -pub(crate) use self::collision_mesh_binding::bind_to_cells; +pub(crate) use self::collision_mesh::SoftTopologyRemap; pub use self::collision_mesh::{ SoftCollisionMesh, SoftMeshCellBinding, SoftMeshId, SoftMeshMapping, SoftMeshRef, }; -pub(crate) use self::collision_mesh::SoftTopologyRemap; +pub(crate) use self::collision_mesh_binding::bind_to_cells; +pub use self::collision_mesh_binding::{SoftBindingError, SoftMeshBinding, SoftMeshBindingMode}; pub(super) use super::SoftBodyParticle; pub(super) use super::soft_body_builder; pub(super) use super::soft_body_builder::{ diff --git a/src/dynamics/soft_body/mod.rs b/src/dynamics/soft_body/mod.rs index df704b888..eb6058e66 100644 --- a/src/dynamics/soft_body/mod.rs +++ b/src/dynamics/soft_body/mod.rs @@ -1,28 +1,28 @@ //! Soft bodies: deformable bodies made of particles linked by elastic constraints, solved //! together with rigid bodies, contacts and joints. -pub use self::soft_body_cluster::{SoftBodyCluster, SoftClusterRemoval}; -pub(crate) use self::soft_body_cluster::{inertia_noise_floor, pseudo_inverse_inertia}; pub use self::collision_mesh::{ SoftBindingError, SoftCollisionMesh, SoftMeshBinding, SoftMeshBindingMode, SoftMeshCellBinding, SoftMeshId, SoftMeshMapping, SoftMeshRef, }; +pub use self::soft_body::{SOFT_BODY_MAX_CONSTRAINT_PARTICLES, SoftBody}; +pub use self::soft_body_builder::{SoftBodyBuilder, SoftBodyParticleSettings}; +pub use self::soft_body_cluster::{SoftBodyCluster, SoftClusterRemoval}; +pub(crate) use self::soft_body_cluster::{inertia_noise_floor, pseudo_inverse_inertia}; +pub use self::soft_body_contacts::SoftVolumeContact; #[cfg(feature = "dim3")] pub use self::soft_body_elements::SoftBodyDihedral; #[cfg(feature = "fem")] pub use self::soft_body_elements::SoftBodySolver; -pub use self::soft_body::{SOFT_BODY_MAX_CONSTRAINT_PARTICLES, SoftBody}; -pub use self::soft_body_contacts::SoftVolumeContact; pub use self::soft_body_elements::{ SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyEdgeKind, SoftBodyParticle, }; +pub use self::soft_body_handle::SoftBodyHandle; pub use self::soft_body_material::SoftBodyMaterial; pub use self::soft_body_motion::SoftParticleAttachment; -pub use self::soft_body_volume::SoftVolumePiece; -pub use self::soft_body_builder::{SoftBodyBuilder, SoftBodyParticleSettings}; -pub use self::soft_body_handle::SoftBodyHandle; -pub use self::soft_body_set::SoftBodySet; pub use self::soft_body_plasticity::SoftEdgePlasticFlow; +pub use self::soft_body_set::SoftBodySet; +pub use self::soft_body_volume::SoftVolumePiece; pub(crate) use self::soft_body::SOFT_BODY_OVERFLOW_COLOR; pub(crate) use self::soft_body_contacts::{ @@ -32,9 +32,9 @@ use self::soft_body_elements::CELL_IMPULSES; mod soft_recovery_settings; pub use soft_recovery_settings::{SoftPatchConstraints, SoftRecoverySettings}; mod soft_body_settings; +pub use soft_body_settings::SoftBodiesSettings; #[cfg(feature = "fem")] pub use soft_body_settings::SoftFemParameters; -pub use soft_body_settings::SoftBodiesSettings; pub(crate) mod collision_mesh; mod soft_body; diff --git a/src/dynamics/soft_body/soft_body.rs b/src/dynamics/soft_body/soft_body.rs index 2673fd918..f1d4c3467 100644 --- a/src/dynamics/soft_body/soft_body.rs +++ b/src/dynamics/soft_body/soft_body.rs @@ -1,17 +1,17 @@ //! The `SoftBody` struct and the shared constants of the soft-body constraints. -use crate::alloc_prelude::*; -use crate::dynamics::RigidBodyHandle; -use crate::math::{DIM, Real, Vector}; -use super::{ - SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial, SoftBodyParticle, - SoftParticleAttachment, -}; #[cfg(feature = "dim3")] use super::SoftBodyDihedral; #[cfg(feature = "fem")] use super::SoftBodySolver; #[cfg(feature = "serde-serialize")] use super::soft_body_material::default_true; +use super::{ + SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial, SoftBodyParticle, + SoftParticleAttachment, +}; +use crate::alloc_prelude::*; +use crate::dynamics::RigidBodyHandle; +use crate::math::{DIM, Real, Vector}; /// Number of particles a soft-body constraint can touch (a simplex cell). pub const SOFT_BODY_MAX_CONSTRAINT_PARTICLES: usize = DIM + 1; diff --git a/src/dynamics/soft_body/soft_body_accessors.rs b/src/dynamics/soft_body/soft_body_accessors.rs index 93e66c491..c2716a355 100644 --- a/src/dynamics/soft_body/soft_body_accessors.rs +++ b/src/dynamics/soft_body/soft_body_accessors.rs @@ -1,12 +1,12 @@ //! Accessors and simple setters of a soft body: particles, elements, material, volume preservation, sleeping and enabling. -use super::{ - SoftBody, SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial, SoftBodyParticle, - SoftVolumePiece, -}; #[cfg(feature = "dim3")] use super::SoftBodyDihedral; #[cfg(feature = "fem")] use super::SoftBodySolver; +use super::{ + SoftBody, SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial, SoftBodyParticle, + SoftVolumePiece, +}; use crate::dynamics::RigidBodyHandle; use crate::math::{DIM, Real, Vector}; @@ -173,12 +173,18 @@ impl SoftBody { /// The signed area (2D) or volume (3D) enclosed by the volume pieces at rest (`0.0` for a /// body without a closed boundary). pub fn rest_volume(&self) -> Real { - self.volume_pieces.iter().map(|piece| piece.rest_volume).sum() + self.volume_pieces + .iter() + .map(|piece| piece.rest_volume) + .sum() } /// The signed area (2D) or volume (3D) currently enclosed by the volume pieces. pub fn volume(&self) -> Real { - self.volume_pieces.iter().map(|piece| piece.volume(self)).sum() + self.volume_pieces + .iter() + .map(|piece| piece.volume(self)) + .sum() } /// The multiplier applied to each volume piece's rest volume to obtain its diff --git a/src/dynamics/soft_body/soft_body_builder/mod.rs b/src/dynamics/soft_body/soft_body_builder/mod.rs index f0295e322..5d5c84780 100644 --- a/src/dynamics/soft_body/soft_body_builder/mod.rs +++ b/src/dynamics/soft_body/soft_body_builder/mod.rs @@ -7,11 +7,11 @@ mod soft_body_builder_settings; mod soft_body_builder_shapes; pub use self::soft_body_builder::{SoftBodyBuilder, SoftBodyParticleSettings}; +#[cfg(feature = "dim3")] +pub(crate) use self::soft_body_builder_mesh_helpers::surface_edge_table; pub(crate) use self::soft_body_builder_mesh_helpers::{ cell_faces, element_vertices, element_vertices_mut, surface_element_cells, surface_is_closed, surface_rings, surface_vertex_elements, }; -#[cfg(feature = "dim3")] -pub(crate) use self::soft_body_builder_mesh_helpers::surface_edge_table; #[cfg(feature = "fem")] pub(super) use super::SoftBodySolver; 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 f74efe629..53e818f11 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 @@ -14,10 +14,12 @@ use super::super::{ SoftBody, SoftBodyCell, SoftBodyCellModel, SoftBodyEdge, SoftBodyEdgeKind, SoftBodyParticle, SoftCollisionMesh, soft_body_coloring, }; +use super::SoftBodyBuilder; #[cfg(feature = "dim3")] use super::soft_body_builder_mesh_helpers::dihedral_angle; -use super::soft_body_builder_mesh_helpers::{cell_boundary, surface_element_cells, surface_is_closed}; -use super::SoftBodyBuilder; +use super::soft_body_builder_mesh_helpers::{ + cell_boundary, surface_element_cells, surface_is_closed, +}; impl SoftBodyBuilder { /* diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs index 160af2f2b..9d5931fcf 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder.rs @@ -104,7 +104,7 @@ pub struct SoftBodyBuilder { /// its particles; `None` disables collisions. pub collider_template: Option, /// Mesh held by the cells, if any: `(vertices, elements)` in world space, bound to the - /// cells at build time (see [`super::SoftBodySkin`]). + /// cells at build time. pub skin: Option<(Vec, Vec<[u32; DIM]>)>, /// The segments the body collides through when it has no surface: a rope or a wire (3D /// only; in 2D the surface is already made of segments). diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs index 461273eef..2379ae04f 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs @@ -295,7 +295,11 @@ pub(crate) fn dihedral_angle(p0: Vector, p1: Vector, p2: Vector, p3: Vector) -> /// An icosphere: vertices on the sphere of the given center and radius, outward-oriented /// triangles. #[cfg(feature = "dim3")] -pub(super) fn icosphere(center: Vector, radius: Real, subdivisions: usize) -> (Vec, Vec<[u32; 3]>) { +pub(super) fn icosphere( + center: Vector, + radius: Real, + subdivisions: usize, +) -> (Vec, Vec<[u32; 3]>) { let t = (1.0 + Real::sqrt(5.0)) * 0.5; let mut vertices: Vec = [ (-1.0, t, 0.0), diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs index 0205cb14d..5ff2bd344 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_settings.rs @@ -187,7 +187,7 @@ impl SoftBodyBuilder { } /// Sets the simplex cells (triangles in 2D, tetrahedra in 3D). Structural edges along the - /// cell edges are added by [`Self::build`] if none were given. + /// cell edges are added at build time if none were given. pub fn cells(mut self, cells: Vec<[u32; DIM + 1]>) -> Self { self.cells = cells; self diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs index 6a8219332..9da9765bc 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs @@ -12,9 +12,9 @@ use parry::utils::hashmap::HashMap; use simba::scalar::{ComplexField as _, RealField as _}; use super::super::{SoftBodyCellModel, SoftBodyMaterial}; -use super::soft_body_builder_mesh_helpers::{cell_faces, drop_stray_pieces, mean_edge_length}; #[cfg(feature = "dim3")] use super::soft_body_builder_mesh_helpers::icosphere; +use super::soft_body_builder_mesh_helpers::{cell_faces, drop_stray_pieces, mean_edge_length}; use super::{SoftBodyBuilder, SoftBodyParticleSettings}; impl SoftBodyBuilder { @@ -126,7 +126,7 @@ impl SoftBodyBuilder { .edges(edges) .surface(boundary) .shape_matching(true) - .particle_radius(mean_edge * 0.5), + .particle_radius(mean_edge * 0.5), ) } } diff --git a/src/dynamics/soft_body/soft_body_cluster.rs b/src/dynamics/soft_body/soft_body_cluster.rs index a7eddf63f..28eb87296 100644 --- a/src/dynamics/soft_body/soft_body_cluster.rs +++ b/src/dynamics/soft_body/soft_body_cluster.rs @@ -56,6 +56,7 @@ fn invalid_cell() -> u32 { } impl SoftBodyCluster { + /// A cluster over `particles`, backed by the proxy rigid body `proxy`. pub fn new(particles: Vec, proxy: RigidBodyHandle, shape_matching: bool) -> Self { SoftBodyCluster { particles, @@ -122,6 +123,7 @@ impl SoftBodyCluster { self.shape_matching } + /// Sets the pose the cluster's particles are shape-matched toward (`None`: its own frame). pub fn set_shape_matching_target(&mut self, target: Option) { self.shape_matching_target = target; } @@ -205,7 +207,8 @@ impl SoftBody { } /// Enables or disables shape matching of the `i`-th cluster's particles toward the cluster's - /// frame (independent from the whole-body [`Self::enable_shape_matching`]). + /// frame (independent from the whole-body + /// [`crate::dynamics::SoftBodyBuilder::shape_matching`]). pub fn enable_cluster_shape_matching(&mut self, i: u32, enabled: bool) { if let Some(c) = self.clusters.get_mut(i as usize) { if c.is_live() && c.shape_matching != enabled { @@ -372,7 +375,8 @@ impl SoftBody { c.particles.insert(pos, copy); if !c.shape_impulses.is_empty() { // Keep the warm shape impulses aligned with the particle list. - c.shape_impulses.insert(pos.min(c.shape_impulses.len()), Vector::ZERO); + c.shape_impulses + .insert(pos.min(c.shape_impulses.len()), Vector::ZERO); } if let Some(r) = self.cluster_refs.get_mut(copy as usize) { *r += 1; @@ -403,7 +407,10 @@ impl SoftBody { /// Removes every particle whose `dead` flag is set (they must belong to no live cluster), the /// elements touching them and the attachments on them, compacting the remaining indices. /// Returns the removal counts and the old-to-new remap (`u32::MAX` if removed; empty if none). - pub(crate) fn remove_dead_particles(&mut self, dead: &[bool]) -> (SoftClusterRemoval, Vec) { + pub(crate) fn remove_dead_particles( + &mut self, + dead: &[bool], + ) -> (SoftClusterRemoval, Vec) { let mut counts = SoftClusterRemoval::default(); if !dead.contains(&true) { return (counts, Vec::new()); @@ -712,11 +719,7 @@ impl SoftBody { angmom += r.gcross(vel(v) * p.mass); } } - ( - com, - linvel * inv_mass, - inv_inertia.transform_vector(angmom), - ) + (com, linvel * inv_mass, inv_inertia.transform_vector(angmom)) } } @@ -762,8 +765,15 @@ pub(crate) fn pseudo_inverse_inertia(inertia: AngularInertia, abs_floor: Real) - pub(crate) fn pseudo_inverse_inertia(inertia: AngularInertia, abs_floor: Real) -> AngularInertia { const RELATIVE_EPS: Real = 1.0e-5; let m = na::Matrix3::new( - inertia.m11, inertia.m12, inertia.m13, inertia.m12, inertia.m22, inertia.m23, inertia.m13, - inertia.m23, inertia.m33, + inertia.m11, + inertia.m12, + inertia.m13, + inertia.m12, + inertia.m22, + inertia.m23, + inertia.m13, + inertia.m23, + inertia.m33, ); let eig = m.symmetric_eigen(); let max = eig.eigenvalues.amax(); diff --git a/src/dynamics/soft_body/soft_body_coloring.rs b/src/dynamics/soft_body/soft_body_coloring.rs index e191c64be..89176c6d2 100644 --- a/src/dynamics/soft_body/soft_body_coloring.rs +++ b/src/dynamics/soft_body/soft_body_coloring.rs @@ -38,7 +38,7 @@ pub(crate) fn assign_colors( for d in dihedrals.iter_mut() { d.color = colors.next().unwrap(); } - + (num_colors, has_overflow) } diff --git a/src/dynamics/soft_body/soft_body_contacts.rs b/src/dynamics/soft_body/soft_body_contacts.rs index e7947161a..f30a93f9c 100644 --- a/src/dynamics/soft_body/soft_body_contacts.rs +++ b/src/dynamics/soft_body/soft_body_contacts.rs @@ -1,6 +1,6 @@ //! The contact records a soft body keeps between steps (edge, vertex, overlap and volume contacts) and their debug-render accessors. -use crate::alloc_prelude::*; use super::{SoftBody, SoftCollisionMesh, SoftMeshRef}; +use crate::alloc_prelude::*; use crate::math::{AngVector, DIM, Pose, Real, Vector}; /// An edge-vs-edge contact between two surface edges (segments in 2D, triangle edges in 3D): @@ -196,7 +196,12 @@ mod test { let mut cells = Vec::new(); for i in 0..3u32 { for j in 0..3u32 { - let (a, b, c, d) = (i * 4 + j, i * 4 + j + 1, (i + 1) * 4 + j, (i + 1) * 4 + j + 1); + let (a, b, c, d) = ( + i * 4 + j, + i * 4 + j + 1, + (i + 1) * 4 + j, + (i + 1) * 4 + j + 1, + ); cells.push([a, b, c]); cells.push([b, d, c]); } diff --git a/src/dynamics/soft_body/soft_body_material.rs b/src/dynamics/soft_body/soft_body_material.rs index 15f1ee087..f50da2539 100644 --- a/src/dynamics/soft_body/soft_body_material.rs +++ b/src/dynamics/soft_body/soft_body_material.rs @@ -1,6 +1,6 @@ //! The `SoftBodyMaterial`: per-family stiffness, damping, plasticity and tearing parameters. -use crate::dynamics::soft_body::SoftEdgePlasticFlow; use crate::dynamics::SpringCoefficients; +use crate::dynamics::soft_body::SoftEdgePlasticFlow; use crate::math::Real; #[cfg(not(feature = "std"))] #[allow(unused_imports)] diff --git a/src/dynamics/soft_body/soft_body_meshes.rs b/src/dynamics/soft_body/soft_body_meshes.rs index b7d1efa7f..4dce558aa 100644 --- a/src/dynamics/soft_body/soft_body_meshes.rs +++ b/src/dynamics/soft_body/soft_body_meshes.rs @@ -1,6 +1,6 @@ //! The collision meshes of a soft body: lookup, binding to clusters, insertion and removal, and update after a step or a tear. -use crate::alloc_prelude::*; use super::{SoftBody, SoftBodyCell, SoftCollisionMesh, SoftMeshId}; +use crate::alloc_prelude::*; use crate::math::{DIM, Vector}; impl SoftBody { @@ -105,7 +105,12 @@ impl SoftBody { let closest = cluster .particles() .iter() - .map(|&p| (p, (self.particles[p as usize].position - *position).length())) + .map(|&p| { + ( + p, + (self.particles[p as usize].position - *position).length(), + ) + }) .filter(|(_, d)| *d <= *eps) .min_by(|a, b| a.1.total_cmp(&b.1)); let (particle, _) = closest.ok_or(SoftBindingError::UnmatchedVertex { @@ -124,12 +129,10 @@ impl SoftBody { let owned: Vec = cells.iter().map(|&c| self.cells[c]).collect(); // The mesh is given in world space, so it binds against where the particles // are now (`rest_position` is relative to the rest center of mass). - let positions: Vec = - self.particles.iter().map(|p| p.position).collect(); - let bindings = super::collision_mesh::bind_to_cells( - &vertices, &indices, &owned, &positions, - ) - .ok_or(SoftBindingError::UnboundVertex { vertex: 0 })?; + let positions: Vec = self.particles.iter().map(|p| p.position).collect(); + let bindings = + super::collision_mesh::bind_to_cells(&vertices, &indices, &owned, &positions) + .ok_or(SoftBindingError::UnboundVertex { vertex: 0 })?; // `bind_to_cells` indexes the cells it was given: back to the body's own ids. SoftMeshMapping::Skinned { bindings: bindings @@ -194,7 +197,10 @@ impl SoftBody { } /// Runs `f` on every mesh of this body, with the body itself in hand. - pub(crate) fn for_each_mesh_mut(&mut self, mut f: impl FnMut(&SoftBody, &mut SoftCollisionMesh)) { + pub(crate) fn for_each_mesh_mut( + &mut self, + mut f: impl FnMut(&SoftBody, &mut SoftCollisionMesh), + ) { let mut clusters = core::mem::take(&mut self.clusters); for cluster in &mut clusters { for mesh in cluster.meshes.iter_mut().flatten() { diff --git a/src/dynamics/soft_body/soft_body_plasticity.rs b/src/dynamics/soft_body/soft_body_plasticity.rs index d9623c34d..11a64542e 100644 --- a/src/dynamics/soft_body/soft_body_plasticity.rs +++ b/src/dynamics/soft_body/soft_body_plasticity.rs @@ -48,7 +48,12 @@ impl SoftBodyEdge { /// Flows the rest length toward `length` when the strain exceeds the material's edge yield /// ([`SoftBodyMaterial::edge_plastic_yield`]; `dt`: the step length). Returns whether the edge /// flowed significantly (such a body is kept awake). - pub(crate) fn plastic_flow(&mut self, length: Real, material: &SoftBodyMaterial, dt: Real) -> bool { + pub(crate) fn plastic_flow( + &mut self, + length: Real, + material: &SoftBodyMaterial, + dt: Real, + ) -> bool { let yield_strain = material.edge_plastic_yield; if yield_strain <= 0.0 || material.edge_plastic_creep <= 0.0 || self.rest_length <= 0.0 { return false; @@ -63,7 +68,11 @@ impl SoftBodyEdge { return false; } // The rest length that would leave exactly the yield strain: only the excess flows. - let signed_yield = if strain > 0.0 { yield_strain } else { -yield_strain }; + let signed_yield = if strain > 0.0 { + yield_strain + } else { + -yield_strain + }; let target = length / (1.0 + signed_yield); let blend = (material.edge_plastic_creep * dt).min(1.0); let initial = self.initial_rest_length(); diff --git a/src/dynamics/soft_body/soft_body_set/mod.rs b/src/dynamics/soft_body/soft_body_set/mod.rs index 87db21abe..6fa3cf626 100644 --- a/src/dynamics/soft_body/soft_body_set/mod.rs +++ b/src/dynamics/soft_body/soft_body_set/mod.rs @@ -6,8 +6,8 @@ mod soft_body_set_proxies; mod soft_body_set_split; mod soft_body_set_step_sync; -pub use self::soft_body_set::SoftBodySet; pub(crate) use self::soft_body_set::SoftBodyIslandEvent; -pub(super) use super::{SoftBodyParticleSettings, SoftCollisionMesh}; +pub use self::soft_body_set::SoftBodySet; #[cfg(feature = "dim3")] pub(super) use super::soft_body_builder; +pub(super) use super::{SoftBodyParticleSettings, SoftCollisionMesh}; diff --git a/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs b/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs index d54315871..9efe41aca 100644 --- a/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs +++ b/src/dynamics/soft_body/soft_body_set/soft_body_set_clusters.rs @@ -1,11 +1,13 @@ //! The cluster paths of a `SoftBodySet`: adding, removing and dissolving clusters. +use super::soft_body_set_proxies::spawn_proxy; +use super::{SoftBodyIslandEvent, SoftBodySet}; use crate::alloc_prelude::*; -use crate::dynamics::{ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyHandle, RigidBodySet}; -use crate::geometry::ColliderSet; use crate::dynamics::soft_body::{SoftBodyCluster, SoftBodyHandle, SoftClusterRemoval}; +use crate::dynamics::{ + ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyHandle, RigidBodySet, +}; +use crate::geometry::ColliderSet; use crate::math::Rotation; -use super::soft_body_set_proxies::spawn_proxy; -use super::{SoftBodyIslandEvent, SoftBodySet}; impl SoftBodySet { /// Adds a cluster to a soft body: a set of its particles (invalid indices ignored, duplicates 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 ec3b453ce..2a7c0a38e 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 @@ -1,11 +1,14 @@ //! Insertion and removal of the soft bodies of a `SoftBodySet`. +use super::soft_body_set_proxies::{spawn_mesh_collider, spawn_proxy}; +use super::{SoftBodyIslandEvent, SoftBodySet}; use crate::alloc_prelude::*; -use crate::dynamics::{ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyHandle, RigidBodySet, SoftBodyBuilder}; -use crate::geometry::ColliderSet; use crate::dynamics::soft_body::{SoftBody, SoftBodyCluster, SoftBodyHandle, SoftMeshId}; +use crate::dynamics::{ + ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyHandle, RigidBodySet, + SoftBodyBuilder, +}; +use crate::geometry::ColliderSet; use crate::math::{Pose, Rotation}; -use super::soft_body_set_proxies::{spawn_mesh_collider, spawn_proxy}; -use super::{SoftBodyIslandEvent, SoftBodySet}; impl SoftBodySet { /// Inserts a soft body, creating its root rigid body and its colliders from the collider @@ -67,8 +70,13 @@ impl SoftBodySet { user_data: 0, })); - soft_body.root_body = - spawn_proxy(&soft_body.particle_settings, soft_body.user_data, handle, 0, bodies); + soft_body.root_body = spawn_proxy( + &soft_body.particle_settings, + soft_body.user_data, + handle, + 0, + bodies, + ); let meshes = soft_body_builder.build_meshes(&soft_body); soft_body.clusters = alloc::vec![SoftBodyCluster { particles: (0..soft_body.particles.len() as u32).collect(), @@ -106,7 +114,13 @@ impl SoftBodySet { continue; } let Some(co_handle) = spawn_mesh_collider( - template, handle, mesh, &soft_body, cluster.proxy, &frame, bodies, + template, + handle, + mesh, + &soft_body, + cluster.proxy, + &frame, + bodies, colliders, ) else { // No shape to build (no element): the mesh collides through nothing, 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 bdf0f6eb4..783048cde 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 @@ -1,12 +1,12 @@ //! Spawning the cluster proxies and mesh colliders of a soft body, and the per-body end-of-step particle sync. use crate::alloc_prelude::*; +use crate::dynamics::soft_body::{SoftBody, SoftBodyHandle, SoftMeshRef}; use crate::dynamics::{IntegrationParameters, RigidBodyBuilder, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ColliderBuilder, ColliderHandle, ColliderSet, SharedShape}; +use crate::math::{DIM, Pose, Real, Vector}; #[cfg(not(feature = "std"))] #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use crate::dynamics::soft_body::{SoftBody, SoftBodyHandle, SoftMeshRef}; -use crate::math::{DIM, Pose, Real, Vector}; /// The deformable surface shape of `mesh` (a 2D polyline or 3D triangle mesh over `vertices`), /// flagged deformable so the narrow phase never trusts contact points cached across a vertex 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 d4287d27c..0d3cdd406 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 @@ -1,6 +1,8 @@ //! Splitting clusters and soft bodies along the cracks a tear or a cut opened: a cluster comes //! apart with the material it covers, its meshes and joints following the pieces, and a piece the //! crack disconnected from the rest of the body becomes a soft body of its own. +use super::soft_body_set_proxies::{clone_mesh_collider, spawn_proxy}; +use super::{SoftBodyIslandEvent, SoftBodySet}; use crate::alloc_prelude::*; use crate::dynamics::soft_body::{ SoftBody, SoftBodyCluster, SoftBodyHandle, SoftBodyPiece, SoftBodyTearEvent, SoftClusterSplit, @@ -11,8 +13,6 @@ use crate::dynamics::{ }; use crate::geometry::{ColliderHandle, ColliderSet}; use crate::math::{Pose, Real, Vector}; -use super::soft_body_set_proxies::{clone_mesh_collider, spawn_proxy}; -use super::{SoftBodyIslandEvent, SoftBodySet}; /// The poses the proxies had before a split, by proxy (a fresh proxy records the pose of the /// proxy it was split from): what the joints and rigid colliders hung on them are re-based from @@ -66,11 +66,7 @@ fn rest_centroid(sb: &SoftBody, particles: &[u32]) -> Vector { com += p.rest_position * p.mass; mass += p.mass; } - if mass > 0.0 { - com / mass - } else { - com - } + if mass > 0.0 { com / mass } else { com } } impl SoftBodySet { @@ -138,7 +134,9 @@ impl SoftBodySet { let sb = &mut self.bodies[handle.0]; let source = &sb.clusters[cluster as usize]; let proxy = source.proxy; - let old_pose = bodies.get(proxy).map_or(Pose::IDENTITY, |rb| *rb.position()); + let old_pose = bodies + .get(proxy) + .map_or(Pose::IDENTITY, |rb| *rb.position()); poses.record(proxy, old_pose); let shape_matching = source.shape_matching; let rest_com = rest_centroid(sb, &source.particles); @@ -148,8 +146,7 @@ impl SoftBodySet { let mut new_clusters: Vec = Vec::new(); for piece in &pieces[1..] { let index = sb.clusters.len() as u32; - let new_proxy = - spawn_proxy(&sb.particle_settings, sb.user_data, handle, index, bodies); + let new_proxy = spawn_proxy(&sb.particle_settings, sb.user_data, handle, index, bodies); let mut c = SoftBodyCluster::new(piece.clone(), new_proxy, shape_matching); c.cell = sb.matching_cell(piece); sb.clusters.push(c); @@ -161,7 +158,10 @@ impl SoftBodySet { { let c = &mut sb.clusters[cluster as usize]; if c.shape_impulses.len() == c.particles.len() { - let mut keep = c.particles.iter().map(|v| pieces[0].binary_search(v).is_ok()); + let mut keep = c + .particles + .iter() + .map(|v| pieces[0].binary_search(v).is_ok()); c.shape_impulses.retain(|_| keep.next().unwrap()); } else { c.shape_impulses.clear(); @@ -435,7 +435,8 @@ impl SoftBodySet { particles: kept_particles(&remap, num_particles), clusters: moves.iter().map(|&(old, new)| [old, new]).collect(), }); - self.island_events.push(SoftBodyIslandEvent { handle: new_handle }); + self.island_events + .push(SoftBodyIslandEvent { handle: new_handle }); new_handles.push(new_handle); } @@ -539,8 +540,7 @@ pub(super) fn rebase_proxy_attachments( continue; }; let new_pose = *rb.position(); - if new_pose.translation == old_pose.translation && new_pose.rotation == old_pose.rotation - { + if new_pose.translation == old_pose.translation && new_pose.rotation == old_pose.rotation { continue; } let delta = new_pose.inverse() * old_pose; 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 aa42e6db3..dede1e918 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 @@ -1,18 +1,18 @@ //! Per-step maintenance of the set: applying user changes, updating proxies and colliders, syncing particle positions, tearing and cutting. +use super::soft_body_set_proxies::{rebuilt_surface_shape, sync_soft_body}; +use super::soft_body_set_split::{ProxyPoses, rebase_proxy_attachments}; +use super::{SoftBodyIslandEvent, SoftBodySet}; use crate::alloc_prelude::*; +use crate::dynamics::soft_body::{SoftBody, SoftBodyHandle, SoftBodyTearEvent}; use crate::dynamics::{ ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, RigidBodySet, }; use crate::geometry::ColliderSet; +use crate::math::{DIM, Pose, Real, Vector}; +use crate::pipeline::EventHandler; #[cfg(not(feature = "std"))] #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use super::soft_body_set_proxies::{rebuilt_surface_shape, sync_soft_body}; -use super::soft_body_set_split::{ProxyPoses, rebase_proxy_attachments}; -use crate::dynamics::soft_body::{SoftBody, SoftBodyHandle, SoftBodyTearEvent}; -use super::{SoftBodyIslandEvent, SoftBodySet}; -use crate::pipeline::EventHandler; -use crate::math::{DIM, Pose, Real, Vector}; impl SoftBodySet { /// Applies the user's changes since the last step: wakes the soft bodies whose settings @@ -327,9 +327,14 @@ impl SoftBodySet { multibody_joints: &mut MultibodyJointSet, ) -> Option { let _ = multibody_joints; - self.change_topology(handle, islands, bodies, colliders, impulse_joints, |sb, event| { - sb.tear_topology(edges, cells, event) - }) + self.change_topology( + handle, + islands, + bodies, + colliders, + impulse_joints, + |sb, event| sb.tear_topology(edges, cells, event), + ) } /// Applies a topology change to a soft body (`change` returns whether anything changed and @@ -416,9 +421,14 @@ impl SoftBodySet { multibody_joints: &mut MultibodyJointSet, ) -> Option { let _ = multibody_joints; - self.change_topology(handle, islands, bodies, colliders, impulse_joints, |sb, event| { - sb.cut_topology(blade, event) - }) + self.change_topology( + handle, + islands, + bodies, + colliders, + impulse_joints, + |sb, event| sb.cut_topology(blade, event), + ) } /// Refreshes every mesh's vertex cache from the particles, before a narrow-phase update @@ -428,7 +438,9 @@ impl SoftBodySet { { use rayon::prelude::*; let mut bodies: Vec<&mut SoftBody> = self.bodies.iter_mut().map(|(_, sb)| sb).collect(); - bodies.par_iter_mut().for_each(|sb| sb.refresh_vertex_caches()); + bodies + .par_iter_mut() + .for_each(|sb| sb.refresh_vertex_caches()); } #[cfg(not(feature = "parallel"))] for (_, sb) in self.bodies.iter_mut() { diff --git a/src/dynamics/soft_body/soft_body_volume.rs b/src/dynamics/soft_body/soft_body_volume.rs index bd2907fea..3694399a6 100644 --- a/src/dynamics/soft_body/soft_body_volume.rs +++ b/src/dynamics/soft_body/soft_body_volume.rs @@ -154,7 +154,9 @@ impl SoftBody { particles.sort_unstable(); particles.dedup(); let rest_volume = Self::boundary_volume(&elements, &rest_position); - let old = self.volume_pieces.get(previous[particles[0] as usize] as usize); + let old = self + .volume_pieces + .get(previous[particles[0] as usize] as usize); pieces.push(SoftVolumePiece { elements: group, particles, diff --git a/src/dynamics/soft_body/soft_recovery_settings.rs b/src/dynamics/soft_body/soft_recovery_settings.rs index 7d82c1a4c..919f43e05 100644 --- a/src/dynamics/soft_body/soft_recovery_settings.rs +++ b/src/dynamics/soft_body/soft_recovery_settings.rs @@ -97,8 +97,7 @@ pub struct SoftRecoverySettings { /// overlap they are wrong-sided and freeze it). (default: `true`) pub overlap_edge_stand_down: bool, /// Bound on the velocity change the coupled constraint may hand any side per step, in multiples - /// of [`Self::recovery_pace`]; `Real::MAX` makes it hard. With - /// [`Self::overlap_velocity_correction`] it bounds the closing rate instead. (default: `1.0`) + /// of [`Self::recovery_pace`]; `Real::MAX` makes it hard. (default: `1.0`) pub overlap_constraint_pace: Real, /// What the per-point constraints (manifold constraints against rigid colliders, vertex /// constraints against soft surfaces) of the features inside a volume constraint's patch do, @@ -133,7 +132,6 @@ pub struct SoftRecoverySettings { /// Relative drop of the estimate that counts as progress for the patience. /// (default: `0.02`) pub overlap_progress_margin: Real, - } impl Default for SoftRecoverySettings { diff --git a/src/dynamics/soft_body/tearing/tearing.rs b/src/dynamics/soft_body/tearing/tearing.rs index 36cffb524..51bd27a4f 100644 --- a/src/dynamics/soft_body/tearing/tearing.rs +++ b/src/dynamics/soft_body/tearing/tearing.rs @@ -68,7 +68,8 @@ impl SoftBody { .iter() .enumerate() .filter(|(_, c)| { - (0..DIM + 1).any(|a| (a + 1..DIM + 1).any(|b| crosses(c.vertices[a], c.vertices[b]))) + (0..DIM + 1) + .any(|a| (a + 1..DIM + 1).any(|b| crosses(c.vertices[a], c.vertices[b]))) }) .map(|(i, _)| i as u32) .collect(); @@ -126,7 +127,11 @@ impl SoftBody { /// follow, and the boundary tables, rest volume and coloring are rebuilt. pub(super) fn finish_topology_change(&mut self, log: &SplitLog) { let mut sources = log.split_particles.clone(); - sources.extend(log.inserted.iter().flat_map(|&[a, b, p, q]| [(p, a), (q, b)])); + sources.extend( + log.inserted + .iter() + .flat_map(|&[a, b, p, q]| [(p, a), (q, b)]), + ); // This also matches the clusters to the updated cell list. self.inherit_cluster_membership(&sources); let remap_tables = super::collision_mesh::SoftTopologyRemap { diff --git a/src/dynamics/soft_body/tearing/tearing_crack.rs b/src/dynamics/soft_body/tearing/tearing_crack.rs index ab31fd1ce..47aecea9a 100644 --- a/src/dynamics/soft_body/tearing/tearing_crack.rs +++ b/src/dynamics/soft_body/tearing/tearing_crack.rs @@ -7,9 +7,9 @@ use crate::math::{DIM, Real, Vector}; use parry::utils::hashmap::HashMap; use super::super::SoftBody; +use super::tearing::pair; use super::tearing_event::SoftBodyTearEvent; use super::tearing_particle_split::{Fan, MeasureKind, SplitLog}; -use super::tearing::pair; /// The mean load of the fan elements on the given side of the split plane. fn side_load(fan: &Fan, side: usize, load: impl Fn(u32) -> Real) -> Real { @@ -18,7 +18,9 @@ fn side_load(fan: &Fan, side: usize, load: impl Fn(u32) -> Real) -> Real { .iter() .zip(&fan.sides) .filter(|(_, s)| **s == side) - .fold((0.0, 0usize), |(sum, count), (&e, _)| (sum + load(e), count + 1)); + .fold((0.0, 0usize), |(sum, count), (&e, _)| { + (sum + load(e), count + 1) + }); sum / count.max(1) as Real } @@ -139,7 +141,11 @@ impl SoftBody { load }; match kind { - MeasureKind::Cells => self.cells.iter().map(|c| load(c.stress, &c.vertices)).collect(), + MeasureKind::Cells => self + .cells + .iter() + .map(|c| load(c.stress, &c.vertices)) + .collect(), MeasureKind::Triangles => self.boundary.iter().map(|t| load(0.0, t)).collect(), MeasureKind::Segments => Vec::new(), } @@ -168,7 +174,11 @@ impl SoftBody { } for &x in &fa { for &y in &fb { - if self.edges.iter().any(|e| e.vertices == [x, y] || e.vertices == [y, x]) { + if self + .edges + .iter() + .any(|e| e.vertices == [x, y] || e.vertices == [y, x]) + { return Some((x, y)); } } diff --git a/src/dynamics/soft_body/tearing/tearing_cut.rs b/src/dynamics/soft_body/tearing/tearing_cut.rs index 92da82959..1fa3ff963 100644 --- a/src/dynamics/soft_body/tearing/tearing_cut.rs +++ b/src/dynamics/soft_body/tearing/tearing_cut.rs @@ -237,6 +237,7 @@ impl SoftBody { /// Inserts `p`, `q` where the blade crosses structural edge `ei` at `t`: the edge becomes /// `(a, p)`, `(q, b)` (proportional rest lengths and mass, surface segment too) and spanning /// edges go to `straddling`. `false` for a massless segment or, unless `force`, an endpoint. + #[allow(clippy::neg_cmp_op_on_partial_ord)] // A NaN measure must be rejected too. fn insert_into_segment( &mut self, ei: usize, diff --git a/src/dynamics/soft_body/tearing/tearing_helpers.rs b/src/dynamics/soft_body/tearing/tearing_helpers.rs index b391180bf..47508d42f 100644 --- a/src/dynamics/soft_body/tearing/tearing_helpers.rs +++ b/src/dynamics/soft_body/tearing/tearing_helpers.rs @@ -1,6 +1,6 @@ //! Tearing-related helpers of a soft body: connected pieces, tear resistance and particle damage. -use crate::alloc_prelude::*; use super::super::SoftBody; +use crate::alloc_prelude::*; use crate::math::Real; #[cfg(not(feature = "std"))] #[allow(unused_imports)] @@ -82,7 +82,10 @@ impl SoftBody { if !member(vertices[j]) { continue; } - let (a, b) = (find(&mut parent, vertices[i]), find(&mut parent, vertices[j])); + let (a, b) = ( + find(&mut parent, vertices[i]), + find(&mut parent, vertices[j]), + ); if a != b { parent[a.max(b) as usize] = a.min(b); } diff --git a/src/dynamics/soft_body/tearing/tearing_particle_split.rs b/src/dynamics/soft_body/tearing/tearing_particle_split.rs index 7fde623d0..7f53bcf37 100644 --- a/src/dynamics/soft_body/tearing/tearing_particle_split.rs +++ b/src/dynamics/soft_body/tearing/tearing_particle_split.rs @@ -245,7 +245,10 @@ impl SoftBody { { let rest = |v: u32| self.particles[v as usize].initial_rest_position; let t = self.boundary[i as usize]; - (rest(t[1]) - rest(t[0])).cross(rest(t[2]) - rest(t[0])).length() * 0.5 + (rest(t[1]) - rest(t[0])) + .cross(rest(t[2]) - rest(t[0])) + .length() + * 0.5 } #[cfg(feature = "dim2")] { @@ -389,7 +392,9 @@ impl SoftBody { let mut elements: HashMap> = HashMap::default(); self.for_each_measure_element(kind, |i, vertices| { elements.insert(i, vertices.to_vec()); - facets(vertices, pivot, |key| by_facet.entry(key).or_default().push(i)); + facets(vertices, pivot, |key| { + by_facet.entry(key).or_default().push(i) + }); }); let mut seen: HashMap = HashMap::default(); let mut stack: Vec = seeds.to_vec(); @@ -654,7 +659,8 @@ impl SoftBody { let cell = self.cells[fan.elements[k] as usize].vertices; let copy = copies[fan.groups[k]]; let on_facet = |w: &u32| { - *w == copy || (*w != v && fan.vertices[i].contains(w) && fan.vertices[j].contains(w)) + *w == copy + || (*w != v && fan.vertices[i].contains(w) && fan.vertices[j].contains(w)) }; if let Some(face) = cell_faces(cell) .into_iter() diff --git a/src/dynamics/soft_body/tearing/tearing_validate.rs b/src/dynamics/soft_body/tearing/tearing_validate.rs index 9f94fa2b4..120493fe3 100644 --- a/src/dynamics/soft_body/tearing/tearing_validate.rs +++ b/src/dynamics/soft_body/tearing/tearing_validate.rs @@ -6,11 +6,11 @@ use crate::math::DIM; use alloc::format; use parry::utils::hashmap::HashMap; -#[cfg(feature = "dim3")] -use super::tearing::pair; +use super::super::SoftBody; use super::super::soft_body::SOFT_BODY_OVERFLOW_COLOR; use super::super::soft_body_builder::element_vertices; -use super::super::SoftBody; +#[cfg(feature = "dim3")] +use super::tearing::pair; impl SoftBody { /// Checks the topology and its derived tables (element indices, every particle in a measure @@ -61,7 +61,10 @@ impl SoftBody { let nv = mesh.vertex_count(); if let super::collision_mesh::SoftMeshMapping::Skinned { bindings } = mesh.binding() { if bindings.len() != nv { - return Err(format!("mesh {mi} has {} bindings for {nv} vertices", bindings.len())); + return Err(format!( + "mesh {mi} has {} bindings for {nv} vertices", + bindings.len() + )); } // A body left without any cell has nothing to bind to. let dead = |b: &super::collision_mesh::SoftMeshCellBinding| { @@ -94,7 +97,9 @@ impl SoftBody { mesh.vertex_elements_offsets[v as usize + 1] as usize, ); if !mesh.vertex_elements[start..end].contains(&(i as u32)) { - return Err(format!("mesh {mi} element {i} missing from vertex {v}'s list")); + return Err(format!( + "mesh {mi} element {i} missing from vertex {v}'s list" + )); } let (start, end) = ( mesh.ring_offsets[v as usize] as usize, @@ -115,7 +120,9 @@ impl SoftBody { .get(e as usize) .is_some_and(|s| s.contains(&(v as u32))); if !ok { - return Err(format!("mesh {mi}: vertex {v} lists element {e} it is not in")); + return Err(format!( + "mesh {mi}: vertex {v} lists element {e} it is not in" + )); } } } diff --git a/src/dynamics/soft_body/tearing/tests.rs b/src/dynamics/soft_body/tearing/tests.rs index 1c45ab67b..63fb961ee 100644 --- a/src/dynamics/soft_body/tearing/tests.rs +++ b/src/dynamics/soft_body/tearing/tests.rs @@ -52,7 +52,10 @@ fn tears_never_split_off_a_chip() { ); } assert!(relative(total_mass, mass), "{kind:?}: mass changed"); - assert!(relative(total_measure, measure), "{kind:?}: rest measure changed"); + assert!( + relative(total_measure, measure), + "{kind:?}: rest measure changed" + ); } } @@ -67,7 +70,9 @@ fn seeded_components_follow_the_tear() { let sb = &world.soft_bodies[handle]; (sb.rest_measure(), sb.mass()) }; - let event = world.tear_soft_body(handle, &[4], &[]).expect("nothing tore"); + let event = world + .tear_soft_body(handle, &[4], &[]) + .expect("nothing tore"); // The tear split the rope in two bodies of four segments; the event says which is which. assert_eq!(event.seeds(), vec![[9, 4]]); assert_eq!(event.pieces.len(), 2); @@ -98,11 +103,16 @@ fn seeded_components_follow_the_tear() { edges.push([rope * 3, rope * 3 + 1]); edges.push([rope * 3 + 1, rope * 3 + 2]); } - let ropes = SoftBodyBuilder::new(positions).edges(edges).no_surface_collider(); + let ropes = SoftBodyBuilder::new(positions) + .edges(edges) + .no_surface_collider(); let (world, handle) = world_with(ropes); let sb = &world.soft_bodies[handle]; // Restricted graphs: particles 0 and 2 share no element, 0, 1 and 2 chain through 1. - assert_eq!(sb.seeded_components(Some(&[0, 2]), &[[0, 2]]), vec![vec![0], vec![2]]); + assert_eq!( + sb.seeded_components(Some(&[0, 2]), &[[0, 2]]), + vec![vec![0], vec![2]] + ); assert!(sb.seeded_components(Some(&[0, 1, 2]), &[[0, 2]]).is_empty()); assert_eq!( sb.seeded_components(None, &[[2, 3]]), @@ -120,7 +130,9 @@ fn min_piece_sets_the_smallest_piece_a_tear_leaves() { let (mut world, handle) = world_with(rope()); assert!(world.tear_soft_body(handle, &[2], &[]).is_none()); let (mut world, handle) = world_with(rope().min_piece(2)); - let event = world.tear_soft_body(handle, &[2], &[]).expect("nothing tore"); + let event = world + .tear_soft_body(handle, &[2], &[]) + .expect("nothing tore"); assert_eq!(event.pieces.len(), 2); for body in event.bodies() { let sb = &world.soft_bodies[body]; @@ -139,7 +151,9 @@ fn pinned_particles_are_never_split() { .min_piece(1) .pinned_particles([0, 8]); let (mut world, handle) = world_with(rope); - let event = world.tear_soft_body(handle, &[0], &[]).expect("nothing tore"); + let event = world + .tear_soft_body(handle, &[0], &[]) + .expect("nothing tore"); assert_eq!(event.split_particles, vec![(9, 1)]); assert_eq!(event.pieces.len(), 2); let stub = &world.soft_bodies[event.pieces[1].soft_body]; @@ -163,7 +177,9 @@ fn pinned_particles_are_never_split() { Vector::new(0.05, 2.0, -1.0), Vector::new(0.05, -1.0, 2.0), ]; - let event = world.cut_soft_body(handle, &blade).expect("the blade crossed the rope"); + let event = world + .cut_soft_body(handle, &blade) + .expect("the blade crossed the rope"); assert!(event.split_particles.is_empty()); assert_eq!(event.inserted_particles.len(), 2); let stub = &world.soft_bodies[event.pieces[1].soft_body]; diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/mod.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/mod.rs index 93730b51f..c05c6636f 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/mod.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/mod.rs @@ -10,12 +10,14 @@ mod soft_constraint_writeback; mod soft_constraints; mod soft_constraints_set; -use super::{soft_attachment, soft_contact_assembly, soft_contact_chunks, soft_element_constraint_assembly}; +use super::{ + soft_attachment, soft_contact_assembly, soft_contact_chunks, soft_element_constraint_assembly, +}; +pub(crate) use self::soft_constraint_plasticity::*; #[cfg(feature = "fem")] pub(crate) use self::soft_constraint_prepare::soft_fem_amplification_cap; -pub(crate) use self::soft_constraint_plasticity::*; pub(crate) use self::soft_constraint_rigid_coupling::*; +use self::soft_constraint_writeback::*; pub(crate) use self::soft_constraints::*; pub(crate) use self::soft_constraints_set::*; -use self::soft_constraint_writeback::*; diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs index afa2d236e..d43a45d3a 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_plasticity.rs @@ -149,7 +149,11 @@ mod tests { let err = frobenius_norm(&(cell.inv_rest_matrix * dm - Matrix::IDENTITY)); assert!(err < 1.0e-5, "rest matrix drifted: {err}"); // The strain rows' warm start followed the flow (full creep: nothing elastic left). - assert!(cell.impulses[..STRAIN_ROWS].iter().all(|i| i.abs() < 1.0e-6)); + assert!( + cell.impulses[..STRAIN_ROWS] + .iter() + .all(|i| i.abs() < 1.0e-6) + ); assert_eq!(cell.impulses[STRAIN_ROWS], 1.0); } @@ -172,7 +176,9 @@ mod tests { plastic_creep: 1.0, ..clay() }; - assert!(!plastic_flow(&mut cell, &strain, true, &rest0, &material, 0.3)); + assert!(!plastic_flow( + &mut cell, &strain, true, &rest0, &material, 0.3 + )); assert_eq!(cell.plastic_stretch, before.plastic_stretch); assert_eq!(cell.inv_rest_matrix, before.inv_rest_matrix); // The same rows on a cell that is not inverted flow, bounded by the plastic maximum. @@ -182,7 +188,10 @@ mod tests { } assert!(flowed > 0); let deviation = frobenius_norm(&(cell.plastic_stretch - Matrix::IDENTITY)); - assert!(deviation <= 1.0 + 1.0e-3, "flow past the maximum: {deviation}"); + assert!( + deviation <= 1.0 + 1.0e-3, + "flow past the maximum: {deviation}" + ); } /// The tear strain counts the plastic stretch: an unflowed cell reads its largest diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs index e034ab92e..46678908c 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_prepare.rs @@ -4,8 +4,8 @@ #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use crate::dynamics::solver::solver_body::{SolverBodies, SolverPose, SolverVel}; use crate::dynamics::SoftBody; +use crate::dynamics::solver::solver_body::{SolverBodies, SolverPose, SolverVel}; use crate::math::{AngVector, Matrix, Real, Vector}; use crate::utils::{AngularInertiaOps, CrossProduct}; @@ -109,10 +109,11 @@ impl SoftConstraintsSet { let p = &sb.particles[v as usize]; a += outer((pos(v as usize) - com) * p.mass, p.rest_position - rest_com); } - let rot = crate::dynamics::soft_body::soft_body_shape_matching::extract_rotation( - a, - awake_cluster.rotation, - ); + let rot = + crate::dynamics::soft_body::soft_body_shape_matching::extract_rotation( + a, + awake_cluster.rotation, + ); let mut dyn_mass = 0.0; let mut dyn_com = Vector::ZERO; for &v in cluster.particles() { @@ -232,7 +233,10 @@ impl SoftConstraintsSet { .map(|&i| (i, grads[i as usize])); side.gain = system.response_into(entries, out, &fem.params); side.u_max = out.iter().map(|u| u.length()).fold(0.0, Real::max); - w = if out.iter().any(|u| u.length() > super::soft_fem_amplification_cap() * side.gain) { + w = if out + .iter() + .any(|u| u.length() > super::soft_fem_amplification_cap() * side.gain) + { // Ill-conditioned (see `MAX_RESPONSE_AMPLIFICATION`): under-relax with the // lumped gain, the response still spreads the impulse consistently. w @@ -398,6 +402,7 @@ fn rigid_fit_velocity( /// The response amplification past which a FEM constraint falls back to its lumped gain (see /// `soft_fem::MAX_RESPONSE_AMPLIFICATION`); the constraint path never asks (no FEM body). +#[cfg(feature = "fem")] #[inline] pub(crate) fn soft_fem_amplification_cap() -> crate::math::Real { #[cfg(feature = "fem")] diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs index 94ddcf51f..a01ba9dc1 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraint_solve.rs @@ -180,11 +180,14 @@ impl SoftConstraintsSet { } // The rigid side's effective mass (its mass properties are step-constant, read from // the solver body). - let rigid = constraint.rigid.filter(|r| live(r.0)).map(|(slot, g_lin, g_ang)| { - let pose = bodies.get_pose(slot); - let ii_g = pose.ii.transform_vector(g_ang); - (slot, g_lin, g_ang, pose.im, ii_g) - }); + let rigid = constraint + .rigid + .filter(|r| live(r.0)) + .map(|(slot, g_lin, g_ang)| { + let pose = bodies.get_pose(slot); + let ii_g = pose.ii.transform_vector(g_ang); + (slot, g_lin, g_ang, pose.im, ii_g) + }); if let Some((_, g_lin, g_ang, im, ii_g)) = rigid { w += g_lin.gdot(im.component_mul(&g_lin)) + ii_g.gdot(g_ang); g_max = g_max.max(im.component_mul(&g_lin).length()); @@ -235,8 +238,9 @@ impl SoftConstraintsSet { let v = bodies.get_vel(slot); dc += g_lin.gdot(v.linear) + g_ang.gdot(v.angular); } - let mut total = - (constraint.impulse + inv_lhs * (dc + rhs_bias - cfm_gain * constraint.impulse)).max(0.0); + let mut total = (constraint.impulse + + inv_lhs * (dc + rhs_bias - cfm_gain * constraint.impulse)) + .max(0.0); // A soft overlap constraint's accumulated impulse is bounded by the pace (released elastic // energy is metered, not shot into the lighter side); a hard constraint keeps its velocity // part, and one applying the correction itself (`rhs > 0`) bounds the bias instead. @@ -324,7 +328,10 @@ impl SoftConstraintsSet { let num_scalar = self.scalar_constraints.len(); for ci in colors.chain(core::iter::once(usize::MAX)) { let (constraints, elastic_constraints) = if ci == usize::MAX { - (serial.scalar_constraints.clone(), serial.elastic_constraints.clone()) + ( + serial.scalar_constraints.clone(), + serial.elastic_constraints.clone(), + ) } else { let r = &self.color_ranges[ci]; (r.scalar_constraints.clone(), r.elastic_constraints.clone()) 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 57614db4d..49bc33eb0 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 @@ -9,9 +9,9 @@ use simba::scalar::{ComplexField as _, RealField as _}; use crate::dynamics::solver::solver_body::SolverBodies; use crate::math::{DIM, Real, Vector}; +use super::super::soft_element_constraint::SoftScalarConstraintWriteback; #[cfg(feature = "dim3")] use super::super::soft_element_constraint::dihedral_gradients; -use super::super::soft_element_constraint::SoftScalarConstraintWriteback; use super::*; impl SoftConstraintsSet { @@ -87,8 +87,8 @@ impl SoftConstraintsSet { 1.0 }; let cell = &mut sb.cells[constraint.element as usize]; - let impulses = - (constraint.strain_impulse.iter()).chain(core::iter::once(&constraint.vol_impulse)); + let impulses = (constraint.strain_impulse.iter()) + .chain(core::iter::once(&constraint.vol_impulse)); for (dst, src) in cell.impulses.iter_mut().zip(impulses) { *dst = crate::utils::canonicalize_zero(*src); } diff --git a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs index 77d811839..12ab745a2 100644 --- a/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs +++ b/src/dynamics/solver/soft_constraint/soft_constraints_set/soft_constraints.rs @@ -5,8 +5,8 @@ use core::ops::Range; #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use crate::dynamics::solver::solver_body::SolverBodies; use crate::dynamics::soft_body::SoftMeshId; +use crate::dynamics::solver::solver_body::SolverBodies; use crate::geometry::ColliderHandle; use crate::math::{AngVector, DIM, Matrix, Real, Rotation, Vector}; use crate::utils::ComponentMul; @@ -140,6 +140,7 @@ pub(crate) struct SoftVolumeConstraint { #[derive(Copy, Clone, Debug)] pub(crate) struct FemVolumeSide { pub start: u32, + #[cfg(feature = "fem")] pub gain: Real, pub u_max: Real, } 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 091123afd..b84e09462 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 @@ -33,7 +33,6 @@ pub(crate) struct AwakeSoftBody { /// particles' slots are contiguous, laid out group-major after the rigid bodies'). pub slot_start: usize, pub num_particles: usize, - pub shape_com: Vector, /// Range of this body's constraints in `shape_constraints`. pub shape_constraints: Range, /// Range of this body's volume constraints in `volume_constraints` (one per volume piece). diff --git a/src/dynamics/solver/soft_constraint/soft_contact.rs b/src/dynamics/solver/soft_constraint/soft_contact.rs index 70f1d6795..779a84fa6 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact.rs @@ -356,7 +356,14 @@ impl SoftContact { /// Applies the accumulated (warm-start) impulses. pub fn warmstart(&self, bodies: &mut SolverBodies, pool: &[Vector]) { - self.apply(bodies, pool, 0, self.dir, self.ii_torque_dir, self.impulse_normal); + self.apply( + bodies, + pool, + 0, + self.dir, + self.ii_torque_dir, + self.impulse_normal, + ); for j in 0..DIM - 1 { self.apply( bodies, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs index 303bb6e71..dd1364742 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly.rs @@ -4,9 +4,9 @@ #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use super::{AssemblyCtx, BodyContacts}; use super::super::soft_constraints_set::{SoftConstraintsSet, SoftOverlapConstraint}; use super::super::soft_contact::SoftContact; +use super::{AssemblyCtx, BodyContacts}; use crate::dynamics::{IntegrationParameters, RigidBodySet, SoftBodySet, SpringCoefficients}; use crate::geometry::{ColliderSet, NarrowPhase}; use crate::math::{Real, Vector}; @@ -134,7 +134,10 @@ impl SoftConstraintsSet { // The intersection-volume constraints of the group's bodies, with the contact softness. let ostart = self.overlap_constraints.len(); for ai in self.groups[gi].awake.clone() { - let mesh_id = per_body[ai].meshes.get(per_body[ai].current_mesh).map(|m| m.id); + let mesh_id = per_body[ai] + .meshes + .get(per_body[ai].current_mesh) + .map(|m| m.id); for constraint in per_body[ai].overlap_constraints.drain(..) { let gs = self.overlap_grads.len(); let n = constraint.grads.len(); @@ -166,7 +169,11 @@ impl SoftConstraintsSet { impulse, // A hard constraint's slack is speculative: consumed within the substep, // never past it. - speculative_inv_dt: if constraint.hard { 1.0 / group_dt(gi) } else { 0.0 }, + speculative_inv_dt: if constraint.hard { + 1.0 / group_dt(gi) + } else { + 0.0 + }, rigid: constraint.rigid, hard: constraint.hard, fem_sides: 0..0, diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs index 77575dc19..4415a6393 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_body_contacts.rs @@ -6,13 +6,13 @@ use simba::scalar::{ComplexField as _, RealField as _}; use crate::alloc_prelude::*; -use super::soft_contact_assembly_workspace::SOURCE_RIGID_VERTEX; -use super::{AssemblyCtx, BodyContacts, mesh_ref}; use super::super::soft_constraints_set::{SoftConstraintsSet, barycentric_weights}; use super::super::soft_contact::CONTACT_ANCHORS; use super::super::soft_contact::{SoftContact, SoftContactSource}; -use crate::dynamics::soft_body::SoftPatchConstraints; +use super::soft_contact_assembly_workspace::{RigidPatch, SOURCE_RIGID_VERTEX}; +use super::{AssemblyCtx, BodyContacts, mesh_ref}; use crate::dynamics::SoftBodyHandle; +use crate::dynamics::soft_body::SoftPatchConstraints; use crate::geometry::ColliderHandle; use crate::geometry::soft_contacts::SoftEdgePass; use crate::math::{DIM, Real, Vector}; @@ -22,7 +22,12 @@ impl SoftConstraintsSet { /// The contact constraints of awake body `ai`: its surface's narrow-phase pairs, then the /// vertex-vs-surface and edge-vs-edge constraints against the other soft surfaces met and against /// itself (self contacts). - pub(super) fn assemble_body_contacts(&self, ai: usize, ctx: &AssemblyCtx, out: &mut BodyContacts) { + pub(super) fn assemble_body_contacts( + &self, + ai: usize, + ctx: &AssemblyCtx, + out: &mut BodyContacts, + ) { use crate::geometry::contact_pair::NEW_CONTACT_BIT; let AssemblyCtx { island_id, @@ -101,7 +106,8 @@ impl SoftConstraintsSet { if same_cluster || !mesh.self_contacts_enabled() || !other_mesh.self_contacts_enabled() - || !sb.clusters_are_disjoint(mesh.id().cluster, other_ref.id.cluster) + || !sb + .clusters_are_disjoint(mesh.id().cluster, other_ref.id.cluster) { continue; } @@ -122,7 +128,11 @@ impl SoftConstraintsSet { let flips: &[bool] = if both { &[false, true] } else { &[false] }; for &flipped in flips { // The vertex pass whose surface side this pass assembles. - let surface = if flipped { other_handle } else { surface_handle }; + let surface = if flipped { + other_handle + } else { + surface_handle + }; let Some(vertex_pass) = detected.vertex_pass_on(surface) else { continue; }; @@ -176,8 +186,7 @@ impl SoftConstraintsSet { // The other side's solver body (its contact point is tracked in that body's // CoM frame), if it is simulated in this island: a rigid body, or the // particle's solver body (its frame is at the particle). - let (other_slot, other_com_pose) = match other_rb - { + let (other_slot, other_com_pose) = match other_rb { Some(rb) if rb.ids.active_island_id == island_id as u32 && !rb.is_sleeping() @@ -268,21 +277,19 @@ impl SoftConstraintsSet { // Normal approach speed of the other body (rigid ones only: soft particles // count through their own body's speeds) toward the element's point. let rigid_other = other_rb.filter(|rb| rb.soft_body().is_none()); - let approach_speed = |element: &[u32], - weights: &[Real; DIM], - anchor: Vector, - dir: Vector| { - let Some(rb) = rigid_other else { - return 0.0; + let approach_speed = + |element: &[u32], weights: &[Real; DIM], anchor: Vector, dir: Vector| { + let Some(rb) = rigid_other else { + return 0.0; + }; + let v_other = + rb.linvel() + rb.angvel().gcross(anchor - rb.center_of_mass()); + let mut v_surface = Vector::ZERO; + for (k, v) in element.iter().enumerate() { + v_surface += mesh.vertex_velocity(sb, *v as usize) * weights[k]; + } + (v_other - v_surface).gdot(dir).max(0.0) }; - let v_other = - rb.linvel() + rb.angvel().gcross(anchor - rb.center_of_mass()); - let mut v_surface = Vector::ZERO; - for (k, v) in element.iter().enumerate() { - v_surface += mesh.vertex_velocity(sb, *v as usize) * weights[k]; - } - (v_other - v_surface).gdot(dir).max(0.0) - }; // A solid surface's reversed interior contacts (an intruder seen from inside, // a fold's far layer) are dropped: the volume constraints and the elasticity @@ -300,27 +307,31 @@ impl SoftConstraintsSet { // The features a volume constraint acts on (see `overlap_patch_constraints`). let patch_policy = params.soft_bodies.recovery.overlap_patch_constraints; - let rigid_patch: Option<(Vec, Vec<(Vector, Vector)>)> = - (patch_policy != SoftPatchConstraints::Keep) - .then(|| { - out.rigid_patches - .iter() - .find(|p| p.0 == other_handle) - .map(|p| (p.1.clone(), p.2.clone())) - }) - .flatten(); + let rigid_patch: Option = (patch_policy + != SoftPatchConstraints::Keep) + .then(|| { + out.rigid_patches + .iter() + .find(|p| p.0 == other_handle) + .map(|p| p.1.clone()) + }) + .flatten(); for (mi, manifold) in pair.manifolds().iter().enumerate() { let Some((element_id, element)) = manifold_element(manifold) else { continue; }; if let Some(p) = self_particle { - if mesh.self_contact_excluded(sb, p, element, surface_co.contact_skin()) { + if mesh.self_contact_excluded(sb, p, element, surface_co.contact_skin()) + { continue; } } let in_patch = rigid_patch.as_ref().is_some_and(|(depths, _)| { element.iter().any(|&v| { - depths.get(v as usize).copied().unwrap_or(Real::NEG_INFINITY) + depths + .get(v as usize) + .copied() + .unwrap_or(Real::NEG_INFINITY) > Real::NEG_INFINITY }) }); @@ -390,7 +401,14 @@ impl SoftConstraintsSet { .flat_map(|sc| contact_points(sc).map(move |p| (sc, p))) { let point_id = (sc.contact_id[0] & !NEW_CONTACT_BIT) as usize; - let (positions, weights, surface_point0, other_anchor, sc_dist, warm_share) = point; + let ( + positions, + weights, + surface_point0, + other_anchor, + sc_dist, + warm_share, + ) = point; // A constraint disagreeing with the volume constraint (see // `overlap_patch_constraints`) pushes the surface into the rigid body // along the nearest cell's normal: stood down, or bent along it. @@ -411,8 +429,7 @@ impl SoftConstraintsSet { let mut depth = 0.0; for k in 0..DIM { if let Some(&v) = element.get(k) { - depth += weights[k] - * depths[v as usize].max(0.0); + depth += weights[k] * depths[v as usize].max(0.0); } } Some((-n, -depth, false)) @@ -420,7 +437,8 @@ impl SoftConstraintsSet { } else { None }; - if disagreeing.is_some() && patch_policy == SoftPatchConstraints::StandDown + if disagreeing.is_some() + && patch_policy == SoftPatchConstraints::StandDown { continue; } @@ -467,10 +485,8 @@ impl SoftConstraintsSet { // A small ball (a soft particle) touching a surface vertex/edge while // its center projects inside a neighboring element is a ghost contact // (spurious normal): skipped; large balls and split endpoints are real. - if let Some(ball) = other_co - .shape() - .as_ball() - .filter(|_| !split_endpoints) + if let Some(ball) = + other_co.shape().as_ball().filter(|_| !split_endpoints) { let size = (positions[1] - positions[0]).length(); if ball.radius < size { diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs index 598589f59..fdb71afb2 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_edge_contacts.rs @@ -4,11 +4,10 @@ #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; - -use super::{BodyContacts, SOURCE_EDGE_CONTACT, mesh_of, mesh_ref}; use super::super::soft_constraints_set::SoftConstraintsSet; use super::super::soft_contact::CONTACT_ANCHORS; use super::super::soft_contact::{SoftContact, SoftContactElement, SoftContactSource}; +use super::{BodyContacts, SOURCE_EDGE_CONTACT, mesh_of, mesh_ref}; use crate::dynamics::soft_body::{SoftCollisionMesh, SoftEdgeContact}; use crate::dynamics::{IntegrationParameters, SoftBody}; use crate::geometry::soft_contacts::SoftEdgePass; @@ -52,7 +51,10 @@ impl SoftConstraintsSet { return; } #[cfg(feature = "dim3")] - if !params.soft_bodies.recovery.edge_speculation && mesh.is_closed() && other_mesh.is_closed() { + if !params.soft_bodies.recovery.edge_speculation + && mesh.is_closed() + && other_mesh.is_closed() + { return; } // While a volume constraint owns a crossed closed-closed pair, its edge constraints stand down @@ -138,7 +140,15 @@ impl SoftConstraintsSet { -c.dir, ) } else { - (c.edge, c.other_edge, c.bcoords, c.other_bcoords, c.point, c.other_point, c.dir) + ( + c.edge, + c.other_edge, + c.bcoords, + c.other_bcoords, + c.point, + c.other_point, + c.dir, + ) }; let dist = c.dist; let (va, vb) = (mesh.edge_vertices(ea), other_mesh.edge_vertices(eb)); diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs index a6c6cccce..0abe8fa46 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_vertex_contacts.rs @@ -6,14 +6,14 @@ use simba::scalar::{ComplexField as _, RealField as _}; use crate::alloc_prelude::*; -use super::soft_contact_assembly_volume_constraints::{emit_volume_bins, mirrored_bins}; -use super::{AssemblyCtx, BodyContacts, SOURCE_VERTEX_CONTACT, mesh_of, mesh_ref}; use super::super::soft_constraints_set::SoftConstraintsSet; use super::super::soft_contact::CONTACT_ANCHORS; use super::super::soft_contact::{SoftContact, SoftContactElement, SoftContactSource}; +use super::soft_contact_assembly_volume_constraints::{emit_volume_bins, mirrored_bins}; +use super::{AssemblyCtx, BodyContacts, SOURCE_VERTEX_CONTACT, mesh_of, mesh_ref}; +use crate::dynamics::SoftBody; use crate::dynamics::soft_body::SoftPatchConstraints; use crate::dynamics::soft_body::{SoftCollisionMesh, SoftVertexContact}; -use crate::dynamics::SoftBody; use crate::geometry::soft_contacts::{SoftVertexPass, SoftVolumePatch, VolumeBin, element_plane}; use crate::geometry::{Collider, ColliderHandle}; use crate::math::{DIM, Real, Vector}; @@ -97,9 +97,27 @@ impl SoftConstraintsSet { }; // The vertex side and the element side. let (vb, vb_mesh, vb_slots, vb_co, eb, eb_mesh, eb_slots, eb_co) = if flipped { - (sb, mesh, Some(own_slots), surface_co, other, other_mesh, other_slots, other_co) + ( + sb, + mesh, + Some(own_slots), + surface_co, + other, + other_mesh, + other_slots, + other_co, + ) } else { - (other, other_mesh, other_slots, other_co, sb, mesh, Some(own_slots), surface_co) + ( + other, + other_mesh, + other_slots, + other_co, + sb, + mesh, + Some(own_slots), + surface_co, + ) }; // Crossings between the two surfaces (see `soft_contacts`): the pair's keep-apart // constraints are wrong-sided there and would freeze the crossing (or push material through @@ -292,7 +310,11 @@ impl SoftConstraintsSet { // and drops when the neighborhood straddles the element. let flagged = if is_self { out.tangled_vertices.get(v).copied().unwrap_or(false) - || out.tangled_elements.get(c.element as usize).copied().unwrap_or(false) + || out + .tangled_elements + .get(c.element as usize) + .copied() + .unwrap_or(false) } else { out.cross_tangled_vertices.get(v).copied().unwrap_or(false) || out diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs index 239b21c50..887bf685c 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_volume_constraints.rs @@ -6,8 +6,8 @@ use simba::scalar::{ComplexField as _, RealField as _}; use crate::alloc_prelude::*; -use super::{AssemblyCtx, BodyContacts, MeshContacts, OverlapConstraintWorkspace, material_pace}; use super::super::soft_constraints_set::SoftConstraintsSet; +use super::{AssemblyCtx, BodyContacts, MeshContacts, OverlapConstraintWorkspace, material_pace}; use crate::dynamics::soft_body::SoftPatchConstraints; use crate::dynamics::soft_body::{SoftCollisionMesh, SoftOverlapState, SoftVolumeContact}; use crate::dynamics::{IntegrationParameters, SoftBody}; @@ -141,8 +141,10 @@ pub(super) fn emit_volume_bins( // The warm impulse of every entry, and the multiplier fitted to them (least // squares along the new gradients): what the solve starts from. let (warm, warm_impulses, warm_rigid) = if hard { - let warm_impulses: Vec = - particles.iter().map(|&(side, p)| prev_warm_of(side, p)).collect(); + let warm_impulses: Vec = particles + .iter() + .map(|&(side, p)| prev_warm_of(side, p)) + .collect(); let mut num = 0.0; let mut den = 0.0; for (&(_, _, g, _), &p) in grads.iter().zip(&warm_impulses) { @@ -213,8 +215,16 @@ fn overlap_arming( error: Real, ) -> OverlapArming { let margin = params.soft_bodies.recovery.overlap_progress_margin; - let patience = params.soft_bodies.recovery.overlap_patience.min(u16::MAX as u32) as u16; - let previous = mesh.overlap_states.iter().find(|s| s.other == other).copied(); + let patience = params + .soft_bodies + .recovery + .overlap_patience + .min(u16::MAX as u32) as u16; + let previous = mesh + .overlap_states + .iter() + .find(|s| s.other == other) + .copied(); let state = match previous { Some(prev) => { let needed = if prev.stalled { 5.0 * margin } else { margin }; @@ -361,7 +371,7 @@ impl SoftConstraintsSet { ); if recovery.overlap_patch_constraints != SoftPatchConstraints::Keep { out.rigid_patches - .push((other_handle, patch.depth.clone(), cells)); + .push((other_handle, (patch.depth.clone(), cells))); } } } diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs index 22201bf71..b26b7ffef 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_workspace.rs @@ -11,11 +11,15 @@ use super::super::soft_contact::SoftContact; use crate::dynamics::soft_body::{ SoftCollisionMesh, SoftEdgeContact, SoftMeshId, SoftMeshRef, SoftVertexContact, }; +use crate::dynamics::soft_body::{SoftOverlapState, SoftVolumeContact}; use crate::dynamics::{IntegrationParameters, RigidBodySet, SoftBody, SoftBodyHandle, SoftBodySet}; use crate::geometry::{Collider, ColliderHandle, ColliderSet, NarrowPhase}; -use crate::dynamics::soft_body::{SoftOverlapState, SoftVolumeContact}; use crate::math::{AngVector, Real, Rotation, Vector}; +/// A rigid collider's volume-constraint patch: the surface vertices' depths in it +/// (`NEG_INFINITY` outside), and the constraint's cells (center, normal). +pub(super) type RigidPatch = (Vec, Vec<(Vector, Vector)>); + /// The collision mesh a soft body's collider holds. pub(super) fn mesh_of(sb: &SoftBody, collider: ColliderHandle) -> Option<&SoftCollisionMesh> { sb.mesh_of(collider) @@ -97,7 +101,7 @@ pub(super) struct BodyContacts { /// The rigid pairs of the current mesh with a volume constraint (see /// `overlap_patch_constraints`): the collider, the surface vertices' depths in its patch /// (`NEG_INFINITY` outside), and the constraint's cells (center, normal). - pub(super) rigid_patches: Vec<(ColliderHandle, Vec, Vec<(Vector, Vector)>)>, + pub(super) rigid_patches: Vec<(ColliderHandle, RigidPatch)>, } /// An intersection-volume constraint assembled by one body (see `SoftOverlapConstraint`): the diff --git a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs index 1e997e06f..7fc9dc4b5 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_assembly/soft_contact_assembly_writeback.rs @@ -6,10 +6,10 @@ use simba::scalar::{ComplexField as _, RealField as _}; use crate::alloc_prelude::*; -use super::soft_contact_assembly_workspace::SOURCE_RIGID_VERTEX; -use super::{SOURCE_EDGE_CONTACT, SOURCE_VERTEX_CONTACT}; use super::super::soft_constraints_set::SoftConstraintsSet; use super::super::soft_contact::SoftContact; +use super::soft_contact_assembly_workspace::SOURCE_RIGID_VERTEX; +use super::{SOURCE_EDGE_CONTACT, SOURCE_VERTEX_CONTACT}; use crate::dynamics::soft_body::SoftOverlapWarm; use crate::geometry::NarrowPhase; use crate::math::{AngVector, DIM, Vector}; @@ -18,9 +18,8 @@ use crate::math::{AngVector, DIM, Vector}; /// before its pair's constraints report; a point split into endpoint constraints sums them): /// the step's total for the events, the last substep's for the warm start. fn report_impulses(data: &mut crate::geometry::ContactData, c: &SoftContact) { - data.impulse = crate::utils::canonicalize_zero( - data.impulse + c.impulse_normal_acc + c.impulse_normal, - ); + data.impulse = + crate::utils::canonicalize_zero(data.impulse + c.impulse_normal_acc + c.impulse_normal); data.warmstart_impulse = crate::utils::canonicalize_zero(data.warmstart_impulse + c.impulse_normal); #[cfg(feature = "dim2")] diff --git a/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs b/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs index 2a38083c3..334ffc919 100644 --- a/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs +++ b/src/dynamics/solver/soft_constraint/soft_contact_chunks.rs @@ -254,7 +254,10 @@ impl SoftConstraintsSet { let rows = &workspace.chunk_rows[workspace.chunk_row_offsets[chunk] as usize ..workspace.chunk_row_offsets[chunk + 1] as usize]; let contacts = &self.contacts[constraints.clone()]; - let slots = || rows.iter().flat_map(|&i| written_slots(&contacts[i as usize])); + let slots = || { + rows.iter() + .flat_map(|&i| written_slots(&contacts[i as usize])) + }; let color = workspace .color_slots .iter() @@ -314,17 +317,27 @@ impl SoftConstraintsSet { for color in &self.contact_colors[g.contact_colors.clone()] { let mut seen = vec![usize::MAX; num_slots]; for chunk in color.clone() { - for &row in &self.contact_chunk_constraints[self.contact_chunks[chunk].clone()] { + for &row in &self.contact_chunk_constraints[self.contact_chunks[chunk].clone()] + { let c = &self.contacts[row as usize]; for slot in written_slots(c) { let prev = core::mem::replace(&mut seen[slot as usize], chunk); if prev != usize::MAX && prev != chunk { - let key = |ch: usize| workspace.keys[workspace.ordered[ch - chunks_start]]; + let key = |ch: usize| { + workspace.keys[workspace.ordered[ch - chunks_start]] + }; let (k1, k2) = (key(prev), key(chunk)); panic!( "slot {slot} shared: chunk {prev} (soft {:?} rigid {}) and chunk {chunk} (soft {:?} rigid {}); row body {} element {} soft_other {} fem {:?} particles {:?}", - k1.soft, k1.rigid, k2.soft, k2.rigid, c.body, c.element.is_some(), c.soft_other, - c.fem.iter().map(|f| f.is_some()).collect::>(), c.particles + k1.soft, + k1.rigid, + k2.soft, + k2.rigid, + c.body, + c.element.is_some(), + c.soft_other, + c.fem.iter().map(|f| f.is_some()).collect::>(), + c.particles ); } } diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs index 95327d780..d9222107a 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_elastic_constraint.rs @@ -1,5 +1,6 @@ //! The elastic-cell block constraint: strain blocks, their inverse, the per-substep update, warm start and the corotational and Neo-Hookean solves. +use super::*; use crate::dynamics::SoftBody; use crate::dynamics::soft_body::soft_body_shape_matching::extract_rotation; use crate::dynamics::solver::solver_body::SolverBodies; @@ -8,7 +9,6 @@ use crate::utils::{DotProduct, RotationOps}; #[cfg(not(feature = "std"))] #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use super::*; /// Number of strain rows of an elastic cell: the independent components of the symmetric /// strain tensor. @@ -219,6 +219,7 @@ impl SoftElasticConstraint { /// Gradient of strain row `r` w.r.t. particle `p`'s velocity, in the cell frame: /// `½ (coeffs[p][b] e_a + coeffs[p][a] e_b)`. + #[cfg(any(feature = "fem", test))] #[inline] pub(crate) fn strain_gradient( coeffs: &[Vector; MAX_CONSTRAINT_PARTICLES], @@ -270,7 +271,9 @@ impl SoftElasticConstraint { a[(r, r)] = 1.0; } } - let mut inv = a.cholesky().map_or_else(StrainMatrix::zeros, |chol| chol.inverse()); + let mut inv = a + .cholesky() + .map_or_else(StrainMatrix::zeros, |chol| chol.inverse()); for (r, inert) in inert.iter().enumerate() { if *inert { inv.row_mut(r).fill(0.0); diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs index 0a3e6f919..3a36c7124 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/soft_neo_hookean.rs @@ -1,10 +1,10 @@ //! The stable Neo-Hookean state of an elastic cell: stiffness update, energy gradient and Hessian. +use super::*; use crate::math::{DIM, Matrix, Real, Vector}; #[cfg(not(feature = "std"))] #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use super::*; /// Stable Neo-Hookean state of a [`SoftElasticConstraint`]: the energy of Smith et al. 2018 in /// Kim & Eberle 2020's form, bounded below when inverted. `update` refreshes the gradient every @@ -65,7 +65,8 @@ impl NeoHookeanConstraint { self.m = (hess * dt + StrainMatrix::from_diagonal(&self.damping)) * dt; // `(M A + I)⁻¹`, with the effective-mass block `A_rs = Σₚ wₚ gᵣ[p]·gₛ[p]` built from // `G = Σₚ wₚ cₚ cₚᵀ` (the gradients are axis-aligned). - let weighted: [Vector; MAX_CONSTRAINT_PARTICLES] = core::array::from_fn(|p| coeffs[p] * im[p]); + let weighted: [Vector; MAX_CONSTRAINT_PARTICLES] = + core::array::from_fn(|p| coeffs[p] * im[p]); let a = Self::strain_block_from_g(&outer_sum(&weighted, coeffs)); *inv_a = (self.m * a + StrainMatrix::identity()) .try_inverse() diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs index b2f564049..488544b5d 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint/test.rs @@ -1,5 +1,6 @@ //! Finite-difference and linear-algebra checks of the soft constraints. +use super::*; use crate::dynamics::SoftBody; use crate::dynamics::soft_body::soft_body_shape_matching::extract_rotation; use crate::math::{DIM, Matrix, Real, Rotation, Vector}; @@ -7,7 +8,6 @@ use crate::utils::{DotProduct, RotationOps}; #[cfg(not(feature = "std"))] #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use super::*; #[cfg(feature = "dim2")] fn base_positions() -> [Vector; 3] { @@ -51,7 +51,10 @@ fn gradients_match_finite_differences() { impulse_bounds: [-Real::MAX, Real::MAX], }; #[cfg(feature = "dim2")] - let kinds = [(SoftScalarConstraintKind::Distance, 2), (SoftScalarConstraintKind::CellVolume, 3)]; + let kinds = [ + (SoftScalarConstraintKind::Distance, 2), + (SoftScalarConstraintKind::CellVolume, 3), + ]; #[cfg(feature = "dim3")] let kinds = [ (SoftScalarConstraintKind::Distance, 2), @@ -170,7 +173,8 @@ fn elastic_cell_gradients_match_finite_differences() { let local = rot.inverse() * e; for r in 0..STRAIN_ROWS { let fd = (sp[r] - sm[r]) / (2.0 * eps); - let expected = SoftElasticConstraint::strain_gradient(&constraint.coeffs, r, k).gdot(local); + let expected = + SoftElasticConstraint::strain_gradient(&constraint.coeffs, r, k).gdot(local); assert!( (fd - expected).abs() < 2.0e-3 * (1.0 + expected.abs()), "strain row {r}: particle {k} axis {axis}: fd {fd} vs {expected}" @@ -259,7 +263,8 @@ fn neo_hookean_gradient_and_hessian_match_finite_differences() { let minus = perturbed(strain, r, -eps); let (gp, _) = NeoHookeanConstraint::gradient_and_hessian(mu, lambda, &plus); let (gm, _) = NeoHookeanConstraint::gradient_and_hessian(mu, lambda, &minus); - h.row_mut(r).copy_from(&((gp - gm) / (2.0 * eps)).transpose()); + h.row_mut(r) + .copy_from(&((gp - gm) / (2.0 * eps)).transpose()); } h }; @@ -305,7 +310,8 @@ fn neo_hookean_gradient_and_hessian_match_finite_differences() { ); // At rest the Hessian is the linear-elastic block: 2μ + λ on the diagonal rows, λ between // them, 4μ on the shears. - let (grad, hess) = NeoHookeanConstraint::gradient_and_hessian(mu, lambda, &StrainVector::zeros()); + let (grad, hess) = + NeoHookeanConstraint::gradient_and_hessian(mu, lambda, &StrainVector::zeros()); assert!(grad.amax() < 1.0e-6, "nonzero rest gradient {grad:?}"); for r in 0..STRAIN_ROWS { for s in 0..STRAIN_ROWS { @@ -333,7 +339,12 @@ fn cofactor_is_the_determinant_gradient() { let cof = cofactor(&f); for k in 0..DIM { let err = (cof.col(k) - expected.col(k)).abs().max_element(); - assert!(err < 1.0e-5, "column {k}: {:?} vs {:?}", cof.col(k), expected.col(k)); + assert!( + err < 1.0e-5, + "column {k}: {:?} vs {:?}", + cof.col(k), + expected.col(k) + ); } } @@ -351,11 +362,18 @@ fn symmetric_eigen_handles_degenerate_matrices() { let orthonormality = vectors.transpose() * vectors; for k in 0..DIM { let err = (rebuilt.col(k) - m.col(k)).abs().max_element(); - let ortho = (orthonormality.col(k) - Matrix::IDENTITY.col(k)).abs().max_element(); - assert!(err < 1.0e-4 && ortho < 1.0e-4, "{m:?}: column {k}: {err} {ortho}"); + let ortho = (orthonormality.col(k) - Matrix::IDENTITY.col(k)) + .abs() + .max_element(); + assert!( + err < 1.0e-4 && ortho < 1.0e-4, + "{m:?}: column {k}: {err} {ortho}" + ); } let max = max_symmetric_eigenvalue(&m); - assert!((max - values.max_element()).abs() < 1.0e-4, "{m:?}: {max} vs {values:?}"); + assert!( + (max - values.max_element()).abs() < 1.0e-4, + "{m:?}: {max} vs {values:?}" + ); } } - diff --git a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs index 2f357fa2b..5b7896dca 100644 --- a/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs +++ b/src/dynamics/solver/soft_constraint/soft_element_constraint_assembly.rs @@ -10,13 +10,14 @@ use crate::alloc_prelude::*; use core::ops::Range; use core::sync::atomic::AtomicBool; +use super::soft_constraints_set::AwakeCluster; use super::soft_constraints_set::{ AwakeSoftBody, SoftColorRange, SoftConstraintsSet, SoftGroupLayout, SoftShapeConstraint, SoftVolumeConstraint, }; use super::soft_element_constraint::{ CorotationalConstraint, MAX_CONSTRAINT_PARTICLES, NeoHookeanConstraint, STRAIN_ROWS, - SoftElasticModel, SoftElasticConstraint, SoftScalarConstraint, SoftScalarConstraintKind, + SoftElasticConstraint, SoftElasticModel, SoftScalarConstraint, SoftScalarConstraintKind, SoftScalarConstraintWriteback, StrainMatrix, StrainVector, }; use crate::dynamics::soft_body::SOFT_BODY_OVERFLOW_COLOR; @@ -28,7 +29,6 @@ use crate::dynamics::{ use crate::math::{DIM, Matrix, Real, Vector}; use crate::utils::RotationOps; use na::SimdRealField; -use super::soft_constraints_set::AwakeCluster; /// Number of element colors: the parallel ones, then the overflow color. const NUM_COLORS: usize = SOFT_BODY_OVERFLOW_COLOR as usize + 1; @@ -89,7 +89,12 @@ impl BodyCursors { #[inline] fn push_shape_constraint(&mut self, constraint: SoftShapeConstraint) { // SAFETY: the body's own slot (see `ConstraintSlots`). - unsafe { self.slots.shape_constraints.add(self.shape_constraints).write(constraint) }; + unsafe { + self.slots + .shape_constraints + .add(self.shape_constraints) + .write(constraint) + }; self.shape_constraints += 1; } } @@ -310,21 +315,22 @@ impl SoftConstraintsSet { let num_chunks = num_elements.div_ceil(FILL_CHUNK); let chunk_range = |c: usize| c * FILL_CHUNK..((c + 1) * FILL_CHUNK).min(num_elements); - let mut chunk_counts: Vec<([usize; NUM_COLORS], [usize; NUM_COLORS])> = - (0..num_chunks) - .into_par_iter() - .map(|c| { - let mut counts = ([0; NUM_COLORS], [0; NUM_COLORS]); - this.count_body_constraints( - ai, - &mut counts.0, - &mut counts.1, - chunk_range(c), - ); - counts - }) - .collect(); + let mut chunk_counts: Vec<([usize; NUM_COLORS], [usize; NUM_COLORS])> = (0 + ..num_chunks) + .into_par_iter() + .map(|c| { + let mut counts = ([0; NUM_COLORS], [0; NUM_COLORS]); + this.count_body_constraints( + ai, + &mut counts.0, + &mut counts.1, + chunk_range(c), + ); + counts + }) + .collect(); for (scalar, elastic) in chunk_counts.iter_mut() { + #[allow(clippy::manual_memcpy)] // Each slot is swapped, not copied. for color in 0..NUM_COLORS { let (count, elastic_count) = (scalar[color], elastic[color]); scalar[color] = cursors.scalar_constraints[color]; @@ -366,7 +372,10 @@ impl SoftConstraintsSet { 0..num_elements, ); this.fill_body_shape_constraints(ai, dt, &mut cursors); - debug_assert_eq!(cursors.shape_constraints, this.awake[ai].shape_constraints.end); + debug_assert_eq!( + cursors.shape_constraints, + this.awake[ai].shape_constraints.end + ); }; #[cfg(feature = "parallel")] { @@ -439,8 +448,7 @@ impl SoftConstraintsSet { sb.rebuild_volume_pieces(&[]); } - let awake_clusters = - sb + let awake_clusters = sb .clusters .iter() .enumerate() @@ -449,14 +457,20 @@ impl SoftConstraintsSet { let mut target_linvel = Default::default(); let mut target_angvel = Default::default(); - if let (Some(prev_target_pose), Some(target_pose)) = (&c.prev_shape_matching_target, &c.shape_matching_target) { + if let (Some(prev_target_pose), Some(target_pose)) = + (&c.prev_shape_matching_target, &c.shape_matching_target) + { let dpos = target_pose.translation - prev_target_pose.translation; let drot = target_pose.rotation * prev_target_pose.rotation.inverse(); target_linvel = dpos * inv_dt; #[cfg(feature = "dim2")] - { target_angvel = drot.angle() * inv_dt; } + { + target_angvel = drot.angle() * inv_dt; + } #[cfg(feature = "dim3")] - { target_angvel = drot.to_scaled_axis() * inv_dt; } + { + target_angvel = drot.to_scaled_axis() * inv_dt; + } } AwakeCluster { @@ -480,7 +494,6 @@ impl SoftConstraintsSet { frozen, slot_start: 0, num_particles: sb.particles.len(), - shape_com: Vector::ZERO, shape_constraints: 0..0, volume_constraints: 0..0, damping_factor: 0.0, @@ -608,13 +621,13 @@ impl SoftConstraintsSet { }; let pos_of = |i: u32| sb.particles[i as usize].position; let base_constraint = |kind: SoftScalarConstraintKind, - writeback: SoftScalarConstraintWriteback, - element: usize, - vertices: &[u32], - rest: Real, - erp: Real, - cfm: Real, - impulse: Real| + writeback: SoftScalarConstraintWriteback, + element: usize, + vertices: &[u32], + rest: Real, + erp: Real, + cfm: Real, + impulse: Real| -> SoftScalarConstraint { let mut solver_ids = [u32::MAX; MAX_CONSTRAINT_PARTICLES]; let mut pos = [Vector::ZERO; MAX_CONSTRAINT_PARTICLES]; @@ -699,7 +712,13 @@ impl SoftConstraintsSet { out.push_constraint(d.color, constraint); } - for (ci, c) in sb.cells.iter().enumerate().take(cells.end).skip(cells.start) { + for (ci, c) in sb + .cells + .iter() + .enumerate() + .take(cells.end) + .skip(cells.start) + { match sb.cell_model { // Integrated implicitly by the FEM solver instead (see `soft_fem`). SoftBodyCellModel::Volume if sb.uses_fem() => continue, @@ -761,7 +780,11 @@ impl SoftConstraintsSet { let w = block[(r, r)]; // The row's deviatoric stiffness: the stress-based strain of both models // reads through it (an isotropic material's deviatoric response is `2μ`). - let k_dev = if r < DIM { 2.0 * mu * vol } else { 4.0 * mu * vol }; + let k_dev = if r < DIM { + 2.0 * mu * vol + } else { + 4.0 * mu * vol + }; let k = if neo_hookean { NeoHookeanConstraint::rest_stiffness(mu, lambda, r) * vol } else { @@ -843,7 +866,6 @@ impl SoftConstraintsSet { } } } - } /// Builds the shape-matching constraints of awake body `ai` into its slots (see @@ -883,7 +905,11 @@ impl SoftConstraintsSet { inv_lhs: Vector::ZERO, cfm_gain: Vector::ZERO, rhs: Vector::ZERO, - impulse: cluster.shape_impulses.get(k).copied().unwrap_or(Vector::ZERO), + impulse: cluster + .shape_impulses + .get(k) + .copied() + .unwrap_or(Vector::ZERO), fem: None, inv_lhs_block: Matrix::ZERO, cfm_gain_block: Matrix::ZERO, @@ -934,7 +960,11 @@ fn elastic_cell_terms( sb: &SoftBody, slots: &[u32], c: &crate::dynamics::SoftBodyCell, -) -> ([Vector; MAX_CONSTRAINT_PARTICLES], [Real; MAX_CONSTRAINT_PARTICLES], Real) { +) -> ( + [Vector; MAX_CONSTRAINT_PARTICLES], + [Real; MAX_CONSTRAINT_PARTICLES], + Real, +) { let coeffs = SoftElasticConstraint::coefficients(&c.inv_rest_matrix); let mut im = [0.0; MAX_CONSTRAINT_PARTICLES]; for (k, &v) in c.vertices.iter().enumerate() { diff --git a/src/dynamics/solver/soft_fem/soft_fem_set.rs b/src/dynamics/solver/soft_fem/soft_fem_set.rs index 8a335348c..63ba4f6f5 100644 --- a/src/dynamics/solver/soft_fem/soft_fem_set.rs +++ b/src/dynamics/solver/soft_fem/soft_fem_set.rs @@ -4,8 +4,10 @@ use super::system::SoftFemSystem; use crate::alloc_prelude::*; use crate::dynamics::solver::soft_constraint::SoftConstraintsSet; -use crate::dynamics::solver::soft_constraint::soft_constraints_set::{FemOverlapSide, FemVolumeSide}; use crate::dynamics::solver::soft_constraint::soft_attachment::FemAttachment; +use crate::dynamics::solver::soft_constraint::soft_constraints_set::{ + FemOverlapSide, FemVolumeSide, +}; use crate::dynamics::solver::soft_constraint::soft_contact::{CONTACT_ANCHORS, FemContactSide}; use crate::dynamics::solver::solver_body::SolverBodies; use crate::dynamics::{SoftBody, SoftBodyHandle, SoftBodySet, SoftFemParameters}; @@ -32,11 +34,17 @@ struct ActiveFem { #[derive(Copy, Clone, Debug)] enum FemConstraintRef { /// `contacts[constraint].fem[side]` (the combined same-body response when it `covers_other`). - Contact { constraint: u32, side: u8 }, + Contact { + constraint: u32, + side: u8, + }, Attachment(u32), Shape(u32), /// `overlap_fem_sides[side]` of `overlap_constraints[constraint]`. - Overlap { constraint: u32, side: u32 }, + Overlap { + constraint: u32, + side: u32, + }, } // SAFETY: the pointers are only dereferenced during the solve, where the stage discipline gives @@ -254,7 +262,8 @@ impl SoftFemSet { continue; }; let entry = &self.active[fi]; - if r.particle < entry.first_slot || r.particle >= entry.first_slot + entry.num_particles { + if r.particle < entry.first_slot || r.particle >= entry.first_slot + entry.num_particles + { continue; } r.fem = Some(FemAttachment { @@ -299,6 +308,7 @@ impl SoftFemSet { let entry = &self.active[fi]; constraint.fem = Some(FemVolumeSide { start: reserve(entry.num_particles as usize), + #[cfg(feature = "fem")] gain: 0.0, u_max: 0.0, }); @@ -480,9 +490,8 @@ impl SoftFemSet { } } let anchors = &anchors[..num]; - let directions: [Vector; DIM] = core::array::from_fn(|k| { - if k == 0 { c.dir } else { c.tangents[k - 1] } - }); + let directions: [Vector; DIM] = + core::array::from_fn(|k| if k == 0 { c.dir } else { c.tangents[k - 1] }); let mut gains = [0.0; DIM]; for (k, d) in directions.iter().enumerate() { let d = *d; diff --git a/src/dynamics/solver/soft_fem/soft_fem_skyline.rs b/src/dynamics/solver/soft_fem/soft_fem_skyline.rs index 2c3dc371c..bbe95b159 100644 --- a/src/dynamics/solver/soft_fem/soft_fem_skyline.rs +++ b/src/dynamics/solver/soft_fem/soft_fem_skyline.rs @@ -86,6 +86,7 @@ impl SkylineCholesky { /// Factorizes `matrix` (same pattern as the one this was built from). Returns `false` when /// the matrix is not positive definite (the factor is then unusable). + #[allow(clippy::neg_cmp_op_on_partial_ord)] // A NaN pivot must fail the factorization. pub fn factorize(&mut self, matrix: &BlockMatrix) -> bool { let Self { inv_perm, @@ -168,7 +169,11 @@ impl SkylineCholesky { let mut sum = scratch[i]; if fi < i { let row = &values[ri..ri + i - fi]; - sum -= row.iter().zip(&scratch[fi..i]).map(|(l, y)| l * y).sum::(); + sum -= row + .iter() + .zip(&scratch[fi..i]) + .map(|(l, y)| l * y) + .sum::(); } scratch[i] = sum / values[ri + i - fi]; } @@ -256,7 +261,12 @@ fn reverse_cuthill_mckee<'a>(n: usize, neighbors: impl Fn(usize) -> &'a [u32]) - let v = order[head] as usize; head += 1; sorted.clear(); - sorted.extend(neighbors(v).iter().copied().filter(|&w| !visited[w as usize])); + sorted.extend( + neighbors(v) + .iter() + .copied() + .filter(|&w| !visited[w as usize]), + ); sorted.sort_unstable_by_key(|&w| (degree(w as usize), w)); for &w in &sorted { if !visited[w as usize] { diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs index 6bc923640..01512d45e 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_assemble.rs @@ -59,7 +59,8 @@ impl SoftFemSystem { // below for inverted cells, and its tangent is floored mode-wise at the rest // curvature so one implicit step per substep needs no line search. let lambda = cell.lambda + cell.mu; - let (s, kappa, gradient) = NeoHookeanConstraint::gradient(cell.mu, lambda, &cell.strain); + let (s, kappa, gradient) = + NeoHookeanConstraint::gradient(cell.mu, lambda, &cell.strain); let stale = (cell.strain - cell.tangent_strain) .iter() .any(|d| d.abs() > STIFFNESS_UPDATE_STRAIN); @@ -166,7 +167,8 @@ impl SoftFemSystem { for q in 0..MAX_CONSTRAINT_PARTICLES { let slot = blocks[p * MAX_CONSTRAINT_PARTICLES + q]; if slot != u32::MAX { - self.matrix.blocks[slot as usize] += outer_product(grad[p], grad[q]) * scale; + self.matrix.blocks[slot as usize] += + outer_product(grad[p], grad[q]) * scale; } } } @@ -318,7 +320,8 @@ impl SoftFemSystem { } } } - let edge_plasticity = material.edge_plastic_yield > 0.0 && material.edge_plastic_creep > 0.0; + let edge_plasticity = + material.edge_plastic_yield > 0.0 && material.edge_plastic_creep > 0.0; #[cfg(feature = "dim3")] if edge_plasticity { for (dihedral, d) in self.dihedrals.iter().zip(sb.dihedrals.iter_mut()) { diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs index 70864af7e..2d1171d63 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_prepare.rs @@ -1,10 +1,10 @@ //! The step-constant preparation: the sparsity pattern, the per-element material and shape functions, and the particle state. +use super::super::soft_fem_sparse::BlockMatrix; use super::SoftFemSystem; -use super::soft_fem_system_elements::{CELL_BLOCKS, FemCell, FemSpring, FemVolumeCell}; #[cfg(feature = "dim3")] use super::soft_fem_system_elements::FemDihedral; -use super::super::soft_fem_sparse::BlockMatrix; +use super::soft_fem_system_elements::{CELL_BLOCKS, FemCell, FemSpring, FemVolumeCell}; #[cfg(feature = "dim3")] use crate::dynamics::solver::soft_constraint::soft_element_constraint::dihedral_gradients; use crate::dynamics::solver::soft_constraint::soft_element_constraint::{ diff --git a/src/dynamics/solver/soft_fem/system/soft_fem_system_response.rs b/src/dynamics/solver/soft_fem/system/soft_fem_system_response.rs index f05d8c52b..38e1b1d1a 100644 --- a/src/dynamics/solver/soft_fem/system/soft_fem_system_response.rs +++ b/src/dynamics/solver/soft_fem/system/soft_fem_system_response.rs @@ -1,8 +1,8 @@ //! The step matrix: its factorization at the step start, and the response and column solves the constraints go through. +use super::super::soft_fem_skyline::SkylineCholesky; use super::SoftFemSystem; use crate::dynamics::SoftFemParameters; -use super::super::soft_fem_skyline::SkylineCholesky; use crate::math::{DIM, Matrix, Real, Vector}; impl SoftFemSystem { diff --git a/src/dynamics/solver/soft_fem/system/tests.rs b/src/dynamics/solver/soft_fem/system/tests.rs index f5f37b4cc..fcbbb01b6 100644 --- a/src/dynamics/solver/soft_fem/system/tests.rs +++ b/src/dynamics/solver/soft_fem/system/tests.rs @@ -3,8 +3,8 @@ use super::SoftFemSystem; use crate::alloc_prelude::*; use crate::dynamics::SoftFemParameters; -use crate::math::Vector; use crate::dynamics::{SoftBodyBuilder, SoftBodyCellModel, SoftBodyMaterial}; +use crate::math::Vector; /// A response solves `A_step u = Jᵀ` (checked against the matrix), is zero at the pinned /// particles and has gain `J · u`; the dense and conjugate-gradient paths agree. @@ -51,7 +51,10 @@ fn response_solves_the_step_matrix() { system.step_matrix.mul(&pool, &mut lhs); for (k, l) in lhs.iter().enumerate() { let rhs = if k == 1 { Vector::ONE } else { Vector::ZERO }; - assert!((*l - rhs).length() < 1.0e-3 * (1.0 + rhs.length()), "row {k}: {l:?} vs {rhs:?}"); + assert!( + (*l - rhs).length() < 1.0e-3 * (1.0 + rhs.length()), + "row {k}: {l:?} vs {rhs:?}" + ); } assert!((gain - Vector::ONE.dot(pool[1])).abs() < 1.0e-6); results.push((gain, pool)); @@ -69,7 +72,10 @@ fn response_solves_the_step_matrix() { #[cfg(feature = "dim2")] let entries = [(1u32, Vector::new(0.3, -0.7)), (2u32, Vector::ONE * 0.5)]; #[cfg(feature = "dim3")] - let entries = [(1u32, Vector::new(0.3, -0.7, 0.2)), (2u32, Vector::ONE * 0.5)]; + let entries = [ + (1u32, Vector::new(0.3, -0.7, 0.2)), + (2u32, Vector::ONE * 0.5), + ]; let mut direct = vec![Vector::ZERO; n]; let gain = system.response_into(entries.iter().copied(), &mut direct, ¶ms); system.clear_columns(); @@ -92,7 +98,12 @@ fn response_solves_the_step_matrix() { ); } let (dense, cg) = (&results[0], &results[1]); - assert!((dense.0 - cg.0).abs() < 1.0e-3 * dense.0, "gains {} vs {}", dense.0, cg.0); + assert!( + (dense.0 - cg.0).abs() < 1.0e-3 * dense.0, + "gains {} vs {}", + dense.0, + cg.0 + ); for (a, b) in dense.1.iter().zip(&cg.1) { assert!((*a - *b).length() < 1.0e-3 * (1.0 + a.length())); } diff --git a/src/dynamics/solver/staged_island_solver/init.rs b/src/dynamics/solver/staged_island_solver/init.rs index f464c3cf3..476deff93 100644 --- a/src/dynamics/solver/staged_island_solver/init.rs +++ b/src/dynamics/solver/staged_island_solver/init.rs @@ -243,8 +243,12 @@ impl StagedIslandSolver { } } let vs = &self.velocity_solver; - self.soft_constraints - .assemble_clusters(bodies, colliders, &vs.solver_bodies, &jointed_proxies); + self.soft_constraints.assemble_clusters( + bodies, + colliders, + &vs.solver_bodies, + &jointed_proxies, + ); // The FEM bodies' answers to the step's constraints, once every constraint exists: planned here, // computed by the workers' first stage. @@ -263,7 +267,8 @@ impl StagedIslandSolver { let soft = &self.soft_constraints; let approx_chunks = num_two_body / SIMD_WIDTH + joint_indices.len() / 4 - + (soft.scalar_constraints.len() + soft.shape_constraints.len() + soft.contacts.len()) / 8 + + (soft.scalar_constraints.len() + soft.shape_constraints.len() + soft.contacts.len()) + / 8 + fem_chunks; let num_workers = num_workers.clamp(1, (approx_chunks / 16).max(1)); diff --git a/src/dynamics/solver/staged_island_solver/joints.rs b/src/dynamics/solver/staged_island_solver/joints.rs index f702f1225..2ff3fc66d 100644 --- a/src/dynamics/solver/staged_island_solver/joints.rs +++ b/src/dynamics/solver/staged_island_solver/joints.rs @@ -8,11 +8,13 @@ use crate::dynamics::solver::categorization::categorize_joints; use crate::dynamics::solver::contact_constraint::joint_num_constraints; use crate::dynamics::solver::joint_constraint::JointConstraintBuilder; use crate::dynamics::solver::joint_constraint::JointConstraintBuilderSimd; +use crate::dynamics::solver::joint_constraint::{ + joint_uses_angular_axes, soft_frame_angular_degenerate, +}; use crate::dynamics::solver::reset_buffer; use crate::dynamics::{ JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyHandle, RigidBodySet, }; -use crate::dynamics::solver::joint_constraint::{joint_uses_angular_axes, soft_frame_angular_degenerate}; use parry::math::SIMD_WIDTH; use super::{GroupJointRanges, JOINT_BATCH, LAYOUT_REF_WORKERS, StagedIslandSolver}; @@ -187,10 +189,9 @@ impl StagedIslandSolver { let joint = &impulse_joints[*joint_i].weight; // No SIMD for joints whose angular part may be masked out; rare enough. - let ang_maskable = joint_uses_angular_axes( - &joint.data, - ) && (soft_frame_angular_degenerate(&bodies[joint.body1]) - || soft_frame_angular_degenerate(&bodies[joint.body2])); + let ang_maskable = joint_uses_angular_axes(&joint.data) + && (soft_frame_angular_degenerate(&bodies[joint.body1]) + || soft_frame_angular_degenerate(&bodies[joint.body2])); if joint.data.supports_simd_constraints() && !ang_maskable { self.joint_sig_workspace .push((joint.data.simd_row_signature(), *joint_i)); @@ -366,11 +367,10 @@ impl StagedIslandSolver { for joint_i in color { let joint = &impulse_joints[*joint_i].weight; // No SIMD for joints whose angular part may be masked out; rare enough. - let ang_maskable = joint_uses_angular_axes( - &joint.data, - ) && (soft_frame_angular_degenerate(&bodies[joint.body1]) - || soft_frame_angular_degenerate(&bodies[joint.body2])); - if joint.data.supports_simd_constraints() && !ang_maskable { + let ang_maskable = joint_uses_angular_axes(&joint.data) + && (soft_frame_angular_degenerate(&bodies[joint.body1]) + || soft_frame_angular_degenerate(&bodies[joint.body2])); + if joint.data.supports_simd_constraints() && !ang_maskable { self.joint_sig_workspace .push((joint.data.simd_row_signature(), *joint_i)); } else { diff --git a/src/dynamics/solver/staged_island_solver/solve.rs b/src/dynamics/solver/staged_island_solver/solve.rs index b38aa1e57..955f79bc5 100644 --- a/src/dynamics/solver/staged_island_solver/solve.rs +++ b/src/dynamics/solver/staged_island_solver/solve.rs @@ -446,7 +446,12 @@ unsafe fn solve_soft_constraints( let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; let claimed_len = claimed.len(); for constraint_id in claimed { - soft.solve_shape_constraint(constraint_id, solver_bodies, soft_update, soft_warmstart); + soft.solve_shape_constraint( + constraint_id, + solver_bodies, + soft_update, + soft_warmstart, + ); } done += claimed_len; } @@ -459,14 +464,22 @@ unsafe fn solve_soft_constraints( let soft = unsafe { &mut *ctx.soft_constraints }; let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; for constraint_id in sg.shape_constraints.clone() { - soft.solve_shape_constraint(constraint_id, solver_bodies, soft_update, soft_warmstart); + soft.solve_shape_constraint( + constraint_id, + solver_bodies, + soft_update, + soft_warmstart, + ); } sync.complete(stage, 1, 1); } stage = sync.sync(stage, 1); } - if sg.serial.len() > 0 || !sg.volume_constraints.is_empty() || !sg.overlap_constraints.is_empty() { + if sg.serial.len() > 0 + || !sg.volume_constraints.is_empty() + || !sg.overlap_constraints.is_empty() + { if worker_id == 0 { let soft = unsafe { &mut *ctx.soft_constraints }; let solver_bodies = unsafe { &mut (*ctx.velocity_solver).solver_bodies }; diff --git a/src/geometry/broad_phase_bvh/update.rs b/src/geometry/broad_phase_bvh/update.rs index 46e69a77b..e3a198b2f 100644 --- a/src/geometry/broad_phase_bvh/update.rs +++ b/src/geometry/broad_phase_bvh/update.rs @@ -160,8 +160,9 @@ impl BroadPhaseBvh { &mut self.update_batch_statuses, ); - for ((modified, _, _), status) in - update_workspace.iter().zip(self.update_batch_statuses.iter()) + for ((modified, _, _), status) in update_workspace + .iter() + .zip(self.update_batch_statuses.iter()) { let leaf_index = modified.into_raw_parts().0; match status { @@ -193,7 +194,8 @@ impl BroadPhaseBvh { // mixes moved colliders with newly added ones would otherwise build a // different tree here than it does there. let mut deferred_inserts: Vec = Vec::new(); - for (i, (modified, aabb, change_detection_skin)) in update_workspace.iter().enumerate() { + for (i, (modified, aabb, change_detection_skin)) in update_workspace.iter().enumerate() + { let leaf_index = modified.into_raw_parts().0; // `..._if_present` reports a missing leaf through the lookup it already // performs, so deferring insertions costs no extra probe. diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index d0ef780bf..a1c2f8e45 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -96,18 +96,6 @@ impl Collider { self.changes.insert(ColliderChanges::DEFORMED); } - /// Moves a soft-body particle ball to its particle's world position (a simulation-driven - /// motion, like a deformation: no wake-up, no narrow-phase workspace invalidation). `frame` - /// is the parent proxy's pose: the ball's local pose is kept consistent with it. - pub(crate) fn deform_position(&mut self, frame: &Pose, position: Vector) { - let world = Pose::from_translation(position); - if let Some(parent) = self.parent.as_mut() { - parent.pos_wrt_parent = frame.inverse() * world; - } - self.pos = ColliderPosition(world); - self.changes.insert(ColliderChanges::DEFORMED); - } - /// Moves a soft-body surface collider along with its cluster proxy (`frame` is the proxy's /// pose, the collider keeps its pose relative to it), as a deformation: no wake-up, no /// workspace invalidation. The shape's vertices are expressed in the resulting frame. @@ -627,9 +615,7 @@ impl Collider { params: &IntegrationParameters, bodies: &RigidBodySet, ) -> Aabb { - let parent = self - .parent - .and_then(|p| Some((p, bodies.get(p.handle)?))); + let parent = self.parent.and_then(|p| Some((p, bodies.get(p.handle)?))); // Take soft-ccd into account by growing the aabb. let next_pose = parent.and_then(|(p, parent)| { (parent.soft_ccd_prediction() > 0.0).then(|| { @@ -642,8 +628,7 @@ impl Collider { let soft_motion_margin = parent.map_or(0.0, |(_, parent)| parent.soft_motion_margin); let prediction_distance = params.prediction_distance(); - let mut aabb = - self.compute_collision_aabb(prediction_distance / 2.0 + soft_motion_margin); + let mut aabb = self.compute_collision_aabb(prediction_distance / 2.0 + soft_motion_margin); if let Some(next_pose) = next_pose { let next_aabb = self .shape diff --git a/src/geometry/collider_components.rs b/src/geometry/collider_components.rs index d61979323..35fc59029 100644 --- a/src/geometry/collider_components.rs +++ b/src/geometry/collider_components.rs @@ -338,8 +338,16 @@ impl ActiveCollisionTypes { // rb_type1 and rb_type2. // Soft-frame proxies collide as dynamic bodies. - let rb_type1 = if rb_type1.is_soft_frame() { RigidBodyType::Dynamic } else { rb_type1 }; - let rb_type2 = if rb_type2.is_soft_frame() { RigidBodyType::Dynamic } else { rb_type2 }; + let rb_type1 = if rb_type1.is_soft_frame() { + RigidBodyType::Dynamic + } else { + rb_type1 + }; + let rb_type2 = if rb_type2.is_soft_frame() { + RigidBodyType::Dynamic + } else { + rb_type2 + }; ((self.bits() >> (rb_type1 as u32 * 4)) & 0b0000_1111) & (1 << rb_type2 as u32) != 0 || ((self.bits() >> (rb_type2 as u32 * 4)) & 0b0000_1111) & (1 << rb_type1 as u32) != 0 } diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 1f42ebe4f..e83d6bbe0 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -285,11 +285,15 @@ impl ColliderSet { // The shape's `ORIENTED` flag says whether the closed mesh encloses solid matter. #[cfg(feature = "dim2")] let oriented = coll.shape().as_polyline().is_some_and(|polyline| { - polyline.flags().contains(parry::shape::PolylineFlags::ORIENTED) + polyline + .flags() + .contains(parry::shape::PolylineFlags::ORIENTED) }); #[cfg(feature = "dim3")] let oriented = coll.shape().as_trimesh().is_some_and(|trimesh| { - trimesh.flags().contains(parry::shape::TriMeshFlags::ORIENTED) + trimesh + .flags() + .contains(parry::shape::TriMeshFlags::ORIENTED) }); mesh.oriented = oriented; coll.set_density(0.0); @@ -576,7 +580,9 @@ fn mesh_geometry( #[cfg(feature = "dim2")] let (deformable, geometry) = { use parry::shape::PolylineFlags; - let polyline = shape.as_polyline().ok_or(SoftBindingError::UnsupportedShape)?; + let polyline = shape + .as_polyline() + .ok_or(SoftBindingError::UnsupportedShape)?; ( polyline.flags().contains(PolylineFlags::DEFORMABLE), (polyline.vertices().to_vec(), polyline.indices().to_vec()), @@ -585,7 +591,9 @@ fn mesh_geometry( #[cfg(feature = "dim3")] let (deformable, geometry) = { use parry::shape::TriMeshFlags; - let trimesh = shape.as_trimesh().ok_or(SoftBindingError::UnsupportedShape)?; + let trimesh = shape + .as_trimesh() + .ok_or(SoftBindingError::UnsupportedShape)?; ( trimesh.flags().contains(TriMeshFlags::DEFORMABLE), (trimesh.vertices().to_vec(), trimesh.indices().to_vec()), diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 3636d10df..a7dc3b060 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -226,6 +226,7 @@ pub struct ContactPair { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] #[repr(C, u8)] +#[allow(clippy::large_enum_variant)] // Boxing would move the manifolds off the fixed layout. pub enum PairContacts { /// Contact manifolds: every pair but the pairs of two soft surfaces. Rigid(RigidPairContacts), @@ -335,9 +336,9 @@ impl RigidPairContacts { self.soft = None; } + /// Is there any active contact among this side's solver manifolds? pub fn has_any_active_contact(&self) -> bool { - self - .solver_manifolds() + self.solver_manifolds() .iter() .any(|m| !m.data.solver_contacts.is_empty()) } @@ -503,13 +504,15 @@ impl ContactPair { /// solver actually sees (the solver clusters if any, else the plain manifolds). #[cfg_attr(feature = "parallel", allow(dead_code))] // Single-threaded solver path. pub(crate) fn solver_manifolds_mut(&mut self) -> &mut [ContactManifold] { - self.rigid_mut().map_or(&mut [], |r| r.solver_manifolds_mut()) + self.rigid_mut() + .map_or(&mut [], |r| r.solver_manifolds_mut()) } /// The pair's solver graph colour (`SOLVER_COLOR_UNCOLORED` for a pair of two soft /// surfaces, which the rigid solver never sees). pub(crate) fn solver_color(&self) -> u8 { - self.rigid().map_or(SOLVER_COLOR_UNCOLORED, |r| r.solver_color) + self.rigid() + .map_or(SOLVER_COLOR_UNCOLORED, |r| r.solver_color) } /// Is there any active contact in this contact pair? diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index deb31f299..c7d09c136 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -23,9 +23,8 @@ pub(crate) use self::contact_pair::relative_pose_drift; #[cfg(feature = "alloc")] pub use self::contact_pair::{ ContactData, ContactId, ContactManifoldData, ContactPair, IntersectionPair, NEW_CONTACT_BIT, - PairContacts, RigidPairContacts, - SimdSolverContact, SolverContact, SolverContactGeneric, SolverContacts, SolverFlags, is_bouncy, - is_bouncy_simd, + PairContacts, RigidPairContacts, SimdSolverContact, SolverContact, SolverContactGeneric, + SolverContacts, SolverFlags, is_bouncy, is_bouncy_simd, }; #[cfg(feature = "alloc")] pub use self::interaction_graph::{ @@ -36,11 +35,11 @@ pub use self::interaction_groups::{Group, InteractionGroups, InteractionTestMode pub use self::mesh_converter::{MeshConverter, MeshConverterError}; #[cfg(feature = "alloc")] pub use self::narrow_phase::NarrowPhase; +pub(crate) use self::narrow_phase::soft_contacts; pub use self::narrow_phase::soft_contacts::{ SoftContactImpulse, SoftEdgeCandidate, SoftEdgePass, SoftPairContacts, SoftVertexCandidate, SoftVertexHits, SoftVertexPass, SoftVolumePatch, VolumeBin, }; -pub(crate) use self::narrow_phase::soft_contacts; #[cfg(feature = "alloc")] pub use parry::utils::Array2; diff --git a/src/geometry/narrow_phase/contacts.rs b/src/geometry/narrow_phase/contacts.rs index ea4400673..65d72b085 100644 --- a/src/geometry/narrow_phase/contacts.rs +++ b/src/geometry/narrow_phase/contacts.rs @@ -6,12 +6,12 @@ use super::pair_update::{ self, HintsPtr, OUTCOME_CLEARED_IN_GRAPH, OUTCOME_FULL, OUTCOME_FULL_COMPOSITE, OUTCOME_RECYCLED_REQUALIFIED, PairTransition, }; +use super::soft_contacts::SoftDetectionCtx; use super::{ NarrowPhase, assign_pair_solver_color, clear_pair_solver_color, collect_pairs_to_update, pack_color_body_info, strong_wake_sleeping_side, unpack_color_body_info, }; use crate::alloc_prelude::*; -use super::soft_contacts::SoftDetectionCtx; use crate::dynamics::{ ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, RigidBodySet, SoftBodySet, diff --git a/src/geometry/narrow_phase/pair_update.rs b/src/geometry/narrow_phase/pair_update.rs index 2a6bfbd9c..10f9b8c3c 100644 --- a/src/geometry/narrow_phase/pair_update.rs +++ b/src/geometry/narrow_phase/pair_update.rs @@ -13,8 +13,8 @@ use crate::dynamics::{ RigidBodyType, }; use crate::geometry::{ - BoundingVolume, ColliderChanges, ColliderSet, ContactData, - ContactManifoldData, ContactPair, SolverContact, SolverFlags, + BoundingVolume, ColliderChanges, ColliderSet, ContactData, ContactManifoldData, ContactPair, + SolverContact, SolverFlags, }; use crate::math::{MAX_MANIFOLD_POINTS, Real}; use crate::pipeline::{ @@ -195,8 +195,10 @@ pub(super) fn process_pair( let rb_type2 = rb2.map(|rb| rb.body_type).unwrap_or(RigidBodyType::Fixed); #[cfg(feature = "dim3")] - let soft_surface_pair = co1.deformable_mesh_ref.is_some() || co2.deformable_mesh_ref.is_some(); - let two_soft_surfaces = co1.deformable_mesh_ref.is_some() && co2.deformable_mesh_ref.is_some(); + let soft_surface_pair = + co1.deformable_mesh_ref.is_some() || co2.deformable_mesh_ref.is_some(); + let two_soft_surfaces = + co1.deformable_mesh_ref.is_some() && co2.deformable_mesh_ref.is_some(); // Two colliders of one soft body: its collision meshes decide between themselves (see // `soft_contacts::update_pair`), but a rigid collider hung on a cluster proxy must not @@ -320,8 +322,7 @@ pub(super) fn process_pair( 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); let soft_body_prediction = soft_margin1 + soft_margin2; - let contact_skin_sum = - co1.contact_skin() + co2.contact_skin(); + 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); let soft_ccd_prediction2 = rb2.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0); let effective_prediction_distance = if soft_ccd_prediction1 > 0.0 @@ -346,7 +347,9 @@ pub(super) fn process_pair( break 'emit_events; } - prediction_distance.max(dt * (linvel1 - linvel2).length()) + contact_skin_sum + soft_body_prediction + prediction_distance.max(dt * (linvel1 - linvel2).length()) + + contact_skin_sum + + soft_body_prediction } else { prediction_distance + contact_skin_sum + soft_body_prediction }; @@ -441,7 +444,9 @@ pub(super) fn process_pair( &mut rigid.workspace, ); - if let Some(soft) = soft.filter(|_| co1.deformable_mesh_ref.is_some() || co2.deformable_mesh_ref.is_some()) { + if let Some(soft) = + soft.filter(|_| co1.deformable_mesh_ref.is_some() || co2.deformable_mesh_ref.is_some()) + { // Soft-vs-rigid: the predictive vertex contacts of the elements the manifolds reached // (within the solver contacts' separation bound), and the intersection volume. super::soft_contacts::update_pair_soft_rigid( @@ -716,8 +721,7 @@ pub(super) fn process_pair( // A pair without contacts has an (unknown) separation larger than // the prediction distance. Cap its recycle window by the // prediction distance so an incoming contact can't be missed. - let max_drift = if rigid.has_any_active_contact() - { + let max_drift = if rigid.has_any_active_contact() { contact_recycle_distance } else { contact_recycle_distance.min(prediction_distance) diff --git a/src/geometry/narrow_phase/soft_contacts/mod.rs b/src/geometry/narrow_phase/soft_contacts/mod.rs index 83ca08f72..abb2b7a26 100644 --- a/src/geometry/narrow_phase/soft_contacts/mod.rs +++ b/src/geometry/narrow_phase/soft_contacts/mod.rs @@ -13,14 +13,14 @@ mod soft_self_contacts; pub(crate) use soft_contacts_edge_pass::detect_edges; pub(crate) use soft_contacts_pairs::{update_pair_soft_rigid, update_pair_soft_soft}; -pub(crate) use soft_self_contacts::SoftSelfContacts; +use soft_contacts_types::Side; +pub(crate) use soft_contacts_types::{ + SelfTangles, SoftDetectionCtx, SoftRigidContacts, body_frozen, rest_gap_skins, +}; pub use soft_contacts_types::{ SoftContactImpulse, SoftEdgeCandidate, SoftEdgePass, SoftPairContacts, SoftRigidPatch, SoftVertexCandidate, SoftVertexHits, SoftVertexPass, SoftVolumePatch, }; -pub(crate) use soft_contacts_types::{ - SelfTangles, SoftDetectionCtx, SoftRigidContacts, body_frozen, rest_gap_skins, -}; -use soft_contacts_types::Side; pub(crate) use soft_contacts_vertex_pass::{detect_vertex_pass, element_plane, elements_cross}; pub use soft_contacts_volume::VolumeBin; +pub(crate) use soft_self_contacts::SoftSelfContacts; diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_driver.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_driver.rs index c95b904cf..405d39357 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_driver.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_driver.rs @@ -38,6 +38,11 @@ impl NarrowPhase { continue; } let collider = mesh.collider(); + // The `enhanced-determinism` map is an `IndexMap`, whose `remove` is a + // deprecated alias of `swap_remove`; hashbrown's only has `remove`. + #[cfg(feature = "enhanced-determinism")] + let payload = self.soft_self.swap_remove(&collider).unwrap_or_default(); + #[cfg(not(feature = "enhanced-determinism"))] let payload = self.soft_self.remove(&collider).unwrap_or_default(); jobs.push(( collider, @@ -52,8 +57,11 @@ impl NarrowPhase { // The entries left are those of the sleeping bodies (kept for their wake-up) and of // the removed meshes (dropped). let colliders = ctx.colliders; - self.soft_self - .retain(|h, _| colliders.get(*h).is_some_and(|c| c.deformable_mesh_ref.is_some())); + self.soft_self.retain(|h, _| { + colliders + .get(*h) + .is_some_and(|c| c.deformable_mesh_ref.is_some()) + }); let run = |(collider, mesh_ref, payload): &mut ( ColliderHandle, SoftMeshRef, diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs index 3cb3a3c5c..cfcc59b9e 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_edge_pass.rs @@ -72,7 +72,9 @@ pub(crate) fn detect_edges( let mut crossed_own: Vec = Vec::new(); let mut crossed_other: Vec = Vec::new(); let other_inv_pose = other_co.position().inverse(); - if !is_self && params.soft_bodies.recovery.cross_body_detection && params.soft_bodies.recovery.edge_stand_down + if !is_self + && params.soft_bodies.recovery.cross_body_detection + && params.soft_bodies.recovery.edge_stand_down { for i in 0..mesh.indices().len() { let element = mesh.element(i); @@ -188,8 +190,16 @@ impl EdgeScan<'_> { } // Tangled elements (inverted cells, or part of a surface self-crossing): // their self contacts stand down (see `detect_self_tangles`). - if self.tangled_elements.get(i as usize).copied().unwrap_or(false) - || self.tangled_elements.get(j as usize).copied().unwrap_or(false) + if self + .tangled_elements + .get(i as usize) + .copied() + .unwrap_or(false) + || self + .tangled_elements + .get(j as usize) + .copied() + .unwrap_or(false) { continue; } @@ -225,8 +235,10 @@ impl EdgeScan<'_> { self.other_mesh.cached_vertex(vb[0] as usize), self.other_mesh.cached_vertex(vb[1] as usize), ]; - if !box_a.intersects(&crate::geometry::Aabb::new(pb[0].min(pb[1]), pb[0].max(pb[1]))) - { + if !box_a.intersects(&crate::geometry::Aabb::new( + pb[0].min(pb[1]), + pb[0].max(pb[1]), + )) { continue; } // Near-parallel edges never cross: their proximity is a face contact, @@ -278,10 +290,9 @@ impl EdgeScan<'_> { (&rb[0], &rb[1]), ); let (ca, cb) = (la.barycentric_coordinates(), lb.barycentric_coordinates()); - let rest_gap = (ra[0] * ca[0] + ra[1] * ca[1] - - rb[0] * cb[0] - - rb[1] * cb[1]) - .length(); + let rest_gap = + (ra[0] * ca[0] + ra[1] * ca[1] - rb[0] * cb[0] - rb[1] * cb[1]) + .length(); rest_gap_skins(self.skins, rest_gap) } else { self.skins diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs index 67f87a891..dbd6ae396 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_pairs.rs @@ -14,8 +14,8 @@ use simba::scalar::{ComplexField as _, RealField as _}; use super::soft_contacts_types::SoftRigidVertexContact; use super::soft_contacts_volume::{detect_rigid_patch, detect_volume_patch}; use super::{ - SoftDetectionCtx, SoftVertexPass, SoftEdgePass, SoftPairContacts, body_frozen, detect_vertex_pass, - detect_edges, + SoftDetectionCtx, SoftEdgePass, SoftPairContacts, SoftVertexPass, body_frozen, detect_edges, + detect_vertex_pass, }; /// The edge-pass owner of a pair of meshes: the lower `(body handle, cluster, mesh)`, which is @@ -25,7 +25,6 @@ fn owns_edge_pass(a: SoftMeshRef, b: SoftMeshRef) -> bool { <= (b.body.0.into_raw_parts(), b.id.cluster, b.id.mesh) } - pub(crate) fn update_pair_soft_soft( pair: &mut ContactPair, co1: &Collider, @@ -136,7 +135,10 @@ fn detect_rigid_vertices( let mut previous = previous.iter().peekable(); let mut rest = &candidates[..]; while let Some(&(vertex, _)) = rest.first() { - let count = rest.iter().position(|c| c.0 != vertex).unwrap_or(rest.len()); + let count = rest + .iter() + .position(|c| c.0 != vertex) + .unwrap_or(rest.len()); let (group, tail) = rest.split_at(count); rest = tail; let point = mesh.vertex(sb, vertex as usize); 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 b4c376387..4a84c8ba1 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_types.rs @@ -4,8 +4,7 @@ use crate::alloc_prelude::*; use core::ops::Range; use crate::dynamics::{ - IntegrationParameters, RigidBodySet, SoftBody, SoftBodyHandle, SoftBodySet, - SoftCollisionMesh, + IntegrationParameters, RigidBodySet, SoftBody, SoftBodyHandle, SoftBodySet, SoftCollisionMesh, }; use crate::geometry::{Collider, ColliderHandle, ColliderSet}; use crate::math::{DIM, Real, Rotation, Vector}; @@ -126,16 +125,19 @@ pub struct SoftVertexPass { pub surface: ColliderHandle, /// The collider whose surface vertices are tested. pub vertices_of: ColliderHandle, - /// Elements of the surface side, and vertices of the vertex side, taking part in a - /// crossing between the two surfaces (empty while they do not cross): constraints touching them - /// may only expel, never hold. + /// Elements of the surface side taking part in a crossing between the two surfaces (empty + /// while they do not cross): constraints touching them may only expel, never hold. pub cross_tangled_elements: Vec, + /// Vertices of the vertex side taking part in a crossing, same rule as + /// [`Self::cross_tangled_elements`]. pub cross_tangled_vertices: Vec, - /// Element-granularity crossing flags on the vertex-side mesh, and the recorded - /// (surface-side, vertex-side) crossing element pairs. + /// Element-granularity crossing flags on the vertex-side mesh. pub cross_tangled_vb_elements: Vec, + /// The recorded (surface-side, vertex-side) crossing element pairs. pub cross_pairs: Vec<(u32, u32)>, + /// Per tested vertex of the vertex side, its candidates against the surface side. pub hits: Vec, + /// The candidate pool the [`Self::hits`] ranges index into. pub candidates: Vec, /// The volume normals guiding the crossing repulsion (see `crossing_repulsion_guide`): per /// grid cell, its center and its normal from the surface side into the vertex side (for a @@ -285,7 +287,9 @@ impl SoftPairContacts { /// The vertex pass whose surface side is `surface`. pub fn vertex_pass_on(&self, surface: ColliderHandle) -> Option<&SoftVertexPass> { - self.vertex_passes.iter().find(|pass| pass.surface == surface) + self.vertex_passes + .iter() + .find(|pass| pass.surface == surface) } /// Disables every candidate (the contact-modification hook's way of dropping the pair's diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs index 89182f06b..b96059bc8 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_vertex_pass.rs @@ -5,8 +5,8 @@ use crate::alloc_prelude::*; #[cfg(feature = "dim2")] use crate::dynamics::soft_body_crossing_tests::segments_cross; use crate::dynamics::{SoftBody, SoftCollisionMesh}; -use crate::math::{DIM, Real, Vector}; use crate::geometry::PointQueryWithLocation; +use crate::math::{DIM, Real, Vector}; use crate::utils::DotProduct; use parry::bounding_volume::BoundingVolume; use parry::query::PointQuery; @@ -14,8 +14,12 @@ use parry::query::PointQuery; #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use super::soft_contacts_classify::{classify_inside, classify_inside_self, project_on_element, ring_depths}; -use super::soft_contacts_volume::{VolumeSide, fill_depths, patch_vertices, volume_bins, volume_split}; +use super::soft_contacts_classify::{ + classify_inside, classify_inside_self, project_on_element, ring_depths, +}; +use super::soft_contacts_volume::{ + VolumeSide, fill_depths, patch_vertices, volume_bins, volume_split, +}; use super::{ SelfTangles, Side, SoftDetectionCtx, SoftVertexCandidate, SoftVertexHits, SoftVertexPass, rest_gap_skins, @@ -273,7 +277,14 @@ pub(crate) fn detect_vertex_pass( crossing[j as usize] = true; } let mut inside = Vec::new(); - if classify_inside_self(vb_mesh, vb, vb_co.contact_skin(), &crossing, &vertex_elements, &mut inside) { + if classify_inside_self( + vb_mesh, + vb, + vb_co.contact_skin(), + &crossing, + &vertex_elements, + &mut inside, + ) { let mut depth_fold = vec![Real::NEG_INFINITY; n_v]; let mut depth_face = vec![Real::NEG_INFINITY; n_v]; for v in 0..n_v { @@ -501,7 +512,11 @@ impl VertexScan<'_> { if self.is_self && self.params.soft_bodies.recovery.self_stand_down && !self.params.soft_bodies.recovery.crossing_repulsion - && self.tangled_elements.get(e as usize).copied().unwrap_or(false) + && self + .tangled_elements + .get(e as usize) + .copied() + .unwrap_or(false) { continue; } @@ -551,7 +566,8 @@ impl VertexScan<'_> { if !self.rest_gaps || !self.closed { continue; } - let Some(outward) = self.eb_mesh + let Some(outward) = self + .eb_mesh .element_outward_normal(self.eb, e as usize) .and_then(|n| n.try_normalize()) else { @@ -580,9 +596,9 @@ impl VertexScan<'_> { let pair_skins = if self.rest_gaps { let rest_vertex = self.vb_mesh.rest_vertex(self.vb, v); let rest: [Vector; DIM] = core::array::from_fn(|k| { - element - .get(k) - .map_or(Vector::ZERO, |v| self.eb_mesh.rest_vertex(self.eb, *v as usize)) + element.get(k).map_or(Vector::ZERO, |v| { + self.eb_mesh.rest_vertex(self.eb, *v as usize) + }) }); let rest_gap = if element.len() < DIM { parry::shape::Segment::new(rest[0], rest[1]) @@ -624,7 +640,9 @@ impl VertexScan<'_> { // than the element wrap several elements, so their feature contacts are real support. let size = (positions[1] - positions[0]).length(); if self.vb_co.contact_skin() < size - && self.eb_mesh.contact_is_ghost(self.eb, e as usize, &weights, vertex_pos) + && self + .eb_mesh + .contact_is_ghost(self.eb, e as usize, &weights, vertex_pos) { continue; } @@ -663,8 +681,10 @@ impl VertexScan<'_> { let bound = 2.0 * self.skins + self.eb_co.contact_skin(); for &f in &self.vertex_elements[v] { for &e in &self.targets[f as usize] { - let near = element_plane(self.eb_mesh, e as usize, |v| self.eb_mesh.cached_vertex(v)) - .is_some_and(|(p0, n)| (vertex_pos - p0).dot(n).abs() <= bound); + let near = element_plane(self.eb_mesh, e as usize, |v| { + self.eb_mesh.cached_vertex(v) + }) + .is_some_and(|(p0, n)| (vertex_pos - p0).dot(n).abs() <= bound); if near && scratch.iter().all(|c| c.element != e) { scratch.push(SoftVertexCandidate { element: e, @@ -734,7 +754,7 @@ impl VertexScan<'_> { && !seen_outside_any && self.eb_mesh.contains_point_parity(self.eb, vertex_pos); let start = out_candidates.len() as u32; - out_candidates.extend_from_slice(&scratch); + out_candidates.extend_from_slice(scratch); hits.push(SoftVertexHits { vertex: v as u32, candidates: start..out_candidates.len() as u32, diff --git a/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs b/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs index 54161b60d..b8465dbd5 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_contacts_volume.rs @@ -11,8 +11,10 @@ use parry::utils::hashmap::HashMap; #[allow(unused_imports)] use simba::scalar::{ComplexField as _, RealField as _}; -use super::soft_contacts_classify::{classify_inside, classify_skin, classify_skin_rigid, ring_depths}; -use super::{Side, SoftDetectionCtx, SoftVertexPass, SoftRigidPatch, SoftVolumePatch, body_frozen}; +use super::soft_contacts_classify::{ + classify_inside, classify_skin, classify_skin_rigid, ring_depths, +}; +use super::{Side, SoftDetectionCtx, SoftRigidPatch, SoftVertexPass, SoftVolumePatch, body_frozen}; /// The multi-volume grid's cells per tangent axis (see `overlap_multi_volume`). pub(super) fn volume_split(params: &IntegrationParameters) -> u32 { @@ -202,7 +204,8 @@ pub(super) fn detect_rigid_patch( let bins = volume_bins(&own, None, volume_split(params)); Some(SoftRigidPatch { bins, - depth: if recovery.overlap_patch_constraints != crate::dynamics::SoftPatchConstraints::Keep { + depth: if recovery.overlap_patch_constraints != crate::dynamics::SoftPatchConstraints::Keep + { depth } else { Vec::new() @@ -408,7 +411,11 @@ fn side_entries(side: &VolumeSide, picked: &[usize]) -> Vec<(u32, Vector)> { /// Splits a pair's patches into the volume constraints: one bin (mono-volume), or `split` cells /// per tangent axis of a regular grid over the patches, aligned with the pair's mean normal /// (multi-volume). A cell missing one side joins the nearest two-sided cell; with none, one bin. -pub(super) fn volume_bins(own: &VolumeSide, other: Option<&VolumeSide>, split: u32) -> Vec { +pub(super) fn volume_bins( + own: &VolumeSide, + other: Option<&VolumeSide>, + split: u32, +) -> Vec { let n_own = own.vertices.len(); let n_other = other.map_or(0, |o| o.vertices.len()); if n_own + n_other == 0 { @@ -557,4 +564,3 @@ pub(super) fn volume_bins(own: &VolumeSide, other: Option<&VolumeSide>, split: u }) .collect() } - diff --git a/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs b/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs index 85a7c3dc9..cae0bd434 100644 --- a/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs +++ b/src/geometry/narrow_phase/soft_contacts/soft_self_contacts.rs @@ -16,8 +16,10 @@ use simba::scalar::{ComplexField as _, RealField as _}; use super::soft_contacts_classify::{classify_inside_self, project_on_element}; use super::soft_contacts_edge_pass::detect_edges; use super::soft_contacts_vertex_pass::detect_vertex_pass; -use super::soft_contacts_volume::{VolumeBin, VolumeSide, patch_vertices, volume_bins, volume_split}; -use super::{SelfTangles, SoftDetectionCtx, SoftVertexPass, SoftEdgePass}; +use super::soft_contacts_volume::{ + VolumeBin, VolumeSide, patch_vertices, volume_bins, volume_split, +}; +use super::{SelfTangles, SoftDetectionCtx, SoftEdgePass, SoftVertexPass}; /// The self contact detection of one soft collision mesh (see /// [`NarrowPhase::soft_self_contacts`]): its tangle signal, and its self candidates. @@ -229,6 +231,7 @@ impl TangleSink { } /// Appends a later chunk's findings (its records after this sink's, same rule). + #[cfg(feature = "parallel")] fn merge(&mut self, other: TangleSink) { self.marks.extend(other.marks); for pair in other.crossings { @@ -373,7 +376,14 @@ pub(super) fn update_self( detect_self_tangles(sb, mesh, co, run_crossing_sweep, out); let side = (sb, mesh, handle, co); let mut vertex_pass = core::mem::take(&mut out.vertex_pass); - detect_vertex_pass(&mut vertex_pass, side, side, Some(out.tangles()), false, ctx); + detect_vertex_pass( + &mut vertex_pass, + side, + side, + Some(out.tangles()), + false, + ctx, + ); out.vertex_pass = vertex_pass; let mut edges = out.edges.take().unwrap_or_default(); let has_edges = detect_edges(&mut edges, side, side, Some(out.tangles()), false, ctx); @@ -416,7 +426,14 @@ fn detect_self_regions( } } let mut inside = Vec::new(); - if !classify_inside_self(mesh, sb, co.contact_skin(), &crossing, &vertex_elements, &mut inside) { + if !classify_inside_self( + mesh, + sb, + co.contact_skin(), + &crossing, + &vertex_elements, + &mut inside, + ) { return; } // Each inside vertex's facing element (the nearest one outside its neighborhood) and @@ -540,4 +557,3 @@ fn detect_self_regions( .push(volume_bins(&own, Some(&other), volume_split(params))); } } - diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs index ec6cf5960..2584985a4 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs @@ -148,6 +148,7 @@ impl DebugRenderPipeline { /// the particles' positions at the end of the last step) and their soft-vs-soft contacts /// (vertex-vs-surface and edge-vs-edge). #[profiling::function] + #[allow(clippy::unnecessary_cast)] // Casts are needed for switching between f32/f64. pub fn render_soft_bodies( &mut self, backend: &mut B, @@ -184,7 +185,7 @@ impl DebugRenderPipeline { // cell around it, and duplicates only thicken the picture); each edge keeps the largest // load of the elements sharing it. self.drawn_edges.clear(); - let mut record = |drawn: &mut HashMap<[u32; 2], f32>, e: [u32; 2], load: f32| { + let record = |drawn: &mut HashMap<[u32; 2], f32>, e: [u32; 2], load: f32| { let key = [e[0].min(e[1]), e[0].max(e[1])]; let slot = drawn.entry(key).or_insert(0.0); *slot = slot.max(load); @@ -215,12 +216,14 @@ impl DebugRenderPipeline { // Sorted: the backend sees the same lines in the same order every frame. let mut edges: Vec<([u32; 2], f32)> = self.drawn_edges.iter().map(|(k, v)| (*k, *v)).collect(); - edges.sort_unstable_by(|a, b| a.0.cmp(&b.0)); + edges.sort_unstable_by_key(|e| e.0); for (key, load) in edges { let color = if by_stress { let t = load.clamp(0.0, 1.0); - let (slack, loaded) = - (self.style.soft_body_slack_color, self.style.soft_body_loaded_color); + let (slack, loaded) = ( + self.style.soft_body_slack_color, + self.style.soft_body_loaded_color, + ); core::array::from_fn(|k| slack[k] + (loaded[k] - slack[k]) * t) } else { element_color diff --git a/src/pipeline/physics_hooks.rs b/src/pipeline/physics_hooks.rs index 552ff6825..a4178e460 100644 --- a/src/pipeline/physics_hooks.rs +++ b/src/pipeline/physics_hooks.rs @@ -107,7 +107,7 @@ impl<'a> ContactModificationContext<'a> { pub fn soft(&self) -> Option<&SoftPairContacts> { match &self.contacts { ModifiableContacts::Rigid(_) => None, - ModifiableContacts::Soft(soft) => Some(&*soft), + ModifiableContacts::Soft(soft) => Some(soft), } } diff --git a/src/pipeline/physics_pipeline/substep.rs b/src/pipeline/physics_pipeline/substep.rs index 504752c9e..b4df95b69 100644 --- a/src/pipeline/physics_pipeline/substep.rs +++ b/src/pipeline/physics_pipeline/substep.rs @@ -125,7 +125,9 @@ impl PhysicsPipeline { // Non-finite pose: leave the body at its last valid state for // `Quarantine::apply_end_step` to neutralize. if !rb.pos.next_position.is_finite() { - self.quarantine.body_workspace.push((handle, rb.pos.position)); + self.quarantine + .body_workspace + .push((handle, rb.pos.position)); continue; } rb.pos.position = rb.pos.next_position; @@ -309,7 +311,7 @@ impl PhysicsPipeline { soft_bodies.apply_user_changes( bodies, colliders, - &integration_parameters, + integration_parameters, &mut self.quarantine.soft_bodies, ); diff --git a/src/pipeline/physics_world.rs b/src/pipeline/physics_world.rs index b335a2db8..fef03e24d 100644 --- a/src/pipeline/physics_world.rs +++ b/src/pipeline/physics_world.rs @@ -1,6 +1,11 @@ use crate::alloc_prelude::*; -use crate::dynamics::{CCDSolver, GenericJoint, ImpulseJoint, ImpulseJointHandle, ImpulseJointSet, IntegrationParameters, IslandManager, Multibody, MultibodyJointHandle, MultibodyJointSet, MultibodyLink, MultibodyLinkId, RigidBody, RigidBodyHandle, RigidBodySet, SoftBindingError, SoftBody, SoftBodyBuilder, SoftBodyHandle, SoftBodySet, - SoftBodyTearEvent, SoftClusterRemoval, SoftMeshBinding}; +use crate::dynamics::{ + CCDSolver, GenericJoint, ImpulseJoint, ImpulseJointHandle, ImpulseJointSet, + IntegrationParameters, IslandManager, Multibody, MultibodyJointHandle, MultibodyJointSet, + MultibodyLink, MultibodyLinkId, RigidBody, RigidBodyHandle, RigidBodySet, SoftBindingError, + SoftBody, SoftBodyBuilder, SoftBodyHandle, SoftBodySet, SoftBodyTearEvent, SoftClusterRemoval, + SoftMeshBinding, +}; use crate::geometry::{ BroadPhaseBvh, Collider, ColliderHandle, ColliderSet, ContactPair, DefaultBroadPhase, NarrowPhase, @@ -286,14 +291,13 @@ impl PhysicsWorld { /// /// Returns the removed collider, or `None` if the handle was invalid. pub fn remove_collider(&mut self, handle: ColliderHandle) -> Option { - self.colliders - .remove( - handle, - &mut self.islands, - &mut self.bodies, - &mut self.soft_bodies, - true, - ) + self.colliders.remove( + handle, + &mut self.islands, + &mut self.bodies, + &mut self.soft_bodies, + true, + ) } // ── Impulse joints ────────────────────────────────────────────────── @@ -474,7 +478,8 @@ impl PhysicsWorld { } /// Adds a cluster to a soft body: a set of its particles backed by a fresh proxy rigid - /// body ([`RigidBodyType::SoftFrame`]) that impulse joints and colliders can attach to. + /// body ([`crate::dynamics::RigidBodyType::SoftFrame`]) that impulse joints and colliders + /// can attach to. /// Returns the cluster's index (see [`SoftBodySet::add_cluster`]). pub fn add_soft_body_cluster( &mut self, diff --git a/src_testbed/grab.rs b/src_testbed/grab.rs index 61ee4a228..39c2d39d3 100644 --- a/src_testbed/grab.rs +++ b/src_testbed/grab.rs @@ -104,8 +104,8 @@ impl MouseGrab { world.remove_soft_body_cluster(sb_handle, cluster); return false; }; - let mouse_body = world - .insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); + let mouse_body = + world.insert_body(RigidBodyBuilder::kinematic_position_based().translation(anchor)); let joint = world.insert_impulse_joint(mouse_body, proxy, mouse_joint(Vector::ZERO)); Grabbed { body: proxy, @@ -237,16 +237,18 @@ fn pick(mouse: &SceneMouse, world: &PhysicsWorld) -> Option<(RigidBodyHandle, Ve ); // An exact hit inside a solid collider. A 2D soft body's surface polyline is oriented, so // parry classifies its interior (holes included) with the vertex pseudo-normals. - let inside = query_pipeline.intersect_point(point).find_map(|(handle, co)| { - // An open soft surface (rope, cloth) only has an outward side, not an interior. - if let Some(mesh_ref) = co.deformable_mesh_ref() { - let mesh = world.soft_bodies.get(mesh_ref.body)?.mesh_of(handle)?; - if !mesh.is_closed() { - return None; + let inside = query_pipeline + .intersect_point(point) + .find_map(|(handle, co)| { + // An open soft surface (rope, cloth) only has an outward side, not an interior. + if let Some(mesh_ref) = co.deformable_mesh_ref() { + let mesh = world.soft_bodies.get(mesh_ref.body)?.mesh_of(handle)?; + if !mesh.is_closed() { + return None; + } } - } - co.parent() - }); + co.parent() + }); if let Some(body) = inside { return Some((body, point)); } diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs index d60cef91c..c0b309a2d 100644 --- a/src_testbed/graphics.rs +++ b/src_testbed/graphics.rs @@ -769,7 +769,7 @@ impl GraphicsManager { } } - pub fn set_body_color(&mut self, b: RigidBodyHandle, mut color: Color, tmp_color: bool) { + pub fn set_body_color(&mut self, b: RigidBodyHandle, color: Color, tmp_color: bool) { if !tmp_color { self.b2color.insert(b, color); } diff --git a/src_testbed/graphics/graphics_polyline.rs b/src_testbed/graphics/graphics_polyline.rs index 607e1e39f..fb85ee237 100644 --- a/src_testbed/graphics/graphics_polyline.rs +++ b/src_testbed/graphics/graphics_polyline.rs @@ -18,8 +18,12 @@ const POLYLINE_JOIN_SUBDIV: usize = 8; /// so the corners join cleanly. The layout only depends on the segment and vertex counts, so a /// deforming polyline's vertex buffer can be rewritten in place (see `update_deformable_node`). #[cfg(feature = "dim2")] -pub(super) fn stroked_polyline(vertices: &[Vec2], indices: &[[u32; 2]]) -> (Vec, Vec<[u32; 3]>) { - let mut vtx = Vec::with_capacity(indices.len() * 4 + vertices.len() * (POLYLINE_JOIN_SUBDIV + 1)); +pub(super) fn stroked_polyline( + vertices: &[Vec2], + indices: &[[u32; 2]], +) -> (Vec, Vec<[u32; 3]>) { + let mut vtx = + Vec::with_capacity(indices.len() * 4 + vertices.len() * (POLYLINE_JOIN_SUBDIV + 1)); let mut idx = Vec::with_capacity(indices.len() * 2 + vertices.len() * POLYLINE_JOIN_SUBDIV); for segment in indices { @@ -41,11 +45,7 @@ pub(super) fn stroked_polyline(vertices: &[Vec2], indices: &[[u32; 2]]) -> (Vec< } for k in 0..POLYLINE_JOIN_SUBDIV { let next = (k + 1) % POLYLINE_JOIN_SUBDIV; - idx.push([ - center, - center + 1 + k as u32, - center + 1 + next as u32, - ]); + idx.push([center, center + 1 + k as u32, center + 1 + next as u32]); } } @@ -56,7 +56,9 @@ pub(super) fn stroked_polyline(vertices: &[Vec2], indices: &[[u32; 2]]) -> (Vec< #[cfg(feature = "dim2")] pub(super) fn polyline_geometry(shape: &dyn Shape) -> Option<(Vec, Vec<[u32; 2]>)> { let to_vec2 = |pts: &[rapier::math::Vector]| -> Vec { - pts.iter().map(|p| Vec2::new(p.x as f32, p.y as f32)).collect() + pts.iter() + .map(|p| Vec2::new(p.x as f32, p.y as f32)) + .collect() }; match shape.shape_type() { ShapeType::Polyline => { diff --git a/src_testbed/graphics/graphics_soft_bodies.rs b/src_testbed/graphics/graphics_soft_bodies.rs index 4c5fd36a3..1fa6bab47 100644 --- a/src_testbed/graphics/graphics_soft_bodies.rs +++ b/src_testbed/graphics/graphics_soft_bodies.rs @@ -259,7 +259,12 @@ impl GraphicsManager { body: handle, id: mesh.id(), }; - if self.soft_graphics.mesh_nodes.iter().any(|n| n.mesh == mesh_ref) { + if self + .soft_graphics + .mesh_nodes + .iter() + .any(|n| n.mesh == mesh_ref) + { continue; } let Some(shape) = diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 24d01f992..73a64852b 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -611,20 +611,32 @@ fn soft_recovery_section(ui: &mut Ui, r: &mut rapier::dynamics::SoftRecoverySett .show(ui, |ui| { ui.label("Prevention"); ui.checkbox(&mut r.authored_velocity_margin, "Authored-velocity margin"); - ui.checkbox(&mut r.edge_speculation, "Edge speculation (3D closed pairs)"); + ui.checkbox( + &mut r.edge_speculation, + "Edge speculation (3D closed pairs)", + ); ui.separator(); ui.label("Detection"); ui.checkbox(&mut r.inverted_cell_detection, "Inverted cells"); ui.checkbox(&mut r.self_crossing_detection, "Self-crossings"); - ui.checkbox(&mut r.detection_motion_gating, "Self-crossing sweep motion gating"); + ui.checkbox( + &mut r.detection_motion_gating, + "Self-crossing sweep motion gating", + ); ui.checkbox(&mut r.cross_body_detection, "Cross-body crossings"); ui.separator(); ui.label("Passive stand-down"); ui.checkbox(&mut r.self_stand_down, "Self stand-down"); ui.checkbox(&mut r.cross_body_expel_gate, "Cross expel-only gate"); ui.checkbox(&mut r.edge_stand_down, "Edge-pass stand-down"); - ui.checkbox(&mut r.crossing_repulsion, "Crossing repulsion (repel, not drop)"); - ui.checkbox(&mut r.crossing_repulsion_guide, "Repulsion guided by the volume normal"); + ui.checkbox( + &mut r.crossing_repulsion, + "Crossing repulsion (repel, not drop)", + ); + ui.checkbox( + &mut r.crossing_repulsion_guide, + "Repulsion guided by the volume normal", + ); ui.checkbox( &mut r.crossing_repulsion_self_guide, "Self-repulsion guided by the fold's volume normal", @@ -637,8 +649,14 @@ fn soft_recovery_section(ui: &mut Ui, r: &mut rapier::dynamics::SoftRecoverySett Slider::new(&mut r.overlap_kept_depth, 0.0..=1.0) .text("Kept skin overlap (fraction of skins)"), ); - ui.checkbox(&mut r.overlap_normal_push, "Push along the normal instead of the gradients"); - ui.checkbox(&mut r.overlap_self_regions, "Self-overlaps between distinct regions"); + ui.checkbox( + &mut r.overlap_normal_push, + "Push along the normal instead of the gradients", + ); + ui.checkbox( + &mut r.overlap_self_regions, + "Self-overlaps between distinct regions", + ); ui.horizontal(|ui| { ui.label("Per-point constraints on the patch's features"); for (policy, name) in [ @@ -653,7 +671,10 @@ fn soft_recovery_section(ui: &mut Ui, r: &mut rapier::dynamics::SoftRecoverySett ui.add(Slider::new(&mut r.overlap_split, 1..=8).text("Grid cells per tangent axis")); ui.checkbox(&mut r.overlap_rigid, "Against rigid colliders"); ui.checkbox(&mut r.overlap_skip_self_tangled, "Skip self-crossed meshes"); - ui.checkbox(&mut r.overlap_edge_stand_down, "Edge constraints stand down on owned pairs (3D)"); + ui.checkbox( + &mut r.overlap_edge_stand_down, + "Edge constraints stand down on owned pairs (3D)", + ); ui.add( Slider::new(&mut r.recovery_pace, 0.05..=8.0) .logarithmic(true) @@ -681,7 +702,10 @@ fn debug_color_picker(ui: &mut Ui, label: &str, color: &mut rapier::pipeline::De let rgba = crate::debug_render::hsla_to_rgb(color[0], color[1], color[2], color[3]); let mut srgba = rgba.map(|c| (c.clamp(0.0, 1.0) * 255.0).round() as u8); ui.horizontal(|ui| { - if ui.color_edit_button_srgba_unmultiplied(&mut srgba).changed() { + if ui + .color_edit_button_srgba_unmultiplied(&mut srgba) + .changed() + { *color = crate::debug_render::rgb_to_hsla(srgba.map(|c| c as f32 / 255.0)); } ui.label(label); @@ -814,14 +838,10 @@ fn debug_render_tab(ui: &mut Ui, debug_render: &mut DebugRenderPipelineResource) .on_hover_text("Segments approximating a curved shape (balls, capsules, cones)."); ui.add(Slider::new(&mut style.border_subdivisions, 1..=32).text("Border subdivisions")) .on_hover_text("Segments approximating the rounded border of a round shape."); - ui.add( - Slider::new(&mut style.rigid_body_axes_length, 0.0..=2.0).text("Axes length"), - ) - .on_hover_text("Length of the rigid-body axes."); - ui.add( - Slider::new(&mut style.contact_normal_length, 0.0..=1.0).text("Normal length"), - ) - .on_hover_text("Length of the contact normals."); + ui.add(Slider::new(&mut style.rigid_body_axes_length, 0.0..=2.0).text("Axes length")) + .on_hover_text("Length of the rigid-body axes."); + ui.add(Slider::new(&mut style.contact_normal_length, 0.0..=1.0).text("Normal length")) + .on_hover_text("Length of the contact normals."); ui.add( Slider::new(&mut style.pseudo_normal_length, 0.0..=1.0).text("Pseudo-normal length"), ) @@ -830,10 +850,22 @@ fn debug_render_tab(ui: &mut Ui, debug_render: &mut DebugRenderPipelineResource) ui.collapsing("Colors", |ui| { debug_color_picker(ui, "Dynamic colliders", &mut style.collider_dynamic_color); debug_color_picker(ui, "Fixed colliders", &mut style.collider_fixed_color); - debug_color_picker(ui, "Kinematic colliders", &mut style.collider_kinematic_color); - debug_color_picker(ui, "Parentless colliders", &mut style.collider_parentless_color); + debug_color_picker( + ui, + "Kinematic colliders", + &mut style.collider_kinematic_color, + ); + debug_color_picker( + ui, + "Parentless colliders", + &mut style.collider_parentless_color, + ); debug_color_picker(ui, "Collider AABBs", &mut style.collider_aabb_color); - debug_color_picker(ui, "Impulse joint anchors", &mut style.impulse_joint_anchor_color); + debug_color_picker( + ui, + "Impulse joint anchors", + &mut style.impulse_joint_anchor_color, + ); debug_color_picker( ui, "Impulse joint separation", @@ -867,7 +899,11 @@ fn debug_render_tab(ui: &mut Ui, debug_render: &mut DebugRenderPipelineResource) ui.add_space(4.0); ui.label("Multipliers (per HSLA component)"); debug_multiplier_row(ui, "Sleeping", &mut style.sleep_color_multiplier); - debug_multiplier_row(ui, "Sleep-ready", &mut style.sleep_eligible_color_multiplier); + debug_multiplier_row( + ui, + "Sleep-ready", + &mut style.sleep_eligible_color_multiplier, + ); debug_multiplier_row(ui, "Disabled", &mut style.disabled_color_multiplier); if ui.button("Reset colors").clicked() { let default = rapier::pipeline::DebugRenderStyle::default(); @@ -894,7 +930,12 @@ fn performance_tab(ui: &mut Ui, state: &TestbedState, world: &PhysicsWorld) { let num_contacts: usize = world .narrow_phase .contact_pairs() - .map(|pair| pair.manifolds().iter().map(|m| m.points.len()).sum::()) + .map(|pair| { + pair.manifolds() + .iter() + .map(|m| m.points.len()) + .sum::() + }) .sum(); let num_sleeping = world diff --git a/src_testbed/viewer.rs b/src_testbed/viewer.rs index b7d10b5a7..b58a66a6f 100644 --- a/src_testbed/viewer.rs +++ b/src_testbed/viewer.rs @@ -559,9 +559,7 @@ impl TestbedViewer { WindowEvent::MouseButton(kiss3d::event::MouseButton::Button1, action, _) => { match action { Action::Press => { - if !egui_wants_pointer - && self.grab.try_grab(&self.scene_mouse, world) - { + if !egui_wants_pointer && self.grab.try_grab(&self.scene_mouse, world) { event.inhibited = true; } } From a92192f8e1fd48348c6f97ed7fa7f1712e2be0ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 19 Sep 2026 19:49:11 +0200 Subject: [PATCH 31/32] chore: small demo tweaks --- examples2d/soft_jelly2.rs | 2 +- examples2d/soft_pile2.rs | 15 --------------- examples3d/soft_cloth_stress3.rs | 17 ++++++++++------- 3 files changed, 11 insertions(+), 23 deletions(-) diff --git a/examples2d/soft_jelly2.rs b/examples2d/soft_jelly2.rs index 4bf09e80b..273c25bf5 100644 --- a/examples2d/soft_jelly2.rs +++ b/examples2d/soft_jelly2.rs @@ -22,7 +22,7 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { for i in 0..(levels - level) { let x = -8.0 + (i as Real - (levels - level) as Real * 0.5 + 0.5) * 1.6; let y = 0.75 + level as Real * 1.5; - let square = SoftBodyBuilder::grid(Vector::new(x, y), Vector::splat(0.75), 5, 5) + let square = SoftBodyBuilder::grid(Vector::new(x, y), Vector::splat(0.6), 5, 5) .cell_model(SoftBodyCellModel::Corotational) .material(SoftBodyMaterial { young_modulus: young, diff --git a/examples2d/soft_pile2.rs b/examples2d/soft_pile2.rs index 47e8dc848..ce3457bc8 100644 --- a/examples2d/soft_pile2.rs +++ b/examples2d/soft_pile2.rs @@ -88,21 +88,6 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { } } - /* - * Ropes thrown on top. - */ - for i in 0..3 { - let rope = SoftBodyBuilder::rope( - Vector::new(-6.0 + i as Real, 36.0 + i as Real), - Vector::new(4.0 + i as Real, 37.0 + i as Real), - 30, - ) - .softness(SpringCoefficients::new(30.0, 1.0)) - .particle_mass(0.03) - .surface_collider(ColliderBuilder::ball(0.08).friction(0.6)); - world.insert_soft_body(rope); - } - viewer.set_world(&mut world); viewer.look_at(Vec2::new(0.0, 10.0), 20.0); diff --git a/examples3d/soft_cloth_stress3.rs b/examples3d/soft_cloth_stress3.rs index 9f361886f..1528d8f63 100644 --- a/examples3d/soft_cloth_stress3.rs +++ b/examples3d/soft_cloth_stress3.rs @@ -42,13 +42,16 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { for k in 0..6 { let origin = Vector::new(-6.0, 2.0, 0.0) + rot * Vector::new(-3.5 + k as Real * 0.3, 0.3 + k as Real * 0.12, -1.5); - world.insert_soft_body(cloth( - origin, - rot * Vector::new(0.15, 0.0, 0.0), - Vector::new(0.0, 0.0, 0.15), - 21, - 21, - )); + world.insert_soft_body( + cloth( + origin, + rot * Vector::new(0.15, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.15), + 21, + 21, + ) + .self_contacts(true), + ); } /* From 4eab4e5b08acd45d875a7b63842a7479a7995ee9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 19 Sep 2026 20:12:57 +0200 Subject: [PATCH 32/32] chore: more CI fixes --- .github/workflows/rapier-ci-build.yml | 24 +++++++++++++++---- .typos.toml | 2 ++ crates/rapier2d-f64/Cargo.toml | 5 ++-- crates/rapier2d/Cargo.toml | 5 ++-- crates/rapier2d/tests/soft_bodies.rs | 12 +++++++--- python/rapier-py-3d/src/debug_render.rs | 1 + python/rapier-py-3d/src/soft_body.rs | 10 ++++---- src/dynamics/integration_parameters.rs | 2 ++ src/dynamics/mod.rs | 1 + .../soft_body_builder_mesh_helpers.rs | 1 + .../soft_body_builder_shapes.rs | 14 ++++++++++- src/dynamics/soft_body/soft_body_cluster.rs | 4 ++++ src/geometry/mod.rs | 2 ++ typescript/Cargo.toml | 6 ----- .../2d/javascript/package-lock.json | 13 ++++++---- .../docs-examples/2d/javascript/package.json | 2 +- .../3d/javascript/package-lock.json | 13 ++++++---- .../docs-examples/3d/javascript/package.json | 2 +- website/docs-examples/Cargo.toml | 7 ------ website/docs-examples/README.md | 14 ++++++++++- 20 files changed, 97 insertions(+), 43 deletions(-) diff --git a/.github/workflows/rapier-ci-build.yml b/.github/workflows/rapier-ci-build.yml index 016b661d6..2724fb166 100644 --- a/.github/workflows/rapier-ci-build.yml +++ b/.github/workflows/rapier-ci-build.yml @@ -310,18 +310,34 @@ jobs: workspaces: website/docs-examples - name: Tests injection + code snippets run: cd website/docs-examples && cargo test - # Checks the JavaScript documentation snippets build (against the published @dimforge/rapier). + # Checks the JavaScript documentation snippets build against the in-tree bindings, so a + # snippet may use an API before it is released on npm. The snippet package depends on + # `typescript/builds/rapier{2,3}d/pkg` through a `file:` path, which this job builds first + # (the same two steps a local checkout runs). website-javascript-snippets: runs-on: ubuntu-latest strategy: matrix: dimension: [2d, 3d] - defaults: - run: - working-directory: ./website/docs-examples/${{ matrix.dimension }}/javascript steps: - uses: actions/checkout@v4 + - uses: Swatinem/rust-cache@v2 + with: + workspaces: typescript + - name: Generate the binding crates + working-directory: ./typescript + run: | + npm ci + cargo run -p prepare_builds -- -d dim${{ matrix.dimension == '2d' && '2' || '3' }} -f non-deterministic + - name: Build the in-tree bindings + working-directory: ./typescript/builds/rapier${{ matrix.dimension }} + run: | + npm i + npm run build:wasm + npm run build:ts - name: Install dependencies + working-directory: ./website/docs-examples/${{ matrix.dimension }}/javascript run: npm ci - name: Builds javascript snippets. + working-directory: ./website/docs-examples/${{ matrix.dimension }}/javascript run: npm run build diff --git a/.typos.toml b/.typos.toml index b0123245a..9bd857110 100644 --- a/.typos.toml +++ b/.typos.toml @@ -16,6 +16,8 @@ ignore-hidden = false [default] extend-ignore-re = [ "\\bPolygon::PN\\b", + # Index notation in the strain-constraint docs: `m_ba`, `ε_ba` (the b,a entry). + "_ba\\b", ] # Case sensitive, matches entire word. diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index 7b64eacf6..77a24a4d2 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -35,13 +35,14 @@ block-solver = [] alloc = [ "nalgebra/alloc", "parry2d-f64/alloc", - # Needed by `SoftBodyBuilder::volumetric`, for the Delaunay refinement. - "parry2d-f64/spade", "serde?/alloc", ] std = [ "alloc", "parry2d-f64/std", + # Needed by `SoftBodyBuilder::volumetric`, for the Delaunay refinement. Kept out of + # `alloc`: spade's `robust` dependency does not build without the standard library. + "parry2d-f64/spade", "nalgebra/std", "simba/std", "num-traits/std", diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index 366e833e9..b6326a0d5 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -35,13 +35,14 @@ block-solver = [] alloc = [ "nalgebra/alloc", "parry2d/alloc", - # Needed by `SoftBodyBuilder::volumetric`, for the Delaunay refinement. - "parry2d/spade", "serde?/alloc", ] std = [ "alloc", "parry2d/std", + # Needed by `SoftBodyBuilder::volumetric`, for the Delaunay refinement. Kept out of + # `alloc`: spade's `robust` dependency does not build without the standard library. + "parry2d/spade", "nalgebra/std", "simba/std", "num-traits/std", diff --git a/crates/rapier2d/tests/soft_bodies.rs b/crates/rapier2d/tests/soft_bodies.rs index dbbdb5b8d..e100ed2f2 100644 --- a/crates/rapier2d/tests/soft_bodies.rs +++ b/crates/rapier2d/tests/soft_bodies.rs @@ -340,8 +340,8 @@ fn stiff_neo_hookean_square_is_stable() { } } -/// A soft Neo-Hookean square squashed to 30% of its height by a kinematic plate keeps its cells -/// positively oriented (up to a couple of transient flips under the plate while held) and +/// A soft Neo-Hookean square pressed to under half its height by a kinematic plate keeps its +/// cells positively oriented (up to a couple of transient flips under the plate while held) and /// recovers its area within 5% once the plate lifts, with no inverted cell left. #[test] fn neo_hookean_square_survives_large_compression() { @@ -411,8 +411,12 @@ fn neo_hookean_square_survives_large_compression() { ); } } + // How far the press bites is platform-dependent: the plate swallows the top vertices, and + // which ones it catches turns on the last bits of the contact solve (0.35 on aarch64, 0.40 + // on x86_64). This only guards that the press happened; the cell orientation and the area + // recovery below are what the test is about. assert!( - min_height < 0.35, + min_height < 0.45, "the plate did not squash the square: {min_height}" ); assert_eq!(inverted(&world), 0, "inverted cells after release"); @@ -3328,6 +3332,7 @@ fn appended_disks_keep_their_own_areas() { /// Whether two segments cross at a point interior to both (touching or collinear segments do /// not cross). +#[cfg(feature = "fem")] fn segments_cross(a: [Vector; 2], b: [Vector; 2]) -> bool { let orient = |p: Vector, q: Vector, r: Vector| (q - p).perp_dot(r - p); let (o1, o2) = (orient(a[0], a[1], b[0]), orient(a[0], a[1], b[1])); @@ -3336,6 +3341,7 @@ fn segments_cross(a: [Vector; 2], b: [Vector; 2]) -> bool { } /// The number of inverted cells and of crossing pairs of boundary segments of a body. +#[cfg(feature = "fem")] fn tangles(sb: &SoftBody) -> (usize, usize) { let area = |x: [Vector; 3]| (x[1] - x[0]).perp_dot(x[2] - x[0]); let inverted = sb diff --git a/python/rapier-py-3d/src/debug_render.rs b/python/rapier-py-3d/src/debug_render.rs index 18ee54663..bc59a5444 100644 --- a/python/rapier-py-3d/src/debug_render.rs +++ b/python/rapier-py-3d/src/debug_render.rs @@ -1051,6 +1051,7 @@ pub struct DebugRenderPipeline { } impl DebugRenderPipeline { + #[allow(clippy::too_many_arguments)] fn _run_with_backend( &mut self, bodies: &RigidBodySet, diff --git a/python/rapier-py-3d/src/soft_body.rs b/python/rapier-py-3d/src/soft_body.rs index 8e22e2be6..fa38cb267 100644 --- a/python/rapier-py-3d/src/soft_body.rs +++ b/python/rapier-py-3d/src/soft_body.rs @@ -945,7 +945,7 @@ impl SoftBodyBuilder { "masses" => b.masses(v.extract()?), "pinned_particles" => b.pinned_particles(extract_indices_1d(v)?), "softness" => b.softness(spring(v)?), - "material" => b.material(v.extract::>()?.0.clone()), + "material" => b.material(v.extract::>()?.0), "tear_strain" => b.tear_strain(v.extract()?), "tear_force" => b.tear_force(v.extract()?), "min_piece" => b.min_piece(v.extract()?), @@ -1034,7 +1034,7 @@ impl SoftBodyBuilder { /// The material (a copy). #[getter] fn current_material(&self) -> SoftBodyMaterial { - SoftBodyMaterial(self.builder.material.clone()) + SoftBodyMaterial(self.builder.material) } /// The edges of the surface elements, deduplicated, as an ``(E, 2)`` ndarray. @@ -1126,7 +1126,7 @@ impl SoftBodyBuilder { } /// Set the material. fn material(&self, material: &SoftBodyMaterial) -> Self { - let m = material.0.clone(); + let m = material.0; self.chained(|b| b.material(m)) } /// Set a uniform softness (a :class:`SpringCoefficients` or a ``(frequency, damping)`` @@ -2381,11 +2381,11 @@ impl SoftBody { /// The material (a copy: assign it back or use :meth:`set_material` to apply changes). #[getter] fn material(&self) -> SoftBodyMaterial { - self.with_ref(|b| SoftBodyMaterial(b.material().clone())) + self.with_ref(|b| SoftBodyMaterial(*b.material())) } #[setter] fn set_material(&mut self, material: &SoftBodyMaterial) { - let m = material.0.clone(); + let m = material.0; self.with_mut(|b| b.set_material(m)); } /// The constitutive model of the cells. diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 53b6acebe..8f8ba431a 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -249,6 +249,7 @@ pub struct IntegrationParameters { /// substeps, contact stiffening and FEM solver tuning (see [`SoftBodiesSettings`]). /// /// [`SoftBodiesSettings`]: crate::dynamics::SoftBodiesSettings + #[cfg(feature = "alloc")] #[cfg_attr(feature = "serde-serialize", serde(default))] pub soft_bodies: crate::dynamics::SoftBodiesSettings, @@ -422,6 +423,7 @@ impl Default for IntegrationParameters { friction_in_bias_pass: false, warmstart_joints: false, length_unit: 1.0, + #[cfg(feature = "alloc")] soft_bodies: Default::default(), #[cfg(feature = "dim3")] friction_model: FrictionModel::default(), diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 2b082b4a7..d255be294 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -29,6 +29,7 @@ pub use parry::mass_properties::MassProperties; pub use self::rigid_body::{RigidBody, RigidBodyBuilder}; #[cfg(feature = "alloc")] pub use self::rigid_body_set::{BodyPair, RigidBodySet}; +#[cfg(feature = "alloc")] pub(crate) use self::soft_body::soft_body_crossing_tests; #[cfg(feature = "alloc")] pub use self::soft_body::*; diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs index 2379ae04f..0ce9c2f78 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_mesh_helpers.rs @@ -11,6 +11,7 @@ use super::super::SoftBodyCell; /// Drops the pieces of a filled mesh that are negligible next to its largest one; pieces of a /// comparable size are all kept, since dropping them would lose limbs rather than specks. +#[cfg(any(feature = "dim3", feature = "std"))] pub(super) fn drop_stray_pieces( mesh: &mut parry::transformation::VolumeMesh, components: &[u32], diff --git a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs index 9da9765bc..5803219dc 100644 --- a/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs +++ b/src/dynamics/soft_body/soft_body_builder/soft_body_builder_shapes.rs @@ -14,7 +14,9 @@ use simba::scalar::{ComplexField as _, RealField as _}; use super::super::{SoftBodyCellModel, SoftBodyMaterial}; #[cfg(feature = "dim3")] use super::soft_body_builder_mesh_helpers::icosphere; -use super::soft_body_builder_mesh_helpers::{cell_faces, drop_stray_pieces, mean_edge_length}; +use super::soft_body_builder_mesh_helpers::mean_edge_length; +#[cfg(any(feature = "dim3", feature = "std"))] +use super::soft_body_builder_mesh_helpers::{cell_faces, drop_stray_pieces}; use super::{SoftBodyBuilder, SoftBodyParticleSettings}; impl SoftBodyBuilder { @@ -417,6 +419,9 @@ impl SoftBodyBuilder { /// A volumetric body filling a closed, outward-oriented mesh (triangles in 3D, segments in 2D) /// with cells of size `cell_size`; the surface is their boundary. Particle radius defaults to /// half the mean cell edge; `None` if the mesh is empty, open or encloses nothing at that size. + // Filling a 2D boundary goes through parry's Delaunay refinement, which rapier2d + // enables with `std` (see the crate's feature list). + #[cfg(any(feature = "dim3", feature = "std"))] pub fn volumetric( vertices: &[Vector], indices: &[[u32; DIM]], @@ -432,6 +437,9 @@ impl SoftBodyBuilder { /// The same as [`Self::volumetric`], with the meshing parameters spelled out: element size, /// the cover's smoothing, guard and subdivision, and whether the surface alone is covered /// (the crust). + // Filling a 2D boundary goes through parry's Delaunay refinement, which rapier2d + // enables with `std` (see the crate's feature list). + #[cfg(any(feature = "dim3", feature = "std"))] pub fn volumetric_with( vertices: &[Vector], indices: &[[u32; DIM]], @@ -447,6 +455,7 @@ impl SoftBodyBuilder { } /// A body of the cells of a filled mesh, with the particle radius the mesh asks for. + #[cfg(any(feature = "dim3", feature = "std"))] fn from_cells(mesh: parry::transformation::VolumeMesh) -> Option { let mut builder = Self::new(mesh.vertices).cells(mesh.cells); // Half the mean edge of the *boundary*: with a subdivided cover the interior edges are @@ -481,6 +490,9 @@ impl SoftBodyBuilder { /// The same as [`Self::volumetric`], keeping the boundary mesh as the body's skin: the cells /// hold it and the body is drawn as it, so thin features survive a cell size that cannot /// resolve them; it still collides through its cells, so contacts are as coarse as they are. + // Filling a 2D boundary goes through parry's Delaunay refinement, which rapier2d + // enables with `std` (see the crate's feature list). + #[cfg(any(feature = "dim3", feature = "std"))] pub fn volumetric_skinned( vertices: &[Vector], indices: &[[u32; DIM]], diff --git a/src/dynamics/soft_body/soft_body_cluster.rs b/src/dynamics/soft_body/soft_body_cluster.rs index 28eb87296..a70561ac7 100644 --- a/src/dynamics/soft_body/soft_body_cluster.rs +++ b/src/dynamics/soft_body/soft_body_cluster.rs @@ -2,6 +2,10 @@ //! ([`crate::dynamics::RigidBodyType::SoftFrame`]) standing for it in the islands and joints. //! Index 0 is the whole-body cluster; removing a cluster deletes the particles it alone covers. +#[cfg(not(feature = "std"))] +#[allow(unused_imports)] +use simba::scalar::{ComplexField as _, RealField as _}; + use crate::alloc_prelude::*; use crate::dynamics::RigidBodyHandle; use crate::math::{AngVector, AngularInertia, DIM, Pose, Real, Rotation, Vector}; diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index c7d09c136..23f50ce6b 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -35,7 +35,9 @@ pub use self::interaction_groups::{Group, InteractionGroups, InteractionTestMode pub use self::mesh_converter::{MeshConverter, MeshConverterError}; #[cfg(feature = "alloc")] pub use self::narrow_phase::NarrowPhase; +#[cfg(feature = "alloc")] pub(crate) use self::narrow_phase::soft_contacts; +#[cfg(feature = "alloc")] pub use self::narrow_phase::soft_contacts::{ SoftContactImpulse, SoftEdgeCandidate, SoftEdgePass, SoftPairContacts, SoftVertexCandidate, SoftVertexHits, SoftVertexPass, SoftVolumePatch, VolumeBin, diff --git a/typescript/Cargo.toml b/typescript/Cargo.toml index 70f4fde22..d7a8b8b40 100644 --- a/typescript/Cargo.toml +++ b/typescript/Cargo.toml @@ -17,12 +17,6 @@ strip = true # Workaround for https://github.com/bevyengine/bevy/issues/16030 (t rapier2d = { path = "../crates/rapier2d" } rapier3d = { path = "../crates/rapier3d" } -# Temporary: the composite-shape queries returning the part id alongside the result (parry -# branch `composite-subshape-ids`), to be replaced by a version bump when parry is released. -# Mirrors the patch of the root workspace. -parry2d = { path = "../../parry/crates/parry2d" } -parry3d = { path = "../../parry/crates/parry3d" } - # nalgebra and simba are still pulled from crates.io, matching the versions the # in-repo rapier crates depend on. Uncomment to build against local checkouts # placed next to this repository during development. diff --git a/website/docs-examples/2d/javascript/package-lock.json b/website/docs-examples/2d/javascript/package-lock.json index ec86e1987..54e881d39 100644 --- a/website/docs-examples/2d/javascript/package-lock.json +++ b/website/docs-examples/2d/javascript/package-lock.json @@ -9,7 +9,7 @@ "version": "1.0.0", "license": "Apache-2.0", "dependencies": { - "@dimforge/rapier2d": "0.17.3" + "@dimforge/rapier2d": "file:../../../../typescript/builds/rapier2d/pkg" }, "devDependencies": { "html-loader": "^5.0.0", @@ -21,12 +21,15 @@ "webpack-cli": "^5.1.4" } }, - "node_modules/@dimforge/rapier2d": { - "version": "0.17.3", - "resolved": "https://registry.npmjs.org/@dimforge/rapier2d/-/rapier2d-0.17.3.tgz", - "integrity": "sha512-yWV3FTlOqPoeEpB2ptv//sb6hq6E1MO6u7auMnC49SawdUMD3ZGrRtpv52YZfL44D90AWxu6GNBseq00kdn5nA==", + "../../../../typescript/builds/rapier2d/pkg": { + "name": "@dimforge/rapier2d", + "version": "0.20.0", "license": "Apache-2.0" }, + "node_modules/@dimforge/rapier2d": { + "resolved": "../../../../typescript/builds/rapier2d/pkg", + "link": true + }, "node_modules/@discoveryjs/json-ext": { "version": "0.5.7", "resolved": "https://registry.npmjs.org/@discoveryjs/json-ext/-/json-ext-0.5.7.tgz", diff --git a/website/docs-examples/2d/javascript/package.json b/website/docs-examples/2d/javascript/package.json index 6024ab132..3bcbd2fe2 100644 --- a/website/docs-examples/2d/javascript/package.json +++ b/website/docs-examples/2d/javascript/package.json @@ -19,6 +19,6 @@ "webpack-cli": "^5.1.4" }, "dependencies": { - "@dimforge/rapier2d": "0.17.3" + "@dimforge/rapier2d": "file:../../../../typescript/builds/rapier2d/pkg" } } \ No newline at end of file diff --git a/website/docs-examples/3d/javascript/package-lock.json b/website/docs-examples/3d/javascript/package-lock.json index b4cf9ff40..3d72c2169 100644 --- a/website/docs-examples/3d/javascript/package-lock.json +++ b/website/docs-examples/3d/javascript/package-lock.json @@ -9,7 +9,7 @@ "version": "1.0.0", "license": "Apache-2.0", "dependencies": { - "@dimforge/rapier3d": "0.17.3" + "@dimforge/rapier3d": "file:../../../../typescript/builds/rapier3d/pkg" }, "devDependencies": { "html-loader": "^5.0.0", @@ -19,12 +19,15 @@ "webpack-cli": "^5.1.4" } }, - "node_modules/@dimforge/rapier3d": { - "version": "0.17.3", - "resolved": "https://registry.npmjs.org/@dimforge/rapier3d/-/rapier3d-0.17.3.tgz", - "integrity": "sha512-//CVavUmqsIv66M/0zFVZBYsx5vWuOXVAsxEhcjBXlDiNqsX0d0I4/9G2ArHGKXTvGxcYorJ7PbLumlNY0weJQ==", + "../../../../typescript/builds/rapier3d/pkg": { + "name": "@dimforge/rapier3d", + "version": "0.20.0", "license": "Apache-2.0" }, + "node_modules/@dimforge/rapier3d": { + "resolved": "../../../../typescript/builds/rapier3d/pkg", + "link": true + }, "node_modules/@discoveryjs/json-ext": { "version": "0.5.7", "resolved": "https://registry.npmjs.org/@discoveryjs/json-ext/-/json-ext-0.5.7.tgz", diff --git a/website/docs-examples/3d/javascript/package.json b/website/docs-examples/3d/javascript/package.json index 70e7e83c2..f4a4da7c2 100644 --- a/website/docs-examples/3d/javascript/package.json +++ b/website/docs-examples/3d/javascript/package.json @@ -17,6 +17,6 @@ "webpack-cli": "^5.1.4" }, "dependencies": { - "@dimforge/rapier3d": "0.17.3" + "@dimforge/rapier3d": "file:../../../../typescript/builds/rapier3d/pkg" } } \ No newline at end of file diff --git a/website/docs-examples/Cargo.toml b/website/docs-examples/Cargo.toml index eb98b34cd..efb32bf5d 100644 --- a/website/docs-examples/Cargo.toml +++ b/website/docs-examples/Cargo.toml @@ -1,10 +1,3 @@ [workspace] members = ["2d/bevy", "2d/rust", "3d/bevy", "3d/rust", "inject_file"] resolver = "2" - -[patch.crates-io] -# Temporary: the composite-shape queries returning the part id alongside the result (parry -# branch `composite-subshape-ids`), to be replaced by a version bump when parry is released. -# Mirrors the patch of the root workspace. -parry2d = { path = "../../../parry/crates/parry2d" } -parry3d = { path = "../../../parry/crates/parry3d" } diff --git a/website/docs-examples/README.md b/website/docs-examples/README.md index df5fba181..42dd41079 100644 --- a/website/docs-examples/README.md +++ b/website/docs-examples/README.md @@ -11,4 +11,16 @@ intended usage is to run `cargo check --workspace --examples`. ## Javascript -Within the javascript folder, run `npm run build`. +The snippets build against the bindings **of this repository**, not the ones published on +npm, so a snippet may use an API before it is released. `@dimforge/rapier{2,3}d` is a `file:` +dependency on `typescript/builds/rapier{2,3}d/pkg`, which is generated; build it once (per +dimension) before installing: + +```sh +cd typescript +npm ci +cargo run -p prepare_builds -- -d dim2 -f non-deterministic +cd builds/rapier2d && npm i && npm run build:wasm && npm run build:ts +``` + +Then, within the javascript folder, run `npm ci` and `npm run build`.