diff --git a/.github/workflows/rapier-ci-build.yml b/.github/workflows/rapier-ci-build.yml index 54ff380d0..016b661d6 100644 --- a/.github/workflows/rapier-ci-build.yml +++ b/.github/workflows/rapier-ci-build.yml @@ -6,8 +6,16 @@ on: pull_request: branches: [master] +# A new push to a PR cancels the still-running build of the previous push. +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} + env: CARGO_TERM_COLOR: always + # Incremental compilation only pays off for local rebuild loops; in CI it just + # slows the build down and bloats the caches. + CARGO_INCREMENTAL: 0 jobs: check-fmt: @@ -22,42 +30,80 @@ jobs: RUSTDOCFLAGS: -D warnings steps: - uses: actions/checkout@v4 + - uses: Swatinem/rust-cache@v2 - name: Cargo doc run: cargo doc --features parallel,serde-serialize,debug-render -p rapier3d -p rapier2d -p rapier3d-meshloader -p rapier3d-urdf && cargo doc -p mjcf-rs --features msh && cargo doc -p rapier3d-mjcf --features stl,wavefront,msh - build-native: + # All the lints, plus the testbed/example feature-combination checks. This is the only + # job (besides publish-dry-run) that compiles the graphical stack (kiss3d), so it owns + # the cmake/xcb setup and the zune-core pin. + clippy: runs-on: ubuntu-latest env: RUSTFLAGS: -D warnings steps: - uses: actions/checkout@v4 + - uses: Swatinem/rust-cache@v2 - run: sudo apt-get install -y cmake libxcb-composite0-dev # WORKAROUND: zune-core 0.5.2 made its `warn!` macro expand to nothing, which breaks # zune-jpeg (all 0.5.x), where it is called in expression position. Reached through the # testbeds' kiss3d -> gltf/image dependency. Remove once upstream is fixed. - name: Pin zune-core (0.5.2 breaks zune-jpeg) run: cargo update -p zune-core@0.5 --precise 0.5.1 + # Bare clippy covers every default-member with default features: the rapier crates, + # the testbeds, and the example crates (so no separate `cargo check` for those). - name: Clippy run: cargo clippy - - name: Clippy rapier2d + - name: Clippy rapier2d examples (parallel) run: cargo clippy -p rapier-examples-2d --features parallel - - name: Clippy rapier3d + - name: Clippy rapier3d examples (parallel) run: cargo clippy -p rapier-examples-3d --features parallel - name: Clippy mjcf-rs run: cargo clippy -p mjcf-rs --all-targets --features msh - name: Clippy rapier3d-mjcf run: cargo clippy -p rapier3d-mjcf --all-targets --features stl,wavefront,msh + - name: Check rapier_testbed2d --features parallel + run: cd crates/rapier_testbed2d; cargo check --features parallel; + - name: Check rapier_testbed3d --features parallel + run: cd crates/rapier_testbed3d; cargo check --features parallel; + - name: Check rapier2d --features enhanced-determinism + run: cd crates/rapier2d; cargo check --features enhanced-determinism; + - name: Check rapier3d --features enhanced-determinism + run: cd crates/rapier3d; cargo check --features enhanced-determinism; + # Builds the core crates in their feature variants, and runs the debug test suites. + # Scoped to the non-graphical crates: the testbeds/examples are compiled by `clippy`. + build-native: + runs-on: ubuntu-latest + env: + RUSTFLAGS: -D warnings + steps: + - uses: actions/checkout@v4 + - uses: Swatinem/rust-cache@v2 - name: Build rapier2d - run: cargo build --verbose -p rapier2d; + run: cargo build -p rapier2d; - name: Build rapier3d - run: cargo build --verbose -p rapier3d; + run: cargo build -p rapier3d; - name: Build rapier2d Parallel - run: cd crates/rapier2d; cargo build --verbose --features parallel; + run: cd crates/rapier2d; cargo build --features parallel; - name: Build rapier3d Parallel - run: cd crates/rapier3d; cargo build --verbose --features parallel; + run: cd crates/rapier3d; cargo build --features parallel; - name: Build rapier3d 8-lanes SIMD - run: cd crates/rapier3d; cargo build --verbose --features simd8; + run: cd crates/rapier3d; cargo build --features simd8; - name: Run tests - run: cargo test + run: cargo test -p rapier2d -p rapier2d-f64 -p rapier3d -p rapier3d-f64 -p rapier3d-urdf -p rapier3d-meshloader + - name: Test mjcf-rs + run: cargo test -p mjcf-rs --features msh + - name: Test rapier3d-mjcf + run: cargo test -p rapier3d-mjcf --features stl,wavefront,msh + # The release-mode determinism suites. Grouped so that invocations sharing a feature + # set reuse each other's build artifacts; kept apart from the other release tests so + # both halves run in parallel. + tests-determinism: + runs-on: ubuntu-latest + env: + RUSTFLAGS: -D warnings + steps: + - uses: actions/checkout@v4 + - uses: Swatinem/rust-cache@v2 - name: Test determinism (SIMD backend) run: cargo test -p rapier3d --release --features enhanced-determinism --test simd_backend_determinism - name: Test SIMD backend op-level parity @@ -66,18 +112,48 @@ jobs: run: cargo test -p rapier3d --release --features enhanced-determinism,serde-serialize --test parallel_path_parity - name: Test parallel-path parity (feature on) run: cargo test -p rapier3d --release --features enhanced-determinism,serde-serialize,parallel --test parallel_path_parity + - name: Test parallel-path parity (unsync-callbacks) + run: cargo test -p rapier3d --release --features enhanced-determinism,serde-serialize,parallel,unsync-callbacks --test parallel_path_parity + # The native half of the cross-target check: the `wasm-determinism` job runs + # these same two tests, against the same goldens, on 32-bit pointers. + - name: Test snapshot portability (3D, native) + run: cargo test -p rapier3d --release --features enhanced-determinism,serde-serialize --test snapshot_portability + - name: Test snapshot portability (2D, native) + run: cargo test -p rapier2d --release --features enhanced-determinism,serde-serialize --test snapshot_portability + # The aarch64 leg of the cross-architecture determinism check: the same goldens the + # x86_64 job (above) and the wasm32 job (below) verify must also hold on arm NEON. + # This is what catches platform-split float semantics that are dynamically invisible — + # e.g. the signed zero a min/max tie stores differs between SSE/wasm and NEON, which + # diverged serialized snapshots on aarch64 for months without any x86-only job noticing. + tests-determinism-arm64: + runs-on: ubuntu-24.04-arm + env: + RUSTFLAGS: -D warnings + steps: + - uses: actions/checkout@v4 + - uses: Swatinem/rust-cache@v2 + - name: Test snapshot portability (3D, aarch64) + run: cargo test -p rapier3d --release --features enhanced-determinism,serde-serialize --test snapshot_portability + - name: Test snapshot portability (2D, aarch64) + run: cargo test -p rapier2d --release --features enhanced-determinism,serde-serialize --test snapshot_portability + - name: Test parallel-path parity (aarch64) + run: cargo test -p rapier3d --release --features enhanced-determinism,serde-serialize --test parallel_path_parity + - name: Test determinism (SIMD backend, aarch64) + run: cargo test -p rapier3d --release --features enhanced-determinism --test simd_backend_determinism + # The remaining release-mode suites (non-enhanced-determinism feature sets). + tests-release: + runs-on: ubuntu-latest + env: + RUSTFLAGS: -D warnings + steps: + - uses: actions/checkout@v4 + - uses: Swatinem/rust-cache@v2 - name: Test thread-count determinism run: cargo test -p rapier3d --release --features parallel,serde-serialize --test thread_count_determinism - name: Test snapshot round-trip (3D) run: cargo test -p rapier3d --release --features serde-serialize --test snapshot_roundtrip - name: Test snapshot round-trip (2D) run: cargo test -p rapier2d --release --features serde-serialize --test snapshot_roundtrip - # The native half of the cross-target check: the `wasm-determinism` job below runs - # these same two tests, against the same goldens, on 32-bit pointers. - - name: Test snapshot portability (3D, native) - run: cargo test -p rapier3d --release --features enhanced-determinism,serde-serialize --test snapshot_portability - - name: Test snapshot portability (2D, native) - run: cargo test -p rapier2d --release --features enhanced-determinism,serde-serialize --test snapshot_portability - name: Test single-worker deferred BVH run: cargo test -p rapier3d --release --features parallel --test single_worker_deferred_bvh # `unsync-callbacks` drops the `Sync` bound off the hooks/event traits; the test's @@ -88,28 +164,6 @@ jobs: # configuration rather than one that trivially has no bound. - name: Test unsync callbacks (no parallel) run: cargo test -p rapier3d --release --features unsync-callbacks --test unsync_callbacks - - name: Test parallel-path parity (unsync-callbacks) - run: cargo test -p rapier3d --release --features enhanced-determinism,serde-serialize,parallel,unsync-callbacks --test parallel_path_parity - - name: Test mjcf-rs - run: cargo test -p mjcf-rs --features msh - - name: Test rapier3d-mjcf - run: cargo test -p rapier3d-mjcf --features stl,wavefront,msh - - name: Check rapier_testbed2d - run: cargo check --verbose -p rapier_testbed2d; - - name: Check rapier_testbed3d - run: cargo check --verbose -p rapier_testbed3d; - - name: Check rapier_testbed2d --features parallel - run: cd crates/rapier_testbed2d; cargo check --verbose --features parallel; - - name: Check rapier_testbed3d --features parallel - run: cd crates/rapier_testbed3d; cargo check --verbose --features parallel; - - name: Check rapier_testbed2d --features enhanced-determinism - run: cd crates/rapier2d; cargo check --verbose --features enhanced-determinism; - - name: Check rapier_testbed3d --features enhanced-determinism - run: cd crates/rapier3d; cargo check --verbose --features enhanced-determinism; - - name: Check rapier-examples-2d - run: cargo check -j 1 --verbose -p rapier-examples-2d; - - name: Check rapier-examples-3d - run: cargo check -j 1 --verbose -p rapier-examples-3d; build-no-std: runs-on: ubuntu-latest env: @@ -121,6 +175,7 @@ jobs: with: toolchain: stable targets: "x86_64-unknown-linux-gnu,thumbv7em-none-eabihf" + - uses: Swatinem/rust-cache@v2 - name: Check rapier2d thumbv7em-none-eabihf (no alloc) run: cargo check --verbose -p rapier2d --no-default-features --features dim2,f32 --target=thumbv7em-none-eabihf - name: Check rapier3d thumbv7em-none-eabihf (no alloc) @@ -145,6 +200,13 @@ jobs: # (manifold store, solver-graph buckets, raw color-mask slices) for UB. # x86_64 only: glam's aarch64 NEON backend hits foreign intrinsics Miri does # not implement (on Apple Silicon, use --target x86_64-unknown-linux-gnu). + # + # The SIMD solver paths lean harder on transmutes and raw slices; worth the + # extra interpretation time (~3.5x the scalar run). `parallel` also passes, + # but only manually (not in CI): crossbeam-epoch's container_of pattern + # violates Stacked Borrows in rayon's steal path, so it needs + # MIRIFLAGS="-Zmiri-tree-borrows -Zmiri-ignore-leaks" (ignore-leaks for + # rayon's never-joined global pool) and RAYON_NUM_THREADS=2. miri: runs-on: ubuntu-latest steps: @@ -154,18 +216,11 @@ jobs: with: toolchain: nightly components: miri + - uses: Swatinem/rust-cache@v2 - name: Miri test rapier3d (tiny scenes) run: cargo miri test -p rapier3d --test miri_scenes - name: Miri test rapier2d (tiny scenes) run: cargo miri test -p rapier2d --test miri_scenes - # The SIMD solver paths lean harder on transmutes and raw slices; worth the - # extra interpretation time (~3.5x the scalar run). `parallel` also passes, - # but only manually (not in CI): crossbeam-epoch's container_of pattern - # violates Stacked Borrows in rayon's steal path, so it needs - # MIRIFLAGS="-Zmiri-tree-borrows -Zmiri-ignore-leaks" (ignore-leaks for - # rayon's never-joined global pool) and RAYON_NUM_THREADS=2. - - name: Miri test rapier3d (tiny scenes) - run: cargo miri test -p rapier3d --test miri_scenes build-wasm: runs-on: ubuntu-latest env: @@ -173,6 +228,7 @@ jobs: steps: - uses: actions/checkout@v4 - run: rustup target add wasm32-unknown-unknown + - uses: Swatinem/rust-cache@v2 - name: build rapier2d run: cd crates/rapier2d && cargo build --verbose --target wasm32-unknown-unknown; - name: build rapier3d @@ -195,6 +251,7 @@ jobs: with: toolchain: stable targets: wasm32-wasip1 + - uses: Swatinem/rust-cache@v2 # `node:wasi` needs no command-line flag from 22 on. - uses: actions/setup-node@v4 with: @@ -217,8 +274,9 @@ jobs: RUSTFLAGS: -D warnings steps: - uses: actions/checkout@v4 + - uses: Swatinem/rust-cache@v2 - run: sudo apt-get install -y cmake libxcb-composite0-dev - # See `build-native`: zune-core 0.5.2 breaks zune-jpeg, reached here through the + # See `clippy`: zune-core 0.5.2 breaks zune-jpeg, reached here through the # testbed crates' kiss3d dependency. Remove once upstream is fixed. - name: Pin zune-core (0.5.2 breaks zune-jpeg) run: cargo update -p zune-core@0.5 --precise 0.5.1 @@ -247,6 +305,9 @@ jobs: - uses: actions/checkout@v4 - name: Install Linux dependencies uses: ./.github/actions/install-linux-deps + - uses: Swatinem/rust-cache@v2 + with: + workspaces: website/docs-examples - name: Tests injection + code snippets run: cd website/docs-examples && cargo test # Checks the JavaScript documentation snippets build (against the published @dimforge/rapier). diff --git a/CHANGELOG.md b/CHANGELOG.md index a5c17d55d..5aafc094f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,17 +2,45 @@ ### Added +- `CoefficientCombineRule::GeometricMean`: combines friction/restitution as + `sqrt(c1 * c2)`, the convention used by several other engines. It has the highest + rule priority, and clamps negative coefficients to zero before the square root. - `DebugRenderStyle::sleep_eligible_color_multiplier`: the debug-renderer now draws bodies that are eligible for sleep but still awake in a distinct color, making it easy to spot what is keeping a pile awake. ### Fixed +- `Wheel::rotation` now accumulates from the wheel's own rolling direction on the + contact plane (`contact normal × axle`, the same forward direction used by the + friction update) instead of the chassis' `index_forward_axis`, so wheels visually + spin whenever the vehicle actually rolls (adapted from PR #950 by @tomo0613). +- `DynamicRayCastVehicleController` no longer has unlimited braking friction when a + wheel's side impulse is exactly zero (e.g. braking in a straight line): the skid + clamp now applies to the forward impulse as well, so braking strength is bounded by + `friction_slip` (PR #947 by @tomo0613). +- Angular joint limits are no longer restricted to `(-π, π)`: the limit rows now measure the + wrapped joint angle from the middle of the allowed range (with the joint axis as the row's + jacobian, like the angular motor rows), so a range may sit anywhere on the circle + (`[0, 3π/2]` stops the joint at 3π/2 instead of π/2, and ranges crossing the ±π seam like + `[3π/4, 5π/4]` work at all). A range wider than a full turn is indistinguishable from "no + limit" for an angle read off a relative rotation, so it now leaves the axis free instead of + clamping it at some folded-back angle. +- Joint limit rows (linear and angular) now cap their position-correction bias by + `IntegrationParameters::max_corrective_velocity`, like contacts always did: a deep limit + violation recovers over a few steps instead of catapulting the bodies. - Python: `DynamicRayCastVehicleController.update_vehicle` now excludes the chassis body from the suspension raycasts by default (their origins sit on its own collider). ### Modified +- The broad phase no longer creates pairs between colliders attached to the same + rigid-body (they can never collide). A single awake body carrying thousands of + mutually-overlapping colliders previously flooded the pair map and contact graph with + pairs the narrow phase re-discarded every step; the issue #970 scene (2,210 convex + colliders on one always-awake body) steps ~90x faster while stationary and ~18x + faster while moving. Note that `NarrowPhase::contact_pair` now returns `None` for + same-parent colliders instead of a manifold-less pair. - Sleep eligibility is now judged on the actual per-step pose displacement (measured at the body’s farthest point) instead of the velocities: a body held in place by contacts can sleep even with residual solver velocities, while a body creeping through solver position diff --git a/crates/rapier2d/tests/issue_499_angular_limits.rs b/crates/rapier2d/tests/issue_499_angular_limits.rs new file mode 100644 index 000000000..fbfab0090 --- /dev/null +++ b/crates/rapier2d/tests/issue_499_angular_limits.rs @@ -0,0 +1,140 @@ +//! Regression test for https://github.com/dimforge/rapier/issues/499 (2D counterpart of the +//! rapier3d test of the same name). +//! +//! Angular limit rows compare half-angle sines, which only rank angles correctly over a half +//! turn. Measured from the joint's rest frame that capped limits at +-pi and folded anything +//! past it back: `[0, 270deg]` stopped the joint at 90deg. The rows now measure the angle from +//! the middle of the allowed range, so any range up to a full turn works wherever it sits on +//! the circle. + +use rapier2d::prelude::*; +use std::f32::consts::{PI, TAU}; + +/// How the joint is pushed against its limit — which also picks the constraint path the limit +/// row is built by. +#[derive(Copy, Clone, PartialEq, Debug)] +enum Drive { + /// A velocity motor (the 2D angular motor has a wide row, so this is the SIMD path). + Motor, + /// A constant external torque, so the joint's rows are limit rows only. + Torque, + /// Like `Torque`, but the joint is a multibody joint (the reduced-coordinates path). + MultibodyTorque, +} + +/// Pushes a revolute joint against its `[min, max]` limit and returns the angle, in degrees, it +/// settles at (unwrapped: it keeps counting past +-180). +fn settled_angle_with(drive: Drive, limits_deg: [f32; 2], dir: f32) -> f32 { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.dt = 1.0 / 60.0; + + let body1 = world.bodies.insert(RigidBodyBuilder::fixed()); + let body2 = world.bodies.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(1.0, 0.0)) + // Caps the speed the joint arrives at its limit with, so the (soft) limit row + // settles it right at the limit instead of a few degrees past it. + .angular_damping(3.0) + .can_sleep(false), + ); + world + .colliders + .insert_with_parent(ColliderBuilder::cuboid(0.5, 0.1), body2, &mut world.bodies); + + let mut joint = RevoluteJointBuilder::new() + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::new(-1.0, 0.0)) + .limits([limits_deg[0].to_radians(), limits_deg[1].to_radians()]); + if drive == Drive::Motor { + joint = joint.motor_velocity(dir * 5.0, 20.0); + } + if drive == Drive::MultibodyTorque { + world + .multibody_joints + .insert(body1, body2, joint, true) + .unwrap(); + } else { + world.impulse_joints.insert(body1, body2, joint, true); + } + + let mut unwrapped = 0.0; + let mut prev = 0.0; + for _ in 0..600 { + if drive != Drive::Motor { + world.bodies[body2].add_torque(dir * 0.1, true); + } + world.step(); + // The body's own rotation, in (-pi, pi]. + let ang = world.bodies[body2].rotation().angle(); + let mut delta = ang - prev; + if delta > PI { + delta -= TAU; + } else if delta < -PI { + delta += TAU; + } + unwrapped += delta; + prev = ang; + } + + unwrapped.to_degrees() +} + +/// The joint, pushed either way, settles at `limits[1]` (resp. `limits[0]`) — on every +/// constraint path. +fn assert_limits_reached(limits_deg: [f32; 2]) { + for drive in [Drive::Motor, Drive::Torque, Drive::MultibodyTorque] { + let max = settled_angle_with(drive, limits_deg, 1.0); + let min = settled_angle_with(drive, limits_deg, -1.0); + assert!( + (max - limits_deg[1]).abs() < 2.0, + "limits {limits_deg:?} ({drive:?}): driving + settled at {max} deg instead of {} deg", + limits_deg[1] + ); + assert!( + (min - limits_deg[0]).abs() < 2.0, + "limits {limits_deg:?} ({drive:?}): driving - settled at {min} deg instead of {} deg", + limits_deg[0] + ); + } +} + +#[test] +fn angular_limits_within_half_a_turn_are_reached() { + // Ranges that already worked before the fix — they must keep working. + assert_limits_reached([-45.0, 45.0]); + assert_limits_reached([-135.0, 135.0]); + assert_limits_reached([0.0, 90.0]); + assert_limits_reached([-170.0, -10.0]); +} + +#[test] +fn angular_limits_past_half_a_turn_are_reached() { + // Issue #499: these used to fold back (`[0, 270]` stopped at 90 deg). + assert_limits_reached([0.0, 270.0]); + assert_limits_reached([-270.0, 0.0]); + assert_limits_reached([-90.0, 200.0]); + assert_limits_reached([-350.0, 0.0]); +} + +#[test] +fn angular_limits_straddling_half_a_turn_are_reached() { + // Ranges going through the +-pi seam: representable at all only because the row measures + // the angle from the middle of the range (here 180 deg). + assert_limits_reached([45.0, 315.0]); + assert_limits_reached([-315.0, -45.0]); + assert_limits_reached([135.0, 225.0]); +} + +#[test] +fn angular_limits_wider_than_a_turn_leave_the_joint_free() { + // A wrapped angle can't tell a more-than-full-turn range from no limit at all, so the row + // is disabled instead of clamping at some arbitrary folded-back angle. + for limits in [[-180.0, 180.0], [-200.0, 200.0], [-350.0, 350.0]] { + let angle = settled_angle_with(Drive::Motor, limits, 1.0); + assert!( + angle > 360.0, + "limits {limits:?} span more than a turn but stopped the joint at {angle} deg" + ); + } +} diff --git a/crates/rapier2d/tests/issue_798_empty_polyline.rs b/crates/rapier2d/tests/issue_798_empty_polyline.rs new file mode 100644 index 000000000..6c3d78ad9 --- /dev/null +++ b/crates/rapier2d/tests/issue_798_empty_polyline.rs @@ -0,0 +1,36 @@ +//! Regression test for issue #798: a polyline collider with zero vertices used to hit an +//! `unreachable!` (index underflow while building segment indices) inside parry. An empty +//! polyline is now simply a collider with no segments. + +use rapier2d::prelude::*; + +#[test] +fn empty_polyline_collider_steps_and_queries_without_panicking() { + let mut world = PhysicsWorld::new(); + + world.insert_collider(ColliderBuilder::polyline(vec![], None), None); + + // A ball falling right where the empty polyline sits: narrow-phase pairs, queries, + // and the broad phase must all cope with the segment-less shape. + let (ball, _) = world.insert( + RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 1.0)), + ColliderBuilder::ball(0.5), + ); + + for _ in 0..10 { + world.step(); + } + + // No contact is expected (there is no geometry), and nothing must panic. + assert!( + world.bodies[ball].translation().y < 1.0, + "ball should fall freely" + ); + + let ray = Ray::new(Vector::new(0.0, 10.0), Vector::new(0.0, -1.0)); + let hit = world.cast_ray(&ray, Real::MAX, true, QueryFilter::default()); + // Only the ball can be hit; the empty polyline must simply never match. + if let Some((handle, _)) = hit { + assert_eq!(world.colliders[handle].parent(), Some(ball)); + } +} diff --git a/crates/rapier2d/tests/issue_949_coupled_linear_accel_motor.rs b/crates/rapier2d/tests/issue_949_coupled_linear_accel_motor.rs new file mode 100644 index 000000000..bc66b1a2d --- /dev/null +++ b/crates/rapier2d/tests/issue_949_coupled_linear_accel_motor.rs @@ -0,0 +1,85 @@ +//! Characterization tests for issue #949 (AccelerationBased coupled linear-axes motor +//! misbehaving in 2D). +//! +//! The simplest headless setups do NOT reproduce the reported misbehavior: a symmetric +//! soft pin (both linear axes coupled, position motor targeting zero separation) +//! converges with both motor models, and the acceleration-based model is properly +//! mass-invariant. These tests pin that down so the working baseline can't regress. +//! +//! What the issue's videos show (a chain of offset-anchor soft pins sagging while +//! dragged) still misbehaves subtly: with an offset anchor the acceleration-based +//! steady-state droop deviates from the force-based one noticeably more than the +//! coupled row's single-scalar effective-mass treatment would suggest. Reproducing the +//! full pathology headless needs the reporter's scene; the issue stays open. + +use rapier2d::prelude::*; + +fn separation_trajectory(model: MotorModel, mass: Real, k: Real, c: Real) -> Vec { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + + let a = world + .bodies + .insert(RigidBodyBuilder::dynamic().additional_mass(mass)); + let b = world.bodies.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(1.0, 0.0)) + .additional_mass(mass), + ); + + let mut joint = GenericJoint::new(JointAxesMask::FREE_FIXED_AXES); + joint.coupled_axes = JointAxesMask::LIN_AXES; + joint + .set_motor(JointAxis::LinX, 0.0, 0.0, k, c) + .set_motor_model(JointAxis::LinX, model) + .set_motor_max_force(JointAxis::LinX, Real::MAX) + .set_motor(JointAxis::LinY, 0.0, 0.0, k, c) + .set_motor_model(JointAxis::LinY, model) + .set_motor_max_force(JointAxis::LinY, Real::MAX); + world.impulse_joints.insert(a, b, joint, true); + + let mut out = Vec::new(); + for _ in 0..600 { + world.step(); + out.push((world.bodies[b].translation() - world.bodies[a].translation()).length()); + } + out +} + +/// A force-based coupled soft pin converges to zero separation. +#[test] +fn coupled_linear_force_based_motor_converges() { + let traj = separation_trajectory(MotorModel::ForceBased, 1.0, 50.0, 15.0); + let last = *traj.last().unwrap(); + let late_max = traj[300..].iter().cloned().fold(0.0, Real::max); + assert!( + last < 1.0e-2 && late_max < 5.0e-2, + "force-based soft pin should have converged; final {last}, late max {late_max}" + ); +} + +/// An acceleration-based coupled soft pin converges too (the issue's basic setup works). +#[test] +fn coupled_linear_acceleration_based_motor_converges() { + let traj = separation_trajectory(MotorModel::AccelerationBased, 1.0, 100.0, 30.0); + let last = *traj.last().unwrap(); + let late_max = traj[300..].iter().cloned().fold(0.0, Real::max); + assert!( + last < 1.0e-2 && late_max < 5.0e-2, + "acceleration-based soft pin should have converged; final {last}, late max {late_max}" + ); +} + +/// The whole point of `AccelerationBased` is mass independence: the same coefficients +/// must produce the same trajectory whatever the body masses. +#[test] +fn coupled_linear_acceleration_based_motor_is_mass_invariant() { + let light = separation_trajectory(MotorModel::AccelerationBased, 1.0, 100.0, 30.0); + let heavy = separation_trajectory(MotorModel::AccelerationBased, 10.0, 100.0, 30.0); + for (i, (l, h)) in light.iter().zip(&heavy).enumerate() { + assert!( + (l - h).abs() < 1.0e-3, + "acceleration-based response must not depend on mass; step {i}: {l} vs {h}" + ); + } +} diff --git a/crates/rapier2d/tests/snapshot_portability.rs b/crates/rapier2d/tests/snapshot_portability.rs index 1df243c6a..e482c9a90 100644 --- a/crates/rapier2d/tests/snapshot_portability.rs +++ b/crates/rapier2d/tests/snapshot_portability.rs @@ -31,7 +31,7 @@ use rapier2d::prelude::*; /// Snapshot size in bytes, and its FNV-1a digest. Both, because the size alone localizes a /// failure: a differing size means a container's *encoding* changed, an equal size with a /// differing digest means the values did. -const GOLDEN: (usize, u64) = (88_532, 0x36ef_5e87_c2c2_b819); +const GOLDEN: (usize, u64) = (88_532, 0x2984_7b60_fb36_4a99); const STEPS: usize = 60; diff --git a/crates/rapier3d/tests/issue_499_angular_limits.rs b/crates/rapier3d/tests/issue_499_angular_limits.rs new file mode 100644 index 000000000..55222f19a --- /dev/null +++ b/crates/rapier3d/tests/issue_499_angular_limits.rs @@ -0,0 +1,196 @@ +//! Regression test for https://github.com/dimforge/rapier/issues/499 +//! +//! Angular limit rows compare half-angle sines, which only rank angles correctly over a half +//! turn. Measured from the joint's rest frame that capped limits at +-pi and folded anything +//! past it back: `[0, 270deg]` stopped the joint at 90deg, `[-200deg, 200deg]` at 160deg, and a +//! joint pushed beyond pi escaped through the back of its own limit. The rows now measure the +//! angle from the middle of the allowed range, so any range up to a full turn works wherever +//! it sits on the circle. + +use rapier3d::prelude::*; +use std::f32::consts::{PI, TAU}; + +/// How the joint is pushed against its limit — which also picks the constraint path the limit +/// row is built by. +#[derive(Copy, Clone, PartialEq, Debug)] +enum Drive { + /// A velocity motor. In 3D a motorized joint has no wide row formulation, so this goes + /// through the scalar impulse-joint path. + Motor, + /// A constant external torque, so the joint's rows are limit rows only: the SIMD path. + Torque, + /// Like `Torque`, but the joint is a multibody joint (the reduced-coordinates path). + MultibodyTorque, +} + +/// Pushes a revolute joint (about +Z) against its `[min, max]` limit and returns the angle, in +/// degrees, it settles at (unwrapped: it keeps counting past +-180). +fn settled_angle_with(drive: Drive, limits_deg: [f32; 2], dir: f32) -> f32 { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.dt = 1.0 / 60.0; + + let body1 = world.bodies.insert(RigidBodyBuilder::fixed()); + let body2 = world.bodies.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(1.0, 0.0, 0.0)) + // Caps the speed the joint arrives at its limit with, so the (soft) limit row + // settles it right at the limit instead of a few degrees past it. + .angular_damping(3.0) + .can_sleep(false), + ); + world.colliders.insert_with_parent( + ColliderBuilder::cuboid(0.5, 0.1, 0.1), + body2, + &mut world.bodies, + ); + + let mut joint = RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::new(-1.0, 0.0, 0.0)) + .limits([limits_deg[0].to_radians(), limits_deg[1].to_radians()]); + if drive == Drive::Motor { + joint = joint.motor_velocity(dir * 5.0, 20.0); + } + if drive == Drive::MultibodyTorque { + world + .multibody_joints + .insert(body1, body2, joint, true) + .unwrap(); + } else { + world.impulse_joints.insert(body1, body2, joint, true); + } + + let mut unwrapped = 0.0; + let mut prev = 0.0; + for _ in 0..600 { + if drive != Drive::Motor { + // Enough torque to push against the limit, little enough not to blow through it. + world.bodies[body2].add_torque(Vector::Z * dir * 0.1, true); + } + world.step(); + let rot = *world.bodies[body2].rotation(); + // Signed angle about +Z, in (-pi, pi]. + let ang = 2.0 * rot.z.atan2(rot.w); + let mut delta = ang - prev; + if delta > PI { + delta -= TAU; + } else if delta < -PI { + delta += TAU; + } + unwrapped += delta; + prev = ang; + } + + unwrapped.to_degrees() +} + +fn settled_angle(limits_deg: [f32; 2], dir: f32) -> f32 { + settled_angle_with(Drive::Motor, limits_deg, dir) +} + +/// The joint, pushed either way, settles at `limits[1]` (resp. `limits[0]`) — on every +/// constraint path. +fn assert_limits_reached(limits_deg: [f32; 2]) { + for drive in [Drive::Motor, Drive::Torque, Drive::MultibodyTorque] { + let max = settled_angle_with(drive, limits_deg, 1.0); + let min = settled_angle_with(drive, limits_deg, -1.0); + assert!( + (max - limits_deg[1]).abs() < 2.0, + "limits {limits_deg:?} ({drive:?}): driving + settled at {max} deg instead of {} deg", + limits_deg[1] + ); + assert!( + (min - limits_deg[0]).abs() < 2.0, + "limits {limits_deg:?} ({drive:?}): driving - settled at {min} deg instead of {} deg", + limits_deg[0] + ); + } +} + +#[test] +fn angular_limits_within_half_a_turn_are_reached() { + // Ranges that already worked before the fix — they must keep working. + assert_limits_reached([-45.0, 45.0]); + assert_limits_reached([-135.0, 135.0]); + assert_limits_reached([0.0, 90.0]); + assert_limits_reached([-170.0, -10.0]); +} + +#[test] +fn angular_limits_past_half_a_turn_are_reached() { + // Issue #499: these used to fold back (`[0, 270]` stopped at 90 deg, `[-200, 200]` at 160). + assert_limits_reached([0.0, 270.0]); + assert_limits_reached([-270.0, 0.0]); + assert_limits_reached([-90.0, 200.0]); + assert_limits_reached([-350.0, 0.0]); +} + +#[test] +fn angular_limits_straddling_half_a_turn_are_reached() { + // Ranges going through the +-pi seam: representable at all only because the row measures + // the angle from the middle of the range (here 180 deg). + assert_limits_reached([45.0, 315.0]); + assert_limits_reached([-315.0, -45.0]); + assert_limits_reached([135.0, 225.0]); +} + +#[test] +fn angular_limits_wider_than_a_turn_leave_the_joint_free() { + // A wrapped angle can't tell a more-than-full-turn range from no limit at all, so the row + // is disabled instead of clamping at some arbitrary folded-back angle (it used to stop a + // `[-200, 200]` joint at 160 deg, and a `[-350, 350]` one at 10 deg). + for limits in [[-180.0, 180.0], [-200.0, 200.0], [-350.0, 350.0]] { + let angle = settled_angle(limits, 1.0); + assert!( + angle > 360.0, + "limits {limits:?} span more than a turn but stopped the joint at {angle} deg" + ); + } +} + +#[test] +fn a_joint_shoved_past_its_limit_comes_back() { + // A joint knocked beyond the +-pi seam used to keep going the wrong way round: the row + // read its angle as a large NEGATIVE one, so it pushed it further forward instead of back + // to the limit — the "bodies bug out at the limit" symptom reported on the issue. + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + world.integration_parameters.dt = 1.0 / 60.0; + + let body1 = world.bodies.insert(RigidBodyBuilder::fixed()); + // Starts at 185 deg: 15 deg past the joint's 170 deg limit, and past the seam. + let body2 = world.bodies.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(-0.996, -0.087, 0.0)) + .rotation(Vector::new(0.0, 0.0, 185f32.to_radians())) + .can_sleep(false), + ); + world.colliders.insert_with_parent( + ColliderBuilder::cuboid(0.5, 0.1, 0.1), + body2, + &mut world.bodies, + ); + world.impulse_joints.insert( + body1, + body2, + RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::new(-1.0, 0.0, 0.0)) + .limits([0.0, 170f32.to_radians()]), + true, + ); + + for _ in 0..300 { + world.step(); + } + + let rot = *world.bodies[body2].rotation(); + let angle = (2.0 * rot.z.atan2(rot.w)).to_degrees(); + // Back inside `[0, 170]` (it keeps whatever momentum the push-back left it with, so it + // may coast anywhere inside the range) instead of having run off past 180 deg. + assert!( + (-2.0..172.0).contains(&angle), + "the joint should have been pushed back into its [0, 170] deg range, but ended at {angle} deg" + ); +} diff --git a/crates/rapier3d/tests/issue_925_wheel_rotation.rs b/crates/rapier3d/tests/issue_925_wheel_rotation.rs new file mode 100644 index 000000000..5b3ead687 --- /dev/null +++ b/crates/rapier3d/tests/issue_925_wheel_rotation.rs @@ -0,0 +1,94 @@ +//! Regression test for issue #925: `Wheel::rotation` was derived from the chassis' +//! `index_forward_axis` instead of the wheel's own rolling direction, so a vehicle whose +//! wheels roll along a different axis (or, through the JS bindings, one that simply drives +//! without steering) never saw its wheels rotate. + +use rapier3d::control::{DynamicRayCastVehicleController, WheelTuning}; +use rapier3d::prelude::*; + +/// A vehicle with wheel axles along +X drives along -Z (its per-wheel forward direction), +/// which is orthogonal to the default `index_forward_axis` (X). The wheels must still +/// accumulate rotation, consistently across all four wheels. +#[test] +fn wheels_rotate_when_rolling_off_the_chassis_forward_axis() { + let mut world = PhysicsWorld::new(); + + let ground = + ColliderBuilder::cuboid(100.0, 0.1, 100.0).translation(Vector::new(0.0, -0.1, 0.0)); + world.insert_collider(ground, None); + + let hw = 0.3; + let hh = 0.15; + let chassis = RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 0.3, 0.0)); + let collider = ColliderBuilder::cuboid(hw, hh, hw * 2.0).density(100.0); + let (vehicle_handle, _) = world.insert(chassis, collider); + + let tuning = WheelTuning { + suspension_stiffness: 100.0, + suspension_damping: 10.0, + ..WheelTuning::default() + }; + let mut vehicle = DynamicRayCastVehicleController::new(vehicle_handle); + // Axles along +X: the wheels roll along -Z, not along the default forward axis (X). + for pos in [ + Vector::new(hw, -hh, hw * 1.5), + Vector::new(-hw, -hh, hw * 1.5), + Vector::new(hw, -hh, -hw * 1.5), + Vector::new(-hw, -hh, -hw * 1.5), + ] { + vehicle.add_wheel(pos, -Vector::Y, Vector::X, hh, hh / 4.0, &tuning); + } + + // Settle, then drive straight with no steering. + for _ in 0..50 { + let q = world.broad_phase.as_query_pipeline_mut( + world.narrow_phase.query_dispatcher(), + &mut world.bodies, + &mut world.colliders, + QueryFilter::exclude_dynamic().exclude_rigid_body(vehicle_handle), + ); + vehicle.update_vehicle(world.integration_parameters.dt, q); + world.step(); + } + + let rotation_before: Vec = vehicle.wheels().iter().map(|w| w.rotation).collect(); + let z_before = world.bodies[vehicle_handle].translation().z; + + for _ in 0..120 { + for wheel in vehicle.wheels_mut() { + wheel.engine_force = 20.0; + } + let q = world.broad_phase.as_query_pipeline_mut( + world.narrow_phase.query_dispatcher(), + &mut world.bodies, + &mut world.colliders, + QueryFilter::exclude_dynamic().exclude_rigid_body(vehicle_handle), + ); + vehicle.update_vehicle(world.integration_parameters.dt, q); + world.step(); + } + + let z_after = world.bodies[vehicle_handle].translation().z; + assert!( + (z_after - z_before).abs() > 0.5, + "the vehicle should have driven along Z, moved {}", + z_after - z_before + ); + + let deltas: Vec = vehicle + .wheels() + .iter() + .zip(&rotation_before) + .map(|(w, before)| w.rotation - before) + .collect(); + for (i, delta) in deltas.iter().enumerate() { + assert!( + delta.abs() > 1.0, + "wheel {i} did not rotate while driving straight (delta {delta})" + ); + assert!( + delta.signum() == deltas[0].signum(), + "wheels rotated in inconsistent directions: {deltas:?}" + ); + } +} diff --git a/crates/rapier3d/tests/issue_946_vehicle_braking_friction.rs b/crates/rapier3d/tests/issue_946_vehicle_braking_friction.rs new file mode 100644 index 000000000..bf33b2d45 --- /dev/null +++ b/crates/rapier3d/tests/issue_946_vehicle_braking_friction.rs @@ -0,0 +1,112 @@ +//! Regression test for issue #946: when a wheel's side impulse is exactly zero (e.g. no +//! steering and no lateral slip), the skid clamp was skipped entirely and the brake +//! impulse was applied unclamped, giving the vehicle infinite effective braking friction. + +use rapier3d::control::{DynamicRayCastVehicleController, WheelTuning}; +use rapier3d::prelude::*; + +/// Braking in a straight line must be limited by tire friction (`friction_slip`): a huge +/// brake input on a fast vehicle must slow it down over several steps, not stop it dead +/// in a single step, and must never push it backwards. +#[test] +fn straight_line_braking_is_friction_limited() { + let mut world = PhysicsWorld::new(); + + let ground = + ColliderBuilder::cuboid(100.0, 0.1, 100.0).translation(Vector::new(0.0, -0.1, 0.0)); + world.insert_collider(ground, None); + + let hw = 0.3; + let hh = 0.15; + let chassis = RigidBodyBuilder::dynamic().translation(Vector::new(0.0, 0.3, 0.0)); + let collider = ColliderBuilder::cuboid(hw * 2.0, hh, hw).density(100.0); + let (vehicle_handle, _) = world.insert(chassis, collider); + + let tuning = WheelTuning { + suspension_stiffness: 100.0, + suspension_damping: 10.0, + // A small friction budget so that friction-limited braking takes many steps. + friction_slip: 2.0, + // No lateral tire force: the side impulse stays exactly 0.0, which is the + // configuration that used to skip the skid clamp entirely. + side_friction_stiffness: 0.0, + ..WheelTuning::default() + }; + let mut vehicle = DynamicRayCastVehicleController::new(vehicle_handle); + for pos in [ + Vector::new(hw * 1.5, -hh, hw), + Vector::new(hw * 1.5, -hh, -hw), + Vector::new(-hw * 1.5, -hh, hw), + Vector::new(-hw * 1.5, -hh, -hw), + ] { + vehicle.add_wheel(pos, -Vector::Y, Vector::Z, hh, hh / 4.0, &tuning); + } + + // Let the suspension settle before launching the vehicle. + for _ in 0..50 { + let q = world.broad_phase.as_query_pipeline_mut( + world.narrow_phase.query_dispatcher(), + &mut world.bodies, + &mut world.colliders, + QueryFilter::exclude_dynamic().exclude_rigid_body(vehicle_handle), + ); + vehicle.update_vehicle(world.integration_parameters.dt, q); + world.step(); + } + + let speed_before_braking = 10.0; + { + let body = &mut world.bodies[vehicle_handle]; + let vel = Vector::new(speed_before_braking, body.linvel().y, 0.0); + body.set_linvel(vel, true); + } + + // An absurdly large brake input: the resulting impulse must be clamped by the + // friction budget (suspension force * dt * friction_slip), not applied verbatim. + let mut min_speed: Real = Real::MAX; + let mut max_step_decel: Real = 0.0; + let mut speed_after_5_steps: Real = 0.0; + for i in 0..400 { + for wheel in vehicle.wheels_mut() { + wheel.brake = 1.0e9; + } + + let speed_before = world.bodies[vehicle_handle].linvel().x; + let q = world.broad_phase.as_query_pipeline_mut( + world.narrow_phase.query_dispatcher(), + &mut world.bodies, + &mut world.colliders, + QueryFilter::exclude_dynamic().exclude_rigid_body(vehicle_handle), + ); + vehicle.update_vehicle(world.integration_parameters.dt, q); + world.step(); + let speed_after = world.bodies[vehicle_handle].linvel().x; + + min_speed = min_speed.min(speed_after); + max_step_decel = max_step_decel.max(speed_before - speed_after); + if i == 4 { + speed_after_5_steps = speed_after; + } + } + + // With the friction clamp, each step can only remove + // ~2 * suspension_force * dt * friction_slip / mass of velocity (a fraction of the + // initial speed). Without it, the first braking step zeroed the velocity outright. + assert!( + speed_after_5_steps > 0.5 * speed_before_braking, + "friction-limited braking must take many steps; after 5 steps speed was {speed_after_5_steps}" + ); + assert!( + max_step_decel < 0.2 * speed_before_braking, + "per-step deceleration must be bounded by the friction budget, got {max_step_decel}" + ); + assert!( + min_speed > -0.5, + "an over-braked vehicle must not be pushed backwards, got min speed {min_speed}" + ); + let final_speed = world.bodies[vehicle_handle].linvel().x; + assert!( + final_speed.abs() < 0.5, + "vehicle should still have braked to a stop, got {final_speed}" + ); +} diff --git a/crates/rapier3d/tests/parallel_path_parity.rs b/crates/rapier3d/tests/parallel_path_parity.rs index d21f904c5..ab6a56890 100644 --- a/crates/rapier3d/tests/parallel_path_parity.rs +++ b/crates/rapier3d/tests/parallel_path_parity.rs @@ -26,7 +26,7 @@ use rapier3d::prelude::*; /// Golden hash of [`run`]. Identical in every build; re-mint (with a note saying why) /// only when a change is *meant* to alter the simulation. -const GOLDEN: u64 = 0xa8b2_0bad_6b09_7e3b; +const GOLDEN: u64 = 0x0c46_183e_4fbf_cfbb; /// FNV-1a. struct Fnv(u64); diff --git a/crates/rapier3d/tests/snapshot_portability.rs b/crates/rapier3d/tests/snapshot_portability.rs index 220bb7090..e3410b499 100644 --- a/crates/rapier3d/tests/snapshot_portability.rs +++ b/crates/rapier3d/tests/snapshot_portability.rs @@ -31,7 +31,7 @@ use rapier3d::prelude::*; /// Snapshot size in bytes, and its FNV-1a digest. Both, because the size alone localizes a /// failure: a differing size means a container's *encoding* changed, an equal size with a /// differing digest means the values did. -const GOLDEN: (usize, u64) = (481_524, 0x8cd3_f6bb_375f_8a47); +const GOLDEN: (usize, u64) = (481_524, 0x6f27_44ea_f0b9_67c7); const STEPS: usize = 60; diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 5e632599b..b3908c272 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -21,9 +21,11 @@ mod character_controller2; mod collision_groups2; mod convex_polygons2; mod damping2; +mod debug_angular_limits2; mod debug_box_ball2; mod debug_compression2; mod debug_intersection2; +mod debug_many_colliders2; mod debug_total_overlap2; mod debug_vertical_column2; mod drum2; @@ -112,9 +114,11 @@ pub async fn main() { // ── Controls ──────────────────────────────────────────────────────── CONTROLS, "Character controller", character_controller2::run; // ── Debug ─────────────────────────────────────────────────────────── + DEBUG, "Angular limits", debug_angular_limits2::run; DEBUG, "Box ball", debug_box_ball2::run; DEBUG, "Compression", debug_compression2::run; DEBUG, "Intersection", debug_intersection2::run; + DEBUG, "Many colliders", debug_many_colliders2::run; DEBUG, "Total overlap", debug_total_overlap2::run; DEBUG, "Vertical column", debug_vertical_column2::run; // ── Inspired by Solver2D ──────────────────────────────────────────── diff --git a/examples2d/debug_angular_limits2.rs b/examples2d/debug_angular_limits2.rs new file mode 100644 index 000000000..7758d49bd --- /dev/null +++ b/examples2d/debug_angular_limits2.rs @@ -0,0 +1,93 @@ +//! Showcases angular joint limits sitting anywhere on the circle (issue #499): each "dial" +//! is a motor-driven arm on a revolute joint whose limit range is drawn from a different +//! family. The top row is driven counter-clockwise and parks at the range's max; the bottom +//! row is driven clockwise and parks at the min. The last column's range is wider than a +//! full turn, which is indistinguishable from "no limit" for a wrapped angle: those arms +//! spin forever. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +/// The showcased `[min, max]` limit ranges, in degrees, with the family each illustrates +/// (shown in the UI, one entry per dial column from left to right). +const LIMITS_DEG: [([f32; 2], &str); 5] = [ + // Already worked before the fix. + ([-45.0, 45.0], "within half a turn"), + // Used to fold back (stopped at 90° instead of 270°). + ([0.0, 270.0], "past half a turn"), + ([135.0, 225.0], "straddles the ±180° seam"), + ([-350.0, 0.0], "nearly a full turn"), + // The limit row is disabled, the arm spins freely. + ([-200.0, 200.0], "wider than a turn: free"), +]; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + /* + * World: no gravity so the motors alone decide where each arm settles. + */ + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + + let settings = viewer.example_settings_mut(); + let use_multibody = settings.get_or_set_bool("Multibody joints", false); + + // The dial ranges, shown in the Example Settings window (left to right). + const COLUMN_KEYS: [&str; 5] = ["Column 1", "Column 2", "Column 3", "Column 4", "Column 5"]; + for (key, (limits, family)) in COLUMN_KEYS.iter().zip(LIMITS_DEG.iter()) { + settings.set_label(key, format!("[{}°, {}°] — {family}", limits[0], limits[1])); + } + settings.set_label("Rows", "top: driven to the max — bottom: to the min"); + + let spacing = 4.0; + + for (i, (limits_deg, _)) in LIMITS_DEG.iter().enumerate() { + // dir = 1: driven counter-clockwise toward the max; dir = -1: toward the min. + for dir in [1.0f32, -1.0] { + let center = Vector::new(i as f32 * spacing, if dir > 0.0 { 0.0 } else { -spacing }); + + // A small fixed disc marking the dial's center. + let anchor = world + .bodies + .insert(RigidBodyBuilder::fixed().translation(center)); + world.insert_collider(ColliderBuilder::ball(0.2), Some(anchor)); + + // The arm. Angular damping caps the speed it reaches the limit with, so it + // settles right at the bound instead of oscillating around it. + let arm = world.bodies.insert( + RigidBodyBuilder::dynamic() + .translation(center + Vector::new(1.0, 0.0)) + .angular_damping(3.0) + .can_sleep(false), + ); + world.insert_collider(ColliderBuilder::cuboid(0.5, 0.1), Some(arm)); + + let joint = RevoluteJointBuilder::new() + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::new(-1.0, 0.0)) + .limits([limits_deg[0].to_radians(), limits_deg[1].to_radians()]) + .motor_velocity(dir * 5.0, 20.0); + + if use_multibody { + world + .multibody_joints + .insert(anchor, arm, joint, true) + .unwrap(); + } else { + world.impulse_joints.insert(anchor, arm, joint, true); + } + } + } + + /* + * Set up the testbed. + */ + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(2.0 * spacing, -0.5 * spacing), 40.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples2d/debug_many_colliders2.rs b/examples2d/debug_many_colliders2.rs new file mode 100644 index 000000000..c05a6c9d6 --- /dev/null +++ b/examples2d/debug_many_colliders2.rs @@ -0,0 +1,205 @@ +//! Reproduction of issue #970: a single always-awake dynamic body carrying 2,210 convex +//! colliders (130 copies of a 17-part convex decomposition, all at the same pose), in a +//! zero-gravity world with nothing else. The body spins forever, so the step cost is +//! entirely spent processing the moving colliders of that one body. +//! +//! Set [`WITH_COMPOUND_COMPARISON`] to `true` to also spawn a second body carrying the +//! very same parts as one compound-shape collider per copy: the broad phase then sees +//! 130 colliders for it instead of 2,210. + +use rapier_testbed2d::TestbedViewer; +use rapier2d::prelude::*; + +/// The poster's 17 convex polygons (a convex decomposition of a complex outline), +/// in the original 0.01-scaled coordinates. +// The vertex data is verbatim from the issue's JS reproduction, where two vertices carry +// f64 precision. +#[allow(clippy::excessive_precision)] +fn decomposition_parts() -> Vec> { + let parts: &[&[[f32; 2]]] = &[ + &[[525.0, 104.0], [540.0, 104.0], [419.0, 119.0]], + &[[419.0, 119.0], [449.0, 74.0], [510.0, 59.0], [525.0, 104.0]], + &[ + [299.0, 134.0], + [419.0, 119.0], + [540.0, 104.0], + [540.0, 134.0], + ], + &[ + [315.0, 450.0], + [179.0, 284.0], + [179.0, 254.0], + [224.0, 224.0], + ], + &[ + [224.0, 224.0], + [224.0, 223.0], + [299.0, 134.0], + [540.0, 134.0], + [555.0, 134.0], + [555.0, 209.0], + [359.0, 465.0], + [315.0, 450.0], + ], + &[ + [119.0, 359.0], + [134.0, 314.0], + [179.0, 284.0], + [315.0, 450.0], + [315.0, 465.0], + [300.0, 465.0], + ], + &[[164.0, 510.0], [134.0, 495.0], [240.0, 510.0]], + &[ + [240.0, 510.0], + [240.0, 525.0], + [164.0, 525.0], + [164.0, 510.0], + ], + &[ + [134.0, 495.0], + [104.0, 359.0], + [119.0, 359.0], + [300.0, 465.0], + [270.0, 510.0], + [240.0, 510.0], + ], + &[[615.0, 269.0], [660.0, 284.0], [660.0, 359.0]], + &[ + [673.6813186813187, 390.010989010989], + [660.0, 359.0], + [675.0, 359.0], + [675.0, 389.0], + ], + &[ + [675.0, 389.0], + [735.0, 434.0], + [735.0, 495.0], + [720.0, 495.0], + [673.6813186813187, 390.010989010989], + ], + &[ + [645.0, 540.0], + [645.0, 555.0], + [494.0, 555.0], + [494.0, 540.0], + ], + &[ + [705.0, 525.0], + [645.0, 540.0], + [494.0, 540.0], + [464.0, 540.0], + [464.0, 525.0], + ], + &[ + [660.0, 359.0], + [720.0, 495.0], + [705.0, 525.0], + [464.0, 525.0], + [434.0, 525.0], + [434.0, 510.0], + ], + &[ + [660.0, 359.0], + [434.0, 510.0], + [404.0, 510.0], + [404.0, 495.0], + ], + &[ + [404.0, 495.0], + [359.0, 495.0], + [359.0, 465.0], + [555.0, 209.0], + [570.0, 209.0], + [615.0, 269.0], + [660.0, 359.0], + ], + ]; + + const SCALING: f32 = 0.01; + parts + .iter() + .map(|part| { + part.iter() + .map(|[x, y]| Vector::new(x * SCALING, y * SCALING)) + .collect() + }) + .collect() +} + +const NUM_COPIES: usize = 130; + +/// Spawn a second body using compound shapes instead of individual colliders, for +/// an A/B comparison of the two approaches. +const WITH_COMPOUND_COMPARISON: bool = false; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + /* + * World: no gravity, and the body is never allowed to sleep — the whole step cost + * comes from the pipeline processing the colliders of a single body. + */ + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + + let parts = decomposition_parts(); + + /* + * One dynamic body with 130 × 17 = 2,210 individual convex colliders, all at the + * same pose (the poster's scene, with `wakeUp()` before every step replaced by + * disabling sleep). The constant spin never decays (no gravity, damping, or + * contacts), so the colliders keep moving indefinitely and the scene exercises the + * broad phase's moving-leaves path on every step, not just the awake-body one. + */ + let body = world + .bodies + .insert(RigidBodyBuilder::dynamic().can_sleep(false).angvel(1.0)); + for _ in 0..NUM_COPIES { + for part in &parts { + let collider = ColliderBuilder::convex_polyline(part.clone()) + .expect("the decomposition parts are valid convex polygons") + .density(1.0); + world.insert_collider(collider, Some(body)); + } + } + + /* + * The optional comparison body: the same 130 copies of the decomposition, but each + * copy is a single compound-shape collider (17 parts each), so the broad phase sees + * 130 colliders instead of 2,210. + */ + if WITH_COMPOUND_COMPARISON { + let compound_parts: Vec<_> = parts + .iter() + .map(|part| { + ( + Pose::IDENTITY, + SharedShape::convex_polyline(part.clone()) + .expect("the decomposition parts are valid convex polygons"), + ) + }) + .collect(); + let compound_body = world.bodies.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(10.0, 0.0)) + .can_sleep(false) + .angvel(1.0), + ); + for _ in 0..NUM_COPIES { + let collider = ColliderBuilder::compound(compound_parts.clone()).density(1.0); + world.insert_collider(collider, Some(compound_body)); + } + } + + /* + * Set up the testbed. + */ + viewer.set_world(&mut world); + viewer.look_at(Vec2::new(5.0, 3.0), 40.0); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 6f0e5d3b5..de352617a 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -23,6 +23,7 @@ mod convex_decomposition3; mod convex_polyhedron3; mod damping3; mod debug_add_remove_collider3; +mod debug_angular_limits3; mod debug_articulations3; mod debug_balls3; mod debug_big_colliders3; @@ -138,6 +139,7 @@ pub async fn main() { ROBOTICS, "MJCF", mjcf3::run; ROBOTICS, "Mujoco Menagerie", mujoco_menagerie3::run; // ── Debug ─────────────────────────────────────────────────────────── + DEBUG, "Angular limits", debug_angular_limits3::run; DEBUG, "Multibody joints", debug_articulations3::run; DEBUG, "Add/rm collider", debug_add_remove_collider3::run; DEBUG, "Multi-collider body", debug_multi_collider_body3::run; diff --git a/examples3d/debug_angular_limits3.rs b/examples3d/debug_angular_limits3.rs new file mode 100644 index 000000000..dce320f5d --- /dev/null +++ b/examples3d/debug_angular_limits3.rs @@ -0,0 +1,101 @@ +//! Showcases angular joint limits sitting anywhere on the circle (issue #499): each "dial" +//! is a motor-driven arm on a revolute joint (axis Z) whose limit range is drawn from a +//! different family. The top row is driven counter-clockwise and parks at the range's max; +//! the bottom row is driven clockwise and parks at the min. The last column's range is +//! wider than a full turn, which is indistinguishable from "no limit" for a wrapped angle: +//! those arms spin forever. + +use rapier_testbed3d::TestbedViewer; +use rapier3d::prelude::*; + +/// The showcased `[min, max]` limit ranges, in degrees, with the family each illustrates +/// (shown in the UI, one entry per dial column from left to right). +const LIMITS_DEG: [([f32; 2], &str); 5] = [ + // Already worked before the fix. + ([-45.0, 45.0], "within half a turn"), + // Used to fold back (stopped at 90° instead of 270°). + ([0.0, 270.0], "past half a turn"), + ([135.0, 225.0], "straddles the ±180° seam"), + ([-350.0, 0.0], "nearly a full turn"), + // The limit row is disabled, the arm spins freely. + ([-200.0, 200.0], "wider than a turn: free"), +]; + +pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { + /* + * World: no gravity so the motors alone decide where each arm settles. + */ + let mut world = PhysicsWorld::new(); + world.gravity = Vector::ZERO; + + let settings = viewer.example_settings_mut(); + let use_multibody = settings.get_or_set_bool("Multibody joints", false); + + // The dial ranges, shown in the Example Settings window (left to right). + const COLUMN_KEYS: [&str; 5] = ["Column 1", "Column 2", "Column 3", "Column 4", "Column 5"]; + for (key, (limits, family)) in COLUMN_KEYS.iter().zip(LIMITS_DEG.iter()) { + settings.set_label(key, format!("[{}°, {}°] — {family}", limits[0], limits[1])); + } + settings.set_label("Rows", "top: driven to the max — bottom: to the min"); + + let spacing = 4.0; + + for (i, (limits_deg, _)) in LIMITS_DEG.iter().enumerate() { + // dir = 1: driven counter-clockwise (looking down -Z) toward the max; + // dir = -1: toward the min. + for dir in [1.0f32, -1.0] { + let center = Vector::new( + i as f32 * spacing, + if dir > 0.0 { 0.0 } else { -spacing }, + 0.0, + ); + + // A small fixed ball marking the dial's center. + let anchor = world + .bodies + .insert(RigidBodyBuilder::fixed().translation(center)); + world.insert_collider(ColliderBuilder::ball(0.2), Some(anchor)); + + // The arm. Angular damping caps the speed it reaches the limit with, so it + // settles right at the bound instead of oscillating around it. + let arm = world.bodies.insert( + RigidBodyBuilder::dynamic() + .translation(center + Vector::new(1.0, 0.0, 0.0)) + .angular_damping(3.0) + .can_sleep(false), + ); + world.insert_collider(ColliderBuilder::cuboid(0.5, 0.1, 0.1), Some(arm)); + + let joint = RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::new(-1.0, 0.0, 0.0)) + .limits([limits_deg[0].to_radians(), limits_deg[1].to_radians()]) + .motor_velocity(dir * 5.0, 20.0); + + if use_multibody { + world + .multibody_joints + .insert(anchor, arm, joint, true) + .unwrap(); + } else { + world.impulse_joints.insert(anchor, arm, joint, true); + } + } + } + + /* + * Set up the testbed. + */ + viewer.set_world(&mut world); + viewer.look_at( + Vec3::new(2.0 * spacing, -0.5 * spacing, 25.0), + Vec3::new(2.0 * spacing, -0.5 * spacing, 0.0), + ); + + while viewer.render_frame(&mut world).await { + if viewer.simulating() { + world.step(); + } + } + Ok(()) +} diff --git a/python/rapier-py-3d/src/dynamics.rs b/python/rapier-py-3d/src/dynamics.rs index 56f4336a2..3ed77a4c2 100644 --- a/python/rapier-py-3d/src/dynamics.rs +++ b/python/rapier-py-3d/src/dynamics.rs @@ -211,6 +211,8 @@ pub enum CoefficientCombineRule { MAX, /// Sum of the two coefficients, clamped to ``[0, 1]``. CLAMPED_SUM, + /// Square root of the product of the two coefficients. + GEOMETRIC_MEAN, } impl CoefficientCombineRule { @@ -223,6 +225,7 @@ impl CoefficientCombineRule { Self::MULTIPLY => rapier::dynamics::CoefficientCombineRule::Multiply, Self::MAX => rapier::dynamics::CoefficientCombineRule::Max, Self::CLAMPED_SUM => rapier::dynamics::CoefficientCombineRule::ClampedSum, + Self::GEOMETRIC_MEAN => rapier::dynamics::CoefficientCombineRule::GeometricMean, } } #[allow(dead_code)] @@ -234,6 +237,7 @@ impl CoefficientCombineRule { rapier::dynamics::CoefficientCombineRule::Multiply => Self::MULTIPLY, rapier::dynamics::CoefficientCombineRule::Max => Self::MAX, rapier::dynamics::CoefficientCombineRule::ClampedSum => Self::CLAMPED_SUM, + rapier::dynamics::CoefficientCombineRule::GeometricMean => Self::GEOMETRIC_MEAN, } } } diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs index 1822e3167..3395aedb4 100644 --- a/src/control/ray_cast_vehicle_controller.rs +++ b/src/control/ray_cast_vehicle_controller.rs @@ -465,10 +465,15 @@ impl DynamicRayCastVehicleController { let vel = chassis.velocity_at_point(wheel.raycast_info.hard_point_ws); if wheel.raycast_info.is_in_contact { - let mut fwd = - chassis.position().rotation * Vector::ith(self.index_forward_axis, 1.0); - let proj = fwd.dot(wheel.raycast_info.contact_normal_ws); - fwd -= wheel.raycast_info.contact_normal_ws * proj; + // Use the same per-wheel forward direction as `update_friction` (rolling + // direction on the contact plane) instead of the chassis' forward axis: + // the latter yields a zero or sign-flipped rotation whenever it doesn't + // match the wheel's actual rolling direction. + let fwd = wheel + .raycast_info + .contact_normal_ws + .cross(wheel.wheel_axle_ws) + .normalize_or_zero(); let proj2 = fwd.dot(vel); @@ -659,7 +664,7 @@ impl DynamicRayCastVehicleController { if sliding { for wheel in &mut self.wheels { - if wheel.side_impulse != 0.0 && wheel.skid_info < 1.0 { + if wheel.skid_info < 1.0 { wheel.forward_impulse *= wheel.skid_info; wheel.side_impulse *= wheel.skid_info; } diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs index 721e2b036..4a8dafebb 100644 --- a/src/dynamics/coefficient_combine_rule.rs +++ b/src/dynamics/coefficient_combine_rule.rs @@ -1,10 +1,14 @@ use crate::math::Real; +// Provides `sqrt` in no-std builds (same pattern as island_manager/local_split.rs). +#[allow(unused_imports)] +use simba::scalar::ComplexField as _; /// How to combine friction/restitution values when two colliders touch. /// /// When two colliders with different friction (or restitution) values collide, Rapier /// needs to decide what the effective friction/restitution should be. Each collider has -/// a combine rule, and the "stronger" rule wins (Max > Multiply > Min > Average). +/// a combine rule, and the "stronger" rule wins +/// (GeometricMean > ClampedSum > Max > Multiply > Min > Average). /// /// ## Combine Rules /// @@ -15,6 +19,7 @@ use crate::math::Real; /// - **Multiply**: `friction1 × friction2` - Both must be high for high friction /// - **Max**: `max(friction1, friction2)` - "Sticky wins" (rubber on any surface = rubber) /// - **ClampedSum**: `sum(friction1, friction2).clamp(0, 1)` - Sum of both frictions, clamped to range 0, 1. +/// - **GeometricMean**: `sqrt(friction1 × friction2)` - Between Multiply and Average; zero if either is zero. /// /// ## Example /// ``` @@ -27,7 +32,8 @@ use crate::math::Real; /// ``` /// /// ## Priority System -/// If colliders disagree on rules, the "higher" one wins: ClampedSum > Max > Multiply > Min > Average +/// If colliders disagree on rules, the "higher" one wins: +/// GeometricMean > ClampedSum > Max > Multiply > Min > Average #[derive(Default, Copy, Clone, Debug, PartialEq, Eq, PartialOrd, Ord)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub enum CoefficientCombineRule { @@ -42,6 +48,12 @@ pub enum CoefficientCombineRule { Max = 3, /// The clamped sum of the two coefficients. ClampedSum = 4, + /// The square root of the product of the two values. + /// + /// A common convention in other engines (e.g. Bullet, PhysX): stricter than Average + /// (either value being zero results in zero) but less aggressive than Multiply for + /// values below 1. + GeometricMean = 5, } impl CoefficientCombineRule { @@ -65,6 +77,53 @@ impl CoefficientCombineRule { CoefficientCombineRule::Multiply => coeff1 * coeff2, CoefficientCombineRule::Max => coeff1.max(coeff2), CoefficientCombineRule::ClampedSum => (coeff1 + coeff2).clamp(0.0, 1.0), + // Negative coefficients are tolerated (see the Min comment above), so clamp + // before taking the square root to avoid NaN on a negative product. + CoefficientCombineRule::GeometricMean => (coeff1.max(0.0) * coeff2.max(0.0)).sqrt(), } } } + +#[cfg(test)] +mod test { + use super::CoefficientCombineRule; + use crate::math::Real; + + fn combine(c1: Real, c2: Real, rule: CoefficientCombineRule) -> Real { + CoefficientCombineRule::combine(c1, c2, rule, rule) + } + + #[test] + fn geometric_mean_combine() { + assert_eq!( + combine(0.25, 1.0, CoefficientCombineRule::GeometricMean), + 0.5 + ); + assert_eq!( + combine(0.7, 0.0, CoefficientCombineRule::GeometricMean), + 0.0 + ); + // Negative coefficients (tolerated for the godot use-case) must not produce NaN. + assert_eq!( + combine(-0.5, 0.5, CoefficientCombineRule::GeometricMean), + 0.0 + ); + assert_eq!( + combine(-0.5, -0.5, CoefficientCombineRule::GeometricMean), + 0.0 + ); + } + + #[test] + fn geometric_mean_wins_rule_priority() { + assert_eq!( + CoefficientCombineRule::combine( + 0.25, + 1.0, + CoefficientCombineRule::GeometricMean, + CoefficientCombineRule::Average, + ), + 0.5 + ); + } +} diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index ed17ee95b..e814f0c24 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -132,6 +132,11 @@ impl From for JointAxesMask { /// - Elbow that bends 0-150°: revolute joint with limits `[0.0, 5*PI/6]` /// /// When a joint hits its limit, forces are applied to prevent further movement in that direction. +/// +/// An angular range may sit anywhere on the circle (`[0, 3π/2]` and `[π, 3π/2]` both work), but +/// it can't be wider than a full turn: the joint's angle is derived from the bodies' relative +/// rotation, which doesn't count revolutions, so a wider range is indistinguishable from no +/// limit at all and leaves the axis free. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] pub struct JointLimits { diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index a3c0466ba..7f18b0d9b 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -304,7 +304,7 @@ impl MultibodyJointSet { /// Returns the link of this multibody attached to the given rigid-body. /// - /// Returns `None` if `rb` isn’t part of any rigid-body. + /// Returns `None` if `rb` isn’t part of any multibody. pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyLinkId> { self.rb2mb.get(rb.0) } diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index e6264348b..01692cdb6 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -190,7 +190,10 @@ impl RevoluteJoint { /// Restricts rotation to a specific angle range. /// /// # Parameters - /// * `limits` - `[min_angle, max_angle]` in radians + /// * `limits` - `[min_angle, max_angle]` in radians. The range may sit anywhere on the + /// circle (e.g. `[0, 3π/2]`), but it can't be wider than a full turn: the joint's angle + /// is derived from the bodies' relative rotation, which doesn't count revolutions, so a + /// wider range is indistinguishable from no limit at all and leaves the joint free. /// /// # Example /// ``` diff --git a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs index 94a747016..941bc6473 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs @@ -681,11 +681,16 @@ impl ContactWithCoulombFriction { #[cfg(feature = "dim3")] let tangent2 = self.dir1.gcross(self.tangent1); for k in 0..self.num_contacts as usize { - let warmstart_impulses: [_; SIMD_WIDTH] = self.normal_part[k].impulse.into(); - let warmstart_tangent_impulses = self.tangent_part[k].impulse; + // The stored impulses are serialized state: canonicalize signed zeros (see + // `utils::canonicalize_zero`) so snapshots stay cross-platform deterministic. + let warmstart_impulses: [_; SIMD_WIDTH] = + utils::canonicalize_zero(self.normal_part[k].impulse).into(); + let warmstart_tangent_impulses = utils::canonicalize_zero(self.tangent_part[k].impulse); #[cfg(feature = "dim3")] - let warmstart_tangent_world = self.tangent1 * warmstart_tangent_impulses.x - + tangent2 * warmstart_tangent_impulses.y; + let warmstart_tangent_world = utils::canonicalize_zero( + self.tangent1 * warmstart_tangent_impulses.x + + tangent2 * warmstart_tangent_impulses.y, + ); #[cfg(feature = "dim3")] let (wx, wy, wz): ( [Real; SIMD_WIDTH], @@ -696,8 +701,9 @@ impl ContactWithCoulombFriction { warmstart_tangent_world.y.into(), warmstart_tangent_world.z.into(), ); - let impulses: [_; SIMD_WIDTH] = self.normal_part[k].total_impulse().into(); - let tangent_impulses = self.tangent_part[k].total_impulse(); + let impulses: [_; SIMD_WIDTH] = + utils::canonicalize_zero(self.normal_part[k].total_impulse()).into(); + let tangent_impulses = utils::canonicalize_zero(self.tangent_part[k].total_impulse()); for ii in 0..SIMD_WIDTH { let contact_id = self.manifold_contact_id[k][ii]; diff --git a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs index 5ae3a60e1..11c9236a9 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs @@ -775,21 +775,27 @@ impl ContactWithTwistFriction { } pub fn writeback_impulses(&self, manifolds_all: &ManifoldStore) { - let warmstart_tangent_impulses = self.tangent_part.impulse; + // The stored impulses are serialized state: canonicalize signed zeros (see + // `utils::canonicalize_zero`) so snapshots stay cross-platform deterministic. + let warmstart_tangent_impulses = utils::canonicalize_zero(self.tangent_part.impulse); // World-space friction impulse (see `ContactData::warmstart_tangent_world`). let tangent2 = self.dir1.gcross(self.tangent1); - let warmstart_tangent_world = - self.tangent1 * warmstart_tangent_impulses.x + tangent2 * warmstart_tangent_impulses.y; + let warmstart_tangent_world = utils::canonicalize_zero( + self.tangent1 * warmstart_tangent_impulses.x + tangent2 * warmstart_tangent_impulses.y, + ); let (wx, wy, wz): ([Real; SIMD_WIDTH], [Real; SIMD_WIDTH], [Real; SIMD_WIDTH]) = ( warmstart_tangent_world.x.into(), warmstart_tangent_world.y.into(), warmstart_tangent_world.z.into(), ); - let warmstart_twist_impulses: [_; SIMD_WIDTH] = self.twist_part.impulse.into(); + let warmstart_twist_impulses: [_; SIMD_WIDTH] = + utils::canonicalize_zero(self.twist_part.impulse).into(); for k in 0..self.num_contacts as usize { - let warmstart_impulses: [_; SIMD_WIDTH] = self.normal_part[k].impulse.into(); - let impulses: [_; SIMD_WIDTH] = self.normal_part[k].total_impulse().into(); + let warmstart_impulses: [_; SIMD_WIDTH] = + utils::canonicalize_zero(self.normal_part[k].impulse).into(); + let impulses: [_; SIMD_WIDTH] = + utils::canonicalize_zero(self.normal_part[k].total_impulse()).into(); for ii in 0..SIMD_WIDTH { let contact_id = self.manifold_contact_id[k][ii]; diff --git a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs index 861ccf0d4..3e0a0a70f 100644 --- a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs @@ -4,7 +4,7 @@ use crate::geometry::ContactManifold; #[cfg(feature = "dim3")] use crate::math::TangentImpulse; use crate::math::{DIM, DVector, MAX_MANIFOLD_POINTS, Real}; -use crate::utils::{AngularInertiaOps, CrossProduct, DotProduct}; +use crate::utils::{self, AngularInertiaOps, CrossProduct, DotProduct}; use super::{ContactConstraintNormalPart, ContactConstraintTangentPart}; use crate::dynamics::solver::CoulombContactPointInfos; @@ -702,16 +702,22 @@ impl GenericContactConstraint { for k in 0..self.num_contacts as usize { let contact_id = self.manifold_contact_id[k]; let active_contact = &mut manifold.points[contact_id as usize]; - active_contact.data.warmstart_impulse = self.normal_part[k].impulse; - active_contact.data.warmstart_tangent_impulse = self.tangent_part[k].impulse; + // The stored impulses are serialized state: canonicalize signed zeros (see + // `utils::canonicalize_zero`) so snapshots stay cross-platform deterministic. + active_contact.data.warmstart_impulse = + utils::canonicalize_zero(self.normal_part[k].impulse); + active_contact.data.warmstart_tangent_impulse = + utils::canonicalize_zero(self.tangent_part[k].impulse); #[cfg(feature = "dim3")] { - let imp = self.tangent_part[k].impulse; + let imp = active_contact.data.warmstart_tangent_impulse; active_contact.data.warmstart_tangent_world = - self.tangent1 * imp.x + tangent2 * imp.y; + utils::canonicalize_zero(self.tangent1 * imp.x + tangent2 * imp.y); } - active_contact.data.impulse = self.normal_part[k].total_impulse(); - active_contact.data.tangent_impulse = self.tangent_part[k].total_impulse(); + active_contact.data.impulse = + utils::canonicalize_zero(self.normal_part[k].total_impulse()); + active_contact.data.tangent_impulse = + utils::canonicalize_zero(self.tangent_part[k].total_impulse()); } } diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs index 8a563b46f..b2d32f1a8 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -4,7 +4,9 @@ use simba::scalar::ComplexField; use crate::dynamics::solver::MotorParameters; use crate::dynamics::solver::joint_constraint::generic_joint_constraint::GenericJointConstraint; use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId; -use crate::dynamics::solver::joint_constraint::{JointConstraintHelper, JointSolverBody}; +use crate::dynamics::solver::joint_constraint::{ + AngularLimitParams, JointConstraintHelper, JointSolverBody, +}; use crate::dynamics::{ GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex, Multibody, MultibodyJointSet, MultibodyLinkId, RigidBodySet, @@ -559,7 +561,10 @@ impl JointConstraintHelper { let erp_inv_dt = softness.erp_inv_dt(params.dt); - let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt; + // See `limit_linear`: the bias is capped so deep violations don't catapult. + let max_bias = params.max_corrective_velocity(); + let rhs_bias = (((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt) + .clamp(-max_bias, max_bias); constraint.rhs += rhs_bias; constraint.impulse_bounds = [ min_enabled as u32 as Real * -Real::MAX, @@ -681,10 +686,14 @@ impl JointConstraintHelper { softness: SpringCoefficients, writeback_id: WritebackId, ) -> GenericJointConstraint { + // See `AngularLimitParams`: the row measures the wrapped angle from the middle of + // the allowed range; the measure's gradient is the plain joint axis (like the + // angular motor row). + let limit = AngularLimitParams::new(limits[0], limits[1]); #[cfg(feature = "dim2")] let ang_jac: AngVector = 1.0; #[cfg(feature = "dim3")] - let ang_jac = self.ang_basis.column(_limited_axis); + let ang_jac = self.basis.col(_limited_axis); let mut constraint = self.lock_jacobians_generic( jacobians, @@ -700,21 +709,21 @@ impl JointConstraintHelper { ang_jac, ); - let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()]; - #[cfg(feature = "dim2")] - let s_ang = (self.ang_err.angle() / 2.0).sin(); - #[cfg(feature = "dim3")] - let s_ang = self.ang_err.xyz()[_limited_axis]; - let min_enabled = s_ang <= s_limits[0]; - let max_enabled = s_limits[1] <= s_ang; + let ang_limits = [-limit.half_range, limit.half_range]; + let ang = self.recentered_angle(_limited_axis, &limit); + let min_enabled = ang <= ang_limits[0]; + let max_enabled = ang_limits[1] <= ang; let impulse_bounds = [ min_enabled as u32 as Real * -Real::MAX, max_enabled as u32 as Real * Real::MAX, ]; let erp_inv_dt = softness.erp_inv_dt(params.dt); - let rhs_bias = - ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt; + // See `limit_angular`: the bias is capped so deep violations don't catapult. + let max_bias = params.max_corrective_velocity(); + let rhs_bias = (((ang - ang_limits[1]).max(0.0) - (ang_limits[0] - ang).max(0.0)) + * erp_inv_dt) + .clamp(-max_bias, max_bias); constraint.rhs += rhs_bias; constraint.impulse_bounds = impulse_bounds; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 0d14a72f0..72dbdff54 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -1,15 +1,13 @@ -use crate::dynamics::solver::joint_constraint::JointSolverBody; use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ JointConstraint, WritebackId, }; +use crate::dynamics::solver::joint_constraint::{AngularLimitParams, JointSolverBody}; use crate::dynamics::solver::solver_body::SolverBodies; use crate::dynamics::solver::{joint_data_num_constraints, joint_num_constraints}; use crate::dynamics::{GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex}; -use crate::math::{Real, SPATIAL_DIM}; +use crate::math::{ANG_DIM, Real, SPATIAL_DIM}; use crate::prelude::RigidBodySet; -#[cfg(not(feature = "std"))] -use simba::scalar::ComplexField as _; use { crate::dynamics::SpringCoefficients, crate::dynamics::solver::MotorParameters, @@ -23,6 +21,9 @@ pub struct JointConstraintBuilder { body2: u32, joint_id: JointIndex, joint: GenericJoint, + /// The limited angular axes' limits, in the form the rows consume. Pre-computed here so + /// the per-substep row rebuild never calls `sin`/`cos`. + ang_limits: [AngularLimitParams; ANG_DIM], constraint_id: usize, /// The per-dof impulses written back at the end of the previous step, used to /// seed the constraint impulses when joint warm-starting is enabled. @@ -47,6 +48,10 @@ impl JointConstraintBuilder { body2: solver_body2, joint_id, joint: joint.data, + ang_limits: core::array::from_fn(|ang_axis| { + let limits = &joint.data.limits[crate::math::DIM + ang_axis]; + AngularLimitParams::new(limits.min, limits.max) + }), constraint_id: *out_constraint_id, prev_dof_impulses: joint.impulses, }; @@ -123,6 +128,7 @@ impl JointConstraintBuilder { &frame1, &frame2, &self.joint, + &self.ang_limits, out_rows, ); @@ -178,10 +184,12 @@ pub struct JointConstraintBuilderSimd { /// Like `prev_dof_impulses`, for the 2D angular motor row. #[cfg(feature = "dim2")] prev_motor_impulse: SimdReal, - /// Per-axis `[min, max]` limits of the limited axes (unset axes are zero). Linear axes hold - /// raw limits; angular axes hold the SINES OF THE HALF-ANGLE limits (`sin(limit / 2)`, what - /// `limit_angular` consumes) — pre-computed so the per-substep row rebuild never calls `sin`. - limits: [[SimdReal; 2]; SPATIAL_DIM], + /// Per-axis `[min, max]` limits of the limited linear axes (unset axes are zero). + /// The angular axes go through `ang_limits`. + limits: [[SimdReal; 2]; DIM], + /// The limited angular axes' limits, in the form the rows consume — pre-computed so the + /// per-substep row rebuild never calls `sin`/`cos`. + ang_limits: [AngularLimitParams; ANG_DIM], softness: SpringCoefficients, constraint_id: usize, /// Per-dof impulses written back at the end of the previous step (one SIMD lane @@ -274,18 +282,28 @@ impl JointConstraintBuilderSimd { prev_motor_impulse: array![|ii| ang_motor(ii).impulse].into(), limits: core::array::from_fn(|axis| { if limit_axes & (1 << axis) != 0 { - // Angular limits are stored as half-angle sines (see the - // field docs); the scalar `sin` runs once per assembly - // rebuild, not per substep. - let map = |x: Real| if axis >= DIM { (x * 0.5).sin() } else { x }; [ - array![|ii| map(joint[ii].data.limits[axis].min)].into(), - array![|ii| map(joint[ii].data.limits[axis].max)].into(), + array![|ii| joint[ii].data.limits[axis].min].into(), + array![|ii| joint[ii].data.limits[axis].max].into(), ] } else { zero2 } }), + ang_limits: core::array::from_fn(|ang_axis| { + // The per-lane scalar `sin`/`cos` run once per assembly rebuild, not per substep. + let per_lane = array![|ii| { + let limits = &joint[ii].data.limits[DIM + ang_axis]; + AngularLimitParams::new(limits.min, limits.max) + }]; + AngularLimitParams { + center: [ + array![|ii| per_lane[ii].center[0]].into(), + array![|ii| per_lane[ii].center[1]].into(), + ], + half_range: array![|ii| per_lane[ii].half_range].into(), + } + }), softness: SpringCoefficients { natural_frequency: array![|ii| joint[ii].data.softness.natural_frequency].into(), damping_ratio: array![|ii| joint[ii].data.softness.damping_ratio].into(), @@ -447,6 +465,7 @@ impl JointConstraintBuilderSimd { self.locked_axes, self.limit_axes, &self.limits, + &self.ang_limits, self.softness, ang_motor_params.as_ref(), out_rows, diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_helper.rs b/src/dynamics/solver/joint_constraint/joint_constraint_helper.rs index 37dd1c41d..5e22560fe 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_helper.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_helper.rs @@ -17,12 +17,61 @@ use crate::utils::{ PoseOps, RotationOps, ScalarType, SimdLength, }; +#[cfg(not(feature = "std"))] +use simba::scalar::ComplexField as _; + +use crate::num::FloatConst; #[cfg(feature = "dim2")] use crate::num::One; #[cfg(feature = "dim3")] use parry::math::Rot3; +/// The parameters of one angular limit row, expressed relative to the MIDDLE of the +/// allowed range: the row measures the wrapped, re-centered joint angle and compares it +/// against the symmetric bound `±half_range`. +#[derive(Debug, Copy, Clone)] +pub struct AngularLimitParams { + /// The center of the allowed range, as `[cos, sin]` — of *half* the center angle in 3D, + /// where the relative rotation is a quaternion. + pub center: [N; 2], + /// Half the allowed range (radians): the symmetric bound the re-centered angle is + /// tested against. Larger than π when the joint is effectively free, which no wrapped + /// angle can trigger. + pub half_range: N, +} + +impl AngularLimitParams { + /// The row parameters of an angular limit allowing `[min, max]` (radians). + pub fn new(min: Real, max: Real) -> Self { + let half_range = (max - min) * 0.5; + + // A range of a full turn or more is indistinguishable from "no limit" for an angle + // read off a relative rotation, so the row is disabled instead. This is also where the + // huge range of an unset limit (`JointLimits::default`) lands, and where NaN bounds + // are caught before they can poison the row. + if half_range >= Real::PI() || half_range.is_nan() { + return Self { + center: [1.0, 0.0], + half_range: 10.0, // Value greater than π means it’s unconstrained. + }; + } + + let center = (min + max) * 0.5; + #[cfg(feature = "dim2")] + let (sin, cos) = center.sin_cos(); + #[cfg(feature = "dim3")] + let (sin, cos) = (center * 0.5).sin_cos(); + + Self { + center: [cos, sin], + // Negative for an empty range (`min > max`), which makes both rows active and + // pulls the joint to the center — the only sensible reading of such a range. + half_range, + } + } +} + #[derive(Debug, Copy, Clone)] pub struct JointConstraintHelper { pub basis: N::Matrix, @@ -142,8 +191,12 @@ impl JointConstraintHelper { let min_enabled = dist.simd_le(limits[0]); let max_enabled = limits[1].simd_le(dist); - let rhs_bias = - ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt; + // Like the contact solver, cap the bias so a deep limit violation recovers over a + // few steps instead of catapulting the bodies (the erp gain is ~1/dt). + let max_bias = N::splat(params.max_corrective_velocity()); + let rhs_bias = (((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) + * erp_inv_dt) + .simd_clamp(-max_bias, max_bias); constraint.rhs = constraint.rhs_wo_bias + rhs_bias; constraint.cfm_coeff = cfm_coeff; constraint.impulse_bounds = [ @@ -201,7 +254,9 @@ impl JointConstraintHelper { let ii_ang_jac1 = body1.ii.transform_vector(ang_jac1); let ii_ang_jac2 = body2.ii.transform_vector(ang_jac2); - let rhs_bias = (dist - limits[1]).simd_max(zero) * erp_inv_dt; + let max_bias = N::splat(params.max_corrective_velocity()); + let rhs_bias = + ((dist - limits[1]).simd_max(zero) * erp_inv_dt).simd_clamp(-max_bias, max_bias); let rhs = rhs_wo_bias + rhs_bias; let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; @@ -402,10 +457,49 @@ impl JointConstraintHelper { } } - /// `s_limits` are the SINES OF THE HALF-ANGLE limits (`sin(limit / 2)`), not raw angles: - /// the row compares them against the relative rotation's half-angle sine. Pre-computing - /// keeps `sin` out of the per-substep rebuild (the wide `sin` also has pathological - /// aarch64-apple codegen: `From` repeat-expression constants lower to `memset_pattern16`). + /// The relative rotation's angle around `_limited_axis`, measured from `limit`'s center + /// and wrapped to (-π, π] — the quantity an angular limit row compares against + /// [`AngularLimitParams::half_range`]. + /// + /// Measuring the angle itself (rather than a sine of it) keeps the row's gradient with + /// respect to the joint angle equal to one everywhere on the circle, so the plain joint + /// axis is an exact jacobian for the row: a sine-space measure has a vanishing gradient + /// at the antipode of the range's center, where a limit row would degenerate. + pub fn recentered_angle(&self, _limited_axis: usize, limit: &AngularLimitParams) -> N { + let [c_cos, c_sin] = limit.center; + + // Rotate the angular error by minus the range's center, so the row measures the angle + // from there instead of from the joint's rest frame (see `AngularLimitParams`). + #[cfg(feature = "dim2")] + { + // `ang_err` is a unit complex here: (cos θ, sin θ), so re-centering is a plain + // complex product, and `atan2` wraps the result to (-π, π] by itself. + let re = c_cos * self.ang_err.real() + c_sin * self.ang_err.imag(); + let im = c_cos * self.ang_err.imag() - c_sin * self.ang_err.real(); + im.simd_atan2(re) + } + #[cfg(feature = "dim3")] + { + // Only the limited axis' imaginary part and the real part of + // `conj(center) * ang_err` are needed; the two other imaginary components only + // mix with each other. + let x = self.ang_err.imag()[_limited_axis]; + let w = self.ang_err.real(); + let sin_half = c_cos * x - c_sin * w; + let cos_half = c_cos * w + c_sin * x; + // The re-centered HALF angle, in (-π, π]. Doubling it must wrap back to + // (-π, π] as a full angle, which is a ±π shift of the half angle whenever it + // leaves (-π/2, π/2]. + let half = sin_half.simd_atan2(cos_half); + let half_pi = N::splat(Real::FRAC_PI_2()); + let shift = N::splat(Real::PI()).simd_copysign(half); + let wrapped_half = (half - shift).select(half.simd_abs().simd_gt(half_pi), half); + wrapped_half * N::splat(2.0) + } + } + + /// The limit is measured as the wrapped joint angle relative to the middle of the + /// allowed range (see [`AngularLimitParams`]). pub fn limit_angular( &self, _params: &IntegrationParameters, @@ -413,39 +507,36 @@ impl JointConstraintHelper { body1: &JointSolverBody, body2: &JointSolverBody, _limited_axis: usize, - s_limits: [N; 2], + limit: AngularLimitParams, writeback_id: WritebackId, erp_inv_dt: N, cfm_coeff: N, ) -> JointConstraint { let zero = N::zero(); - #[cfg(feature = "dim2")] - let half = N::splat(0.5); - // Half-angle identity on the unit complex: sin(θ/2) = copysign(√((1 − re)/2), im) — - // exact for θ ∈ [-π, π], much cheaper than angle() (per-lane atan2) + simd_sin, and the - // 2D analogue of the 3D branch below (quaternion imaginary part = axis·sin(θ/2)). - #[cfg(feature = "dim2")] - let s_ang = ((N::one() - self.ang_err.real()).simd_max(zero) * half) - .simd_sqrt() - .simd_copysign(self.ang_err.imag()); - #[cfg(feature = "dim3")] - let s_ang = self.ang_err.imag()[_limited_axis]; - let min_enabled = s_ang.simd_le(s_limits[0]); - let max_enabled = s_limits[1].simd_le(s_ang); + let ang = self.recentered_angle(_limited_axis, &limit); + let ang_limits = [-limit.half_range, limit.half_range]; + let min_enabled = ang.simd_le(ang_limits[0]); + let max_enabled = ang_limits[1].simd_le(ang); let impulse_bounds = [ N::splat(-Real::INFINITY).select(min_enabled, zero), N::splat(Real::INFINITY).select(max_enabled, zero), ]; + // The angle measure's gradient is the plain joint axis (like the angular motor row). #[cfg(feature = "dim2")] let ang_jac = N::AngVector::one(); #[cfg(feature = "dim3")] - let ang_jac = self.ang_basis.column(_limited_axis).into(); + let ang_jac = self.basis.column(_limited_axis).into(); let rhs_wo_bias = N::zero(); - let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero) - - (s_limits[0] - s_ang).simd_max(zero)) - * erp_inv_dt; + // Like the contact solver, cap the bias so a deep limit violation recovers over a + // few steps instead of catapulting the bodies (the erp gain is ~1/dt and the wrapped + // angular error can approach π). + let max_bias = N::splat(_params.max_corrective_velocity()); + let rhs_bias = (((ang - ang_limits[1]).simd_max(zero) + - (ang_limits[0] - ang).simd_max(zero)) + * erp_inv_dt) + .simd_clamp(-max_bias, max_bias); let ii_ang_jac1 = body1.ii.transform_vector(ang_jac); let ii_ang_jac2 = body2.ii.transform_vector(ang_jac); @@ -667,7 +758,10 @@ impl JointConstraintHelper { let rhs_wo_bias = 0.0; - let rhs_bias = ((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt; + // See `limit_angular`: the bias is capped so deep violations don't catapult. + let max_bias = _params.max_corrective_velocity(); + let rhs_bias = (((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt) + .clamp(-max_bias, max_bias); let ii_ang_jac1 = body1.ii.transform_vector(ang_jac); let ii_ang_jac2 = body2.ii.transform_vector(ang_jac); @@ -694,3 +788,60 @@ impl JointConstraintHelper { } } } + +#[cfg(all(test, feature = "dim3"))] +mod test { + use super::*; + use crate::math::{Pose, Real, Rotation, Vector}; + + /// The limit row measures the wrapped re-centered joint angle with the plain joint + /// axis as its jacobian; for that pair to be consistent, the measure's slope with + /// respect to the joint angle must be exactly one everywhere on the circle (away from + /// the wrap discontinuity at the antipode of the range's center). A sine-space + /// measure fails this: its gradient vanishes at the antipode, degenerating the row + /// (issue #499 follow-up). + #[test] + fn recentered_angle_slope_is_one_everywhere() { + let helper_at = |theta: Real| { + JointConstraintHelper::::new( + &Pose::IDENTITY, + &Pose::from_rotation(Rotation::from_axis_angle(Vector::X, theta)), + &Vector::ZERO, + &Vector::ZERO, + 0, + ) + }; + for center_deg in [-180, -135, -90, 0, 45, 135, 180] { + let limit = AngularLimitParams::::new( + (center_deg as Real - 45.0).to_radians(), + (center_deg as Real + 45.0).to_radians(), + ); + // At the center of the range the measure is zero; at the limits, ±half_range. + let at_center = + helper_at((center_deg as Real).to_radians()).recentered_angle(0, &limit); + assert!(at_center.abs() < 1.0e-5, "center {center_deg}: {at_center}"); + let at_max = + helper_at((center_deg as Real + 45.0).to_radians()).recentered_angle(0, &limit); + assert!( + (at_max - (45.0 as Real).to_radians()).abs() < 1.0e-4, + "center {center_deg}: at_max {at_max}" + ); + + for theta_deg in (-350..=350).step_by(7) { + let theta = (theta_deg as Real).to_radians(); + let eps = 1.0e-3; + let a0 = helper_at(theta - eps).recentered_angle(0, &limit); + let a1 = helper_at(theta + eps).recentered_angle(0, &limit); + // Skip the wrap discontinuity itself. + if (a1 - a0).abs() > 1.0 { + continue; + } + let slope = (a1 - a0) / (2.0 * eps); + assert!( + (slope - 1.0).abs() < 1.0e-2, + "center {center_deg} theta {theta_deg}: slope {slope}" + ); + } + } + } +} diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index db0b85bcf..e72272fff 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -1,12 +1,10 @@ use crate::dynamics::solver::SolverVel; -use crate::dynamics::solver::joint_constraint::JointConstraintHelper; +use crate::dynamics::solver::joint_constraint::{AngularLimitParams, JointConstraintHelper}; use crate::dynamics::{ GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex, }; -use crate::math::{DIM, Real, SPATIAL_DIM}; +use crate::math::{ANG_DIM, DIM, Real, SPATIAL_DIM}; use crate::utils::{ComponentMul, DotProduct, ScalarType, SimdRealCopy}; -#[cfg(not(feature = "std"))] -use simba::scalar::ComplexField as _; use crate::dynamics::solver::solver_body::SolverBodies; use crate::math::{SIMD_WIDTH, SimdReal}; @@ -153,6 +151,9 @@ impl JointConstraint { frame1: &Pose, frame2: &Pose, joint: &GenericJoint, + // The angular limits, in the form the rows consume (built once per assembly by + // `JointConstraintBuilder`, not once per substep). + ang_limits: &[AngularLimitParams; ANG_DIM], out: &mut [Self], ) -> usize { let mut len = 0; @@ -289,11 +290,7 @@ impl JointConstraint { body1, body2, i - DIM, - // `limit_angular` takes the sines of the half-angle limits. - [ - (joint.limits[i].min * 0.5).sin(), - (joint.limits[i].max * 0.5).sin(), - ], + ang_limits[i - DIM], WritebackId::Limit(i), erp_inv_dt, cfm_coeff, @@ -400,7 +397,8 @@ impl JointConstraint { frame2: &::Pose, locked_axes: u8, limit_axes: u8, - limits: &[[SimdReal; 2]; SPATIAL_DIM], + limits: &[[SimdReal; 2]; DIM], + ang_limits: &[AngularLimitParams; ANG_DIM], softness: crate::dynamics::SpringCoefficients, // `Some` = emit the (2D) angular motor row. Kept out of 3D until the // wide builder gathers per-axis motors. @@ -477,7 +475,7 @@ impl JointConstraint { body1, body2, i - DIM, - limits[i], + ang_limits[i - DIM], WritebackId::Limit(i), erp_inv_dt, cfm_coeff, diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs index fb5c867eb..1da2352d6 100644 --- a/src/dynamics/solver/joint_constraint/mod.rs +++ b/src/dynamics/solver/joint_constraint/mod.rs @@ -8,7 +8,7 @@ pub use generic_joint_constraint_builder::{ }; pub(crate) use joint_constraint_builder::JointConstraintBuilder; pub(crate) use joint_constraint_builder::JointConstraintBuilderSimd; -pub use joint_constraint_helper::JointConstraintHelper; +pub use joint_constraint_helper::{AngularLimitParams, JointConstraintHelper}; pub use joint_constraints_set::JointConstraintsSet; mod any_joint_constraint; diff --git a/src/geometry/broad_phase_bvh/update.rs b/src/geometry/broad_phase_bvh/update.rs index b52752053..42b040604 100644 --- a/src/geometry/broad_phase_bvh/update.rs +++ b/src/geometry/broad_phase_bvh/update.rs @@ -342,6 +342,18 @@ impl BroadPhaseBvh { core::mem::swap(&mut collider1, &mut collider2); } + // Same-parent colliders never collide; keeping their pairs out of the + // pair map and contact graph keeps bodies with many mutually-overlapping + // colliders from flooding the narrow phase (issue #970). Reparenting + // re-discovers via the forced re-insertion pre-pass (`PARENT` above), + // and the narrow phase's own per-update same-parent check handles pairs + // whose colliders become same-parent after creation. + if let (Some(p1), Some(p2)) = (&collider1.parent, &collider2.parent) { + if p1.handle == p2.handle { + return None; + } + } + if self.pairs.contains_key(&(handle1, handle2)) { return None; } diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index fd69ef275..13189bdf8 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -719,6 +719,10 @@ impl SimdSolverContact { impl SolverContactGeneric { /// The manifold contact indices, with the is-new bit masked off. + /// + /// These indices are only valid within the timestep that produced this solver + /// contact: manifold points may be reordered or replaced by the next narrow-phase + /// update. #[inline] pub fn contact_indices(&self) -> [ContactId; LANES] { self.contact_id.map(|id| id & !NEW_CONTACT_BIT) diff --git a/src/geometry/narrow_phase/test.rs b/src/geometry/narrow_phase/test.rs index 0baa05535..f1bef5297 100644 --- a/src/geometry/narrow_phase/test.rs +++ b/src/geometry/narrow_phase/test.rs @@ -77,10 +77,15 @@ pub fn collider_set_parent_depenetration() { let collider_2_position = collider_set.get(collider_2_handle).unwrap().pos; assert!((collider_1_position.translation - collider_2_position.translation).length() < 0.5f32); - let contact_pair = narrow_phase - .contact_pair(collider_1_handle, collider_2_handle) - .expect("The contact pair should exist."); - assert_eq!(contact_pair.manifolds.len(), 0); + // Same-parent pairs are filtered out by the broad phase (issue #970), so no (empty) + // contact pair is registered while both colliders share their parent. If one of them + // is re-parented, the broad phase must re-generate the pair (asserted below). + assert!( + narrow_phase + .contact_pair(collider_1_handle, collider_2_handle) + .is_none_or(|pair| pair.manifolds.is_empty()), + "No contact should be simulated between same-parent colliders." + ); assert!( narrow_phase .intersection_pair(collider_1_handle, collider_2_handle) diff --git a/src/utils/mod.rs b/src/utils/mod.rs index 254d14c7b..54322a340 100644 --- a/src/utils/mod.rs +++ b/src/utils/mod.rs @@ -76,6 +76,31 @@ pub fn try_normalize_and_get_length(v: Vector, threshold: Real) -> Option<(Vecto } } +/// Forces a negative-zero float (or any negative-zero component) to positive zero via +/// `x + 0.0`, leaving every other value untouched. +/// +/// Serialized solver state must not carry `-0.0`: whether a min/max/clamp returns `+0.0` +/// or `-0.0` for a signed-zero tie is platform-specific (SSE returns one of the operands +/// picked by argument order, NEON's `fminnm`/`fmaxnm` order the zeros), so a stored +/// signed zero breaks cross-platform snapshot determinism even though `-0.0 == +0.0` +/// dynamically. +// Unused in no-alloc builds: the contact solver, its only caller, needs alloc. +#[allow(dead_code)] +#[inline(always)] +pub(crate) fn canonicalize_zero(x: T) -> T +where + T: core::ops::Add + Default, +{ + #[cfg(feature = "enhanced-determinism")] + { + x + T::default() + } + #[cfg(not(feature = "enhanced-determinism"))] + { + x + } +} + /// Convert glam Vector to nalgebra `SimdVector` #[cfg(not(target_arch = "spirv"))] #[inline] diff --git a/src/utils/rotation_ops.rs b/src/utils/rotation_ops.rs index 05b51d039..be546ce1c 100644 --- a/src/utils/rotation_ops.rs +++ b/src/utils/rotation_ops.rs @@ -29,6 +29,8 @@ pub trait RotationOps: fn dot(&self, rhs: &Self) -> N; /// The imaginary part of the quaternion. fn imag(&self) -> N::Vector; + /// The real (scalar) part of the quaternion. + fn real(&self) -> N; /// Multiply this quaternion by a scalar without renormalizing. fn mul_assign_unchecked(&mut self, rhs: N); } @@ -93,6 +95,11 @@ impl RotationOps for UnitQuaternion { (**self).imag() } + #[inline] + fn real(&self) -> SimdReal { + self.w + } + #[inline] fn mul_assign_unchecked(&mut self, rhs: SimdReal) { *self.as_mut_unchecked() *= rhs; @@ -142,6 +149,11 @@ impl RotationOps for Rotation { self.xyz() } + #[inline] + fn real(&self) -> Real { + self.w + } + #[inline] fn mul_assign_unchecked(&mut self, rhs: Real) { *self *= rhs; diff --git a/typescript/CHANGELOG.md b/typescript/CHANGELOG.md index f64da078b..b0f4f4060 100644 --- a/typescript/CHANGELOG.md +++ b/typescript/CHANGELOG.md @@ -46,9 +46,19 @@ ### 0.18.2 (13 August 2025) +### Added + +- `SphericalImpulseJoint` now exposes its per-axis motors like the Rust API does: + `configureMotorModel`, `setMotorMaxForce`, `configureMotorVelocity`, + `configureMotorPosition` and `configureMotor`, each taking the angular `JointAxis` + to configure. The new `JointAxis` enum is exported for this purpose. + ### Fixed - Fix rollup configuration adding `types: "./rapier.d.ts"` to the export config. +- The `-compat` packages no longer trigger the wasm-bindgen deprecation warning + ("using deprecated parameters for the initialization function") on `init()`: the + embedded wasm module is now passed as `{ module_or_path: ... }`. ### 0.18.1 (8 August 2025) diff --git a/typescript/rapier-compat/fix_raw_file.sh b/typescript/rapier-compat/fix_raw_file.sh index 66f8bfafc..8f146a54e 100644 --- a/typescript/rapier-compat/fix_raw_file.sh +++ b/typescript/rapier-compat/fix_raw_file.sh @@ -5,7 +5,10 @@ for feature in \ 3d 3d-deterministic 3d-simd do -echo 'export * from "'"./rapier_wasm$feature"'"' > builds/${feature}/pkg/raw.d.ts -echo 'export * from "'"./rapier_wasm$feature"'"' > builds/${feature}/pkg/raw.d.ts +# The wasm-bindgen module is always named after the crate (rapier_wasm2d/rapier_wasm3d), +# whatever the feature variant (-deterministic/-simd) of the build. +dimension="${feature%%-*}" + +echo 'export * from "'"./rapier_wasm$dimension"'"' > builds/${feature}/pkg/raw.d.ts done; \ No newline at end of file diff --git a/typescript/rapier-compat/src2d/init.ts b/typescript/rapier-compat/src2d/init.ts index 668bfe130..7add6604f 100644 --- a/typescript/rapier-compat/src2d/init.ts +++ b/typescript/rapier-compat/src2d/init.ts @@ -8,5 +8,8 @@ import base64 from "base64-js"; * Has to be called and awaited before using any library methods. */ export async function init() { - await wasmInit(base64.toByteArray(wasmBase64 as unknown as string).buffer); + await wasmInit({ + module_or_path: base64.toByteArray(wasmBase64 as unknown as string) + .buffer, + }); } diff --git a/typescript/rapier-compat/src3d/init.ts b/typescript/rapier-compat/src3d/init.ts index 15b14ada1..cc143df67 100644 --- a/typescript/rapier-compat/src3d/init.ts +++ b/typescript/rapier-compat/src3d/init.ts @@ -8,5 +8,8 @@ import base64 from "base64-js"; * Has to be called and awaited before using any library methods. */ export async function init() { - await wasmInit(base64.toByteArray(wasmBase64 as unknown as string).buffer); + await wasmInit({ + module_or_path: base64.toByteArray(wasmBase64 as unknown as string) + .buffer, + }); } diff --git a/typescript/src.ts/dynamics/impulse_joint.ts b/typescript/src.ts/dynamics/impulse_joint.ts index 68185efeb..54d908807 100644 --- a/typescript/src.ts/dynamics/impulse_joint.ts +++ b/typescript/src.ts/dynamics/impulse_joint.ts @@ -46,6 +46,28 @@ export enum MotorModel { ForceBased, } +/** + * An enum representing a single joint axis, used to configure per-axis joint + * properties like the motors of a spherical joint. + */ +// #if DIM2 +export enum JointAxis { + LinX, + LinY, + AngX, +} +// #endif +// #if DIM3 +export enum JointAxis { + LinX, + LinY, + LinZ, + AngX, + AngY, + AngZ, +} +// #endif + /** * An enum representing the possible joint axes of a generic joint. * They can be ORed together, like: @@ -400,26 +422,103 @@ export class RevoluteImpulseJoint extends UnitImpulseJoint { export class GenericImpulseJoint extends ImpulseJoint {} export class SphericalImpulseJoint extends ImpulseJoint { - /* Unsupported by this alpha release. - public configureMotorModel(model: MotorModel) { - this.rawSet.jointConfigureMotorModel(this.handle, model); + /** + * Sets the motor model of one of this joint's angular axes. + * + * @param axis - The angular axis (`JointAxis.AngX/AngY/AngZ`) to configure. + * @param model - The motor model to apply to that axis. + */ + public configureMotorModel(axis: JointAxis, model: MotorModel) { + this.rawSet.jointConfigureMotorModel( + this.handle, + axis as number as RawJointAxis, + model as number as RawMotorModel, + ); } - public configureMotorVelocity(targetVel: Vector, factor: number) { - this.rawSet.jointConfigureBallMotorVelocity(this.handle, targetVel.x, targetVel.y, targetVel.z, factor); + /** + * Sets the maximum force/torque the motor of the given angular axis can deliver. + * + * @param axis - The angular axis (`JointAxis.AngX/AngY/AngZ`) to configure. + * @param maxForce - The maximum torque the axis motor can deliver. + */ + public setMotorMaxForce(axis: JointAxis, maxForce: number) { + this.rawSet.jointSetMotorMaxForce( + this.handle, + axis as number as RawJointAxis, + maxForce, + ); } - public configureMotorPosition(targetPos: Quaternion, stiffness: number, damping: number) { - this.rawSet.jointConfigureBallMotorPosition(this.handle, targetPos.w, targetPos.x, targetPos.y, targetPos.z, stiffness, damping); + /** + * Makes the motor of the given angular axis target a specific angular velocity. + * + * @param axis - The angular axis (`JointAxis.AngX/AngY/AngZ`) to configure. + * @param targetVel - The target angular velocity along the axis, in radians per second. + * @param factor - The strength used to reach the target velocity (a damping coefficient). + */ + public configureMotorVelocity( + axis: JointAxis, + targetVel: number, + factor: number, + ) { + this.rawSet.jointConfigureMotorVelocity( + this.handle, + axis as number as RawJointAxis, + targetVel, + factor, + ); } - public configureMotor(targetPos: Quaternion, targetVel: Vector, stiffness: number, damping: number) { - this.rawSet.jointConfigureBallMotor(this.handle, - targetPos.w, targetPos.x, targetPos.y, targetPos.z, - targetVel.x, targetVel.y, targetVel.z, - stiffness, damping); + /** + * Makes the motor of the given angular axis target a specific angle. + * + * @param axis - The angular axis (`JointAxis.AngX/AngY/AngZ`) to configure. + * @param targetPos - The target angle along the axis, in radians. + * @param stiffness - The spring-like stiffness used to reach the target angle. + * @param damping - The damping applied to the axis' angular velocity. + */ + public configureMotorPosition( + axis: JointAxis, + targetPos: number, + stiffness: number, + damping: number, + ) { + this.rawSet.jointConfigureMotorPosition( + this.handle, + axis as number as RawJointAxis, + targetPos, + stiffness, + damping, + ); } + + /** + * Configures the motor of the given angular axis with both an angle and an + * angular-velocity target. + * + * @param axis - The angular axis (`JointAxis.AngX/AngY/AngZ`) to configure. + * @param targetPos - The target angle along the axis, in radians. + * @param targetVel - The target angular velocity along the axis, in radians per second. + * @param stiffness - The spring-like stiffness used to reach the target angle. + * @param damping - The damping applied to the axis' angular velocity. */ + public configureMotor( + axis: JointAxis, + targetPos: number, + targetVel: number, + stiffness: number, + damping: number, + ) { + this.rawSet.jointConfigureMotor( + this.handle, + axis as number as RawJointAxis, + targetPos, + targetVel, + stiffness, + damping, + ); + } } // #endif diff --git a/website/docs/user_guides/templates/colliders.mdx b/website/docs/user_guides/templates/colliders.mdx index 24ee2eb9a..88a85eae9 100644 --- a/website/docs/user_guides/templates/colliders.mdx +++ b/website/docs/user_guides/templates/colliders.mdx @@ -170,9 +170,16 @@ be cuboids, balls, convex meshes, etc.) This is commonly known as a **convex dec :::info An alternative to using a **compound shape** is to attach **multiple colliders** to the same rigid-body: all the colliders -will move with the rigid-body automatically, behaving in a very similar way than using a single collider with a -compound shape. The main differences between the two approaches is about collision events: each collider generates -individual collision start/stop events. +will move with the rigid-body automatically, and the simulation quality (contact resolution, stability) is identical with +both approaches. They differ in other ways, so pick based on how you use the object: +- **Performance:** a compound shape is a single collider, so the broad-phase handles one entry (with its own internal + acceleration structure for the parts) instead of one entry per collider. With many parts (hundreds or more), a compound + shape makes the physics step significantly cheaper, especially while the rigid-body is awake. +- **Collision events:** each collider generates its own individual collision start/stop events and can have its own + friction, restitution, collision groups, or sensor status. A compound shape is a single collider: one set of events + and properties for the whole shape. +- **Mutability:** adding or removing one collider from a rigid-body is easy and cheap, whereas adding or removing a part + of a compound shape requires rebuilding the whole compound shape. ::: To build a compound shape, it is possible to directly provide the set of shapes as well as their position in the