diff --git a/CHANGELOG.md b/CHANGELOG.md index e671d8d08..a4ec7945a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,12 @@ - Rename `RigidBodyChanges::CHANGED` to `::IN_CHANGED_SET` to make its meaning more precise. - Fix colliders ignoring user-changes after the first simulation step. - Fix broad-phase incorrectly taking into account disabled colliders attached to an enabled dynamic rigid-body. +- Grouped `IntegrationParameters::contact_natural_frequency` and `IntegrationParameters::contact_damping_ratio` behind + a single `IntegrationParameters::contact_softness` (#789). +- Removed the `Integration_parameters` methods related to `erp` and `cfm`. They are now methods of the + `IntegrationParameters::softness` and `GenericJoint::softness` fields (#789). +- Removed `IntegrationParameters::joint_natural_frequency` and `IntegrationParameters::joint_damping_ratio` in favor of + per-joint softness coefficients `GenericJoint::softness` (#789). ## v0.30.1 (17 Oct. 2025) diff --git a/examples2d/joints2.rs b/examples2d/joints2.rs index 4c4cf3503..7c780f1c0 100644 --- a/examples2d/joints2.rs +++ b/examples2d/joints2.rs @@ -10,6 +10,12 @@ pub fn init_world(testbed: &mut Testbed) { let mut impulse_joints = ImpulseJointSet::new(); let multibody_joints = MultibodyJointSet::new(); + /* + * Enable/disable softness. + */ + let settings = testbed.example_settings_mut(); + let variable_softness = settings.get_or_set_bool("Variable softness", false); + /* * Create the balls */ @@ -41,10 +47,22 @@ pub fn init_world(testbed: &mut Testbed) { let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, &mut bodies); + let softness = if variable_softness { + // If variable softness is enabled, joints closer to the fixed body are softer. + SpringCoefficients { + natural_frequency: 5.0 * (i.max(k) + 1) as f32, + damping_ratio: 0.1 * (i.max(k) + 1) as f32, + } + } else { + SpringCoefficients::joint_defaults() + }; + // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]); + let joint = RevoluteJointBuilder::new() + .local_anchor2(point![0.0, shift]) + .softness(softness); impulse_joints.insert(parent_handle, child_handle, joint, true); } @@ -52,7 +70,9 @@ pub fn init_world(testbed: &mut Testbed) { if k > 0 { let parent_index = body_handles.len() - numi; let parent_handle = body_handles[parent_index]; - let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]); + let joint = RevoluteJointBuilder::new() + .local_anchor2(point![-shift, 0.0]) + .softness(softness); impulse_joints.insert(parent_handle, child_handle, joint, true); } diff --git a/examples2d/pin_slot_joint2.rs b/examples2d/pin_slot_joint2.rs index 1c8100b2e..c5ba43faf 100644 --- a/examples2d/pin_slot_joint2.rs +++ b/examples2d/pin_slot_joint2.rs @@ -1,5 +1,8 @@ -use rapier2d::prelude::*; +use crate::utils::character; +use crate::utils::character::CharacterControlMode; use rapier_testbed2d::Testbed; +use rapier2d::control::{KinematicCharacterController, PidController}; +use rapier2d::prelude::*; pub fn init_world(testbed: &mut Testbed) { /* @@ -77,10 +80,29 @@ pub fn init_world(testbed: &mut Testbed) { .build(); impulse_joints.insert(character_handle, cube_handle, pin_slot_joint, true); + /* + * Callback to update the character based on user inputs. + */ + let mut control_mode = CharacterControlMode::Kinematic(0.1); + let mut controller = KinematicCharacterController::default(); + let mut pid = PidController::default(); + + testbed.add_callback(move |graphics, physics, _, _| { + if let Some(graphics) = graphics { + character::update_character( + graphics, + physics, + &mut control_mode, + &mut controller, + &mut pid, + character_handle, + ); + } + }); + /* * Set up the testbed. */ testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); - testbed.set_character_body(character_handle); testbed.look_at(point![0.0, 1.0], 100.0); } diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 0e9b362c9..6d007f98a 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,8 +1,7 @@ -use crate::math::Real; -use na::RealField; - #[cfg(doc)] use super::RigidBodyActivation; +use crate::math::Real; +use simba::simd::SimdRealField; // TODO: enabling the block solver in 3d introduces a lot of jitters in // the 3D domino demo. So for now we dont enable it in 3D. @@ -30,6 +29,114 @@ pub enum FrictionModel { Coulomb, } +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +// TODO: we should be able to combine this with MotorModel. +/// Coefficients for a spring, typically used for configuring constraint softness for contacts and +/// joints. +pub struct SpringCoefficients { + /// Sets the natural frequency (Hz) of the spring-like constraint. + /// + /// Higher values make the constraint stiffer and resolve constraint violations more quickly. + pub natural_frequency: N, + /// Sets the damping ratio for the spring-like constraint. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + pub damping_ratio: N, +} + +impl + Copy> SpringCoefficients { + /// Initializes spring coefficients from the spring natural frequency and damping ratio. + pub fn new(natural_frequency: N, damping_ratio: N) -> Self { + Self { + natural_frequency, + damping_ratio, + } + } + + /// Default softness coefficients for contacts. + pub fn contact_defaults() -> Self { + Self { + natural_frequency: N::splat(30.0), + damping_ratio: N::splat(5.0), + } + } + + /// Default softness coefficients for joints. + pub fn joint_defaults() -> Self { + Self { + natural_frequency: N::splat(1.0e6), + damping_ratio: N::splat(1.0), + } + } + + /// The contact’s spring angular frequency for constraints regularization. + pub fn angular_frequency(&self) -> N { + self.natural_frequency * N::simd_two_pi() + } + + /// The [`Self::erp`] coefficient, multiplied by the inverse timestep length. + pub fn erp_inv_dt(&self, dt: N) -> N { + let ang_freq = self.angular_frequency(); + ang_freq / (dt * ang_freq + N::splat(2.0) * self.damping_ratio) + } + + /// The effective Error Reduction Parameter applied for calculating regularization forces. + /// + /// This parameter is computed automatically from [`Self::natural_frequency`], + /// [`Self::damping_ratio`] and the substep length. + pub fn erp(&self, dt: N) -> N { + dt * self.erp_inv_dt(dt) + } + + /// Compute CFM assuming a critically damped spring multiplied by the damping ratio. + /// + /// This coefficient softens the impulse applied at each solver iteration. + pub fn cfm_coeff(&self, dt: N) -> N { + let one = N::one(); + let erp = self.erp(dt); + let erp_is_not_zero = erp.simd_ne(N::zero()); + let inv_erp_minus_one = one / erp - one; + + // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass + // / (dt * dt * inv_erp_minus_one * inv_erp_minus_one); + // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass + // / (dt * inv_erp_minus_one); + // let cfm = 1.0 / (dt * dt * stiffness + dt * damping); + // NOTE: This simplifies to cfm = cfm_coeff / projected_mass: + let result = inv_erp_minus_one * inv_erp_minus_one + / ((one + inv_erp_minus_one) * N::splat(4.0) * self.damping_ratio * self.damping_ratio); + result.select(erp_is_not_zero, N::zero()) + } + + /// The CFM factor to be used in the constraint resolution. + /// + /// This parameter is computed automatically from [`Self::natural_frequency`], + /// [`Self::damping_ratio`] and the substep length. + pub fn cfm_factor(&self, dt: N) -> N { + let one = N::one(); + let cfm_coeff = self.cfm_coeff(dt); + + // We use this coefficient inside the impulse resolution. + // Surprisingly, several simplifications happen there. + // Let `m` the projected mass of the constraint. + // Let `m’` the projected mass that includes CFM: `m’ = 1 / (1 / m + cfm_coeff / m) = m / (1 + cfm_coeff)` + // We have: + // new_impulse = old_impulse - m’ (delta_vel - cfm * old_impulse) + // = old_impulse - m / (1 + cfm_coeff) * (delta_vel - cfm_coeff / m * old_impulse) + // = old_impulse * (1 - cfm_coeff / (1 + cfm_coeff)) - m / (1 + cfm_coeff) * delta_vel + // = old_impulse / (1 + cfm_coeff) - m * delta_vel / (1 + cfm_coeff) + // = 1 / (1 + cfm_coeff) * (old_impulse - m * delta_vel) + // So, setting cfm_factor = 1 / (1 + cfm_coeff). + // We obtain: + // new_impulse = cfm_factor * (old_impulse - m * delta_vel) + // + // The value returned by this function is this cfm_factor that can be used directly + // in the constraint solver. + one / (one + cfm_coeff) + } +} + /// Configuration parameters that control the physics simulation quality and behavior. /// /// These parameters affect how the physics engine advances time, resolves collisions, and @@ -80,34 +187,8 @@ pub struct IntegrationParameters { /// to numerical instabilities. pub min_ccd_dt: Real, - /// > 0: the damping ratio used by the springs for contact constraint stabilization. - /// - /// Larger values make the constraints more compliant (allowing more visible - /// penetrations before stabilization). - /// (default `5.0`). - pub contact_damping_ratio: Real, - - /// > 0: the natural frequency used by the springs for contact constraint regularization. - /// - /// Increasing this value will make it so that penetrations get fixed more quickly at the - /// expense of potential jitter effects due to overshooting. In order to make the simulation - /// look stiffer, it is recommended to increase the [`Self::contact_damping_ratio`] instead of this - /// value. - /// (default: `30.0`). - pub contact_natural_frequency: Real, - - /// > 0: the natural frequency used by the springs for joint constraint regularization. - /// - /// Increasing this value will make it so that penetrations get fixed more quickly. - /// (default: `1.0e6`). - pub joint_natural_frequency: Real, - - /// The fraction of critical damping applied to the joint for constraints regularization. - /// - /// Larger values make the constraints more compliant (allowing more joint - /// drift before stabilization). - /// (default `1.0`). - pub joint_damping_ratio: Real, + /// Softness coefficients for contact constraints. + pub contact_softness: SpringCoefficients, /// The coefficient in `[0, 1]` applied to warmstart impulses, i.e., impulses that are used as the /// initial solution (instead of 0) at the next simulation step. @@ -193,110 +274,7 @@ impl IntegrationParameters { } } - /// The contact’s spring angular frequency for constraints regularization. - pub fn contact_angular_frequency(&self) -> Real { - self.contact_natural_frequency * Real::two_pi() - } - - /// The [`Self::contact_erp`] coefficient, multiplied by the inverse timestep length. - pub fn contact_erp_inv_dt(&self) -> Real { - let ang_freq = self.contact_angular_frequency(); - ang_freq / (self.dt * ang_freq + 2.0 * self.contact_damping_ratio) - } - - /// The effective Error Reduction Parameter applied for calculating regularization forces - /// on contacts. - /// - /// This parameter is computed automatically from [`Self::contact_natural_frequency`], - /// [`Self::contact_damping_ratio`] and the substep length. - pub fn contact_erp(&self) -> Real { - self.dt * self.contact_erp_inv_dt() - } - - /// The joint’s spring angular frequency for constraint regularization. - pub fn joint_angular_frequency(&self) -> Real { - self.joint_natural_frequency * Real::two_pi() - } - - /// The [`Self::joint_erp`] coefficient, multiplied by the inverse timestep length. - pub fn joint_erp_inv_dt(&self) -> Real { - let ang_freq = self.joint_angular_frequency(); - ang_freq / (self.dt * ang_freq + 2.0 * self.joint_damping_ratio) - } - - /// The effective Error Reduction Parameter applied for calculating regularization forces - /// on joints. - /// - /// This parameter is computed automatically from [`Self::joint_natural_frequency`], - /// [`Self::joint_damping_ratio`] and the substep length. - pub fn joint_erp(&self) -> Real { - self.dt * self.joint_erp_inv_dt() - } - - /// The CFM factor to be used in the constraint resolution. - /// - /// This parameter is computed automatically from [`Self::contact_natural_frequency`], - /// [`Self::contact_damping_ratio`] and the substep length. - pub fn contact_cfm_factor(&self) -> Real { - // Compute CFM assuming a critically damped spring multiplied by the damping ratio. - // The logic is similar to [`Self::joint_cfm_coeff`]. - let contact_erp = self.contact_erp(); - if contact_erp == 0.0 { - return 0.0; - } - let inv_erp_minus_one = 1.0 / contact_erp - 1.0; - - // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass - // / (dt * dt * inv_erp_minus_one * inv_erp_minus_one); - // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass - // / (dt * inv_erp_minus_one); - // let cfm = 1.0 / (dt * dt * stiffness + dt * damping); - // NOTE: This simplifies to cfm = cfm_coeff / projected_mass: - let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one - / ((1.0 + inv_erp_minus_one) - * 4.0 - * self.contact_damping_ratio - * self.contact_damping_ratio); - - // Furthermore, we use this coefficient inside of the impulse resolution. - // Surprisingly, several simplifications happen there. - // Let `m` the projected mass of the constraint. - // Let `m’` the projected mass that includes CFM: `m’ = 1 / (1 / m + cfm_coeff / m) = m / (1 + cfm_coeff)` - // We have: - // new_impulse = old_impulse - m’ (delta_vel - cfm * old_impulse) - // = old_impulse - m / (1 + cfm_coeff) * (delta_vel - cfm_coeff / m * old_impulse) - // = old_impulse * (1 - cfm_coeff / (1 + cfm_coeff)) - m / (1 + cfm_coeff) * delta_vel - // = old_impulse / (1 + cfm_coeff) - m * delta_vel / (1 + cfm_coeff) - // = 1 / (1 + cfm_coeff) * (old_impulse - m * delta_vel) - // So, setting cfm_factor = 1 / (1 + cfm_coeff). - // We obtain: - // new_impulse = cfm_factor * (old_impulse - m * delta_vel) - // - // The value returned by this function is this cfm_factor that can be used directly - // in the constraint solver. - 1.0 / (1.0 + cfm_coeff) - } - - /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization. - /// - /// This parameter is computed automatically from [`Self::joint_natural_frequency`], - /// [`Self::joint_damping_ratio`] and the substep length. - pub fn joint_cfm_coeff(&self) -> Real { - // Compute CFM assuming a critically damped spring multiplied by the damping ratio. - // The logic is similar to `Self::contact_cfm_factor`. - let joint_erp = self.joint_erp(); - if joint_erp == 0.0 { - return 0.0; - } - let inv_erp_minus_one = 1.0 / joint_erp - 1.0; - inv_erp_minus_one * inv_erp_minus_one - / ((1.0 + inv_erp_minus_one) - * 4.0 - * self.joint_damping_ratio - * self.joint_damping_ratio) - } - - /// Amount of penetration the engine won’t attempt to correct (default: `0.001` multiplied by + /// Amount of penetration the engine won't attempt to correct (default: `0.001` multiplied by /// [`Self::length_unit`]). pub fn allowed_linear_error(&self) -> Real { self.normalized_allowed_linear_error * self.length_unit @@ -326,10 +304,7 @@ impl Default for IntegrationParameters { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, - contact_natural_frequency: 30.0, - contact_damping_ratio: 5.0, - joint_natural_frequency: 1.0e6, - joint_damping_ratio: 1.0, + contact_softness: SpringCoefficients::contact_defaults(), warmstart_coefficient: 1.0, num_internal_pgs_iterations: 1, num_internal_stabilization_iterations: 1, diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index 08b3316f0..c0869dbcb 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -1,3 +1,4 @@ +use crate::dynamics::integration_parameters::SpringCoefficients; use crate::dynamics::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::math::{Isometry, Point, Real}; @@ -91,6 +92,19 @@ impl FixedJoint { self.data.set_local_anchor2(anchor2); self } + + /// Gets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(&self) -> SpringCoefficients { + self.data.softness + } + + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn set_softness(&mut self, softness: SpringCoefficients) -> &mut Self { + self.data.softness = softness; + self + } } impl From for GenericJoint { @@ -138,13 +152,20 @@ impl FixedJointBuilder { self } - /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body. + /// Sets the joint's anchor, expressed in the local-space of the second rigid-body. #[must_use] pub fn local_anchor2(mut self, anchor2: Point) -> Self { self.0.set_local_anchor2(anchor2); self } + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; + self + } + /// Build the fixed joint. #[must_use] pub fn build(self) -> FixedJoint { diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 03493dd69..83d77dee6 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,5 +1,7 @@ #![allow(clippy::bad_bit_mask)] // Clippy will complain about the bitmasks due to JointAxesMask::FREE_FIXED_AXES being 0. +#![allow(clippy::unnecessary_cast)] // Casts are needed for switching between f32/f64. +use crate::dynamics::integration_parameters::SpringCoefficients; use crate::dynamics::solver::MotorParameters; use crate::dynamics::{ FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RigidBody, RopeJoint, @@ -282,6 +284,8 @@ pub struct GenericJoint { /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes` /// bitmask is applied to the coupled linear (resp. angular) axes. pub motors: [JointMotor; SPATIAL_DIM], + /// The coefficients controlling the joint constraints’ softness. + pub softness: SpringCoefficients, /// Are contacts between the attached rigid-bodies enabled? pub contacts_enabled: bool, /// Whether the joint is enabled. @@ -301,6 +305,7 @@ impl Default for GenericJoint { coupled_axes: JointAxesMask::empty(), limits: [JointLimits::default(); SPATIAL_DIM], motors: [JointMotor::default(); SPATIAL_DIM], + softness: SpringCoefficients::joint_defaults(), contacts_enabled: true, enabled: JointEnabled::Enabled, user_data: 0, @@ -440,6 +445,13 @@ impl GenericJoint { self } + /// Sets the spring coefficients controlling this joint constraint’s softness. + #[must_use] + pub fn set_softness(&mut self, softness: SpringCoefficients) -> &mut Self { + self.softness = softness; + self + } + /// The joint limits along the specified axis. #[must_use] pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits> { @@ -767,6 +779,13 @@ impl GenericJointBuilder { self } + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.softness = softness; + self + } + /// An arbitrary user-defined 128-bit integer associated to the joints built by this builder. pub fn user_data(mut self, data: u128) -> Self { self.0.user_data = data; diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 84886ca21..a8d24c7e6 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -314,6 +314,7 @@ impl MultibodyJoint { jacobians, constraints, &mut num_constraints, + self.data.softness, ); } curr_free_dof += 1; @@ -349,6 +350,7 @@ impl MultibodyJoint { jacobians, constraints, &mut num_constraints, + self.data.softness, ); Some(limits) } else { diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index 0f8f71df5..5f6672052 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -1,5 +1,6 @@ #![allow(missing_docs)] // For downcast. +use crate::dynamics::integration_parameters::SpringCoefficients; use crate::dynamics::joint::MultibodyLink; use crate::dynamics::solver::{GenericJointConstraint, WritebackId}; use crate::dynamics::{IntegrationParameters, JointMotor, Multibody}; @@ -19,12 +20,16 @@ pub fn unit_joint_limit_constraint( jacobians: &mut DVector, constraints: &mut [GenericJointConstraint], insert_at: &mut usize, + softness: SpringCoefficients, ) { let ndofs = multibody.ndofs(); let min_enabled = curr_pos < limits[0]; let max_enabled = limits[1] < curr_pos; - let erp_inv_dt = params.joint_erp_inv_dt(); - let cfm_coeff = params.joint_cfm_coeff(); + + // Compute per-joint ERP and CFM + let erp_inv_dt = softness.erp_inv_dt(params.dt); + let cfm_coeff = softness.cfm_coeff(params.dt); + let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt; let rhs_wo_bias = 0.0; diff --git a/src/dynamics/joint/pin_slot_joint.rs b/src/dynamics/joint/pin_slot_joint.rs index 3980a7a2f..5a50b063e 100644 --- a/src/dynamics/joint/pin_slot_joint.rs +++ b/src/dynamics/joint/pin_slot_joint.rs @@ -1,6 +1,7 @@ #[cfg(feature = "dim2")] use crate::dynamics::joint::{GenericJointBuilder, JointAxesMask}; +use crate::dynamics::integration_parameters::SpringCoefficients; use crate::dynamics::joint::GenericJoint; use crate::dynamics::{JointAxis, MotorModel}; use crate::math::{Point, Real, UnitVector}; @@ -155,6 +156,19 @@ impl PinSlotJoint { self.data.set_limits(JointAxis::LinX, limits); self } + + /// Gets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(&self) -> SpringCoefficients { + self.data.softness + } + + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn set_softness(&mut self, softness: SpringCoefficients) -> &mut Self { + self.data.softness = softness; + self + } } impl From for GenericJoint { @@ -255,13 +269,20 @@ impl PinSlotJointBuilder { self } - /// Sets the `[min,max]` limit distances attached bodies can translate along the joint’s principal axis. + /// Sets the `[min,max]` limit distances attached bodies can translate along the joint's principal axis. #[must_use] pub fn limits(mut self, limits: [Real; 2]) -> Self { self.0.set_limits(limits); self } + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; + self + } + /// Builds the pin slot joint. #[must_use] pub fn build(self) -> PinSlotJoint { diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index 9fbc81f95..3993a8f6f 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -1,3 +1,4 @@ +use crate::dynamics::integration_parameters::SpringCoefficients; use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, MotorModel}; use crate::math::{Point, Real, UnitVector}; @@ -176,6 +177,19 @@ impl PrismaticJoint { self.data.set_limits(JointAxis::LinX, limits); self } + + /// Gets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(&self) -> SpringCoefficients { + self.data.softness + } + + /// Sets the softness of this joint. + #[must_use] + pub fn set_softness(&mut self, softness: SpringCoefficients) -> &mut Self { + self.data.softness = softness; + self + } } impl From for GenericJoint { @@ -275,13 +289,20 @@ impl PrismaticJointBuilder { self } - /// Sets the `[min,max]` limit distances attached bodies can translate along the joint’s principal axis. + /// Sets the `[min,max]` limit distances attached bodies can translate along the joint's principal axis. #[must_use] pub fn limits(mut self, limits: [Real; 2]) -> Self { self.0.set_limits(limits); self } + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; + self + } + /// Builds the prismatic joint. #[must_use] pub fn build(self) -> PrismaticJoint { diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index fdc75869b..0bdeb1338 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -1,3 +1,4 @@ +use crate::dynamics::integration_parameters::SpringCoefficients; use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, JointLimits, JointMotor, MotorModel}; use crate::math::{Point, Real, Rotation}; @@ -203,6 +204,19 @@ impl RevoluteJoint { self.data.set_limits(JointAxis::AngX, limits); self } + + /// Gets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(&self) -> SpringCoefficients { + self.data.softness + } + + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn set_softness(&mut self, softness: SpringCoefficients) -> &mut Self { + self.data.softness = softness; + self + } } impl From for GenericJoint { @@ -296,13 +310,20 @@ impl RevoluteJointBuilder { self } - /// Sets the `[min,max]` limit angles attached bodies can rotate along the joint’s principal axis. + /// Sets the `[min,max]` limit angles attached bodies can rotate along the joint's principal axis. #[must_use] pub fn limits(mut self, limits: [Real; 2]) -> Self { self.0.set_limits(limits); self } + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; + self + } + /// Builds the revolute joint. #[must_use] pub fn build(self) -> RevoluteJoint { diff --git a/src/dynamics/joint/rope_joint.rs b/src/dynamics/joint/rope_joint.rs index 2fc5e8102..d9e5cb683 100644 --- a/src/dynamics/joint/rope_joint.rs +++ b/src/dynamics/joint/rope_joint.rs @@ -1,3 +1,4 @@ +use crate::dynamics::integration_parameters::SpringCoefficients; use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, MotorModel}; use crate::math::{Point, Real}; @@ -153,6 +154,19 @@ impl RopeJoint { self.data.set_limits(JointAxis::LinX, [0.0, max_dist]); self } + + /// Gets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(&self) -> SpringCoefficients { + self.data.softness + } + + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn set_softness(&mut self, softness: SpringCoefficients) -> &mut Self { + self.data.softness = softness; + self + } } impl From for GenericJoint { @@ -245,6 +259,13 @@ impl RopeJointBuilder { self } + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; + self + } + /// Builds the rope joint. #[must_use] pub fn build(self) -> RopeJoint { diff --git a/src/dynamics/joint/spherical_joint.rs b/src/dynamics/joint/spherical_joint.rs index 5c63db6a9..fc99bf26a 100644 --- a/src/dynamics/joint/spherical_joint.rs +++ b/src/dynamics/joint/spherical_joint.rs @@ -1,3 +1,4 @@ +use crate::dynamics::integration_parameters::SpringCoefficients; use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, JointMotor, MotorModel}; use crate::math::{Isometry, Point, Real}; @@ -192,6 +193,19 @@ impl SphericalJoint { self.data.set_limits(axis, limits); self } + + /// Gets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(&self) -> SpringCoefficients { + self.data.softness + } + + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn set_softness(&mut self, softness: SpringCoefficients) -> &mut Self { + self.data.softness = softness; + self + } } impl From for GenericJoint { @@ -305,6 +319,13 @@ impl SphericalJointBuilder { self } + /// Sets the softness of this joint’s locked degrees of freedom. + #[must_use] + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; + self + } + /// Builds the spherical joint. #[must_use] pub fn build(self) -> SphericalJoint { diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 75a91554f..41e91dadb 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -2,7 +2,7 @@ pub use self::ccd::CCDSolver; pub use self::coefficient_combine_rule::CoefficientCombineRule; -pub use self::integration_parameters::IntegrationParameters; +pub use self::integration_parameters::{IntegrationParameters, SpringCoefficients}; pub use self::island_manager::IslandManager; #[cfg(feature = "dim3")] 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 595dbb0be..50c0bc057 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs @@ -254,10 +254,10 @@ impl ContactWithCoulombFrictionBuilder { _multibodies: &MultibodyJointSet, constraint: &mut ContactWithCoulombFriction, ) { - let cfm_factor = SimdReal::splat(params.contact_cfm_factor()); + let cfm_factor = SimdReal::splat(params.contact_softness.cfm_factor(params.dt)); let inv_dt = SimdReal::splat(params.inv_dt()); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error()); - let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt()); + let erp_inv_dt = SimdReal::splat(params.contact_softness.erp_inv_dt(params.dt)); let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity()); let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient); 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 0dd927a85..fc50997f2 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs @@ -261,10 +261,10 @@ impl ContactWithTwistFrictionBuilder { _multibodies: &MultibodyJointSet, constraint: &mut ContactWithTwistFriction, ) { - let cfm_factor = SimdReal::splat(params.contact_cfm_factor()); + let cfm_factor = SimdReal::splat(params.contact_softness.cfm_factor(params.dt)); let inv_dt = SimdReal::splat(params.inv_dt()); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error()); - let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt()); + let erp_inv_dt = SimdReal::splat(params.contact_softness.erp_inv_dt(params.dt)); let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity()); let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient); diff --git a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs index 17ef870df..c8294bc82 100644 --- a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs @@ -346,9 +346,9 @@ impl GenericContactConstraintBuilder { multibodies: &MultibodyJointSet, constraint: &mut GenericContactConstraint, ) { - let cfm_factor = params.contact_cfm_factor(); + let cfm_factor = params.contact_softness.cfm_factor(params.dt); let inv_dt = params.inv_dt(); - let erp_inv_dt = params.contact_erp_inv_dt(); + let erp_inv_dt = params.contact_softness.erp_inv_dt(params.dt); // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint. let pose1 = multibodies diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs index ee48b273f..f5f320761 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs @@ -140,6 +140,7 @@ impl GenericJointConstraint { mb1, mb2, i - DIM, + joint.softness, WritebackId::Dof(i), ); len += 1; @@ -157,6 +158,7 @@ impl GenericJointConstraint { mb1, mb2, i, + joint.softness, WritebackId::Dof(i), ); len += 1; @@ -176,6 +178,7 @@ impl GenericJointConstraint { mb2, i - DIM, [joint.limits[i].min, joint.limits[i].max], + joint.softness, WritebackId::Limit(i), ); len += 1; @@ -194,6 +197,7 @@ impl GenericJointConstraint { mb2, i, [joint.limits[i].min, joint.limits[i].max], + joint.softness, WritebackId::Limit(i), ); len += 1; 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 d9bbf2669..476edef55 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -11,6 +11,7 @@ use crate::utils; use crate::utils::IndexMut2; use na::{DVector, SVector}; +use crate::dynamics::integration_parameters::SpringCoefficients; use crate::dynamics::solver::ConstraintsCounts; use crate::dynamics::solver::solver_body::SolverBodies; #[cfg(feature = "dim3")] @@ -406,6 +407,7 @@ impl JointConstraintHelper { mb1: LinkOrBodyRef, mb2: LinkOrBodyRef, locked_axis: usize, + softness: SpringCoefficients, writeback_id: WritebackId, ) -> GenericJointConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); @@ -426,7 +428,7 @@ impl JointConstraintHelper { ang_jac2, ); - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = softness.erp_inv_dt(params.dt); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; c.rhs += rhs_bias; c @@ -444,6 +446,7 @@ impl JointConstraintHelper { mb2: LinkOrBodyRef, limited_axis: usize, limits: [Real; 2], + softness: SpringCoefficients, writeback_id: WritebackId, ) -> GenericJointConstraint { let lin_jac = self.basis.column(limited_axis).into_owned(); @@ -468,7 +471,8 @@ impl JointConstraintHelper { let min_enabled = dist <= limits[0]; let max_enabled = limits[1] <= dist; - let erp_inv_dt = params.joint_erp_inv_dt(); + 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; constraint.rhs += rhs_bias; constraint.impulse_bounds = [ @@ -545,6 +549,7 @@ impl JointConstraintHelper { mb1: LinkOrBodyRef, mb2: LinkOrBodyRef, _locked_axis: usize, + softness: SpringCoefficients, writeback_id: WritebackId, ) -> GenericJointConstraint { #[cfg(feature = "dim2")] @@ -566,7 +571,7 @@ impl JointConstraintHelper { ang_jac, ); - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = softness.erp_inv_dt(params.dt); #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] @@ -587,6 +592,7 @@ impl JointConstraintHelper { mb2: LinkOrBodyRef, _limited_axis: usize, limits: [Real; 2], + softness: SpringCoefficients, writeback_id: WritebackId, ) -> GenericJointConstraint { #[cfg(feature = "dim2")] @@ -620,7 +626,7 @@ impl JointConstraintHelper { max_enabled as u32 as Real * Real::MAX, ]; - let erp_inv_dt = params.joint_erp_inv_dt(); + 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; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 05587821b..2dc702d10 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -15,7 +15,10 @@ use crate::utils::{SimdBasis, SimdQuat}; use na::SMatrix; #[cfg(feature = "simd-is-enabled")] -use crate::math::{SIMD_WIDTH, SimdReal}; +use { + crate::dynamics::SpringCoefficients, + crate::math::{SIMD_WIDTH, SimdReal}, +}; pub struct JointConstraintBuilder { body1: u32, @@ -103,6 +106,7 @@ pub struct JointConstraintBuilderSimd { local_frame1: Isometry, local_frame2: Isometry, locked_axes: u8, + softness: SpringCoefficients, constraint_id: usize, } @@ -149,6 +153,10 @@ impl JointConstraintBuilderSimd { local_frame1, local_frame2, locked_axes: joint[0].data.locked_axes.bits(), + softness: SpringCoefficients { + natural_frequency: array![|ii| joint[ii].data.softness.natural_frequency].into(), + damping_ratio: array![|ii| joint[ii].data.softness.damping_ratio].into(), + }, constraint_id: *out_constraint_id, }; @@ -191,6 +199,7 @@ impl JointConstraintBuilderSimd { &frame1, &frame2, self.locked_axes, + self.softness, &mut out[self.constraint_id..], ); } @@ -277,17 +286,25 @@ impl JointConstraintHelper { limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, + erp_inv_dt: N, + cfm_coeff: N, ) -> JointConstraint { let zero = N::zero(); - let mut constraint = - self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id); + let mut constraint = self.lock_linear( + params, + joint_id, + body1, + body2, + limited_axis, + writeback_id, + erp_inv_dt, + cfm_coeff, + ); let dist = self.lin_err.dot(&constraint.lin_jac); let min_enabled = dist.simd_le(limits[0]); let max_enabled = limits[1].simd_le(dist); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt; constraint.rhs = constraint.rhs_wo_bias + rhs_bias; @@ -309,6 +326,8 @@ impl JointConstraintHelper { coupled_axes: u8, limits: [N; 2], writeback_id: WritebackId, + erp_inv_dt: N, + cfm_coeff: N, ) -> JointConstraint { let zero = N::zero(); let mut lin_jac = Vector::zeros(); @@ -345,8 +364,6 @@ impl JointConstraintHelper { let ii_ang_jac1 = body1.ii * ang_jac1; let ii_ang_jac2 = body2.ii * ang_jac2; - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = (dist - limits[1]).simd_max(zero) * erp_inv_dt; let rhs = rhs_wo_bias + rhs_bias; let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; @@ -385,8 +402,18 @@ impl JointConstraintHelper { writeback_id: WritebackId, ) -> JointConstraint { let inv_dt = N::splat(params.inv_dt()); - let mut constraint = - self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id); + let mut constraint = self.lock_linear( + params, + joint_id, + body1, + body2, + motor_axis, + writeback_id, + // Set regularization factors to zero. + // The motor impl. will overwrite them after. + N::zero(), + N::zero(), + ); let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { @@ -491,12 +518,14 @@ impl JointConstraintHelper { pub fn lock_linear( &self, - params: &IntegrationParameters, + _params: &IntegrationParameters, joint_id: [JointIndex; LANES], body1: &JointSolverBody, body2: &JointSolverBody, locked_axis: usize, writeback_id: WritebackId, + erp_inv_dt: N, + cfm_coeff: N, ) -> JointConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); #[cfg(feature = "dim2")] @@ -509,8 +538,6 @@ impl JointConstraintHelper { let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); let rhs_wo_bias = N::zero(); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; let ii_ang_jac1 = body1.ii * ang_jac1; @@ -540,13 +567,15 @@ impl JointConstraintHelper { pub fn limit_angular( &self, - params: &IntegrationParameters, + _params: &IntegrationParameters, joint_id: [JointIndex; LANES], body1: &JointSolverBody, body2: &JointSolverBody, _limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, + erp_inv_dt: N, + cfm_coeff: N, ) -> JointConstraint { let zero = N::zero(); let half = N::splat(0.5); @@ -568,8 +597,6 @@ impl JointConstraintHelper { #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); let rhs_wo_bias = N::zero(); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero) - (s_limits[0] - s_ang).simd_max(zero)) * erp_inv_dt; @@ -663,12 +690,14 @@ impl JointConstraintHelper { pub fn lock_angular( &self, - params: &IntegrationParameters, + _params: &IntegrationParameters, joint_id: [JointIndex; LANES], body1: &JointSolverBody, body2: &JointSolverBody, _locked_axis: usize, writeback_id: WritebackId, + erp_inv_dt: N, + cfm_coeff: N, ) -> JointConstraint { #[cfg(feature = "dim2")] let ang_jac = N::one(); @@ -676,8 +705,6 @@ impl JointConstraintHelper { let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); let rhs_wo_bias = N::zero(); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] @@ -760,13 +787,15 @@ impl JointConstraintHelper { #[cfg(feature = "dim3")] pub fn limit_angular_coupled( &self, - params: &IntegrationParameters, + _params: &IntegrationParameters, joint_id: [JointIndex; 1], body1: &JointSolverBody, body2: &JointSolverBody, coupled_axes: u8, limits: [Real; 2], writeback_id: WritebackId, + erp_inv_dt: Real, + cfm_coeff: Real, ) -> JointConstraint { // NOTE: right now, this only supports exactly 2 coupled axes. let ang_coupled_axes = coupled_axes >> DIM; @@ -791,8 +820,6 @@ impl JointConstraintHelper { let rhs_wo_bias = 0.0; - let erp_inv_dt = params.joint_erp_inv_dt(); - let cfm_coeff = params.joint_cfm_coeff(); let rhs_bias = ((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt; let ii_ang_jac1 = body1.ii * ang_jac; diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index e33c13193..f64a820dc 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -11,6 +11,8 @@ use crate::dynamics::solver::solver_body::SolverBodies; use crate::math::{SIMD_WIDTH, SimdReal}; #[cfg(feature = "dim2")] use crate::num::Zero; +#[cfg(feature = "simd-is-enabled")] +use na::SimdValue; #[derive(Copy, Clone, PartialEq, Debug)] pub struct MotorParameters { @@ -142,6 +144,10 @@ impl JointConstraint { let limit_axes = joint.limit_axes.bits() & !locked_axes; let coupled_axes = joint.coupled_axes.bits(); + // Compute per-joint ERP and CFM coefficients + let erp_inv_dt = joint.softness.erp_inv_dt(params.dt); + let cfm_coeff = joint.softness.cfm_coeff(params.dt); + // The has_lin/ang_coupling test is needed to avoid shl overflow later. let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0; let first_coupled_lin_axis_id = @@ -236,14 +242,24 @@ impl JointConstraint { body2, i - DIM, WritebackId::Dof(i), + erp_inv_dt, + cfm_coeff, ); len += 1; } } for i in 0..DIM { if locked_axes & (1 << i) != 0 { - out[len] = - builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i)); + out[len] = builder.lock_linear( + params, + [joint_id], + body1, + body2, + i, + WritebackId::Dof(i), + erp_inv_dt, + cfm_coeff, + ); len += 1; } } @@ -258,6 +274,8 @@ impl JointConstraint { i - DIM, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + erp_inv_dt, + cfm_coeff, ); len += 1; } @@ -272,6 +290,8 @@ impl JointConstraint { i, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + erp_inv_dt, + cfm_coeff, ); len += 1; } @@ -290,6 +310,8 @@ impl JointConstraint { joint.limits[first_coupled_ang_axis_id].max, ], WritebackId::Limit(first_coupled_ang_axis_id), + erp_inv_dt, + cfm_coeff, ); len += 1; } @@ -306,6 +328,8 @@ impl JointConstraint { joint.limits[first_coupled_lin_axis_id].max, ], WritebackId::Limit(first_coupled_lin_axis_id), + erp_inv_dt, + cfm_coeff, ); len += 1; } @@ -344,8 +368,13 @@ impl JointConstraint { frame1: &Isometry, frame2: &Isometry, locked_axes: u8, + softness: crate::dynamics::SpringCoefficients, out: &mut [Self], ) -> usize { + let dt = SimdReal::splat(params.dt); + let erp_inv_dt = softness.erp_inv_dt(dt); + let cfm_coeff = softness.cfm_coeff(dt); + let builder = JointConstraintHelper::new( frame1, frame2, @@ -357,8 +386,16 @@ impl JointConstraint { let mut len = 0; for i in 0..DIM { if locked_axes & (1 << i) != 0 { - out[len] = - builder.lock_linear(params, joint_id, body1, body2, i, WritebackId::Dof(i)); + out[len] = builder.lock_linear( + params, + joint_id, + body1, + body2, + i, + WritebackId::Dof(i), + erp_inv_dt, + cfm_coeff, + ); len += 1; } } @@ -372,6 +409,8 @@ impl JointConstraint { body2, i - DIM, WritebackId::Dof(i), + erp_inv_dt, + cfm_coeff, ); len += 1; } diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 17bd64444..c89a0b264 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -189,33 +189,24 @@ pub(crate) fn update_ui( let mut substep_params = *integration_parameters; substep_params.dt /= substep_params.num_solver_iterations as Real; - let curr_erp = substep_params.contact_erp(); - let curr_cfm_factor = substep_params.contact_cfm_factor(); + let curr_erp = substep_params.contact_softness.erp(substep_params.dt); + let curr_cfm_factor = substep_params + .contact_softness + .cfm_factor(substep_params.dt); ui.add( Slider::new( - &mut integration_parameters.contact_natural_frequency, + &mut integration_parameters.contact_softness.natural_frequency, 0.01..=120.0, ) .text(format!("contacts Hz (erp = {curr_erp:.3})")), ); ui.add( Slider::new( - &mut integration_parameters.contact_damping_ratio, + &mut integration_parameters.contact_softness.damping_ratio, 0.01..=20.0, ) .text(format!("damping ratio (cfm-factor = {curr_cfm_factor:.3})",)), ); - ui.add( - Slider::new( - &mut integration_parameters.joint_natural_frequency, - 0.0..=1200000.0, - ) - .text("joint erp"), - ); - ui.add( - Slider::new(&mut integration_parameters.joint_damping_ratio, 0.0..=20.0) - .text("joint damping ratio"), - ); } #[cfg(feature = "parallel")]