From 49f331922c4471cde879cd012fec0a1242e8e653 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 13 Aug 2026 14:13:03 +0200 Subject: [PATCH 1/8] fix(mjcf): parse margin as the predictive collision distance instead of contact skin --- crates/rapier3d-mjcf/src/hooks.rs | 23 ++++++-------- crates/rapier3d-mjcf/src/loader/geom.rs | 35 ++++++++++++++++++++-- crates/rapier3d-mjcf/src/loader/runtime.rs | 3 +- 3 files changed, 43 insertions(+), 18 deletions(-) diff --git a/crates/rapier3d-mjcf/src/hooks.rs b/crates/rapier3d-mjcf/src/hooks.rs index bc36ed205..e50295736 100644 --- a/crates/rapier3d-mjcf/src/hooks.rs +++ b/crates/rapier3d-mjcf/src/hooks.rs @@ -1,6 +1,6 @@ //! Physics-hook implementations for MJCF features that need runtime //! filtering: `` pair-suppression and `` -//! per-pair friction / margin overrides. +//! per-pair friction overrides. use std::collections::HashMap; @@ -8,19 +8,21 @@ use rapier3d::geometry::{ColliderHandle, SolverFlags}; use rapier3d::math::Real; use rapier3d::pipeline::{ContactModificationContext, PairFilterContext, PhysicsHooks}; -/// Friction / margin override for a specific collider pair, sourced from a +/// Friction override for a specific collider pair, sourced from a /// `` element. +/// +/// The element's `margin` is not represented: it is a contact-generation +/// distance, which rapier's speculative contacts already cover, so applying it +/// here would only shift the resting separation. #[derive(Copy, Clone, Debug, Default)] pub struct PairOverride { /// Friction coefficient override. pub friction: Option, - /// Margin override (added to the contact distance threshold). - pub margin: Option, } /// Hook implementation honouring MJCF's `` (suppress /// contact between two collider sets) and `` (override -/// friction / margin on a specific pair). +/// friction on a specific pair). /// /// The user opts in by passing this object to the rapier physics pipeline /// (`pipeline.step(&hooks, ...)`). Without it, excludes are ignored and @@ -30,7 +32,7 @@ pub struct MjcfContactHooks { /// Excluded ordered collider pairs. We store both `(a, b)` and `(b, a)` /// for O(1) lookup regardless of which side rapier presents first. pub(crate) exclude: std::collections::HashSet<(ColliderHandle, ColliderHandle)>, - /// Per-pair friction / margin overrides. Both orderings stored. + /// Per-pair friction overrides. Both orderings stored. pub(crate) overrides: HashMap<(ColliderHandle, ColliderHandle), PairOverride>, } @@ -46,7 +48,7 @@ impl MjcfContactHooks { self.exclude.insert((b, a)); } - /// Register a friction / margin override for a specific pair. + /// Register a friction override for a specific pair. pub fn add_override(&mut self, a: ColliderHandle, b: ColliderHandle, ov: PairOverride) { self.overrides.insert((a, b), ov); self.overrides.insert((b, a), ov); @@ -80,13 +82,6 @@ impl PhysicsHooks for MjcfContactHooks { // Contact materials are per-manifold since the solver-contact slimming. *ctx.friction = f; } - // Margin: rapier's solver uses `dist` as penetration depth; - // shifting it acts like adding to the contact margin. - if let Some(m) = ov.margin { - for c in ctx.solver_contacts.iter_mut() { - c.dist -= m; - } - } } } } diff --git a/crates/rapier3d-mjcf/src/loader/geom.rs b/crates/rapier3d-mjcf/src/loader/geom.rs index 663cefd16..da696db53 100644 --- a/crates/rapier3d-mjcf/src/loader/geom.rs +++ b/crates/rapier3d-mjcf/src/loader/geom.rs @@ -6,7 +6,7 @@ use mjcf_rs::Pose as MPose; use mjcf_rs::body as mb; use mjcf_rs::glam::{DQuat, DVec3}; use mjcf_rs::model::{BodyEntry, BodyId}; -use rapier3d::dynamics::RigidBody; +use rapier3d::dynamics::{IntegrationParameters, RigidBody}; #[cfg(feature = "__meshloader_is_enabled")] use rapier3d::geometry::MeshConverter; use rapier3d::geometry::{Collider, Group, InteractionGroups, SharedShape}; @@ -350,6 +350,34 @@ impl<'a> Conversion<'a> { Some((shape, Pose::IDENTITY)) } + /// A geom's `margin` in rapier length units. + /// + /// MuJoCo uses `margin` as the distance at which a contact is generated, + /// with force only starting at `margin - gap`. Rapier already generates + /// contacts within its speculative distance, so a margin below that is + /// redundant and is dropped; a larger one is honored as a soft-CCD + /// prediction on the parent body (see + /// [`Self::body_soft_ccd_prediction`]). Mapping it to `contact_skin` + /// instead would thicken the surface and make the geoms rest that far + /// apart, which is not what the attribute means. + fn geom_margin(&self, g: &mb::Geom) -> Real { + (g.margin as Real) * self.options.scale + } + + /// The soft-CCD prediction distance a body inherits from its geoms' + /// `margin`, or `0.0` if no geom asks for more than rapier's speculative + /// contacts already cover. + fn body_soft_ccd_prediction(&self, entry: &BodyEntry) -> Real { + let speculative = IntegrationParameters::default().prediction_distance(); + entry + .body + .geoms + .iter() + .map(|g| self.geom_margin(g)) + .filter(|m| *m > speculative) + .fold(0.0, Real::max) + } + /// Convert one MJCF geom into a rapier collider. pub(super) fn build_collider(&self, g: &mb::Geom) -> Option { // `compiler/discardvisual` drops visual-only geoms during compile; @@ -369,7 +397,6 @@ impl<'a> Conversion<'a> { builder.shape = shape; builder = builder.position(body_frame_pose); builder = builder.friction(g.friction[0] as Real); - builder = builder.contact_skin(g.margin as Real); builder = builder.collision_groups(self.interaction_groups(g)); // Enable contact-pair filtering / contact-modification hooks // unconditionally — the hooks themselves only kick in if the user @@ -407,6 +434,10 @@ impl<'a> Conversion<'a> { _world_pose: MPose, _body: &mut RigidBody, ) { + let soft_ccd = self.body_soft_ccd_prediction(entry); + if soft_ccd > 0.0 { + _body.set_soft_ccd_prediction(soft_ccd); + } self.staged_colliders.clear(); self.staged_collider_names.clear(); self.staged_visual_meshes.clear(); diff --git a/crates/rapier3d-mjcf/src/loader/runtime.rs b/crates/rapier3d-mjcf/src/loader/runtime.rs index 8bb76838a..42dfa8468 100644 --- a/crates/rapier3d-mjcf/src/loader/runtime.rs +++ b/crates/rapier3d-mjcf/src/loader/runtime.rs @@ -97,7 +97,7 @@ impl MjcfRobotHandles { } } - // : per-pair friction / margin overrides between two named geoms. + // : per-pair friction overrides between two named geoms. for p in &robot.contact_pairs { let Some(&(b1, gi1)) = robot.geom_name_to_collider.get(&p.geom1) else { log::warn!(": unknown geom1 `{}`", p.geom1); @@ -125,7 +125,6 @@ impl MjcfRobotHandles { }; let ov = PairOverride { friction: p.friction.map(|f| f[0] as Real), - margin: p.margin.map(|m| m as Real), }; hooks.add_override(h1.handle, h2.handle, ov); } From b43f0ce9eae1476110aa395e3d03af049048704b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 13 Aug 2026 14:13:27 +0200 Subject: [PATCH 2/8] fix(mjcf): load actuators as ForceBased motors instead of AccelerationBased --- crates/rapier3d-mjcf/src/loader/runtime.rs | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/crates/rapier3d-mjcf/src/loader/runtime.rs b/crates/rapier3d-mjcf/src/loader/runtime.rs index 42dfa8468..6d78d9ece 100644 --- a/crates/rapier3d-mjcf/src/loader/runtime.rs +++ b/crates/rapier3d-mjcf/src/loader/runtime.rs @@ -5,7 +5,7 @@ use mjcf_rs::extras::Keyframe; use rapier3d::dynamics::{ - GenericJoint, ImpulseJointHandle, ImpulseJointSet, JointAxis, MultibodyIndex, + GenericJoint, ImpulseJointHandle, ImpulseJointSet, JointAxis, MotorModel, MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, RigidBody, RigidBodySet, RigidBodyType, }; use rapier3d::math::{Pose, Real, Rotation, Vector}; @@ -565,6 +565,13 @@ fn configure_actuator_motor( let ax = JointAxis::AngX; let lin_ax = JointAxis::LinX; + // MuJoCo actuators produce absolute generalized forces: a `` + // servo applies `kp·(ctrl − q) − kv·q̇` in N·m, straight into `M q̈ = τ`. + // That is the force-based model; the acceleration-based one would rescale + // those gains by the link's inertia and change what the authored `kp` + // means from one link to the next. + data.set_motor_model(ax, MotorModel::ForceBased); + data.set_motor_model(lin_ax, MotorModel::ForceBased); // MuJoCo's `` defaults `kp` to 1 when it isn't set (directly or // through a `` class), not 0. Defaulting to 0 here would give a // zero-gain — i.e. completely limp — servo, which is what made e.g. the From 664fe2d734d8d6e0adf47ee37dc25d63949db578 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 13 Aug 2026 14:17:47 +0200 Subject: [PATCH 3/8] fix compliance for impulse-based unit multibody joints --- crates/rapier3d/tests/multibody_coupling.rs | 6 ++++-- .../joint/multibody_joint/unit_multibody_joint.rs | 10 ++++++---- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/crates/rapier3d/tests/multibody_coupling.rs b/crates/rapier3d/tests/multibody_coupling.rs index eb8fd5ef9..2b9f227d3 100644 --- a/crates/rapier3d/tests/multibody_coupling.rs +++ b/crates/rapier3d/tests/multibody_coupling.rs @@ -54,9 +54,11 @@ fn run_coupling(coeff: Real, offset: Real, target: Real) -> (Real, Real) { coeff, offset, }); - // Drive link1's hinge to `target`. + // Drive link1's hinge to `target`. Internal motors honor their + // compliance (they are real springs, not rigid servos), so use + // critically damped gains that settle well within the simulated time. let link1_joint = &mut mb.links_mut().nth(link1_id).unwrap().joint.data; - link1_joint.set_motor_position(JointAxis::AngX, target, 20.0, 2.0); + link1_joint.set_motor_position(JointAxis::AngX, target, 100.0, 20.0); } let gravity = Vector::ZERO; diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index cb1c18715..5dcac40c6 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -45,6 +45,7 @@ pub fn unit_joint_limit_constraint( min_enabled as u32 as Real * -Real::MAX, max_enabled as u32 as Real * Real::MAX, ]; + let cfm_gain = lhs * cfm_coeff; let constraint = GenericJointConstraint { is_rigid_body1: false, @@ -59,11 +60,11 @@ pub fn unit_joint_limit_constraint( joint_id: usize::MAX, // TODO: we don’t support impulse writeback for internal constraints yet. impulse: 0.0, impulse_bounds, - inv_lhs: crate::utils::inv(lhs), + inv_lhs: crate::utils::inv(lhs + cfm_gain), rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, cfm_coeff, - cfm_gain: 0.0, + cfm_gain, writeback_id: WritebackId::Limit(dof_id), }; @@ -102,6 +103,7 @@ pub fn unit_joint_motor_constraint( let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; + let cfm_gain = lhs * motor_params.cfm_coeff + motor_params.cfm_gain; let mut rhs_wo_bias = 0.0; if motor_params.erp_inv_dt != 0.0 { @@ -132,8 +134,8 @@ pub fn unit_joint_motor_constraint( impulse: 0.0, impulse_bounds, cfm_coeff: motor_params.cfm_coeff, - cfm_gain: motor_params.cfm_gain, - inv_lhs: crate::utils::inv(lhs), + cfm_gain, + inv_lhs: crate::utils::inv(lhs + cfm_gain), rhs: rhs_wo_bias, rhs_wo_bias, writeback_id: WritebackId::Limit(dof_id), From 1268e3cb9a84d032c557785c85b8ad86419f2917 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 13 Aug 2026 14:19:20 +0200 Subject: [PATCH 4/8] fix joint constraint allocation size with both motors and limits --- .../generic_joint_constraint_builder.rs | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) 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 b2d32f1a8..8f0fe987e 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -116,15 +116,13 @@ impl JointGenericExternalConstraintBuilder { return; } - // For each solver contact we generate up to SPATIAL_DIM constraints, and each - // constraints appends the multibodies jacobian and weighted jacobians. - // Also note that for impulse_joints, the rigid-bodies will also add their jacobians - // to the generic DVector. - // TODO: is this count correct when we take both motors and limits into account? - let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM; - - // TODO: use a more precise increment. - *j_id += multibodies_ndof * 2 * SPATIAL_DIM; + // Each constraint row appends the jacobian and the weighted jacobian for + // both sides, i.e. `2 * multibodies_ndof` entries. Reserve exactly the + // rows this joint emits: an axis carrying both a motor and a limit + // produces two rows, so a joint can exceed `SPATIAL_DIM` of them. + let num_rows = joint_num_constraints(joint); + let required_jacobian_len = *j_id + multibodies_ndof * 2 * num_rows; + *j_id += multibodies_ndof * 2 * num_rows; // Grow the jacobian buffer to fit this constraint: runs serially in the staged solver's // pre-phase, so race-free (see `generic_contact_constraint`); the internal-constraint From ed2a03e473b530eb70272d3faae78c88c9be8129 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 13 Aug 2026 14:27:44 +0200 Subject: [PATCH 5/8] =?UTF-8?q?fix=20accumulated=20impulse=20reporting=20w?= =?UTF-8?q?hen=20the=20warmstart=20coeff=20isn=E2=80=99t=201?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../contact_constraint/contact_with_coulomb_friction.rs | 4 ++-- .../contact_constraint/contact_with_twist_friction.rs | 6 +++--- .../solver/contact_constraint/generic_contact_constraint.rs | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) 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 941bc6473..c3bcb4df4 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs @@ -430,14 +430,14 @@ impl ContactWithCoulombFrictionBuilder { // the twist-friction `update`). normal_part.cfm_factor = cfm_factor.select(dist.simd_le(SimdReal::zero()), SimdReal::splat(1.0)); - normal_part.impulse_accumulator += normal_part.impulse; normal_part.impulse *= warmstart_coeff; + normal_part.impulse_accumulator += normal_part.impulse; } // tangent parts. { - tangent_part.impulse_accumulator += tangent_part.impulse; tangent_part.impulse *= warmstart_coeff; + tangent_part.impulse_accumulator += tangent_part.impulse; for j in 0..DIM - 1 { let bias = (p1 - p2).gdot(tangents1[j]) * inv_dt; 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 11c9236a9..d1f0376b0 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs @@ -492,8 +492,8 @@ impl ContactWithTwistFrictionBuilder { // rocking. Only penetrating points get the soft treatment. normal_part.cfm_factor = cfm_factor.select(dist.simd_le(SimdReal::zero()), SimdReal::splat(1.0)); - normal_part.impulse_accumulator += normal_part.impulse; normal_part.impulse *= warmstart_coeff; + normal_part.impulse_accumulator += normal_part.impulse; } } @@ -506,10 +506,10 @@ impl ContactWithTwistFrictionBuilder { let bias = (p1 - p2).gdot(tangents1[j]) * inv_dt; tangent_part.rhs[j] = tangent_part.rhs_wo_bias[j] + bias; } - tangent_part.impulse_accumulator += tangent_part.impulse; tangent_part.impulse *= warmstart_coeff; - twist_part.impulse_accumulator += twist_part.impulse; + tangent_part.impulse_accumulator += tangent_part.impulse; twist_part.impulse *= warmstart_coeff; + twist_part.impulse_accumulator += twist_part.impulse; } constraint.cfm_factor = cfm_factor; diff --git a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs index 3e0a0a70f..5e55fe33f 100644 --- a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs @@ -469,14 +469,14 @@ impl GenericContactConstraintBuilder { normal_part.rhs_wo_bias = rhs_wo_bias; normal_part.rhs = new_rhs; - normal_part.impulse_accumulator += normal_part.impulse; normal_part.impulse *= params.warmstart_coefficient; + normal_part.impulse_accumulator += normal_part.impulse; } // Tangent part. { - tangent_part.impulse_accumulator += tangent_part.impulse; tangent_part.impulse *= params.warmstart_coefficient; + tangent_part.impulse_accumulator += tangent_part.impulse; for j in 0..DIM - 1 { let bias = (p1 - p2).gdot(tangents1[j]) * inv_dt; From df8c5369baa2f7aff263187866515c1f9dc6ec92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 13 Aug 2026 15:47:52 +0200 Subject: [PATCH 6/8] =?UTF-8?q?fix:=E2=80=AFguard=20against=20instabilitie?= =?UTF-8?q?s=20that=20can=20be=20introduced=20by=20near-singular=20multibo?= =?UTF-8?q?dy=20mass=20matrix?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- examples3d/debug_long_chain3.rs | 4 +- .../joint/multibody_joint/multibody.rs | 110 +++++++++++++++++- 2 files changed, 111 insertions(+), 3 deletions(-) diff --git a/examples3d/debug_long_chain3.rs b/examples3d/debug_long_chain3.rs index 96eaf17bc..183967331 100644 --- a/examples3d/debug_long_chain3.rs +++ b/examples3d/debug_long_chain3.rs @@ -6,12 +6,12 @@ pub async fn run(viewer: &mut TestbedViewer) -> anyhow::Result<()> { * World */ let mut world = PhysicsWorld::new(); - let use_articulations = false; + let use_articulations = true; /* * Create the long chain. */ - let num = 100; + let num = 85; let rad = 0.2; let shift = rad * 2.2; diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 9404e824a..623f0e8c5 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -479,6 +479,15 @@ impl Multibody { self.accelerations.fill(0.0); + // If there is significant movement, the semi-implicit coriolis term + // occasionally cause instabilities due to the matrix being near singular. + // To prevent that, we enable a check that verifies the result and fallbacks + // to the explicit term if needed + let check_implicit_coriolis_divergence = dt * self.velocities.amax() >= 1.0e-3; + // Generalized forces that derive from positions or external inputs + // (gravity, user forces, spring position terms). + let mut pos_forces = DVector::zeros(if check_implicit_coriolis_divergence { self.ndofs } else { 0 }); + // Eqn 42 to 45 for i in 0..self.links.len() { let link = &self.links[i]; @@ -539,6 +548,16 @@ impl Multibody { external_forces.as_vector(), 1.0, ); + + if check_implicit_coriolis_divergence { + let applied_forces = Force::new(rb.forces.force, rb.forces.torque); + pos_forces.gemv_tr( + 1.0, + &self.body_jacobians[i], + applied_forces.as_vector(), + 1.0, + ); + } } self.accelerations @@ -556,17 +575,106 @@ impl Multibody { if k != 0.0 { let q = self.links[li].joint.coords[a]; let rest = self.links[li].joint.spring_ref[a]; - self.accelerations[idx] += -k * (q - rest) - k * dt * self.velocities[idx]; + let spring_pos_force = -k * (q - rest); + self.accelerations[idx] += + spring_pos_force - k * dt * self.velocities[idx]; + if check_implicit_coriolis_divergence { + pos_forces[idx] += spring_pos_force; + } } idx += 1; } } } + // Snapshot the full generalized forces: the energy guard may need them + // for a re-solve with the plain mass matrix. + let gen_forces = if check_implicit_coriolis_divergence { + self.accelerations.clone() + } else { + DVector::zeros(0) + }; + self.augmented_mass_indices .with_rearranged_rows_mut(&mut self.accelerations, |accs| { self.acc_inv_augmented_mass.solve_mut(accs); }); + + if check_implicit_coriolis_divergence { + self.free_velocity_energy_guard(dt, &gen_forces, &pos_forces); + } + } + + // If the semi-implicit coriolis solve introduces more energy than the external + // forces’ work, we recalculate without the coriolis term in the mass matrix. + // + // This operates by calculating the effective kinematic energy added by the + // external forces after the solve with the semi-implicit mass matrix, and + // compare it with the force’s work. If the energy delta exceeds a threshold + // (indicating undesired/unrealistic energy injection), we fall back to re-solving + // the forces effect but using only the mass matrix without the implicit coriolis + // terms. + fn free_velocity_energy_guard( + &mut self, + dt: Real, + gen_forces: &DVector, + pos_forces: &DVector, + ) { + let eff_dim = self.augmented_mass_indices.dim_after_removal(self.ndofs); + if eff_dim == 0 { + return; + } + + // Move all quantities to the kinematic-reduced ordering of the + // factorized matrices (no-op when there is no kinematic dof). + let mut v = self.velocities.clone(); + let mut f = gen_forces.clone(); + let mut f_pos = pos_forces.clone(); + self.augmented_mass_indices.rearrange_rows(&mut v, true); + self.augmented_mass_indices.rearrange_rows(&mut f, true); + self.augmented_mass_indices.rearrange_rows(&mut f_pos, true); + + let m = self.augmented_mass.view((0, 0), (eff_dim, eff_dim)); + let v = v.rows(0, eff_dim); + let f = f.rows(0, eff_dim); + let f_pos = f_pos.rows(0, eff_dim); + + let energy_delta_minus_work = |accelerations: &DVector| -> Option { + let mut a = accelerations.clone(); + self.augmented_mass_indices.rearrange_rows(&mut a, true); + let a = a.rows(0, eff_dim).into_owned(); + if !a.iter().all(|x| x.is_finite()) { + return None; + } + let m_a = &m * &a; + let energy_delta = dt * v.dot(&m_a) + 0.5 * dt * dt * a.dot(&m_a); + let work = dt * v.dot(&f) + 0.5 * dt * dt * a.dot(&f_pos); + // Margin: a small fraction of the current kinetic energy to absorb + // discretization error on healthy steps. + let kinetic_energy = 0.5 * v.dot(&(&m * &v)); + let energy_margin = 1.0e-2 * kinetic_energy + 1.0e-8; + Some(energy_delta - work - energy_margin) + }; + + match energy_delta_minus_work(&self.accelerations) { + Some(excess) if excess <= 0.0 => (), // Implicit update didn’t introduce too much extra energy. + _ => { + // The implicit solve injected energy (or produced non-finite + // values): fall back to the plain mass matrix. + self.accelerations.copy_from(gen_forces); + self.augmented_mass_indices + .with_rearranged_rows_mut(&mut self.accelerations, |accs| { + self.inv_augmented_mass.solve_mut(accs); + }); + + // Last-chance check: if all else fails, just clear the accelerations vector + // instead of breaking the simulation. Hopefully it will get back to a + // sane result in a close timestep. + if !self.accelerations.iter().all(|x| x.is_finite()) { + self.accelerations.fill(0.0); + } + } + } } /// Computes the constant terms of the dynamics. From 302f5d656cb650a91437eae7d1c5dea3627f6824 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 13 Aug 2026 15:48:56 +0200 Subject: [PATCH 7/8] chore: cargo fmt --- .../joint/multibody_joint/multibody.rs | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 623f0e8c5..298affb0e 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -486,7 +486,11 @@ impl Multibody { let check_implicit_coriolis_divergence = dt * self.velocities.amax() >= 1.0e-3; // Generalized forces that derive from positions or external inputs // (gravity, user forces, spring position terms). - let mut pos_forces = DVector::zeros(if check_implicit_coriolis_divergence { self.ndofs } else { 0 }); + let mut pos_forces = DVector::zeros(if check_implicit_coriolis_divergence { + self.ndofs + } else { + 0 + }); // Eqn 42 to 45 for i in 0..self.links.len() { @@ -576,8 +580,7 @@ impl Multibody { let q = self.links[li].joint.coords[a]; let rest = self.links[li].joint.spring_ref[a]; let spring_pos_force = -k * (q - rest); - self.accelerations[idx] += - spring_pos_force - k * dt * self.velocities[idx]; + self.accelerations[idx] += spring_pos_force - k * dt * self.velocities[idx]; if check_implicit_coriolis_divergence { pos_forces[idx] += spring_pos_force; } @@ -614,12 +617,7 @@ impl Multibody { // (indicating undesired/unrealistic energy injection), we fall back to re-solving // the forces effect but using only the mass matrix without the implicit coriolis // terms. - fn free_velocity_energy_guard( - &mut self, - dt: Real, - gen_forces: &DVector, - pos_forces: &DVector, - ) { + fn free_velocity_energy_guard(&mut self, dt: Real, gen_forces: &DVector, pos_forces: &DVector) { let eff_dim = self.augmented_mass_indices.dim_after_removal(self.ndofs); if eff_dim == 0 { return; @@ -662,10 +660,12 @@ impl Multibody { // The implicit solve injected energy (or produced non-finite // values): fall back to the plain mass matrix. self.accelerations.copy_from(gen_forces); - self.augmented_mass_indices - .with_rearranged_rows_mut(&mut self.accelerations, |accs| { + self.augmented_mass_indices.with_rearranged_rows_mut( + &mut self.accelerations, + |accs| { self.inv_augmented_mass.solve_mut(accs); - }); + }, + ); // Last-chance check: if all else fails, just clear the accelerations vector // instead of breaking the simulation. Hopefully it will get back to a From b83515c1208823b61289f06aa576377afa9e553d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 13 Aug 2026 16:37:36 +0200 Subject: [PATCH 8/8] =?UTF-8?q?chore:=E2=80=AFclippy=20fixes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crates/rapier2d/tests/snapshot_portability.rs | 2 +- crates/rapier3d/tests/snapshot_portability.rs | 2 +- src/dynamics/joint/multibody_joint/multibody.rs | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/crates/rapier2d/tests/snapshot_portability.rs b/crates/rapier2d/tests/snapshot_portability.rs index e482c9a90..fe9d7e4d0 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, 0x2984_7b60_fb36_4a99); +const GOLDEN: (usize, u64) = (88_532, 0x6b31_7afb_d1fa_bf95); const STEPS: usize = 60; diff --git a/crates/rapier3d/tests/snapshot_portability.rs b/crates/rapier3d/tests/snapshot_portability.rs index e3410b499..6e7c37a6d 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, 0x6f27_44ea_f0b9_67c7); +const GOLDEN: (usize, u64) = (481_520, 0x506f_f44e_db9a_7b9b); const STEPS: usize = 60; diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 298affb0e..46f9cb660 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -644,12 +644,12 @@ impl Multibody { if !a.iter().all(|x| x.is_finite()) { return None; } - let m_a = &m * &a; + let m_a = m * &a; let energy_delta = dt * v.dot(&m_a) + 0.5 * dt * dt * a.dot(&m_a); let work = dt * v.dot(&f) + 0.5 * dt * dt * a.dot(&f_pos); // Margin: a small fraction of the current kinetic energy to absorb // discretization error on healthy steps. - let kinetic_energy = 0.5 * v.dot(&(&m * &v)); + let kinetic_energy = 0.5 * v.dot(&(m * v)); let energy_margin = 1.0e-2 * kinetic_energy + 1.0e-8; Some(energy_delta - work - energy_margin) };