diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..2282c30d5 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "parry"] + path = parry + url = https://github.com/Polari-Stars-MC/parry.git diff --git a/Cargo.toml b/Cargo.toml index 5c32941e3..c8655eb10 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -41,7 +41,7 @@ default-members = [ ] # `typescript/` is the JS/Wasm bindings' own Cargo workspace; keep it out of # this one so root-level `cargo` commands never try to absorb its crates. -exclude = ["typescript"] +exclude = ["typescript", "parry", "nalgebra"] resolver = "2" [workspace.package] @@ -69,13 +69,15 @@ nalgebra = { version = "0.35", default-features = false, features = ["macros"] } glamx = { version = "0.3", default-features = false } simba = { version = "0.10.2", default-features = false } num-traits = { version = "0.2", default-features = false } +# Bit-identical gemm backend (same crate nalgebra delegates to for dynamic matrices). +matrixmultiply = "0.3" 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 = { package = "parry2d", path = "parry/crates/parry2d", default-features = false, features = ["required-features"] } +parry3d = { package = "parry3d", path = "parry/crates/parry3d", default-features = false, features = ["required-features"] } +parry2d-f64 = { package = "parry2d-f64", path = "parry/crates/parry2d-f64", default-features = false, features = ["required-features"] } +parry3d-f64 = { package = "parry3d-f64", path = "parry/crates/parry3d-f64", default-features = false, features = ["required-features"] } # Utilities arrayvec = { version = "0.7", default-features = false } diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index d9792065e..227d14793 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -57,6 +57,7 @@ serde-serialize = [ "nalgebra/serde-serialize", "parry2d-f64/serde-serialize", "dep:serde", + "dep:bincode", "std", ] enhanced-determinism = ["simba/libm_force", "parry2d-f64/enhanced-determinism"] @@ -104,6 +105,7 @@ web-time = { workspace = true, optional = true } rayon = { workspace = true, optional = true } serde = { workspace = true, optional = true } bytemuck = { workspace = true, optional = true } +bincode = { workspace = true, optional = true } # Nalgebra is only included on non-spirv targets. On spirv, rapier exposes only the # subset of its API that doesn't require nalgebra. diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index 706a05ca6..9d0e3c84e 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -63,6 +63,7 @@ serde-serialize = [ "nalgebra/serde-serialize", "parry2d/serde-serialize", "dep:serde", + "dep:bincode", "std", ] enhanced-determinism = ["simba/libm_force", "parry2d/enhanced-determinism"] @@ -111,6 +112,7 @@ web-time = { workspace = true, optional = true } rayon = { workspace = true, optional = true } serde = { workspace = true, optional = true } bytemuck = { workspace = true, optional = true } +bincode = { workspace = true, optional = true } # Nalgebra is only included on non-spirv targets. On spirv, rapier exposes only the # subset of its API that doesn't require nalgebra. diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml index 1b31f1330..9a79c74e4 100644 --- a/crates/rapier3d-f64/Cargo.toml +++ b/crates/rapier3d-f64/Cargo.toml @@ -58,6 +58,7 @@ serde-serialize = [ "nalgebra/serde-serialize", "parry3d-f64/serde-serialize", "dep:serde", + "dep:bincode", "std", ] enhanced-determinism = ["simba/libm_force", "parry3d-f64/enhanced-determinism"] @@ -105,12 +106,14 @@ web-time = { workspace = true, optional = true } rayon = { workspace = true, optional = true } serde = { workspace = true, optional = true } bytemuck = { workspace = true, optional = true } +bincode = { workspace = true, optional = true } # Nalgebra is only included on non-spirv targets. On spirv, rapier exposes only the # subset of its API that doesn't require nalgebra. [target.'cfg(not(target_arch = "spirv"))'.dependencies] nalgebra = { workspace = true } glamx = { workspace = true, features = ["nalgebra"] } +matrixmultiply = { workspace = true } [dev-dependencies] bincode.workspace = true diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml index cfbf8362a..5c1a5ee8a 100644 --- a/crates/rapier3d/Cargo.toml +++ b/crates/rapier3d/Cargo.toml @@ -64,6 +64,7 @@ serde-serialize = [ "nalgebra/serde-serialize", "parry3d/serde-serialize", "dep:serde", + "dep:bincode", "std", ] enhanced-determinism = ["simba/libm_force", "parry3d/enhanced-determinism"] @@ -111,6 +112,7 @@ web-time = { workspace = true, optional = true } rayon = { workspace = true, optional = true } serde = { workspace = true, optional = true } bytemuck = { workspace = true, optional = true } +bincode = { workspace = true, optional = true } # Nalgebra is only included on non-spirv targets. On spirv, rapier exposes only the # subset of its API that doesn't require nalgebra. diff --git a/parry b/parry new file mode 160000 index 000000000..1be4b1a7c --- /dev/null +++ b/parry @@ -0,0 +1 @@ +Subproject commit 1be4b1a7cd0a090bd7efb1207b7bc0d453f4132e diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs index 742684e78..3b71d886b 100644 --- a/src/control/character_controller.rs +++ b/src/control/character_controller.rs @@ -271,10 +271,33 @@ impl KinematicCharacterController { .contact(&pos12, character_shape, collider.shape(), 0.0) { if contact.dist < -1.0e-5 { + // A negative `dist` means the character overlaps the collider. This can + // happen because (a) the character was pushed into a static obstacle, or + // (b) a *moving* (kinematic) platform advanced into the character this + // step. For (b) — the heart of issue #488 — the overlap is the platform's + // motion, and we must resolve the *full* penetration here so the character + // is carried with the platform instead of lagging a frame (a visible dip) + // or, once the platform moves faster than `max_correction` per step, + // tunneling straight through. So contacts against a kinematic parent are + // depenetrated without the height-based correction cap. + let is_kinematic = collider + .parent + .and_then(|p| queries.bodies.get(p.handle)) + .map(|rb| rb.is_kinematic()) + .unwrap_or(false); + let correction_budget = if is_kinematic { + offset - contact.dist // uncapped: resolve the full overlap + } else { + max_correction - applied + }; + // Push out until the usual `offset` gap is restored. - let push = (offset - contact.dist).min(max_correction - applied); + let push = (offset - contact.dist).min(correction_budget); if push <= 0.0 { - return; // The per-call correction budget is exhausted. + if !is_kinematic { + return; // The per-call correction budget is exhausted. + } + continue; } // `normal1` (expressed in the character’s local frame) points towards @@ -1375,3 +1398,145 @@ mod test { assert!(movement.grounded); } } + +#[cfg(feature = "dim3")] +#[cfg(test)] +mod moving_platform_tests { + use crate::control::KinematicCharacterController; + use crate::prelude::*; + + #[test] + fn character_controller_moving_platform() { + // Regression test for issue #488: a kinematic character controller that stands + // still on top of a *moving* kinematic platform must be carried along with the + // platform (no dip, no fall-through). When the platform moves into the character + // it creates penetration that `check_and_fix_penetrations` must resolve in full; + // if that resolution is capped (as it is for static obstacles) a fast platform + // leaves the character behind until it tunnels straight through. + fn run(platform_speed: Real) { + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let mut pipeline = PhysicsPipeline::new(); + let mut bf = BroadPhaseBvh::new(); + let mut nf = NarrowPhase::new(); + let mut islands = IslandManager::new(); + let mut bodies = RigidBodySet::new(); + + let gravity = Vector::ZERO; // disable gravity; we drive the platform by hand + let integration_parameters = IntegrationParameters::default(); + let dt = integration_parameters.dt; + + // Vertical kinematic platform: a thin slab centered at y = 0. + let platform_half_height = 0.5; + let platform = RigidBodyBuilder::kinematic_position_based() + .translation(Vector::new(0.0, 0.0, 0.0)) + .build(); + let platform_handle = bodies.insert(platform); + colliders.insert_with_parent( + ColliderBuilder::cuboid(5.0, platform_half_height, 5.0), + platform_handle, + &mut bodies, + ); + + // Character: a ball of radius 0.5 resting on top of the platform. + let character = RigidBodyBuilder::kinematic_position_based() + .translation(Vector::new(0.0, platform_half_height + 0.5, 0.0)) + .build(); + let character_handle = bodies.insert(character); + let character_collider = ColliderBuilder::ball(0.5).build(); + let character_shape = character_collider.shape(); + colliders.insert_with_parent(character_collider.clone(), character_handle, &mut bodies); + + let controller = KinematicCharacterController { + slide: true, + ..Default::default() + }; + + // Track the character's world position explicitly: after `set_next_kinematic_translation` + // the body's `position()` only updates on the next `step`, so read it from our own copy. + // The controller keeps a small `offset` gap above the platform, so we assert on the + // *per-step displacement* (it must match the platform's motion) and that the character + // never sinks below the platform's top face (the #488 failure: fall-through). + let mut char_pos = *bodies.get(character_handle).unwrap().position(); + let mut prev_char_y = char_pos.translation.y; + for i in 0..120 { + let expected_platform_y = platform_speed * dt * (i as Real + 1.0); + bodies + .get_mut(platform_handle) + .unwrap() + .set_next_kinematic_translation(Vector::new(0.0, expected_platform_y, 0.0)); + + pipeline.step( + gravity, + &integration_parameters, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut CCDSolver::new(), + &(), + &(), + ); + + let filter = QueryFilter::new().exclude_rigid_body(character_handle); + let query_pipeline = + bf.as_query_pipeline(nf.query_dispatcher(), &bodies, &colliders, filter); + + // Stationary character: zero desired translation. + let movement = controller.move_shape( + dt, + &query_pipeline, + character_shape, + &char_pos, + Vector::ZERO, + |_| {}, + ); + + char_pos.translation += movement.translation; + bodies + .get_mut(character_handle) + .unwrap() + .set_next_kinematic_translation(char_pos.translation); + + let step_delta = char_pos.translation.y - prev_char_y; + let expected_delta = platform_speed * dt; + // The character must follow the platform's per-step motion (issue #488: a lagging + // character dips and, for a fast platform, falls through entirely). + assert!( + (step_delta - expected_delta).abs() < 2.0e-2, + "character should track platform motion (speed {}, iter {}): delta={}, expected={}", + platform_speed, + i, + step_delta, + expected_delta + ); + // And it must never sink below the platform's top face. + let platform_top = expected_platform_y + platform_half_height; + assert!( + char_pos.translation.y >= platform_top - 1.0e-3, + "character fell through platform (speed {}, iter {}): char_y={}, top={}", + platform_speed, + i, + char_pos.translation.y, + platform_top + ); + assert!( + movement.grounded, + "character must stay grounded (speed {}, iter {})", + platform_speed, i + ); + prev_char_y = char_pos.translation.y; + } + } + + // Normal platform (already worked before the fix). + run(2.0); + // Fast platform: 10 m/s * dt (~0.016s) ≈ 0.16 m/step, which exceeds the old + // `max_correction = height * 0.25` cap for this character (would tunnel). + run(10.0); + } +} diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index ee5cd9b07..47afd8819 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -20,7 +20,7 @@ use super::sweeps::{ /// 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 +/// a *bullet* that also sweeps kinematic/dynamic bodies (including other bullets — issue #984). 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. #[derive(Clone, Default)] @@ -230,7 +230,7 @@ impl CCDSolver { } Self::apply_clamps(bodies, &all_results); - // Pass 2: bullets vs everything except other bullets. Targets read the (already + // Pass 2: bullets vs every (possibly already clamped) body, including other bullets. // clamped) `next_position` from pass 1 (deferred bullet stage). if !bullets.is_empty() { let bullet_results = { @@ -335,7 +335,229 @@ impl CCDSolver { rb.mprops.local_mprops.local_com, ); rb.pos.next_position = sweep.transform_at(result.fraction); + // Record the point of impact (issue #548) from the earliest solid hit. + if let Some(toi) = &result.toi { + rb.ccd.toi_point = toi.point; + rb.ccd.toi_normal = toi.normal; + } } } } } + +#[cfg(feature = "dim3")] +#[cfg(test)] +mod ccd_tests { + use crate::dynamics::{ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodySet}; + use crate::geometry::{ColliderSet, NarrowPhase}; + use crate::math::Vector; + use crate::prelude::{ + CCDSolver, ColliderBuilder, DefaultBroadPhase, IntegrationParameters, PhysicsPipeline, + RigidBodyBuilder, + }; + use std::vec::Vec; + + /// Regression test for #984 (CCD tunneling between two fast bodies). + /// + /// Two dynamic bullets approach head-on at 100 m/s. After one step a raycast along the + /// center line must hit the OTHER bullet before reaching its starting body (no tunnel). + #[test] + fn ccd_between_two_fast_bullets_does_not_tunnel() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut island_manager = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut impulse_joint_set = ImpulseJointSet::new(); + let mut multibody_joint_set = MultibodyJointSet::new(); + let mut ccd_solver = CCDSolver::new(); + let mut physics_pipeline = PhysicsPipeline::new(); + let gravity = Vector::new(0.0, 0.0, 0.0); + let integration_parameters = IntegrationParameters::default(); + + // Two bullets about to collide: A at -0.7 moving +x, B at +0.7 moving -x, each 100 m/s. + // Spacing is within the swept-AABB reach so the broad phase emits the candidate pair. + let a = RigidBodyBuilder::dynamic() + .translation(Vector::new(-0.7, 0.0, 0.0)) + .ccd_enabled(true) + .build(); + let a_h = bodies.insert(a); + let b = RigidBodyBuilder::dynamic() + .translation(Vector::new(0.7, 0.0, 0.0)) + .ccd_enabled(true) + .build(); + let b_h = bodies.insert(b); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), a_h, &mut bodies); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), b_h, &mut bodies); + + // One real step populates broad/narrow phase + islands; then we overwrite the solved + // poses to the *crossing* configuration and force CCD active, calling solve_continuous + // directly to exercise the bullet-vs-bullet sweep (issue #984). + physics_pipeline.step( + gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut bodies, + &mut colliders, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + &(), + &(), + ); + + let a_mut = bodies.get_mut(a_h).unwrap(); + a_mut.pos.position.translation.x = -0.7; + a_mut.pos.next_position.translation.x = 0.5; // naive integration would cross past B + a_mut.ccd.ccd_active = true; + let b_mut = bodies.get_mut(b_h).unwrap(); + b_mut.pos.position.translation.x = 0.7; + b_mut.pos.next_position.translation.x = -0.5; // naive integration would cross past A + b_mut.ccd.ccd_active = true; + + broad_phase.update( + &integration_parameters, + &colliders, + &bodies, + &[], + &[], + &mut Vec::new(), + ); + + ccd_solver.solve_continuous( + &integration_parameters, + &island_manager, + &mut bodies, + &colliders, + &mut broad_phase, + &narrow_phase, + &(), + &(), + true, + ); + + let a_pos = bodies.get(a_h).unwrap().pos.next_position.translation.x; + let b_pos = bodies.get(b_h).unwrap().pos.next_position.translation.x; + + assert!( + a_pos < 0.0 && b_pos > 0.0 && a_pos < b_pos, + "bullets tunneled through each other: A.x={}, B.x={}", + a_pos, + b_pos + ); + } + + /// Regression test for #548 (CCD point of impact). + /// + /// After the bullet-vs-bullet solve, each body must record a world-space point of impact + /// and a separating normal (the relative axis between the two balls, i.e. ∓X here). + #[test] + fn ccd_records_point_of_impact() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut island_manager = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut impulse_joint_set = ImpulseJointSet::new(); + let mut multibody_joint_set = MultibodyJointSet::new(); + let mut ccd_solver = CCDSolver::new(); + let mut physics_pipeline = PhysicsPipeline::new(); + let gravity = Vector::new(0.0, 0.0, 0.0); + let integration_parameters = IntegrationParameters::default(); + + let a = RigidBodyBuilder::dynamic() + .translation(Vector::new(-0.7, 0.0, 0.0)) + .ccd_enabled(true) + .build(); + let a_h = bodies.insert(a); + let b = RigidBodyBuilder::dynamic() + .translation(Vector::new(0.7, 0.0, 0.0)) + .ccd_enabled(true) + .build(); + let b_h = bodies.insert(b); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), a_h, &mut bodies); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), b_h, &mut bodies); + + physics_pipeline.step( + gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut bodies, + &mut colliders, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + &(), + &(), + ); + + let a_mut = bodies.get_mut(a_h).unwrap(); + a_mut.pos.position.translation.x = -0.7; + a_mut.pos.next_position.translation.x = 0.5; + a_mut.ccd.ccd_active = true; + let b_mut = bodies.get_mut(b_h).unwrap(); + b_mut.pos.position.translation.x = 0.7; + b_mut.pos.next_position.translation.x = -0.5; + b_mut.ccd.ccd_active = true; + + broad_phase.update( + &integration_parameters, + &colliders, + &bodies, + &[], + &[], + &mut Vec::new(), + ); + + ccd_solver.solve_continuous( + &integration_parameters, + &island_manager, + &mut bodies, + &colliders, + &mut broad_phase, + &narrow_phase, + &(), + &(), + true, + ); + + let a_toi = bodies.get(a_h).unwrap().ccd_point_of_impact(); + let b_toi = bodies.get(b_h).unwrap().ccd_point_of_impact(); + assert!(a_toi.is_some(), "A must record a CCD point of impact"); + assert!(b_toi.is_some(), "B must record a CCD point of impact"); + + let (a_point, a_normal) = a_toi.unwrap(); + let (b_point, b_normal) = b_toi.unwrap(); + // The hit point lies on the A-B center line (x-axis), y=z=0. + assert!( + a_point.y.abs() < 1.0e-6 && a_point.z.abs() < 1.0e-6, + "impact point must lie on the A-B center line, got {:?}", + a_point + ); + // The separating normals are along ±X (the A↔B axis) and oppose each other: one body + // records +X, the other -X, since the normal is the relative separating axis. + assert!( + a_normal.x.abs() > 0.99 && b_normal.x.abs() > 0.99, + "both normals must align with X, got A={:?} B={:?}", + a_normal, + b_normal + ); + assert!( + a_normal.x * b_normal.x < 0.0, + "A and B separating normals must oppose, got A={:?} B={:?}", + a_normal, + b_normal + ); + // The two records describe the same contact point. + assert!( + (a_point.x - b_point.x).abs() < 1.0e-6, + "A and B must agree on the impact x, got {} vs {}", + a_point.x, + b_point.x + ); + } +} diff --git a/src/dynamics/ccd/sweeps.rs b/src/dynamics/ccd/sweeps.rs index c4d301b8d..893400f9f 100644 --- a/src/dynamics/ccd/sweeps.rs +++ b/src/dynamics/ccd/sweeps.rs @@ -13,7 +13,7 @@ use crate::pipeline::{ActiveHooks, PairFilterContext, PhysicsHooks}; #[cfg(feature = "dim2")] use parry::query::sweep_toi::CORE_FRACTION; use parry::query::sweep_toi::{ - Sweep, SweepCompositeFastShape, SweepToiStatus, ToiProxy, sweep_time_of_impact, + Sweep, SweepCompositeFastShape, SweepToiOutput, SweepToiStatus, ToiProxy, sweep_time_of_impact, sweep_time_of_impact_composite, }; use parry::query::{NonlinearRigidMotion, QueryDispatcher}; @@ -32,10 +32,13 @@ 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 **fixed** targets; a bullet sweeps against every body type — including other +/// bullets (issue #984: two fast `ccd_enabled` bodies previously never swept each other and +/// tunneled straight through). Targets are read at their — possibly already clamped — +/// `next_position`, so the swept geometry is the solved end-of-step pose. fn tier_allows(rb1: &RigidBody, rb2: Option<&RigidBody>) -> bool { if is_bullet(rb1) { - !rb2.map(is_bullet).unwrap_or(false) + true } else { is_fixed_target(rb2) } @@ -275,6 +278,10 @@ pub(super) struct BodyContinuousResult { pub(super) handle: RigidBodyHandle, /// The earliest solid impact fraction in `[0, 1]`; `1.0` if the body sweeps freely. pub(super) fraction: Real, + /// The world-space point and separating normal of the earliest solid impact (issue #548), + /// `None` when the body sweeps freely (no CCD hit). `point`/`normal` are taken from the + /// `SweepToiOutput` of the earliest-hit candidate. + pub(super) toi: Option, pub(super) pseudo_hits: Vec, } @@ -291,13 +298,28 @@ fn cast_collider_pair( dt: Real, linear_slop: Real, is_pseudo: bool, -) -> Option { +) -> Option { let target_pose = target_collider_pose(co2, rb2); + // The target's swept motion: for a *moving* target body (e.g. another fast bullet, issue + // #984) we sweep it along its own start→next pose so the time-of-impact accounts for the + // *relative* motion of both bodies. Static/fixed targets have start == next, collapsing to + // the previous constant (stationary) sweep. + let target_sweep = match (rb2, co2.parent.as_ref()) { + (Some(rb2), Some(parent)) if rb2.ccd.ccd_active => Sweep::from_poses( + &(rb2.pos.position * parent.pos_wrt_parent), + &(rb2.pos.next_position * parent.pos_wrt_parent), + co2.shape.mass_properties(1.0).local_com, + ), + _ => Sweep::constant(&target_pose, Vector::ZERO), + }; + let sub_shapes: &[FastSubShape] = match &fast.kind { FastShapeKind::Convex(sub) => core::slice::from_ref(sub), FastShapeKind::Compound(children) => children, FastShapeKind::Nonlinear => { + // Nonlinear fallback only resolves a fraction (no witness point); wrap it in a + // default `SweepToiOutput` so callers still get the impact fraction uniformly. return fallback_nonlinear_fraction( dispatcher, fast, @@ -306,7 +328,13 @@ fn cast_collider_pair( max_fraction, dt, is_pseudo, - ); + ) + .map(|fraction| SweepToiOutput { + status: SweepToiStatus::Hit, + fraction, + point: Vector::ZERO, + normal: Vector::ZERO, + }); } }; @@ -325,27 +353,35 @@ fn cast_collider_pair( max_fraction, dt, is_pseudo, - ); + ) + .map(|fraction| SweepToiOutput { + status: SweepToiStatus::Hit, + fraction, + point: Vector::ZERO, + normal: Vector::ZERO, + }); } }; // Each accepted piece fraction tightens `max_fraction`, so the returned value is the // earliest impact across all the pieces (both the solid and pseudo accept conditions - // only pass at or below the current bound). - let mut best = None; + // only pass at or below the current bound). We keep the full `SweepToiOutput` of that + // earliest hit so the caller can read its world-space point/normal (issue #548). + let mut best: Option = None; let mut max_fraction = max_fraction; for sub in sub_shapes { - if let Some(fraction) = cast_sub_shape( + if let Some(output) = cast_sub_shape( sub, &target, shape2, &target_pose, + &target_sweep, max_fraction, linear_slop, is_pseudo, ) { - best = Some(fraction); - max_fraction = fraction; + best = Some(output); + max_fraction = output.fraction; } } best @@ -358,22 +394,20 @@ fn cast_sub_shape( target: &TargetKind, shape2: &dyn Shape, target_pose: &Pose, + target_sweep: &Sweep, max_fraction: Real, linear_slop: Real, is_pseudo: bool, -) -> Option { +) -> Option { let output = match target { - TargetKind::Proxy(target_proxy) => { - let target_sweep = Sweep::constant(target_pose, Vector::ZERO); - sweep_time_of_impact( - target_proxy, - &target_sweep, - &sub.proxy, - &sub.sweep, - max_fraction, - linear_slop, - ) - } + TargetKind::Proxy(target_proxy) => sweep_time_of_impact( + target_proxy, + target_sweep, + &sub.proxy, + &sub.sweep, + max_fraction, + linear_slop, + ), TargetKind::Composite => { // Heightfields are treated as one-sided in 3D; oriented polylines and // oriented meshes enable the one-sided early-outs from their own flags inside @@ -408,14 +442,14 @@ fn cast_sub_shape( SweepToiStatus::Hit | SweepToiStatus::Failed | SweepToiStatus::Overlapped if output.fraction <= max_fraction => { - Some(output.fraction) + Some(output) } _ => None, }; } if 0.0 < output.fraction && output.fraction < max_fraction { - return Some(output.fraction); + return Some(output); } #[cfg(feature = "dim2")] @@ -424,17 +458,16 @@ fn cast_sub_shape( // so a body already touching a surface isn't pinned at fraction 0. if let TargetKind::Proxy(target_proxy) = target { let core = ToiProxy::point(sub.local_centroid, CORE_FRACTION * sub.min_extent); - let target_sweep = Sweep::constant(target_pose, Vector::ZERO); let output = sweep_time_of_impact( target_proxy, - &target_sweep, + target_sweep, &core, &sub.sweep, max_fraction, linear_slop, ); if 0.0 < output.fraction && output.fraction < max_fraction { - return Some(output.fraction); + return Some(output); } } } @@ -518,6 +551,7 @@ pub(super) fn sweep_fast_body( ) -> BodyContinuousResult { let rb1 = &bodies[handle]; let mut fraction: Real = 1.0; + let mut best_toi: Option = None; let mut pseudo_hits = Vec::new(); for ch1 in &rb1.colliders.0 { @@ -572,7 +606,7 @@ pub(super) fn sweep_fast_body( return; } - if let Some(hit_fraction) = cast_collider_pair( + if let Some(hit) = cast_collider_pair( dispatcher, &fast, co2, @@ -586,10 +620,11 @@ pub(super) fn sweep_fast_body( pseudo_hits.push(PseudoHit { ch1: *ch1, ch2, - fraction: hit_fraction, + fraction: hit.fraction, }); } else { - fraction = hit_fraction; + fraction = hit.fraction; + best_toi = Some(hit); } } }; @@ -613,6 +648,7 @@ pub(super) fn sweep_fast_body( BodyContinuousResult { handle, fraction, + toi: best_toi, pseudo_hits, } } diff --git a/src/dynamics/fluid.rs b/src/dynamics/fluid.rs new file mode 100644 index 000000000..8825dd7e4 --- /dev/null +++ b/src/dynamics/fluid.rs @@ -0,0 +1,343 @@ +//! Fluid-body (SPH — Smoothed-Particle Hydrodynamics) support — Phase 0. +//! +//! This module is the **data-structure + particle integrator** skeleton for +//! incompressible-fluid simulation, on par with `soft_body.rs` (Phase 0). It is +//! intentionally independent of the SoA SIMD solver boundary so it can be +//! compiled and unit-tested in isolation. +//! +//! ## Design notes (see `.hermes/plans/2026-08-30_fluid-sph-roadmap.md`) +//! +//! * A fluid is a cloud of `FluidParticle`s. Each carries position, velocity, +//! accumulated force, mass, and (per-step) density / pressure. +//! * Integration is **semi-implicit (symplectic) Euler**: velocities first +//! (`v += dt·a`), then positions (`x += dt·v`) — the same operation order used +//! by `SoftBody::integrate` and the rigid-body integrator, keeping the +//! floating-point sequence bit-identical across runs under +//! `enhanced-determinism`. +//! * SPH kernels (Poly6 density, Spiky pressure gradient, viscosity Laplacian) +//! are implemented locally in `f64` (reusing `crate::math::Vector`/`Real`), +//! so the fork stays self-contained — it does **not** depend on `mps-formula` +//! (which keeps its own SPH formula layer for the mps-core estimation FFI). +//! * Neighbour search is naive O(n²) in Phase 0 (small scale, bit-identical); +//! a spatial hash arrives in a later phase. +//! +//! Phase 0 = data structures + SPH force model + integrator + tests. Later +//! phases wire `FluidWorld` into `PhysicsWorld` and add rigid-body coupling. + +use crate::alloc_prelude::Vec; +use crate::math::{Real, Vector}; + +/// A single SPH fluid particle. +#[derive(Clone, Debug)] +pub struct FluidParticle { + /// Current world-space position. + pub pos: Vector, + /// Current world-space linear velocity. + pub vel: Vector, + /// Accumulated acceleration for the current step (cleared each `step`). + pub accel: Vector, + /// Particle mass (> 0). + pub mass: Real, + /// Recomputed each step: local SPH density (Poly6 sum of neighbour masses). + pub density: Real, + /// Recomputed each step: pressure `max(gas·(density − rest_density), 0)`. + pub pressure: Real, +} + +impl FluidParticle { + /// Creates a free fluid particle. + pub fn new(pos: Vector, vel: Vector, mass: Real) -> Self { + Self { + pos, + vel, + accel: Vector::ZERO, + mass, + density: 0.0, + pressure: 0.0, + } + } +} + +/// SPH tunable parameters for a [`FluidWorld`]. +#[derive(Clone, Copy, Debug)] +pub struct FluidParams { + /// Smoothing radius `h` — kernel cutoff. Particles farther than `h` do not + /// interact. Must be `> 0`. + pub smoothing_radius: Real, + /// Equation-of-state gas constant `k` (Tait/Murnaghan stiffness). + pub gas_constant: Real, + /// Rest density `ρ₀` (target density at rest). Must be `> 0`. + pub rest_density: Real, + /// Dynamic viscosity `μ` (velocity-diffusion / cohesion). `>= 0`. + pub viscosity: Real, + /// Surface tension coefficient `σ` (optional, Phase 0 keeps it for API + /// completeness; the force model uses it as an extra inward pull toward the + /// local centroid of neighbours). `>= 0`. + pub surface_tension: Real, + /// Constant body acceleration (typically gravity). + pub gravity: Vector, +} + +impl Default for FluidParams { + fn default() -> Self { + Self { + smoothing_radius: 1.0, + gas_constant: 100.0, + rest_density: 1000.0, + viscosity: 0.1, + surface_tension: 0.0, + gravity: Vector::ZERO, + } + } +} + +/// A cloud of `FluidParticle`s sharing one set of [`FluidParams`]. +#[derive(Clone, Debug)] +pub struct FluidWorld { + /// Particles. Index into this `Vec` is the particle id. + pub particles: Vec, + /// Shared SPH parameters. + pub params: FluidParams, +} + +impl FluidWorld { + /// Creates an empty fluid world with the given parameters. + pub fn new(params: FluidParams) -> Self { + Self { + particles: Vec::new(), + params, + } + } + + /// Number of particles. + pub fn len(&self) -> usize { + self.particles.len() + } + + /// True when there are no particles. + pub fn is_empty(&self) -> bool { + self.particles.is_empty() + } + + /// Appends a particle, returning its index. + pub fn add_particle(&mut self, pos: Vector, vel: Vector, mass: Real) -> usize { + let i = self.particles.len(); + self.particles.push(FluidParticle::new(pos, vel, mass)); + i + } + + /// SPH Poly6 kernel `W(r, h)` — used for density estimation. + fn poly6(distance: Real, h: Real) -> Real { + if distance >= h { + return 0.0; + } + let h2 = h * h; + let r2 = distance * distance; + let diff = h2 - r2; + if diff <= 0.0 { + return 0.0; + } + 315.0 / (64.0 * std::f64::consts::PI * h.powi(9)) * diff.powi(3) + } + + /// SPH Spiky pressure-gradient kernel `∇W(r, h)` (points from neighbour to + /// self, i.e. along `-r̂` scaled by the spiky slope). Returns the zero vector + /// when `distance <= 0` or `>= h`. + fn spiky_gradient(offset: Vector, h: Real) -> Vector { + let distance = offset.length(); + if distance <= 1e-9 || distance >= h { + return Vector::ZERO; + } + let diff = h - distance; + // -r̂ · (45 / (π h⁶)) · (h - r)² — standard spiky gradient. + -offset / distance * (45.0 / (std::f64::consts::PI * h.powi(6)) * diff * diff) + } + + /// SPH viscosity Laplacian `∇²W(r, h)`. + fn viscosity_laplacian(distance: Real, h: Real) -> Real { + if distance >= h { + return 0.0; + } + 45.0 / (std::f64::consts::PI * h.powi(6)) * (h - distance) + } + + /// Advance the fluid by one timestep `dt` using semi-implicit Euler. + /// + /// Per step: (1) recompute each particle's density + pressure from its + /// neighbours; (2) compute the SPH pressure + viscosity + gravity + /// acceleration; (3) integrate velocities then positions. + pub fn step(&mut self, dt: Real) { + let n = self.particles.len(); + if n == 0 { + return; + } + let h = self.params.smoothing_radius; + let h2 = h * h; + let k = self.params.gas_constant; + let rho0 = self.params.rest_density; + let mu = self.params.viscosity; + + // (1) Density + pressure for every particle. + for i in 0..n { + let pi = self.particles[i].pos; + let mi = self.particles[i].mass; + let mut density = Self::poly6(0.0, h) * mi; // self contribution + for j in 0..n { + let pj = self.particles[j].pos; + let mj = self.particles[j].mass; + let d2 = (pi - pj).length_squared(); + if d2 < h2 { + density += mj * Self::poly6(d2.sqrt(), h); + } + } + let density = density.max(1e-9); + let pressure = (k * (density - rho0)).max(0.0); + self.particles[i].density = density; + self.particles[i].pressure = pressure; + } + + // (2) Accelerations. + let mut accels: Vec = Vec::new(); + accels.resize(n, Vector::ZERO); + for i in 0..n { + let pi = self.particles[i].pos; + let vi = self.particles[i].vel; + let rho_i = self.particles[i].density; + let p_i = self.particles[i].pressure; + let mut pressure_force = Vector::ZERO; + let mut viscosity_force = Vector::ZERO; + let mut centroid = Vector::ZERO; + let mut nbr = 0_usize; + for j in 0..n { + if i == j { + continue; + } + let pj = self.particles[j].pos; + let vj = self.particles[j].vel; + let rho_j = self.particles[j].density; + let p_j = self.particles[j].pressure; + let rij = pi - pj; // from j to i + let d2 = rij.length_squared(); + if d2 >= h2 || d2 <= 1e-18 { + continue; + } + let d = d2.sqrt(); + // Pressure force (symmetrised): -Σ m_j (p_i/ρ_i² + p_j/ρ_j²) ∇W_ij + let grad = Self::spiky_gradient(rij, h); + pressure_force += + self.particles[j].mass * (p_i / (rho_i * rho_i) + p_j / (rho_j * rho_j)) * grad; + // Viscosity force: μ Σ m_j (v_j − v_i)/ρ_j ∇²W_ij + let lap = Self::viscosity_laplacian(d, h); + viscosity_force += self.particles[j].mass * (vj - vi) / rho_j * lap; + centroid += pj; + nbr += 1; + } + let pressure_accel = -pressure_force; // force already carries the -grad sign + let viscosity_accel = mu * viscosity_force; + let mut a = pressure_accel + viscosity_accel + self.params.gravity; + if self.params.surface_tension > 0.0 && nbr > 0 { + centroid /= nbr as Real; + // Pull toward neighbour centroid (cohesion). + a += self.params.surface_tension * (centroid - pi); + } + accels[i] = a; + } + + // (3) Semi-implicit Euler. + for i in 0..n { + let a = accels[i]; + let p = &mut self.particles[i]; + p.vel += a * dt; + p.pos += p.vel * dt; + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn params() -> FluidParams { + FluidParams { + smoothing_radius: 1.0, + gas_constant: 100.0, + rest_density: 1000.0, + viscosity: 0.0, + surface_tension: 0.0, + gravity: Vector::new(0.0, -9.81, 0.0), + } + } + + #[test] + fn sph_single_particle_free_fall_is_analytic() { + // One particle, no neighbours → no pressure/viscosity, pure gravity. + let mut fw = FluidWorld::new(params()); + fw.add_particle(Vector::new(0.0, 0.0, 0.0), Vector::ZERO, 1.0); + let dt = 1.0 / 60.0; + for _ in 0..60 { + fw.step(dt); + } + let p = &fw.particles[0]; + // Semi-implicit Euler under constant gravity: x(t) = ½ g t² + ½ g·dt·t + // (an O(dt) drift vs the analytic ½ g t²). With dt = 1/60 and t = 1 s the + // drift is ≈ 0.082, so we assert within 0.1 and that it falls monotonically. + let expected = 0.5 * (-9.81) * 1.0_f64 * 1.0; + assert!( + (p.pos.y - expected).abs() < 0.1, + "free-fall y={} expected≈{}", + p.pos.y, + expected + ); + assert!(p.pos.y < -4.0, "particle fell downward under gravity"); + assert!(p.pos.x.abs() < 1e-12 && p.pos.z.abs() < 1e-12); + } + + #[test] + fn sph_two_particles_repel_under_compression() { + // Two particles closer than rest spacing → local density > rest_density + // → positive pressure → they push apart (centre of mass stays fixed). + // rest_density is set low (2.0) so the ~3.1 local density of two unit-mass + // particles 0.1 apart exceeds it and yields positive pressure. + let mut p = params(); + p.rest_density = 2.0; + p.gas_constant = 200.0; + p.viscosity = 0.0; + let mut fw = FluidWorld::new(p); + fw.add_particle(Vector::new(-0.05, 0.0, 0.0), Vector::ZERO, 1.0); + fw.add_particle(Vector::new(0.05, 0.0, 0.0), Vector::ZERO, 1.0); + let dt = 1.0 / 120.0; + for _ in 0..60 { + fw.step(dt); + } + let sep = (fw.particles[1].pos - fw.particles[0].pos).length(); + assert!( + sep > 0.1, + "compressed particles should repel, sep={sep} (started 0.1)" + ); + // Centre of mass x/z stay at origin (symmetric start, symmetric forces). + // y is free to fall under gravity (both particles drop together). + let com = (fw.particles[0].pos + fw.particles[1].pos) / 2.0; + assert!( + com.x.abs() < 1e-9 && com.z.abs() < 1e-9, + "centre of mass x/z should stay at origin, com={com:?}" + ); + } + + #[test] + fn sph_step_is_deterministic() { + let mut a = FluidWorld::new(params()); + a.add_particle(Vector::new(0.0, 0.0, 0.0), Vector::ZERO, 1.0); + a.add_particle(Vector::new(0.2, 0.1, 0.0), Vector::ZERO, 1.0); + a.add_particle(Vector::new(-0.15, 0.05, 0.1), Vector::ZERO, 1.0); + let mut b = a.clone(); + let dt = 1.0 / 60.0; + for _ in 0..20 { + a.step(dt); + b.step(dt); + } + for (pa, pb) in a.particles.iter().zip(b.particles.iter()) { + assert_eq!(pa.pos, pb.pos, "positions bit-identical across runs"); + assert_eq!(pa.vel, pb.vel, "velocities bit-identical across runs"); + } + } +} diff --git a/src/dynamics/force_containers.rs b/src/dynamics/force_containers.rs new file mode 100644 index 000000000..422af0120 --- /dev/null +++ b/src/dynamics/force_containers.rs @@ -0,0 +1,453 @@ +//! Force containers — a kind-classified, self-integrating force model. +//! +//! ## Motivation +//! +//! Rapier's legacy force model is "flat": user forces (`user_force`) are +//! accumulated and then wiped every step by `reset_forces`, so a persistent +//! force (thrust, a magnetic anchor) must be re-`add_force`d every frame — a +//! tedious remove-then-re-apply ritual. Gravity, by contrast, is already a +//! *persistent* force (it is reapplied every step, never cleared). +//! +//! This module replaces that flat model with a **{persistent, transient}** +//! lifecycle expressed through **force containers classified by kind**: +//! +//! * Each force *kind* (gravity, thrust, magnetic, wind, friction, contact +//! reaction, one-shot event, user, …) owns a [`KindContainer`]. +//! * A container is **self-integrating**: it holds its own entries, its own +//! [`ForceKind`], and — crucially — its own [`Persistence`] flag recorded +//! *inside* the container (not via two separate struct types). +//! * Whether a force survives across steps is decided by the container's own +//! [`ForceContainer::end_frame`] using its internal `persistence` field: a +//! `Persistent` container (e.g. gravity, steady thrust) keeps its entries; +//! a `Transient` container (e.g. friction, contact reaction, one-shot event) +//! drains itself every step. +//! +//! Adding a new force kind = add a `ForceKind` variant + create a +//! `KindContainer` with the right `kind`/`persistence`. The effective-force +//! summation (`compute_body_effective_forces`) is transparent to kinds. +//! +//! ## Trait design +//! +//! * [`ForceContribution`] — one force entry. Provides a single generic +//! [`ForceContribution::accumulate`] method that writes the entry into an +//! [`EffectiveForce`] accumulator (including the `r × F` torque term). +//! * [`ForceContainer`] — a kind-classified, self-integrating holder of +//! `ForceContribution`s. Knows its own `kind()` and `persistence()`, and +//! implements frame-end cleanup (`end_frame`/`clear`). + +use std::collections::HashMap; +use std::vec::Vec; + +#[cfg(feature = "serde-serialize")] +use serde::{Deserialize, Serialize}; + +use crate::dynamics::RigidBodySet; +use crate::dynamics::rigid_body::RigidBody; +use crate::geometry::NarrowPhase; +use crate::math::{AngVector, Real, Vector}; +use crate::utils::CrossProduct; +use crate::utils::OrthonormalBasis; + +/// Effective (already-summed) force + torque for one body, consumed by the solver. +#[derive(Clone, Copy, Debug, Default)] +pub struct EffectiveForce { + /// Linear force (world frame). + pub force: Vector, + /// Torque / angular force (world frame). + pub torque: AngVector, +} + +/// Whether a container's forces survive across steps. +/// +/// Recorded **inside** each [`KindContainer`] (see [`ForceContainer::persistence`]), +/// not as two separate struct types. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum Persistence { + /// Forces persist until explicitly removed (`remove`/`clear`). + /// Example: gravity, steady thrust, magnetic anchor. + Persistent, + /// Forces are valid only for the current step and are drained at frame end. + /// Example: one-shot events, wind gusts, contact friction / reaction. + Transient, +} + +/// The kind of a force, used to classify containers. +/// +/// New force kinds are added here; a container simply stores the matching +/// `ForceKind` and a [`Persistence`] flag. +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[non_exhaustive] +pub enum ForceKind { + /// World gravity (always persistent). + Gravity, + /// Steady / pulsed thrust. + Thrust, + /// Magnetic / electromagnetic field force. + Magnetic, + /// Aerodynamic / fluid wind. + Wind, + /// Surface friction (solver-emergent, transient). + Friction, + /// Contact normal reaction (solver-emergent, transient). + ContactReaction, + /// One-shot external event force. + Event, + /// Legacy `add_force` user force. + User, + /// User-defined custom kind. + Custom(u32), +} + +/// One force contribution inside a container. +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ForceEntry { + /// Caller-managed id (so it can be removed individually). + pub id: u64, + /// Linear force component (world frame). + pub force: Vector, + /// Torque / angular force component (world frame). + pub torque: AngVector, + /// World-space application point; `None` = center of mass (no extra torque). + pub point: Option, +} + +/// A single force contribution that can be summed into an [`EffectiveForce`]. +pub trait ForceContribution { + /// Linear force component (world frame). + fn force(&self) -> Vector; + /// Torque / angular force component (world frame). + fn torque(&self) -> AngVector; + /// World-space application point; `None` = center of mass. + fn point(&self) -> Option; + + /// Generic method: write this contribution into the accumulator. + /// + /// This is the single unified "store the force data" point — every force + /// kind uses the same write path, including the `r × F` torque term from + /// an off-center application point. + fn accumulate(&self, eff: &mut EffectiveForce, world_com: Vector) { + eff.force += self.force(); + eff.torque += self.torque(); + if let Some(p) = self.point() { + eff.torque += (p - world_com).gcross(self.force()); + } + } +} + +impl ForceContribution for ForceEntry { + fn force(&self) -> Vector { + self.force + } + fn torque(&self) -> AngVector { + self.torque + } + fn point(&self) -> Option { + self.point + } +} + +/// A self-integrating force container classified by [`ForceKind`]. +/// +/// The container holds its own entries, its own `kind`, and its own +/// [`Persistence`] flag (recorded inside — see [`persistence`](Self::persistence)). +/// Frame-end cleanup is delegated to the container via [`end_frame`](Self::end_frame). +pub trait ForceContainer { + /// The force kind this container holds. + fn kind(&self) -> ForceKind; + /// Whether the contained forces persist across steps — **recorded inside the container**. + fn persistence(&self) -> Persistence; + /// Iterate the live contributions. + fn contributions(&self) -> impl Iterator; + /// Remove one contribution by id. Returns `true` if removed. + fn remove(&mut self, id: u64) -> bool; + /// Clear all contributions (persistent containers usually shouldn't be cleared). + fn clear(&mut self); + + /// Frame-end: self-integrating cleanup. Drains only if the container's + /// internal `persistence` is `Transient`; `Persistent` containers keep their + /// entries (e.g. gravity stays constant, no per-step re-apply needed). + fn end_frame(&mut self) { + if self.persistence() == Persistence::Transient { + self.clear(); + } + } +} + +/// The concrete, kind-classified, self-integrating container. +/// +/// A `GravityContainer`, `ThrustContainer`, `FrictionContainer`, … are all just +/// `KindContainer` instances differing in `kind` and `persistence`. This keeps +/// the container taxonomy flat and data-driven: adding a force kind is a data +/// change, not a new type. +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct KindContainer { + kind: ForceKind, + persistence: Persistence, + entries: Vec, +} + +impl KindContainer { + /// Create an empty container of the given kind and persistence. + pub fn new(kind: ForceKind, persistence: Persistence) -> Self { + Self { + kind, + persistence, + entries: Vec::new(), + } + } + + /// Push a contribution, returning its id (auto-assigned if `id == 0`). + pub fn push(&mut self, mut entry: ForceEntry) -> u64 { + if entry.id == 0 { + // Deterministic auto-id from current length + 1 (0 reserved as "auto"). + entry.id = (self.entries.len() as u64).wrapping_add(1); + if entry.id == 0 { + entry.id = 1; + } + } + let id = entry.id; + self.entries.push(entry); + id + } + + /// Number of live contributions. + pub fn len(&self) -> usize { + self.entries.len() + } + + /// Whether the container is empty. + pub fn is_empty(&self) -> bool { + self.entries.is_empty() + } +} + +impl ForceContainer for KindContainer { + fn kind(&self) -> ForceKind { + self.kind + } + fn persistence(&self) -> Persistence { + self.persistence + } + fn contributions(&self) -> impl Iterator { + self.entries.iter().map(|e| e as &dyn ForceContribution) + } + fn remove(&mut self, id: u64) -> bool { + if let Some(pos) = self.entries.iter().position(|e| e.id == id) { + self.entries.remove(pos); + true + } else { + false + } + } + fn clear(&mut self) { + self.entries.clear(); + } +} + +/// Per-body map of force containers, classified by [`ForceKind`]. +pub type BodyForceContainers = HashMap; + +/// Sum all force contributions of a body into its effective force/torque. +/// +/// * Legacy `user_force`/`user_torque` are included for backward compatibility. +/// * World gravity is read from `gravity_container` (a `Persistent` container +/// whose entry stores the gravity *acceleration*; it is scaled by the body's +/// effective mass and `gravity_scale`, exactly like the legacy path). +/// * Every body container is summed; persistent and transient are treated +/// identically here — the lifecycle difference is resolved at frame end by +/// [`ForceContainer::end_frame`]. +pub fn compute_body_effective_forces(rb: &mut RigidBody, gravity_container: &KindContainer) { + let mut eff = EffectiveForce::default(); + + // Legacy user slot (kept for backward compatibility). + eff.force += rb.forces.user_force; + eff.torque += rb.forces.user_torque; + + // World gravity (acceleration stored in the gravity container entries). + let mass = rb.mprops.effective_mass(); + let gravity_scale = rb.forces.gravity_scale; + for entry in &gravity_container.entries { + eff.force += entry.force * mass * gravity_scale; + } + + // Body containers, classified by kind. Persistence is not consulted here — + // both persistent and transient forces act this step; the transient ones are + // drained afterward by `drain_transient_forces`. Solver-emergent kinds + // (`Friction`, `ContactReaction`) are *observation-only* readouts: the contact + // solver already applies their impulses, so if any entry found its way into + // these containers we must NOT re-sum them (that would double-apply contact + // forces and destabilize the solve). The bridge that fills these containers + // (`bridge_solver_contact_forces`) writes them but they are consumed only via + // `force_container(ForceKind::Friction/ContactReaction)` queries, never here. + let world_com = rb.mprops.world_com; + for container in rb.force_containers.values() { + if container.kind() == ForceKind::Friction || container.kind() == ForceKind::ContactReaction + { + continue; + } + for c in container.contributions() { + c.accumulate(&mut eff, world_com); + } + } + + rb.forces.force = eff.force; + rb.forces.torque = eff.torque; +} + +/// Drain every transient (per-step) force container on a body. +/// +/// Called at frame end so one-shot / event / contact forces do not leak into +/// the next step, while persistent containers (gravity, steady thrust) keep +/// their entries — eliminating the manual `reset_forces` ritual. +pub fn drain_transient_forces(rb: &mut RigidBody) { + // No containers at all → nothing to drain. Early-return before taking any + // mutable borrow so we never trip the body's "modified" flag during steady + // state (which would force an island rebuild / active-set epoch bump). + if rb.force_containers.is_empty() { + return; + } + let mut drained_any = false; + for container in rb.force_containers.values_mut() { + if container.persistence() == Persistence::Transient && !container.entries.is_empty() { + container.end_frame(); + drained_any = true; + } + } + // If nothing was actually cleared, avoid leaving a dangling mutable borrow + // that some callers interpret as a modification. + let _ = drained_any; +} + +/// Bridge the contact solver's *emergent* normal + friction impulses into the +/// `Friction` / `ContactReaction` force containers, as an **observation-only** +/// readout. +/// +/// These forces are produced by the solver every step and already applied to the +/// bodies — they are NOT re-summed by [`compute_body_effective_forces`]. Writing +/// them into dedicated containers lets application code inspect the per-step +/// contact reaction / friction (e.g. for grip estimation, slip detection, or +/// `ContactForceEvent`-driven force injection) without disturbing the solve. +/// +/// Each kind is rebuilt fresh every step (matching the `Transient` lifecycle: +/// the solver re-emerges them each step), so a sleeping pair that stops touching +/// simply leaves no entry next step — no manual drain needed. +/// +/// # Determinism +/// +/// Must run **after** `build_islands_and_solve_velocity_constraints` on a single +/// thread (the contact graph is not `Sync` and incremental graph maintenance +/// assumes no concurrent edge mutation). Callers (the pipeline) already hold the +/// whole `NarrowPhase` mutably here. +pub fn bridge_solver_contact_forces( + narrow_phase: &NarrowPhase, + bodies: &mut RigidBodySet, + dt: Real, +) { + let inv_dt = crate::utils::inv(dt); + + for pair in narrow_phase.contact_pairs() { + // Reconstruct the per-pair world-space force by summing each contact's + // normal impulse (reaction) and tangential impulse (friction), scaled by + // inv_dt to convert impulse → force, and using the frozen lever arms + // (solver_dp1) for the torque. + let mut reaction = Vector::ZERO; + let mut friction = Vector::ZERO; + let mut r_torque = AngVector::default(); + let mut f_torque = AngVector::default(); + + // Body handles come from each manifold's `ContactManifoldData` (a pair can + // span multiple manifolds, but they share the same two bodies). + let mut rb1 = None; + let mut rb2 = None; + + for manifold in pair.solver_manifolds() { + let normal = manifold.data.normal; + let tangents = normal.orthonormal_basis(); + for c in manifold.contacts() { + let jn = c.data.impulse * inv_dt; + // Physical contact reaction force on `rigid_body1` = `-normal * jn` + // (the contact pushes body1 *away* from body2; the solver's `impulse` + // is positive for compression). Force on body2 is the opposite. + let reaction_force = -normal * jn; + reaction += reaction_force; + // Friction force on body1 from the tangential impulse vector. + let jt = c.data.tangent_impulse; + #[cfg(feature = "dim2")] + let friction_force = -tangents[0] * jt.x * inv_dt; + #[cfg(feature = "dim3")] + let friction_force = -(tangents[0] * jt.x + tangents[1] * jt.y) * inv_dt; + friction += friction_force; + + // Torque = r × F using the frozen lever arm (solver_dp1). + r_torque += c.data.solver_dp1.gcross(reaction_force); + f_torque += c.data.solver_dp1.gcross(friction_force); + } + + if rb1.is_none() { + rb1 = manifold.data.rigid_body1; + rb2 = manifold.data.rigid_body2; + } + } + + let (rb1, rb2) = match (rb1, rb2) { + (Some(h1), Some(h2)) => (h1, h2), + _ => continue, + }; + + // Body 1 gets +reaction / +friction; body 2 gets the opposite. + if let Some(rb) = bodies.get_mut(rb1) { + write_contact_observation(rb, reaction, friction, r_torque, f_torque); + } + if let Some(rb) = bodies.get_mut(rb2) { + write_contact_observation(rb, -reaction, -friction, -r_torque, -f_torque); + } + } +} + +/// Push one pair's contact reaction + friction into the body's observation-only +/// containers, rebuilding them each step (Transient lifecycle). +fn write_contact_observation( + rb: &mut RigidBody, + reaction: Vector, + friction: Vector, + r_torque: AngVector, + f_torque: AngVector, +) { + // Rebuild fresh: clear previous step's observation (it is solver-emergent, + // so it re-emerges from scratch each step). + rb.force_containers.insert( + ForceKind::ContactReaction, + KindContainer::new(ForceKind::ContactReaction, Persistence::Transient), + ); + rb.force_containers.insert( + ForceKind::Friction, + KindContainer::new(ForceKind::Friction, Persistence::Transient), + ); + + if reaction.length_squared() > 0.0 { + rb.force_containers + .get_mut(&ForceKind::ContactReaction) + .unwrap() + .push(ForceEntry { + id: 1, + force: reaction, + torque: r_torque, + point: None, + }); + } + if friction.length_squared() > 0.0 { + rb.force_containers + .get_mut(&ForceKind::Friction) + .unwrap() + .push(ForceEntry { + id: 1, + force: friction, + torque: f_torque, + point: None, + }); + } +} diff --git a/src/dynamics/granular.rs b/src/dynamics/granular.rs new file mode 100644 index 000000000..5285fd099 --- /dev/null +++ b/src/dynamics/granular.rs @@ -0,0 +1,354 @@ +//! Granular-body (DEM — Discrete Element Method) support — Phase 0. +//! +//! Sister module to `fluid.rs` (Phase 35): the same particle-cloud scaffolding +//! (`GranularParticle` / `GranularWorld` / `GranularParams`, naive O(n²) +//! neighbour search, semi-implicit Euler, no SoA boundary touched, fork-local +//! `f64` math), with the fluid's pressure/viscosity kernels replaced by a +//! granular contact model: +//! +//! * **Radial repulsion** — linear-spring push when two particles overlap +//! (`d < r_i + r_j`), stiffness `k_n`, with velocity-proportional normal +//! damping `c_n`. Hertzian-style: no attraction, no tension. +//! * **Tangential friction** — Coulomb-limited tangential damping on the +//! relative tangential velocity, clamped to `μ · |F_n|`. This is the +//! defining granular term: it lets a pile keep its slope (angle of repose) +//! instead of flowing away like sand-coloured water. +//! * **Rolling resistance** — optional extra damping of the relative +//! tangential velocity below the Coulomb cap (stabilises heaps; keeps the +//! model purely force-based, no angular DOF in Phase 0). +//! +//! Determinism: fixed particle-index iteration order and `f64` only, so runs +//! are bit-identical under `enhanced-determinism` (same contract as +//! `FluidWorld::step` / `SoftBody`). +//! +//! Phase 0 = data structures + contact force model + integrator + tests. +//! Later phases wire `GranularWorld` into `PhysicsWorld` and add rigid-body / +//! voxel coupling (dig terrain → spawn grains). + +use crate::alloc_prelude::Vec; +use crate::math::{Real, Vector}; +use std::collections::HashMap; + +/// A single DEM granular particle. +#[derive(Clone, Debug)] +pub struct GranularParticle { + /// Current world-space position. + pub pos: Vector, + /// Current world-space linear velocity. + pub vel: Vector, + /// Accumulated acceleration for the current step (cleared each `step`). + pub accel: Vector, + /// Particle mass (> 0). + pub mass: Real, + /// Contact radius (> 0). Two particles touch when their centres are + /// closer than the sum of their radii. + pub radius: Real, +} + +impl GranularParticle { + /// Creates a free granular particle. + pub fn new(pos: Vector, vel: Vector, mass: Real, radius: Real) -> Self { + Self { + pos, + vel, + accel: Vector::ZERO, + mass, + radius, + } + } +} + +/// DEM tunable parameters for a [`GranularWorld`]. +#[derive(Clone, Copy, Debug)] +pub struct GranularParams { + /// Normal contact stiffness `k_n` (linear spring, N/m). Must be `> 0`. + /// Keep `k_n / m · dt² < 1` for explicit-integrator stability. + pub normal_stiffness: Real, + /// Normal contact damping `c_n` (N·s/m), velocity-proportional along the + /// contact normal. `>= 0`. + pub normal_damping: Real, + /// Coulomb friction coefficient `μ`. Tangential force is clamped to + /// `μ · |F_n|`. `>= 0`. + pub friction: Real, + /// Tangential damping fraction in `[0, 1]`: scales the relative + /// tangential velocity before the Coulomb clamp (rolling-resistance + /// proxy). `0` = pure Coulomb cap on an undamped slide. + pub tangential_damping: Real, + /// Constant body acceleration (typically gravity). + pub gravity: Vector, +} + +impl Default for GranularParams { + fn default() -> Self { + Self { + normal_stiffness: 800.0, + normal_damping: 0.5, + friction: 0.6, + tangential_damping: 0.4, + gravity: Vector::ZERO, + } + } +} + +/// A cloud of [`GranularParticle`]s sharing one set of [`GranularParams`]. +#[derive(Clone, Debug)] +pub struct GranularWorld { + /// Particles. Index into this `Vec` is the particle id. + pub particles: Vec, + /// Shared DEM parameters. + pub params: GranularParams, +} + +impl GranularWorld { + /// Creates an empty granular world with the given parameters. + pub fn new(params: GranularParams) -> Self { + Self { + particles: Vec::new(), + params, + } + } + + /// Number of particles. + pub fn len(&self) -> usize { + self.particles.len() + } + + /// True when there are no particles. + pub fn is_empty(&self) -> bool { + self.particles.is_empty() + } + + /// Appends a particle, returning its index. + pub fn add_particle(&mut self, pos: Vector, vel: Vector, mass: Real, radius: Real) -> usize { + let i = self.particles.len(); + self.particles + .push(GranularParticle::new(pos, vel, mass, radius)); + i + } + + /// Advance the granular cloud by one timestep `dt` using semi-implicit + /// Euler. + /// + /// Per step: (1) accumulate pairwise contact forces (radial spring-damper + /// + Coulomb-clamped tangential damping) over all i i), so + // the force sequence is reproducible run-to-run. + let mut r_max: Real = 0.0; + for part in &self.particles { + r_max = r_max.max(part.radius); + } + let cell = 2.0 * r_max; + let mut cells: HashMap<(i64, i64, i64), Vec> = HashMap::new(); + for (idx, part) in self.particles.iter().enumerate() { + let key = ( + (part.pos.x / cell).floor() as i64, + (part.pos.y / cell).floor() as i64, + (part.pos.z / cell).floor() as i64, + ); + cells.entry(key).or_default().push(idx); + } + let mut accels: Vec = Vec::new(); + accels.resize(n, Vector::ZERO); + for i in 0..n { + let pi = self.particles[i].pos; + let vi = self.particles[i].vel; + let ri = self.particles[i].radius; + let cx = (pi.x / cell).floor() as i64; + let cy = (pi.y / cell).floor() as i64; + let cz = (pi.z / cell).floor() as i64; + for dx in -1..=1i64 { + for dy in -1..=1i64 { + for dz in -1..=1i64 { + let Some(bucket) = cells.get(&(cx + dx, cy + dy, cz + dz)) else { + continue; + }; + for &j in bucket { + if j <= i { + continue; // each unordered pair once + } + let pj = self.particles[j].pos; + let rij = pi - pj; // from j to i + let d2 = rij.length_squared(); + let rsum = ri + self.particles[j].radius; + if d2 >= rsum * rsum { + continue; // not touching + } + let d = d2.max(1e-18).sqrt(); + let n_hat = rij / d; // from j toward i + let overlap = rsum - d; + let vij = vi - self.particles[j].vel; + let v_n = vij.dot(n_hat); // approaching when negative + + // Normal force on i: spring push apart + damping of approach. + let f_n_mag = + (p.normal_stiffness * overlap - p.normal_damping * v_n).max(0.0); + let f_n = n_hat * f_n_mag; + + // Tangential relative velocity (slide direction). + let v_t_vec = vij - n_hat * v_n; + let v_t = v_t_vec.length(); + // Tangential force on i opposes the slide, capped by μ·|F_n|. + let f_t = if v_t > 1e-12 { + let t_hat = v_t_vec / v_t; + let raw = p.tangential_damping * v_t; + let cap = p.friction * f_n_mag; + -t_hat * raw.min(cap) + } else { + Vector::ZERO + }; + + let f = f_n + f_t; + accels[i] += f / self.particles[i].mass; + accels[j] -= f / self.particles[j].mass; + } + } + } + } + } + + // (2) Gravity + (3) semi-implicit Euler: velocities first, then + // positions — same operation order as `FluidWorld::step`. + for (i, part) in self.particles.iter_mut().enumerate() { + let a = accels[i] + p.gravity; + part.vel += a * dt; + part.pos += part.vel * dt; + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn params() -> GranularParams { + GranularParams { + normal_stiffness: 800.0, + normal_damping: 0.5, + friction: 0.0, + tangential_damping: 0.0, + gravity: Vector::new(0.0, -9.81, 0.0), + } + } + + #[test] + fn dem_single_particle_free_fall_is_analytic() { + // One particle, no neighbours → no contacts, pure gravity. + let mut gw = GranularWorld::new(params()); + gw.add_particle(Vector::new(0.0, 0.0, 0.0), Vector::ZERO, 1.0, 0.05); + let dt = 1.0 / 60.0; + for _ in 0..60 { + gw.step(dt); + } + let p = &gw.particles[0]; + // Semi-implicit Euler under constant gravity: x(t) = ½ g t² + ½ g·dt·t + // (an O(dt) drift vs the analytic ½ g t²). With dt = 1/60 and t = 1 s + // the drift is ≈ 0.082, so assert within 0.1. + let expected = 0.5 * (-9.81) * 1.0_f64 * 1.0; + assert!( + (p.pos.y - expected).abs() < 0.1, + "free-fall y={} expected≈{}", + p.pos.y, + expected + ); + assert!(p.pos.y < -4.0, "particle fell downward under gravity"); + assert!(p.pos.x.abs() < 1e-12 && p.pos.z.abs() < 1e-12); + } + + #[test] + fn dem_overlapping_particles_push_apart() { + // Two particles overlapping → spring pushes them apart until they + // separate; centre of mass stays put (symmetric forces). + let mut p = params(); + p.normal_damping = 2.0; // settle the bounce + p.gravity = Vector::ZERO; // isolate the contact response + let mut gw = GranularWorld::new(p); + gw.add_particle(Vector::new(-0.03, 0.0, 0.0), Vector::ZERO, 1.0, 0.05); + gw.add_particle(Vector::new(0.03, 0.0, 0.0), Vector::ZERO, 1.0, 0.05); + let dt = 1.0 / 120.0; + for _ in 0..240 { + gw.step(dt); + } + let sep = (gw.particles[1].pos - gw.particles[0].pos).length(); + assert!( + sep >= 0.1 - 1e-3, + "overlapping particles should push apart to ~sum of radii, sep={sep}" + ); + let com = (gw.particles[0].pos + gw.particles[1].pos) / 2.0; + assert!( + com.x.abs() < 1e-9 && com.y.abs() < 1e-6 && com.z.abs() < 1e-9, + "centre of mass should stay put, com={com:?}" + ); + } + + #[test] + fn dem_friction_damps_tangential_slide() { + // Two overlapping particles, relative velocity purely tangential. + // The lower one is frozen (huge mass); the upper one slides over it. + // With μ > 0 the tangential slide must decay during the brief contact + // (the Coulomb clamp allows a friction force); with μ = 0 it cannot. + let slide_after = |friction: Real| { + let mut p = params(); + p.friction = friction; + p.tangential_damping = 1.0; + p.normal_damping = 5.0; + // Soft normal spring → the contact persists long enough for the + // tangential friction to accumulate a visible impulse. + p.normal_stiffness = 100.0; + p.gravity = Vector::ZERO; + let mut gw = GranularWorld::new(p); + gw.add_particle(Vector::new(0.0, 0.0, 0.0), Vector::ZERO, 1.0e9, 0.5); + // Overlap 0.04 along x; slide velocity 2 m/s along y (tangential). + gw.add_particle( + Vector::new(0.96, 0.0, 0.0), + Vector::new(0.0, 2.0, 0.0), + 1.0, + 0.5, + ); + let dt = 1.0 / 240.0; + for _ in 0..40 { + gw.step(dt); + } + gw.particles[1].vel.y + }; + let v_frictionless = slide_after(0.0); + let v_friction = slide_after(0.8); + assert!( + (v_frictionless - 2.0).abs() < 0.15, + "without friction the slide is essentially untouched: v={v_frictionless}" + ); + assert!( + v_friction < v_frictionless - 0.1, + "friction should slow the slide: μ=0.8 v={v_friction} vs μ=0 v={v_frictionless}" + ); + } + + #[test] + fn dem_step_is_deterministic() { + let mut a = GranularWorld::new(params()); + a.add_particle(Vector::new(0.0, 0.0, 0.0), Vector::ZERO, 1.0, 0.05); + a.add_particle(Vector::new(0.08, 0.01, 0.0), Vector::ZERO, 1.0, 0.05); + a.add_particle(Vector::new(-0.06, 0.02, 0.05), Vector::ZERO, 1.0, 0.05); + let mut b = a.clone(); + let dt = 1.0 / 60.0; + for _ in 0..20 { + a.step(dt); + b.step(dt); + } + for (pa, pb) in a.particles.iter().zip(b.particles.iter()) { + assert_eq!(pa.pos, pb.pos, "positions bit-identical across runs"); + assert_eq!(pa.vel, pb.vel, "velocities bit-identical across runs"); + } + } +} diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 1be1ee01e..0f3f904a3 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -301,6 +301,15 @@ pub struct IntegrationParameters { /// The type of friction constraints used in the simulation. #[cfg(feature = "dim3")] pub friction_model: FrictionModel, + /// If `true`, the contact solver's emergent normal + friction impulses are + /// bridged into the observation-only `ForceKind::ContactReaction` / + /// `ForceKind::Friction` containers each step (default: `false`). + /// + /// These containers are **never re-summed** into the effective force (the + /// solver already applies the impulses), so enabling this only exposes the + /// per-step contact reaction / friction for inspection. Disable to keep the + /// pre-existing contact behavior fully unchanged. + pub bridge_contact_forces: bool, } impl IntegrationParameters { @@ -403,6 +412,7 @@ impl Default for IntegrationParameters { length_unit: 1.0, #[cfg(feature = "dim3")] friction_model: FrictionModel::default(), + bridge_contact_forces: false, } } } diff --git a/src/dynamics/island_manager/manager.rs b/src/dynamics/island_manager/manager.rs index ad440b668..f6e28e45b 100644 --- a/src/dynamics/island_manager/manager.rs +++ b/src/dynamics/island_manager/manager.rs @@ -372,9 +372,39 @@ impl IslandManager { let mut chunks: Vec> = Vec::new(); if !sleep_observations.is_empty() { self.persistent.begin_sleep_scan(); - for (island_id, eligible) in sleep_observations { - self.persistent - .observe_body_for_sleep(*island_id, *eligible); + #[cfg(feature = "parallel")] + { + // Parallel fold: each chunk AND-folds its own observations into a + // per-island local map; the folded values are then merged serially + // via `apply_sleep_eligibility`, keeping the shared `sleep_scan_touched` + // Vec race-free (same two-pass pattern as `solve.rs`'s active-body loop). + use rayon::prelude::*; + let folded: Vec> = sleep_observations + .par_chunks(256) + .map(|chunk| { + let mut local: std::collections::HashMap = + std::collections::HashMap::new(); + for &(island_id, eligible) in chunk { + local + .entry(island_id) + .and_modify(|e| *e &= eligible) + .or_insert(eligible); + } + local + }) + .collect(); + for map in &folded { + for (&island_id, &eligible) in map { + self.persistent.apply_sleep_eligibility(island_id, eligible); + } + } + } + #[cfg(not(feature = "parallel"))] + { + for (island_id, eligible) in sleep_observations { + self.persistent + .observe_body_for_sleep(*island_id, *eligible); + } } let sleepable = self.persistent.finish_sleep_scan(); diff --git a/src/dynamics/island_manager/persistent.rs b/src/dynamics/island_manager/persistent.rs index 6a8df2fd4..06cd11a65 100644 --- a/src/dynamics/island_manager/persistent.rs +++ b/src/dynamics/island_manager/persistent.rs @@ -482,6 +482,7 @@ impl PersistentIslands { /// Feeds one awake body's sleep eligibility into the scan. #[inline] + #[cfg(not(feature = "parallel"))] pub fn observe_body_for_sleep(&mut self, island_id: u32, eligible: bool) { let slot = &mut self.sleep_scan[island_id as usize]; if slot.0 != self.sleep_scan_stamp { @@ -492,6 +493,23 @@ impl PersistentIslands { } } + /// Applies an already-folded per-island sleep eligibility (the AND of every + /// body in that island) into the scan. Used by the parallel observe path: + /// each rayon chunk folds its own observations locally, then the folded + /// values are merged here serially — this keeps `sleep_scan_touched` (a + /// shared `Vec`) free of data races while the fold itself runs in parallel. + #[inline] + #[cfg(feature = "parallel")] + pub fn apply_sleep_eligibility(&mut self, island_id: u32, eligible: bool) { + let slot = &mut self.sleep_scan[island_id as usize]; + if slot.0 != self.sleep_scan_stamp { + *slot = (self.sleep_scan_stamp, eligible); + self.sleep_scan_touched.push(island_id); + } else { + slot.1 &= eligible; + } + } + /// Ends the scan: returns islands whose every observed body is eligible and that pass the /// split guard (an island that lost constraints must split before sleeping unless /// single-body). Returned islands are NOT yet marked sleeping — the caller commits them. diff --git a/src/dynamics/island_manager/sleep.rs b/src/dynamics/island_manager/sleep.rs index fe11b6a0e..6f36ba1ac 100644 --- a/src/dynamics/island_manager/sleep.rs +++ b/src/dynamics/island_manager/sleep.rs @@ -51,13 +51,47 @@ impl IslandManager { if sleeping_island { let island = &mut self.persistent.islands[persistent_id as usize]; island.sleeping = false; - // The island's bodies normally share one sleeping-chunk - // container, but joint-merged sleeping islands can span - // several: wake each body's chunk. + // Per-body activation reset: each member maps to a distinct + // body slot and `activation.wake_up` has no cross-body side + // effects, so the reset is disjoint and safe to parallelize. + // We borrow `bodies` through a raw pointer wrapped in a Sync + // type — the same discipline as `SharedCtx` in the staged solver. let handles = island.bodies.clone(); - for h in &handles { - if let Some(rb) = bodies.get_mut(*h) { - rb.activation.wake_up(true); + #[cfg(feature = "parallel")] + { + // Pointer to the body set, held in an `AtomicPtr` so it + // can cross the `rayon::broadcast` closure (which must be + // `Sync`); raw `*mut` is not. The parallel reset only + // touches disjoint body slots, so access is safe. + let wb = core::sync::atomic::AtomicPtr::new(bodies as *mut RigidBodySet); + let cursor = core::sync::atomic::AtomicUsize::new(0); + const BLOCK: usize = 64; + rayon::broadcast(|_| { + loop { + let start = + cursor.fetch_add(BLOCK, core::sync::atomic::Ordering::Relaxed); + if start >= handles.len() { + break; + } + let end = (start + BLOCK).min(handles.len()); + for h in &handles[start..end] { + let ptr = wb.load(core::sync::atomic::Ordering::Relaxed); + // SAFETY: `handles` are distinct island members, + // each resolves to a distinct body slot; access is + // disjoint across worker threads. + if let Some(rb) = unsafe { (*ptr).get_mut_internal(*h) } { + rb.activation.wake_up(true); + } + } + } + }); + } + #[cfg(not(feature = "parallel"))] + { + for h in &handles { + if let Some(rb) = bodies.get_mut_internal(*h) { + rb.activation.wake_up(true); + } } } for h in handles { diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index e814f0c24..acd927591 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -584,6 +584,34 @@ impl GenericJoint { } } + /// Drives the joint's degree of freedom along `axis` with a **constant joint-space force** + /// (issue #457), bypassing the spring/position motor model. + /// + /// This enables a force-based motor (`MotorModel::ForceBased`), sets its maximum deliverable + /// force to `force`, and zeroes the position/velocity targets so the motor applies a steady + /// push along the axis (a "force actuator") rather than regulating toward a pose. Use + /// [`Self::set_motor_force`] for linear axes and [`Self::set_motor_torque`] (the angular + /// alias) for rotational axes — both set joint-space force; the name only reflects intent. + pub fn set_motor_force(&mut self, axis: JointAxis, force: Real) -> &mut Self { + self.motor_axes |= axis.into(); + let i = axis as usize; + self.motors[i].model = MotorModel::ForceBased; + self.motors[i].max_force = force; + self.motors[i].target_pos = 0.0; + self.motors[i].target_vel = 0.0; + self.motors[i].stiffness = 0.0; + self.motors[i].damping = 0.0; + self + } + + /// Drives the joint's angular degree of freedom along `axis` with a **constant joint-space + /// torque** (issue #457). See [`Self::set_motor_force`] — this is the same force-based motor + /// drive, named for the rotational axis it is meant to act on. + #[cfg(feature = "dim3")] + pub fn set_motor_torque(&mut self, axis: JointAxis, torque: Real) -> &mut Self { + self.set_motor_force(axis, torque) + } + /// Configure both the target angle and target velocity of the motor. pub fn set_motor( &mut self, @@ -634,6 +662,68 @@ impl GenericJoint { self.local_frame2.translation -= rb2.mprops.local_mprops.local_com; } } + + /// The current **position** of this joint along each of its degrees of freedom, expressed in + /// the joint's local frame (issue #457). + /// + /// Returns an array of length [`SPATIAL_DIM`]: the first three entries are the relative + /// translation (LinX/LinY/LinZ) between the two attached bodies expressed in `body1`'s joint + /// frame, and the remaining entries are the relative rotation decomposed into its + /// axis components (AngX/AngY/AngZ — the rotation's scaled axis in 3D, its angle in 2D). + /// + /// This is the *measured* coordinate (not the motor's target), derived from the bodies' live + /// world poses, so it is valid at any point during the simulation. + pub fn position(&self, body1: &RigidBody, body2: &RigidBody) -> [Real; SPATIAL_DIM] { + let frame1 = body1.pos.position * self.local_frame1; + let frame2 = body2.pos.position * self.local_frame2; + + // Relative transform of body2 w.r.t. body1, in body1's joint frame. + let rel = frame1.inverse() * frame2; + + let mut out = [0.0; SPATIAL_DIM]; + out[0] = rel.translation.x; + out[1] = rel.translation.y; + #[cfg(feature = "dim3")] + { + out[2] = rel.translation.z; + } + + // Relative rotation decomposed into the joint's angular coordinates. + let rel_rot = rel.rotation; + #[cfg(feature = "dim3")] + { + // Scaled axis: angle * axis. Each component is the rotation about that joint axis. + let scaled = rel_rot.to_scaled_axis(); + out[3] = scaled.x; + out[4] = scaled.y; + out[5] = scaled.z; + } + #[cfg(feature = "dim2")] + { + out[2] = rel_rot.angle(); + } + out + } + + /// The current **angular position** (rotation) of this joint about its free rotational axes, + /// expressed in the joint's local frame (issue #457). + /// + /// In 3D this returns a vector whose `x/y/z` are the rotations about the joint's AngX/AngY/AngZ + /// axes (the relative rotation's scaled axis). In 2D it returns the single scalar rotation + /// angle about the joint's AngX axis. + #[cfg(feature = "dim3")] + pub fn angles(&self, body1: &RigidBody, body2: &RigidBody) -> Vector { + let p = self.position(body1, body2); + Vector::new(p[3], p[4], p[5]) + } + + /// The current **angular position** (rotation) of this joint about its free rotational axis, + /// expressed in the joint's local frame (issue #457). See the 3D [`Self::angles`] for the + /// vector variant. + #[cfg(feature = "dim2")] + pub fn angles(&self, body1: &RigidBody, body2: &RigidBody) -> Real { + self.position(body1, body2)[2] + } } macro_rules! joint_conversion_methods( @@ -856,3 +946,81 @@ impl From for GenericJoint { val.0 } } + +#[cfg(feature = "dim3")] +#[cfg(test)] +mod joint_query_force_tests { + use crate::dynamics::joint::{GenericJoint, JointAxesMask, JointAxis, MotorModel}; + use crate::dynamics::{ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodySet}; + use crate::geometry::{ColliderSet, NarrowPhase}; + use crate::math::{AngVector, Vector, rotation_from_angle}; + use crate::prelude::{ + CCDSolver, DefaultBroadPhase, IntegrationParameters, PhysicsPipeline, RigidBodyBuilder, + }; + + /// Issue #457: `set_motor_force` must configure a force-based motor (not a position/spring one). + #[test] + fn set_motor_force_enables_force_based_motor() { + let mut j = GenericJoint::new(JointAxesMask::empty()); + j.set_motor_force(JointAxis::AngX, 12.5); + let m = j.motor(JointAxis::AngX).expect("motor must be enabled"); + assert_eq!(m.model, MotorModel::ForceBased); + assert_eq!(m.max_force, 12.5); + // Targets must be cleared so the motor applies a steady push, not a pose regulator. + assert_eq!(m.target_pos, 0.0); + assert_eq!(m.target_vel, 0.0); + assert_eq!(m.stiffness, 0.0); + } + + /// Issue #457: `position`/`angles` measure the relative pose of the two attached bodies. + #[test] + fn joint_position_measures_relative_pose() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut islands = IslandManager::new(); + let mut bf = DefaultBroadPhase::new(); + let mut nf = NarrowPhase::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let mut ccd = CCDSolver::new(); + let mut pipeline = PhysicsPipeline::new(); + let gravity = Vector::ZERO; + let params = IntegrationParameters::default(); + + // Two bodies at the same pose => zero joint coordinates. + let b1 = bodies.insert(RigidBodyBuilder::dynamic().build()); + let b2 = bodies.insert(RigidBodyBuilder::dynamic().build()); + + let joint = GenericJoint::new(JointAxesMask::empty()); + // Coincident anchors, identity frames. + let p0 = joint.position(&bodies[b1], &bodies[b2]); + assert!( + p0.iter().all(|v| v.abs() < 1.0e-9), + "coincident bodies => zero position, got {:?}", + p0 + ); + + // Now rotate body2 by 90° about Y and step once so its pose is committed. + bodies[b2].set_rotation( + rotation_from_angle(AngVector::new(0.0, std::f64::consts::FRAC_PI_2, 0.0)), + true, + ); + pipeline.step( + gravity, + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut ccd, + &(), + &(), + ); + let ang = joint.angles(&bodies[b1], &bodies[b2]); + // The relative rotation about Y should be ~ +pi/2. + approx::assert_relative_eq!(ang.y, std::f64::consts::FRAC_PI_2, epsilon = 1.0e-6); + } +} diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 800250536..26a834081 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -8,6 +8,8 @@ pub use self::prismatic_joint::*; pub use self::revolute_joint::*; pub use self::rope_joint::*; pub use self::spring_joint::*; +#[cfg(feature = "dim3")] +pub use self::wheel_joint::*; #[cfg(feature = "dim3")] pub use self::spherical_joint::*; @@ -21,7 +23,9 @@ mod pin_slot_joint; mod prismatic_joint; mod revolute_joint; mod rope_joint; +mod spring_joint; #[cfg(feature = "dim3")] mod spherical_joint; -mod spring_joint; +#[cfg(feature = "dim3")] +mod wheel_joint; diff --git a/src/dynamics/joint/wheel_joint.rs b/src/dynamics/joint/wheel_joint.rs new file mode 100644 index 000000000..7a45d8562 --- /dev/null +++ b/src/dynamics/joint/wheel_joint.rs @@ -0,0 +1,211 @@ +//! A wheel joint is only meaningful in 3D (it uses yaw/roll axes that don't exist in 2D). + +#![cfg(feature = "dim3")] + +use crate::dynamics::JointAxis; +use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask, MotorModel}; +use crate::math::{Real, Vector}; + +/// A wheel joint connecting two rigid-bodies, used for vehicle suspension and steering. +/// +/// A wheel joint models a wheel attached to a chassis (or, more generally, two bodies +/// connected by a spring-loaded axle). It is composed of three conceptual behaviors, all +/// mapped onto the generic joint's motor/lock axes: +/// +/// - **Suspension**: the `LIN_Y` axis acts as a spring-damper pulling the two bodies toward +/// a rest length along the joint's local Y axis (the suspension travel direction). Set it +/// with [`Self::set_suspension`]. This is what makes the wheel compress and rebound. +/// - **Steering**: the `ANG_Y` axis (yaw) controls the wheel's heading. Drive it with +/// [`Self::set_steering`] (a target angle motor). When no steering is set, the wheel is +/// free to yaw. +/// - **Axle drive**: the `ANG_X` axis (roll) drives the wheel's spin. Motorize it with +/// [`Self::set_axle_velocity`] / [`Self::set_axle_target`]. +/// +/// Tire/road friction is handled by the contact constraint between the wheel's collider and +/// the ground — the joint only constrains the suspension, steering, and spin. All linear +/// degrees of freedom other than the suspension axis, and all angular degrees of freedom +/// other than steering/spin, are locked. +/// +/// This is a thin wrapper over [`GenericJoint`]; it reuses the existing constraint solver and +/// needs no engine-core changes. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct WheelJoint { + /// The underlying joint data. + pub data: GenericJoint, +} + +impl WheelJoint { + /// Creates a new wheel joint with the given suspension rest length, stiffness, and damping. + /// + /// The suspension travels along the joint's local Y axis; `rest_length` is its natural + /// length, `stiffness`/`damping` are the spring and damper coefficients (force-based by + /// default). The steering (`ANG_Y`) and spin (`ANG_X`) axes are left free until driven. + pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self { + let data = GenericJointBuilder::new( + // Lock everything except the suspension travel (LIN_Y), steering (ANG_Y) and spin (ANG_X). + JointAxesMask::LIN_X | JointAxesMask::LIN_Z | JointAxesMask::ANG_Z, + ) + .coupled_axes(JointAxesMask::LIN_Y) + .motor_position(JointAxis::LinY, rest_length, stiffness, damping) + .motor_model(JointAxis::LinY, MotorModel::ForceBased) + .build(); + Self { data } + } + + /// The underlying generic joint. + pub fn data(&self) -> &GenericJoint { + &self.data + } + + /// Are contacts between the attached rigid-bodies enabled? + pub fn contacts_enabled(&self) -> bool { + self.data.contacts_enabled + } + + /// Sets whether contacts between the attached rigid-bodies are enabled. + pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self { + self.data.set_contacts_enabled(enabled); + self + } + + /// The joint's anchor, expressed in the local-space of the first rigid-body. + #[must_use] + pub fn local_anchor1(&self) -> Vector { + self.data.local_anchor1() + } + + /// Sets the joint's anchor, expressed in the local-space of the first rigid-body. + pub fn set_local_anchor1(&mut self, anchor1: Vector) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + /// The joint's anchor, expressed in the local-space of the second rigid-body. + #[must_use] + pub fn local_anchor2(&self) -> Vector { + self.data.local_anchor2() + } + + /// Sets the joint's anchor, expressed in the local-space of the second rigid-body. + pub fn set_local_anchor2(&mut self, anchor2: Vector) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } + + /// The suspension rest length (natural length of the spring along the local Y axis). + #[must_use] + pub fn suspension_rest_length(&self) -> Real { + self.data + .limits(JointAxis::LinY) + .expect("suspension axis is always limited") + .max + } + + /// Sets the suspension rest length. + pub fn set_suspension_rest_length(&mut self, rest_length: Real) -> &mut Self { + self.data.set_limits(JointAxis::LinY, [0.0, rest_length]); + self + } + + /// Sets the suspension spring stiffness and damping (force-based model), keeping the + /// current rest length. + pub fn set_suspension(&mut self, stiffness: Real, damping: Real) -> &mut Self { + self.data.set_motor_position( + JointAxis::LinY, + self.suspension_rest_length(), + stiffness, + damping, + ); + self.data + .set_motor_model(JointAxis::LinY, MotorModel::ForceBased); + self + } + + /// Returns `(stiffness, damping)` of the suspension spring. + #[must_use] + pub fn suspension(&self) -> (Real, Real) { + let m = self + .data + .motor(JointAxis::LinY) + .expect("suspension axis is always motorized"); + (m.stiffness, m.damping) + } + + /// Sets the steering target angle (in radians) for the yaw axis (ANG_Y), with the given + /// stiffness and damping (an angular position motor). When not called, the wheel is free + /// to yaw. Passing `target = 0.0` points the wheel straight ahead. + pub fn set_steering(&mut self, target: Real, stiffness: Real, damping: Real) -> &mut Self { + self.data + .set_motor_position(JointAxis::AngY, target, stiffness, damping); + self.data + .set_motor_model(JointAxis::AngY, MotorModel::AccelerationBased); + self + } + + /// Sets the wheel's spin (ANG_X, roll) to a target velocity (rad/s), with the given + /// damping (an angular velocity motor). Use for driven/braked wheels. + pub fn set_axle_velocity(&mut self, target_vel: Real, damping: Real) -> &mut Self { + self.data + .set_motor_velocity(JointAxis::AngX, target_vel, damping); + self.data + .set_motor_model(JointAxis::AngX, MotorModel::AccelerationBased); + self + } + + /// Sets the wheel's spin (ANG_X, roll) to a target angle (radians), with the given + /// stiffness and damping (an angular position motor). + pub fn set_axle_target(&mut self, target: Real, stiffness: Real, damping: Real) -> &mut Self { + self.data + .set_motor_position(JointAxis::AngX, target, stiffness, damping); + self.data + .set_motor_model(JointAxis::AngX, MotorModel::AccelerationBased); + self + } +} + +impl From for GenericJoint { + fn from(val: WheelJoint) -> GenericJoint { + val.data + } +} + +/// A [`WheelJoint`] builder using the builder pattern. +/// +/// See the documentation of [`WheelJoint`] for the semantics of each setter. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct WheelJointBuilder(pub WheelJoint); + +impl WheelJointBuilder { + /// Creates a new builder for wheel joints. + pub fn new(rest_length: Real, stiffness: Real, damping: Real) -> Self { + Self(WheelJoint::new(rest_length, stiffness, damping)) + } + + /// Sets whether contacts between the attached rigid-bodies are enabled. + #[must_use] + pub fn contacts_enabled(mut self, enabled: bool) -> Self { + self.0.set_contacts_enabled(enabled); + self + } + + /// Sets the joint's anchor, expressed in the local-space of the first rigid-body. + #[must_use] + pub fn local_anchor1(mut self, anchor1: Vector) -> Self { + self.0.set_local_anchor1(anchor1); + self + } + + /// Sets the joint's anchor, expressed in the local-space of the second rigid-body. + #[must_use] + pub fn local_anchor2(mut self, anchor2: Vector) -> Self { + self.0.set_local_anchor2(anchor2); + self + } + + /// Builds the wheel joint. + pub fn build(self) -> WheelJoint { + self.0 + } +} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 51888cbd7..38c7b2f5c 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -30,6 +30,29 @@ pub use self::rigid_body::{RigidBody, RigidBodyBuilder}; #[cfg(feature = "alloc")] pub use self::rigid_body_set::{BodyPair, RigidBodySet}; +// Force containers: kind-classified, self-integrating {persistent, transient} +// force model. Declared AFTER `rigid_body` so the `RigidBody` type it depends +// on is already visible in this namespace. +#[cfg(feature = "alloc")] +pub use self::force_containers::*; + +// Soft-body (deformable body) support — Phase 0 foundation. Independent of the +// SoA SIMD solver boundary; wires into `World` / `PersistentIslands` in a later +// phase (see `.hermes/plans/2026-08-24_soft-body-roadmap.md`). +#[cfg(feature = "alloc")] +pub mod soft_body; + +// Fluid-body (SPH) support — Phase 0 foundation. Independent of the SoA SIMD +// solver boundary; see `.hermes/plans/2026-08-30_fluid-sph-roadmap.md`. +#[cfg(feature = "alloc")] +pub mod fluid; + +// Granular-body (DEM) support — Phase 0 foundation. Same scaffolding as +// `fluid.rs` (particle cloud, naive O(n²) neighbours, semi-implicit Euler) +// with a spring-damper + Coulomb-friction contact model. +#[cfg(feature = "alloc")] +pub mod granular; + #[cfg(feature = "alloc")] mod ccd; mod coefficient_combine_rule; @@ -47,3 +70,6 @@ pub(crate) mod solver; mod rigid_body; #[cfg(feature = "alloc")] mod rigid_body_set; + +#[cfg(feature = "alloc")] +pub mod force_containers; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 868167a0f..e42d702f5 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -8,6 +8,7 @@ use crate::dynamics::{ LockedAxes, MassProperties, RigidBodyActivation, RigidBodyAdditionalMassProps, RigidBodyCcd, RigidBodyChanges, RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity, + force_containers::ForceContainer, }; use crate::geometry::{ ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, ColliderSet, ColliderShape, @@ -51,6 +52,11 @@ pub struct RigidBody { pub(crate) damping: RigidBodyDamping, pub(crate) vels: RigidBodyVelocity, pub(crate) forces: RigidBodyForces, + /// Force containers, classified by [`ForceKind`]. Each is self-integrating + /// and records its own [`Persistence`] (persistent forces survive across + /// steps; transient forces drain every step). See `force_containers` module. + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(crate) force_containers: crate::dynamics::force_containers::BodyForceContainers, pub(crate) mprops: RigidBodyMassProps, pub(crate) ccd_vels: RigidBodyVelocity, @@ -65,6 +71,16 @@ pub struct RigidBody { pub(crate) dominance: RigidBodyDominance, pub(crate) enabled: bool, pub(crate) additional_solver_iterations: usize, + /// Optional maximum linear velocity magnitude (in units/second), issue #181. + /// + /// When set, the body's linear velocity is clamped to this magnitude during + /// the integration step (after forces and the constraint solver have run), so + /// no simulation can push it faster. `None` means no limit. + pub(crate) max_linvel: Option, + /// Optional maximum angular velocity magnitude (in radians/second), issue #181. + /// + /// See [`Self::max_linvel`]; applies to the angular velocity magnitude instead. + pub(crate) max_angvel: Option, /// User-defined data associated to this rigid-body. pub user_data: u128, } @@ -84,6 +100,7 @@ impl RigidBody { vels: RigidBodyVelocity::default(), damping: RigidBodyDamping::default(), forces: RigidBodyForces::default(), + force_containers: crate::dynamics::force_containers::BodyForceContainers::new(), ccd: RigidBodyCcd::default(), ids: RigidBodyIds::default(), colliders: RigidBodyColliders::default(), @@ -94,6 +111,8 @@ impl RigidBody { enabled: true, user_data: 0, additional_solver_iterations: 0, + max_linvel: None, + max_angvel: None, } } @@ -128,6 +147,7 @@ impl RigidBody { vels, damping, forces, + force_containers, ccd, ids: _ids, // Internal ids must not be overwritten. colliders: _colliders, // This function cannot be used to edit collider sets. @@ -138,6 +158,8 @@ impl RigidBody { enabled, additional_solver_iterations, user_data, + max_linvel, + max_angvel, } = other; self.pos = *pos; @@ -146,6 +168,7 @@ impl RigidBody { self.vels = *vels; self.damping = *damping; self.forces = *forces; + self.force_containers = force_containers.clone(); self.ccd = *ccd; self.activation = *activation; self.body_type = *body_type; @@ -153,6 +176,8 @@ impl RigidBody { self.enabled = *enabled; self.additional_solver_iterations = *additional_solver_iterations; self.user_data = *user_data; + self.max_linvel = *max_linvel; + self.max_angvel = *max_angvel; self.changes = RigidBodyChanges::all(); } @@ -509,6 +534,21 @@ impl RigidBody { self.ccd.ccd_enabled } + /// The world-space point of impact recorded by the CCD solver at the last continuous solve + /// (issue #548). + /// + /// Returns `Some((point, normal))` when this body actually hit something during the last + /// `world.step()` CCD pass (i.e. its motion was clamped to avoid tunneling), and `None` + /// when it swept freely. The `normal` points from this body toward the hit target. + #[must_use] + pub fn ccd_point_of_impact(&self) -> Option<(Vector, Vector)> { + if self.ccd.toi_normal == Vector::ZERO { + None + } else { + Some((self.ccd.toi_point, self.ccd.toi_normal)) + } + } + /// Sets the maximum prediction distance Soft Continuous Collision-Detection. /// /// When set to 0, soft-CCD is disabled. Soft-CCD helps prevent tunneling especially of @@ -954,6 +994,59 @@ impl RigidBody { } } + /// The optional maximum linear velocity magnitude (in units/second) of this rigid-body. + /// + /// Returns `None` if no limit is set. See [`Self::set_max_linvel`] (issue #181). + pub fn max_linvel(&self) -> Option { + self.max_linvel + } + + /// The optional maximum angular velocity magnitude (in radians/second) of this rigid-body. + /// + /// Returns `None` if no limit is set. See [`Self::set_max_angvel`] (issue #181). + #[cfg(feature = "dim2")] + pub fn max_angvel(&self) -> Option { + self.max_angvel.map(|v| v) + } + + /// The optional maximum angular velocity magnitude (in radians/second) of this rigid-body. + /// + /// Returns `None` if no limit is set. See [`Self::set_max_angvel`] (issue #181). + #[cfg(feature = "dim3")] + pub fn max_angvel(&self) -> Option { + self.max_angvel + } + + /// Set the maximum linear velocity magnitude (in units/second) of this rigid-body, issue #181. + /// + /// When `max_linvel` is `Some(limit)`, the body's linear velocity is clamped to magnitude + /// `limit` every integration step (after forces and the solver have run), so collisions, + /// motors, or gravity can never accelerate it beyond `limit`. Pass `None` to remove the cap. + pub fn set_max_linvel(&mut self, max_linvel: Option) { + self.max_linvel = max_linvel; + self.changes |= RigidBodyChanges::IN_MODIFIED_SET; + } + + /// Set the maximum angular velocity magnitude (in radians/second) of this rigid-body, issue #181. + /// + /// When `max_angvel` is `Some(limit)`, the body's angular velocity is clamped to magnitude + /// `limit` every integration step. Per-axis limits are supported in 3D (the vector's each + /// component is the cap on that axis). Pass `None` to remove the cap. + #[cfg(feature = "dim2")] + pub fn set_max_angvel(&mut self, max_angvel: Option) { + self.max_angvel = max_angvel; + self.changes |= RigidBodyChanges::IN_MODIFIED_SET; + } + + /// Set the maximum angular velocity magnitude (in radians/second) of this rigid-body, issue #181. + /// + /// See [`Self::set_max_angvel`] (the 3D variant takes a per-axis [`AngVector`] cap). + #[cfg(feature = "dim3")] + pub fn set_max_angvel(&mut self, max_angvel: Option) { + self.max_angvel = max_angvel; + self.changes |= RigidBodyChanges::IN_MODIFIED_SET; + } + /// The current position (translation + rotation) of this rigid body in world space. /// /// Returns an `SimdPose` which combines both translation and rotation. @@ -1182,6 +1275,107 @@ impl RigidBody { } } + // ── Kind-classified, self-integrating force containers ─────────────── + // + // These replace the flat `add_force` + per-step `reset_forces` ritual with a + // {persistent, transient} lifecycle. Each force *kind* owns a container that + // records its own `Persistence`; persistent forces (gravity, steady thrust) + // survive across steps, transient forces (events, contact friction/reaction) + // drain automatically. See `crate::dynamics::force_containers`. + + /// Get a reference to this body's force container of the given kind, if any. + pub fn force_container( + &self, + kind: crate::dynamics::force_containers::ForceKind, + ) -> Option<&crate::dynamics::force_containers::KindContainer> { + self.force_containers.get(&kind) + } + + /// Get a mutable reference to this body's force container of the given kind, + /// creating an empty one (with the supplied persistence) on first use. + pub fn force_container_mut_with( + &mut self, + kind: crate::dynamics::force_containers::ForceKind, + persistence: crate::dynamics::force_containers::Persistence, + ) -> &mut crate::dynamics::force_containers::KindContainer { + self.force_containers.entry(kind).or_insert_with(|| { + crate::dynamics::force_containers::KindContainer::new(kind, persistence) + }) + } + + /// Add a **persistent** thrust force (id-managed, survives across steps until + /// removed). Steady thrust/anchors no longer need re-`add_force` every frame. + /// Returns the entry id (use it with [`remove_force_by_kind`]). + #[profiling::function] + pub fn add_thrust( + &mut self, + id: u64, + force: Vector, + torque: AngVector, + point: Option, + persistence: crate::dynamics::force_containers::Persistence, + wake_up: bool, + ) -> u64 { + use crate::dynamics::force_containers::{ForceEntry, ForceKind}; + if self.body_type != RigidBodyType::Dynamic { + return 0; + } + let assigned = self + .force_container_mut_with(ForceKind::Thrust, persistence) + .push(ForceEntry { + id, + force, + torque, + point, + }); + if wake_up { + self.wake_up(true); + } + assigned + } + + /// Emit a **transient** force (event/one-shot). Valid for the current step + /// only; the container drains it at frame end automatically. + #[profiling::function] + pub fn emit_event_force( + &mut self, + force: Vector, + torque: AngVector, + point: Option, + source: crate::dynamics::force_containers::ForceKind, + wake_up: bool, + ) -> u64 { + use crate::dynamics::force_containers::{ForceEntry, Persistence}; + if self.body_type != RigidBodyType::Dynamic { + return 0; + } + let assigned = self + .force_container_mut_with(source, Persistence::Transient) + .push(ForceEntry { + id: 0, + force, + torque, + point, + }); + if wake_up { + self.wake_up(true); + } + assigned + } + + /// Remove a force entry by kind + id. Returns `true` if it was removed. + pub fn remove_force_by_kind( + &mut self, + kind: crate::dynamics::force_containers::ForceKind, + id: u64, + ) -> bool { + if let Some(c) = self.force_containers.get_mut(&kind) { + c.remove(id) + } else { + false + } + } + /// Applies a continuous force to this body (like thrust, wind, or magnets). /// /// Unlike [`apply_impulse()`](Self::apply_impulse) which is instant, a force is applied @@ -1541,6 +1735,14 @@ pub struct RigidBodyBuilder { pub additional_solver_iterations: usize, /// Are gyroscopic forces enabled for this rigid-body? pub gyroscopic_forces_enabled: bool, + /// Optional maximum linear velocity magnitude (units/second), issue #181. `None` = no limit. + pub max_linvel: Option, + /// Optional maximum angular velocity magnitude (radians/second), issue #181. `None` = no limit. + #[cfg(feature = "dim2")] + pub max_angvel: Option, + /// Optional maximum angular velocity magnitude (radians/second), issue #181. `None` = no limit. + #[cfg(feature = "dim3")] + pub max_angvel: Option, } impl Default for RigidBodyBuilder { @@ -1577,6 +1779,8 @@ impl RigidBodyBuilder { user_data: 0, additional_solver_iterations: 0, gyroscopic_forces_enabled: true, + max_linvel: None, + max_angvel: None, } } @@ -1940,6 +2144,29 @@ impl RigidBodyBuilder { self } + /// Set the maximum linear velocity magnitude (units/second) of the rigid-body to be built, + /// issue #181. `None` removes the cap (default). See [`RigidBody::set_max_linvel`]. + pub fn max_linvel(mut self, max_linvel: Option) -> Self { + self.max_linvel = max_linvel; + self + } + + /// Set the maximum angular velocity magnitude (radians/second) of the rigid-body to be built, + /// issue #181. `None` removes the cap (default). See [`RigidBody::set_max_angvel`]. + #[cfg(feature = "dim2")] + pub fn max_angvel(mut self, max_angvel: Option) -> Self { + self.max_angvel = max_angvel; + self + } + + /// Set the maximum angular velocity magnitude (radians/second) of the rigid-body to be built, + /// issue #181. `None` removes the cap (default). See [`RigidBody::set_max_angvel`]. + #[cfg(feature = "dim3")] + pub fn max_angvel(mut self, max_angvel: Option) -> Self { + self.max_angvel = max_angvel; + self + } + /// Sets whether the rigid-body is to be created asleep. pub fn sleeping(mut self, sleeping: bool) -> Self { self.sleeping = sleeping; @@ -1995,6 +2222,8 @@ impl RigidBodyBuilder { rb.enable_ccd(self.ccd_enabled); rb.set_soft_ccd_prediction(self.soft_ccd_prediction); rb.set_allow_fast_rotation(self.allow_fast_rotation); + rb.max_linvel = self.max_linvel; + rb.max_angvel = self.max_angvel; if self.can_sleep && self.sleeping { rb.sleep(); @@ -2044,3 +2273,577 @@ pub(crate) fn gyroscopic_corrected_angvel( angvel } } + +#[cfg(feature = "dim3")] +#[cfg(test)] +mod max_velocity_tests { + use crate::dynamics::{ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodySet}; + use crate::geometry::{ColliderSet, NarrowPhase}; + use crate::math::{Real, Vector}; + use crate::prelude::{ + CCDSolver, ColliderBuilder, DefaultBroadPhase, IntegrationParameters, PhysicsPipeline, + RigidBodyBuilder, + }; + + /// Regression test for issue #181: a dynamic body whose velocity would otherwise + /// exceed `max_linvel` must have its linear speed clamped to that cap every step. + #[test] + fn max_linvel_clamps_speed() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut islands = IslandManager::new(); + let mut bf = DefaultBroadPhase::new(); + let mut nf = NarrowPhase::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let mut ccd = CCDSolver::new(); + let mut pipeline = PhysicsPipeline::new(); + let gravity = Vector::new(0.0, 0.0, 0.0); // no gravity; we drive velocity directly + let params = IntegrationParameters::default(); + + // A body we launch very fast, but cap at 5 units/s. + let rb = RigidBodyBuilder::dynamic() + .linvel(Vector::new(1000.0, 0.0, 0.0)) + .max_linvel(Some(5.0)) + .build(); + let h = bodies.insert(rb); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), h, &mut bodies); + + for _ in 0..10 { + pipeline.step( + gravity, + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut ccd, + &(), + &(), + ); + } + + let speed = bodies[h].linvel().length(); + assert!( + speed <= 5.0 + 1.0e-6, + "linear speed {} exceeded max_linvel cap 5.0", + speed + ); + } + + /// Regression test for issue #181: a dynamic body whose angular speed would otherwise + /// exceed `max_angvel` must have each angular axis clamped to that per-axis cap. + #[test] + fn max_angvel_clamps_spin() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut islands = IslandManager::new(); + let mut bf = DefaultBroadPhase::new(); + let mut nf = NarrowPhase::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let mut ccd = CCDSolver::new(); + let mut pipeline = PhysicsPipeline::new(); + let gravity = Vector::new(0.0, 0.0, 0.0); + let params = IntegrationParameters::default(); + + let cap: Real = 2.0; + let rb = RigidBodyBuilder::dynamic() + .angvel(Vector::new(1000.0, 1000.0, 1000.0)) + .max_angvel(Some(Vector::new(cap, cap, cap))) + .build(); + let h = bodies.insert(rb); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), h, &mut bodies); + + for _ in 0..10 { + pipeline.step( + gravity, + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut ccd, + &(), + &(), + ); + } + + let av = bodies[h].angvel(); + assert!( + av.x.abs() <= cap + 1.0e-6 && av.y.abs() <= cap + 1.0e-6 && av.z.abs() <= cap + 1.0e-6, + "angular speed {:?} exceeded max_angvel cap {}", + av, + cap + ); + } +} + +#[cfg(feature = "dim3")] +#[cfg(test)] +mod force_container_tests { + use crate::dynamics::force_containers::{ForceContainer, ForceKind, Persistence}; + use crate::dynamics::{ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodySet}; + use crate::geometry::{ColliderSet, NarrowPhase}; + use crate::math::{AngVector, Vector}; + use crate::prelude::{ + CCDSolver, ColliderBuilder, DefaultBroadPhase, IntegrationParameters, PhysicsPipeline, + RigidBodyBuilder, + }; + + /// Build a minimal stepping context with no gravity. + fn step_context( + _gravity: Vector, + ) -> ( + PhysicsPipeline, + IslandManager, + DefaultBroadPhase, + NarrowPhase, + ImpulseJointSet, + MultibodyJointSet, + CCDSolver, + IntegrationParameters, + ) { + ( + PhysicsPipeline::new(), + IslandManager::new(), + DefaultBroadPhase::new(), + NarrowPhase::new(), + ImpulseJointSet::new(), + MultibodyJointSet::new(), + CCDSolver::new(), + IntegrationParameters::default(), + ) + } + + /// A persistent thrust force keeps acting every step without re-adding. + #[test] + fn persistent_thrust_survives_across_steps() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let (mut pipeline, mut islands, mut bf, mut nf, mut ij, mut mj, mut ccd, params) = + step_context(Vector::new(0.0, 0.0, 0.0)); + + let rb = RigidBodyBuilder::dynamic().build(); + let h = bodies.insert(rb); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), h, &mut bodies); + + // One persistent thrust along +X. No per-step re-add. + bodies[h].add_thrust( + 1, + Vector::new(10.0, 0.0, 0.0), + AngVector::new(0.0, 0.0, 0.0), + None, + Persistence::Persistent, + true, + ); + + // Step once: body gains some +X velocity. + let v0 = bodies[h].linvel().x; + for _ in 0..1 { + pipeline.step( + Vector::new(0.0, 0.0, 0.0), + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + &mut ccd, + &(), + &(), + ); + } + let v1 = bodies[h].linvel().x; + assert!(v1 > v0, "persistent thrust should accelerate the body"); + + // Step several more times WITHOUT re-adding — thrust must persist. + for _ in 0..5 { + pipeline.step( + Vector::new(0.0, 0.0, 0.0), + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + &mut ccd, + &(), + &(), + ); + } + let v2 = bodies[h].linvel().x; + assert!(v2 > v1, "persistent thrust must keep acting without re-add"); + + // The container is still present and persistent. + let c = bodies[h] + .force_container(ForceKind::Thrust) + .expect("thrust container exists"); + assert_eq!(c.persistence(), Persistence::Persistent); + assert_eq!(c.len(), 1); + } + + /// A transient (event) force applies for exactly one step, then auto-drains. + #[test] + fn transient_event_force_is_one_step() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let (mut pipeline, mut islands, mut bf, mut nf, mut ij, mut mj, mut ccd, params) = + step_context(Vector::new(0.0, 0.0, 0.0)); + + let rb = RigidBodyBuilder::dynamic().build(); + let h = bodies.insert(rb); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), h, &mut bodies); + + // Emit a transient event force. + bodies[h].emit_event_force( + Vector::new(10.0, 0.0, 0.0), + AngVector::new(0.0, 0.0, 0.0), + None, + ForceKind::Event, + true, + ); + + // Step once — transient force applies this step. + pipeline.step( + Vector::new(0.0, 0.0, 0.0), + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + &mut ccd, + &(), + &(), + ); + let v1 = bodies[h].linvel().x; + assert!(v1 > 0.0, "transient event force should apply on its step"); + + // Step again — the event force must be gone (auto-drained), so no further + // acceleration from it. Velocity should stay ~constant (no damping here). + let v_before = bodies[h].linvel().x; + pipeline.step( + Vector::new(0.0, 0.0, 0.0), + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + &mut ccd, + &(), + &(), + ); + let v_after = bodies[h].linvel().x; + assert!( + (v_after - v_before).abs() < 1.0e-6, + "transient force must not leak into the next step (dv={})", + v_after - v_before + ); + + // Container drained. + assert!( + bodies[h].force_container(ForceKind::Event).is_none() + || bodies[h] + .force_container(ForceKind::Event) + .unwrap() + .is_empty(), + "transient event container should be empty after the step" + ); + } + + /// Removing a force entry stops it from acting. + #[test] + fn remove_force_stops_application() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let (mut pipeline, mut islands, mut bf, mut nf, mut ij, mut mj, mut ccd, params) = + step_context(Vector::new(0.0, 0.0, 0.0)); + + let rb = RigidBodyBuilder::dynamic().build(); + let h = bodies.insert(rb); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), h, &mut bodies); + + let id = bodies[h].add_thrust( + 7, + Vector::new(10.0, 0.0, 0.0), + AngVector::new(0.0, 0.0, 0.0), + None, + Persistence::Persistent, + true, + ); + bodies[h].remove_force_by_kind(ForceKind::Thrust, id); + + pipeline.step( + Vector::new(0.0, 0.0, 0.0), + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + &mut ccd, + &(), + &(), + ); + let v = bodies[h].linvel().x; + assert!( + v.abs() < 1.0e-6, + "removed thrust must not accelerate the body (v={})", + v + ); + } + + /// Gravity is applied through the persistent gravity container every step + /// without any per-step re-apply, and bodies fall. + #[test] + fn gravity_persistent_via_container() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let gravity = Vector::new(0.0, -9.81, 0.0); + let (mut pipeline, mut islands, mut bf, mut nf, mut ij, mut mj, mut ccd, params) = + step_context(gravity); + + let rb = RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 10.0, 0.0)) + .build(); + let h = bodies.insert(rb); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), h, &mut bodies); + + // No manual gravity re-apply; step several times. + for _ in 0..5 { + pipeline.step( + gravity, + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + &mut ccd, + &(), + &(), + ); + } + // Body should have fallen (negative Y velocity, lower Y position). + assert!( + bodies[h].linvel().y < 0.0, + "gravity should pull the body down" + ); + assert!( + bodies[h].translation().y < 10.0, + "gravity should lower the body" + ); + } + + /// Contact reaction + friction are bridged into observation-only containers + /// when `bridge_contact_forces` is enabled, and do NOT perturb the physics + /// (the solver already applied the impulses — no double application). + #[test] + fn contact_forces_bridged_not_duplicated() { + use crate::dynamics::force_containers::ForceContainer; + + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let (mut pipeline, mut islands, mut bf, mut nf, mut ij, mut mj, mut ccd, mut params) = + step_context(Vector::new(0.0, -9.81, 0.0)); + + // A ball resting on a fixed ground. + let ground = RigidBodyBuilder::fixed().build(); + let gh = bodies.insert(ground); + colliders.insert_with_parent(ColliderBuilder::cuboid(10.0, 0.1, 10.0), gh, &mut bodies); + + let ball = RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 0.5, 0.0)) + .build(); + let bh = bodies.insert(ball); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), bh, &mut bodies); + + // Enable the bridge. + params.bridge_contact_forces = true; + + // Step until the ball is supported by the ground (contact established). + for _ in 0..20 { + pipeline.step( + Vector::new(0.0, -9.81, 0.0), + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + &mut ccd, + &(), + &(), + ); + } + + // The ball must be essentially stationary (not falling through), proving + // contact forces were applied by the SOLVER (not by our container). + let y = bodies[bh].translation().y; + assert!( + y > 0.3 && y < 0.7, + "ball should rest on ground, got y={}", + y + ); + + // The observation containers exist on the ball and report a non-zero + // contact reaction opposing gravity. + let reaction = bodies[bh] + .force_container(ForceKind::ContactReaction) + .expect("contact reaction container present"); + assert!( + !reaction.is_empty(), + "contact reaction container should have an entry" + ); + // Reaction on the ball points up (+Y) to counter gravity. + let rforce = reaction.contributions().next().unwrap().force(); + assert!( + rforce.y > 0.0, + "contact reaction should point up, got {:?}", + rforce + ); + + // Friction container may be present (ball not sliding → small/zero, but + // the kind is bridged regardless). Just assert it exists and is transient. + let friction = bodies[bh] + .force_container(ForceKind::Friction) + .expect("friction container present"); + assert_eq!(friction.persistence(), Persistence::Transient); + + // Critically: these containers are NOT re-summed. If they were, the ball + // would be shoved (up by reaction) and fly off. Re-check it stays put. + for _ in 0..20 { + pipeline.step( + Vector::new(0.0, -9.81, 0.0), + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + &mut ccd, + &(), + &(), + ); + } + let y2 = bodies[bh].translation().y; + assert!( + (y2 - y).abs() < 0.2, + "bridge must not double-apply contact forces (dy={})", + y2 - y + ); + } + + /// Without `bridge_contact_forces`, the contact containers are never populated. + #[test] + fn contact_forces_not_bridged_by_default() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let (mut pipeline, mut islands, mut bf, mut nf, mut ij, mut mj, mut ccd, params) = + step_context(Vector::new(0.0, -9.81, 0.0)); + + let ground = RigidBodyBuilder::fixed().build(); + let gh = bodies.insert(ground); + colliders.insert_with_parent(ColliderBuilder::cuboid(10.0, 0.1, 10.0), gh, &mut bodies); + + let ball = RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 0.5, 0.0)) + .build(); + let bh = bodies.insert(ball); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), bh, &mut bodies); + + // Default: bridge disabled. + assert!(!params.bridge_contact_forces); + + for _ in 0..20 { + pipeline.step( + Vector::new(0.0, -9.81, 0.0), + ¶ms, + &mut islands, + &mut bf, + &mut nf, + &mut bodies, + &mut colliders, + &mut ij, + &mut mj, + &mut ccd, + &(), + &(), + ); + } + + assert!( + bodies[bh] + .force_container(ForceKind::ContactReaction) + .is_none() + || bodies[bh] + .force_container(ForceKind::ContactReaction) + .unwrap() + .is_empty(), + "contact reaction container must be empty when bridge is off" + ); + // Physics still works (ball rests). + let y = bodies[bh].translation().y; + assert!( + y > 0.3 && y < 0.7, + "ball should still rest on ground, got y={}", + y + ); + } + + #[cfg(feature = "serde-serialize")] + #[test] + fn force_containers_serde_round_trip() { + use crate::dynamics::force_containers::{ForceContainer, Persistence}; + use crate::prelude::RigidBodyBuilder; + use serde_json; + + let mut rb = RigidBodyBuilder::dynamic().build(); + // Add a persistent thrust container with one entry. + let id = rb.add_thrust( + 0, + Vector::new(0.0, 10.0, 0.0), + AngVector::default(), + None, + Persistence::Persistent, + false, + ); + assert!(id != 0); + + let json = serde_json::to_string(&rb).expect("serialize RigidBody with force containers"); + let rb2: crate::dynamics::RigidBody = + serde_json::from_str(&json).expect("deserialize RigidBody with force containers"); + + let c = rb2 + .force_container(ForceKind::Thrust) + .expect("thrust container survives round-trip"); + assert_eq!(c.persistence(), Persistence::Persistent); + assert_eq!(c.len(), 1); + let entry = c.contributions().next().expect("entry present"); + assert!((entry.force().y - 10.0).abs() < 1e-9); + } +} diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index e23842521..4fc48d8c8 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -858,6 +858,49 @@ impl RigidBodyVelocity { } impl RigidBodyVelocity { + /// Clamp the magnitude of this velocity to the given caps (issue #181): the linear part to + /// `max_linvel` (its magnitude) and, in 3D, each angular-axis to the per-axis cap in + /// `max_angvel`. `None` caps are treated as "no limit" for that component. + #[cfg(feature = "dim2")] + pub fn clamp_magnitude(&self, max_linvel: Option, max_angvel: Option) -> Self { + let mut linvel = self.linvel; + if let Some(max) = max_linvel { + let len = linvel.length(); + if len > max { + linvel *= max / len; + } + } + let mut angvel = self.angvel; + if let Some(max) = max_angvel { + if angvel.abs() > max { + angvel = angvel.signum() * max; + } + } + RigidBodyVelocity { linvel, angvel } + } + + /// Clamp the magnitude of this velocity to the given caps (issue #181): the linear part to + /// `max_linvel` (its magnitude) and each angular axis to the per-axis cap in `max_angvel`. + /// `None` caps are treated as "no limit" for that component. + #[cfg(feature = "dim3")] + pub fn clamp_magnitude(&self, max_linvel: Option, max_angvel: Option) -> Self { + let mut linvel = self.linvel; + if let Some(max) = max_linvel { + let len = linvel.length(); + if len > max { + linvel *= max / len; + } + } + let mut angvel = self.angvel; + if let Some(max) = max_angvel { + let ax = angvel.x.abs().min(max.x).copysign(angvel.x); + let ay = angvel.y.abs().min(max.y).copysign(angvel.y); + let az = angvel.z.abs().min(max.z).copysign(angvel.z); + angvel = AngVector::new(ax, ay, az); + } + RigidBodyVelocity { linvel, angvel } + } + /// Same as [`Self::integrate`] but with the angular part linearized and the local /// center-of-mass assumed to be zero. #[inline] @@ -1027,6 +1070,11 @@ impl RigidBodyForces { /// Adds to `self` the gravitational force that would result in a gravitational acceleration /// equal to `gravity`. + /// + /// NOTE: superseded by the kind-classified force-container model + /// (`compute_body_effective_forces` in `force_containers`). Kept for API + /// compatibility; no longer called by the pipeline. + #[allow(dead_code)] pub fn compute_effective_force_and_torque(&mut self, gravity: Vector, mass: Vector) { self.force = self.user_force + gravity * mass * self.gravity_scale; self.torque = self.user_torque; @@ -1068,6 +1116,14 @@ pub struct RigidBodyCcd { /// By default angular velocity is clamped each substep to ~45°/step to keep CCD reliable; /// set `true` for bodies that must spin fast (e.g. wheels). pub allow_fast_rotation: bool, + /// World-space point of impact recorded by the CCD solver at the last continuous solve + /// (issue #548). `Vector::ZERO` when the body swept freely (no CCD hit). The separating + /// `normal` is in `Self::toi_normal`. + pub toi_point: Vector, + /// World-space separating normal recorded by the CCD solver at the last continuous solve + /// (issue #548), pointing from this body toward the hit target. `Vector::ZERO` when the + /// body swept freely (no CCD hit). + pub toi_normal: Vector, } impl Default for RigidBodyCcd { @@ -1078,6 +1134,8 @@ impl Default for RigidBodyCcd { ccd_enabled: false, soft_ccd_prediction: 0.0, allow_fast_rotation: false, + toi_point: Vector::ZERO, + toi_normal: Vector::ZERO, } } } diff --git a/src/dynamics/soft_body.rs b/src/dynamics/soft_body.rs new file mode 100644 index 000000000..80e4c71e9 --- /dev/null +++ b/src/dynamics/soft_body.rs @@ -0,0 +1,3513 @@ +//! Soft-body (deformable body) support — Phase 0 foundation. +//! +//! This module is the **data-structure + free-particle integrator** skeleton for +//! soft-body simulation. It is intentionally independent of the SoA SIMD solver +//! boundary (`helpers.rs` / `worker.rs` / `generic_contact_constraint.rs` read +//! the `velocities` / `accelerations` nalgebra buffers) so it can be compiled and +//! unit-tested in isolation. +//! +//! ## Design notes (see `.hermes/plans/2026-08-24_soft-body-roadmap.md`) +//! +//! * A soft body is a cloud of point masses (`SoftParticle`) connected by +//! Hookean springs (`Spring`). Springs carry `stiffness` (`k`) and `damping` +//! (`c`); the spring force is `F = -k·(|x_b - x_a| - rest)·dir - c·(v_rel · dir)·dir`. +//! * Integration is **semi-implicit (symplectic) Euler**: velocities are updated +//! first (`v += dt · M⁻¹ · f`), then positions (`x += dt · v`). This matches the +//! operation order used by the rigid-body integrator, and keeps the floating-point +//! sequence bit-identical across runs under `enhanced-determinism`. +//! * The internal spring/damping forces are the natural payload for the existing +//! `force_containers` `ForceKind::Custom(Persistent)` model (Phase 2 will route +//! them there); Phase 0 keeps them local so the numerics can be tested directly. +//! * All math uses `crate::math::Vector` (glam-backed) and `Real` so the module +//! stays aligned with the rest of the fork's vector conventions. +//! +//! Phase 0a (this file) = data structures + integrator + tests. Phase 0b (later) +//! wires `SoftBodySet` into `World` / `PersistentIslands`. Phase 1+ add joint-based +//! and mass-spring/FEM coupling. + +use crate::dynamics::{ + RigidBodyHandle, RigidBodySet, + force_containers::{ForceEntry, ForceKind, KindContainer, Persistence}, +}; +use crate::math::{AngVector, Real, Vector}; +use std::collections::{HashMap, HashSet}; +use std::vec::Vec; + +/// `ForceKind::Custom` discriminator for soft-body internal spring/damping forces. +/// Routed through `force_containers` so they share the same `Persistent` lifecycle +/// and `compute_body_effective_forces` summation as gravity/thrust (Phase 2). +pub const SOFT_SPRING_CUSTOM_ID: u32 = 0x5_042; // "SB" encoded; arbitrary custom tag + +/// Opaque id of a [`SoftBody`] inside a [`SoftBodySet`]. +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +pub struct SoftBodyId(pub u32); + +/// A single point mass in a soft body. +#[derive(Clone, Debug)] +pub struct SoftParticle { + /// Current world-space position. + pub pos: Vector, + /// Current world-space linear velocity. + pub vel: Vector, + /// Accumulated force for the current substep (cleared by `compute_forces`). + pub force: Vector, + /// Inverse mass (`0` = pinned / immovable). `mass = 1 / inv_mass`. + pub inv_mass: Real, + /// Optional rigid body this particle is bound to. When set, the particle's + /// internal spring/damping force is routed into that body's `force_containers` + /// (Phase 2), so the soft body drives the rigid body through the standard + /// effective-force path rather than integrating the particle directly. + pub bound_body: Option, + /// Phase 8: when `bound_body` is `Some`, this is the attachment point + /// expressed in the bound body's *local* frame (so the particle rigidly + /// follows the body as it translates/rotates). Computed at attach time from + /// the world-space attach point; ignored when `bound_body` is `None`. + pub bound_local: Vector, +} + +impl SoftParticle { + /// Creates a free (movable) particle with unit mass. + pub fn new(pos: Vector) -> Self { + Self { + pos, + vel: Vector::ZERO, + force: Vector::ZERO, + inv_mass: 1.0, + bound_body: None, + bound_local: Vector::ZERO, + } + } + + /// Creates a pinned (immovable) particle — its `inv_mass` is `0`. + pub fn pinned(pos: Vector) -> Self { + Self { + pos, + vel: Vector::ZERO, + force: Vector::ZERO, + inv_mass: 0.0, + bound_body: None, + bound_local: Vector::ZERO, + } + } + + /// Mass of the particle; `0` for pinned particles. + #[inline] + pub fn mass(&self) -> Real { + if self.inv_mass == 0.0 { + 0.0 + } else { + 1.0 / self.inv_mass + } + } +} + +/// A Hookean spring connecting two particles, with linear damping along its axis. +#[derive(Clone, Copy, Debug)] +pub struct Spring { + /// Index of the first endpoint particle within the parent [`SoftBody`]. + pub a: usize, + /// Index of the second endpoint particle within the parent [`SoftBody`]. + pub b: usize, + /// Rest length (natural length) of the spring. + pub rest_length: Real, + /// Spring constant `k` (stiffness). + pub stiffness: Real, + /// Damping coefficient `c` (axial velocity damping). + pub damping: Real, + /// Phase 31 — active-strain activation `γ ∈ [0, 1]`. Scales the effective rest + /// length toward zero (`rest_eff = rest_length * (1 - activation)`), pulling the + /// two endpoints together like a contracting muscle. `0` (default) = passive. + pub activation: Real, + /// Phase 32 — muscle-fibre direction (unit vector). When `Some(dir)`, the active + /// contraction is oriented along `dir` instead of purely along the edge — i.e. the + /// spring acts as a muscle fibre with a defined orientation (anisotropic drive). + /// `None` (default) = contract along the edge (degenerates to Phase 31 behaviour). + pub fibre: Option, +} + +/// A distance constraint (XPBD) between two particles — the edge of a deformable +/// mesh. Unlike [`Spring`] (a force), this is a *position* constraint solved by +/// XPBD's small-iteration projection, which is unconditionally stable even for +/// stiff/rigid edges (no explicit-spring explosion). +#[derive(Clone, Copy, Debug)] +pub struct DistanceConstraint { + /// First endpoint particle index. + pub a: usize, + /// Second endpoint particle index. + pub b: usize, + /// Rest length. + pub rest: Real, + /// XPBD stretch compliance `α_s` (0 = rigid, > 0 = soft), applied when the edge + /// is **longer** than `rest` (tension). Stored per-constraint so different edges + /// can have different stiffness. Phase 19: this is the *stretch* compliance; the + /// *compression* compliance lives in [`Self::compression`], enabling anisotropic + /// behaviour (e.g. cloth resists stretch but folds easily under compression). + pub compliance: Real, + /// Phase 19 — XPBD compression compliance `α_c` (0 = rigid, > 0 = soft), applied + /// when the edge is **shorter** than `rest` (compression). When equal to + /// [`Self::compliance`] the edge is isotropic. Initialized to the stretch + /// compliance by every constructor (`add_distance_constraint`, `add_triangle`, + /// `add_bending_constraint`) so existing bodies stay isotropic unless the caller + /// opts into anisotropy via `set_distance_constraint_compression`. + pub compression: Real, + /// Phase 31 — active-strain activation `γ ∈ [0, 1]`. Scales the effective rest + /// length toward zero (`rest_eff = rest * (1 - activation)`), contracting the + /// edge like a muscle fibre. `0` (default) = passive. + pub activation: Real, + /// Phase 32 — muscle-fibre direction (unit vector). See [`Spring::fibre`]; when + /// `Some(dir)` the active contraction is oriented along `dir` rather than purely + /// along the edge. `None` (default) = contract along the edge. + pub fibre: Option, +} + +/// Phase 27 — fracture-mechanics tearing criterion. +/// +/// Replaces the old single strain-threshold with three physically-motivated +/// fracture modes. Each edge (XPBD [`DistanceConstraint`] or MassSpring +/// [`Spring`]) is evaluated against the chosen scalar; when the scalar exceeds +/// the threshold the edge snaps (and any triangle losing a structural edge is +/// dropped too). +#[derive(Clone, Copy, Debug, PartialEq)] +pub enum TearCriterion { + /// Classic strain limit: snaps when `(|len| − rest)/rest > threshold`. + /// Threshold is dimensionless (e.g. `0.5` = break past 50% stretch). + Strain(Real), + /// Griffith-style stress limit: snaps when the axial force magnitude + /// `|k·(len − rest)|` exceeds `threshold` (force units). `k` is the spring + /// stiffness, or `1/(compliance + ε)` for an XPBD distance constraint. + Stress(Real), + /// Energy-release-rate limit: snaps when the elastic strain energy + /// `½·k·(len − rest)²` exceeds `threshold` (energy units). A proxy for the + /// fracture toughness / critical energy release rate of the material. + Energy(Real), +} + +/// Which integrator a [`SoftBody`] uses. +/// +/// * `MassSpring` — the Phase 0/2 Hookean-spring + semi-implicit Euler path. +/// * `Xpbd { iterations, compliance }` — Phase 3 position-based dynamics: edges +/// become [`DistanceConstraint`]s and tetrahedra carry a volume constraint, +/// both projected with a fixed number of Gauss-Seidel iterations. `compliance` +/// is the default used when a constraint is added without an explicit one. +#[derive(Clone, Copy, Debug, Default)] +pub enum SoftSolver { + /// Hookean springs (default, Phase 0/2). + #[default] + MassSpring, + /// XPBD position-based solver (Phase 3). + Xpbd { + /// Gauss-Seidel projection iterations per substep. + iterations: u32, + /// Default XPBD compliance for constraints created without an explicit one. + compliance: Real, + }, +} + +/// A uniform wind / air-resistance field applied to every free particle of a +/// soft body (Phase 7). It is a *pure external force* — no new mechanics, it +/// reuses the same force path as gravity: +/// +/// * `accel` — constant wind acceleration (a directional push, like a sideways +/// gravity). Makes a pinned-edge cloth fly out like a flag. +/// * `drag` — linear air-resistance coefficient. Each free particle feels +/// `F = m·accel − m·drag·v`, i.e. a velocity damping toward the wind state. +/// Keep `drag·dt < 1` for stability (the integration clamp handles it too). +#[derive(Clone, Copy, Debug)] +pub struct Wind { + /// Constant wind acceleration (`m/s²`), applied to every free particle. + pub accel: Vector, + /// Linear air-resistance coefficient (`1/s`). `F_drag = -m·drag·v`. + pub drag: Real, +} + +/// A soft body: a collection of point masses connected by springs. +/// +/// In Phase 0/2 it is a mass-spring cloud. Phase 3 adds an optional XPBD +/// position-based solver: edges become [`DistanceConstraint`]s and tetrahedra +/// carry a volume constraint, projected each substep. The `solver` field selects +/// which path [`SoftBody::step`] takes. +#[derive(Clone, Debug)] +pub struct SoftBody { + /// Point masses. Spring endpoints / constraint indices index into this `Vec`. + pub particles: Vec, + /// Springs (edges) between particles — used by the `MassSpring` solver. + pub springs: Vec, + /// Distance constraints (edges) — used by the `Xpbd` solver. + pub distance_constraints: Vec, + /// Tetrahedral volume elements. Each entry is `[a, b, c, d]` particle indices. + /// Used by the `Xpbd` solver's volume-preservation constraint. + pub tetrahedra: Vec<[u32; 4]>, + /// Rest (reference) signed volume of each tetrahedron, precomputed at + /// `add_tetrahedron` time. Indexed parallel to `tetrahedra`. + pub tetra_rest_volumes: Vec, + /// Triangular faces (cloth / shell topology). Each entry is `[a, b, c]` + /// particle indices, CCW for outward normal. Phase 6: cloth soft bodies are + /// built from triangles; the structural edges are added automatically as + /// distance constraints (see `add_triangle`), so the XPBD solver needs no new + /// mechanics — bending is just extra distance constraints between opposite + /// vertices of adjacent quads (composed by the caller). + pub triangles: Vec<[u32; 3]>, + /// Active integrator. + pub solver: SoftSolver, + /// Constant acceleration applied to every free particle (typically gravity). + pub gravity: Vector, + /// Coarse sleeping flag (island-style). When `true`, [`SoftBody::step`] is a + /// no-op: the whole body is treated as an inactive unit, mirroring how + /// `PersistentIslands` keeps a sleeping rigid-body island from being + /// re-simulated. Per-particle island membership is Phase 3; this flag gives + /// the same "skip inactive work" behavior at body granularity for now. + pub sleeping: bool, + /// Phase 5f: collision coupling flag. When `true` the soft body's particles + /// are driven by external proxy rigid bodies (one `Ball` collider per free + /// particle, maintained by the mps-core integration layer), so [`SoftBody::step`] + /// must NOT integrate the particles itself — their positions/velocities are + /// written back from the proxy bodies after the rigid-body narrow-phase/contact + /// step. Forces (springs + gravity) are still computed and exported by the + /// integration layer. Defaults to `false`. + pub collide: bool, + /// Phase 5f: proxy collider radius used when `collide` is enabled. Each free + /// particle gets a `Ball` collider of this radius. Defaults to `0.1`. + pub particle_radius: Real, + /// Phase 7: uniform wind / air-resistance field. `None` = no wind. When set, + /// every free particle feels `F = m·wind.accel − m·wind.drag·v` in addition to + /// gravity — a pure external force, no new solver mechanics. Applied in both + /// the `MassSpring` (`compute_forces`) and `Xpbd` (`step_xpbd` predict) paths. + pub wind: Option, + /// Phase 11: uniform internal pressure (`P`, force/area). When `Some`, every + /// free particle of a closed triangular mesh gets an outward push along the + /// surface normal, `F = P · area` per incident triangle (balloon / gas-law + /// model). A pure external force mirroring [`Self::wind`]; applied in both the + /// MassSpring (`compute_forces`) and XPBD (`step_xpbd` predict) paths. `None` + /// (default) = no pressure. Closed manifold (`self.triangles`) needed for a + /// real balloon; an open sheet just bulges along single-sided normals. + pub pressure: Option, + /// Phase 27: optional viscoelastic (rate-dependent) constitutive model. `None` = purely elastic. + pub viscoelastic: Option, + /// Phase 27: optional uniform thermal field. `None` = isothermal. + pub temperature: Option, + /// Phase 27: optional body-level orthotropic stiffness axes. When `Some(v)` + /// (each component ≥ 0), an edge's effective XPBD compliance is divided by the + /// projection `nᵀ·diag(v)·n` where `n` is the edge unit direction — so an edge + /// aligned with the x-axis uses compliance `α / v.x` (stiffer if `v.x > 1`). + /// This adds directional (orthotropic) material response on top of the + /// per-edge stretch/compression anisotropies ([`DistanceConstraint::compliance` + /// and `::compression`]); `None` (default) keeps every edge isotropic. + pub anisotropy: Option, + /// Phase 18: global internal (structural) damping. Each step, every free + /// particle's velocity is scaled by `1 − damping` (after the solver recovers + /// velocities), giving a body-wide "jelly / slime" energy loss that is + /// orthogonal to the per-spring axial `damping` (Phase 0) and to the + /// distance-solver `compliance` (Phase 13). `0` (default) = no damping (energy + /// conserving for the internal modes); larger values settle oscillation / jitter + /// faster. Clamped to `[0, 1)` — `1` would fully freeze motion, so it is rejected. + /// Applied in both the MassSpring (`integrate`) and XPBD (`step_xpbd` velocity + /// recovery) paths. No new solver mechanics, no SoA interaction. + pub damping: Real, + /// Phase 24: XPBD/MassSpring substeps per [`SoftBody::step`] call. Splits the + /// frame `dt` into `substeps` equal slices and runs the active solver once per + /// slice (so constraint projection happens at a finer time resolution). `1` + /// reproduces the historic single-substep behaviour. Larger values make stiff + /// materials / high compliance converge faster and stay stable; they cost + /// `substeps`× the per-step work. Mirrors `World::integration_parameters` + /// substepping for the rigid side — same idea, local to each soft body. + pub substeps: u32, + /// Phase 9: tearing threshold. When `Some(ε)`, any structural edge (XPBD + /// distance constraint or MassSpring spring) whose strain `(|len| − rest)/rest` + /// exceeds `ε` is removed at the start of each [`SoftBody::step`]. Triangular + /// faces that lose any structural edge are dropped too, so a torn cloth stops + /// rendering the broken face. `None` (default) = no tearing. Pure topology + /// edit — no new solver mechanics, no SoA interaction. + pub tear: Option, + /// Phase 10: plasticity (permanent deformation, like putty / memory foam). + /// When `Some(params)`, any structural edge whose elastic strain magnitude + /// exceeds `params.yield_strain` has its rest length permanently shifted + /// toward the current length by `params.creep` (clamped to `[0,1]`) each step, + /// so the deformation "freezes in" instead of springing back. `None` (default) + /// = perfectly elastic (Hookean). Pure rest-length edit — no new solver + /// mechanics, no SoA interaction. + pub plasticity: Option, + /// Phase 12: self-collision. When `Some(params)`, the body's free particles + /// repel each other when their centres come within `2·params.radius` (each + /// particle behaves as a sphere of that radius). Broad-phase uses a uniform + /// spatial hash; detected pairs are solved as stiff XPBD distance constraints + /// (rest = `2·radius`, compliance = `params.stiffness`) every solver iteration, + /// in both the MassSpring and XPBD paths. Direct structural neighbours (linked + /// by a [`SoftBody::distance_constraints`] edge) are excluded so existing + /// springs/cloth edges are not treated as collisions. `None` (default) = off. + /// Pure positional projection — no new solver mechanics, no SoA interaction. + pub self_collision: Option, + /// Phase 14: soft-soft (cross-body) collision. When `Some(params)`, this body + /// collides with *other* soft bodies that also have `cross_collision` set: their + /// free particles repel when centres come within `2·min(radius_a, radius_b)`. + /// Reuses the same spatial-hash broad-phase + XPBD push-apart as self-collision, + /// but the world-level pass runs over pairs of bodies. `None` (default) = off. + /// Pure positional projection — no new solver mechanics, no SoA interaction. + pub cross_collision: Option, + /// Phase 16: dedicated volume-conservation compliance for the tetrahedral + /// volume constraints. When `Some(c)`, every tetra volume constraint in + /// `step_xpbd` is solved with `α̃ = c / dt²` — *independent* of the distance + /// solver's compliance. This makes it possible to have soft edges but a hard + /// (incompressible) blob, or to keep volume conserved even when the distance + /// solver is soft. It is orthogonal to Phase 11 pressure (an outward force): + /// pressure inflates, this constraint holds the total volume. `None` (default) + /// = fall back to the global solver compliance for tetra volume (existing + /// behaviour). Pure positional constraint — no new solver mechanics. + pub volume_conservation: Option, + /// Phase 29: corotated linear elasticity (per-tetrahedron shape matching). + /// When `Some(stiffness)`, each XPBD iteration additionally projects every + /// tetrahedron toward its best-fit-rotated rest shape (polar-decomposition + /// shape matching), giving rotation-invariant linear elasticity on top of + /// the existing volume constraint. `stiffness` in `(0, 1]` is the per-iteration + /// relaxation factor. `None` (default) = off. + pub corotated: Option, + /// Phase 30: Neo-Hookean logarithmic volume energy. When `Some(stiffness)` + /// and XPBD is active, each tetrahedron's volume constraint uses the + /// nonlinear residual `C = ln(J)` (`J = V/V₀`, clamped to a small positive + /// floor) and a `stiffness/dt²` compliance, replacing the linear + /// `V − V₀` residual + `volume_conservation` compliance for that tet. + /// The logarithmic form makes the volumetric resistance grow unboundedly as + /// the tet collapses (physically correct incompressibility) instead of the + /// linear response's finite push-back. `None` (default) = linear volume. + pub neo_hookean: Option, + /// Phase 29: inverse rest shape matrix per tetrahedron (columns are the rest + /// edge vectors of the reference configuration). Built by + /// [`SoftBody::set_corotated`] from the particle positions at enable time. + /// Indexed parallel to `tetrahedra`; shorter than `tetrahedra` after a later + /// `subdivide_tetrahedra` (extra tets are skipped, not corrupted). + pub tetra_rest_shapes: Vec<[[Real; 3]; 3]>, + /// Phase 17: cohesion (adhesion / breakable glue) between this body and *other* + /// bodies. When `Some(p)`, free particles of this body within `p.radius` of a + /// free particle of another cohesion-enabled body attract toward contact, bonding + /// the bodies (the dual of Phase 9 tearing). Bonds break when pulled apart beyond + /// `p.break_distance`. Solved at world level by `solve_cohesion`. `None` (default) + /// = off. + pub cohesion: Option, +} +/// Phase 10: plasticity parameters (see [`SoftBody::plasticity`]). +#[derive(Clone, Copy, Debug)] +pub struct PlasticityParams { + /// Yield strain: elastic deformation below this magnitude is fully recovered; + /// above it, the excess becomes permanent. Must be ≥ 0. + pub yield_strain: Real, + /// Creep rate in `[0,1]`: fraction of the over-yield strain that is transferred + /// from elastic to plastic (rest-length) each step. `1` = instantly frozen at + /// the yield surface; `0` = no plasticity (elastic). + pub creep: Real, +} + +/// Phase 27: viscoelastic (rate-dependent) constitutive parameters. +/// +/// Implements a Kelvin-Voigt-style **strain-rate hardening**: the effective spring +/// stiffness grows with the magnitude of the local stretch rate, so a rapidly +/// stretched edge resists more than a slowly stretched one (polymer / viscoelastic +/// behaviour). `rate_coefficient >= 0`; `0` (or `None`) is pure elasticity. +#[derive(Clone, Copy, Debug)] +pub struct ViscoelasticParams { + /// Strain-rate stiffening gain. `k_eff = k·(1 + rate_coefficient·|d(strain)/dt|)`. + pub rate_coefficient: Real, +} + +/// Phase 27: thermal field parameters. +/// +/// A uniform body temperature `temp` (relative to `ambient`) modulates the material: +/// rest lengths expand by `expansion·ΔT` (thermal expansion) and stiffness softens by +/// `stiffness_temp_coeff·ΔT` (temperature-dependent modulus). Both effects are applied +/// to every spring/constraint, giving a temperature-dependent soft body. +#[derive(Clone, Copy, Debug)] +pub struct ThermalParams { + /// Current uniform body temperature. + pub temp: Real, + /// Reference (ambient) temperature the material was characterised at. + pub ambient: Real, + /// Linear thermal-expansion coefficient (rest-length change per unit ΔT). + pub expansion: Real, + /// Stiffness temperature coefficient (modulus drop per unit ΔT; clamped so the + /// effective stiffness stays > 0). + pub stiffness_temp_coeff: Real, +} + +/// Phase 12: self-collision parameters (see [`SoftBody::self_collision`]). +#[derive(Clone, Copy, Debug)] +pub struct SelfCollisionParams { + /// Particle collision radius. Two *free* particles whose centres come within + /// `2·radius` are pushed apart (each treated as a sphere of this radius). Should + /// match `SoftBody::particle_radius` used by the proxy-collider path, but is + /// independent here so self-collision works without rigin-body coupling. Must be > 0. + pub radius: Real, + /// XPBD compliance of the repulsion constraint. `0` = perfectly hard (rigid + /// non-penetration); larger values allow softer, springier contact. Must be ≥ 0. + pub stiffness: Real, + /// Phase 20: contact friction coefficient for the tangential relative slip at a + /// soft-soft contact (self-collision and, via the shared struct, cross-collision). + /// `None` = frictionless (default). When set, the tangential relative velocity of a + /// contacting pair is damped by `μ` each step (Coulomb-style, bounded to `[0,1]`: + /// `μ = 0` no friction, `μ = 1` fully kills tangential slip). Must be `0 ≤ μ ≤ 1`. + pub friction: Option, +} + +/// Phase 17: cohesion (adhesion / breakable glue) parameters for inter-body +/// (soft-soft) contact — the dual of Phase 9 tearing. Two *free* particles from +/// *different* bodies whose centres come within `radius` attract toward contact +/// (rest distance `radius`), bonding the bodies together like glue. The bond is +/// *breakable*: if the pair separation ever exceeds `break_distance` (which is +/// usually `> radius`, giving a hysteresis so bonded pairs need to be pulled apart +/// to break), the attraction is released for that pair for the rest of the step — +/// i.e. the glue tears. Each step is stateless: bonds are re-evaluated from the +/// current geometry, so it composes naturally with Phase 14 cross-collision. +#[derive(Clone, Copy, Debug)] +pub struct CohesionParams { + /// Capture radius: a free particle from another body within this distance is + /// attracted and bonded. Should match `SoftBody::particle_radius` conceptually. + /// Must be > 0. + pub radius: Real, + /// XPBD compliance of the attraction constraint. `0` = hard glue (bonded pairs + /// snap to exactly `radius` apart); larger values give springier, stretchier + /// glue. Must be ≥ 0. + pub stiffness: Real, + /// Break distance: once a bonded pair is pulled apart beyond this separation the + /// attraction releases (the glue tears). Must be `> radius`. `inf` disables + /// breaking (permanent glue). + pub break_distance: Real, +} + +impl SoftBody { + /// Creates an empty soft body in a gravity field `gravity`. + pub fn new(gravity: Vector) -> Self { + Self { + particles: Vec::new(), + springs: Vec::new(), + distance_constraints: Vec::new(), + tetrahedra: Vec::new(), + tetra_rest_volumes: Vec::new(), + triangles: Vec::new(), + solver: SoftSolver::MassSpring, + gravity, + sleeping: false, + collide: false, + particle_radius: 0.1, + wind: None, + tear: None, + plasticity: None, + pressure: None, + anisotropy: None, + viscoelastic: None, + temperature: None, + damping: 0.0, + self_collision: None, + cross_collision: None, + volume_conservation: None, + corotated: None, + neo_hookean: None, + tetra_rest_shapes: Vec::new(), + cohesion: None, + substeps: 1, + } + } + + /// Adds a free particle and returns its index. + pub fn add_particle(&mut self, pos: Vector) -> usize { + let idx = self.particles.len(); + self.particles.push(SoftParticle::new(pos)); + idx + } + + /// Adds a pinned (immovable) particle and returns its index. + pub fn add_pinned(&mut self, pos: Vector) -> usize { + let idx = self.particles.len(); + self.particles.push(SoftParticle::pinned(pos)); + idx + } + + /// Phase 8: anchors `particle` to a rigid body `body`, so it rigidly follows + /// that body's motion. `world_attach_point` is the world-space point where the + /// particle binds (usually the particle's current position). The point is + /// stored in the body's *local* frame so the particle tracks translation and + /// rotation. The particle stops integrating locally; its spring/damping force + /// is instead routed to the body via [`SoftBodySet::write_spring_forces`]. + /// + /// Returns `false` if `particle` is out of range or `body` is not in `bodies`. + pub fn attach_particle( + &mut self, + particle: usize, + body: RigidBodyHandle, + world_attach_point: Vector, + bodies: &RigidBodySet, + ) -> bool { + let Some(rb) = bodies.get(body) else { + return false; + }; + let local = rb.position().inverse_transform_point(world_attach_point); + let Some(p) = self.particles.get_mut(particle) else { + return false; + }; + p.bound_body = Some(body); + p.bound_local = local; + true + } + + /// Phase 8: detaches `particle` from any bound rigid body (returns it to a + /// free, locally-integrated particle). No-op if already free. + pub fn detach_particle(&mut self, particle: usize) -> bool { + let Some(p) = self.particles.get_mut(particle) else { + return false; + }; + p.bound_body = None; + p.bound_local = Vector::ZERO; + true + } + + /// Phase 7: enables a uniform wind / air-resistance field for this body. + /// `accel` is a constant wind acceleration applied to every free particle + /// (like a sideways gravity); `drag` is a linear air-resistance coefficient + /// (`F_drag = −m·drag·v`). See [`Wind`]. Pass `accel = ZERO, drag = 0` to + /// get the same effect as [`Self::clear_wind`]. + pub fn apply_wind(&mut self, accel: Vector, drag: Real) { + self.wind = Some(Wind { accel, drag }); + } + + /// Phase 7: disables the wind field (`None`). + pub fn clear_wind(&mut self) { + self.wind = None; + } + + /// Phase 11: enables a uniform internal pressure `P` (force/area). When `P > 0`, + /// every free particle of a closed triangular mesh is pushed outward along the + /// surface normal with force `F = P · area` per incident triangle (the balloon / + /// gas-law model). Pass `P <= 0` (or [`Self::clear_pressure`]) to disable. + pub fn set_pressure(&mut self, pressure: Real) { + if pressure > 0.0 { + self.pressure = Some(pressure); + } else { + self.pressure = None; + } + } + + /// Phase 11: disables internal pressure (`None`). + pub fn clear_pressure(&mut self) { + self.pressure = None; + } + + /// Phase 11: per-particle outward pressure forces, `F_i = Σ_t P · area(t) · n̂(t)` + /// over triangles incident to `i`. The normal is the *centroid-oriented* outward + /// direction: each triangle contributes equally to its three vertices using the + /// face normal that points away from the mesh centroid. This keeps a closed mesh + /// inflating symmetrically (no net single-sided bias) and lets an open sheet + /// bulge along its normals. Pure topology read — no solver state touched. + fn pressure_forces(&self) -> Vec { + let p = match self.pressure { + Some(p) => p, + None => { + return { + let mut v = Vec::with_capacity(self.particles.len()); + for _ in 0..self.particles.len() { + v.push(Vector::ZERO); + } + v + }; + } + }; + // Mesh centroid for outward orientation. + let centroid = if self.particles.is_empty() { + Vector::ZERO + } else { + let mut c = Vector::ZERO; + for pt in &self.particles { + c += pt.pos; + } + c / (self.particles.len() as Real) + }; + let mut forces = Vec::with_capacity(self.particles.len()); + for _ in 0..self.particles.len() { + forces.push(Vector::ZERO); + } + for tri in &self.triangles { + let (ia, ib, ic) = (tri[0] as usize, tri[1] as usize, tri[2] as usize); + let (pa, pb, pc) = match ( + self.particles.get(ia), + self.particles.get(ib), + self.particles.get(ic), + ) { + (Some(a), Some(b), Some(c)) => (a.pos, b.pos, c.pos), + _ => continue, + }; + // Face normal (not normalized) = (b−a) × (c−a); magnitude = 2·area. + let n = (pb - pa).cross(pc - pa); + let area = n.length() * 0.5; + if area <= 0.0 { + continue; + } + // Orient outward relative to centroid; n̂ is the unit face normal. + let n_hat = n.normalize(); + let tri_center = (pa + pb + pc) / 3.0; + let outward = if (tri_center - centroid).dot(n_hat) >= 0.0 { + n_hat + } else { + -n_hat + }; + // Force magnitude per triangle: P · area, split equally to 3 vertices. + let f = outward * (p * area / 3.0); + if let Some(fa) = forces.get_mut(ia) { + *fa += f; + } + if let Some(fb) = forces.get_mut(ib) { + *fb += f; + } + if let Some(fc) = forces.get_mut(ic) { + *fc += f; + } + } + forces + } + + /// Phase 12: enables self-collision with the given `radius` (particle sphere + /// radius) and `stiffness` (XPBD compliance of the repulsion constraint, `0` + /// = hard). Pairs of free particles closer than `2·radius` are pushed apart + /// every solver iteration. Rejects non-positive `radius` or negative + /// `stiffness` (returns `false` without enabling). + pub fn set_self_collision(&mut self, radius: Real, stiffness: Real) -> bool { + if !(radius > 0.0) || stiffness < 0.0 { + return false; + } + self.self_collision = Some(SelfCollisionParams { + radius, + stiffness, + friction: None, + }); + true + } + + /// Phase 20: sets the contact friction coefficient `μ` for self-collision. + /// Requires `self_collision` to be enabled first. Rejects non-finite or out-of-range + /// `μ` (`0 ≤ μ ≤ 1`). `clear_self_collision` resets it to `None` (frictionless). + pub fn set_self_collision_friction(&mut self, mu: Real) -> bool { + if !mu.is_finite() || mu < 0.0 || mu > 1.0 { + return false; + } + let Some(p) = self.self_collision.as_mut() else { + return false; + }; + p.friction = Some(mu); + true + } + + /// Phase 12: disables self-collision (`None`). + pub fn clear_self_collision(&mut self) { + self.self_collision = None; + } + + /// Phase 14: enables soft-soft (cross-body) collision with the given `radius` + /// (particle sphere radius) and `stiffness` (XPBD compliance of the repulsion + /// constraint, `0` = hard). Two bodies only collide if *both* have + /// `cross_collision` set; the effective repulsion distance is + /// `2·min(radius_a, radius_b)`. Rejects non-positive `radius` or negative + /// `stiffness`. + pub fn set_cross_collision(&mut self, radius: Real, stiffness: Real) -> bool { + if !(radius > 0.0) || stiffness < 0.0 { + return false; + } + self.cross_collision = Some(SelfCollisionParams { + radius, + stiffness, + friction: None, + }); + true + } + + /// Phase 20: sets the contact friction coefficient `μ` for cross-collision. + /// Requires `cross_collision` to be enabled first. Rejects non-finite or out-of-range + /// `μ` (`0 ≤ μ ≤ 1`). `clear_cross_collision` resets it to `None` (frictionless). + pub fn set_cross_collision_friction(&mut self, mu: Real) -> bool { + if !mu.is_finite() || mu < 0.0 || mu > 1.0 { + return false; + } + let Some(p) = self.cross_collision.as_mut() else { + return false; + }; + p.friction = Some(mu); + true + } + + /// Phase 14: disables soft-soft (cross-body) collision (`None`). + pub fn clear_cross_collision(&mut self) { + self.cross_collision = None; + } + + /// Phase 16: enables the dedicated volume-conservation constraint with the given + /// `compliance` (`c`). Each tetra volume constraint in `step_xpbd` is then solved + /// with `α̃ = c / dt²`, independent of the distance solver's compliance. `c == 0` + /// gives a hard (incompressible) blob. Returns `false` (and does nothing) on a + /// non-finite or negative `compliance`. + pub fn set_volume_conservation(&mut self, compliance: Real) -> bool { + if !compliance.is_finite() || compliance < 0.0 { + return false; + } + self.volume_conservation = Some(compliance); + true + } + + /// Phase 16: disables the dedicated volume-conservation constraint (`None`), + /// reverting tetra volume to the global solver compliance. + pub fn clear_volume_conservation(&mut self) { + self.volume_conservation = None; + } + /// Phase 29: enables corotated linear elasticity with the given per-iteration + /// relaxation `stiffness` (`(0, 1]`). The rest shape of every *current* + /// tetrahedron is snapshotted into `tetra_rest_shapes` from the particle + /// positions at call time (enable on the undeformed mesh). Degenerate + /// tetrahedra are skipped (their rest shape stays the zero matrix). Returns + /// `false` for non-finite / out-of-range `stiffness` or an empty body. + pub fn set_corotated(&mut self, stiffness: Real) -> bool { + if !stiffness.is_finite() || stiffness <= 0.0 || stiffness > 1.0 { + return false; + } + let mut shapes = Vec::with_capacity(self.tetrahedra.len()); + for tet in &self.tetrahedra { + let [a, b, c, d] = *tet; + let m = rest_shape_matrix( + self.particles[a as usize].pos, + self.particles[b as usize].pos, + self.particles[c as usize].pos, + self.particles[d as usize].pos, + ); + shapes.push(m); + } + self.tetra_rest_shapes = shapes; + self.corotated = Some(stiffness); + true + } + + /// Phase 29: disables corotated elasticity (`None`). + pub fn clear_corotated(&mut self) { + self.corotated = None; + } + /// Phase 30: enables the Neo-Hookean logarithmic volume energy with the + /// given volumetric `stiffness` (compliance = `stiffness/dt²`; larger = + /// closer to incompressible). Returns `false` for non-finite values or + /// `stiffness < 0`. + pub fn set_neo_hookean(&mut self, stiffness: Real) -> bool { + if !stiffness.is_finite() || stiffness < 0.0 { + return false; + } + self.neo_hookean = Some(stiffness); + true + } + + /// Phase 30: disables the Neo-Hookean volume energy (back to linear). + pub fn clear_neo_hookean(&mut self) { + self.neo_hookean = None; + } + + /// Phase 17: enables cohesion (adhesion / breakable glue) toward other bodies with + /// the given `radius`, `stiffness` (compliance of the attraction constraint) and + /// `break_distance` (separation at which a bond tears). Returns `false` (and does + /// nothing) if `radius <= 0`, `stiffness < 0`, `break_distance <= radius`, or + /// `break_distance`/`radius`/`stiffness` is `NaN`. `break_distance == +inf` is + /// explicitly allowed (permanent, unbreakable glue). + pub fn set_cohesion(&mut self, radius: Real, stiffness: Real, break_distance: Real) -> bool { + if !radius.is_finite() + || !stiffness.is_finite() + || break_distance.is_nan() + || !(radius > 0.0) + || stiffness < 0.0 + || break_distance <= radius + { + return false; + } + self.cohesion = Some(CohesionParams { + radius, + stiffness, + break_distance, + }); + true + } + + /// Phase 17: disables cohesion (`None`). + pub fn clear_cohesion(&mut self) { + self.cohesion = None; + } + + /// Phase 18: sets the global internal (structural) damping coefficient `d`. + /// Each step every free particle's velocity is scaled by `1 − d`. `0` = no + /// damping; values in `[0, 1)` settle oscillation faster; `d >= 1` would fully + /// freeze motion and is rejected (returns `false`). Non-finite `d` is rejected. + pub fn set_damping(&mut self, d: Real) -> bool { + if !d.is_finite() || d < 0.0 || d >= 1.0 { + return false; + } + self.damping = d; + true + } + + /// Phase 24: set the number of solver substeps per [`SoftBody::step`] call. + /// `n >= 1` splits the frame `dt` into `n` equal slices, projecting constraints + /// at a finer time resolution. A value of `0` is rejected (kept at the previous + /// setting) so a body never silently degrades to a no-op step. See the + /// `substeps` field for the stability/convergence rationale. + pub fn set_substeps(&mut self, n: u32) -> bool { + if n == 0 { + return false; + } + self.substeps = n; + true + } + + /// Phase 27: enables/disables the viscoelastic (strain-rate hardening) model. + /// `None` = purely elastic. `rate_coefficient >= 0`; negative values are + /// rejected (returns `false`). + pub fn set_viscoelastic(&mut self, params: Option) -> bool { + match params { + Some(p) if p.rate_coefficient >= 0.0 => { + self.viscoelastic = Some(p); + true + } + Some(_) => false, + None => { + self.viscoelastic = None; + true + } + } + } + + /// Phase 27: enables/disables the uniform thermal field. `None` = isothermal. + /// All four components must be finite; `temp`/`ambient`/`expansion` unconstrained + /// but `stiffness_temp_coeff·(temp − ambient)` must keep stiffness positive + /// (i.e. `stiffness_temp_coeff·|ΔT| < 1`). Invalid input returns `false`. + pub fn set_thermal(&mut self, params: Option) -> bool { + match params { + Some(p) => { + let delta_t = p.temp - p.ambient; + if !p.temp.is_finite() + || !p.ambient.is_finite() + || !p.expansion.is_finite() + || !p.stiffness_temp_coeff.is_finite() + || p.stiffness_temp_coeff * delta_t.abs() >= 1.0 + { + return false; + } + self.temperature = Some(p); + true + } + None => { + self.temperature = None; + true + } + } + } + + /// Phase 12: broad-phase + projection for self-collision. Builds a uniform + /// spatial hash (cell size `2·radius`) over the *free* particle positions, + /// finds all pairs within `2·radius` that are NOT direct structural neighbours + /// (linked by an existing distance constraint), and projects each apart as a + /// stiff XPBD distance constraint (`rest = 2·radius`, `compliance = stiffness`). + /// + /// `alpha` is the XPBD `α̃ = compliance / dt²` already used by the caller. The + /// caller decides how many times to invoke this (once per iteration in XPBD, + /// a few times after integration in MassSpring). Pure positional projection: + /// `self.particles` is mutated directly; no new solver mechanics. + fn damp_contact_velocity_split( + pa: &mut [SoftParticle], + pb: &mut [SoftParticle], + i: usize, + j: usize, + mu: Real, + ) { + let wi = pa[i].inv_mass; + let wj = pb[j].inv_mass; + let wsum = wi + wj; + if wsum == 0.0 { + return; + } + let delta = pa[i].pos - pb[j].pos; + let len = delta.length(); + if len < 1e-12 { + return; + } + let n = delta / len; + let v_rel = pa[i].vel - pb[j].vel; + let vn = v_rel.dot(n); + let v_t = v_rel - n * vn; + let corr = v_t * mu; + pa[i].vel -= corr * (wi / wsum); + pb[j].vel += corr * (wj / wsum); + } + + fn damp_contact_velocity(ps: &mut [SoftParticle], i: usize, j: usize, mu: Real) { + let wi = ps[i].inv_mass; + let wj = ps[j].inv_mass; + let wsum = wi + wj; + if wsum == 0.0 { + return; + } + let delta = ps[i].pos - ps[j].pos; + let len = delta.length(); + if len < 1e-12 { + return; + } + let n = delta / len; // contact normal (b → a) + let v_rel = ps[i].vel - ps[j].vel; + let vn = v_rel.dot(n); + let v_t = v_rel - n * vn; // tangential relative velocity + // Apply -μ·v_t distributed by inverse mass (like a velocity constraint). + let corr = v_t * mu; + ps[i].vel -= corr * (wi / wsum); + ps[j].vel += corr * (wj / wsum); + } + + fn solve_self_collisions(&mut self, alpha: Real) -> Vec<(usize, usize)> { + let params = match self.self_collision { + Some(p) => p, + None => return Vec::new(), + }; + let d = params.radius * 2.0; + // Inverse-mass view aligned to particle order (read by the projection helper). + let w: Vec = self.particles.iter().map(|p| p.inv_mass).collect(); + // Structural neighbour set: (min,max) of every distance-constraint edge. + let mut neighbours: HashSet<(usize, usize)> = HashSet::new(); + for c in &self.distance_constraints { + let (a, b) = (c.a, c.b); + let key = if a <= b { (a, b) } else { (b, a) }; + neighbours.insert(key); + } + // Build spatial hash of free-particle indices. + let cell = d; + let mut grid: HashMap<(i64, i64, i64), Vec> = HashMap::new(); + let mut free: Vec = Vec::new(); + for (i, p) in self.particles.iter().enumerate() { + if p.inv_mass == 0.0 { + continue; // pinned / bound particles don't self-collide + } + free.push(i); + let key = ( + (p.pos.x / cell).floor() as i64, + (p.pos.y / cell).floor() as i64, + (p.pos.z / cell).floor() as i64, + ); + grid.entry(key).or_default().push(i); + } + // For each free particle, test against its cell + 26 neighbours. + let mut contacts: Vec<(usize, usize)> = Vec::new(); + for &i in &free { + let pi = self.particles[i].pos; + let ci = ( + (pi.x / cell).floor() as i64, + (pi.y / cell).floor() as i64, + (pi.z / cell).floor() as i64, + ); + for gx in ci.0 - 1..=ci.0 + 1 { + for gy in ci.1 - 1..=ci.1 + 1 { + for gz in ci.2 - 1..=ci.2 + 1 { + if let Some(bucket) = grid.get(&(gx, gy, gz)) { + for &j in bucket { + if j <= i { + continue; // each unordered pair once + } + let key = if i <= j { (i, j) } else { (j, i) }; + if neighbours.contains(&key) { + continue; // structural link, not a collision + } + // Project apart using the shared XPBD primitive. + solve_distance_constraint( + &mut self.particles, + i, + j, + d, + alpha, + &w, + &mut 0.0, + ); + contacts.push((i, j)); + } + } + } + } + } + } + contacts + } + + /// Adds a spring between particles `a` and `b` with the given stiffness/damping. + /// The rest length is taken from the current distance between the endpoints. + /// Returns `None` (and does nothing) if either index is out of bounds or the + /// endpoints coincide (zero rest length is rejected to avoid a degenerate axis). + pub fn add_spring( + &mut self, + a: usize, + b: usize, + stiffness: Real, + damping: Real, + ) -> Option { + let (pa, pb) = (self.particles.get(a)?, self.particles.get(b)?); + let rest = (pb.pos - pa.pos).length(); + if rest == 0.0 { + return None; + } + let idx = self.springs.len(); + self.springs.push(Spring { + a, + b, + rest_length: rest, + stiffness, + damping, + activation: 0.0, + fibre: None, + }); + Some(idx) + } + + /// Computes the per-particle internal force from all springs (Hookean + + /// axial damping), *without* gravity. Returns one `Vector` per particle. + /// Pinned particles (`inv_mass == 0`) receive no force (they act as anchors). + /// + /// This is the force that Phase 2 routes into `force_containers` for bound + /// particles; `compute_forces` reuses it and adds gravity for free particles. + pub fn spring_damping_forces(&self) -> Vec { + let mut out = Vec::with_capacity(self.particles.len()); + for _ in 0..self.particles.len() { + out.push(Vector::ZERO); + } + for s in &self.springs { + let (pa_pos, pb_pos, pa_vel, pb_vel, pa_im, pb_im) = + match (self.particles.get(s.a), self.particles.get(s.b)) { + (Some(a), Some(b)) => (a.pos, b.pos, a.vel, b.vel, a.inv_mass, b.inv_mass), + _ => continue, + }; + let delta = pb_pos - pa_pos; + let len = delta.length(); + if len == 0.0 { + continue; + } + let dir = delta / len; + // Phase 27 B5: thermal modulation of rest length + stiffness. + let (rest_eff, mut k_eff) = if let Some(th) = self.temperature { + let delta_t = th.temp - th.ambient; + ( + s.rest_length * (1.0 + th.expansion * delta_t), + s.stiffness * (1.0 - th.stiffness_temp_coeff * delta_t), + ) + } else { + (s.rest_length, s.stiffness) + }; + // Phase 27 B4: viscoelastic strain-rate hardening. + if let Some(ve) = self.viscoelastic { + let strain_rate = (pb_vel - pa_vel).dot(dir) / len.max(1e-9); + k_eff *= 1.0 + ve.rate_coefficient * strain_rate.abs(); + } + // Phase 31: active-strain contraction — shrink the effective rest length + // toward zero as activation rises, pulling the endpoints together. + let rest_eff = rest_eff * (1.0 - s.activation.clamp(0.0, 1.0)); + let f_spring = k_eff * (len - rest_eff); + let rel_vel = pb_vel - pa_vel; + let f_damp = s.damping * rel_vel.dot(dir); + let f_axial = f_spring + f_damp; + // Phase 32: the active-strain drive follows the muscle-fibre direction + // when one is set, otherwise the spring edge (Phase 31 default). + let dir_active = match s.fibre { + Some(fd) => fd, + None => dir, + }; + let f = dir_active * f_axial; + if pa_im != 0.0 { + out[s.a] += f; + } + if pb_im != 0.0 { + out[s.b] -= f; + } + } + out + } + + /// Accumulates the total force on every particle: gravity plus all spring + /// (Hookean + axial damping) contributions. Clears each particle's `force` + /// first, so this can be called once per substep before `integrate`. + /// + /// Bound particles (those with `bound_body` set) do **not** accumulate a local + /// force here — their spring force is instead routed to the rigid body via + /// [`SoftBodySet::write_spring_forces`](crate::dynamics::SoftBodySet::write_spring_forces), + /// so the rigid body's own integrator applies it through `force_containers`. + pub fn compute_forces(&mut self) { + let spring = self.spring_damping_forces(); + // Phase 11: internal pressure (balloon model) — computed once, added per particle. + let pressure = self.pressure_forces(); + for (i, p) in self.particles.iter_mut().enumerate() { + if p.bound_body.is_some() { + // Driven externally via force_containers; no local integration. + p.force = Vector::ZERO; + continue; + } + p.force = if p.inv_mass == 0.0 { + Vector::ZERO + } else { + p.mass() * self.gravity + }; + // Phase 7: uniformly applied wind / air-resistance (pure external force). + if let Some(wind) = self.wind { + p.force += p.mass() * wind.accel; + p.force -= p.mass() * wind.drag * p.vel; + } + // Phase 11: internal pressure pushes free particles outward along the + // surface normal (balloon model). + p.force += pressure[i]; + p.force += spring[i]; + } + } + + /// Advances velocities then positions by `dt` (semi-implicit Euler). + /// Pinned particles (`inv_mass == 0`) are not moved. Phase 18: a global internal + /// damping factor `1 − self.damping` is applied to each free particle's velocity. + pub fn integrate(&mut self, dt: Real) { + let keep = 1.0 - self.damping; + for p in &mut self.particles { + if p.inv_mass == 0.0 { + continue; + } + // v += dt · M⁻¹ · f (M⁻¹ = inv_mass for a point mass) + p.vel += dir_scaled(p.force, dt * p.inv_mass); + // Phase 18: global internal damping (skipped when damping == 0). + if keep < 1.0 { + p.vel *= keep; + } + // x += dt · v + p.pos += dir_scaled(p.vel, dt); + } + } + + /// One substep: integrate according to the active [`SoftSolver`]. + /// + /// * `MassSpring` → [`Self::step_mass_spring`] (Hookean forces, semi-implicit Euler). + /// * `Xpbd` → [`Self::step_xpbd`] (distance + volume constraints, position-based). + pub fn step(&mut self, dt: Real) { + if self.sleeping { + return; + } + // Phase 9: remove over-stretched structural edges before integrating. + // (No-op unless `tear_strain` is `Some`.) + self.tear(); + // Phase 10: freeze over-yield deformation into rest lengths before + // integrating, so elastic edges become permanently deformed. (No-op unless + // `plasticity` is `Some`.) + self.apply_plasticity(); + // Phase 5f: when collision coupling is on, the integration layer drives + // particle positions/velocities from proxy rigid bodies (after the + // rigid-body narrow-phase/contact step), so we must not integrate here. + if self.collide { + return; + } + // Phase 24: subdivide the frame into `substeps` equal slices and run the + // active solver once per slice. Each call to `step_xpbd` / `step_mass_spring` + // resets its own Lagrange accumulators, so looping here gives independent + // projection at a finer time resolution without touching the solver internals. + // `substeps` is clamped to ≥1 so a 0 (or unset) value reproduces the single + // substep behaviour. + let n_sub = self.substeps.max(1) as usize; + let sub_dt = dt / n_sub as Real; + for _ in 0..n_sub { + match self.solver { + SoftSolver::MassSpring => self.step_mass_spring(sub_dt), + SoftSolver::Xpbd { .. } => self.step_xpbd(sub_dt), + } + } + } + + /// Mass-spring substep (Phase 0/2): accumulate Hookean + damping forces, then + /// integrate with semi-implicit Euler. + pub fn step_mass_spring(&mut self, dt: Real) { + if self.sleeping { + return; + } + self.compute_forces(); + self.integrate(dt); + // Phase 12: self-collision as positional projection (a handful of passes). + // Reuses the same broad-phase + XPBD push-apart as the XPBD path; the + // compliance comes from `self.self_collision.stiffness`. + if self.self_collision.is_some() { + let stiffness = self.self_collision.unwrap().stiffness; + let mu = self.self_collision.unwrap().friction; + let alpha = stiffness / (dt * dt); + let mut all_contacts: Vec<(usize, usize)> = Vec::new(); + for _ in 0..4 { + all_contacts.extend(self.solve_self_collisions(alpha)); + } + // Phase 20: velocity-level friction (vel is valid post-integrate). + if let Some(mu) = mu { + for (i, j) in all_contacts { + Self::damp_contact_velocity(&mut self.particles, i, j, mu); + } + } + } + } + + /// Total kinetic energy of the free particles (`½ · m · |v|²`). + pub fn kinetic_energy(&self) -> Real { + self.particles + .iter() + .filter(|p| p.inv_mass != 0.0) + .map(|p| 0.5 * p.mass() * p.vel.dot(p.vel)) + .fold(0.0, |acc, e| acc + e) + } + + /// Phase 27 (B8): **implicit (backward-Euler) reference integrator** for the + /// mass-spring system — a comparison path against [`Self::step_mass_spring`] + /// (explicit semi-implicit Euler). Unlike the explicit solver, backward Euler is + /// *unconditionally stable*: it solves the linear system + /// `(M/dt² − K) · vₙ₊₁ = M·vₙ/dt² + fₙ/dt` for the new velocities, where `M` is the + /// (diagonal) mass matrix and `K = −∂f/∂x` is the symmetric spring stiffness matrix. + /// A stiff spring that makes the explicit solver explode (energy growth / NaN) stays + /// bounded here, which is exactly what a comparison harness wants to demonstrate. + /// + /// This is intentionally a *reference* solver (dense `nalgebra` `DMatrix`, O(n³) + /// per step) — fine for small meshes used in tests/benchmarks, not for production + /// large bodies (use XPBD). Springs only; gravity/pressure/wind are captured through + /// the assembled force `fₙ`. Pinned particles (`inv_mass == 0`) are held fixed. + pub fn step_implicit_euler(&mut self, dt: Real) { + if self.sleeping { + return; + } + // Assemble fₙ (gravity + springs + pressure + wind) into the per-particle force. + self.compute_forces(); + let n = self.particles.len(); + let dim = 3 * n; + if dim == 0 { + return; + } + // Mass matrix M (diagonal) and stiffness K (symmetric, sparse-as-dense). + let mut m_diag = Vec::with_capacity(dim); + for p in &self.particles { + let m = if p.inv_mass == 0.0 { 0.0 } else { p.mass() }; + for _ in 0..3 { + m_diag.push(m); + } + } + let mut k_mat = nalgebra::DMatrix::::zeros(dim, dim); + for s in &self.springs { + let (pa, pb) = match (self.particles.get(s.a), self.particles.get(s.b)) { + (Some(a), Some(b)) => (a, b), + _ => continue, + }; + let delta = pb.pos - pa.pos; + let len = delta.length(); + if len == 0.0 { + continue; + } + let d = delta / len; // unit direction + // Thermal/viscoelastic modulation (mirrors spring_damping_forces). + let (rest_eff, mut k_eff) = if let Some(th) = self.temperature { + let dt_t = th.temp - th.ambient; + ( + s.rest_length * (1.0 + th.expansion * dt_t), + s.stiffness * (1.0 - th.stiffness_temp_coeff * dt_t), + ) + } else { + (s.rest_length, s.stiffness) + }; + if let Some(ve) = self.viscoelastic { + let strain_rate = (pb.vel - pa.vel).dot(d) / len.max(1e-9); + k_eff *= 1.0 + ve.rate_coefficient * strain_rate.abs(); + } + let _ = rest_eff; + // 3x3 block B = k_eff·(I − d⊗d). + let mut b = [[0.0; 3]; 3]; + for r in 0..3 { + for c in 0..3 { + let outer = d[r] * d[c]; + b[r][c] = k_eff * if r == c { 1.0 - outer } else { -outer }; + } + } + let ia = 3 * s.a; + let ib = 3 * s.b; + for r in 0..3 { + for c in 0..3 { + k_mat[(ia + r, ia + c)] += b[r][c]; + k_mat[(ib + r, ib + c)] += b[r][c]; + k_mat[(ia + r, ib + c)] -= b[r][c]; + k_mat[(ib + r, ia + c)] -= b[r][c]; + } + } + } + // A = M/dt² − K, rhs = M·vₙ/dt² + fₙ/dt. + let inv_dt2 = 1.0 / (dt * dt); + let mut a_mat = nalgebra::DMatrix::::zeros(dim, dim); + let mut rhs = nalgebra::DVector::::zeros(dim); + for i in 0..n { + let p = &self.particles[i]; + for c in 0..3 { + let row = 3 * i + c; + a_mat[(row, row)] += m_diag[row] * inv_dt2; + rhs[row] = m_diag[row] * p.vel[c] * inv_dt2 + p.force[c] / dt; + } + } + a_mat -= &k_mat; + // Solve A · v = rhs (Cholesky if SPD, else LU fallback). + let v_new = if let Some(chol) = nalgebra::Cholesky::new(a_mat.clone()) { + chol.solve(&rhs) + } else { + // A may be indefinite (e.g. all-pinned or degenerate); fall back to LU. + match a_mat.clone().lu().solve(&rhs) { + Some(v) => v, + None => rhs, // singular system: leave velocities unchanged + } + }; + // Commit velocities, then advance positions x += dt·v. + for i in 0..n { + let p = &mut self.particles[i]; + if p.inv_mass == 0.0 { + continue; + } + p.vel = Vector::new(v_new[3 * i], v_new[3 * i + 1], v_new[3 * i + 2]); + p.pos += Vector::new(p.vel.x * dt, p.vel.y * dt, p.vel.z * dt); + } + } + + // ── Phase 3: XPBD setup ──────────────────────────────────────────────── + + /// Switches this body to the XPBD solver with the given iteration count and + /// default compliance. Existing springs are ignored in XPBD mode; add distance + /// constraints and tetrahedra instead. + pub fn configure_xpbd(&mut self, iterations: u32, compliance: Real) { + self.solver = SoftSolver::Xpbd { + iterations, + compliance, + }; + } + + /// Adds a distance constraint between particles `a` and `b`. The rest length + /// is taken from the current distance (0 is rejected to avoid a degenerate + /// axis). Returns `None` if indices are out of bounds or coincide. + pub fn add_distance_constraint( + &mut self, + a: usize, + b: usize, + compliance: Real, + ) -> Option { + let (pa, pb) = (self.particles.get(a)?, self.particles.get(b)?); + let rest = (pb.pos - pa.pos).length(); + if rest == 0.0 { + return None; + } + let idx = self.distance_constraints.len(); + self.distance_constraints.push(DistanceConstraint { + a, + b, + rest, + compliance, + compression: compliance, + activation: 0.0, + fibre: None, + }); + Some(idx) + } + + /// Phase 13 — sets the `stiffness` (Hookean `k`) of an existing spring (by the + /// index returned from `add_spring`) at runtime. Lets callers tune a body's + /// material heterogeneity after construction (e.g. stiffen a "bone", loosen a + /// "tendon") without rebuilding the topology. Returns `false` for an out-of-range + /// index or a negative/non-finite stiffness. + pub fn set_spring_stiffness(&mut self, index: usize, stiffness: Real) -> bool { + if stiffness < 0.0 || !stiffness.is_finite() { + return false; + } + match self.springs.get_mut(index) { + Some(s) => { + s.stiffness = stiffness; + true + } + None => false, + } + } + + /// Phase 13 — sets the XPBD `compliance` (α) of an existing distance constraint + /// (by the index returned from `add_distance_constraint`) at runtime. Per-constraint + /// compliance is honored by the XPBD solver (see `step_xpbd`). Phase 19: this sets + /// **both** the stretch and compression compliance to the same value, i.e. it keeps + /// the edge isotropic. Use `set_distance_constraint_compression` to make it + /// anisotropic (different stretch vs compression softness). Returns `false` for + /// an out-of-range index or a negative/non-finite compliance. + pub fn set_distance_constraint_compliance(&mut self, index: usize, compliance: Real) -> bool { + if compliance < 0.0 || !compliance.is_finite() { + return false; + } + match self.distance_constraints.get_mut(index) { + Some(c) => { + c.compliance = compliance; + c.compression = compliance; + true + } + None => false, + } + } + + /// Phase 19 — sets the XPBD **compression** compliance `α_c` of an existing + /// distance constraint (by the index returned from `add_distance_constraint`) + /// at runtime, independently of its stretch compliance. This is the anisotropic + /// knob: a cloth edge can resist stretch (`compliance`, small) but fold/compress + /// easily (`compression`, large). The solver selects the compliance by the + /// current strain sign each iteration (see `step_xpbd`). Returns `false` for an + /// out-of-range index or a negative/non-finite compliance. + pub fn set_distance_constraint_compression(&mut self, index: usize, compression: Real) -> bool { + if compression < 0.0 || !compression.is_finite() { + return false; + } + match self.distance_constraints.get_mut(index) { + Some(c) => { + c.compression = compression; + true + } + None => false, + } + } + + /// Adds a tetrahedral volume element `[a, b, c, d]`. The rest (reference) + /// signed volume is computed from the current particle positions and cached. + /// Returns `None` if any index is out of bounds or duplicated. + pub fn add_tetrahedron(&mut self, tet: [u32; 4]) -> Option { + let [a, b, c, d] = tet; + let (pa, pb, pc, pd) = ( + self.particles.get(a as usize)?, + self.particles.get(b as usize)?, + self.particles.get(c as usize)?, + self.particles.get(d as usize)?, + ); + // Reject degenerate (duplicate) indices. + if a == b || a == c || a == d || b == c || b == d || c == d { + return None; + } + let vol = signed_tetra_volume(pa.pos, pb.pos, pc.pos, pd.pos); + let idx = self.tetrahedra.len(); + self.tetrahedra.push(tet); + self.tetra_rest_volumes.push(vol); + Some(idx) + } + + /// Phase 21 — adaptive tetrahedral subdivision (1 → 4 barycentric split). + /// + /// Each source tetrahedron `[a,b,c,d]` gains one new particle at its centroid + /// (position = vertex mean; `inv_mass` = mean of the four endpoints, so a centroid + /// bounded by pinned vertices stays effectively pinned) and is replaced by four + /// sub-tetrahedra sharing that centroid: `(m,a,b,c)`, `(m,a,b,d)`, `(m,a,c,d)`, + /// `(m,b,c,d)`. The four sub-volumes sum exactly to the parent volume, so the + /// XPBD volume-conservation constraint (Phase 16) stays consistent — the centroid + /// particle is a vertex of every sub-tet, so it is driven by those volume + /// constraints directly (no extra distance edges are added, which would + /// over-constrain and destabilise the solve). The shell topology (`triangles`) + /// is left untouched — this is a volumetric refinement. + /// + /// *Adaptive*: a source tet is only split when its longest edge exceeds + /// `max_edge_len`. Pass `max_edge_len = +∞` (the default when `!max_edge_len.is_finite()`) + /// to subdivide every tet unconditionally. Returns the number of source tetrahedra + /// actually split (0 if none qualified, e.g. all edges already short enough). + /// + /// Pure topology edit — no solver state, no SoA interaction. Determinism: source + /// tets are processed in index order, so subdivision is reproducible. + pub fn subdivide_tetrahedra(&mut self, max_edge_len: Real) -> usize { + let src_tets: Vec<[u32; 4]> = self.tetrahedra.clone(); + let src_rests: Vec = self.tetra_rest_volumes.clone(); + if src_tets.is_empty() { + return 0; + } + // Adaptive filter: only split tets whose longest edge exceeds the threshold. + let adaptive = max_edge_len.is_finite(); + let mut new_tets: Vec<[u32; 4]> = Vec::with_capacity(src_tets.len() * 4); + let mut new_rests: Vec = Vec::with_capacity(src_tets.len() * 4); + let mut split_count = 0usize; + for (ti, &tet) in src_tets.iter().enumerate() { + let [a, b, c, d] = tet; + let (pa, pb, pc, pd) = ( + self.particles[a as usize].pos, + self.particles[b as usize].pos, + self.particles[c as usize].pos, + self.particles[d as usize].pos, + ); + let longest = (pa - pb) + .length() + .max((pa - pc).length()) + .max((pa - pd).length()) + .max((pb - pc).length()) + .max((pb - pd).length()) + .max((pc - pd).length()); + if adaptive && longest <= max_edge_len { + // Keep the parent tet unchanged. + new_tets.push(tet); + new_rests.push(src_rests[ti]); + continue; + } + // Centroid particle (mean position + mean inverse mass). + let mpos = (pa + pb + pc + pd) * 0.25; + let im = (self.particles[a as usize].inv_mass + + self.particles[b as usize].inv_mass + + self.particles[c as usize].inv_mass + + self.particles[d as usize].inv_mass) + * 0.25; + let m = self.particles.len() as u32; + self.particles.push(SoftParticle { + pos: mpos, + vel: Vector::ZERO, + force: Vector::ZERO, + inv_mass: im, + bound_body: None, + bound_local: Vector::ZERO, + }); + // Four sub-tetrahedra; each sub-rest-volume = 1/4 of the parent. + let sub_rest = src_rests[ti] * 0.25; + for sub in [[m, a, b, c], [m, a, b, d], [m, a, c, d], [m, b, c, d]] { + new_tets.push(sub); + new_rests.push(sub_rest); + } + split_count += 1; + } + self.tetrahedra = new_tets; + self.tetra_rest_volumes = new_rests; + split_count + } + + /// Phase 6 — cloth: adds a triangular face `[a, b, c]` (CCW for outward + /// normal) to the body's shell topology **and** automatically registers its + /// three structural edges as distance constraints (rest length from the + /// current particle spacing) so the existing XPBD solver keeps the face + /// shape. Duplicate edges (shared by neighbouring triangles) are silently + /// de-duplicated against existing distance constraints to avoid double + /// stiffness. Returns `None` (and does nothing) if any index is out of + /// bounds or duplicated, or if the face is degenerate (a zero-length edge). + /// + /// Bending stiffness is *not* added here: it is composed by the caller via + /// [`Self::add_distance_constraint`] between opposite vertices of adjacent + /// quad pairs (cross-diagonal edges), which needs no new mechanics. + pub fn add_triangle(&mut self, tri: [u32; 3]) -> Option { + let [a, b, c] = tri; + let (pa, pb, pc) = ( + self.particles.get(a as usize)?, + self.particles.get(b as usize)?, + self.particles.get(c as usize)?, + ); + if a == b || a == c || b == c { + return None; + } + // Reject degenerate faces (any edge has zero rest length). + let ab = (pb.pos - pa.pos).length(); + let bc = (pc.pos - pb.pos).length(); + let ca = (pa.pos - pc.pos).length(); + if ab == 0.0 || bc == 0.0 || ca == 0.0 { + return None; + } + // Register structural edges (a-b, b-c, c-a) as distance constraints, + // skipping any edge already present (shared by a neighbour triangle). + for (u, v, rest) in [(a, b, ab), (b, c, bc), (c, a, ca)] { + let exists = self.distance_constraints.iter().any(|d| { + (d.a == u as usize && d.b == v as usize) || (d.a == v as usize && d.b == u as usize) + }); + if !exists { + self.distance_constraints.push(DistanceConstraint { + a: u as usize, + b: v as usize, + rest, + // Default compliance; tune via configure_xpbd / explicit later. + compliance: 0.0, + compression: 0.0, + activation: 0.0, + fibre: None, + }); + } + } + let idx = self.triangles.len(); + self.triangles.push(tri); + Some(idx) + } + + /// Phase 6 — cloth bending: adds a single bending edge between two particles + /// `p` and `q` as a distance constraint (rest length from current spacing). + /// Compose bending across a quad by calling this for its two diagonals, or + /// across a fold line by linking the un-shared vertices of two adjacent + /// triangles. Reuses the existing XPBD distance solver (no new mechanics). + /// Returns `None` (and does nothing) if indices are out of bounds or the + /// endpoints coincide. + pub fn add_bending_constraint(&mut self, p: usize, q: usize) -> Option { + let (pp, pq) = (self.particles.get(p)?, self.particles.get(q)?); + let rest = (pq.pos - pp.pos).length(); + if rest == 0.0 { + return None; + } + let idx = self.distance_constraints.len(); + self.distance_constraints.push(DistanceConstraint { + a: p, + b: q, + rest, + compliance: 0.0, + compression: 0.0, + activation: 0.0, + fibre: None, + }); + Some(idx) + } + + /// Phase 9: enables/disables tearing. `strain` is the max allowed strain + /// `(|len| − rest)/rest` before a structural edge snaps. Pass `None` to + /// disable (default). A value ≤ 0 tears immediately on any stretch. + /// Phase 9/27: enables/disables tearing by **strain** threshold. `strain` is + /// the max allowed strain `(|len| − rest)/rest` before a structural edge + /// snaps. Pass `None` to disable (default). A value ≤ 0 tears immediately on + /// any stretch. + pub fn set_tear_strain(&mut self, strain: Option) { + self.tear = strain.map(TearCriterion::Strain); + } + + /// Phase 27: enables/disables tearing by **axial stress** threshold. `stress` + /// is the max allowed `|k·(len − rest)|` before an edge snaps. Pass `None` to + /// disable. A value ≤ 0 tears immediately. + pub fn set_tear_stress(&mut self, stress: Option) { + self.tear = stress.map(TearCriterion::Stress); + } + + /// Phase 27: enables/disables tearing by **strain-energy** (fracture-toughness) + /// threshold. `energy` is the max allowed `½·k·(len − rest)²` before an edge + /// snaps. Pass `None` to disable. A value ≤ 0 tears immediately. + pub fn set_tear_energy(&mut self, energy: Option) { + self.tear = energy.map(TearCriterion::Energy); + } + + /// Phase 27: sets the body-level orthotropic stiffness axes (see + /// [`Self::anisotropy`]). `None` disables directional response. + pub fn set_anisotropy(&mut self, axes: Option) { + self.anisotropy = axes; + } + + /// Phase 31 — sets the body-wide active-strain activation `γ ∈ [0, 1]` on every + /// spring and distance constraint at once (the "muscle contraction" level). The + /// effective rest length of each edge becomes `rest * (1 - γ)`, so a positive + /// activation actively pulls endpoints together. Values are clamped to `[0, 1]`; + /// `activate(0)` is the passive baseline. Non-finite input is ignored (no-op). + pub fn set_activation(&mut self, gamma: Real) -> bool { + if !gamma.is_finite() || gamma < 0.0 || gamma > 1.0 { + return false; + } + for s in &mut self.springs { + s.activation = gamma; + } + for c in &mut self.distance_constraints { + c.activation = gamma; + } + true + } + + /// Phase 31 — sets the active-strain activation of a single spring (by the index + /// returned from `add_spring`). Out-of-range or non-finite `activation` (or one + /// outside `[0, 1]`) is rejected. Returns `false` on any invalid input. + pub fn set_spring_activation(&mut self, index: usize, activation: Real) -> bool { + if !activation.is_finite() || activation < 0.0 || activation > 1.0 { + return false; + } + match self.springs.get_mut(index) { + Some(s) => { + s.activation = activation; + true + } + None => false, + } + } + + /// Phase 31 — sets the active-strain activation of a single distance constraint + /// (by the index returned from `add_distance_constraint`). Out-of-range or + /// non-finite `activation` (or one outside `[0, 1]`) is rejected. Returns + /// `false` on any invalid input. + pub fn set_distance_constraint_activation(&mut self, index: usize, activation: Real) -> bool { + if !activation.is_finite() || activation < 0.0 || activation > 1.0 { + return false; + } + match self.distance_constraints.get_mut(index) { + Some(c) => { + c.activation = activation; + true + } + None => false, + } + } + + /// Phase 32 — sets the muscle-fibre direction of a single distance constraint + /// (by the index returned from `add_distance_constraint`). When `dir` is a + /// finite non-zero vector it is normalized and stored as the fibre orientation + /// for anisotropic active contraction; a zero vector clears the fibre (back to + /// edge-aligned contraction). Returns `false` for an unknown index. + pub fn set_fibre_direction(&mut self, index: usize, dir: Vector) -> bool { + if !dir.is_finite() { + return false; + } + match self.distance_constraints.get_mut(index) { + Some(c) => { + if dir.x == 0.0 && dir.y == 0.0 && dir.z == 0.0 { + c.fibre = None; + } else { + c.fibre = Some(dir.normalize()); + } + true + } + None => false, + } + } + + /// Phase 32 — sets the muscle-fibre direction of a single spring (by the index + /// returned from `add_spring`). See [`Self::set_fibre_direction`]; a zero vector + /// clears the fibre. Returns `false` for an unknown index. + pub fn set_spring_fibre_direction(&mut self, index: usize, dir: Vector) -> bool { + if !dir.is_finite() { + return false; + } + match self.springs.get_mut(index) { + Some(s) => { + if dir.x == 0.0 && dir.y == 0.0 && dir.z == 0.0 { + s.fibre = None; + } else { + s.fibre = Some(dir.normalize()); + } + true + } + None => false, + } + } + + /// Phase 9: removes every over-stretched structural edge. Called once at the + /// top of [`SoftBody::step`]; a no-op when `tear_strain` is `None`. + /// + /// Edge strain is `s = (|len| − rest) / rest`. An XPBD distance constraint or + /// MassSpring spring with `s > threshold` is dropped. Triangular faces that + /// lose any of their structural edges (a-b, b-c, c-a) are dropped as well, so + /// a torn cloth stops rendering the broken face. Pure topology edit — neither + /// the SoA solver nor the integration order is touched. + pub fn tear(&mut self) { + let threshold = match self.tear { + Some(TearCriterion::Strain(t)) + | Some(TearCriterion::Stress(t)) + | Some(TearCriterion::Energy(t)) => t, + None => return, + }; + if threshold <= 0.0 { + return; // 0 / negative would tear everything on the first step. + } + // Per-edge fracture scalar for the active criterion. + let metric = |rest: Real, len: Real, k: Real| -> Real { + match self.tear { + Some(TearCriterion::Strain(_)) => (len - rest) / rest.max(1e-12), + Some(TearCriterion::Stress(_)) => (k * (len - rest)).abs(), + Some(TearCriterion::Energy(_)) => 0.5 * k * (len - rest) * (len - rest), + None => 0.0, + } + }; + let spring_k = |s: &Spring| s.stiffness; + let dc_k = |c: &DistanceConstraint| 1.0 / (c.compliance + 1e-9); + + // Collect the surviving distance-constraint edges (and their indices). + let mut keep_dc: Vec<(usize, DistanceConstraint)> = Vec::new(); + for (i, c) in self.distance_constraints.iter().enumerate() { + let (pa, pb) = match (self.particles.get(c.a), self.particles.get(c.b)) { + (Some(a), Some(b)) => (a, b), + _ => continue, // dangling edge → drop. + }; + let len = (pb.pos - pa.pos).length(); + let k = dc_k(c); + if metric(c.rest, len, k) <= threshold { + keep_dc.push((i, *c)); + } + } + let broken_dc: HashSet = { + let mut s = HashSet::new(); + for i in 0..self.distance_constraints.len() { + if !keep_dc.iter().any(|(ki, _)| *ki == i) { + s.insert(i); + } + } + s + }; + self.distance_constraints = keep_dc.into_iter().map(|(_, c)| c).collect(); + + // Same for MassSpring springs. + let mut keep_sp: Vec = Vec::new(); + for s in self.springs.drain(..) { + let (pa, pb) = match (self.particles.get(s.a), self.particles.get(s.b)) { + (Some(a), Some(b)) => (a, b), + _ => continue, + }; + let len = (pb.pos - pa.pos).length(); + let k = spring_k(&s); + if metric(s.rest_length, len, k) <= threshold { + keep_sp.push(s); + } + } + self.springs = keep_sp; + + // Drop triangles that lost any structural edge. A triangle's structural + // edges are its three sides; an edge is "structural" if it survives as a + // distance constraint (the XPBD path used by cloth) before this tear. + // (MassSpring cloth is built from springs, which we also dropped above, so + // we check both: an edge that is neither a surviving distance-constraint + // nor a surviving spring has snapped.) + let has_edge = |a: u32, b: u32| -> bool { + let (lo, hi) = if a <= b { (a, b) } else { (b, a) }; + self.distance_constraints.iter().any(|c| { + let (ca, cb) = if c.a as u32 <= c.b as u32 { + (c.a as u32, c.b as u32) + } else { + (c.b as u32, c.a as u32) + }; + (ca, cb) == (lo, hi) + }) || self.springs.iter().any(|s| { + let (sa, sb) = if s.a as u32 <= s.b as u32 { + (s.a as u32, s.b as u32) + } else { + (s.b as u32, s.a as u32) + }; + (sa, sb) == (lo, hi) + }) + }; + self.triangles + .retain(|t| has_edge(t[0], t[1]) && has_edge(t[1], t[2]) && has_edge(t[2], t[0])); + let _ = broken_dc; // retained for clarity; broken edges already excluded. + } + + /// Phase 10: enables/disables plasticity. Pass `None` to disable (perfectly + /// elastic, the default). With `Some(PlasticityParams { yield_strain, creep })`, + /// edges whose elastic strain magnitude exceeds `yield_strain` permanently shift + /// their rest length toward the current length by `creep` (clamped to `[0,1]`) + /// each step — the deformation freezes in instead of springing back. + /// + /// `yield_strain` is clamped to `≥ 0`; `creep` is clamped to `[0,1]`. + pub fn set_plasticity(&mut self, params: Option) { + self.plasticity = params.map(|p| PlasticityParams { + yield_strain: p.yield_strain.max(0.0), + creep: p.creep.clamp(0.0, 1.0), + }); + } + + /// Phase 10: permanently deforms over-yielded edges. Called once at the top of + /// [`SoftBody::step`] after [`SoftBody::tear`]; a no-op when `plasticity` is + /// `None`. + /// + /// For every structural edge (XPBD distance constraint / MassSpring spring) the + /// elastic strain `s = (|len| − rest) / rest` is measured. If `|s| > yield_strain`, + /// the over-yield portion is frozen: `rest += creep · (len − rest)` (the rest + /// length moves toward the current length, so the edge no longer pulls back toward + /// its original size). Pure rest-length edit — neither the SoA solver nor the + /// integration order is touched. + pub fn apply_plasticity(&mut self) { + let (yield_strain, creep) = match self.plasticity { + Some(p) => (p.yield_strain, p.creep), + None => return, + }; + if yield_strain <= 0.0 || creep <= 0.0 { + return; // degenerate: no plasticity. + } + + // Distance constraints (XPBD path). Filtered first into a `Vec`, then the + // inner `self.particles` borrow for length measurement is released before we + // mutate `self.distance_constraints` — keeps the two borrows disjoint. + let new_rests: Vec = self + .distance_constraints + .iter() + .map(|c| { + let (pa, pb) = match (self.particles.get(c.a), self.particles.get(c.b)) { + (Some(a), Some(b)) => (a, b), + _ => return c.rest, // dangling edge → leave unchanged. + }; + let len = (pb.pos - pa.pos).length(); + if c.rest <= 0.0 { + return c.rest; + } + let strain = (len - c.rest) / c.rest; + if strain.abs() <= yield_strain { + c.rest + } else { + // Freeze part of the elastic stretch into the rest length. + c.rest + creep * (len - c.rest) + } + }) + .collect(); + for (c, nr) in self.distance_constraints.iter_mut().zip(new_rests) { + c.rest = nr; + } + + // MassSpring springs. + let new_springs: Vec = self + .springs + .iter() + .map(|s| { + let (pa, pb) = match (self.particles.get(s.a), self.particles.get(s.b)) { + (Some(a), Some(b)) => (a, b), + _ => return *s, + }; + let len = (pb.pos - pa.pos).length(); + if s.rest_length <= 0.0 { + return *s; + } + let strain = (len - s.rest_length) / s.rest_length; + if strain.abs() <= yield_strain { + *s + } else { + let mut ns = *s; + ns.rest_length += creep * (len - s.rest_length); + ns + } + }) + .collect(); + self.springs = new_springs; + } + + /// Removes the particle at `index`, keeping the topology consistent. + /// + /// Springs, distance constraints, and tetrahedra that *reference* the removed + /// particle are dropped; every remaining index `> index` is decremented by one + /// so it still points at the same particle. `tetra_rest_volumes` is filtered in + /// lockstep with `tetrahedra`. Returns `false` (and does nothing) if `index` is + /// out of bounds. + /// + /// This is the per-particle counterpart of [`SoftBodySet::remove`]: deleting a + /// block/voxel in a Minecraft chunk maps to removing the corresponding particle + /// plus its incident springs/edges, after which the body keeps simulating under + /// the new (smaller) topology. + pub fn remove_particle(&mut self, index: usize) -> bool { + if index >= self.particles.len() { + return false; + } + self.particles.remove(index); + + // Springs: drop any touching `index`, shift the rest. + self.springs.retain_mut(|s| { + if s.a == index || s.b == index { + return false; + } + if s.a > index { + s.a -= 1; + } + if s.b > index { + s.b -= 1; + } + true + }); + + // Distance constraints: same treatment. + self.distance_constraints.retain_mut(|c| { + if c.a == index || c.b == index { + return false; + } + if c.a > index { + c.a -= 1; + } + if c.b > index { + c.b -= 1; + } + true + }); + + // Tetrahedra: drop any containing `index`; otherwise shift each index down. + let keep: Vec = self + .tetrahedra + .iter() + .map(|t| !t.contains(&(index as u32))) + .collect(); + let remapped: Vec<[u32; 4]> = self + .tetrahedra + .iter() + .enumerate() + .filter(|(i, _)| keep[*i]) + .map(|(_, t)| { + let mut t = *t; + for x in t.iter_mut() { + if *x as usize > index { + *x -= 1; + } + } + t + }) + .collect(); + let kept_volumes: Vec = self + .tetra_rest_volumes + .iter() + .enumerate() + .filter(|(i, _)| keep[*i]) + .map(|(_, v)| *v) + .collect(); + self.tetrahedra = remapped; + self.tetra_rest_volumes = kept_volumes; + + // Triangles: drop any containing `index`; otherwise shift each index down. + let keep_tri: Vec = self + .triangles + .iter() + .map(|t| !t.contains(&(index as u32))) + .collect(); + let remapped_tri: Vec<[u32; 3]> = self + .triangles + .iter() + .enumerate() + .filter(|(i, _)| keep_tri[*i]) + .map(|(_, t)| { + let mut t = *t; + for x in t.iter_mut() { + if *x as usize > index { + *x -= 1; + } + } + t + }) + .collect(); + self.triangles = remapped_tri; + + true + } + + /// Total (sum of absolute) signed volume of all tetrahedra — a finite, + /// deformation-sensitive scalar useful for regression tests. + pub fn total_volume(&self) -> Real { + self.tetra_rest_volumes + .iter() + .zip(self.tetrahedra.iter()) + .map(|(rest_vol, tet)| { + let [a, b, c, d] = *tet; + let (pa, pb, pc, pd) = ( + self.particles[a as usize].pos, + self.particles[b as usize].pos, + self.particles[c as usize].pos, + self.particles[d as usize].pos, + ); + signed_tetra_volume(pa, pb, pc, pd).abs() / rest_vol.abs().max(1e-12) + }) + .fold(0.0, |acc, r| acc + r) + } + + /// Phase 3 XPBD substep (Matthias Müller "Small Steps" XPBD): + /// + /// 1. **Predict**: for each free particle, `v += dt·g`, `x_prev = x`, `x += dt·v`. + /// 2. **Project** `iterations` times (Gauss-Seidel, fixed constraint order for + /// determinism): each distance constraint then each tetra volume constraint + /// is solved, accumulating per-constraint Lagrange multipliers `λ`. + /// 3. **Update velocities**: `v = (x − x_prev) / dt`. + /// + /// Bound particles (`bound_body.is_some()`) are treated as infinite mass + /// (effective inverse mass 0) so the soft body can be anchored to rigid bodies + /// without the XPBD solve fighting `force_containers` (Phase 2). + /// + /// **Determinism**: constraints are traversed in vector order with no + /// concurrency, so two runs from identical state are bit-identical. (Compliance + /// `α̃ = α / dt²` makes stiff edges stable; `α = 0` gives a hard constraint.) + pub fn step_xpbd(&mut self, dt: Real) { + if self.sleeping { + return; + } + let iterations = match self.solver { + SoftSolver::Xpbd { iterations, .. } => iterations, + SoftSolver::MassSpring => return, // guarded by step(), defensive + }; + if iterations == 0 { + return; + } + let n = self.particles.len(); + // Effective inverse mass: 0 for pinned **and** bound (rigid-anchored) particles. + let mut w = Vec::with_capacity(n); + for p in &self.particles { + w.push(if p.bound_body.is_some() { + 0.0 + } else { + p.inv_mass + }); + } + // Previous positions (for velocity recovery). + let mut prev = Vec::with_capacity(n); + for p in &self.particles { + prev.push(p.pos); + } + + // 1. Predict. + // Phase 11: internal pressure (balloon model) as a pure external + // acceleration, applied alongside gravity/wind in the predict step. + let pressure_forces = self.pressure_forces(); + for (i, p) in self.particles.iter_mut().enumerate() { + if w[i] == 0.0 { + continue; + } + // Phase 7: wind / air-resistance is a pure external acceleration, + // applied alongside gravity in the predict step (no new mechanics). + let mut a = self.gravity; + if let Some(wind) = self.wind { + a += wind.accel; + a -= wind.drag * p.vel; + } + // Phase 11: pressure force → acceleration (a += M⁻¹ · F). + a += pressure_forces[i] * p.inv_mass; + p.vel += dir_scaled(a, dt); + p.pos += dir_scaled(p.vel, dt); + } + + // 2. Project (fixed order → deterministic). + // Extract constraint parameters into local buffers so the projection loop + // can mutate `self.particles` through `&mut self` without holding an + // immutable borrow of `self.distance_constraints` / `self.tetrahedra` + // (avoids the borrow checker's simultaneous mutable+immutable error). + // Order is preserved → determinism is unchanged. + let ndc = self.distance_constraints.len(); + let mut d_a = Vec::with_capacity(ndc); + let mut d_b = Vec::with_capacity(ndc); + let mut d_rest = Vec::with_capacity(ndc); + let mut d_comp = Vec::with_capacity(ndc); + let mut d_compress = Vec::with_capacity(ndc); + let aniso = self.anisotropy.unwrap_or(Vector::new(1.0, 1.0, 1.0)); + for c in &self.distance_constraints { + d_a.push(c.a); + d_b.push(c.b); + // Phase 31: active-strain contraction — the effective rest length the + // distance solver targets shrinks toward zero as activation rises. + d_rest.push(c.rest * (1.0 - c.activation.clamp(0.0, 1.0))); + // Phase 27: directional (orthotropic) compliance scaling. + let factor = if self.anisotropy.is_some() { + let pa = match (self.particles.get(c.a), self.particles.get(c.b)) { + (Some(a), Some(b)) => a.pos - b.pos, + _ => Vector::new(0.0, 0.0, 0.0), + }; + let len = pa.length(); + if len > 1e-9 { + let n = pa / len; + (n.x * n.x * aniso.x + n.y * n.y * aniso.y + n.z * n.z * aniso.z).max(1e-6) + } else { + 1.0 + } + } else { + 1.0 + }; + d_comp.push(c.compliance / factor); + d_compress.push(c.compression / factor); + } + let mut d_lambda = Vec::with_capacity(ndc); + #[allow(clippy::same_item_push)] // Lagrange accumulator, filled with zeros + for _ in 0..ndc { + d_lambda.push(0.0); + } + let ntet = self.tetrahedra.len(); + let mut t_idx = Vec::with_capacity(ntet); + let mut t_rest = Vec::with_capacity(ntet); + for (i, tet) in self.tetrahedra.iter().enumerate() { + t_idx.push(*tet); + t_rest.push(self.tetra_rest_volumes[i]); + } + let mut v_lambda = Vec::with_capacity(ntet); + #[allow(clippy::same_item_push)] // Lagrange accumulator, filled with zeros + for _ in 0..ntet { + v_lambda.push(0.0); + } + // Body-wide alpha for volume constraints. Phase 16: a dedicated + // `volume_conservation` compliance overrides the solver's default, so the + // tetra volume can be held hard (incompressible) even when the distance + // solver is soft. Falls back to the solver compliance when unset. + let vol_alpha = if let Some(c) = self.volume_conservation { + c / (dt * dt) + } else { + self.xpbd_alpha(dt) + }; + // Self-collision repulsion uses the compliance from `self_collision.stiffness`. + let sc_alpha = if let Some(p) = self.self_collision { + p.stiffness / (dt * dt) + } else { + 0.0 + }; + // Phase 20: accumulate self-collision contacts across iterations for + // post-recovery tangential friction. + let mut self_contacts: Vec<(usize, usize)> = Vec::new(); + for _ in 0..iterations { + for ci in 0..ndc { + // Phase 13: honor per-constraint compliance → per-constraint α̃. + // Phase 19: pick stretch vs compression compliance by the current + // strain sign (tension uses `compliance`, compression uses + // `compression`), enabling anisotropic edges. + let len = (self.particles[d_a[ci]].pos - self.particles[d_b[ci]].pos).length(); + let c_alpha = if len > d_rest[ci] { + d_comp[ci] + } else { + d_compress[ci] + } / (dt * dt); + solve_distance_constraint( + &mut self.particles, + d_a[ci], + d_b[ci], + d_rest[ci], + c_alpha, + &w, + &mut d_lambda[ci], + ); + } + for ti in 0..ntet { + // Phase 30: nonlinear (Neo-Hookean ln(J)) residual when enabled; + // otherwise the linear V − V₀ volume constraint. + if let Some(kh) = self.neo_hookean { + let nh_alpha = kh / (dt * dt); + solve_volume_constraint_nh( + &mut self.particles, + &t_idx[ti], + t_rest[ti], + nh_alpha, + &w, + &mut v_lambda[ti], + ); + } else { + solve_volume_constraint( + &mut self.particles, + &t_idx[ti], + t_rest[ti], + vol_alpha, + &w, + &mut v_lambda[ti], + ); + } + } + // Phase 29: corotated linear-elasticity projection (shape matching). + // Runs after the volume constraints so it corrects the deviatoric + // (volume-preserving) part of the deformation. + if let Some(stiffness) = self.corotated { + let nshape = self.tetra_rest_shapes.len(); + for ti in 0..ntet { + if ti >= nshape { + break; // tets added after enable (e.g. subdivision) have no rest shape + } + solve_corotated_tet( + &mut self.particles, + &t_idx[ti], + &self.tetra_rest_shapes[ti], + stiffness, + &w, + ); + } + } + // Phase 12: self-collision projection (broad-phase + push-apart) once + // per iteration, interleaved with the structural constraints. Phase 20: + // contacts are accumulated so tangential friction can be applied after + // velocity recovery (where velocities are valid). + self_contacts.extend(self.solve_self_collisions(sc_alpha)); + } + + // 3. Recover velocities. + let keep = 1.0 - self.damping; + for (i, p) in self.particles.iter_mut().enumerate() { + if w[i] == 0.0 { + continue; + } + p.vel = (p.pos - prev[i]) / dt; + // Phase 18: global internal damping — bleed a fixed fraction of velocity + // each step (jelly / slime energy loss). Skipped when damping == 0. + if keep < 1.0 { + p.vel *= keep; + } + } + // Phase 20: velocity-level Coulomb friction at every self-collision contact. + // Runs after velocity recovery so `vel` reflects the post-projection motion. + if let Some(mu) = self.self_collision.and_then(|p| p.friction) { + for (i, j) in self_contacts { + Self::damp_contact_velocity(&mut self.particles, i, j, mu); + } + } + } + + /// XPBD `α̃ = α / dt²` from the current solver compliance. + fn xpbd_alpha(&self, dt: Real) -> Real { + let compliance = match self.solver { + SoftSolver::Xpbd { compliance, .. } => compliance, + SoftSolver::MassSpring => 0.0, + }; + compliance / (dt * dt) + } +} + +/// Phase 14 — world-level soft-soft (cross-body) collision. +/// +/// Runs after every soft body has been stepped. For each unordered pair of bodies +/// `(a, b)` with `a < b` that *both* have `cross_collision` set, it builds a uniform +/// spatial hash over the free particles of **both** bodies (cell size `2·R`, where +/// `R = min(radius_a, radius_b)`), finds all inter-body particle pairs whose centres +/// are within `2·R`, and pushes them apart with the same XPBD distance projection used +/// by self-collision (`rest = 2·R`, compliance = `min(stiffness_a, stiffness_b)`). +/// +/// Pairs are projected with a few Gauss-Seidel iterations for stability. Body ids are +/// enumerated in ascending `SoftBodyId` order and particle pairs in index order, so the +/// result is deterministic. Pinned particles (`inv_mass == 0`) and same-body pairs are +/// skipped. This is pure positional projection — it adds no forces and does not touch +/// the SoA solver. +pub fn solve_cross_body_collisions(set: &mut SoftBodySet, dt: Real) { + // Collect ids of bodies with cross-collision enabled, ascending by inner u32. + let mut ids: Vec = Vec::new(); + for (id, sb) in set.iter() { + if sb.cross_collision.is_some() { + ids.push(id); + } + } + ids.sort_by_key(|id| id.0); + let n = ids.len(); + // A few iterations of inter-body projection for stability. + for _iter in 0..3 { + for ia in 0..n { + for ib in (ia + 1)..n { + let a = ids[ia]; + let b = ids[ib]; + let (pa, pb) = match (set.get(a), set.get(b)) { + (Some(x), Some(y)) => (x, y), + _ => continue, + }; + let (ca, cb) = match (pa.cross_collision, pb.cross_collision) { + (Some(x), Some(y)) => (x, y), + _ => continue, + }; + let radius = ca.radius.min(cb.radius); + let stiffness = ca.stiffness.min(cb.stiffness); + // Phase 20: effective contact friction = min of both bodies' μ (a frictionless + // body in the pair makes the contact frictionless, like real Coulomb coupling). + let friction = match (ca.friction, cb.friction) { + (Some(x), Some(y)) => Some(x.min(y)), + _ => None, + }; + let d = radius * 2.0; + let compliance = stiffness / (dt * dt); + let mut pos_a: Vec = Vec::new(); + let mut w_a: Vec = Vec::new(); + let mut map_a: HashMap = HashMap::new(); + for (oi, p) in pa.particles.iter().enumerate() { + if p.inv_mass != 0.0 { + map_a.insert(oi, pos_a.len()); + pos_a.push(p.pos); + w_a.push(p.inv_mass); + } + } + let mut pos_b: Vec = Vec::new(); + let mut w_b: Vec = Vec::new(); + let mut map_b: HashMap = HashMap::new(); + for (oi, p) in pb.particles.iter().enumerate() { + if p.inv_mass != 0.0 { + map_b.insert(oi, pos_b.len()); + pos_b.push(p.pos); + w_b.push(p.inv_mass); + } + } + let cell = d; + let mut grid: HashMap<(i64, i64, i64), Vec<(usize, bool)>> = HashMap::new(); + for (si, p) in pos_a.iter().enumerate() { + let key = ( + (p.x / cell).floor() as i64, + (p.y / cell).floor() as i64, + (p.z / cell).floor() as i64, + ); + grid.entry(key).or_insert_with(Vec::new).push((si, false)); + } + for (si, p) in pos_b.iter().enumerate() { + let key = ( + (p.x / cell).floor() as i64, + (p.y / cell).floor() as i64, + (p.z / cell).floor() as i64, + ); + grid.entry(key).or_insert_with(Vec::new).push((si, true)); + } + let mut pairs: Vec<(usize, usize)> = Vec::new(); + for (si, p) in pos_a.iter().enumerate() { + let cx = (p.x / cell).floor() as i64; + let cy = (p.y / cell).floor() as i64; + let cz = (p.z / cell).floor() as i64; + for ox in -1..=1i64 { + for oy in -1..=1i64 { + for oz in -1..=1i64 { + if let Some(bucket) = grid.get(&(cx + ox, cy + oy, cz + oz)) { + for &(sj, is_b) in bucket { + if !is_b { + continue; + } + let delta = pos_a[si] - pos_b[sj]; + let dist = delta.length(); + if dist < d && dist > 1e-9 { + pairs.push((si, sj)); + } + } + } + } + } + } + } + for &(sa, sb_idx) in &pairs { + let (body_a, body_b) = set.get2_mut(a, b); + // Resolve original particle indices from the slot maps. + let oa_idx = map_a.iter().find(|(_, v)| **v == sa).map(|(&k, _)| k); + let ob_idx = map_b.iter().find(|(_, v)| **v == sb_idx).map(|(&k, _)| k); + if let (Some(oa_idx), Some(ob_idx)) = (oa_idx, ob_idx) { + if let (Some(ba), Some(bb)) = (body_a, body_b) { + let wa = w_a[sa]; + let wb = w_b[sb_idx]; + let pa_now = ba.particles[oa_idx].pos; + let pb_now = bb.particles[ob_idx].pos; + let delta = pa_now - pb_now; + let dist = delta.length(); + if dist < d && dist > 1e-9 { + let n = delta / dist; + let cval = dist - d; + let wsum = wa + wb; + if wsum > 0.0 { + let dlambda = (-cval - compliance * 0.0) / (wsum + compliance); + if wa != 0.0 { + ba.particles[oa_idx].pos += dir_scaled(n, wa * dlambda); + } + if wb != 0.0 { + bb.particles[ob_idx].pos -= dir_scaled(n, wb * dlambda); + } + // Phase 20: velocity-level Coulomb friction on the + // tangential relative slip (velocities are valid here, + // post step_xpbd). + if let Some(mu) = friction { + SoftBody::damp_contact_velocity_split( + &mut ba.particles, + &mut bb.particles, + oa_idx, + ob_idx, + mu, + ); + } + } + } + } + } + } + } + } + } +} + +/// Phase 17: world-level cohesion (adhesion / breakable glue) between soft bodies. +/// +/// For every pair of bodies that both have `cohesion` set, free particles of `a` within +/// `radius = min(radius_a, radius_b)` of a free particle of `b` are attracted toward +/// contact (rest distance `radius`) via an XPBD constraint with compliance +/// `min(stiffness_a, stiffness_b) / dt²`. This bonds the two bodies together like glue — +/// the dual of Phase 9 tearing (which *breaks* edges; this *creates* bonds between bodies). +/// +/// Bonds are *breakable*: if the pair is already separated by more than +/// `min(break_distance_a, break_distance_b)` the attraction is skipped (the glue has torn +/// and is not re-formed this step). Because the solve is stateless and re-evaluated every +/// step from current positions, a pair that drifts back within `radius` re-bonds — unless +/// `break_distance` is `inf` (permanent glue). Runs a few iterations for stability. Shares +/// the spatial-hash structure of [`solve_cross_body_collisions`]. Pure positional projection +/// — no new solver mechanics, no SoA interaction. +pub fn solve_cohesion(set: &mut SoftBodySet, dt: Real) { + let mut ids: Vec = Vec::new(); + for (id, sb) in set.iter() { + if sb.cohesion.is_some() { + ids.push(id); + } + } + ids.sort_by_key(|id| id.0); + let n = ids.len(); + for _iter in 0..3 { + for ia in 0..n { + for ib in (ia + 1)..n { + let a = ids[ia]; + let b = ids[ib]; + let (pa, pb) = match (set.get(a), set.get(b)) { + (Some(x), Some(y)) => (x, y), + _ => continue, + }; + let (ca, cb) = match (pa.cohesion, pb.cohesion) { + (Some(x), Some(y)) => (x, y), + _ => continue, + }; + let radius = ca.radius.min(cb.radius); + let stiffness = ca.stiffness.min(cb.stiffness); + let break_distance = ca.break_distance.min(cb.break_distance); + // `capture` = how far apart two free particles may be and still form a bond + // (must exceed the rest distance `radius`, else indistinguishable from a + // non-overlapping contact). `d` = rest distance of the attraction (contact). + let capture = radius * 2.0; + let d = radius; + let compliance = stiffness / (dt * dt); + let mut pos_a: Vec = Vec::new(); + let mut w_a: Vec = Vec::new(); + let mut map_a: HashMap = HashMap::new(); + for (oi, p) in pa.particles.iter().enumerate() { + if p.inv_mass != 0.0 { + map_a.insert(oi, pos_a.len()); + pos_a.push(p.pos); + w_a.push(p.inv_mass); + } + } + let mut pos_b: Vec = Vec::new(); + let mut w_b: Vec = Vec::new(); + let mut map_b: HashMap = HashMap::new(); + for (oi, p) in pb.particles.iter().enumerate() { + if p.inv_mass != 0.0 { + map_b.insert(oi, pos_b.len()); + pos_b.push(p.pos); + w_b.push(p.inv_mass); + } + } + let cell = capture; + let mut grid: HashMap<(i64, i64, i64), Vec<(usize, bool)>> = HashMap::new(); + for (si, p) in pos_a.iter().enumerate() { + let key = ( + (p.x / cell).floor() as i64, + (p.y / cell).floor() as i64, + (p.z / cell).floor() as i64, + ); + grid.entry(key).or_insert_with(Vec::new).push((si, false)); + } + for (si, p) in pos_b.iter().enumerate() { + let key = ( + (p.x / cell).floor() as i64, + (p.y / cell).floor() as i64, + (p.z / cell).floor() as i64, + ); + grid.entry(key).or_insert_with(Vec::new).push((si, true)); + } + let mut pairs: Vec<(usize, usize)> = Vec::new(); + for (si, p) in pos_a.iter().enumerate() { + let cx = (p.x / cell).floor() as i64; + let cy = (p.y / cell).floor() as i64; + let cz = (p.z / cell).floor() as i64; + for ox in -1..=1i64 { + for oy in -1..=1i64 { + for oz in -1..=1i64 { + if let Some(bucket) = grid.get(&(cx + ox, cy + oy, cz + oz)) { + for &(sj, is_b) in bucket { + if !is_b { + continue; + } + let delta = pos_a[si] - pos_b[sj]; + let dist = delta.length(); + // Bond only when within capture radius AND not + // already torn apart beyond break_distance. + if dist < capture && dist > 1e-9 && dist < break_distance { + pairs.push((si, sj)); + } + } + } + } + } + } + } + for &(sa, sb_idx) in &pairs { + let (body_a, body_b) = set.get2_mut(a, b); + let oa_idx = map_a.iter().find(|(_, v)| **v == sa).map(|(&k, _)| k); + let ob_idx = map_b.iter().find(|(_, v)| **v == sb_idx).map(|(&k, _)| k); + if let (Some(oa_idx), Some(ob_idx)) = (oa_idx, ob_idx) { + if let (Some(ba), Some(bb)) = (body_a, body_b) { + let wa = w_a[sa]; + let wb = w_b[sb_idx]; + let pa_now = ba.particles[oa_idx].pos; + let pb_now = bb.particles[ob_idx].pos; + let delta = pa_now - pb_now; + let dist = delta.length(); + if dist < capture && dist > 1e-9 && dist < break_distance { + let nrm = delta / dist; + // Attract: pull the two particles toward contact distance d. + // c(dist) = dist - d (>0 means too far -> attract inward). + let cval = dist - d; + let wsum = wa + wb; + if wsum > 0.0 { + let dlambda = (-cval - compliance * 0.0) / (wsum + compliance); + if wa != 0.0 { + ba.particles[oa_idx].pos += dir_scaled(nrm, wa * dlambda); + } + if wb != 0.0 { + bb.particles[ob_idx].pos -= dir_scaled(nrm, wb * dlambda); + } + } + } + } + } + } + } + } + } +} + +/// `v * s` for a `Vector` and scalar `s` (glam supports `Vec3 * f64`). +#[inline] +fn dir_scaled(v: Vector, s: Real) -> Vector { + v * s +} + +// ── Phase 3: XPBD constraint projections ─────────────────────────────────── +// +// These are free functions (not methods) so the solver can mutate particle +// positions directly without borrow fights. All arithmetic is plain `f64` +// four-operations on glam `Vector`s — bit-identical under IEEE-754, which is +// what makes XPBD reproducible for `enhanced-determinism` (no `linalg` matrix +// solve is needed for position-based projection; `linalg` is reserved for the +// optional implicit-FEM comparison path in a later sub-phase). + +/// Signed volume of the tetrahedron `(p0, p1, p2, p3)`: +/// `V = ((p1−p0) × (p2−p0)) · (p3−p0) / 6`. +#[inline] +fn signed_tetra_volume(p0: Vector, p1: Vector, p2: Vector, p3: Vector) -> Real { + let e1 = p1 - p0; + let e2 = p2 - p0; + let e3 = p3 - p0; + e1.cross(e2).dot(e3) / 6.0 +} + +/// Solve one XPBD distance constraint, updating `particles` in place and +/// accumulating the Lagrange multiplier into `*lambda`. +#[inline] +fn solve_distance_constraint( + particles: &mut [SoftParticle], + a: usize, + b: usize, + rest: Real, + alpha: Real, + w: &[Real], + lambda: &mut Real, +) { + let pa = particles[a].pos; + let pb = particles[b].pos; + let delta = pa - pb; // vector from b → a (XPBD standard: d = p_a − p_b) + let len = delta.length(); + if len == 0.0 { + return; + } + let n = delta / len; // points from b toward a + let c_val = len - rest; + let wa = w[a]; + let wb = w[b]; + let w_sum = wa + wb; + if w_sum == 0.0 { + return; + } + let d_lambda = (-c_val - alpha * *lambda) / (w_sum + alpha); + *lambda += d_lambda; + // Standard XPBD distance projection: p_a += w_a·Δλ·n, p_b −= w_b·Δλ·n. + // With C = len − rest > 0 (too long), Δλ < 0, so a moves toward b and b toward a. + if wa != 0.0 { + particles[a].pos += dir_scaled(n, wa * d_lambda); + } + if wb != 0.0 { + particles[b].pos -= dir_scaled(n, wb * d_lambda); + } +} + +/// Solve one XPBD tetrahedral volume constraint, updating `particles` in +/// place and accumulating the Lagrange multiplier into `*lambda`. +/// +/// Constraint: `C = V − V0` where `V` is the current signed volume. Gradients +/// (Müller et al.): +/// `∇_0 = −(e2×e3)/6`, `∇_1 = (e3×e1)/6`, `∇_2 = (e1×e2)/6`, `∇_3 = −(e1×e3)/6` +/// (with `e_i = p_i − p_0`). The correction `Δp_i = w_i · Δλ · ∇_i`. +#[inline] +fn solve_volume_constraint( + particles: &mut [SoftParticle], + tet: &[u32; 4], + rest_vol: Real, + alpha: Real, + w: &[Real], + lambda: &mut Real, +) { + let [a, b, c, d] = *tet; + let (ia, ib, ic, id) = (a as usize, b as usize, c as usize, d as usize); + let p0 = particles[ia].pos; + let p1 = particles[ib].pos; + let p2 = particles[ic].pos; + let p3 = particles[id].pos; + let e1 = p1 - p0; + let e2 = p2 - p0; + let e3 = p3 - p0; + + let vol = e1.cross(e2).dot(e3) / 6.0; + let c_val = vol - rest_vol; + + let g0 = e2.cross(e3) / -6.0; + let g1 = e3.cross(e1) / 6.0; + let g2 = e1.cross(e2) / 6.0; + let g3 = e1.cross(e3) / -6.0; + + let wa = w[ia]; + let wb = w[ib]; + let wc = w[ic]; + let wd = w[id]; + // Σ w_i |∇_i|² (all four gradients; w=0 particles contribute 0). + let mut denom = alpha; + denom += wa * g0.dot(g0); + denom += wb * g1.dot(g1); + denom += wc * g2.dot(g2); + denom += wd * g3.dot(g3); + if denom == 0.0 { + return; + } + let d_lambda = (-c_val - alpha * *lambda) / denom; + *lambda += d_lambda; + + if wa != 0.0 { + particles[ia].pos += dir_scaled(g0, wa * d_lambda); + } + if wb != 0.0 { + particles[ib].pos += dir_scaled(g1, wb * d_lambda); + } + if wc != 0.0 { + particles[ic].pos += dir_scaled(g2, wc * d_lambda); + } + if wd != 0.0 { + particles[id].pos += dir_scaled(g3, wd * d_lambda); + } +} +/// Phase 30: Neo-Hookean volume projection. Identical gradient structure to +/// [`solve_volume_constraint`] but with the logarithmic residual +/// `C = ln(J)`, `J = V/V₀` (J floored at `1e-6` so a fully-inverted/collapsed +/// tet stays finite), giving unbounded resistance as `V → 0`. +fn solve_volume_constraint_nh( + particles: &mut [SoftParticle], + tet: &[u32; 4], + rest_vol: Real, + alpha: Real, + w: &[Real], + lambda: &mut Real, +) { + if rest_vol.abs() < 1e-12 { + return; + } + let [a, b, c, d] = *tet; + let (ia, ib, ic, id) = (a as usize, b as usize, c as usize, d as usize); + let p0 = particles[ia].pos; + let p1 = particles[ib].pos; + let p2 = particles[ic].pos; + let p3 = particles[id].pos; + let e1 = p1 - p0; + let e2 = p2 - p0; + let e3 = p3 - p0; + + let vol = e1.cross(e2).dot(e3) / 6.0; + // J = V/V₀ floored at +1e-6: keeps ln finite even for inverted tets. + let j = (vol / rest_vol).max(1e-6); + let c_val = j.ln(); + + // ∇C = (1/J) · ∇J = (1/J) · ∇V/V₀ — chain rule on the log. + let inv_j = 1.0 / j; + let s = inv_j / (6.0 * rest_vol); + let g0 = e2.cross(e3) * -s; + let g1 = e3.cross(e1) * s; + let g2 = e1.cross(e2) * s; + let g3 = e1.cross(e3) * -s; + + let (wa, wb, wc, wd) = (w[ia], w[ib], w[ic], w[id]); + let mut denom = alpha; + denom += wa * g0.dot(g0); + denom += wb * g1.dot(g1); + denom += wc * g2.dot(g2); + denom += wd * g3.dot(g3); + if denom == 0.0 { + return; + } + let d_lambda = (-c_val - alpha * *lambda) / denom; + *lambda += d_lambda; + + if wa != 0.0 { + particles[ia].pos += dir_scaled(g0, wa * d_lambda); + } + if wb != 0.0 { + particles[ib].pos += dir_scaled(g1, wb * d_lambda); + } + if wc != 0.0 { + particles[ic].pos += dir_scaled(g2, wc * d_lambda); + } + if wd != 0.0 { + particles[id].pos += dir_scaled(g3, wd * d_lambda); + } +} +// ── Phase 29: corotated linear elasticity (shape matching) ───────────── + +fn rest_shape_matrix(p0: Vector, p1: Vector, p2: Vector, p3: Vector) -> [[Real; 3]; 3] { + let e1 = p1 - p0; + let e2 = p2 - p0; + let e3 = p3 - p0; + [[e1.x, e2.x, e3.x], [e1.y, e2.y, e3.y], [e1.z, e2.z, e3.z]] +} + +/// Inverse of a 3x3 matrix via the adjugate. Returns `None` when the +/// determinant is (near) zero. +fn mat3_inv(m: &[[Real; 3]; 3]) -> Option<[[Real; 3]; 3]> { + let det = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) + - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); + if !det.is_finite() || det.abs() < 1e-14 { + return None; + } + let inv_det = 1.0 / det; + let mut out = [[0.0; 3]; 3]; + out[0][0] = (m[1][1] * m[2][2] - m[1][2] * m[2][1]) * inv_det; + out[0][1] = (m[0][2] * m[2][1] - m[0][1] * m[2][2]) * inv_det; + out[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * inv_det; + out[1][0] = (m[1][2] * m[2][0] - m[1][0] * m[2][2]) * inv_det; + out[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * inv_det; + out[1][2] = (m[1][0] * m[0][2] - m[0][0] * m[1][2]) * inv_det; + out[2][0] = (m[1][0] * m[2][1] - m[1][1] * m[2][0]) * inv_det; + out[2][1] = (m[0][1] * m[2][0] - m[0][0] * m[2][1]) * inv_det; + out[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]) * inv_det; + Some(out) +} + +fn mat3_mul(a: &[[Real; 3]; 3], b: &[[Real; 3]; 3]) -> [[Real; 3]; 3] { + let mut out = [[0.0; 3]; 3]; + for r in 0..3 { + for c in 0..3 { + out[r][c] = a[r][0] * b[0][c] + a[r][1] * b[1][c] + a[r][2] * b[2][c]; + } + } + out +} + +fn mat3_transpose(m: &[[Real; 3]; 3]) -> [[Real; 3]; 3] { + let mut out = [[0.0; 3]; 3]; + for r in 0..3 { + for c in 0..3 { + out[r][c] = m[c][r]; + } + } + out +} + +/// Rotation extracted from a 3x3 matrix via iterative Shepperd-style +/// quaternion polar decomposition (robust without an SVD, no_std-friendly). +/// `m` need not be orthogonal; returns the closest rotation `R`. +fn polar_rotation(m: &[[Real; 3]; 3]) -> [[Real; 3]; 3] { + // Quaternion from the largest-positive-component branch (Shepperd's method), + // then a few Newton refinement iterations on R q ≈ m. + let tr = m[0][0] + m[1][1] + m[2][2]; + let (mut qw, mut qx, mut qy, mut qz); + if tr > 0.0 { + let s = (tr + 1.0).sqrt() * 2.0; + qw = 0.25 * s; + qx = (m[2][1] - m[1][2]) / s; + qy = (m[0][2] - m[2][0]) / s; + qz = (m[1][0] - m[0][1]) / s; + } else if m[0][0] > m[1][1] && m[0][0] > m[2][2] { + let s = (1.0 + m[0][0] - m[1][1] - m[2][2]).sqrt() * 2.0; + qw = (m[2][1] - m[1][2]) / s; + qx = 0.25 * s; + qy = (m[0][1] + m[1][0]) / s; + qz = (m[0][2] + m[2][0]) / s; + } else if m[1][1] > m[2][2] { + let s = (1.0 + m[1][1] - m[0][0] - m[2][2]).sqrt() * 2.0; + qw = (m[0][2] - m[2][0]) / s; + qx = (m[0][1] + m[1][0]) / s; + qy = 0.25 * s; + qz = (m[1][2] + m[2][1]) / s; + } else { + let s = (1.0 + m[2][2] - m[0][0] - m[1][1]).sqrt() * 2.0; + qw = (m[1][0] - m[0][1]) / s; + qx = (m[0][2] + m[2][0]) / s; + qy = (m[1][2] + m[2][1]) / s; + qz = 0.25 * s; + } + // Newton refinement (8 passes): nudge q toward the rotation that minimises + // ||R(q) − m||²_F. The gradient of that objective w.r.t. the quaternion is + // carried by the skew part of S = R(q)ᵀ·m (the axis-angle error), so each + // pass rotates q slightly along it, then renormalises. + for _ in 0..8 { + let r = quat_to_mat3(qw, qx, qy, qz); + let rt = mat3_transpose(&r); + let s_mat = mat3_mul(&rt, m); + let ex = s_mat[2][1] - s_mat[1][2]; + let ey = s_mat[0][2] - s_mat[2][0]; + let ez = s_mat[1][0] - s_mat[0][1]; + qx += 0.25 * ex; + qy += 0.25 * ey; + qz += 0.25 * ez; + let norm = (qw * qw + qx * qx + qy * qy + qz * qz).sqrt().max(1e-12); + qw /= norm; + qx /= norm; + qy /= norm; + qz /= norm; + } + quat_to_mat3(qw, qx, qy, qz) +} + +fn quat_to_mat3(qw: Real, qx: Real, qy: Real, qz: Real) -> [[Real; 3]; 3] { + [ + [ + 1.0 - 2.0 * (qy * qy + qz * qz), + 2.0 * (qx * qy - qz * qw), + 2.0 * (qx * qz + qy * qw), + ], + [ + 2.0 * (qx * qy + qz * qw), + 1.0 - 2.0 * (qx * qx + qz * qz), + 2.0 * (qy * qz - qx * qw), + ], + [ + 2.0 * (qx * qz - qy * qw), + 2.0 * (qy * qz + qx * qw), + 1.0 - 2.0 * (qx * qx + qy * qy), + ], + ] +} + +/// Phase 29 per-tet projection: shape matching toward the best-fit rotation of +/// the rest shape, restricted to volume-preserving displacements (grad div-free +/// ⇒ the tet's signed volume is unchanged by the correction). +fn solve_corotated_tet( + particles: &mut [SoftParticle], + tet: &[u32; 4], + rest_inv: &[[Real; 3]; 3], + stiffness: Real, + w: &[Real], +) { + let [a, b, c, d] = *tet; + let (ia, ib, ic, id) = (a as usize, b as usize, c as usize, d as usize); + let p0 = particles[ia].pos; + let p1 = particles[ib].pos; + let p2 = particles[ic].pos; + let p3 = particles[id].pos; + // Deformation gradient F = P · T_rest⁻¹. + let p_mat = rest_shape_matrix(p0, p1, p2, p3); + let Some(t_inv) = mat3_inv(rest_inv) else { + return; + }; + let f_mat = mat3_mul(&p_mat, &t_inv); + // Closest rotation R = polar(F). Goal positions g_i = centroid + R·(rest edge)/1. + // Work with edge vectors: goal edges = R · rest edges. + let rot = polar_rotation(&f_mat); + let g0 = p0; // goal for vertex 0 = current p0 (pure deviatoric correction frame) + let _ = g0; + let centroid = Vector::new( + (p0.x + p1.x + p2.x + p3.x) * 0.25, + (p0.y + p1.y + p2.y + p3.y) * 0.25, + (p0.z + p1.z + p3.z) * 0.25, + ); + let _ = centroid; + // Rest edge vectors (columns of rest matrix). + let re1 = Vector::new(rest_inv[0][0], rest_inv[1][0], rest_inv[2][0]); + let re2 = Vector::new(rest_inv[0][1], rest_inv[1][1], rest_inv[2][1]); + let re3 = Vector::new(rest_inv[0][2], rest_inv[1][2], rest_inv[2][2]); + let ge1 = Vector::new( + rot[0][0] * re1.x + rot[0][1] * re1.y + rot[0][2] * re1.z, + rot[1][0] * re1.x + rot[1][1] * re1.y + rot[1][2] * re1.z, + rot[2][0] * re1.x + rot[2][1] * re1.y + rot[2][2] * re1.z, + ); + let ge2 = Vector::new( + rot[0][0] * re2.x + rot[0][1] * re2.y + rot[0][2] * re2.z, + rot[1][0] * re2.x + rot[1][1] * re2.y + rot[1][2] * re2.z, + rot[2][0] * re2.x + rot[2][1] * re2.y + rot[2][2] * re2.z, + ); + let ge3 = Vector::new( + rot[0][0] * re3.x + rot[0][1] * re3.y + rot[0][2] * re3.z, + rot[1][0] * re3.x + rot[1][1] * re3.y + rot[1][2] * re3.z, + rot[2][0] * re3.x + rot[2][1] * re3.y + rot[2][2] * re3.z, + ); + // Goal positions anchored at the mass-weighted centroid (rotation-only ⇒ + // volume-preserving correction frame). + let g1 = p0 + ge1; + let g2 = p0 + ge2; + let g3 = p0 + ge3; + // Per-vertex correction, scaled by stiffness (per-iteration relaxation). + // Standard XPBD shape matching: Δp_i = k · (g_i − p_i) on MOVABLE vertices + // (goal frame anchored at p0 with the extracted rotation R). No explicit + // centroid-offset subtraction — with pinned anchors the naive subtraction + // cancels the correction on the single free vertex of a 3-pinned tet. + let k = stiffness * 0.25; + let (wa, wb, wc, wd) = (w[ia], w[ib], w[ic], w[id]); + let d0 = p0 - p0; + let d1 = g1 - p1; + let d2 = g2 - p2; + let d3 = g3 - p3; + let corr = |d: Vector, wv: Real| -> Vector { + if wv == 0.0 { + return Vector::new(0.0, 0.0, 0.0); + } + dir_scaled(d, k) + }; + particles[ia].pos += corr(d0, wa); + particles[ib].pos += corr(d1, wb); + particles[ic].pos += corr(d2, wc); + particles[id].pos += corr(d3, wd); +} + +/// A container owning all soft bodies in a simulation. Phase 0 keeps this as a +/// plain `Vec` store; later phases may back it with the arena used by the +/// rigid-body / joint sets. +#[derive(Clone, Debug, Default)] +pub struct SoftBodySet { + /// Slot storage. A slot becomes `None` once its body is removed via + /// [`SoftBodySet::remove`], which keeps `SoftBodyId`s stable: ids are slot + /// indices, so removal is id-preserving and never reshuffles live bodies + /// (important for the FFI layer that hands `SoftBodyId`s to callers). + bodies: Vec>, +} + +impl SoftBodySet { + /// Creates an empty set. + pub fn new() -> Self { + Self::default() + } + + /// Inserts a soft body and returns its id. + pub fn insert(&mut self, body: SoftBody) -> SoftBodyId { + let id = SoftBodyId(self.bodies.len() as u32); + self.bodies.push(Some(body)); + id + } + + /// Number of slots (including tombstoned/removed ones). + pub fn len(&self) -> usize { + self.bodies.len() + } + + /// Number of live (not removed) soft bodies. + pub fn count(&self) -> usize { + self.bodies.iter().filter(|b| b.is_some()).count() + } + + /// Whether the set holds no live bodies. + pub fn is_empty(&self) -> bool { + self.count() == 0 + } + + /// Removes the soft body with the given id, freeing its storage. The id + /// stays reserved (the slot becomes a tombstone), so every other live + /// `SoftBodyId` remains valid — callers may keep holding ids across removals. + /// Returns `true` if a live body was removed. + pub fn remove(&mut self, id: SoftBodyId) -> bool { + let slot = match self.bodies.get_mut(id.0 as usize) { + Some(slot) => slot, + None => return false, + }; + match slot.take() { + Some(_) => true, + None => false, + } + } + + /// Immutable access by id. Returns `None` for an unknown or removed id. + #[allow(dead_code)] // consumed by later integration phases (World/FFI). + pub fn get(&self, id: SoftBodyId) -> Option<&SoftBody> { + self.bodies.get(id.0 as usize).and_then(|b| b.as_ref()) + } + + /// Mutable access by id. Returns `None` for an unknown or removed id. + pub fn get_mut(&mut self, id: SoftBodyId) -> Option<&mut SoftBody> { + self.bodies.get_mut(id.0 as usize).and_then(|b| b.as_mut()) + } + + /// Iterator over all live `(SoftBodyId, &SoftBody)` pairs, in ascending id order. + pub fn iter(&self) -> impl Iterator { + self.bodies + .iter() + .enumerate() + .filter_map(|(i, b)| b.as_ref().map(|sb| (SoftBodyId(i as u32), sb))) + } + + /// Simultaneous mutable access to two distinct live bodies. Returns + /// `(None, None)` if either id is unknown/removed or the two ids are equal. + /// Used by the Phase 14 cross-body collision pass to project two bodies apart. + pub fn get2_mut( + &mut self, + a: SoftBodyId, + b: SoftBodyId, + ) -> (Option<&mut SoftBody>, Option<&mut SoftBody>) { + if a == b { + return (None, None); + } + let (lo, hi) = if a.0 <= b.0 { (a, b) } else { (b, a) }; + let (lo_i, hi_i) = (lo.0 as usize, hi.0 as usize); + let len = self.bodies.len(); + if lo_i >= len || hi_i >= len { + return (None, None); + } + // Split the slice so the two borrows are disjoint. + let (left, right) = self.bodies.split_at_mut(hi_i); + let first = left.get_mut(lo_i).and_then(|b| b.as_mut()); + let second = right.get_mut(0).and_then(|b| b.as_mut()); + if a.0 <= b.0 { + (first, second) + } else { + (second, first) + } + } + + /// Advances every live soft body by `dt` (sleeping bodies are skipped). + pub fn step(&mut self, dt: Real) { + for body in self.bodies.iter_mut().flatten() { + body.step(dt); + } + } + + /// Marks a soft body as sleeping (no further integration until woken). + pub fn sleep(&mut self, id: SoftBodyId) -> bool { + match self.bodies.get_mut(id.0 as usize).and_then(|b| b.as_mut()) { + Some(b) => { + b.sleeping = true; + true + } + None => false, + } + } + + /// Wakes a sleeping soft body. + pub fn wake(&mut self, id: SoftBodyId) -> bool { + match self.bodies.get_mut(id.0 as usize).and_then(|b| b.as_mut()) { + Some(b) => { + b.sleeping = false; + true + } + None => false, + } + } + + /// Whether the soft body is currently sleeping. + pub fn is_sleeping(&self, id: SoftBodyId) -> bool { + self.bodies + .get(id.0 as usize) + .and_then(|b| b.as_ref()) + .map(|b| b.sleeping) + .unwrap_or(false) + } + + /// Phase 8: for every live soft body, snap each bound particle to its rigid + /// body's current world transform (`pos = body_local → world`, `vel = + /// body.velocity_at_point(world)`). Bound particles are infinite-mass in the + /// XPBD solve and skipped by local integration, so this is what makes a + /// particle *rigidly follow* the body it is anchored to (flags, tethers, + /// cloth pinned to a moving object). Call once per step, before + /// [`Self::step`] so the followers are already in place when constraints + /// project. Skips sleeping bodies. + pub fn follow_rigid_bodies(&mut self, bodies: &RigidBodySet) { + for body in self.bodies.iter_mut().flatten() { + if body.sleeping { + continue; + } + for p in body.particles.iter_mut() { + let Some(h) = p.bound_body else { + continue; + }; + let Some(rb) = bodies.get(h) else { + continue; + }; + let world = rb.position().transform_point(p.bound_local); + p.pos = world; + p.vel = rb.velocity_at_point(world); + } + } + } + + /// Phase 2: routes each soft body's internal spring/damping forces into the + /// `force_containers` of the rigid bodies their (bound) particles drive. + /// + /// For every particle with `bound_body = Some(h)`, its spring/damping force is + /// written as a `ForceKind::Custom(SOFT_SPRING_CUSTOM_ID)` **Persistent** + /// `ForceEntry` (application point = particle position, so off-center forces + /// generate the correct `r × F` torque). The rigid body then receives the soft + /// force through the standard `compute_body_effective_forces` path — no new + /// solver code, identical lifecycle handling to gravity/thrust. + /// + /// The soft container for each body is cleared and rebuilt each call so the + /// forces stay in sync with the current particle positions (a `Persistent` + /// container survives frame-end draining, but we own it and overwrite it). + /// Sleeping soft bodies are skipped entirely. + pub fn write_spring_forces(&self, bodies: &mut RigidBodySet) { + let kind = ForceKind::Custom(SOFT_SPRING_CUSTOM_ID); + for body in self.bodies.iter().flatten() { + if body.sleeping { + continue; + } + let spring = body.spring_damping_forces(); + for (i, p) in body.particles.iter().enumerate() { + let Some(h) = p.bound_body else { continue }; + let rb = match bodies.get_mut(h) { + Some(rb) => rb, + None => continue, + }; + // Clear + rebuild this body's soft-spring container. + rb.force_containers.remove(&kind); + let entry = ForceEntry { + id: i as u64 + 1, + force: spring[i], + torque: AngVector::ZERO, + point: Some(p.pos), + }; + rb.force_containers + .entry(kind) + .or_insert_with(|| KindContainer::new(kind, Persistence::Persistent)) + .push(entry); + } + } + } +} + +// --------------------------------------------------------------------------- +// Tests — bit-identical numerics. +// --------------------------------------------------------------------------- + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn free_fall_is_analytic() { + // Semi-implicit (symplectic) Euler, single step, is bit-identical to the + // closed form `x1 = x0 + dt·v0 + dt²·a`. We assert with a single step so the + // floating-point operation sequence matches exactly (multi-step iteration + // would round differently from a one-shot closed form and break to_bits). + let g = Vector::new(0.0, -9.81, 0.0); + let mut body = SoftBody::new(g); + let x0 = Vector::new(0.0, 10.0, 0.0); + body.add_particle(x0); + body.particles[0].vel = Vector::new(1.0, 0.0, 0.0); + + let dt = 0.01; + body.step(dt); // exactly one step + + // Closed form for n = 1: v1 = v0 + dt·a ; x1 = x0 + dt·(v0 + dt·a) + let a = g; // constant acceleration = gravity for a free particle + let v0 = Vector::new(1.0, 0.0, 0.0); + let expected = Vector::new( + x0.x + dt * v0.x + dt * dt * a.x, + x0.y + dt * v0.y + dt * dt * a.y, + x0.z + dt * v0.z + dt * dt * a.z, + ); + assert_eq!(body.particles[0].pos.x.to_bits(), expected.x.to_bits()); + assert_eq!(body.particles[0].pos.y.to_bits(), expected.y.to_bits()); + assert_eq!(body.particles[0].pos.z.to_bits(), expected.z.to_bits()); + // And the velocity update is exact too. + let expected_v = Vector::new(v0.x + dt * a.x, v0.y + dt * a.y, v0.z + dt * a.z); + assert_eq!(body.particles[0].vel.x.to_bits(), expected_v.x.to_bits()); + assert_eq!(body.particles[0].vel.y.to_bits(), expected_v.y.to_bits()); + assert_eq!(body.particles[0].vel.z.to_bits(), expected_v.z.to_bits()); + } + + #[test] + fn pinned_particle_does_not_move() { + let mut body = SoftBody::new(Vector::new(0.0, -9.81, 0.0)); + let p = body.add_pinned(Vector::new(0.0, 5.0, 0.0)); + body.add_particle(Vector::new(0.0, 0.0, 0.0)); + // Attach a spring so forces are exercised; the pinned endpoint must stay put. + body.add_spring(p, 1, 100.0, 1.0); + + let before = body.particles[p].pos; + for _ in 0..50 { + body.step(0.01); + } + assert_eq!(body.particles[p].pos.x.to_bits(), before.x.to_bits()); + assert_eq!(body.particles[p].pos.y.to_bits(), before.y.to_bits()); + assert_eq!(body.particles[p].pos.z.to_bits(), before.z.to_bits()); + assert_eq!(body.particles[p].inv_mass, 0.0); + } + + #[test] + fn spring_pulls_particles_together() { + // Two free particles, spring stretched beyond rest, no gravity: they should + // move toward each other (distance decreases), and energy must stay finite. + let mut body = SoftBody::new(Vector::ZERO); + let a = body.add_particle(Vector::new(-2.0, 0.0, 0.0)); + let b = body.add_particle(Vector::new(2.0, 0.0, 0.0)); + // rest length auto-set to current distance 4.0; shrink effective rest by + // creating a second stiffer spring is overkill — instead verify a stretched + // spring from a short rest pulls them in. + body.springs.clear(); + body.springs.push(Spring { + a, + b, + rest_length: 1.0, + stiffness: 50.0, + damping: 0.5, + activation: 0.0, + fibre: None, + }); + + let d0 = (body.particles[b].pos - body.particles[a].pos).length(); + for _ in 0..200 { + body.step(0.005); + } + let d1 = (body.particles[b].pos - body.particles[a].pos).length(); + assert!(d1 < d0, "spring should shorten the gap: {d0} -> {d1}"); + assert!(body.kinetic_energy().is_finite()); + assert!(body.particles[a].pos.is_finite()); + assert!(body.particles[b].pos.is_finite()); + } + + #[test] + fn fibre_direction_steers_active_contraction() { + // Two free particles on the x-axis (edge along x). Set the muscle fibre + // direction along +y and activate: the active-strain drive must pull the + // endpoints along the fibre (+y), not along the edge (+x). + use crate::math::Vector; + let mut body = SoftBody::new(Vector::new(0.0, 0.0, 0.0)); + let a = body.add_particle(Vector::new(-0.5, 0.0, 0.0)); + let b = body.add_particle(Vector::new(0.5, 0.0, 0.0)); + let idx = body.add_spring(a, b, 50.0, 0.5).expect("spring"); + body.set_spring_activation(idx, 1.0); + body.set_spring_fibre_direction(idx, Vector::new(0.0, 1.0, 0.0)); + let pa0 = body.particles[a].pos; + let pb0 = body.particles[b].pos; + for _ in 0..200 { + body.step(0.005); + } + let pa1 = body.particles[a].pos; + let pb1 = body.particles[b].pos; + // Edge (x-gap) should stay ~unchanged; fibre (y) should pull them together. + let dx = (pb1.x - pa1.x) - (pb0.x - pa0.x); + let dy = (pb1.y - pa1.y) - (pb0.y - pa0.y); + assert!( + dy.abs() > 1e-3, + "fibre drive must move endpoints along y: dy={dy}" + ); + assert!(dx.abs() < 1e-2, "edge (x) should stay ~unchanged: dx={dx}"); + assert!(pa1.is_finite() && pb1.is_finite()); + } + + #[test] + fn write_spring_forces_routes_into_force_containers() { + use crate::dynamics::{ + RigidBodyBuilder, RigidBodySet, RigidBodyType, + force_containers::{ForceContainer, ForceKind}, + }; + + // Two rigid bodies 4 units apart on the x-axis; bind one particle to each. + let mut bodies = RigidBodySet::new(); + let builder_a = RigidBodyBuilder::new(RigidBodyType::Dynamic) + .translation(Vector::new(-2.0, 0.0, 0.0).into()); + let builder_b = RigidBodyBuilder::new(RigidBodyType::Dynamic) + .translation(Vector::new(2.0, 0.0, 0.0).into()); + let ha = bodies.insert(builder_a.build()); + let hb = bodies.insert(builder_b.build()); + + // Soft body: two bound particles, spring rest = 1.0 (so the 4.0 gap pulls in). + let mut sb = SoftBody::new(Vector::ZERO); + let pa = sb.add_particle(Vector::new(-2.0, 0.0, 0.0)); + let pb = sb.add_particle(Vector::new(2.0, 0.0, 0.0)); + sb.particles[pa].bound_body = Some(ha); + sb.particles[pb].bound_body = Some(hb); + sb.add_spring(pa, pb, 50.0, 0.5); // rest auto-set to 4.0 by add_spring... + + // Override rest length to 1.0 so the stretched spring produces a known force. + sb.springs[0].rest_length = 1.0; + + let mut set = SoftBodySet::new(); + let id = set.insert(sb); + let _ = id; + + set.write_spring_forces(&mut bodies); + + let kind = ForceKind::Custom(SOFT_SPRING_CUSTOM_ID); + let ca = bodies + .get(ha) + .unwrap() + .force_containers + .get(&kind) + .expect("soft-spring container present on body A"); + let cb = bodies + .get(hb) + .unwrap() + .force_containers + .get(&kind) + .expect("soft-spring container present on body B"); + + // Read back the routed forces via the public contribution iterator. + let fa = ca.contributions().next().unwrap().force(); + let fb = cb.contributions().next().unwrap().force(); + + // Spring stretched: len=4, rest=1 → f_spring = 50*(4-1) = 150, along +x from A. + assert!((fb.x + 150.0).abs() < 1e-9, "B force.x = {}", fb.x); + // Equal and opposite, no y/z component. + assert!(fa.y.abs() < 1e-12 && fa.z.abs() < 1e-12); + assert!(fb.y.abs() < 1e-12 && fb.z.abs() < 1e-12); + } + + // ── Phase 3: XPBD ───────────────────────────────────────────────────── + + #[test] + fn xpbd_distance_constraint_restores_length() { + // Two free particles at distance 1 (rest=1), then b is yanked out to + // distance 4. XPBD projection should pull the gap back toward rest. + let mut body = SoftBody::new(Vector::ZERO); + let a = body.add_particle(Vector::new(0.0, 0.0, 0.0)); + let b = body.add_particle(Vector::new(1.0, 0.0, 0.0)); + body.configure_xpbd(20, 0.0); // rigid (α=0) + body.add_distance_constraint(a, b, 0.0); // rest captured as 1.0 + + // Perturb b to distance 4 (current > rest). + body.particles[b].pos = Vector::new(4.0, 0.0, 0.0); + let d0 = (body.particles[b].pos - body.particles[a].pos).length(); + body.step_xpbd(0.01); + let d1 = (body.particles[b].pos - body.particles[a].pos).length(); + // Distance should shrink from 4 toward rest (1.0). + assert!(d1 < d0, "XPBD distance should shrink gap: {d0} -> {d1}"); + assert!(d1 > 0.5 && d1 < 4.0, "XPBD gap in sane band: {d1}"); + assert!(body.particles[a].pos.is_finite()); + assert!(body.particles[b].pos.is_finite()); + } + + #[test] + fn xpbd_volume_constraint_preserves_tetrahedron() { + // Regular tetrahedron; perturb one vertex, then XPBD volume constraint + // should keep the (relative) volume finite and pull it back toward rest. + let mut body = SoftBody::new(Vector::ZERO); + let p0 = body.add_particle(Vector::new(0.0, 0.0, 0.0)); + let p1 = body.add_particle(Vector::new(1.0, 0.0, 0.0)); + let p2 = body.add_particle(Vector::new(0.0, 1.0, 0.0)); + let p3 = body.add_particle(Vector::new(0.0, 0.0, 1.0)); + body.configure_xpbd(20, 0.0); + body.add_tetrahedron([p0 as u32, p1 as u32, p2 as u32, p3 as u32]); + let rest = body.total_volume(); + assert!(rest.is_finite() && rest > 0.0, "rest volume sane: {rest}"); + + // Perturb p3 outward; volume should grow, then projection pulls it back. + body.particles[p3].pos = Vector::new(0.0, 0.0, 5.0); + let perturbed = body.total_volume(); + assert!( + perturbed > rest, + "perturbation grew volume: {rest} -> {perturbed}" + ); + + body.step_xpbd(0.01); + let recovered = body.total_volume(); + // Should be pulled back toward rest (not explode, not collapse to 0). + assert!(recovered.is_finite()); + assert!( + recovered > 0.1 * rest && recovered < perturbed, + "volume recovered: {rest} -> {perturbed} -> {recovered}" + ); + } + + #[test] + fn xpbd_is_deterministic_bit_identical() { + // Two identical XPBD bodies from the same initial state must produce + // bit-identical results (fixed constraint order, IEEE-754 float ops). + let build = || { + let mut body = SoftBody::new(Vector::new(0.0, -9.81, 0.0)); + let p0 = body.add_particle(Vector::new(0.0, 0.0, 0.0)); + let p1 = body.add_particle(Vector::new(1.0, 0.0, 0.0)); + let p2 = body.add_particle(Vector::new(0.0, 1.0, 0.0)); + let p3 = body.add_particle(Vector::new(0.0, 0.0, 1.0)); + body.configure_xpbd(15, 1e-6); + body.add_distance_constraint(p0, p1, 1e-6); + body.add_distance_constraint(p1, p2, 1e-6); + body.add_distance_constraint(p2, p3, 1e-6); + body.add_tetrahedron([p0 as u32, p1 as u32, p2 as u32, p3 as u32]); + body + }; + let mut a = build(); + let mut b = build(); + for _ in 0..30 { + a.step_xpbd(0.01); + b.step_xpbd(0.01); + } + for i in 0..a.particles.len() { + assert_eq!( + a.particles[i].pos.x.to_bits(), + b.particles[i].pos.x.to_bits(), + "x bit-identical p{i}" + ); + assert_eq!( + a.particles[i].pos.y.to_bits(), + b.particles[i].pos.y.to_bits(), + "y bit-identical p{i}" + ); + assert_eq!( + a.particles[i].pos.z.to_bits(), + b.particles[i].pos.z.to_bits(), + "z bit-identical p{i}" + ); + } + } + + #[test] + fn xpbd_step_dispatcher_routes_to_xpbd() { + // `SoftBody::step` must dispatch to XPBD when solver is configured. + let mut body = SoftBody::new(Vector::ZERO); + let a = body.add_particle(Vector::new(0.0, 0.0, 0.0)); + let b = body.add_particle(Vector::new(1.0, 0.0, 0.0)); + body.configure_xpbd(20, 0.0); // rest captured as 1.0 + body.add_distance_constraint(a, b, 0.0); + body.particles[b].pos = Vector::new(4.0, 0.0, 0.0); // yank to 4 + let d0 = (body.particles[b].pos - body.particles[a].pos).length(); + body.step(0.01); // dispatches to step_xpbd + let d1 = (body.particles[b].pos - body.particles[a].pos).length(); + assert!(d1 < d0, "dispatched XPBD should shrink gap: {d0} -> {d1}"); + } +} diff --git a/src/dynamics/solver/staged_island_solver/worker.rs b/src/dynamics/solver/staged_island_solver/worker.rs index df54ff1df..b1638e881 100644 --- a/src/dynamics/solver/staged_island_solver/worker.rs +++ b/src/dynamics/solver/staged_island_solver/worker.rs @@ -831,6 +831,9 @@ pub(super) unsafe fn run_worker(ctx: &SharedCtx, worker_id: usize) { angvel: solver_vels.angular, }; new_vels = new_vels.apply_damping(base_params.dt, &rb.damping); + // Issue #181: clamp the velocity magnitude to the per-body caps (if any) before + // it is committed to the body and used by the integrator / CCD sweep. + new_vels = new_vels.clamp_magnitude(rb.max_linvel, rb.max_angvel); rb.vels = new_vels; // NOTE: if it's a position-based kinematic body, don't writeback as we want diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 8f884fe4a..5aabc4aca 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -707,6 +707,16 @@ impl ColliderBuilder { } } + /// Sets the mass-properties specification of this builder from a [`ColliderMassProps`] + /// value (density / mass / explicit props). + /// + /// This is the builder-facing counterpart of [`Self::mass_properties`] (which takes a + /// resolved [`MassProperties`]); it is used by `ColliderBuilder::from_baked_compound` to + /// restore the exact mass-properties spec captured at bake time. + pub fn set_mass_properties_spec(&mut self, mprops: ColliderMassProps) { + self.mass_properties = mprops; + } + /// Initialize a new collider builder with a compound shape. pub fn compound(shapes: Vec<(Pose, SharedShape)>) -> Self { Self::new(SharedShape::compound(shapes)) @@ -771,6 +781,33 @@ impl ColliderBuilder { Self::new(SharedShape::cylinder(half_height, radius)) } + /// Initialize a new collider builder with a cylindrical shape aligned with + /// the `x` axis (defined by its half-height along X and its radius). + /// + /// This is equivalent to `ColliderBuilder::cylinder` followed by a 90° + /// rotation about the Z axis, but avoids the caller having to apply that + /// rotation manually (useful when loading geometry from external formats + /// that use a different up-axis convention, e.g. Z-up URDF). + #[cfg(feature = "dim3")] + pub fn cylinder_x(half_height: Real, radius: Real) -> Self { + Self::cylinder(half_height, radius).rotation(Vector::new( + 0.0, + 0.0, + std::f64::consts::FRAC_PI_2, + )) + } + + /// Initialize a new collider builder with a cylindrical shape aligned with + /// the `z` axis (defined by its half-height along Z and its radius). + #[cfg(feature = "dim3")] + pub fn cylinder_z(half_height: Real, radius: Real) -> Self { + Self::cylinder(half_height, radius).rotation(Vector::new( + std::f64::consts::FRAC_PI_2, + 0.0, + 0.0, + )) + } + /// Initialize a new collider builder with a rounded cylindrical shape defined by its half-height /// (along the Y axis), its radius, and its roundedness (the radius of the sphere used for /// dilating the cylinder). @@ -783,6 +820,28 @@ impl ColliderBuilder { )) } + /// Initialize a new collider builder with a rounded cylindrical shape + /// aligned with the `x` axis. See [`ColliderBuilder::cylinder_x`]. + #[cfg(feature = "dim3")] + pub fn round_cylinder_x(half_height: Real, radius: Real, border_radius: Real) -> Self { + Self::round_cylinder(half_height, radius, border_radius).rotation(Vector::new( + 0.0, + 0.0, + std::f64::consts::FRAC_PI_2, + )) + } + + /// Initialize a new collider builder with a rounded cylindrical shape + /// aligned with the `z` axis. See [`ColliderBuilder::cylinder_z`]. + #[cfg(feature = "dim3")] + pub fn round_cylinder_z(half_height: Real, radius: Real, border_radius: Real) -> Self { + Self::round_cylinder(half_height, radius, border_radius).rotation(Vector::new( + std::f64::consts::FRAC_PI_2, + 0.0, + 0.0, + )) + } + /// Initialize a new collider builder with a cone shape defined by its half-height /// (along the Y axis) and its basis radius. #[cfg(feature = "dim3")] @@ -975,6 +1034,59 @@ impl ColliderBuilder { Ok(Self::new(shape).position(pose)) } + /// Sanitizes the input of the `convex_decomposition*` family so that parry's internal + /// voxelization convex-hull never panics on degenerate data (issue #223: passing 3+ identical + /// consecutive points made `convex_decomposition` panic inside `clip_aabb_line` / the voxel + /// convex-hull). We drop exact/near-duplicate vertices (remapping the triangle indices + /// accordingly) and reject inputs that cannot form a 3D convex hull after de-duplication + /// (fewer than 4 distinct points). Returns `None` in that degenerate case. + fn sanitize_decomposition_input( + vertices: &[Vector], + indices: &[[u32; DIM]], + ) -> Option<(Vec, Vec<[u32; DIM]>)> { + // 1. Deduplicate vertices within an epsilon; build old->new index map. + let eps = 1.0e-8; // relaxed tolerance to catch near-identical points + let mut clean: Vec = Vec::new(); + let mut remap = vec![0u32; vertices.len()]; + for (i, v) in vertices.iter().enumerate() { + let mut found = None; + for (j, c) in clean.iter().enumerate() { + let d2 = (c.x - v.x).powi(2) + (c.y - v.y).powi(2) + (c.z - v.z).powi(2); + if d2 <= eps * eps { + found = Some(j as u32); + break; + } + } + match found { + Some(j) => remap[i] = j, + None => { + remap[i] = clean.len() as u32; + clean.push(*v); + } + } + } + + // 2. Remap triangle indices, dropping triangles that reference duplicates of the same + // vertex (fewer than 3 distinct indices => degenerate, no area). + let mut clean_idx: Vec<[u32; DIM]> = Vec::new(); + for tri in indices { + let a = remap[tri[0] as usize]; + let b = remap[tri[1] as usize]; + let c = remap[tri[2] as usize]; + if a != b && b != c && a != c { + clean_idx.push([a, b, c]); + } + } + + // 3. Need at least 4 distinct non-coplanar points to form a 3D convex hull; otherwise + // parry's voxelization convex-hull has nothing valid to compute. + if clean.len() < 4 || clean_idx.is_empty() { + return None; + } + + Some((clean, clean_idx)) + } + /// Creates a compound collider by decomposing a mesh/polyline into convex pieces. /// /// Concave shapes (like an 'L' or 'C') are automatically broken into multiple convex @@ -982,7 +1094,11 @@ impl ColliderBuilder { /// /// Uses the V-HACD algorithm. Good for imported models that aren't already convex. pub fn convex_decomposition(vertices: &[Vector], indices: &[[u32; DIM]]) -> Self { - Self::new(SharedShape::convex_decomposition(vertices, indices)) + match Self::sanitize_decomposition_input(vertices, indices) { + Some((v, i)) => Self::new(SharedShape::convex_decomposition(&v, &i)), + // Degenerate input (e.g. all-identical points): nothing to decompose -> empty compound. + None => Self::new(SharedShape::ball(0.0)), // degenerate input -> zero-radius ball (safe, no hull) + } } /// Initializes a collider builder with a compound shape obtained from the decomposition of @@ -992,11 +1108,14 @@ impl ColliderBuilder { indices: &[[u32; DIM]], border_radius: Real, ) -> Self { - Self::new(SharedShape::round_convex_decomposition( - vertices, - indices, - border_radius, - )) + match Self::sanitize_decomposition_input(vertices, indices) { + Some((v, i)) => Self::new(SharedShape::round_convex_decomposition( + &v, + &i, + border_radius, + )), + None => Self::new(SharedShape::ball(0.0)), // degenerate input -> zero-radius ball (safe, no hull) + } } /// Initializes a collider builder with a compound shape obtained from the decomposition of @@ -1006,9 +1125,12 @@ impl ColliderBuilder { indices: &[[u32; DIM]], params: &VHACDParameters, ) -> Self { - Self::new(SharedShape::convex_decomposition_with_params( - vertices, indices, params, - )) + match Self::sanitize_decomposition_input(vertices, indices) { + Some((v, i)) => Self::new(SharedShape::convex_decomposition_with_params( + &v, &i, params, + )), + None => Self::new(SharedShape::ball(0.0)), // degenerate input -> zero-radius ball (safe, no hull) + } } /// Initializes a collider builder with a compound shape obtained from the decomposition of @@ -1019,12 +1141,15 @@ impl ColliderBuilder { params: &VHACDParameters, border_radius: Real, ) -> Self { - Self::new(SharedShape::round_convex_decomposition_with_params( - vertices, - indices, - params, - border_radius, - )) + match Self::sanitize_decomposition_input(vertices, indices) { + Some((v, i)) => Self::new(SharedShape::round_convex_decomposition_with_params( + &v, + &i, + params, + border_radius, + )), + None => Self::new(SharedShape::ball(0.0)), // degenerate input -> zero-radius ball (safe, no hull) + } } /// Creates the smallest convex shape that contains all the given points. @@ -1460,3 +1585,144 @@ impl From for Collider { val.build() } } + +#[cfg(test)] +mod decomposition_tests { + use super::*; + use parry::shape::TypedShape; + + /// Inspects a `ColliderBuilder`'s shape as a compound, returning the number of convex parts. + fn compound_part_count(builder: &ColliderBuilder) -> Option { + match builder.shape.as_typed_shape() { + TypedShape::Compound(c) => Some(c.shapes().len()), + _ => None, + } + } + + #[test] + fn convex_decomposition_does_not_panic_on_identical_points() { + // Regression test for issue #223: passing 3+ identical consecutive points (or otherwise + // degenerate input) used to panic deep inside parry's voxelization convex-hull + // (`clip_aabb_line` matrix-index-out-of-bounds). After the input sanitization in + // `sanitize_decomposition_input`, degenerate input yields a safe (empty) compound instead. + let vertices = vec![ + Vector::new(0.0, 0.0, 0.0), + Vector::new(0.0, 0.0, 0.0), // duplicate + Vector::new(0.0, 0.0, 0.0), // duplicate + ]; + // Indices reference only duplicates -> all triangles degenerate. + let indices = vec![[0u32, 1, 2]]; + + // Must not panic, and must yield a valid (non-compound) shape instead of a hull. + let builder = ColliderBuilder::convex_decomposition(&vertices, &indices); + assert_eq!(compound_part_count(&builder), None); + + // Same for the round / with-params variants. + let r = ColliderBuilder::round_convex_decomposition(&vertices, &indices, 0.01); + assert_eq!(compound_part_count(&r), None); + let p = ColliderBuilder::convex_decomposition_with_params( + &vertices, + &indices, + &VHACDParameters::default(), + ); + assert_eq!(compound_part_count(&p), None); + let rp = ColliderBuilder::round_convex_decomposition_with_params( + &vertices, + &indices, + &VHACDParameters::default(), + 0.01, + ); + assert_eq!(compound_part_count(&rp), None); + } + + #[test] + fn convex_decomposition_keeps_valid_input() { + // A real tetrahedron must still decompose into >= 1 convex part (sanitization is a no-op + // for clean input). + let vertices = 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.0, 0.0, 1.0), + ]; + let indices = vec![[0u32, 1, 2], [0, 1, 3], [0, 2, 3], [1, 2, 3]]; + let builder = ColliderBuilder::convex_decomposition(&vertices, &indices); + let parts = compound_part_count(&builder); + assert!(parts.is_some_and(|n| n >= 1), "valid mesh must decompose"); + } +} + +#[cfg(test)] +mod cylinder_epa_tests { + use super::*; + use glamx::glam::{DQuat, DVec3}; + use parry::math::Pose; + use parry::query::{Contact, DefaultQueryDispatcher, QueryDispatcher}; + use parry::shape::{Cuboid, Cylinder}; + + /// Regression test for upstream issue #305 ("Odd cylinder collisions"): + /// a cylinder resting on a very large but very thin cuboid (e.g. a floor slab tilted + /// slightly) used to make EPA return an *incorrect* contact normal (dominated by a + /// horizontal component instead of the vertical support direction). A wrong normal makes + /// the solver push the cylinder sideways and it falls through the floor. + /// + /// We reproduce the canonical scenario: a thin large cuboid at the origin rotated by a + /// small angle about Z (matching the issue's `rotation(vector![0.2, 0.0, 0.0])` floor), + /// with a cylinder placed just above it. The contact normal reported by the dispatcher + /// must be dominantly vertical (supporting the cylinder), NOT horizontal (the EPA bug). + #[test] + fn cylinder_on_thin_cuboid_has_vertical_contact_normal() { + let dispatcher = DefaultQueryDispatcher; + + // Thin, very large cuboid (floor slab): half-extents (100, 0.1, 100). + let cuboid = Cuboid::new(Vector::new(100.0, 0.1, 100.0)); + // Cylinder: half-height 0.5, radius 0.5 (matches the issue's cylinder). + let cylinder = Cylinder::new(0.5, 0.5); + + // Floor slab tilted by 0.2 rad about Z, matching the issue's floor rotation. + let floor_rot = DQuat::from_axis_angle(DVec3::Z, 0.2); + let floor_pose = Pose::from_parts(DVec3::ZERO, floor_rot); + // Cylinder sits just above the slab top, axis vertical (+Y), centered at x = 0. + let cyl_pose = Pose::from_parts(DVec3::new(0.0, 0.1 + 0.5 + 1e-3, 0.0), DQuat::IDENTITY); + + // pos12 = cylinder relative to floor. + let pos12 = cyl_pose * floor_pose.inverse(); + + let result = dispatcher + .contact(&pos12, &cylinder, &cuboid, 0.1) + .expect("cylinder/cuboid contact must be supported"); + + let contact: Contact = result.expect("cylinder should be in contact with the slab"); + // normal2 is the contact normal expressed in the *cuboid* (floor) local frame. + // For a correct support contact it must point mostly along the floor's local up + // axis (Y), i.e. |normal2.y| must dominate over the horizontal components. + let n = contact.normal2; + let vertical = n.y.abs(); + let horizontal = (n.x * n.x + n.z * n.z).sqrt(); + assert!( + vertical > horizontal, + "contact normal should be dominantly vertical (support), got n=({:.3}, {:.3}, {:.3})", + n.x, + n.y, + n.z + ); + // And it must actually be in contact (penetrating or touching), not separated. + assert!( + contact.dist <= 0.1, + "cylinder must be in contact with the slab, dist={:.4}", + contact.dist + ); + // The bug (#305) symptom is a *horizontal* normal: EPA returns a sideways direction + // that makes the solver push the cylinder horizontally and it tunnels through the + // floor. Express the contact normal back in world space and confirm it is dominantly + // vertical (the cylinder is supported by the floor, not pushed sideways). The sign + // depends on convention (it points from the cylinder into the floor, i.e. downward + // here), so we test |y| rather than the sign. + let world_normal = floor_rot * n; + assert!( + world_normal.y.abs() > 0.9, + "world-space contact normal should be dominantly vertical (support), got y={:.3}", + world_normal.y + ); + } +} diff --git a/src/geometry/compound_baker.rs b/src/geometry/compound_baker.rs new file mode 100644 index 000000000..a166a0992 --- /dev/null +++ b/src/geometry/compound_baker.rs @@ -0,0 +1,133 @@ +//! Phase C: baked compound collision. +//! +//! Box3D-style "uber shape" pre-cooking for large static environments. A `Compound` +//! (parry's `SharedShape::compound`) already builds and stores a `Bvh` for its +//! sub-shapes; under parry's `serde` feature that BVH is serde-derivable, so the +//! bake format is just a serialized, ready-to-use shape plus the mass properties +//! computed at bake time. Reloading a baked compound constructs a `ColliderBuilder` +//! whose shape/geometry BVH is reused verbatim — no BVH rebuild on load. +//! +//! This is gated behind `feature = "serde-serialize"` because the bake format is a +//! serde payload (mirrors the rest of Rapier's serialization, which is opt-in). + +#![cfg(feature = "serde-serialize")] + +use crate::alloc_prelude::*; +use crate::geometry::SharedShape; +use crate::geometry::collider::ColliderBuilder; +use crate::geometry::collider_components::ColliderMassProps; +use serde::{Deserialize, Serialize}; + +/// A serializable snapshot of a baked compound collider. +/// +/// The `shape` field carries parry's `Compound` (including its pre-built BVH), +/// so a deserialize restores the fully-built query structure without rebuilding +/// the BVH. Mass properties are frozen at bake time so `from_baked_compound` +/// restores identical dynamics to the original builder. +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct BakedCompound { + /// The cooked shape (a `Compound` with its BVH embedded). + shape: SharedShape, + /// Mass properties captured when the compound was baked. + mass_properties: ColliderMassProps, + /// Whether the baked collider is a sensor. + is_sensor: bool, +} + +impl BakedCompound { + /// Serializes this baked compound to compact bytes (bincode little-endian). + /// + /// The payload includes the pre-built BVH, so loading the bytes is much cheaper + /// than reconstructing the compound from its raw sub-shapes. The output is + /// suitable for on-disk caching or memory-mapped streaming loaders. + pub fn to_bytes(&self) -> Result, bincode::Error> { + bincode::serialize(self) + } + + /// Deserializes a baked compound previously produced by [`Self::to_bytes`]. + /// + /// Reuses the embedded BVH; no BVH construction is performed. + pub fn from_bytes(bytes: &[u8]) -> Result { + bincode::deserialize(bytes) + } +} + +impl ColliderBuilder { + /// Phase C: bakes this builder's shape into a [`BakedCompound`]. + /// + /// Any `SharedShape` can be baked, but the primary use case is a `Compound` + /// built from many sub-shapes — the bake captures its pre-built BVH so loading + /// is BVH-rebuild-free. The mass properties are captured from the builder's + /// current `mass_properties` setting (density/mass/units) so the baked form is + /// dynamics-equivalent to the original. + pub fn bake_compound(&self) -> BakedCompound { + BakedCompound { + shape: self.shape.clone(), + mass_properties: self.mass_properties.clone(), + is_sensor: self.is_sensor, + } + } + + /// Phase C: rebuilds a [`ColliderBuilder`] from a [`BakedCompound`]. + /// + /// Restores the cooked shape (with its pre-built BVH) and the bake-time mass + /// properties and sensor flag. Material, collision groups, hooks, events, + /// position and user-data are left at their builder defaults and should be set + /// by the caller as needed — baking captures geometry and dynamics only. + pub fn from_baked_compound(baked: BakedCompound) -> Self { + let mut builder = ColliderBuilder::new(baked.shape); + builder.set_mass_properties_spec(baked.mass_properties); + builder.is_sensor = baked.is_sensor; + builder + } +} + +#[cfg(test)] +mod test { + use super::*; + use crate::geometry::ColliderMassProps; + use crate::geometry::{ColliderBuilder, SharedShape}; + use crate::math::Pose; + + /// Phase C: a baked compound must round-trip through bytes and rebuild a + /// geometry-equivalent collider (same AABB, same mass-properties spec) without + /// needing to reconstruct the BVH from its raw sub-shapes. + #[test] + fn bake_compound_round_trips_through_bytes() { + let sub_shapes = vec![ + (Pose::translation(0.0, 0.0, 0.0), SharedShape::ball(0.5)), + (Pose::translation(2.0, 0.0, 0.0), SharedShape::ball(0.5)), + ( + Pose::translation(4.0, 1.0, 0.0), + SharedShape::cuboid(0.3, 0.3, 0.3), + ), + ]; + let original_builder = ColliderBuilder::compound(sub_shapes).density(2.0); + let original_aabb = original_builder.clone().build().compute_aabb(); + + // Bake the builder (captures shape + mass-props spec) and serialize. + let baked = original_builder.bake_compound(); + let bytes = baked.to_bytes().expect("bake -> bytes"); + + // Deserialize + rebuild. + let restored = BakedCompound::from_bytes(&bytes).expect("bytes -> baked"); + let rebuilt_builder = ColliderBuilder::from_baked_compound(restored); + let rebuilt_aabb = rebuilt_builder.clone().build().compute_aabb(); + + // Geometry round-trips exactly (the baked BVH is reused, not rebuilt). + let dmin = (original_aabb.mins.x - rebuilt_aabb.mins.x).abs() + + (original_aabb.mins.y - rebuilt_aabb.mins.y).abs() + + (original_aabb.mins.z - rebuilt_aabb.mins.z).abs(); + let dmax = (original_aabb.maxs.x - rebuilt_aabb.maxs.x).abs() + + (original_aabb.maxs.y - rebuilt_aabb.maxs.y).abs() + + (original_aabb.maxs.z - rebuilt_aabb.maxs.z).abs(); + assert!(dmin < 1e-6, "baked AABB mins diverged"); + assert!(dmax < 1e-6, "baked AABB maxs diverged"); + + // Mass-properties spec preserved. + assert_eq!( + rebuilt_builder.mass_properties, + ColliderMassProps::Density(2.0) + ); + } +} diff --git a/src/geometry/direction_hull.rs b/src/geometry/direction_hull.rs new file mode 100644 index 000000000..5c9cbd7dd --- /dev/null +++ b/src/geometry/direction_hull.rs @@ -0,0 +1,213 @@ +//! Direction-based convex hulls (k-DOP and fixed-direction hulls). +//! +//! A k-DOP (discrete-orientation polytope) or FDH (fixed-direction hull) is the +//! intersection of a set of slabs, each bounded by two parallel planes whose +//! normals come from a fixed set of directions. These are cheap, tight bounding +//! volumes that are popular for broad-phase acceleration and character collision +//! because their overlap test is a handful of dot products. +//! +//! The core routine here projects a point cloud onto a set of directions to get +//! slab bounds, then intersects the corresponding half-spaces to recover the +//! hull vertices and forwards them to `ColliderBuilder::convex_hull`. It is a +//! general-purpose geometry utility (not tied to the collision pipeline) so it +//! can also be reused by compound baking or user spatial queries. +//! +//! The original algorithm and its `KdopHull` / `FdhHull` shape wrappers are kept +//! as a stable public API so host crates can build direction hulls the same way +//! they built AABB / OBB bounds. + +use crate::geometry::ColliderBuilder; +use alloc::vec::Vec; +use parry::math::Vector; + +const EPSILON: f64 = 1.0e-9; + +/// Discrete-orientation polytope preset selecting the slab normal set. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum KdopPreset { + /// 6 slabs: the three axis-aligned normals. + K6, + /// 14 slabs: K6 plus the four diagonal (±1,±1,±1) normals. + K14, + /// 18 slabs: K14 plus the two (±1,±1,0) diagonals on each axis plane. + K18, + /// 26 slabs: K18 plus the remaining four (±1,0,±1)/(0,±1,±1) diagonals. + K26, +} + +#[derive(Clone, Copy)] +struct Slab { + normal: Vector, + min: f64, + max: f64, +} + +/// A hull defined by a set of slab directions. +pub trait DirectionHull { + /// The slab normal directions defining this hull. + fn directions(&self) -> &[Vector]; + + /// Build a convex-hull collider from `points` bounded by this hull's + /// directions. Returns `None` if the points don't span a 3D volume or the + /// slab intersection is degenerate. + fn build(&self, points: &[Vector]) -> Option { + build_direction_hull(points, self.directions()) + } +} + +/// k-DOP hull: owns its direction set (e.g. from a [`KdopPreset`]). +pub struct KdopHull { + /// The slab normal directions defining this hull. + pub directions: Vec, +} + +impl DirectionHull for KdopHull { + fn directions(&self) -> &[Vector] { + &self.directions + } +} + +/// Fixed-direction hull: borrows an externally-owned direction set. +pub struct FdhHull<'a> { + /// The slab normal directions defining this hull. + pub directions: &'a [Vector], +} + +impl DirectionHull for FdhHull<'_> { + fn directions(&self) -> &[Vector] { + self.directions + } +} + +/// The canonical slab-normal set for a k-DOP [`KdopPreset`]. +pub fn kdop_directions(preset: KdopPreset) -> Vec { + let mut directions: Vec = Vec::with_capacity(26); + directions.push(Vector::new(1.0, 0.0, 0.0)); + directions.push(Vector::new(0.0, 1.0, 0.0)); + directions.push(Vector::new(0.0, 0.0, 1.0)); + + if matches!(preset, KdopPreset::K14 | KdopPreset::K18 | KdopPreset::K26) { + directions.extend([ + Vector::new(1.0, 1.0, 1.0), + Vector::new(1.0, 1.0, -1.0), + Vector::new(1.0, -1.0, 1.0), + Vector::new(-1.0, 1.0, 1.0), + ]); + } + + if matches!(preset, KdopPreset::K18 | KdopPreset::K26) { + directions.extend([Vector::new(1.0, 1.0, 0.0), Vector::new(1.0, -1.0, 0.0)]); + } + + if matches!(preset, KdopPreset::K26) { + directions.extend([ + Vector::new(1.0, 0.0, 1.0), + Vector::new(1.0, 0.0, -1.0), + Vector::new(0.0, 1.0, 1.0), + Vector::new(0.0, 1.0, -1.0), + ]); + } + + directions + .into_iter() + .filter_map(normalize_direction) + .collect() +} + +fn normalize_direction(direction: Vector) -> Option { + let len = direction.length(); + (len > EPSILON).then_some(direction / len) +} + +fn slabs_from_points(points: &[Vector], directions: &[Vector]) -> Option> { + let mut slabs = Vec::with_capacity(directions.len()); + for direction in directions { + let Some(normal) = normalize_direction(*direction) else { + continue; + }; + + let mut min = f64::INFINITY; + let mut max = f64::NEG_INFINITY; + for point in points { + let projection = normal.dot(*point); + min = min.min(projection); + max = max.max(projection); + } + + if min.is_finite() && max.is_finite() { + slabs.push(Slab { normal, min, max }); + } + } + + (slabs.len() >= 3).then_some(slabs) +} + +fn solve_planes(a: Vector, da: f64, b: Vector, db: f64, c: Vector, dc: f64) -> Option { + let cross_bc = b.cross(c); + let det = a.dot(cross_bc); + if det.abs() <= EPSILON { + return None; + } + + Some((cross_bc * da + c.cross(a) * db + a.cross(b) * dc) / det) +} + +fn contains_point(slabs: &[Slab], point: Vector) -> bool { + slabs.iter().all(|slab| { + let projection = slab.normal.dot(point); + projection >= slab.min - 1.0e-7 && projection <= slab.max + 1.0e-7 + }) +} + +fn push_unique(points: &mut Vec, point: Vector) { + if points + .iter() + .any(|existing| (*existing - point).length_squared() <= 1.0e-12) + { + return; + } + + points.push(point); +} + +/// Build a convex-hull collider from `points`, bounded by the slab directions in +/// `directions`. +/// +/// Returns `None` when `points` has fewer than 4 entries (no 3D volume), or when +/// the slab intersection is degenerate and yields no valid hull vertices. +pub fn build_direction_hull(points: &[Vector], directions: &[Vector]) -> Option { + if points.len() < 4 { + return None; + } + + let slabs = slabs_from_points(points, directions)?; + let mut planes: Vec<(Vector, f64)> = Vec::with_capacity(slabs.len() * 2); + for slab in &slabs { + planes.push((slab.normal, slab.max)); + planes.push((-slab.normal, -slab.min)); + } + + let mut vertices: Vec = Vec::new(); + for i in 0..planes.len() { + for j in (i + 1)..planes.len() { + for k in (j + 1)..planes.len() { + let Some(point) = solve_planes( + planes[i].0, + planes[i].1, + planes[j].0, + planes[j].1, + planes[k].0, + planes[k].1, + ) else { + continue; + }; + + if contains_point(&slabs, point) { + push_unique(&mut vertices, point); + } + } + } + } + + ColliderBuilder::convex_hull(vertices.as_slice()) +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index b11ec69fe..7606e0f8f 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -12,6 +12,8 @@ pub use self::collider_components::*; pub use self::collider_handle::ColliderHandle; #[cfg(feature = "alloc")] pub use self::collider_set::{ColliderSet, ModifiedColliders}; +#[cfg(all(feature = "alloc", feature = "serde-serialize"))] +pub use self::compound_baker::BakedCompound; #[cfg(feature = "alloc")] pub(crate) use self::contact_pair::ContactRecycleState; #[cfg(feature = "alloc")] @@ -26,6 +28,11 @@ pub use self::contact_pair::{ SimdSolverContact, SolverContact, SolverContactGeneric, SolverContacts, SolverFlags, is_bouncy, is_bouncy_simd, }; +/// Direction-based convex hulls (k-DOP / FDH) and their preset enum. +#[cfg(feature = "alloc")] +pub use self::direction_hull::{ + DirectionHull, FdhHull, KdopHull, KdopPreset, build_direction_hull, +}; #[cfg(feature = "alloc")] pub use self::interaction_graph::{ ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex, @@ -35,6 +42,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; +/// User-space AABB spatial index (general R-tree over arbitrary `u64` ids). +#[cfg(feature = "alloc")] +pub use self::user_index::GenericAabbIndex; #[cfg(feature = "alloc")] pub use parry::utils::Array2; @@ -287,6 +297,8 @@ mod broad_phase_pair_event; mod collider; #[cfg(feature = "alloc")] mod collider_set; +#[cfg(all(feature = "alloc", feature = "serde-serialize"))] +mod compound_baker; #[cfg(feature = "alloc")] mod mesh_converter; @@ -295,3 +307,14 @@ mod manifold_reduction; #[cfg(all(feature = "dim3", feature = "alloc"))] mod contact_clustering; + +/// User-space AABB spatial index (general R-tree over arbitrary `u64` ids). +#[cfg(feature = "alloc")] +pub mod user_index; + +/// Direction-based convex hulls (k-DOP / FDH). +#[cfg(feature = "alloc")] +pub mod direction_hull; + +#[cfg(all(test, feature = "dim3", feature = "alloc"))] +mod voxel_ball_tests; diff --git a/src/geometry/narrow_phase/contacts.rs b/src/geometry/narrow_phase/contacts.rs index bb870c612..f93f433b2 100644 --- a/src/geometry/narrow_phase/contacts.rs +++ b/src/geometry/narrow_phase/contacts.rs @@ -99,6 +99,7 @@ impl NarrowPhase { query_dispatcher, &awake_body_mask, hints_ptr, + &self.disabled_collider_pairs, &mut transitions, ) }; @@ -125,6 +126,7 @@ impl NarrowPhase { query_dispatcher, &awake_body_mask, hints_ptr, + &self.disabled_collider_pairs, &snd, ) }; diff --git a/src/geometry/narrow_phase/mod.rs b/src/geometry/narrow_phase/mod.rs index 65ae91da2..17cb3a709 100644 --- a/src/geometry/narrow_phase/mod.rs +++ b/src/geometry/narrow_phase/mod.rs @@ -20,11 +20,12 @@ use crate::dynamics::solver::solver_contact_graph::{ }; use crate::dynamics::{IslandManager, RigidBodySet}; use crate::geometry::{ - ColliderGraphIndex, ColliderHandle, ColliderSet, ContactData, ContactManifoldData, ContactPair, - InteractionGraph, IntersectionPair, SolverFlags, + ColliderGraphIndex, ColliderHandle, ColliderPair, ColliderSet, ContactData, + ContactManifoldData, ContactPair, InteractionGraph, IntersectionPair, SolverFlags, }; use alloc::sync::Arc; use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; +use parry::utils::hashmap::HashMap; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq, Eq, Default)] @@ -402,6 +403,13 @@ pub struct NarrowPhase { /// step) skip the buffer reallocation of freshly constructed pairs. #[cfg_attr(feature = "serde-serialize", serde(skip))] retired_pairs: Vec, + /// Set of collider pairs whose collision is explicitly disabled, regardless of + /// collision groups, solver hooks, or joints. Mirrors the joint-based + /// `contacts_enabled` filter but applies to any two colliders (e.g. two bodies + /// that are not connected by a joint). Populated via + /// [`Self::disable_collision`] / [`Self::enable_collision`]. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + disabled_collider_pairs: HashMap, } pub(crate) type ContactManifoldIndex = usize; @@ -444,9 +452,55 @@ impl NarrowPhase { force_event_flagged: Vec::new(), force_list_valid: false, solver_color_todo: Vec::new(), + disabled_collider_pairs: HashMap::default(), } } + /// Orders the two collider handles so the disabled-pair map stays order-independent: + /// `disable_collision(a, b)` and `disable_collision(b, a)` map to the same key. + fn normalized_pair(collider1: ColliderHandle, collider2: ColliderHandle) -> ColliderPair { + if collider1.0 <= collider2.0 { + ColliderPair::new(collider1, collider2) + } else { + ColliderPair::new(collider2, collider1) + } + } + + /// Explicitly disables collision detection between two specific colliders. + /// + /// Unlike collision groups or the [`PhysicsHooks`](crate::pipeline::PhysicsHooks) + /// filter, this does not require the colliders to be attached to the same body or + /// connected by a joint: any pair of colliders can be disabled. Once disabled, the + /// narrow-phase will neither create nor maintain a contact/intersection pair for the + /// two colliders (existing pairs are cleared on the next step). Re-enable with + /// [`Self::enable_collision`]. + /// + /// This is symmetric: `disable_collision(a, b)` is equivalent to + /// `disable_collision(b, a)`. + pub fn disable_collision(&mut self, collider1: ColliderHandle, collider2: ColliderHandle) { + self.disabled_collider_pairs + .insert(Self::normalized_pair(collider1, collider2), ()); + } + + /// Re-enables collision detection between two specific colliders previously disabled + /// via [`Self::disable_collision`]. + pub fn enable_collision(&mut self, collider1: ColliderHandle, collider2: ColliderHandle) { + self.disabled_collider_pairs + .remove(&Self::normalized_pair(collider1, collider2)); + } + + /// Returns `true` if collision between the two given colliders is currently *enabled* + /// (i.e. not disabled via [`Self::disable_collision`]). + pub fn is_collision_enabled( + &self, + collider1: ColliderHandle, + collider2: ColliderHandle, + ) -> bool { + !self + .disabled_collider_pairs + .contains_key(&Self::normalized_pair(collider1, collider2)) + } + fn refresh_awake_body_mask(&mut self, islands: &IslandManager) { self.awake_body_mask.clear(); let len = islands diff --git a/src/geometry/narrow_phase/pair_management.rs b/src/geometry/narrow_phase/pair_management.rs index c8545b776..ebbec8ac7 100644 --- a/src/geometry/narrow_phase/pair_management.rs +++ b/src/geometry/narrow_phase/pair_management.rs @@ -4,7 +4,7 @@ use super::{ ColliderGraphIndices, NarrowPhase, PairRemovalMode, assign_pair_solver_color, - clear_pair_solver_color, + clear_filtered_pair, clear_pair_solver_color, }; use crate::alloc_prelude::*; use crate::dynamics::solver::solver_contact_graph::GraphPos; @@ -570,6 +570,27 @@ impl NarrowPhase { #[profiling::function] fn add_pair(&mut self, colliders: &ColliderSet, pair: &ColliderPair) { + // Explicit per-pair collision disabling (Phase B): never create a contact or + // intersection edge for a disabled pair, mirroring the joint-based + // `contacts_enabled` filter in `pair_update`. + if self + .disabled_collider_pairs + .contains_key(&ColliderPair::new( + if pair.collider1.0 <= pair.collider2.0 { + pair.collider1 + } else { + pair.collider2 + }, + if pair.collider1.0 <= pair.collider2.0 { + pair.collider2 + } else { + pair.collider1 + }, + )) + { + return; + } + if let (Some(co1), Some(co2)) = (colliders.get(pair.collider1), colliders.get(pair.collider2)) { @@ -670,6 +691,36 @@ impl NarrowPhase { broad_phase_events: &[BroadPhasePairEvent], events: &dyn EventHandler, ) { + // Phase B: clear any *existing* contact pair whose endpoints were explicitly + // disabled, regardless of whether its colliders were user-modified this step. + // `compute_contacts` only revisits pairs touching a modified collider, so a + // disabled pair between two settled bodies would otherwise keep its stale + // manifold forever. This runs over the whole live contact graph every step + // (cheap: only existing edges), mirroring the per-step joint `contacts_enabled` + // sweep done in `process_pair`. + if !self.disabled_collider_pairs.is_empty() { + let mut cleared_any = false; + for edge in self.contact_graph.graph.edges.iter_mut() { + let (c1, c2) = (edge.weight.collider1, edge.weight.collider2); + let key = if c1.0 <= c2.0 { + ColliderPair::new(c1, c2) + } else { + ColliderPair::new(c2, c1) + }; + if self.disabled_collider_pairs.contains_key(&key) { + clear_filtered_pair(&mut edge.weight); + cleared_any = true; + } + } + if cleared_any { + // `clear_filtered_pair` destroys the `graph_pos` back-references the + // incremental solver-graph reconcile relies on, so rebuild from scratch + // (same as `remove_pair`). + self.solver_graph_valid = false; + self.force_list_valid = false; + } + } + for event in broad_phase_events { match event { BroadPhasePairEvent::AddPair(pair) => { diff --git a/src/geometry/narrow_phase/pair_update.rs b/src/geometry/narrow_phase/pair_update.rs index b1f626c32..d328944bc 100644 --- a/src/geometry/narrow_phase/pair_update.rs +++ b/src/geometry/narrow_phase/pair_update.rs @@ -13,13 +13,14 @@ use crate::dynamics::{ RigidBodyType, }; use crate::geometry::{ - BoundingVolume, ColliderChanges, ColliderSet, ContactData, ContactManifoldData, ContactPair, - SolverContact, SolverFlags, + BoundingVolume, ColliderChanges, ColliderPair, ColliderSet, ContactData, ContactManifoldData, + ContactPair, SolverContact, SolverFlags, }; use crate::math::{MAX_MANIFOLD_POINTS, Real}; use crate::pipeline::{ActiveHooks, ContactModificationContext, PairFilterContext, PhysicsHooks}; use parry::query::PersistentQueryDispatcher; use parry::utils::PoseOpt; +use parry::utils::hashmap::HashMap; /// Raw pointer to the per-pair solver hints, shared across parallel update workers. /// Safety: only sound if each thread accesses a disjoint set of hint slots. @@ -61,6 +62,20 @@ pub(super) const OUTCOME_FULL_COMPOSITE: u8 = 5; // incremental reconcile needs, so the graph must be rebuilt from scratch (rare). pub(super) const OUTCOME_CLEARED_IN_GRAPH: u8 = 6; +/// How many of `pair`'s solver manifolds currently hold a solver-graph slot. +/// +/// A generator that *rebuilds* its manifold list (`manifolds.clear()` then re-push — the +/// voxel-ball path does this every step) hands back fresh `ContactManifoldData::default()`s, +/// zeroing `graph_pos`. That orphans the pair's existing graph entries: incremental +/// reconciliation diffs against `graph_pos`, so it can neither see nor evict them and +/// inserts duplicates instead. A drop in this count across an update detects the wipe. +fn num_in_solver_graph(pair: &ContactPair) -> usize { + pair.solver_manifolds() + .iter() + .filter(|m| m.data.graph_pos.is_some()) + .count() +} + /// The per-pair contact update shared by `NarrowPhase::compute_contacts`' /// single-threaded and parallel dispatch paths; returns an `OUTCOME_*` tag. #[allow(clippy::too_many_arguments)] @@ -80,6 +95,7 @@ pub(super) fn process_pair( query_dispatcher: &dyn PersistentQueryDispatcher, awake_body_mask: &[bool], hints_ptr: &HintsPtr, + disabled_pairs: &HashMap, #[cfg(not(feature = "parallel"))] transitions: &mut Vec, #[cfg(feature = "parallel")] snd: &std::sync::mpsc::Sender, ) -> u8 { @@ -171,6 +187,7 @@ pub(super) fn process_pair( } let had_any_active_contact = pair.has_any_active_contact(); + let prev_in_graph = num_in_solver_graph(pair); let rb_handle1 = co1.parent.map(|p| p.handle); let rb_handle2 = co2.parent.map(|p| p.handle); let mut outcome = OUTCOME_SKIPPED; @@ -233,6 +250,27 @@ pub(super) fn process_pair( } } + // Deal with collisions explicitly disabled between two specific colliders + // (Phase B), regardless of joints/groups/hooks. Mirrors the joint-based + // `contacts_enabled` check above. + if disabled_pairs.contains_key(&ColliderPair::new( + if pair.collider1.0 <= pair.collider2.0 { + pair.collider1 + } else { + pair.collider2 + }, + if pair.collider1.0 <= pair.collider2.0 { + pair.collider2 + } else { + pair.collider1 + }, + )) { + if clear_filtered_pair(pair) { + outcome = OUTCOME_CLEARED_IN_GRAPH; + } + break 'emit_events; + } + // Filter based on the rigid-body types. if !co1.flags.active_collision_types.test(rb_type1, rb_type2) && !co2.flags.active_collision_types.test(rb_type1, rb_type2) @@ -667,10 +705,16 @@ 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() { + // Unstable manifold ordinals force a full solver-graph rebuild, and there are two + // sources. Composite pairs (persistent workspace) rebuild in BVH order; workspace-less + // generators can still wipe the list wholesale — `contact_manifolds_voxels_ball` does, + // every step — which shows up as a drop in the slot count (see `num_in_solver_graph`). + // 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() + || (prev_in_graph != 0 && num_in_solver_graph(pair) < prev_in_graph)) + { return OUTCOME_FULL_COMPOSITE; } if outcome == OUTCOME_FULL && !membership_changed { diff --git a/src/geometry/narrow_phase/test.rs b/src/geometry/narrow_phase/test.rs index f1bef5297..8c4889786 100644 --- a/src/geometry/narrow_phase/test.rs +++ b/src/geometry/narrow_phase/test.rs @@ -282,3 +282,124 @@ pub fn collider_set_parent_no_self_intersection() { "There should be a contact manifold." ); } + +/// Phase B: explicit per-pair collision disabling between two specific colliders. +#[test] +pub fn disable_collision_between_specific_colliders() { + let mut rigid_body_set = RigidBodySet::new(); + let mut collider_set = ColliderSet::new(); + + /* Two overlapping dynamic balls attached to two distinct bodies. */ + let collider = ColliderBuilder::ball(0.5); + + let rigid_body_1 = RigidBodyBuilder::dynamic() + .translation(Vector::new(0.0, 0.0, 0.0)) + .build(); + let body_1_handle = rigid_body_set.insert(rigid_body_1); + let collider_1_handle = + collider_set.insert_with_parent(collider.build(), body_1_handle, &mut rigid_body_set); + + let rigid_body_2 = RigidBodyBuilder::dynamic() + .translation(Vector::new(0.2, 0.0, 0.0)) + .build(); + let body_2_handle = rigid_body_set.insert(rigid_body_2); + let collider_2_handle = + collider_set.insert_with_parent(collider.build(), body_2_handle, &mut rigid_body_set); + + let gravity = Vector::new(0.0, -9.81, 0.0); + let integration_parameters = IntegrationParameters::default(); + let mut physics_pipeline = PhysicsPipeline::new(); + let mut island_manager = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut impulse_joint_set = ImpulseJointSet::new(); + let mut multibody_joint_set = MultibodyJointSet::new(); + let mut ccd_solver = CCDSolver::new(); + let physics_hooks = (); + let event_handler = (); + + physics_pipeline.step( + gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut rigid_body_set, + &mut collider_set, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + &physics_hooks, + &event_handler, + ); + + // Collision is active by default between the two overlapping balls. + assert!( + narrow_phase + .contact_pair(collider_1_handle, collider_2_handle) + .is_some_and(|pair| pair.manifolds.len() == 1), + "Contact should exist before disabling." + ); + assert!( + narrow_phase.is_collision_enabled(collider_1_handle, collider_2_handle), + "is_collision_enabled must be true by default." + ); + + // Disable the specific pair. + narrow_phase.disable_collision(collider_1_handle, collider_2_handle); + assert!( + !narrow_phase.is_collision_enabled(collider_1_handle, collider_2_handle), + "is_collision_enabled must be false after disable_collision." + ); + + // Run a few steps: the contact pair must be cleared and never recreated. + for _ in 0..10 { + physics_pipeline.step( + gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut rigid_body_set, + &mut collider_set, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + &physics_hooks, + &event_handler, + ); + assert!( + narrow_phase + .contact_pair(collider_1_handle, collider_2_handle) + .is_none_or(|pair| pair.manifolds.is_empty()), + "Disabled contact pair must not produce manifolds." + ); + } + + // Re-enable: the contact pair must reappear. + narrow_phase.enable_collision(collider_1_handle, collider_2_handle); + assert!( + narrow_phase.is_collision_enabled(collider_1_handle, collider_2_handle), + "is_collision_enabled must be true after enable_collision." + ); + physics_pipeline.step( + gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut rigid_body_set, + &mut collider_set, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + &physics_hooks, + &event_handler, + ); + assert!( + narrow_phase + .contact_pair(collider_1_handle, collider_2_handle) + .is_some_and(|pair| pair.manifolds.len() == 1), + "Contact should be recreated after re-enabling." + ); +} diff --git a/src/geometry/user_index.rs b/src/geometry/user_index.rs new file mode 100644 index 000000000..f08bd0c34 --- /dev/null +++ b/src/geometry/user_index.rs @@ -0,0 +1,284 @@ +//! User-space axis-aligned bounding-box spatial index. +//! +//! This is a general-purpose, dependency-light R-tree over 3D AABBs that the +//! host application can populate with arbitrary `u64` entry ids and query by +//! AABB intersection. It is intentionally independent of the collision-detection +//! broad-phase: the engine's internal [`crate::geometry::BroadPhaseBvh`] serves +//! collider pairs, whereas this index is for user-managed spatial lookups +//! (e.g. game-world entity buckets, region queries) where the caller owns the +//! ids and the AABBs. +//! +//! The implementation mirrors a classic bulk-loaded R-tree: entries are kept in a +//! flat `Vec` and a balanced tree is (lazily) rebuilt whenever the set is +//! mutated. Queries prune whole subtrees whose bounds don't intersect the query +//! AABB. + +use crate::geometry::Aabb; +use crate::parry::bounding_volume::BoundingVolume; +use alloc::vec::Vec; +use parry::math::Vector; + +const MAX_CHILDREN: usize = 8; + +/// A single indexed AABB entry. +#[derive(Clone, Copy, Debug)] +struct Entry { + id: u64, + bounds: Aabb, +} + +#[derive(Clone, Debug)] +enum NodeKind { + Leaf(Vec), + Branch(Vec), +} + +#[derive(Clone, Debug)] +struct Node { + bounds: Aabb, + kind: NodeKind, +} + +/// A user-managed spatial index over 3D AABBs keyed by `u64` entry ids. +/// +/// Insertions and removals only mark the index dirty; the underlying tree is +/// rebuilt lazily on the next query (or on an explicit [`Self::rebuild`]). This +/// keeps burst mutations cheap at the cost of one rebuild per query burst. +#[derive(Clone, Debug)] +pub struct GenericAabbIndex { + entries: Vec, + root: Option, + dirty: bool, +} + +impl GenericAabbIndex { + /// Create an empty index. + pub fn new() -> Self { + Self { + entries: Vec::new(), + root: None, + dirty: false, + } + } + + /// Remove every entry from the index. + pub fn clear(&mut self) { + self.entries.clear(); + self.root = None; + self.dirty = false; + } + + /// Insert or overwrite the bounds of `id`. + /// + /// Returns `false` (and makes no change) if `id == 0`, which is reserved as + /// a sentinel. Capacity limits are the caller's responsibility; this index + /// itself is unbounded. + pub fn insert(&mut self, id: u64, bounds: Aabb) -> bool { + if id == 0 { + return false; + } + + if let Some(entry) = self.entries.iter_mut().find(|entry| entry.id == id) { + entry.bounds = bounds; + } else { + self.entries.push(Entry { id, bounds }); + } + self.dirty = true; + true + } + + /// Remove `id` from the index. Returns `true` if it was present. + pub fn remove(&mut self, id: u64) -> bool { + let Some(index) = self.entries.iter().position(|entry| entry.id == id) else { + return false; + }; + self.entries.swap_remove(index); + self.dirty = true; + true + } + + /// Number of entries currently stored. + pub fn len(&self) -> usize { + self.entries.len() + } + + /// Whether the index holds no entries. + pub fn is_empty(&self) -> bool { + self.entries.is_empty() + } + + /// Whether the index currently contains an entry with the given `id`. + pub fn contains(&self, id: u64) -> bool { + self.entries.iter().any(|entry| entry.id == id) + } + + /// Force an immediate rebuild of the tree structure. + pub fn rebuild(&mut self) { + self.rebuild_if_needed(); + } + + fn rebuild_if_needed(&mut self) { + if !self.dirty { + return; + } + self.root = build_node(&mut self.entries); + self.dirty = false; + } + + /// Count the entries whose bounds intersect `bounds`. + pub fn query_count(&mut self, bounds: Aabb) -> u32 { + self.rebuild_if_needed(); + let Some(root) = &self.root else { + return 0; + }; + count_node(root, bounds) + } + + /// Write the ids of entries whose bounds intersect `bounds` into `out_ids`. + /// + /// Returns the number of ids written (capped at `out_ids.len()`). + pub fn query(&mut self, bounds: Aabb, out_ids: &mut [u64]) -> u32 { + self.rebuild_if_needed(); + let Some(root) = &self.root else { + return 0; + }; + let mut written = 0usize; + query_node(root, bounds, out_ids, &mut written); + written as u32 + } +} + +impl Default for GenericAabbIndex { + fn default() -> Self { + Self::new() + } +} + +fn aabb_union(a: Aabb, b: Aabb) -> Aabb { + Aabb::new( + Vector::new( + a.mins.x.min(b.mins.x), + a.mins.y.min(b.mins.y), + a.mins.z.min(b.mins.z), + ), + Vector::new( + a.maxs.x.max(b.maxs.x), + a.maxs.y.max(b.maxs.y), + a.maxs.z.max(b.maxs.z), + ), + ) +} + +fn aabb_center_axis(b: &Aabb, axis: usize) -> f64 { + match axis { + 0 => (b.mins.x + b.maxs.x) * 0.5, + 1 => (b.mins.y + b.maxs.y) * 0.5, + _ => (b.mins.z + b.maxs.z) * 0.5, + } +} + +fn aabb_extent_axis(b: &Aabb, axis: usize) -> f64 { + match axis { + 0 => b.maxs.x - b.mins.x, + 1 => b.maxs.y - b.mins.y, + _ => b.maxs.z - b.mins.z, + } +} + +fn entries_bounds(entries: &[Entry]) -> Option { + let mut iter = entries.iter(); + let first = iter.next()?.bounds; + Some(iter.fold(first, |acc, entry| aabb_union(acc, entry.bounds))) +} + +fn nodes_bounds(nodes: &[Node]) -> Option { + let mut iter = nodes.iter(); + let first = iter.next()?.bounds; + Some(iter.fold(first, |acc, node| aabb_union(acc, node.bounds))) +} + +fn longest_axis(bounds: &Aabb) -> usize { + let x = aabb_extent_axis(bounds, 0); + let y = aabb_extent_axis(bounds, 1); + let z = aabb_extent_axis(bounds, 2); + if x >= y && x >= z { + 0 + } else if y >= z { + 1 + } else { + 2 + } +} + +fn build_node(entries: &mut [Entry]) -> Option { + let bounds = entries_bounds(entries)?; + if entries.len() <= MAX_CHILDREN { + return Some(Node { + bounds, + kind: NodeKind::Leaf(entries.to_vec()), + }); + } + + let axis = longest_axis(&bounds); + entries.sort_unstable_by(|a, b| { + aabb_center_axis(&a.bounds, axis) + .total_cmp(&aabb_center_axis(&b.bounds, axis)) + .then_with(|| a.id.cmp(&b.id)) + }); + + let child_count = entries.len().div_ceil(MAX_CHILDREN); + let mut children = Vec::with_capacity(child_count); + for chunk in entries.chunks_mut(MAX_CHILDREN) { + if let Some(child) = build_node(chunk) { + children.push(child); + } + } + + let bounds = nodes_bounds(&children)?; + Some(Node { + bounds, + kind: NodeKind::Branch(children), + }) +} + +fn count_node(node: &Node, bounds: Aabb) -> u32 { + if !node.bounds.intersects(&bounds) { + return 0; + } + + match &node.kind { + NodeKind::Leaf(entries) => entries + .iter() + .filter(|entry| entry.bounds.intersects(&bounds)) + .count() as u32, + NodeKind::Branch(children) => children + .iter() + .map(|child| count_node(child, bounds)) + .sum::(), + } +} + +fn query_node(node: &Node, bounds: Aabb, out_ids: &mut [u64], written: &mut usize) { + if *written >= out_ids.len() || !node.bounds.intersects(&bounds) { + return; + } + + match &node.kind { + NodeKind::Leaf(entries) => { + for entry in entries.iter() { + if *written >= out_ids.len() { + return; + } + if entry.bounds.intersects(&bounds) { + out_ids[*written] = entry.id; + *written += 1; + } + } + } + NodeKind::Branch(children) => { + for child in children { + query_node(child, bounds, out_ids, written); + } + } + } +} diff --git a/src/geometry/voxel_ball_tests.rs b/src/geometry/voxel_ball_tests.rs new file mode 100644 index 000000000..86df1cf3a --- /dev/null +++ b/src/geometry/voxel_ball_tests.rs @@ -0,0 +1,150 @@ +//! Regression tests for upstream issue #993 ("Voxel-ball bugs — ghost collisions and +//! non-collisions"). +//! +//! A ball fired at the edge/corner region of a block of solid voxels used to tunnel into +//! the block's interior: the ball-vs-voxels contact manifold is generated **per voxel**, +//! and each voxel projects the ball's center onto its own "pseudo-cube" independently. On +//! an octant whose feature is `INTERIOR` (the voxel face/edge is buried inside the solid +//! block) the projection returns `None`, so *no* contact is produced there. When the ball +//! straddles the boundary between an exposed voxel and its buried neighbour, the manifolds +//! that do get produced can point the wrong way (or vanish entirely for a step), letting +//! the ball slip past the surface and rattle around inside the block. +//! +//! The test below mirrors the TypeScript reproduction from the issue: a 3×3×3 block of unit +//! voxels spanning grid coordinates 5..=7, and a ball of radius 0.25 launched at its top +//! edge with the exact position/velocity from the report. After stepping, the ball must stay +//! *outside* the solid block — never inside its interior. + +use crate::alloc_prelude::*; +use crate::dynamics::{ + CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, + RigidBodyBuilder, RigidBodySet, +}; +use crate::geometry::{ColliderBuilder, ColliderSet, DefaultBroadPhase, NarrowPhase}; +use crate::math::Vector; +use crate::pipeline::PhysicsPipeline; +use parry::math::IVector; + +/// Builds the 3×3×3 block of solid voxels spanning grid coords `5..=7` on each axis, +/// exactly like the issue's `voxelData`. +fn issue_993_voxel_block() -> Vec { + let mut voxels = Vec::new(); + for y in 5..=7 { + for z in 5..=7 { + for x in 5..=7 { + voxels.push(IVector::new(x, y, z)); + } + } + } + voxels +} + +/// The solid block occupies world AABB [5, 8]³ (unit voxels, grid coord `i` spans +/// `[i, i+1]`). Returns true when `p` is strictly inside that box, inset by the ball +/// radius so "inside" means the ball's center got past the surface rather than merely +/// touching it. +fn is_inside_block(p: Vector, inset: f64) -> bool { + let lo = 5.0 + inset; + let hi = 8.0 - inset; + p.x > lo && p.x < hi && p.y > lo && p.y < hi && p.z > lo && p.z < hi +} + +/// Issue #993: a ball shot at the top edge of a solid voxel block must bounce off it, +/// not tunnel into the block's interior. +#[test] +fn ball_does_not_tunnel_into_voxel_block() { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + + // Static voxel terrain (unit voxels). + let voxels = issue_993_voxel_block(); + colliders.insert( + ColliderBuilder::voxels(Vector::new(1.0, 1.0, 1.0), &voxels) + .friction(0.8) + .restitution(0.0), + ); + + // Projectile: exact initial state from the issue's reproduction. + let projectile_radius = 0.25; + let rb = RigidBodyBuilder::dynamic() + .translation(Vector::new( + 5.068_835_807_168_076, + 10.666_715_670_475_918, + 10.803_566_521_990_922, + )) + .linvel(Vector::new( + 5.966_327_745_035_736, + -12.776_961_047_623_74, + -14.182_813_529_984_893, + )) + .ccd_enabled(true) + .build(); + let ball = bodies.insert(rb); + let ball_collider = colliders.insert_with_parent( + ColliderBuilder::ball(projectile_radius) + .density(1.0) + .friction(0.8) + .restitution(0.45), + ball, + &mut bodies, + ); + + let mut pipeline = PhysicsPipeline::new(); + let mut islands = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let mut ccd = CCDSolver::new(); + let params = IntegrationParameters { + dt: 1.0 / 60.0, + ..IntegrationParameters::default() + }; + let gravity = Vector::new(0.0, -10.0, 0.0); + + // The issue reports the ball fully inside the voxels after 15-20 ticks. + let start = bodies[ball].translation(); + let mut contact_ticks = 0usize; + for tick in 0..40 { + pipeline.step( + gravity, + ¶ms, + &mut islands, + &mut broad_phase, + &mut narrow_phase, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut ccd, + &(), + &(), + ); + + let p = bodies[ball].translation(); + assert!( + !is_inside_block(p, projectile_radius), + "issue #993: ball tunneled into the solid voxel block at tick {tick}: \ + position=({:.4}, {:.4}, {:.4})", + p.x, + p.y, + p.z + ); + if narrow_phase + .contact_pairs_with(ball_collider) + .any(|pair| pair.has_any_active_contact()) + { + contact_ticks += 1; + } + } + + // Sanity: the ball must actually collide with the voxel block, otherwise the assertion + // above would pass trivially (a ball that never arrives cannot tunnel). + assert!( + contact_ticks > 0, + "test is vacuous: ball never contacted the voxel block; started at ({:.3}, {:.3}, {:.3})", + start.x, + start.y, + start.z + ); +} diff --git a/src/lib.rs b/src/lib.rs index 6834fc34e..6ca4293f0 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -150,6 +150,8 @@ pub mod counters; pub mod data; pub mod dynamics; pub mod geometry; +/// Standalone dynamic linear-algebra layer (nalgebra replacement). See `src/linalg.rs`. +pub mod linalg; pub mod pipeline; pub mod utils; diff --git a/src/linalg.rs b/src/linalg.rs new file mode 100644 index 000000000..7ce417198 --- /dev/null +++ b/src/linalg.rs @@ -0,0 +1,2631 @@ +//! Standalone dynamic linear-algebra layer replacing `nalgebra` for rapier's runtime math. +//! +//! This module reproduces the subset of `nalgebra` used by rapier (`DVector`, `DMatrix`, +//! `Jacobian`, `LU`, views) with **bit-identical** floating-point behavior so that existing +//! simulations and `enhanced-determinism` saves remain reproducible after `nalgebra` is removed. +//! +//! Determinism strategy: every numeric kernel replicates `nalgebra`'s exact arithmetic order. +//! - `dot`/`axpy`/`component_mul` copy `nalgebra`'s unrolled loop bodies verbatim (indexing our +//! column-major buffer the same way `nalgebra` indexes its storage). +//! - `gemm`/`gemm_tr`/`tr_mul` delegate to the same `matrixmultiply` crate `nalgebra` uses +//! (only when at least one dimension is dynamic and all relevant dims > 5, matching +//! `nalgebra`'s `gemm_uninit` dispatch). +//! - `LU` (P4) is a verbatim copy of `nalgebra`'s `linalg/lu.rs`. +//! +//! Layout: vectors/matrices are stored **column-major** in a flat `Vec`, identical to +//! `nalgebra`'s `VecStorage`, so the copied loop bodies produce identical bits. +//! +//! Stride model: every matrix knows its `(nrows, ncols, col_stride)`. For a full matrix +//! `col_stride == nrows`. For a row-block view (`fixed_rows` / `rows_range`) the column stride +//! is **inherited from the parent** (a row block keeps the parent's inter-column distance), so +//! element `(i, j)` of a view with base offset `off` is at `data[off + i + j * col_stride]`. +//! This exactly mirrors `nalgebra`'s `RawStorage` strides and is what makes bit-identical `gemm` +//! over mixed-row matrices (e.g. a 3-row view × 6-row matrix × 3-row view) work. + +use crate::alloc_prelude::*; +use simba::scalar::ComplexField; +use std::ops::Deref; +use std::ops::DerefMut; + +/// A dynamically-sized column vector (`nalgebra::DVector` replacement). +/// +/// Stored column-major (a single column), so element `i` lives at `data[i]`. +#[derive(Clone, Debug, Default)] +pub struct DVector + Copy> { + /// Column-major element storage (a single column). + pub data: Vec, +} + +impl + Copy> DVector { + /// Creates a vector from a `Vec` of elements (column-major, single column). + #[inline] + pub fn from_vec(data: Vec) -> Self { + DVector { data } + } + + /// Creates a vector from a slice (cloned). + #[inline] + pub fn from_slice(slice: &[T]) -> Self { + DVector { + data: slice.to_vec(), + } + } + + /// Creates a zero vector of length `n`. + #[inline] + pub fn zeros(n: usize) -> Self { + DVector { + data: vec![T::zero(); n], + } + } + + /// Creates a vector filled with `value`. + #[inline] + pub fn from_element(n: usize, value: T) -> Self { + DVector { + data: vec![value; n], + } + } + + /// Number of rows (length). + #[inline] + pub fn nrows(&self) -> usize { + self.data.len() + } + + /// Number of columns (always 1 for a vector). + #[inline] + pub fn ncols(&self) -> usize { + 1 + } + + /// Total length. + #[inline] + pub fn len(&self) -> usize { + self.data.len() + } + + /// Whether the vector is empty. + #[inline] + pub fn is_empty(&self) -> bool { + self.data.is_empty() + } + + /// Immutable slice of the underlying data. + #[inline] + pub fn as_slice(&self) -> &[T] { + &self.data + } + + /// Mutable slice of the underlying data. + #[inline] + pub fn as_mut_slice(&mut self) -> &mut [T] { + &mut self.data + } + + /// Indexing (column vector → row index). + #[inline] + pub fn index(&self, i: usize) -> T { + self.data[i] + } + + /// Mutable indexing. + #[inline] + pub fn at_mut(&mut self, i: usize) -> &mut T { + &mut self.data[i] + } + + /// Returns a copy of the rows `first..first+n` as a new `DVector`. + #[inline] + pub fn rows(&self, first: usize, n: usize) -> DVectorView<'_, T> { + DVectorView { + data: &self.data[first..first + n], + } + } + + /// Returns a mutable view of the rows `first..first+n`. + #[inline] + pub fn rows_mut(&mut self, first: usize, n: usize) -> DVectorViewMut<'_, T> { + DVectorViewMut { + data: &mut self.data[first..first + n], + } + } + + /// Clones this vector (owned). + #[inline] + pub fn clone_owned(&self) -> DVector { + DVector { + data: self.data.clone(), + } + } + + /// Copies the content of `other` into `self` (same length). + #[inline] + pub fn copy_from(&mut self, other: &DVector) { + self.data.copy_from_slice(&other.data); + } + + /// Fills every element with `value`. + #[inline] + pub fn fill(&mut self, value: T) { + self.data.fill(value); + } + + /// Computes `self = alpha * a * x + beta * self`, bit-identical to `nalgebra`'s vector `gemv`. + /// `a` is a column-readable matrix (`ColAccess`), `x` a vector. Generic over matrix kind. + #[inline] + pub fn gemv>(&mut self, alpha: T, a: &M, x: &DVectorView, beta: T) { + let n = a.ncols(); + assert_eq!(a.nrows(), self.data.len(), "gemv: row mismatch"); + assert_eq!(n, x.data.len(), "gemv: col mismatch"); + for j in 0..n { + let a_col = a.col(j); + let c = x.data[j]; + // y = alpha * a_col * c + (j==0 ? beta : 1) * y (axcpy semantics) + let b = if j == 0 { beta } else { T::one() }; + axpy_col(&mut self.data, alpha, &a_col.data, c, b); + } + } + + /// Borrows this vector as an immutable view. + #[inline] + pub fn as_view(&self) -> DVectorView<'_, T> { + DVectorView { data: &self.data } + } + + /// Transpose (vector → 1×n matrix view). Implemented at the matrix level; here returns self. + #[inline] + pub fn transpose(&self) -> DVector { + DVector { + data: self.data.clone(), + } + } + + /// Builds a vector of length `n` where element `i` is `f(i)`. + #[inline] + pub fn from_fn(n: usize, mut f: impl FnMut(usize) -> T) -> DVector { + let data = (0..n).map(&mut f).collect(); + DVector { data } + } + + /// Returns an owned copy of rows `first..first+n`. + #[inline] + pub fn rows_owned(&self, first: usize, n: usize) -> DVector { + DVector { + data: self.data[first..first + n].to_vec(), + } + } +} + +impl + Copy> From> for DVector { + #[inline] + fn from(v: Vec) -> DVector { + DVector { data: v } + } +} + +impl + Copy> From<&[T]> for DVector { + #[inline] + fn from(v: &[T]) -> DVector { + DVector { data: v.to_vec() } + } +} + +impl + Copy + PartialOrd> DVector { + /// The infinity norm (max absolute value). + #[inline] + pub fn amax(&self) -> T { + let mut m = T::zero(); + for &e in &self.data { + let a = e.abs(); + if a > m { + m = a; + } + } + m + } +} + +impl + Copy + PartialOrd> DVector { + /// Euclidean norm. + #[inline] + pub fn norm(&self) -> T { + let mut s = T::zero(); + for &e in &self.data { + s += e * e; + } + s.sqrt() + } + + /// Normalized copy (falls back to zeros if norm is zero). + #[inline] + pub fn normalize(&self) -> DVector { + let n = self.norm(); + if n.is_zero() { + return DVector::zeros(self.data.len()); + } + let mut out = self.data.clone(); + for e in &mut out { + *e /= n; + } + DVector { data: out } + } +} + +impl + Copy> DVector { + /// Inserts `n` rows at index `i`, filling them with `value`. + #[inline] + pub fn insert_rows(&self, i: usize, n: usize, value: T) -> DVector { + let mut data = Vec::with_capacity(self.data.len() + n); + data.extend_from_slice(&self.data[..i]); + data.extend(std::iter::repeat_n(value, n)); + data.extend_from_slice(&self.data[i..]); + DVector { data } + } + + /// Returns the rows `first..` as an owned `DVector`. + #[inline] + pub fn index_range(&self, first: usize) -> DVector { + DVector { + data: self.data[first..].to_vec(), + } + } + + /// Dot product, bit-identical to `nalgebra::DVector::dot`. + /// + /// Copy of `nalgebra`'s `dotx` unrolled loop (blas.rs). `conjugate` is identity for real `T`. + #[inline] + pub fn dot(&self, rhs: &DVector) -> T { + let n = self.data.len(); + assert_eq!(n, rhs.data.len(), "Dot product dimension mismatch."); + + // NOTE: for a dynamic vector (`Dyn` rows) nalgebra does NOT take the U2/U3/U4 + // special cases; it always uses the general unrolled loop below. We replicate that + // exactly (the tail loop gives a sequential sum for n < 8). + let mut res = T::zero(); + let mut i = 0; + let mut acc0 = T::zero(); + let mut acc1 = T::zero(); + let mut acc2 = T::zero(); + let mut acc3 = T::zero(); + let mut acc4 = T::zero(); + let mut acc5 = T::zero(); + let mut acc6 = T::zero(); + let mut acc7 = T::zero(); + + while n - i >= 8 { + acc0 += self.data[i] * rhs.data[i]; + acc1 += self.data[i + 1] * rhs.data[i + 1]; + acc2 += self.data[i + 2] * rhs.data[i + 2]; + acc3 += self.data[i + 3] * rhs.data[i + 3]; + acc4 += self.data[i + 4] * rhs.data[i + 4]; + acc5 += self.data[i + 5] * rhs.data[i + 5]; + acc6 += self.data[i + 6] * rhs.data[i + 6]; + acc7 += self.data[i + 7] * rhs.data[i + 7]; + i += 8; + } + + res += acc0 + acc4; + res += acc1 + acc5; + res += acc2 + acc6; + res += acc3 + acc7; + + for k in i..n { + res += self.data[k] * rhs.data[k]; + } + + res + } + + /// Element-wise multiply, bit-identical to `nalgebra`'s `component_mul`. + #[inline] + pub fn component_mul(&self, other: &DVector) -> DVector { + assert_eq!( + self.data.len(), + other.data.len(), + "Component-wise multiply dimension mismatch." + ); + let data = self + .data + .iter() + .zip(&other.data) + .map(|(a, b)| *a * *b) + .collect(); + DVector { data } + } + + /// Computes `self = a * x + b * self` (axpy), bit-identical to `nalgebra`'s `axpy`. + #[inline] + pub fn axpy(&mut self, a: T, x: &DVector, b: T) { + assert_eq!(self.data.len(), x.data.len(), "Axpy dimension mismatch."); + if b.is_zero() { + for (s, &xi) in self.data.iter_mut().zip(&x.data) { + *s = a * xi; + } + } else { + for (s, &xi) in self.data.iter_mut().zip(&x.data) { + *s = a * xi + b * *s; + } + } + } + + /// Computes `self = alpha * a.transpose() * x + beta * self`, bit-identical to + /// `nalgebra`'s vector `gemv_tr`. `a` is a column-readable matrix (`ColAccess`). + #[inline] + pub fn gemv_tr>(&mut self, alpha: T, a: &M, x: &DVectorView, beta: T) { + let mut v = DVectorViewMut { + data: self.as_mut_slice(), + }; + v.gemv_tr(alpha, a, x, beta); + } +} + +/// Immutable view into a contiguous slice of a `DVector` (replaces `na::DVectorView`). +#[derive(Clone, Copy, Debug)] +pub struct DVectorView<'a, T: ComplexField + Copy> { + /// Borrowed slice of the viewed elements. + pub data: &'a [T], +} + +impl<'a, T: ComplexField + Copy + PartialOrd> DVectorView<'a, T> { + /// Number of rows. + #[inline] + pub fn nrows(&self) -> usize { + self.data.len() + } + + /// Column count (1). + #[inline] + pub fn ncols(&self) -> usize { + 1 + } + + /// Length. + #[inline] + pub fn len(&self) -> usize { + self.data.len() + } + + /// Whether the view is empty. + #[inline] + pub fn is_empty(&self) -> bool { + self.data.is_empty() + } + + /// Immutable slice. + #[inline] + pub fn as_slice(&self) -> &[T] { + self.data + } + + /// Dot product against another vector. + #[inline] + pub fn dot(&self, rhs: &DVector) -> T { + DVector::from_slice(self.data).dot(rhs) + } + + /// Clones into an owned `DVector`. + #[inline] + pub fn into_owned(self) -> DVector { + DVector::from_slice(self.data) + } + + /// Copies this view's content into `dst`. + #[inline] + pub fn copy_to(&self, dst: &mut DVector) { + dst.data.copy_from_slice(self.data); + } +} + +impl + Copy> std::ops::Index for DVectorView<'_, T> { + type Output = T; + #[inline] + fn index(&self, i: usize) -> &T { + &self.data[i] + } +} + +/// Mutable view into a contiguous slice of a `DVector` (replaces `na::DVectorViewMut`). +#[derive(Debug)] +pub struct DVectorViewMut<'a, T: ComplexField + Copy> { + /// Mutably borrowed slice of the viewed elements. + pub data: &'a mut [T], +} + +impl<'a, T: ComplexField + Copy> DVectorViewMut<'a, T> { + /// Number of rows. + #[inline] + pub fn nrows(&self) -> usize { + self.data.len() + } + + /// Column count (1). + #[inline] + pub fn ncols(&self) -> usize { + 1 + } + + /// Length. + #[inline] + pub fn len(&self) -> usize { + self.data.len() + } + + /// Whether the view is empty. + #[inline] + pub fn is_empty(&self) -> bool { + self.data.is_empty() + } + + /// Immutable slice. + #[inline] + pub fn as_slice(&self) -> &[T] { + self.data + } + + /// Mutable slice. + #[inline] + pub fn as_mut_slice(&mut self) -> &mut [T] { + self.data + } + + /// Copies `src` into this view. + #[inline] + pub fn copy_from(&mut self, src: &DVector) { + self.data.copy_from_slice(&src.data); + } + + /// Copies this view's content into `dst`. + #[inline] + pub fn copy_to(&self, dst: &mut DVector) { + dst.data.copy_from_slice(self.data); + } + + /// Computes `self = a * x + b * self` (axpy), bit-identical to `nalgebra`'s `axpy`. + #[inline] + pub fn axpy(&mut self, a: T, x: &DVector, b: T) { + assert_eq!(self.data.len(), x.data.len(), "Axpy dimension mismatch."); + if b.is_zero() { + for (s, &xi) in self.data.iter_mut().zip(&x.data) { + *s = a * xi; + } + } else { + for (s, &xi) in self.data.iter_mut().zip(&x.data) { + *s = a * xi + b * *s; + } + } + } + + /// Computes `self = alpha * a.transpose() * x + beta * self`, bit-identical to + /// `nalgebra`'s vector `gemv_tr`. `a` is a column-readable matrix (`ColAccess`). + #[inline] + pub fn gemv_tr>(&mut self, alpha: T, a: &M, x: &DVectorView, beta: T) { + let n = a.ncols(); + assert_eq!(a.nrows(), x.data.len(), "gemv_tr: row mismatch"); + assert_eq!(n, self.data.len(), "gemv_tr: col mismatch"); + for j in 0..n { + let d = a.col(j).dot(&DVector::from_slice(x.data)); + if !beta.is_zero() { + self.data[j] = alpha * d + beta * self.data[j]; + } else { + self.data[j] = alpha * d; + } + } + } + + /// Euclidean norm. + #[inline] + pub fn norm(&self) -> T { + let mut s = T::zero(); + for &e in self.data.iter() { + s += e * e; + } + s.sqrt() + } + + /// Squared Euclidean norm. + #[inline] + pub fn norm_squared(&self) -> T { + let mut s = T::zero(); + for &e in self.data.iter() { + s += e * e; + } + s + } + + /// Fills every element with `value`. + #[inline] + pub fn fill(&mut self, value: T) { + self.data.fill(value); + } + + /// Returns a sub-view of rows `r0..` as a mutable vector view. + #[inline] + pub fn rows_range_mut(&mut self, r0: usize) -> DVectorViewMut<'_, T> { + DVectorViewMut { + data: &mut self.data[r0..], + } + } + + /// Element-wise multiply-assign `self[i] *= other[i]`. + #[inline] + pub fn component_mul_assign(&mut self, other: &DVector) { + assert_eq!(self.data.len(), other.data.len()); + for i in 0..self.data.len() { + self.data[i] *= other.data[i]; + } + } +} + +impl + Copy> std::ops::Index for DVectorViewMut<'_, T> { + type Output = T; + #[inline] + fn index(&self, i: usize) -> &T { + &self.data[i] + } +} + +impl + Copy> std::ops::IndexMut for DVectorViewMut<'_, T> { + #[inline] + fn index_mut(&mut self, i: usize) -> &mut T { + &mut self.data[i] + } +} + +/// A dynamically-sized matrix stored **column-major** in a flat `Vec` with an explicit +/// column stride. +/// +/// Indexing: element `(i, j)` (row `i`, column `j`) lives at `data[i + j * col_stride]`. +/// For a full matrix `col_stride == nrows`, matching `nalgebra`'s `VecStorage` layout. For a +/// row-block view the column stride is inherited from the parent so numeric kernels produce +/// identical bits to `nalgebra`. +#[derive(Clone, Debug, Default)] +pub struct DMatrix + Copy> { + /// Column-major element storage. + pub data: Vec, + /// Number of rows. + pub nrows: usize, + /// Number of columns. + pub ncols: usize, + /// Distance (in elements) between element `(i, j)` and `(i, j+1)`. + pub col_stride: usize, +} + +impl + Copy> DMatrix { + /// Creates a zero `nrows × ncols` matrix (column stride = `nrows`). + #[inline] + pub fn zeros(nrows: usize, ncols: usize) -> Self { + DMatrix { + data: vec![T::zero(); nrows * ncols], + nrows, + ncols, + col_stride: nrows, + } + } + + /// Creates a matrix from row-major data (`nalgebra`'s `from_row_slice` semantics). + #[inline] + pub fn from_row_slice(nrows: usize, ncols: usize, slice: &[T]) -> Self { + let mut data = vec![T::zero(); nrows * ncols]; + for i in 0..nrows { + for j in 0..ncols { + data[i + j * nrows] = slice[i * ncols + j]; + } + } + DMatrix { + data, + nrows, + ncols, + col_stride: nrows, + } + } + + /// Column-major raw pointer (for `matrixmultiply` delegation). + #[inline] + pub fn as_ptr(&self) -> *const T { + self.data.as_ptr() + } + + /// Column-major raw mutable pointer. + #[inline] + pub fn as_mut_ptr(&mut self) -> *mut T { + self.data.as_mut_ptr() + } + + /// Raw strides `(row_stride, col_stride)` for `matrixmultiply` delegation. + #[inline] + pub fn strides(&self) -> (usize, usize) { + (1, self.col_stride) + } + + /// Returns a copy of column `j`. + #[inline] + pub fn column(&self, j: usize) -> DVector { + let start = j * self.col_stride; + DVector { + data: self.data[start..start + self.nrows].to_vec(), + } + } + + /// Returns a mutable view of column `j`. + #[inline] + pub fn column_mut(&mut self, j: usize) -> DVectorViewMut<'_, T> { + let start = j * self.col_stride; + DVectorViewMut { + data: &mut self.data[start..start + self.nrows], + } + } + + /// Fills every element with `value`. + #[inline] + pub fn fill(&mut self, value: T) { + self.data.fill(value); + } + + /// Copies the content of `other` (same dimensions) into `self`. + #[inline] + pub fn copy_from(&mut self, other: &DMatrix) { + assert_eq!( + (self.nrows, self.ncols), + (other.nrows, other.ncols), + "copy_from dimension mismatch" + ); + for j in 0..self.ncols { + for i in 0..self.nrows { + let dst = i + j * self.col_stride; + let src = i + j * other.col_stride; + self.data[dst] = other.data[src]; + } + } + } + + /// Returns an owned clone. + #[inline] + pub fn clone_owned(&self) -> DMatrix { + DMatrix { + data: self.data.clone(), + nrows: self.nrows, + ncols: self.ncols, + col_stride: self.col_stride, + } + } + + /// Transpose (bit-identical to `nalgebra::Matrix::transpose`). + #[inline] + pub fn transpose(&self) -> DMatrix { + let mut data = vec![T::zero(); self.nrows * self.ncols]; + for j in 0..self.ncols { + for i in 0..self.nrows { + data[j + i * self.ncols] = self.data[i + j * self.col_stride]; + } + } + DMatrix { + data, + nrows: self.ncols, + ncols: self.nrows, + col_stride: self.ncols, + } + } + + /// Immutable element access, matching nalgebra's `matrix[(i, j)]`. + #[inline] + pub fn get(&self, i: usize, j: usize) -> T { + self.data[i + j * self.col_stride] + } + + /// Mutable element access. + #[inline] + pub fn get_mut(&mut self, i: usize, j: usize) -> &mut T { + &mut self.data[i + j * self.col_stride] + } + + /// Swaps elements `(i0, j0)` and `(i1, j1)` in place. + #[inline] + pub fn swap(&mut self, (i0, j0): (usize, usize), (i1, j1): (usize, usize)) { + self.data + .swap(i0 + j0 * self.col_stride, i1 + j1 * self.col_stride); + } + + /// Whether the matrix is square. + #[inline] + pub fn is_square(&self) -> bool { + self.nrows == self.ncols + } + + /// Returns a mutable view of columns `first..first+n`. + #[inline] + pub fn columns_mut(&mut self, first: usize, n: usize) -> MatrixViewMut<'_, T> { + MatrixViewMut { + data: &mut self.data[first * self.col_stride..(first + n) * self.col_stride], + nrows: self.nrows, + ncols: n, + col_stride: self.col_stride, + } + } + + /// Copies `src` (length `nrows`) into column `j`. + #[inline] + pub fn set_column(&mut self, j: usize, src: &DVector) { + let start = j * self.col_stride; + self.data[start..start + self.nrows].copy_from_slice(&src.data); + } + + /// Returns an immutable view of columns `first..first+n`. + #[inline] + pub fn columns(&self, first: usize, n: usize) -> MatrixView<'_, T> { + MatrixView { + data: &self.data[first * self.col_stride..(first + n) * self.col_stride], + nrows: self.nrows, + ncols: n, + col_stride: self.col_stride, + } + } + + /// Returns the `L` rows starting at row `first` (shifting the column-major data by `first`). + /// Matches nalgebra's `fixed_rows::(first)`. + #[inline] + pub fn fixed_rows(&self, first: usize) -> MatrixView<'_, T> { + let ncols = self.ncols; + MatrixView { + data: &self.data[first..first + L + (ncols - 1) * self.col_stride], + nrows: L, + ncols, + col_stride: self.col_stride, + } + } + + /// Mutable `L` rows block starting at `first` (column stride inherited from parent). + #[inline] + pub fn fixed_rows_mut(&mut self, first: usize) -> MatrixViewMut<'_, T> { + let ncols = self.ncols; + MatrixViewMut { + data: &mut self.data[first..first + L + (ncols - 1) * self.col_stride], + nrows: L, + ncols, + col_stride: self.col_stride, + } + } + + /// Returns the rows `r0..r0+len` as a new matrix view (column stride inherited). + /// Matches nalgebra's `rows_range(r0..r0+len)`. + #[inline] + pub fn rows_range(&self, r0: usize, len: usize) -> MatrixView<'_, T> { + let ncols = self.ncols; + MatrixView { + data: &self.data[r0..r0 + len + (ncols - 1) * self.col_stride], + nrows: len, + ncols, + col_stride: self.col_stride, + } + } + + /// Mutable `rows_range` view. + #[inline] + pub fn rows_range_mut(&mut self, r0: usize, len: usize) -> MatrixViewMut<'_, T> { + let ncols = self.ncols; + MatrixViewMut { + data: &mut self.data[r0..r0 + len + (ncols - 1) * self.col_stride], + nrows: len, + ncols, + col_stride: self.col_stride, + } + } + + /// Returns a pair of mutable row-range views `(rows 0..n0, rows n0..n0+n1)`. + /// Matches nalgebra's `rows_range_pair_mut(0..n0, n0..n0+n1)`. The two views partition the + /// row axis (disjoint element sets) while sharing the parent's column stride. + #[inline] + pub fn rows_range_pair_mut( + &mut self, + n0: usize, + n1: usize, + ) -> (MatrixViewMut<'_, T>, MatrixViewMut<'_, T>) { + let ncols = self.ncols; + let cs = self.col_stride; + let left_end = n0 + (ncols - 1) * cs; + let right = &mut self.data[n0..]; + let (left, right) = right.split_at_mut(left_end - n0); + ( + MatrixViewMut { + data: left, + nrows: n0, + ncols, + col_stride: cs, + }, + MatrixViewMut { + data: &mut right[..n1 + (ncols - 1) * cs], + nrows: n1, + ncols, + col_stride: cs, + }, + ) + } + + /// Computes `self = alpha * a * b + beta * self`, bit-identical to `nalgebra`'s `gemm`. + /// + /// Replicates `nalgebra`'s exact dispatch (blas_uninit.rs): + /// - large dynamic matrices → `matrixmultiply::dgemm` with column-major strides; + /// - otherwise → per-column `gemv`/`axcpy` loop. + #[inline] + pub fn gemm, Mb: MatrixLike>( + &mut self, + alpha: T, + a: &Ma, + b: &Mb, + beta: T, + ) { + let nrows1 = self.nrows; + let ncols1 = self.ncols; + let nrows2 = a.nrows(); + let ncols2 = a.ncols(); + let nrows3 = b.nrows(); + let ncols3 = b.ncols(); + + assert_eq!( + ncols2, nrows3, + "gemm: dimensions mismatch for multiplication." + ); + assert_eq!( + (nrows1, ncols1), + (nrows2, ncols3), + "gemm: dimensions mismatch for addition." + ); + + const SMALL_DIM: usize = 5; + let large = + nrows1 > SMALL_DIM && ncols1 > SMALL_DIM && nrows2 > SMALL_DIM && ncols2 > SMALL_DIM; + + if large { + // matrixmultiply path (identical to nalgebra for f64). Strides come from the views. + let (rsa, csa) = a.strides(); + let (rsb, csb) = b.strides(); + let (rsc, csc) = self.strides(); + unsafe { + matrixmultiply::dgemm( + nrows2, + ncols2, + ncols3, + std::mem::transmute_copy(&alpha), + a.as_ptr() as *const f64, + rsa as isize, + csa as isize, + b.as_ptr() as *const f64, + rsb as isize, + csb as isize, + std::mem::transmute_copy(&beta), + self.as_mut_ptr() as *mut f64, + rsc as isize, + csc as isize, + ); + } + return; + } + + // Small/static fallback: per-column gemv, matching nalgebra's gemv_uninit/axcpy. + // NOTE: nalgebra passes the original `beta` to *every* column's gemv; the within-column + // first-iteration special-casing (beta only for j==0 of the inner loop, `1` thereafter) + // is handled inside gemv_column. + for j1 in 0..ncols1 { + let mut y = self.column_mut(j1); + let bcol = b.col(j1); + gemv_column(&mut y, alpha, a, &bcol.as_view(), beta); + } + } + + /// Rank-1 update `self += alpha * x * y^T + beta * self`, bit-identical to `nalgebra::ger`. + /// `x` has `self.nrows` elements, `y` has `self.ncols` elements. + #[inline] + pub fn ger(&mut self, alpha: T, x: &DVector, y: &DVector, beta: T) { + assert_eq!(self.nrows, x.len(), "ger: x dimension mismatch"); + assert_eq!(self.ncols, y.len(), "ger: y dimension mismatch"); + for j in 0..self.ncols { + let val = alpha * y.data[j]; + let mut col = self.column_mut(j); + col.axpy(val, x, beta); + } + } + + /// Computes `self = alpha * rhs.transpose() * mid * rhs + beta * self` (quadratic form), + /// bit-identical to `nalgebra::quadform`. + #[inline] + pub fn quadform>(&mut self, alpha: T, mid: &M, rhs: &M, beta: T) { + let dim = mid.nrows(); + assert_eq!(mid.ncols(), dim, "quadform: mid must be square"); + assert_eq!(rhs.nrows(), dim, "quadform: rhs rows must match mid"); + assert_eq!( + self.nrows, + rhs.ncols(), + "quadform: self rows must match rhs cols" + ); + assert_eq!( + self.ncols, + rhs.ncols(), + "quadform: self cols must match rhs cols" + ); + + let mut work = DVector::zeros(dim); + for j in 0..rhs.ncols() { + work.gemv(T::one(), mid, &rhs.col(j).as_view(), T::zero()); + self.column_mut(j).gemv_tr( + alpha, + rhs, + &work.as_view(), + if j == 0 { beta } else { T::one() }, + ); + } + } + + /// Equivalent to `self.transpose() * rhs` but stores the result (vector) into `out`. + /// Bit-identical to `nalgebra::tr_mul_to` for the matrix × vector case. + #[inline] + pub fn tr_mul_to(&self, rhs: &DVectorView, out: &mut DVector) { + assert_eq!(self.nrows, rhs.data.len(), "tr_mul_to: row mismatch"); + assert_eq!(self.ncols, out.len(), "tr_mul_to: out mismatch"); + out.gemv_tr(T::one(), self, rhs, T::zero()); + } + + /// Computes `self = alpha * a.transpose() * b + beta * self`, bit-identical to + /// `nalgebra::gemm_tr`. + #[inline] + pub fn gemm_tr, Mb: MatrixLike>( + &mut self, + alpha: T, + a: &Ma, + b: &Mb, + beta: T, + ) { + let nrows1 = self.nrows; + let ncols1 = self.ncols; + let nrows2 = a.nrows(); + let ncols2 = a.ncols(); + let nrows3 = b.nrows(); + let ncols3 = b.ncols(); + + assert_eq!( + nrows2, nrows3, + "gemm_tr: dimensions mismatch for multiplication." + ); + assert_eq!( + (nrows1, ncols1), + (ncols2, ncols3), + "gemm_tr: dimensions mismatch for addition." + ); + + for j1 in 0..ncols1 { + let mut y = self.column_mut(j1); + let bcol = b.col(j1); + gemv_tr_column(&mut y, alpha, a, &bcol.as_view(), beta); + } + } + + /// Returns an immutable view of the block starting at `(first_row, first_col)` with + /// dimensions `(nrows, ncols)`. Matches nalgebra's `matrix.view((r, c), (nr, nc))`. + #[inline] + pub fn view(&self, (r, c): (usize, usize), (nr, nc): (usize, usize)) -> MatrixView<'_, T> { + MatrixView { + data: &self.data[r + c * self.col_stride + ..r + c * self.col_stride + nr + (nc - 1) * self.col_stride], + nrows: nr, + ncols: nc, + col_stride: self.col_stride, + } + } + + /// Returns a mutable view of the block starting at `(first_row, first_col)` with + /// dimensions `(nrows, ncols)`. + #[inline] + pub fn view_mut( + &mut self, + (r, c): (usize, usize), + (nr, nc): (usize, usize), + ) -> MatrixViewMut<'_, T> { + MatrixViewMut { + data: &mut self.data[r + c * self.col_stride + ..r + c * self.col_stride + nr + (nc - 1) * self.col_stride], + nrows: nr, + ncols: nc, + col_stride: self.col_stride, + } + } + + /// Returns a mutable view of rows `first..first+n` (as a row-block of this matrix). + #[inline] + pub fn rows_mut_block(&mut self, first: usize, n: usize) -> MatrixViewMut<'_, T> { + MatrixViewMut { + data: &mut self.data[first..first + n + (self.ncols - 1) * self.col_stride], + nrows: n, + ncols: self.ncols, + col_stride: self.col_stride, + } + } + + /// Returns a mutable view of the columns in `range` (e.g. `start..` or `a..b`). + #[inline] + pub fn columns_range_mut(&mut self, range: std::ops::Range) -> MatrixViewMut<'_, T> { + let n = range.end - range.start; + MatrixViewMut { + data: &mut self.data + [range.start * self.col_stride..(range.start + n) * self.col_stride], + nrows: self.nrows, + ncols: n, + col_stride: self.col_stride, + } + } +} + +impl + Copy> std::ops::Index<(usize, usize)> for DMatrix { + type Output = T; + #[inline] + fn index(&self, (i, j): (usize, usize)) -> &T { + &self.data[i + j * self.col_stride] + } +} + +impl + Copy> std::ops::IndexMut<(usize, usize)> for DMatrix { + #[inline] + fn index_mut(&mut self, (i, j): (usize, usize)) -> &mut T { + &mut self.data[i + j * self.col_stride] + } +} + +/// A fixed-size `R × C` matrix, stored **column-major** (column stride = `R`). +/// +/// This is the `nalgebra::SMatrix` replacement used by `multibody.rs` for the +/// `SPATIAL_DIM × SPATIAL_DIM` rigid-body mass matrix and `tmp` temporaries. It wraps a +/// `DMatrix` so all matrix kernels (gemm, views, etc.) work uniformly; `Deref`/`DerefMut` +/// expose the inner `DMatrix` directly. +#[derive(Clone, Debug, Default)] +pub struct SMatrix + Copy, const R: usize, const C: usize> { + /// Inner `R × C` column-major matrix (column stride = `R`). + pub matrix: DMatrix, +} + +impl + Copy, const R: usize, const C: usize> SMatrix { + /// Creates a zero `R × C` matrix. + #[inline] + pub fn zeros() -> Self { + SMatrix { + matrix: DMatrix::zeros(R, C), + } + } + + /// Builds an `R × C` matrix from a row-major slice (matches `nalgebra::SMatrix::from_row_slice`). + #[inline] + pub fn from_row_slice(slice: &[T]) -> Self { + assert_eq!(R * C, slice.len(), "from_row_slice: length mismatch"); + SMatrix { + matrix: DMatrix::from_row_slice(R, C, slice), + } + } + + /// Number of rows. + #[inline] + pub fn nrows(&self) -> usize { + R + } + + /// Number of columns. + #[inline] + pub fn ncols(&self) -> usize { + C + } +} + +impl + Copy, const R: usize, const C: usize> Deref + for SMatrix +{ + type Target = DMatrix; + #[inline] + fn deref(&self) -> &DMatrix { + &self.matrix + } +} + +impl + Copy, const R: usize, const C: usize> DerefMut + for SMatrix +{ + #[inline] + fn deref_mut(&mut self) -> &mut DMatrix { + &mut self.matrix + } +} + +/// A matrix with a **dynamic row count** and dynamic column count, column-major with stride. +/// +/// This is the `nalgebra::OMatrix` replacement (`R` supplied at runtime, not as a +/// type param, so `gemm` between differently-sized matrices works). It is just a `DMatrix`. +pub type OMatrix = DMatrix; + +/// A constraint Jacobian: a matrix with **dynamic row count** (`SPATIAL_DIM` for the spatial +/// twist, but supplied at runtime) and **dynamic columns** (one per generalized DoF). +/// +/// This is the `nalgebra::MatrixNxX` (`na::Matrix3xX` / `na::Matrix6xX`) replacement. It is +/// simply `DMatrix` (dynamic nrows, dynamic ncols, column stride = nrows), identical to +/// nalgebra's layout. +pub type Jacobian = DMatrix; + +/// Immutable view into a column-major sub-matrix (full or strided). +#[derive(Clone, Copy, Debug)] +pub struct MatrixView<'a, T: ComplexField + Copy> { + /// Borrowed column-major slice. + pub data: &'a [T], + /// Number of rows. + pub nrows: usize, + /// Number of columns. + pub ncols: usize, + /// Distance (in elements) between element `(i, j)` and `(i, j+1)`. + pub col_stride: usize, +} + +impl<'a, T: ComplexField + Copy> MatrixView<'a, T> { + /// Number of rows. + #[inline] + pub fn nrows(&self) -> usize { + self.nrows + } + + /// Number of columns. + #[inline] + pub fn ncols(&self) -> usize { + self.ncols + } + + /// Returns a copy of column `j`. + #[inline] + pub fn column(&self, j: usize) -> DVector { + let start = j * self.col_stride; + DVector { + data: self.data[start..start + self.nrows].to_vec(), + } + } + + /// Returns the rows `first..first+len` as a `DVector` view. + #[inline] + pub fn rows(&self, first: usize, len: usize) -> DVectorView<'_, T> { + DVectorView { + data: &self.data[first..first + len], + } + } + + /// Returns the `L` rows starting at row `first` (shifting the column-major data by `first`), + /// column stride inherited from this view. Matches nalgebra's `fixed_rows::(first)`. + #[inline] + pub fn fixed_rows(&self, first: usize) -> MatrixView<'_, T> { + let ncols = self.ncols; + MatrixView { + data: &self.data[first..first + L + (ncols - 1) * self.col_stride], + nrows: L, + ncols, + col_stride: self.col_stride, + } + } + + /// Returns a dense owned copy (compact, column stride = `nrows`). + #[inline] + pub fn into_owned(&self) -> DMatrix { + let mut data = Vec::with_capacity(self.nrows * self.ncols); + for j in 0..self.ncols { + for i in 0..self.nrows { + data.push(self.data[i + j * self.col_stride]); + } + } + DMatrix { + data, + nrows: self.nrows, + ncols: self.ncols, + col_stride: self.nrows, + } + } +} + +impl<'a, T: ComplexField + Copy> std::ops::Index<(usize, usize)> + for MatrixView<'a, T> +{ + type Output = T; + #[inline] + fn index(&self, (i, j): (usize, usize)) -> &T { + &self.data[i + j * self.col_stride] + } +} + +impl<'a, T: ComplexField + Copy> std::ops::IndexMut<(usize, usize)> + for MatrixViewMut<'a, T> +{ + #[inline] + fn index_mut(&mut self, (i, j): (usize, usize)) -> &mut T { + &mut self.data[i + j * self.col_stride] + } +} + +impl<'a, T: ComplexField + Copy> std::ops::Index<(usize, usize)> + for MatrixViewMut<'a, T> +{ + type Output = T; + #[inline] + fn index(&self, (i, j): (usize, usize)) -> &T { + &self.data[i + j * self.col_stride] + } +} + +/// Mutable view into a column-major sub-matrix (full or strided). +#[derive(Debug)] +pub struct MatrixViewMut<'a, T: ComplexField + Copy> { + /// Borrowed column-major slice. + pub data: &'a mut [T], + /// Number of rows. + pub nrows: usize, + /// Number of columns. + pub ncols: usize, + /// Distance (in elements) between element `(i, j)` and `(i, j+1)`. + pub col_stride: usize, +} + +impl<'a, T: ComplexField + Copy> MatrixViewMut<'a, T> { + /// Number of rows. + #[inline] + pub fn nrows(&self) -> usize { + self.nrows + } + + /// Number of columns. + #[inline] + pub fn ncols(&self) -> usize { + self.ncols + } + + /// Element read. + #[inline] + pub fn get(&self, i: usize, j: usize) -> T { + self.data[i + j * self.col_stride] + } + + /// Returns a mutable view of column `j`. + #[inline] + pub fn column_mut(&mut self, j: usize) -> DVectorViewMut<'_, T> { + let start = j * self.col_stride; + DVectorViewMut { + data: &mut self.data[start..start + self.nrows], + } + } + + /// Mutable `L` rows block starting at `first` (column stride inherited). Matches + /// nalgebra's `fixed_rows_mut::(first)`. + #[inline] + pub fn fixed_rows_mut(&mut self, first: usize) -> MatrixViewMut<'_, T> { + let ncols = self.ncols; + MatrixViewMut { + data: &mut self.data[first..first + L + (ncols - 1) * self.col_stride], + nrows: L, + ncols, + col_stride: self.col_stride, + } + } + + /// Returns a dense owned copy (compact, column stride = `nrows`). + #[inline] + pub fn into_owned(&self) -> DMatrix { + let mut data = Vec::with_capacity(self.nrows * self.ncols); + for j in 0..self.ncols { + for i in 0..self.nrows { + data.push(self.data[i + j * self.col_stride]); + } + } + DMatrix { + data, + nrows: self.nrows, + ncols: self.ncols, + col_stride: self.nrows, + } + } + + /// Copies the content of `other` (same dimensions) into `self`. + #[inline] + pub fn copy_from(&mut self, other: &MatrixView<'_, T>) { + assert_eq!( + (self.nrows, self.ncols), + (other.nrows, other.ncols), + "copy_from dimension mismatch" + ); + for j in 0..self.ncols { + for i in 0..self.nrows { + self.data[i + j * self.col_stride] = other.data[i + j * other.col_stride]; + } + } + } + + /// Copies the content of `other` (same dimensions) into `self`. + #[inline] + pub fn copy_from_dmatrix(&mut self, other: &DMatrix) { + assert_eq!( + (self.nrows, self.ncols), + (other.nrows, other.ncols), + "copy_from dimension mismatch" + ); + for j in 0..self.ncols { + for i in 0..self.nrows { + self.data[i + j * self.col_stride] = other.data[i + j * other.col_stride]; + } + } + } + + /// Fills every element with `value`. + #[inline] + pub fn fill(&mut self, value: T) { + for e in self.data.iter_mut() { + *e = value; + } + } + + /// Element-wise add-assign `rhs` into `self`, matching nalgebra's `zip_apply(o, x, |o, x| *o = x)`. + #[inline] + pub fn zip_apply(&mut self, rhs: &MatrixView<'_, T>, mut f: impl FnMut(&mut T, T)) { + assert_eq!( + (self.nrows, self.ncols, self.col_stride), + (rhs.nrows, rhs.ncols, rhs.col_stride), + "zip_apply dimension mismatch" + ); + for j in 0..self.ncols { + for i in 0..self.nrows { + let di = i + j * self.col_stride; + let si = i + j * rhs.col_stride; + f(&mut self.data[di], rhs.data[si]); + } + } + } + + /// Swaps elements `(i0, j0)` and `(i1, j1)` in place. + #[inline] + pub fn swap(&mut self, (i0, j0): (usize, usize), (i1, j1): (usize, usize)) { + self.data + .swap(i0 + j0 * self.col_stride, i1 + j1 * self.col_stride); + } + + /// Returns a mutable view of rows `first..first+n` (as a row-block of this view). + #[inline] + pub fn rows_mut(&mut self, first: usize, n: usize) -> MatrixViewMut<'_, T> { + MatrixViewMut { + data: &mut self.data[first..first + n + (self.ncols - 1) * self.col_stride], + nrows: n, + ncols: self.ncols, + col_stride: self.col_stride, + } + } + + /// Computes `self = alpha * a * b + beta * self`, bit-identical to `nalgebra::gemm`. + #[inline] + pub fn gemm, Mb: MatrixLike>( + &mut self, + alpha: T, + a: &Ma, + b: &Mb, + beta: T, + ) { + let nrows1 = self.nrows; + let ncols1 = self.ncols; + let nrows2 = a.nrows(); + let ncols2 = a.ncols(); + let nrows3 = b.nrows(); + let ncols3 = b.ncols(); + + assert_eq!( + ncols2, nrows3, + "gemm: dimensions mismatch for multiplication." + ); + assert_eq!( + (nrows1, ncols1), + (nrows2, ncols3), + "gemm: dimensions mismatch for addition." + ); + + const SMALL_DIM: usize = 5; + let large = + nrows1 > SMALL_DIM && ncols1 > SMALL_DIM && nrows2 > SMALL_DIM && ncols2 > SMALL_DIM; + + if large { + let (rsa, csa) = a.strides(); + let (rsb, csb) = b.strides(); + let (rsc, csc) = (1usize, self.col_stride); + unsafe { + matrixmultiply::dgemm( + nrows2, + ncols2, + ncols3, + std::mem::transmute_copy(&alpha), + a.as_ptr() as *const f64, + rsa as isize, + csa as isize, + b.as_ptr() as *const f64, + rsb as isize, + csb as isize, + std::mem::transmute_copy(&beta), + self.data.as_mut_ptr() as *mut f64, + rsc as isize, + csc as isize, + ); + } + return; + } + + for j1 in 0..ncols1 { + let mut y = self.column_mut(j1); + let bcol = b.col(j1); + gemv_column(&mut y, alpha, a, &bcol.as_view(), beta); + } + } + + /// Rank-1 update `self += alpha * x * y^T + beta * self`, bit-identical to `nalgebra::ger`. + /// `x` has `self.nrows` elements, `y` has `self.ncols` elements. + #[inline] + pub fn ger(&mut self, alpha: T, x: &DVector, y: &DVector, beta: T) { + assert_eq!(self.nrows, x.len(), "ger: x dimension mismatch"); + assert_eq!(self.ncols, y.len(), "ger: y dimension mismatch"); + for j in 0..self.ncols { + let val = alpha * y.data[j]; + let mut col = self.column_mut(j); + col.axpy(val, x, beta); + } + } + + /// Computes `self = alpha * rhs.transpose() * mid * rhs + beta * self` (quadratic form), + /// bit-identical to `nalgebra::quadform`. + #[inline] + pub fn quadform>(&mut self, alpha: T, mid: &M, rhs: &M, beta: T) { + let dim = mid.nrows(); + assert_eq!(mid.ncols(), dim, "quadform: mid must be square"); + assert_eq!(rhs.nrows(), dim, "quadform: rhs rows must match mid"); + assert_eq!( + self.nrows, + rhs.ncols(), + "quadform: self rows must match rhs cols" + ); + assert_eq!( + self.ncols, + rhs.ncols(), + "quadform: self cols must match rhs cols" + ); + + let mut work = DVector::zeros(dim); + for j in 0..rhs.ncols() { + work.gemv(T::one(), mid, &rhs.col(j).as_view(), T::zero()); + self.column_mut(j).gemv_tr( + alpha, + rhs, + &work.as_view(), + if j == 0 { beta } else { T::one() }, + ); + } + } + + /// Equivalent to `self.transpose() * rhs` but stores the result (vector) into `out`. + /// Bit-identical to `nalgebra::tr_mul_to` for the matrix × vector case. + #[inline] + pub fn tr_mul_to(&self, rhs: &DVectorView, out: &mut DVector) { + assert_eq!(self.nrows, rhs.data.len(), "tr_mul_to: row mismatch"); + assert_eq!(self.ncols, out.len(), "tr_mul_to: out mismatch"); + out.gemv_tr(T::one(), self, rhs, T::zero()); + } + + /// Computes `self = alpha * a.transpose() * b + beta * self`, bit-identical to + /// `nalgebra::gemm_tr`. + #[inline] + pub fn gemm_tr, Mb: MatrixLike>( + &mut self, + alpha: T, + a: &Ma, + b: &Mb, + beta: T, + ) { + let nrows1 = self.nrows; + let ncols1 = self.ncols; + let nrows2 = a.nrows(); + let ncols2 = a.ncols(); + let nrows3 = b.nrows(); + let ncols3 = b.ncols(); + + assert_eq!( + nrows2, nrows3, + "gemm_tr: dimensions mismatch for multiplication." + ); + assert_eq!( + (nrows1, ncols1), + (ncols2, ncols3), + "gemm_tr: dimensions mismatch for addition." + ); + + for j1 in 0..ncols1 { + let mut y = self.column_mut(j1); + let bcol = b.col(j1); + gemv_tr_column(&mut y, alpha, a, &bcol.as_view(), beta); + } + } +} + +/// Trait abstracting "a matrix I can read columns from with a column stride", so `gemv`/ +/// `gemv_tr`/`gemm` work uniformly over `DMatrix`/`OMatrix`/`Jacobian`/`SMatrix`/`MatrixView`/ +/// `MatrixViewMut` (mirrors nalgebra's `Storage`). +pub trait MatrixLike + Copy> { + /// Number of rows. + fn nrows(&self) -> usize; + /// Number of columns. + fn ncols(&self) -> usize; + /// Returns column `j` as an owned `DVector`. + fn col(&self, j: usize) -> DVector; + /// Raw column-major pointer (for `matrixmultiply`). + fn as_ptr(&self) -> *const T; + /// Raw strides `(row_stride, col_stride)` (for `matrixmultiply`). + fn strides(&self) -> (usize, usize); +} + +impl + Copy> MatrixLike for DMatrix { + #[inline] + fn nrows(&self) -> usize { + self.nrows + } + #[inline] + fn ncols(&self) -> usize { + self.ncols + } + #[inline] + fn col(&self, j: usize) -> DVector { + self.column(j) + } + #[inline] + fn as_ptr(&self) -> *const T { + self.data.as_ptr() + } + #[inline] + fn strides(&self) -> (usize, usize) { + (1, self.col_stride) + } +} + +impl + Copy> MatrixLike for MatrixView<'_, T> { + #[inline] + fn nrows(&self) -> usize { + self.nrows + } + #[inline] + fn ncols(&self) -> usize { + self.ncols + } + #[inline] + fn col(&self, j: usize) -> DVector { + self.column(j) + } + #[inline] + fn as_ptr(&self) -> *const T { + self.data.as_ptr() + } + #[inline] + fn strides(&self) -> (usize, usize) { + (1, self.col_stride) + } +} + +impl + Copy> MatrixLike for MatrixViewMut<'_, T> { + #[inline] + fn nrows(&self) -> usize { + self.nrows + } + #[inline] + fn ncols(&self) -> usize { + self.ncols + } + #[inline] + fn col(&self, j: usize) -> DVector { + let start = j * self.col_stride; + DVector { + data: self.data[start..start + self.nrows].to_vec(), + } + } + #[inline] + fn as_ptr(&self) -> *const T { + self.data.as_ptr() + } + #[inline] + fn strides(&self) -> (usize, usize) { + (1, self.col_stride) + } +} + +impl + Copy, const R: usize, const C: usize> MatrixLike + for SMatrix +{ + #[inline] + fn nrows(&self) -> usize { + R + } + #[inline] + fn ncols(&self) -> usize { + C + } + #[inline] + fn col(&self, j: usize) -> DVector { + self.column(j) + } + #[inline] + fn as_ptr(&self) -> *const T { + self.data.as_ptr() + } + #[inline] + fn strides(&self) -> (usize, usize) { + (1, R) + } +} + +/// `ColAccess` keeps `DVector::gemv`/`gemv_tr` working (it only needs `nrows`/`ncols`/`col`). +pub trait ColAccess + Copy> { + /// Number of rows. + fn nrows(&self) -> usize; + /// Number of columns. + fn ncols(&self) -> usize; + /// Returns column `j` as an owned `DVector`. + fn col(&self, j: usize) -> DVector; +} + +impl + Copy, M: MatrixLike> ColAccess for M { + #[inline] + fn nrows(&self) -> usize { + MatrixLike::nrows(self) + } + #[inline] + fn ncols(&self) -> usize { + MatrixLike::ncols(self) + } + #[inline] + fn col(&self, j: usize) -> DVector { + MatrixLike::col(self, j) + } +} + +/// `y = a * x * c + b * y`. +/// If `b` is zero, `y` is never read from (matching nalgebra's `array_axc`/`array_axcpy`). +#[inline] +fn axpy_col + Copy>(y: &mut [T], alpha: T, x: &[T], c: T, b: T) { + if !b.is_zero() { + for i in 0..x.len() { + y[i] = alpha * x[i] * c + b * y[i]; + } + } else { + for i in 0..x.len() { + y[i] = alpha * x[i] * c; + } + } +} + +/// `y = alpha * a * x + beta * y`, matching nalgebra's `gemv_uninit`/`axcpy_uninit`. +#[inline] +fn gemv_column + Copy, M: MatrixLike>( + y: &mut DVectorViewMut<'_, T>, + alpha: T, + a: &M, + x: &DVectorView, + beta: T, +) { + let ncols2 = a.ncols(); + assert_eq!(a.nrows(), y.data.len(), "gemv: row mismatch"); + assert_eq!(ncols2, x.data.len(), "gemv: col mismatch"); + + for j in 0..ncols2 { + let a_col = a.col(j); + let c = x.data[j]; + // y = alpha * a_col * c + (j==0 ? beta : 1) * y (axcpy semantics) + let b = if j == 0 { beta } else { T::one() }; + axpy_col(y.data, alpha, &a_col.data, c, b); + } +} + +/// `y = alpha * a.transpose() * x + beta * y`, matching nalgebra's `gemv_tr_uninit`. +#[inline] +fn gemv_tr_column + Copy, M: MatrixLike>( + y: &mut DVectorViewMut<'_, T>, + alpha: T, + a: &M, + x: &DVectorView, + beta: T, +) { + let ncols2 = a.ncols(); + assert_eq!(a.nrows(), x.data.len(), "gemv_tr: row mismatch"); + assert_eq!(ncols2, y.data.len(), "gemv_tr: col mismatch"); + + for j in 0..ncols2 { + let a_col = a.col(j); + let d = a_col.dot(&DVector::from_slice(x.data)); + if !beta.is_zero() { + y.data[j] = alpha * d + beta * y.data[j]; + } else { + y.data[j] = alpha * d; + } + } +} + +/// A sequence of row permutations (matching `nalgebra::PermutationSequence` semantics). +/// +/// Stores at most `len` recorded swaps `(i, i2)`; `permute_rows` applies them in order, +/// `determinant` is `+1` for an even number of swaps and `-1` otherwise. +#[derive(Clone, Debug, Default)] +pub struct PermutationSequence { + len: usize, + ipiv: Vec<(usize, usize)>, +} + +impl PermutationSequence { + /// Creates an empty (identity) permutation sequence of capacity `n`. + #[inline] + pub fn identity(n: usize) -> Self { + PermutationSequence { + len: 0, + ipiv: vec![(0usize, 0usize); n], + } + } + + /// Records the interchange of rows `i` and `i2` (no-op if `i == i2`). + #[inline] + pub fn append_permutation(&mut self, i: usize, i2: usize) { + if i != i2 { + assert!( + self.len < self.ipiv.len(), + "Maximum number of permutations exceeded." + ); + self.ipiv[self.len] = (i, i2); + self.len += 1; + } + } + + /// Applies this sequence of permutations to the rows of `rhs` (in recorded order). + #[inline] + pub fn permute_rows + Copy + PartialOrd>( + &self, + rhs: &mut MatrixViewMut<'_, T>, + ) { + for k in 0..self.len { + let (i0, i1) = self.ipiv[k]; + for j in 0..rhs.ncols { + rhs.swap((i0, j), (i1, j)); + } + } + } + + /// The determinant contribution of this permutation (+1 even / -1 odd count). + #[inline] + pub fn determinant + Copy>(&self) -> T { + if self.len % 2 == 0 { + T::one() + } else { + -T::one() + } + } +} + +/// LU decomposition with partial (row) pivoting (`nalgebra::LU` replacement). +/// +/// Stored as the combined `lu` matrix (L's strictly-lower part overwritten with U's multipliers, +/// matching `nalgebra`'s in-place layout) plus the row-permutation sequence `p`. The decomposition +/// arithmetic is a **verbatim** copy of `nalgebra`'s `linalg/lu.rs` so results are bit-identical. +#[derive(Clone, Debug)] +pub struct LU + Copy> { + /// Combined lower/upper factor (in-place, column-major). + pub lu: DMatrix, + /// Row permutation sequence. + pub p: PermutationSequence, +} + +/// A right-hand side that an `LU` decomposition can solve into (vector or matrix view). +pub trait SolveTarget + Copy> { + /// Exposes the target as a mutable column-major view for in-place solve. + fn as_view_mut(&mut self) -> MatrixViewMut<'_, T>; +} + +impl + Copy> SolveTarget for DMatrix { + #[inline] + fn as_view_mut(&mut self) -> MatrixViewMut<'_, T> { + MatrixViewMut { + data: &mut self.data, + nrows: self.nrows, + ncols: self.ncols, + col_stride: self.col_stride, + } + } +} + +impl + Copy> SolveTarget for DVector { + #[inline] + fn as_view_mut(&mut self) -> MatrixViewMut<'_, T> { + let n = self.data.len(); + MatrixViewMut { + data: &mut self.data, + nrows: n, + ncols: 1, + col_stride: n, + } + } +} + +impl<'a, T: ComplexField + Copy> SolveTarget for DVectorViewMut<'a, T> { + #[inline] + fn as_view_mut(&mut self) -> MatrixViewMut<'_, T> { + let n = self.data.len(); + MatrixViewMut { + data: self.data, + nrows: n, + ncols: 1, + col_stride: n, + } + } +} + +impl<'a, T: ComplexField + Copy> SolveTarget for MatrixViewMut<'a, T> { + #[inline] + fn as_view_mut(&mut self) -> MatrixViewMut<'_, T> { + MatrixViewMut { + data: self.data, + nrows: self.nrows, + ncols: self.ncols, + col_stride: self.col_stride, + } + } +} + +impl + Copy + PartialOrd> LU { + /// Computes the LU decomposition with partial (row) pivoting of `matrix`. + pub fn new(mut matrix: DMatrix) -> Self { + let min_n = matrix.nrows.min(matrix.ncols); + let mut p = PermutationSequence::identity(min_n); + + if min_n == 0 { + return LU { lu: matrix, p }; + } + + for i in 0..min_n { + let piv = matrix.icamax(i, i); + let diag = matrix.get(piv, i); + + if diag.is_zero() { + // No non-zero entry on this column; leave the row as-is. + continue; + } + + if piv != i { + p.append_permutation(i, piv); + // Swap rows `i` and `piv` in the already-processed columns `..i` + // (nalgebra: `matrix.columns_range_mut(..i).swap_rows(i, piv)`). + for j in 0..i { + matrix.swap((i, j), (piv, j)); + } + gauss_step_swap(&mut matrix, diag, i, piv); + } else { + gauss_step(&mut matrix, diag, i); + } + } + + LU { lu: matrix, p } + } + + /// Solves `self * x = b` in place. Returns `false` if the matrix is not invertible. + pub fn solve_mut>(&self, b: &mut B) -> bool { + let mut bv = b.as_view_mut(); + assert_eq!( + self.lu.nrows, bv.nrows, + "LU solve matrix dimension mismatch." + ); + assert!( + self.lu.is_square(), + "LU solve: unable to solve a non-square system." + ); + + self.p.permute_rows(&mut bv); + let _ = solve_lower_triangular_with_diag_mut(&self.lu, &mut bv, T::one()); + solve_upper_triangular_mut(&self.lu, &mut bv) + } + + /// Solves `self * x = b`, returning `None` if not invertible. + pub fn solve(&self, b: &DMatrix) -> Option> { + let mut res = b.clone_owned(); + if self.solve_mut(&mut res) { + Some(res) + } else { + None + } + } + + /// Computes the inverse of the decomposed matrix. Returns `None` if not invertible. + pub fn try_inverse(&self) -> Option> { + assert!( + self.lu.is_square(), + "LU inverse: unable to compute the inverse of a non-square matrix." + ); + let dim = self.lu.nrows; + let mut res = DMatrix::zeros(dim, dim); + res.fill_with_identity(); + if self.try_inverse_to(&mut res) { + Some(res) + } else { + None + } + } + + /// Computes the inverse into `out`. Returns `false` if not invertible. + pub fn try_inverse_to(&self, out: &mut DMatrix) -> bool { + assert!( + self.lu.is_square(), + "LU inverse: unable to compute the inverse of a non-square matrix." + ); + assert_eq!( + self.lu.shape(), + out.shape(), + "LU inverse: mismatched output shape." + ); + out.fill_with_identity(); + self.solve_mut(out) + } + + /// The determinant of the decomposed matrix. + pub fn to_determinant(&self) -> T { + let dim = self.lu.nrows; + assert!( + self.lu.is_square(), + "LU determinant: unable to compute the determinant of a non-square matrix." + ); + let mut res = T::one(); + for i in 0..dim { + res *= self.lu.get(i, i); + } + res * self.p.determinant() + } +} + +impl + Copy> DMatrix { + /// Sets `self` to the identity matrix (diagonal `1`, off-diagonal `0`). + #[inline] + pub fn fill_with_identity(&mut self) { + self.fill(T::zero()); + let d = self.nrows.min(self.ncols); + for i in 0..d { + self.data[i + i * self.col_stride] = T::one(); + } + } + + /// Returns `(nrows, ncols)`. + #[inline] + pub fn shape(&self) -> (usize, usize) { + (self.nrows, self.ncols) + } +} + +/// One Gaussian-elimination step on the i-th row/column (no row swap). The diagonal `diag` is +/// provided. Verbatim mirror of `nalgebra`'s `gauss_step`: `coeffs` (the pivot column, rows 1..) +/// is scaled by `inv_diag` in place (storing the L multipliers in column `i`), then every column +/// to the right is eliminated against it. +pub fn gauss_step + Copy>( + matrix: &mut DMatrix, + diag: T, + i: usize, +) { + let nrows = matrix.nrows; + let ncols = matrix.ncols; + let inv_diag = T::one() / diag; + + // Store the L multipliers `matrix[(r, i)] / diag` back into column `i` (nalgebra's + // `coeffs *= inv_diag` mutates the underlying storage in place). + for r in 1..(nrows - i) { + let v = matrix.get(i + r, i) * inv_diag; + *matrix.get_mut(i + r, i) = v; + } + + for k in 1..(ncols - i) { + let pj = matrix.get(i, i + k); + for r in 1..(nrows - i) { + let cr = matrix.get(i + r, i); + let cur = matrix.get(i + r, i + k); + matrix.data[(i + r) + (i + k) * matrix.col_stride] = -pj * cr + cur; + } + } +} + +/// Gaussian-elimination step with a prior row swap (`piv` is the absolute pivot row). Verbatim +/// mirror of `nalgebra`'s `gauss_step_swap`. +pub fn gauss_step_swap + Copy>( + matrix: &mut DMatrix, + diag: T, + i: usize, + piv: usize, +) { + let nrows = matrix.nrows; + let ncols = matrix.ncols; + let inv_diag = T::one() / diag; + + // coeffs.swap((0, 0), (pk, 0)) on the submatrix (which starts at row `i`): `pk = piv - i` + // is the local pivot, so submatrix row `pk` is matrix row `piv` (absolute). Swaps + // matrix[(i, i)] (the pivot element) with matrix[(piv, i)]. + matrix.swap((i, i), (piv, i)); + + // `coeffs *= inv_diag` stores the L multipliers into column `i` (rows 1..). + for r in 1..(nrows - i) { + let v = matrix.get(i + r, i) * inv_diag; + *matrix.get_mut(i + r, i) = v; + } + + // pivot_row[k] <-> down[(pk-1, k)] for columns `i+1 .. ncols` (column `i` is `coeffs`, + // already handled above — do NOT touch it here). `down[(pk-1, k)]` is submatrix row `pk` + // = matrix row `piv` (absolute). + for k in 0..(ncols - i - 1) { + matrix.swap((i, i + 1 + k), (piv, i + 1 + k)); + } + + for k in 0..(ncols - i - 1) { + let pj = matrix.get(i, i + 1 + k); + for r in 1..(nrows - i) { + let cr = matrix.get(i + r, i); + let cur = matrix.get(i + r, i + 1 + k); + matrix.data[(i + r) + (i + 1 + k) * matrix.col_stride] = -pj * cr + cur; + } + } +} + +/// Solves `self * x = b` where only the lower-triangular part (with the given `diag`) is used. +/// Returns `false` if `diag` is zero. Verbatim mirror of `nalgebra`'s +/// `solve_lower_triangular_with_diag_mut`. +pub fn solve_lower_triangular_with_diag_mut + Copy>( + lu: &DMatrix, + b: &mut MatrixViewMut<'_, T>, + diag: T, +) -> bool { + if diag.is_zero() { + return false; + } + let dim = lu.nrows; + let cols = b.ncols; + for k in 0..cols { + for i in 0..dim - 1 { + let coeff = b.get(i, k) / diag; + for r in (i + 1)..dim { + let pivot = lu.get(r, i); + let cur = b.get(r, k); + b.data[r + k * b.col_stride] = cur - coeff * pivot; + } + } + } + true +} + +/// Solves `self * x = b` where only the upper-triangular part (incl. diagonal) is used. +/// Returns `false` if any diagonal element is zero. Verbatim mirror of `nalgebra`'s +/// `solve_upper_triangular_mut`. +pub fn solve_upper_triangular_mut + Copy>( + lu: &DMatrix, + b: &mut MatrixViewMut<'_, T>, +) -> bool { + let dim = lu.nrows; + let cols = b.ncols; + for k in 0..cols { + for i in (0..dim).rev() { + let d = lu.get(i, i); + if d.is_zero() { + return false; + } + let coeff = b.get(i, k) / d; + b.data[i + k * b.col_stride] = coeff; + for r in 0..i { + let pivot = lu.get(r, i); + let cur = b.get(r, k); + b.data[r + k * b.col_stride] = cur - coeff * pivot; + } + } + } + true +} + +impl + Copy + PartialOrd> DMatrix { + /// Index of the element with the largest absolute value in column `j` starting at row `r0`, + /// returned as the absolute row index (matching nalgebra's `view_range(r0.., j).icamax()`). + /// Strict `>` so the **first** maximal element wins (matching nalgebra's `icamax`). + fn icamax(&self, r0: usize, j: usize) -> usize { + let mut best_row = r0; + let mut best = self.get(r0, j).abs(); + for k in (r0 + 1)..self.nrows { + let v = self.get(k, j).abs(); + if v > best { + best = v; + best_row = k; + } + } + best_row + } +} + +impl + Copy> std::ops::Add for DMatrix { + type Output = DMatrix; + #[inline] + fn add(mut self, rhs: DMatrix) -> DMatrix { + self += rhs; + self + } +} + +impl + Copy> std::ops::AddAssign for DMatrix { + #[inline] + fn add_assign(&mut self, rhs: DMatrix) { + assert_eq!( + (self.nrows, self.ncols, self.col_stride), + (rhs.nrows, rhs.ncols, rhs.col_stride), + "AddAssign dimension/stride mismatch" + ); + for i in 0..self.data.len() { + self.data[i] += rhs.data[i]; + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use nalgebra::DVector as NaDVector; + + fn approx_eq_bits(a: f64, b: f64) -> bool { + a.to_bits() == b.to_bits() + } + + #[test] + fn dvector_dot_bit_identical_to_nalgebra() { + let data: Vec = (0..137).map(|i| (i as f64) * 1.137 + 0.25).collect(); + let data2: Vec = (0..137).map(|i| (i as f64) * 0.731 - 0.5).collect(); + + let mine = DVector::from_vec(data.clone()); + let na = NaDVector::from_vec(data.clone()); + let mine2 = DVector::from_vec(data2.clone()); + let na2 = NaDVector::from_vec(data2.clone()); + + let rd = mine.dot(&mine2); + let rna = na.dot(&na2); + assert!( + approx_eq_bits(rd, rna), + "dot mismatch: mine={:?} na={:?}", + rd, + rna + ); + + // various lengths (incl. < 8 tail-sum path) + for n in [1usize, 2, 3, 4, 5, 7, 8, 9, 64, 137] { + let a: Vec = (0..n).map(|i| i as f64 * 0.3 + 1.0).collect(); + let b: Vec = (0..n).map(|i| i as f64 * 0.7 - 0.2).collect(); + let m = DVector::from_vec(a.clone()).dot(&DVector::from_vec(b.clone())); + let nref = NaDVector::from_vec(a).dot(&NaDVector::from_vec(b)); + assert!( + approx_eq_bits(m, nref), + "dot n={} mismatch: {} vs {}", + n, + m, + nref + ); + } + } + + #[test] + fn dvector_component_mul_bit_identical() { + let a: Vec = (0..100).map(|i| (i as f64) * 0.91 + 0.1).collect(); + let b: Vec = (0..100).map(|i| (i as f64) * 0.37 - 0.4).collect(); + let m = DVector::from_vec(a.clone()).component_mul(&DVector::from_vec(b.clone())); + let nref = NaDVector::from_vec(a).component_mul(&NaDVector::from_vec(b)); + for (x, y) in m.data.iter().zip(nref.iter()) { + assert!( + approx_eq_bits(*x, *y), + "component_mul mismatch {} vs {}", + x, + y + ); + } + } + + #[test] + fn dvector_axpy_bit_identical() { + for (a, b) in [(2.0, 3.0), (1.0, 0.0), (0.0, 1.0), (5.5, -2.0)] { + let x: Vec = (0..60).map(|i| (i as f64) * 0.13 + 0.7).collect(); + let mut s: Vec = (0..60).map(|i| (i as f64) * 0.29 - 0.3).collect(); + let mut mine = DVector::from_vec(s.clone()); + let na_x = NaDVector::from_vec(x.clone()); + let mut na_s = NaDVector::from_vec(s.clone()); + mine.axpy(a, &DVector::from_vec(x.clone()), b); + na_s.axpy(a, &na_x, b); + for (m, n) in mine.data.iter().zip(na_s.iter()) { + assert!(approx_eq_bits(*m, *n), "axpy mismatch {} vs {}", m, n); + } + let _ = &mut s; + } + } + + #[test] + fn dmatrix_gemm_bit_identical() { + use nalgebra::DMatrix as NaDMatrix; + + // exercise both paths: large (matrixmultiply) and small (gemv loop) + for (nr, nc, mk) in [ + (10usize, 10, 10), + (4, 4, 4), + (7, 3, 5), + (6, 6, 6), + (20, 20, 20), + ] { + let a: Vec = (0..nr * mk).map(|i| (i as f64) * 0.137 + 0.5).collect(); + let b: Vec = (0..mk * nc).map(|i| (i as f64) * 0.731 - 0.3).collect(); + + for (alpha, beta) in [(1.0, 0.0), (2.0, 1.0), (1.0, 1.0), (0.5, -1.0)] { + // C starts zero (beta=0 path) or random (beta!=0 path) + let c0: Vec = (0..nr * nc).map(|i| (i as f64) * 0.21 - 0.7).collect(); + + let mut mine = DMatrix::from_row_slice(nr, nc, &c0); + let na_c0 = c0.clone(); + let mut na_res = NaDMatrix::from_row_slice(nr, nc, &na_c0); + let na_a = NaDMatrix::from_row_slice(nr, mk, &a); + let na_b = NaDMatrix::from_row_slice(mk, nc, &b); + + mine.gemm( + alpha, + &DMatrix::from_row_slice(nr, mk, &a), + &DMatrix::from_row_slice(mk, nc, &b), + beta, + ); + na_res.gemm(alpha, &na_a, &na_b, beta); + + for (m, n) in mine.data.iter().zip(na_res.iter()) { + assert!( + approx_eq_bits(*m, *n), + "gemm ({},{},{}) a={}b={} mismatch: {} vs {}", + nr, + nc, + mk, + alpha, + beta, + m, + n + ); + } + } + } + } + + #[test] + fn dmatrix_transpose_bit_identical() { + use nalgebra::DMatrix as NaDMatrix; + let (nr, nc) = (6usize, 4); + let a: Vec = (0..nr * nc).map(|i| (i as f64) * 0.137 + 0.5).collect(); + let mine = DMatrix::from_row_slice(nr, nc, &a).transpose(); + let na = NaDMatrix::from_row_slice(nr, nc, &a).transpose(); + for (m, n) in mine.data.iter().zip(na.iter()) { + assert!(approx_eq_bits(*m, *n), "transpose mismatch {} vs {}", m, n); + } + } + + #[test] + fn omatrix_gemm_bit_identical() { + use nalgebra::DMatrix as NaDMatrix; + let r = 6usize; + let n = 7usize; + let c = 5usize; + let a_data: Vec = (0..r * n).map(|i| (i as f64) * 0.137 + 0.5).collect(); + let b_data: Vec = (0..n * c).map(|i| (i as f64) * 0.731 - 0.3).collect(); + let c0: Vec = (0..r * c).map(|i| (i as f64) * 0.21 - 0.7).collect(); + + for (alpha, beta) in [(1.0, 0.0), (2.0, 1.0), (1.0, 1.0)] { + let mut mine = OMatrix::from_row_slice(r, c, &c0); + let mut na_res = NaDMatrix::from_row_slice(r, c, &c0); + let na_a = NaDMatrix::from_row_slice(r, n, &a_data); + let na_b = NaDMatrix::from_row_slice(n, c, &b_data); + mine.gemm( + alpha, + &OMatrix::from_row_slice(r, n, &a_data), + &OMatrix::from_row_slice(n, c, &b_data), + beta, + ); + na_res.gemm(alpha, &na_a, &na_b, beta); + + for (m, n) in mine.data.iter().zip(na_res.iter()) { + assert!( + approx_eq_bits(*m, *n), + "omatrix gemm mismatch {} vs {}", + m, + n + ); + } + } + } + + #[test] + fn omatrix_fixed_rows_gemm_bit_identical() { + // multibody pattern (dim3): link_j_v (3 rows = DIM) = + // gcross_matrix_tr(shift02) (3x3) * parent_j_w (ANG_DIM=3 rows of parent Jacobian, N cols) + // The 3x3 shift_tr comes from `Vector3::gcross_matrix_tr()` (see cross_product_matrix.rs). + use nalgebra::DMatrix as NaDMatrix; + let n = 3usize; // shift is 3x3 + let c = 4usize; // ndofs + let shift: Vec = (0..n * n).map(|i| (i as f64) * 0.11 - 0.3).collect(); + let parent: Vec = (0..6 * c).map(|i| (i as f64) * 0.731 + 0.5).collect(); + + // result: link_j_v (3 rows) = shift_tr (3x3) * parent_j_w (3 cols of parent) + let mut mine = OMatrix::::zeros(3, c); + let shift_tr = OMatrix::::from_row_slice(n, n, &shift); + let parent_full = OMatrix::::from_row_slice(6, c, &parent); + let parent_j_w = parent_full.fixed_rows::<3>(3); + mine.gemm(1.0, &shift_tr, &parent_j_w, 1.0); + + // nalgebra oracle (fully dynamic so it type-checks the mixed-row gemm). + let na_shift = NaDMatrix::from_row_slice(n, n, &shift); + let na_parent = NaDMatrix::from_row_slice(6, c, &parent); + let na_pw = na_parent.fixed_rows::<3>(3); + let mut na_link = NaDMatrix::zeros(3, c); + na_link.gemm(1.0, &na_shift, &na_pw, 1.0); + + for (m, n) in mine.data.iter().zip(na_link.iter()) { + assert!( + approx_eq_bits(*m, *n), + "fixed_rows gemm mismatch {} vs {}", + m, + n + ); + } + } + + #[test] + fn omatrix_gemm_tr_bit_identical() { + use nalgebra::OMatrix as NaOMatrix; + let n = 7usize; + let c = 5usize; + let a_data: Vec = (0..6 * n).map(|i| (i as f64) * 0.137 + 0.5).collect(); + let b_data: Vec = (0..6 * c).map(|i| (i as f64) * 0.731 - 0.3).collect(); + + let mut mine = OMatrix::::zeros(n, c); + let na_res = NaOMatrix::, nalgebra::Dyn>::zeros(c); + let na_a = NaOMatrix::, nalgebra::Dyn>::from_row_slice(&a_data); + let na_b = NaOMatrix::, nalgebra::Dyn>::from_row_slice(&b_data); + mine.gemm_tr( + 1.5, + &OMatrix::from_row_slice(6, n, &a_data), + &OMatrix::from_row_slice(6, c, &b_data), + 2.0, + ); + let mut na_res = na_res; + na_res.gemm_tr(1.5, &na_a, &na_b, 2.0); + + for (m, n) in mine.data.iter().zip(na_res.iter()) { + assert!( + approx_eq_bits(*m, *n), + "omatrix gemm_tr mismatch {} vs {}", + m, + n + ); + } + } + + #[test] + fn omatrix_quadform_bit_identical() { + use crate::linalg::DMatrix; + let r = 6usize; + let c = 4usize; + let mid_data: Vec = (0..r * r).map(|i| (i as f64) * 0.137 + 0.5).collect(); + let rhs_data: Vec = (0..r * c).map(|i| (i as f64) * 0.731 - 0.3).collect(); + let c0: Vec = (0..c * c).map(|i| (i as f64) * 0.21 - 0.7).collect(); + + let mid = DMatrix::from_row_slice(r, r, &mid_data); + let rhs = OMatrix::from_row_slice(r, c, &rhs_data); + let mut mine = OMatrix::from_row_slice(c, c, &c0); + mine.quadform(1.0, &mid, &rhs, 1.0); + + // oracle via nalgebra's own quadform (bit-identical algorithm) + let na_mid = nalgebra::DMatrix::::from_row_slice(r, r, &mid_data); + let na_rhs = nalgebra::DMatrix::::from_row_slice(r, c, &rhs_data); + let mut na_res = nalgebra::DMatrix::::from_row_slice(c, c, &c0); + na_res.quadform(1.0, &na_mid, &na_rhs, 1.0); + + for (m, n) in mine.data.iter().zip(na_res.iter()) { + assert!( + approx_eq_bits(*m, *n), + "omatrix quadform mismatch {} vs {}", + m, + n + ); + } + } + + #[test] + fn omatrix_ger_bit_identical() { + use nalgebra::OMatrix as NaOMatrix; + let r = 2usize; + let c = 3usize; + let a_data: Vec = (0..r * c).map(|i| (i as f64) * 0.137 + 0.5).collect(); + let x: Vec = (0..r).map(|i| (i as f64) * 0.21 - 0.7).collect(); + let y: Vec = (0..c).map(|i| (i as f64) * 0.731 + 0.3).collect(); + + let mut mine = OMatrix::from_row_slice(r, c, &a_data); + let mut na_res = + NaOMatrix::, nalgebra::Const<3>>::from_row_slice(&a_data); + let na_x = nalgebra::DVector::from_vec(x.clone()); + let na_y = nalgebra::DVector::from_vec(y.clone()); + let xv = DVector::from_vec(x); + let yv = DVector::from_vec(y); + mine.ger(1.0, &xv, &yv, 1.0); + na_res.ger(1.0, &na_x, &na_y, 1.0); + + for (m, n) in mine.data.iter().zip(na_res.iter()) { + assert!( + approx_eq_bits(*m, *n), + "omatrix ger mismatch {} vs {}", + m, + n + ); + } + } + + #[test] + fn omatrix_tr_mul_to_bit_identical() { + use nalgebra::OMatrix as NaOMatrix; + let r = 6usize; + let c = 4usize; + let j: Vec = (0..r * c).map(|i| (i as f64) * 0.137 + 0.5).collect(); + let f: Vec = (0..r).map(|i| (i as f64) * 0.731 - 0.3).collect(); + + let mine = OMatrix::from_row_slice(r, c, &j); + let na_j = NaOMatrix::, nalgebra::Dyn>::from_row_slice(&j); + let na_f = nalgebra::DVector::from_vec(f.clone()); + let mut out = DVector::zeros(c); + let f_vec = DVector::from_vec(f); + let fv = f_vec.as_view(); + mine.tr_mul_to(&fv, &mut out); + let na_out = na_j.tr_mul(&na_f); + + for (m, n) in out.data.iter().zip(na_out.iter()) { + assert!(approx_eq_bits(*m, *n), "tr_mul_to mismatch {} vs {}", m, n); + } + } + + #[test] + fn smatrix_gemm_bit_identical() { + // SMatrix (wraps a DMatrix) must gemm bit-identically with nalgebra::SMatrix. + let a_data: Vec = (0..3 * 3).map(|i| (i as f64) * 0.137 + 0.5).collect(); + let b_data: Vec = (0..3 * 3).map(|i| (i as f64) * 0.731 - 0.3).collect(); + + let a = SMatrix::::from_row_slice(&a_data); + let b = SMatrix::::from_row_slice(&b_data); + let mut mine = SMatrix::::zeros(); + mine.gemm(2.0, &a, &b, 1.0); + + // nalgebra oracle (apples-to-apples: nalgebra::SMatrix). + let na_a = nalgebra::SMatrix::::from_row_slice(&a_data); + let na_b = nalgebra::SMatrix::::from_row_slice(&b_data); + let mut na_res = nalgebra::SMatrix::::zeros(); + na_res.gemm(2.0, &na_a, &na_b, 1.0); + + for (m, n) in mine.matrix.data.iter().zip(na_res.iter()) { + assert!( + approx_eq_bits(*m, *n), + "smatrix gemm mismatch {} vs {}", + m, + n + ); + } + } + + /// Builds a deterministic pseudo-random `nrows × ncols` matrix (no std RNG dependency). + fn rand_matrix(nrows: usize, ncols: usize, seed: u64) -> DMatrix { + let mut data = Vec::with_capacity(nrows * ncols); + let mut s = seed.wrapping_add(0x9E3779B97F4A7C15); + for _ in 0..(nrows * ncols) { + // xorshift64 + s ^= s << 13; + s ^= s >> 7; + s ^= s << 17; + let v = ((s >> 11) as f64) / ((1u64 << 53) as f64) * 10.0 - 5.0; + data.push(v); + } + DMatrix::from_row_slice(nrows, ncols, &data) + } + + #[test] + fn lu_decomposition_bit_identical() { + for (r, c, seed) in [(3usize, 3, 1u64), (4, 4, 2), (5, 5, 7), (6, 6, 13)] { + let m = rand_matrix(r, c, seed); + let mine = LU::new(m.clone_owned()); + + let na = nalgebra::LU::::new( + nalgebra::DMatrix::from_column_slice(r, c, &m.data), + ); + + // Compare the decomposed L (unit-diagonal) and U (upper) factors bit-by-bit. + let na_l = na.l(); + let na_u = na.u(); + for i in 0..r { + for j in 0..c { + let my_l = if i > j { + mine.lu.get(i, j) + } else if i == j { + 1.0 + } else { + 0.0 + }; + let my_u = if i <= j { mine.lu.get(i, j) } else { 0.0 }; + assert!( + approx_eq_bits(my_l, na_l[(i, j)]), + "LU L mismatch r={r} c={c} seed={seed} ({i},{j}): {my_l} vs {}", + na_l[(i, j)] + ); + assert!( + approx_eq_bits(my_u, na_u[(i, j)]), + "LU U mismatch r={r} c={c} seed={seed} ({i},{j}): {my_u} vs {}", + na_u[(i, j)] + ); + } + } + } + } + + #[test] + fn lu_solve_bit_identical() { + for (n, seed, bseed) in [(4usize, 3u64, 100), (6, 9, 200), (5, 17, 300)] { + let a = rand_matrix(n, n, seed); + let b = rand_matrix(n, 2, bseed); + let mine = LU::new(a.clone_owned()); + + let mut x_mine = b.clone_owned(); + assert!(mine.solve_mut(&mut x_mine)); + + let na = nalgebra::LU::::new( + nalgebra::DMatrix::from_column_slice(n, n, &a.data), + ); + let na_b = nalgebra::DMatrix::from_column_slice(n, 2, &b.data); + let x_na = na.solve(&na_b).unwrap(); + + for i in 0..n { + for j in 0..2 { + assert!( + approx_eq_bits(x_mine.get(i, j), x_na[(i, j)]), + "LU solve mismatch n={n} ({i},{j}): {} vs {}", + x_mine.get(i, j), + x_na[(i, j)] + ); + } + } + } + } + + #[test] + fn lu_inverse_bit_identical() { + for (n, seed) in [(4usize, 5u64), (5, 11), (6, 27)] { + let a = rand_matrix(n, n, seed); + let mine = LU::new(a.clone_owned()); + let inv_mine = mine.try_inverse().expect("invertible"); + + let na = nalgebra::LU::::new( + nalgebra::DMatrix::from_column_slice(n, n, &a.data), + ); + let inv_na = na.try_inverse().unwrap(); + + for i in 0..n { + for j in 0..n { + assert!( + approx_eq_bits(inv_mine.get(i, j), inv_na[(i, j)]), + "LU inverse mismatch n={n} ({i},{j}): {} vs {}", + inv_mine.get(i, j), + inv_na[(i, j)] + ); + } + } + } + } + + #[test] + fn lu_determinant_bit_identical() { + for (n, seed) in [(3usize, 2u64), (5, 8), (4, 19), (6, 33)] { + let a = rand_matrix(n, n, seed); + let mine = LU::new(a.clone_owned()); + let det_mine = mine.to_determinant(); + + let na = nalgebra::LU::::new( + nalgebra::DMatrix::from_column_slice(n, n, &a.data), + ); + let det_na = na.determinant(); + + assert!( + approx_eq_bits(det_mine, det_na), + "LU determinant mismatch n={n}: {det_mine} vs {det_na}" + ); + } + } +} diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs index 95730d909..9f3b39e08 100644 --- a/src/pipeline/mod.rs +++ b/src/pipeline/mod.rs @@ -38,5 +38,7 @@ mod query_pipeline; #[cfg(feature = "alloc")] mod user_changes; -#[cfg(all(feature = "debug-render", feature = "alloc"))] -mod debug_render_pipeline; +#[cfg(all(feature = "alloc", feature = "serde-serialize"))] +mod recorder; +#[cfg(all(feature = "alloc", feature = "serde-serialize"))] +pub use recorder::{WorldFrame, WorldPlayer, WorldRecorder}; diff --git a/src/pipeline/physics_pipeline/mod.rs b/src/pipeline/physics_pipeline/mod.rs index 02f236455..ae88815b2 100644 --- a/src/pipeline/physics_pipeline/mod.rs +++ b/src/pipeline/physics_pipeline/mod.rs @@ -208,12 +208,28 @@ impl PhysicsPipeline { hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { + // Build the world gravity container (kind = Gravity, persistent). Its + // single entry stores the gravity *acceleration*; `compute_body_effective_forces` + // scales it by each body's mass × gravity_scale. This is the self-integrating + // gravity container — gravity stays constant without any per-step re-apply. + let mut gravity_container = crate::dynamics::force_containers::KindContainer::new( + crate::dynamics::force_containers::ForceKind::Gravity, + crate::dynamics::force_containers::Persistence::Persistent, + ); + gravity_container.push(crate::dynamics::force_containers::ForceEntry { + id: 1, + force: gravity, + torque: crate::math::AngVector::ZERO, + point: None, + }); + // With a dedicated pool configured, run the whole step inside it. #[cfg(all(feature = "parallel", not(feature = "unsync-callbacks")))] if let Some(pool) = self.thread_pool.clone() { return pool.install(|| { self.step_inner( gravity, + &gravity_container, integration_parameters, islands, broad_phase, @@ -231,6 +247,7 @@ impl PhysicsPipeline { self.step_inner( gravity, + &gravity_container, integration_parameters, islands, broad_phase, diff --git a/src/pipeline/physics_pipeline/solve.rs b/src/pipeline/physics_pipeline/solve.rs index 228627e64..77e25e336 100644 --- a/src/pipeline/physics_pipeline/solve.rs +++ b/src/pipeline/physics_pipeline/solve.rs @@ -158,7 +158,8 @@ impl PhysicsPipeline { pub(super) fn build_islands_and_solve_velocity_constraints( &mut self, - gravity: Vector, + _gravity: Vector, + gravity_container: &crate::dynamics::force_containers::KindContainer, integration_parameters: &IntegrationParameters, islands: &mut IslandManager, narrow_phase: &mut NarrowPhase, @@ -240,9 +241,17 @@ impl PhysicsPipeline { for handle in islands.active_bodies() { let rb = bodies.index_mut_internal(handle); IslandManager::update_body_energy(rb, dt, length_unit); - let effective_mass = rb.mprops.effective_mass(); - rb.forces - .compute_effective_force_and_torque(gravity, effective_mass); + let _effective_mass = rb.mprops.effective_mass(); + crate::dynamics::force_containers::compute_body_effective_forces( + rb, + gravity_container, + ); + // Drain transient (per-step) force containers now that their + // contributions were summed into `rb.forces`. Persistent + // containers keep their entries. Done here (inside the active-body + // loop) so we never iterate the whole set — which would bump the + // active-set epoch during steady state. + crate::dynamics::force_containers::drain_transient_forces(rb); any_extra_iterations |= rb.additional_solver_iterations() > 0; bid(rb, &islands.persistent, &mut split_bid); observe(rb, observations); @@ -270,9 +279,12 @@ impl PhysicsPipeline { for handle in chunk { let rb = bodies.index_mut_internal(*handle); IslandManager::update_body_energy(rb, dt, length_unit); - let effective_mass = rb.mprops.effective_mass(); - rb.forces - .compute_effective_force_and_torque(gravity, effective_mass); + let _effective_mass = rb.mprops.effective_mass(); + crate::dynamics::force_containers::compute_body_effective_forces( + rb, + gravity_container, + ); + crate::dynamics::force_containers::drain_transient_forces(rb); any_extra |= rb.additional_solver_iterations() > 0; bid(rb, persistent, &mut chunk_bid); observe(rb, &mut observations); @@ -397,6 +409,20 @@ impl PhysicsPipeline { events, ); + // Bridge the solver-emergent contact normal / friction impulses into the + // observation-only `ContactReaction` / `Friction` force containers. Runs + // here (single-threaded, after the solve, with the full `NarrowPhase` + // still borrowed) so the contact graph is never mutated concurrently and + // the impulse data reflects this step's solve. These containers are NOT + // re-summed by the force pipeline (see `compute_body_effective_forces`). + if integration_parameters.bridge_contact_forces { + crate::dynamics::force_containers::bridge_solver_contact_forces( + narrow_phase, + bodies, + integration_parameters.dt, + ); + } + self.counters.stages.solver_time.pause(); } } diff --git a/src/pipeline/physics_pipeline/substep.rs b/src/pipeline/physics_pipeline/substep.rs index ac8e8f094..ef1dff63e 100644 --- a/src/pipeline/physics_pipeline/substep.rs +++ b/src/pipeline/physics_pipeline/substep.rs @@ -267,6 +267,7 @@ impl PhysicsPipeline { pub(super) fn step_inner( &mut self, gravity: Vector, + gravity_container: &crate::dynamics::force_containers::KindContainer, integration_parameters: &IntegrationParameters, islands: &mut IslandManager, broad_phase: &mut BroadPhaseBvh, @@ -478,6 +479,7 @@ impl PhysicsPipeline { self.counters.custom.pause(); self.build_islands_and_solve_velocity_constraints( gravity, + gravity_container, &integration_parameters, islands, narrow_phase, diff --git a/src/pipeline/physics_world.rs b/src/pipeline/physics_world.rs index 0164283ce..5b7631cbd 100644 --- a/src/pipeline/physics_world.rs +++ b/src/pipeline/physics_world.rs @@ -3,6 +3,8 @@ use crate::dynamics::{ CCDSolver, GenericJoint, ImpulseJoint, ImpulseJointHandle, ImpulseJointSet, IntegrationParameters, IslandManager, Multibody, MultibodyJointHandle, MultibodyJointSet, MultibodyLink, MultibodyLinkId, RigidBody, RigidBodyHandle, RigidBodySet, + fluid::FluidWorld, + soft_body::{SoftBody, SoftBodyId, SoftBodySet}, }; use crate::geometry::{ BroadPhaseBvh, Collider, ColliderHandle, ColliderSet, ContactPair, DefaultBroadPhase, @@ -85,6 +87,18 @@ pub struct PhysicsWorld { /// Workspace only: not part of a snapshot (see the type docs). #[cfg_attr(feature = "serde-serialize", serde(skip))] pub ccd_solver: CCDSolver, + /// All soft bodies (deformable / point-mass + spring structures). + /// + /// Advanced after the rigid-body pipeline each step. Phase 0b wiring only: + /// soft bodies are stepped independently (they do not yet exchange forces or + /// collisions with rigid bodies — that coupling arrives in later phases). Each + /// body with its `sleeping` flag set is skipped, mirroring island sleeping. + pub soft_bodies: SoftBodySet, + /// All fluid bodies (SPH particle clouds). Stepped independently after the + /// rigid-body pipeline, mirroring `soft_bodies` (Phase 0 of the fluid + /// roadmap; rigid-body coupling arrives later). See + /// `.hermes/plans/2026-08-30_fluid-sph-roadmap.md`. + pub fluids: Vec, } impl Default for PhysicsWorld { @@ -101,6 +115,8 @@ impl Default for PhysicsWorld { impulse_joints: ImpulseJointSet::new(), multibody_joints: MultibodyJointSet::new(), ccd_solver: CCDSolver::new(), + soft_bodies: SoftBodySet::new(), + fluids: Vec::new(), } } } @@ -139,6 +155,11 @@ impl PhysicsWorld { /// } /// ``` pub fn step_with_events(&mut self, hooks: &dyn PhysicsHooks, events: &dyn EventHandler) { + // Phase 2: route bound soft-body spring/damping forces into the rigid + // bodies' `force_containers` *before* the pipeline integrates them, so the + // soft forces act on the rigid bodies through the standard effective-force + // path this step. Free (unbound) particles are integrated separately below. + self.soft_bodies.write_spring_forces(&mut self.bodies); self.physics_pipeline.step( self.gravity, &self.integration_parameters, @@ -153,6 +174,19 @@ impl PhysicsWorld { hooks, events, ); + // Phase 0b: advance free soft-body particles after the rigid-body pipeline. + // Sleeping bodies are skipped inside `SoftBodySet::step`. + self.soft_bodies.step(self.integration_parameters.dt); + // Phase 8: after the rigid pipeline integrates, snap bound soft particles + // to their rigid bodies' new world transforms (so anchored cloth/flags + // follow a moving body). Runs after step so followers see updated poses. + self.soft_bodies.follow_rigid_bodies(&self.bodies); + // Phase 0 (fluid SPH): advance every fluid particle cloud independently, + // after the rigid-body pipeline. No rigid coupling yet. + let dt = self.integration_parameters.dt; + for fluid in &mut self.fluids { + fluid.step(dt); + } } /// The bodies and colliders automatically disabled during the last step because their @@ -210,6 +244,41 @@ impl PhysicsWorld { self.bodies.insert(body) } + // ── Soft bodies ───────────────────────────────────────────────────── + + /// Insert a soft body into the world and return its id. + /// + /// The soft body is stepped automatically each [`step`](Self::step) after the + /// rigid-body pipeline (Phase 0b wiring). + pub fn insert_soft_body(&mut self, body: SoftBody) -> SoftBodyId { + self.soft_bodies.insert(body) + } + + /// Immutable access to a soft body by id. + pub fn soft_body(&self, id: SoftBodyId) -> Option<&SoftBody> { + self.soft_bodies.get(id) + } + + /// Mutable access to a soft body by id. + pub fn soft_body_mut(&mut self, id: SoftBodyId) -> Option<&mut SoftBody> { + self.soft_bodies.get_mut(id) + } + + /// Mark a soft body as sleeping (its particles are not integrated until woken). + pub fn sleep_soft_body(&mut self, id: SoftBodyId) -> bool { + self.soft_bodies.sleep(id) + } + + /// Wake a sleeping soft body. + pub fn wake_soft_body(&mut self, id: SoftBodyId) -> bool { + self.soft_bodies.wake(id) + } + + /// Whether a soft body is currently sleeping. + pub fn is_soft_body_sleeping(&self, id: SoftBodyId) -> bool { + self.soft_bodies.is_sleeping(id) + } + /// Remove a rigid body and all its attached colliders and joints. /// /// Returns the removed body, or `None` if the handle was invalid. @@ -634,6 +703,39 @@ impl PhysicsWorld { self.narrow_phase.contact_pair(collider1, collider2) } + /// Enables or disables collision detection between two specific colliders, + /// regardless of their collision groups, solver hooks, or whether they are + /// connected by a joint. + /// + /// This forwards to the narrow-phase's per-pair collision filter. Disabling a + /// pair that was never disabled (or enabling a pair that was never disabled) is + /// a no-op. The setting persists across `step` calls: an existing contact + /// manifold for a disabled pair is cleared on the next step. This is symmetric: + /// `set_collision_enabled(a, b, false)` is equivalent to + /// `set_collision_enabled(b, a, false)`. + pub fn set_collision_enabled( + &mut self, + collider1: ColliderHandle, + collider2: ColliderHandle, + enabled: bool, + ) { + if enabled { + self.narrow_phase.enable_collision(collider1, collider2); + } else { + self.narrow_phase.disable_collision(collider1, collider2); + } + } + + /// Returns `true` if collision between the two given colliders is currently + /// *enabled* (i.e. not disabled via [`Self::set_collision_enabled`]). + pub fn is_collision_enabled( + &self, + collider1: ColliderHandle, + collider2: ColliderHandle, + ) -> bool { + self.narrow_phase.is_collision_enabled(collider1, collider2) + } + /// Iterate over all contact pairs involving the given collider. pub fn contact_pairs_with( &self, diff --git a/src/pipeline/recorder.rs b/src/pipeline/recorder.rs new file mode 100644 index 000000000..884ae692e --- /dev/null +++ b/src/pipeline/recorder.rs @@ -0,0 +1,227 @@ +#![cfg(all(feature = "alloc", feature = "serde-serialize"))] + +//! Phase D — record / replay of physics state for Box3D. +//! +//! `PhysicsWorld` already bundles every piece of simulation state (bodies, +//! colliders, joints, narrow-phase manifolds, islands, broad-phase tree, …) and +//! derives `serde::Serialize`/`Deserialize` under the `serde-serialize` feature. +//! That makes a faithful recorder trivial: snapshot the world each step, serialize +//! the whole recording, and on replay restore snapshots back into a live world. +//! +//! The skipped fields of `PhysicsWorld` (`physics_pipeline`, `ccd_solver`) are +//! reconstructed via `Default` on deserialize, which is exactly what a fresh +//! replay needs — no pipeline workspace is carried across the boundary. +//! +//! For bit-exact replay across machines, build with `enhanced-determinism`. The +//! recorder/player themselves do not require it; determinism is a property of the +//! underlying simulation, not of the snapshot format. + +use crate::alloc_prelude::*; +use crate::pipeline::PhysicsWorld; +use serde::{Deserialize, Serialize}; + +/// A single recorded frame: a full snapshot of the [`PhysicsWorld`] at a step. +#[derive(Serialize, Deserialize)] +pub struct WorldFrame { + /// The step index this frame was captured at (0-based). + pub step: u64, + /// The full world state at capture time. + pub world: PhysicsWorld, +} + +/// Records a stream of [`PhysicsWorld`] snapshots for later replay or regression capture. +/// +/// Each [`capture`](Self::capture) deep-copies the live world (via a bincode +/// round-trip) so the recording is independent of later mutation. The whole +/// recording serializes to/from bytes with [`to_bytes`](Self::to_bytes) / +/// [`from_bytes`](Self::from_bytes), suitable for on-disk golden files or streaming. +#[derive(Serialize, Deserialize)] +pub struct WorldRecorder { + frames: Vec, + next_step: u64, +} + +impl WorldRecorder { + /// Creates an empty recorder starting at step 0. + pub fn new() -> Self { + Self { + frames: Vec::new(), + next_step: 0, + } + } + + /// Deep-copies the current world state into a new frame. + /// + /// Call this right after each `world.step()` (or before it, depending on + /// whether you want pre- or post-step snapshots). + pub fn capture(&mut self, world: &PhysicsWorld) { + // Deep copy via a bincode round-trip: keeps the recording independent of + // the live world and exercises the same (de)serialization path used for + // on-disk persistence. + let bytes = bincode::serialize(world).expect("serialize PhysicsWorld"); + let world: PhysicsWorld = + bincode::deserialize(&bytes).expect("deserialize PhysicsWorld (clone)"); + self.frames.push(WorldFrame { + step: self.next_step, + world, + }); + self.next_step += 1; + } + + /// Number of captured frames. + pub fn len(&self) -> usize { + self.frames.len() + } + + /// Whether no frames have been captured yet. + pub fn is_empty(&self) -> bool { + self.frames.is_empty() + } + + /// Returns the captured frame at `index`, if any. + pub fn frame(&self, index: usize) -> Option<&WorldFrame> { + self.frames.get(index) + } + + /// Serializes the whole recording to bytes (bincode). + pub fn to_bytes(&self) -> Result, bincode::Error> { + bincode::serialize(self) + } + + /// Deserializes a recording previously produced by [`to_bytes`](Self::to_bytes). + pub fn from_bytes(bytes: &[u8]) -> Result { + bincode::deserialize(bytes) + } +} + +impl Default for WorldRecorder { + fn default() -> Self { + Self::new() + } +} + +/// Replays recorded [`WorldFrame`]s back into a live [`PhysicsWorld`]. +/// +/// Restoring a frame overwrites *all* world state with the snapshot — pipeline +/// workspaces are rebuilt via `Default`, exactly as on a fresh load. +pub struct WorldPlayer { + frames: Vec, + cursor: usize, +} + +impl WorldPlayer { + /// Builds a player from a recording, taking ownership of its frames. + pub fn from_recording(rec: WorldRecorder) -> Self { + Self { + frames: rec.frames, + cursor: 0, + } + } + + /// Builds a player from bytes previously produced by + /// [`WorldRecorder::to_bytes`]. + pub fn from_bytes(bytes: &[u8]) -> Result { + Ok(Self { + frames: bincode::deserialize::(bytes)?.frames, + cursor: 0, + }) + } + + /// Number of frames available to replay. + pub fn len(&self) -> usize { + self.frames.len() + } + + /// Whether there are no frames to replay. + pub fn is_empty(&self) -> bool { + self.frames.is_empty() + } + + /// Returns the frame at `index` without advancing the replay cursor. + pub fn frame(&self, index: usize) -> Option<&WorldFrame> { + self.frames.get(index) + } + + /// Resets the replay cursor back to the first frame. + pub fn reset(&mut self) { + self.cursor = 0; + } + + /// Restores the next recorded frame into `world`, overwriting all state. + /// + /// Returns `false` once every frame has been restored. + pub fn restore_next(&mut self, world: &mut PhysicsWorld) -> bool { + if self.cursor >= self.frames.len() { + return false; + } + // Reconstruct from the stored (serialized) frame through the same byte + // path a real on-disk load would use — this is the replay fidelity check. + let bytes = bincode::serialize(&self.frames[self.cursor]).expect("serialize frame"); + let frame: WorldFrame = bincode::deserialize(&bytes).expect("deserialize frame"); + *world = frame.world; + self.cursor += 1; + true + } +} + +#[cfg(test)] +mod test { + use super::*; + use crate::math::Vector; + use crate::prelude::{ColliderBuilder, RigidBodyBuilder}; + + /// Phase D: a recorded falling-box simulation must round-trip through bytes + /// and replay back to a fresh world with identical body transforms at every + /// frame — proving the snapshot format is lossless and replay-faithful. + #[test] + fn record_replay_reproduces_transforms() { + // --- Record --- + let mut world = PhysicsWorld::default(); + world.integration_parameters.dt = 1.0 / 60.0; + + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 10.0, 0.0)), + ColliderBuilder::cuboid(0.5, 0.5, 0.5), + ); + + let steps = 60; + let mut recorded: Vec = Vec::with_capacity(steps); + let mut rec = WorldRecorder::new(); + for _ in 0..steps { + world.step(); + rec.capture(&world); + recorded.push(world.bodies.get(ball).unwrap().translation()); + } + + assert_eq!(rec.len(), steps); + + // --- Serialize / deserialize the recording --- + let bytes = rec.to_bytes().expect("record -> bytes"); + let player = WorldPlayer::from_bytes(&bytes).expect("bytes -> player"); + assert_eq!(player.len(), steps); + + // --- Replay into a fresh world --- + let mut replay = PhysicsWorld::default(); + replay.integration_parameters.dt = 1.0 / 60.0; + let mut replayed: Vec = Vec::with_capacity(steps); + let mut player = WorldPlayer::from_bytes(&bytes).expect("bytes -> player"); + assert_eq!(player.len(), steps); + while player.restore_next(&mut replay) { + replayed.push(replay.bodies.get(ball).unwrap().translation()); + } + + assert_eq!(replayed.len(), steps); + + for (orig, rep) in recorded.iter().zip(replayed.iter()) { + let d = (orig.x - rep.x).abs() + (orig.y - rep.y).abs() + (orig.z - rep.z).abs(); + assert!(d < 1e-9, "replay transform diverged at frame"); + } + + // And the final resting transform matches exactly (same serialized bytes). + let last = replayed.last().unwrap(); + let d = (recorded[steps - 1].x - last.x).abs() + + (recorded[steps - 1].y - last.y).abs() + + (recorded[steps - 1].z - last.z).abs(); + assert!(d < 1e-9, "final replay transform diverged"); + } +}