From efc4faa7f993289ef4529e269297feee1302f580 Mon Sep 17 00:00:00 2001 From: st170001 Date: Sat, 14 Dec 2024 21:02:27 +0100 Subject: [PATCH 01/14] Additional groove joint 2D implementation with 1 translation + 1 rotation --- src/dynamics/joint/generic_joint.rs | 2 + src/dynamics/joint/groove_joint.rs | 272 ++++++++++++++++++++++++++++ src/dynamics/joint/mod.rs | 2 + 3 files changed, 276 insertions(+) create mode 100644 src/dynamics/joint/groove_joint.rs diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 8d4066d35..23f5f2727 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -65,6 +65,8 @@ bitflags::bitflags! { const LOCKED_REVOLUTE_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits(); /// The set of degrees of freedom locked by a prismatic joint. const LOCKED_PRISMATIC_AXES = Self::LIN_Y.bits() | Self::ANG_X.bits(); + /// The set of degrees of freedom locked by a groove joint. + const LOCKED_GROOVE_AXES = Self::LIN_Y.bits(); /// The set of degrees of freedom locked by a fixed joint. const LOCKED_FIXED_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::ANG_X.bits(); /// The set of degrees of freedom left free by a revolute joint. diff --git a/src/dynamics/joint/groove_joint.rs b/src/dynamics/joint/groove_joint.rs new file mode 100644 index 000000000..fa84b0815 --- /dev/null +++ b/src/dynamics/joint/groove_joint.rs @@ -0,0 +1,272 @@ +use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; +use crate::dynamics::{JointAxis, MotorModel}; +use crate::math::{Point, Real, UnitVector}; + +use super::{JointLimits, JointMotor}; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] +/// A groove joint, locks all relative motion between two bodies except for translation along the joint’s principal axis and relative rotations. +pub struct GrooveJoint { + /// The underlying joint data. + pub data: GenericJoint, +} + +impl GrooveJoint { + /// Creates a new groove joint allowing only relative translations along the specified axis and relative rotations. + /// + /// This axis is expressed in the local-space of both rigid-bodies. + #[cfg(feature = "dim2")] + pub fn new(axis: UnitVector) -> Self { + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_GROOVE_AXES) + .local_axis1(axis) + .local_axis2(axis) + .build(); + Self { data } + } + + /// The underlying generic joint. + pub fn data(&self) -> &GenericJoint { + &self.data + } + + /// Are contacts between the attached rigid-bodies enabled? + pub fn contacts_enabled(&self) -> bool { + self.data.contacts_enabled + } + + /// Sets whether contacts between the attached rigid-bodies are enabled. + pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self { + self.data.set_contacts_enabled(enabled); + self + } + + /// The joint’s anchor, expressed in the local-space of the first rigid-body. + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.data.local_anchor1() + } + + /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body. + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + /// The joint’s anchor, expressed in the local-space of the second rigid-body. + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.data.local_anchor2() + } + + /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body. + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } + + /// The principal axis of the joint, expressed in the local-space of the first rigid-body. + #[must_use] + pub fn local_axis1(&self) -> UnitVector { + self.data.local_axis1() + } + + /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body. + pub fn set_local_axis1(&mut self, axis1: UnitVector) -> &mut Self { + self.data.set_local_axis1(axis1); + self + } + + /// The principal axis of the joint, expressed in the local-space of the second rigid-body. + #[must_use] + pub fn local_axis2(&self) -> UnitVector { + self.data.local_axis2() + } + + /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body. + pub fn set_local_axis2(&mut self, axis2: UnitVector) -> &mut Self { + self.data.set_local_axis2(axis2); + self + } + + /// The motor affecting the joint’s translational degree of freedom. + #[must_use] + pub fn motor(&self) -> Option<&JointMotor> { + self.data.motor(JointAxis::LinX) + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self { + self.data.set_motor_model(JointAxis::LinX, model); + self + } + + /// Sets the target velocity this motor needs to reach. + pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self { + self.data + .set_motor_velocity(JointAxis::LinX, target_vel, factor); + self + } + + /// Sets the target angle this motor needs to reach. + pub fn set_motor_position( + &mut self, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor_position(JointAxis::LinX, target_pos, stiffness, damping); + self + } + + /// Configure both the target angle and target velocity of the motor. + pub fn set_motor( + &mut self, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor(JointAxis::LinX, target_pos, target_vel, stiffness, damping); + self + } + + /// Sets the maximum force the motor can deliver. + pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self { + self.data.set_motor_max_force(JointAxis::LinX, max_force); + self + } + + /// The limit distance attached bodies can translate along the joint’s principal axis. + #[must_use] + pub fn limits(&self) -> Option<&JointLimits> { + self.data.limits(JointAxis::LinX) + } + + /// Sets the `[min,max]` limit distances attached bodies can translate along the joint’s principal axis. + pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self { + self.data.set_limits(JointAxis::LinX, limits); + self + } +} + +impl From for GenericJoint { + fn from(val: GrooveJoint) -> GenericJoint { + val.data + } +} + +/// Create groove joints using the builder pattern. +/// +/// A groove joint locks all relative motion except for translations along the joint’s principal axis and relative rotations. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct GrooveJointBuilder(pub GrooveJoint); + +impl GrooveJointBuilder { + /// Creates a new builder for groove joints. + /// + /// This axis is expressed in the local-space of both rigid-bodies. + #[cfg(feature = "dim2")] + pub fn new(axis: UnitVector) -> Self { + Self(GrooveJoint::new(axis)) + } + + /// Sets whether contacts between the attached rigid-bodies are enabled. + #[must_use] + pub fn contacts_enabled(mut self, enabled: bool) -> Self { + self.0.set_contacts_enabled(enabled); + self + } + + /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body. + #[must_use] + pub fn local_anchor1(mut self, anchor1: Point) -> Self { + self.0.set_local_anchor1(anchor1); + self + } + + /// Sets the joint’s anchor, expressed in the local-space of the second rigid-body. + #[must_use] + pub fn local_anchor2(mut self, anchor2: Point) -> Self { + self.0.set_local_anchor2(anchor2); + self + } + + /// Sets the principal axis of the joint, expressed in the local-space of the first rigid-body. + #[must_use] + pub fn local_axis1(mut self, axis1: UnitVector) -> Self { + self.0.set_local_axis1(axis1); + self + } + + /// Sets the principal axis of the joint, expressed in the local-space of the second rigid-body. + #[must_use] + pub fn local_axis2(mut self, axis2: UnitVector) -> Self { + self.0.set_local_axis2(axis2); + self + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + #[must_use] + pub fn motor_model(mut self, model: MotorModel) -> Self { + self.0.set_motor_model(model); + self + } + + /// Sets the target velocity this motor needs to reach. + #[must_use] + pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self { + self.0.set_motor_velocity(target_vel, factor); + self + } + + /// Sets the target angle this motor needs to reach. + #[must_use] + pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self { + self.0.set_motor_position(target_pos, stiffness, damping); + self + } + + /// Configure both the target angle and target velocity of the motor. + #[must_use] + pub fn set_motor( + mut self, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> Self { + self.0.set_motor(target_pos, target_vel, stiffness, damping); + self + } + + /// Sets the maximum force the motor can deliver. + #[must_use] + pub fn motor_max_force(mut self, max_force: Real) -> Self { + self.0.set_motor_max_force(max_force); + self + } + + /// 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 + } + + /// Builds the groove joint. + #[must_use] + pub fn build(self) -> GrooveJoint { + self.0 + } +} + +impl From for GenericJoint { + fn from(val: GrooveJointBuilder) -> GenericJoint { + val.0.into() + } +} diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 423d4c285..af398120a 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -1,5 +1,6 @@ pub use self::fixed_joint::*; pub use self::generic_joint::*; +pub use self::groove_joint::*; pub use self::impulse_joint::*; pub use self::motor_model::MotorModel; pub use self::multibody_joint::*; @@ -13,6 +14,7 @@ pub use self::spherical_joint::*; mod fixed_joint; mod generic_joint; +mod groove_joint; mod impulse_joint; mod motor_model; mod multibody_joint; From 0108ae8424365ca0e78f0f2d2745faec82422de0 Mon Sep 17 00:00:00 2001 From: st170001 Date: Sat, 14 Dec 2024 21:35:28 +0100 Subject: [PATCH 02/14] Conditional import for 2d feature flag --- src/dynamics/joint/groove_joint.rs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/dynamics/joint/groove_joint.rs b/src/dynamics/joint/groove_joint.rs index fa84b0815..0b7c60477 100644 --- a/src/dynamics/joint/groove_joint.rs +++ b/src/dynamics/joint/groove_joint.rs @@ -1,4 +1,7 @@ -use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; +#[cfg(feature = "dim2")] +use crate::dynamics::joint::{GenericJointBuilder, JointAxesMask}; + +use crate::dynamics::joint::GenericJoint; use crate::dynamics::{JointAxis, MotorModel}; use crate::math::{Point, Real, UnitVector}; From 1c62ac1298d3a28d86ebcf34df1b40ff3349f836 Mon Sep 17 00:00:00 2001 From: st170001 Date: Tue, 31 Dec 2024 13:00:26 +0100 Subject: [PATCH 03/14] Groove joint 2d tests in testbed examples --- examples2d/all_examples2.rs | 2 + examples2d/groove_joint2.rs | 79 +++++++++++++++++++++++++++++++++++++ 2 files changed, 81 insertions(+) create mode 100644 examples2d/groove_joint2.rs diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 74bd89d0a..83ae9d489 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -20,6 +20,7 @@ mod debug_intersection2; mod debug_total_overlap2; mod debug_vertical_column2; mod drum2; +mod groove_joint2; mod heightfield2; mod inverse_kinematics2; mod joint_motor_position2; @@ -92,6 +93,7 @@ pub fn main() { ("One-way platforms", one_way_platforms2::init_world), ("Platform", platform2::init_world), ("Polyline", polyline2::init_world), + ("Groove Joint", groove_joint2::init_world), ("Pyramid", pyramid2::init_world), ("Restitution", restitution2::init_world), ("Rope Joints", rope_joints2::init_world), diff --git a/examples2d/groove_joint2.rs b/examples2d/groove_joint2.rs new file mode 100644 index 000000000..789411572 --- /dev/null +++ b/examples2d/groove_joint2.rs @@ -0,0 +1,79 @@ +use rapier2d::prelude::*; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground_size = 3.0; + let ground_height = 0.1; + + let rigid_body_floor = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height]); + let floor_handle = bodies.insert(rigid_body_floor); + let floor_collider = ColliderBuilder::cuboid(ground_size, ground_height); + colliders.insert_with_parent(floor_collider, floor_handle, &mut bodies); + + /* + * Character we will control manually. + */ + let rigid_body_character = + RigidBodyBuilder::kinematic_position_based().translation(vector![0.0, 0.3]); + let character_handle = bodies.insert(rigid_body_character); + let character_collider = ColliderBuilder::cuboid(0.15, 0.3); + colliders.insert_with_parent(character_collider, character_handle, &mut bodies); + + /* + * Tethered cube. + */ + let rad = 0.4; + + let rigid_body_cube = + RigidBodyBuilder::new(RigidBodyType::Dynamic).translation(vector![1.0, 1.0]); + let cube_handle = bodies.insert(rigid_body_cube); + let cube_collider = ColliderBuilder::cuboid(rad, rad); + colliders.insert_with_parent(cube_collider, cube_handle, &mut bodies); + + /* + * Rotation axis indicator ball. + */ + let rigid_body_ball = + RigidBodyBuilder::new(RigidBodyType::Dynamic).translation(vector![1.0, 1.0]); + let ball_handle = bodies.insert(rigid_body_ball); + let ball_collider = ColliderBuilder::ball(0.1); + colliders.insert_with_parent(ball_collider, ball_handle, &mut bodies); + + /* + * Fixed joint between rotation axis indicator and cube. + */ + let fixed_joint = FixedJointBuilder::new() + .local_anchor1(point![0.0, 0.0]) + .local_anchor2(point![0.0, -0.4]) + .build(); + impulse_joints.insert(cube_handle, ball_handle, fixed_joint, true); + + /* + * Groove joint between cube and ground. + */ + let axis = UnitVector::new_normalize(vector![1.0, 1.0]); + let groove_joint = GrooveJointBuilder::new(axis) + .local_anchor1(point![2.0, 2.0]) + .local_anchor2(point![0.0, 0.4]) + .limits([-1.0, f32::INFINITY]) // Set the limits for the groove joint + .build(); + impulse_joints.insert(character_handle, cube_handle, groove_joint, true); + + /* + * 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); +} From dd5b1bf9d1447c1ddbca416eb7901b748fb8b841 Mon Sep 17 00:00:00 2001 From: st170001 Date: Thu, 23 Jan 2025 11:13:47 +0100 Subject: [PATCH 04/14] Renamed GrooveJoint to PinSlotJoint --- examples2d/all_examples2.rs | 4 +-- .../{groove_joint2.rs => pin_slot_joint2.rs} | 10 +++--- src/dynamics/joint/generic_joint.rs | 4 +-- src/dynamics/joint/mod.rs | 4 +-- .../{groove_joint.rs => pin_slot_joint.rs} | 34 +++++++++---------- 5 files changed, 28 insertions(+), 28 deletions(-) rename examples2d/{groove_joint2.rs => pin_slot_joint2.rs} (86%) rename src/dynamics/joint/{groove_joint.rs => pin_slot_joint.rs} (89%) diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 83ae9d489..bbfbe8c1b 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -20,7 +20,7 @@ mod debug_intersection2; mod debug_total_overlap2; mod debug_vertical_column2; mod drum2; -mod groove_joint2; +mod pin_slot_joint2; mod heightfield2; mod inverse_kinematics2; mod joint_motor_position2; @@ -93,7 +93,7 @@ pub fn main() { ("One-way platforms", one_way_platforms2::init_world), ("Platform", platform2::init_world), ("Polyline", polyline2::init_world), - ("Groove Joint", groove_joint2::init_world), + ("Pin Slot Joint", pin_slot_joint2::init_world), ("Pyramid", pyramid2::init_world), ("Restitution", restitution2::init_world), ("Rope Joints", rope_joints2::init_world), diff --git a/examples2d/groove_joint2.rs b/examples2d/pin_slot_joint2.rs similarity index 86% rename from examples2d/groove_joint2.rs rename to examples2d/pin_slot_joint2.rs index 789411572..ba0e24987 100644 --- a/examples2d/groove_joint2.rs +++ b/examples2d/pin_slot_joint2.rs @@ -60,15 +60,15 @@ pub fn init_world(testbed: &mut Testbed) { impulse_joints.insert(cube_handle, ball_handle, fixed_joint, true); /* - * Groove joint between cube and ground. + * Pin slot joint between cube and ground. */ - let axis = UnitVector::new_normalize(vector![1.0, 1.0]); - let groove_joint = GrooveJointBuilder::new(axis) + let axis: nalgebra::Unit, nalgebra::Const<1>, nalgebra::ArrayStorage>> = UnitVector::new_normalize(vector![1.0, 1.0]); + let pin_slot_joint = PinSlotJointBuilder::new(axis) .local_anchor1(point![2.0, 2.0]) .local_anchor2(point![0.0, 0.4]) - .limits([-1.0, f32::INFINITY]) // Set the limits for the groove joint + .limits([-1.0, f32::INFINITY]) // Set the limits for the pin slot joint .build(); - impulse_joints.insert(character_handle, cube_handle, groove_joint, true); + impulse_joints.insert(character_handle, cube_handle, pin_slot_joint, true); /* * Set up the testbed. diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 23f5f2727..cdd67829a 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -65,8 +65,8 @@ bitflags::bitflags! { const LOCKED_REVOLUTE_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits(); /// The set of degrees of freedom locked by a prismatic joint. const LOCKED_PRISMATIC_AXES = Self::LIN_Y.bits() | Self::ANG_X.bits(); - /// The set of degrees of freedom locked by a groove joint. - const LOCKED_GROOVE_AXES = Self::LIN_Y.bits(); + /// The set of degrees of freedom locked by a pin slot joint. + const LOCKED_PIN_SLOT_AXES = Self::LIN_Y.bits(); /// The set of degrees of freedom locked by a fixed joint. const LOCKED_FIXED_AXES = Self::LIN_X.bits() | Self::LIN_Y.bits() | Self::ANG_X.bits(); /// The set of degrees of freedom left free by a revolute joint. diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index af398120a..9e7c5f98a 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -1,6 +1,6 @@ pub use self::fixed_joint::*; pub use self::generic_joint::*; -pub use self::groove_joint::*; +pub use self::pin_slot_joint::*; pub use self::impulse_joint::*; pub use self::motor_model::MotorModel; pub use self::multibody_joint::*; @@ -14,7 +14,7 @@ pub use self::spherical_joint::*; mod fixed_joint; mod generic_joint; -mod groove_joint; +mod pin_slot_joint; mod impulse_joint; mod motor_model; mod multibody_joint; diff --git a/src/dynamics/joint/groove_joint.rs b/src/dynamics/joint/pin_slot_joint.rs similarity index 89% rename from src/dynamics/joint/groove_joint.rs rename to src/dynamics/joint/pin_slot_joint.rs index 0b7c60477..de06054ba 100644 --- a/src/dynamics/joint/groove_joint.rs +++ b/src/dynamics/joint/pin_slot_joint.rs @@ -10,19 +10,19 @@ use super::{JointLimits, JointMotor}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] #[repr(transparent)] -/// A groove joint, locks all relative motion between two bodies except for translation along the joint’s principal axis and relative rotations. -pub struct GrooveJoint { +/// A pin slot joint, locks all relative motion between two bodies except for translation along the joint’s principal axis and relative rotations. +pub struct PinSlotJoint { /// The underlying joint data. pub data: GenericJoint, } -impl GrooveJoint { - /// Creates a new groove joint allowing only relative translations along the specified axis and relative rotations. +impl PinSlotJoint { + /// Creates a new pin slot joint allowing only relative translations along the specified axis and relative rotations. /// /// This axis is expressed in the local-space of both rigid-bodies. #[cfg(feature = "dim2")] pub fn new(axis: UnitVector) -> Self { - let data = GenericJointBuilder::new(JointAxesMask::LOCKED_GROOVE_AXES) + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PIN_SLOT_AXES) .local_axis1(axis) .local_axis2(axis) .build(); @@ -156,26 +156,26 @@ impl GrooveJoint { } } -impl From for GenericJoint { - fn from(val: GrooveJoint) -> GenericJoint { +impl From for GenericJoint { + fn from(val: PinSlotJoint) -> GenericJoint { val.data } } -/// Create groove joints using the builder pattern. +/// Create pin slot joints using the builder pattern. /// -/// A groove joint locks all relative motion except for translations along the joint’s principal axis and relative rotations. +/// A pin slot joint locks all relative motion except for translations along the joint’s principal axis and relative rotations. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] -pub struct GrooveJointBuilder(pub GrooveJoint); +pub struct PinSlotJointBuilder(pub PinSlotJoint); -impl GrooveJointBuilder { - /// Creates a new builder for groove joints. +impl PinSlotJointBuilder { + /// Creates a new builder for pin slot joints. /// /// This axis is expressed in the local-space of both rigid-bodies. #[cfg(feature = "dim2")] pub fn new(axis: UnitVector) -> Self { - Self(GrooveJoint::new(axis)) + Self(PinSlotJoint::new(axis)) } /// Sets whether contacts between the attached rigid-bodies are enabled. @@ -261,15 +261,15 @@ impl GrooveJointBuilder { self } - /// Builds the groove joint. + /// Builds the pin slot joint. #[must_use] - pub fn build(self) -> GrooveJoint { + pub fn build(self) -> PinSlotJoint { self.0 } } -impl From for GenericJoint { - fn from(val: GrooveJointBuilder) -> GenericJoint { +impl From for GenericJoint { + fn from(val: PinSlotJointBuilder) -> GenericJoint { val.0.into() } } From 74421f6ecbe3f7a6a261b1ad6ce99f1797a50a95 Mon Sep 17 00:00:00 2001 From: st170001 Date: Thu, 23 Jan 2025 11:16:05 +0100 Subject: [PATCH 05/14] cargo fmt --- examples2d/all_examples2.rs | 2 +- examples2d/pin_slot_joint2.rs | 9 ++++++++- src/dynamics/joint/mod.rs | 4 ++-- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index bbfbe8c1b..87b178348 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -20,13 +20,13 @@ mod debug_intersection2; mod debug_total_overlap2; mod debug_vertical_column2; mod drum2; -mod pin_slot_joint2; mod heightfield2; mod inverse_kinematics2; mod joint_motor_position2; mod joints2; mod locked_rotations2; mod one_way_platforms2; +mod pin_slot_joint2; mod platform2; mod polyline2; mod pyramid2; diff --git a/examples2d/pin_slot_joint2.rs b/examples2d/pin_slot_joint2.rs index ba0e24987..1c8100b2e 100644 --- a/examples2d/pin_slot_joint2.rs +++ b/examples2d/pin_slot_joint2.rs @@ -62,7 +62,14 @@ pub fn init_world(testbed: &mut Testbed) { /* * Pin slot joint between cube and ground. */ - let axis: nalgebra::Unit, nalgebra::Const<1>, nalgebra::ArrayStorage>> = UnitVector::new_normalize(vector![1.0, 1.0]); + let axis: nalgebra::Unit< + nalgebra::Matrix< + f32, + nalgebra::Const<2>, + nalgebra::Const<1>, + nalgebra::ArrayStorage, + >, + > = UnitVector::new_normalize(vector![1.0, 1.0]); let pin_slot_joint = PinSlotJointBuilder::new(axis) .local_anchor1(point![2.0, 2.0]) .local_anchor2(point![0.0, 0.4]) diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 9e7c5f98a..800250536 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -1,9 +1,9 @@ pub use self::fixed_joint::*; pub use self::generic_joint::*; -pub use self::pin_slot_joint::*; pub use self::impulse_joint::*; pub use self::motor_model::MotorModel; pub use self::multibody_joint::*; +pub use self::pin_slot_joint::*; pub use self::prismatic_joint::*; pub use self::revolute_joint::*; pub use self::rope_joint::*; @@ -14,10 +14,10 @@ pub use self::spherical_joint::*; mod fixed_joint; mod generic_joint; -mod pin_slot_joint; mod impulse_joint; mod motor_model; mod multibody_joint; +mod pin_slot_joint; mod prismatic_joint; mod revolute_joint; mod rope_joint; From 56505c8b1019c3c365e124cb4119332f204ad589 Mon Sep 17 00:00:00 2001 From: st170001 Date: Thu, 23 Jan 2025 11:48:19 +0100 Subject: [PATCH 06/14] Cross-reference to groove joint in Godot engine --- src/dynamics/joint/pin_slot_joint.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/dynamics/joint/pin_slot_joint.rs b/src/dynamics/joint/pin_slot_joint.rs index de06054ba..3980a7a2f 100644 --- a/src/dynamics/joint/pin_slot_joint.rs +++ b/src/dynamics/joint/pin_slot_joint.rs @@ -11,6 +11,7 @@ use super::{JointLimits, JointMotor}; #[derive(Copy, Clone, Debug, PartialEq)] #[repr(transparent)] /// A pin slot joint, locks all relative motion between two bodies except for translation along the joint’s principal axis and relative rotations. +/// This joint is also known as a [groove joint in Godot](https://docs.godotengine.org/en/stable/classes/class_groovejoint2d.html). pub struct PinSlotJoint { /// The underlying joint data. pub data: GenericJoint, From 7b724a51f52e9a7b05aecba0f7dd694362cbf7fc Mon Sep 17 00:00:00 2001 From: Dragos Daian Date: Sun, 9 Nov 2025 11:18:08 +0100 Subject: [PATCH 07/14] Add softness to joints fix simd error --- examples2d/joints2.rs | 4 +- examples2d/pin_slot_joint2.rs | 1 - src/dynamics/integration_parameters.rs | 73 +++++------ src/dynamics/joint/fixed_joint.rs | 20 ++- src/dynamics/joint/generic_joint.rs | 115 ++++++++++++++++++ .../joint/multibody_joint/multibody_joint.rs | 4 + .../multibody_joint/unit_multibody_joint.rs | 9 +- src/dynamics/joint/pin_slot_joint.rs | 20 ++- src/dynamics/joint/prismatic_joint.rs | 20 ++- src/dynamics/joint/revolute_joint.rs | 20 ++- src/dynamics/joint/rope_joint.rs | 18 +++ src/dynamics/joint/spherical_joint.rs | 18 +++ src/dynamics/joint/spring_joint.rs | 18 +++ .../generic_joint_constraint.rs | 8 ++ .../generic_joint_constraint_builder.rs | 26 +++- .../joint_constraint_builder.rs | 44 ++++--- .../joint_velocity_constraint.rs | 35 +++++- src_testbed/ui.rs | 13 +- 18 files changed, 377 insertions(+), 89 deletions(-) diff --git a/examples2d/joints2.rs b/examples2d/joints2.rs index 4c4cf3503..d5acd9238 100644 --- a/examples2d/joints2.rs +++ b/examples2d/joints2.rs @@ -44,7 +44,7 @@ pub fn init_world(testbed: &mut Testbed) { // 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]).natural_frequency(5.0 * (i+1) as f32).damping_ratio(0.1 * (i+1) as f32); impulse_joints.insert(parent_handle, child_handle, joint, true); } @@ -52,7 +52,7 @@ 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]).natural_frequency(5.0 * (i+1) as f32).damping_ratio(0.1 * (i+1) as f32); 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..ed0be0d8a 100644 --- a/examples2d/pin_slot_joint2.rs +++ b/examples2d/pin_slot_joint2.rs @@ -81,6 +81,5 @@ pub fn init_world(testbed: &mut Testbed) { * 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..986575a24 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -96,19 +96,6 @@ pub struct IntegrationParameters { /// (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, - /// 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. /// @@ -212,26 +199,7 @@ impl IntegrationParameters { 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. /// @@ -281,22 +249,39 @@ impl IntegrationParameters { /// /// 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(); + + + /// The joint's spring angular frequency for constraint regularization, using per-joint parameter. + pub fn joint_angular_frequency_with_override(&self, natural_frequency: Real) -> Real { + natural_frequency * Real::two_pi() + } + + /// The joint ERP coefficient (multiplied by inverse timestep), using per-joint parameters. + pub fn joint_erp_inv_dt_with_override( + &self, + natural_frequency: Real, + damping_ratio: Real, + ) -> Real { + let ang_freq = self.joint_angular_frequency_with_override(natural_frequency); + ang_freq / (self.dt * ang_freq + 2.0 * damping_ratio) + } + + /// The joint CFM coefficient for constraint regularization, using per-joint parameters. + pub fn joint_cfm_coeff_with_override( + &self, + natural_frequency: Real, + damping_ratio: Real, + ) -> Real { + let erp_inv_dt = self.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); + let joint_erp = self.dt * erp_inv_dt; 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) + inv_erp_minus_one * inv_erp_minus_one / ((1.0 + inv_erp_minus_one) * 4.0 * damping_ratio * 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 @@ -328,8 +313,6 @@ impl Default for IntegrationParameters { 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, 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..99c39f9ed 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -138,13 +138,31 @@ 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 natural frequency (Hz) for this joint's constraint regularization. + /// + /// Higher values make the joint stiffer and resolve constraint violations more quickly. + #[must_use] + pub fn natural_frequency(mut self, frequency: Real) -> Self { + self.0.data.set_natural_frequency(frequency); + self + } + + /// Sets the damping ratio for this joint's constraint regularization. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + #[must_use] + pub fn damping_ratio(mut self, ratio: Real) -> Self { + self.0.data.set_damping_ratio(ratio); + 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..60171b159 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -282,6 +282,16 @@ 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 natural frequency (Hz) used for the joint constraint spring-like regularization. + /// + /// Higher values make the joint stiffer and resolve constraint violations more quickly. + /// (default: `1.0e6`). + pub natural_frequency: Real, + /// The damping ratio used for the joint constraint spring-like regularization. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + /// (default: `1.0`). + pub damping_ratio: Real, /// Are contacts between the attached rigid-bodies enabled? pub contacts_enabled: bool, /// Whether the joint is enabled. @@ -301,6 +311,8 @@ impl Default for GenericJoint { coupled_axes: JointAxesMask::empty(), limits: [JointLimits::default(); SPATIAL_DIM], motors: [JointMotor::default(); SPATIAL_DIM], + natural_frequency: 1.0e6, + damping_ratio: 1.0, contacts_enabled: true, enabled: JointEnabled::Enabled, user_data: 0, @@ -440,6 +452,75 @@ impl GenericJoint { self } + /// The natural frequency (Hz) for this joint's constraint regularization. + #[must_use] + pub fn natural_frequency(&self) -> Real { + self.natural_frequency + } + + /// Sets the natural frequency (Hz) for this joint's constraint regularization. + /// + /// Higher values make the joint stiffer and resolve constraint violations more quickly. + /// + /// # Example + /// ``` + /// # use rapier3d::prelude::*; + /// let mut joint = RevoluteJoint::new(Vector::y_axis()); + /// joint.set_natural_frequency(5.0e5); // Softer than default (1.0e6) + /// ``` + pub fn set_natural_frequency(&mut self, frequency: Real) -> &mut Self { + self.natural_frequency = frequency; + self + } + + /// The damping ratio for this joint's constraint regularization. + #[must_use] + pub fn damping_ratio(&self) -> Real { + self.damping_ratio + } + + /// Sets the damping ratio for this joint's constraint regularization. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + /// + /// # Example + /// ``` + /// # use rapier3d::prelude::*; + /// let mut joint = RevoluteJoint::new(Vector::y_axis()); + /// joint.set_damping_ratio(2.0); // More compliant than default (1.0) + /// ``` + pub fn set_damping_ratio(&mut self, ratio: Real) -> &mut Self { + self.damping_ratio = ratio; + self + } + + /// The joint's spring angular frequency for constraint regularization. + pub fn joint_angular_frequency(&self) -> Real { + self.natural_frequency * std::f64::consts::TAU as Real + } + + /// The joint ERP coefficient (multiplied by inverse timestep). + pub fn joint_erp_inv_dt(&self, dt: Real) -> Real { + let ang_freq = self.joint_angular_frequency(); + ang_freq / (dt * ang_freq + 2.0 * self.damping_ratio) + } + + /// The effective Error Reduction Parameter for calculating regularization forces. + pub fn joint_erp(&self, dt: Real) -> Real { + dt * self.joint_erp_inv_dt(dt) + } + + /// The CFM coefficient for constraint regularization. + pub fn joint_cfm_coeff(&self, dt: Real) -> Real { + let joint_erp = self.joint_erp(dt); + 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.damping_ratio * self.damping_ratio) + } + /// The joint limits along the specified axis. #[must_use] pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits> { @@ -767,6 +848,40 @@ impl GenericJointBuilder { self } + /// Sets the natural frequency (Hz) for this joint's constraint regularization. + /// + /// Higher values make the joint stiffer and resolve constraint violations more quickly. + /// + /// # Example + /// ``` + /// # use rapier3d::prelude::*; + /// let joint = RevoluteJointBuilder::new(Vector::y_axis()) + /// .natural_frequency(5.0e5) // Softer than default + /// .build(); + /// ``` + #[must_use] + pub fn natural_frequency(mut self, frequency: Real) -> Self { + self.0.set_natural_frequency(frequency); + self + } + + /// Sets the damping ratio for this joint's constraint regularization. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + /// + /// # Example + /// ``` + /// # use rapier3d::prelude::*; + /// let joint = RevoluteJointBuilder::new(Vector::y_axis()) + /// .damping_ratio(2.0) // More compliant than default + /// .build(); + /// ``` + #[must_use] + pub fn damping_ratio(mut self, ratio: Real) -> Self { + self.0.set_damping_ratio(ratio); + 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..c77f50570 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -314,6 +314,8 @@ impl MultibodyJoint { jacobians, constraints, &mut num_constraints, + self.data.natural_frequency, + self.data.damping_ratio, ); } curr_free_dof += 1; @@ -349,6 +351,8 @@ impl MultibodyJoint { jacobians, constraints, &mut num_constraints, + self.data.natural_frequency, + self.data.damping_ratio, ); 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..8e91bf7cb 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -19,12 +19,17 @@ pub fn unit_joint_limit_constraint( jacobians: &mut DVector, constraints: &mut [GenericJointConstraint], insert_at: &mut usize, + natural_frequency: Real, + damping_ratio: Real, ) { 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 = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); + let cfm_coeff = params.joint_cfm_coeff_with_override(natural_frequency, damping_ratio); + 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..abeee4d37 100644 --- a/src/dynamics/joint/pin_slot_joint.rs +++ b/src/dynamics/joint/pin_slot_joint.rs @@ -255,13 +255,31 @@ 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 natural frequency (Hz) for this joint's constraint regularization. + /// + /// Higher values make the joint stiffer and resolve constraint violations more quickly. + #[must_use] + pub fn natural_frequency(mut self, frequency: Real) -> Self { + self.0.data.set_natural_frequency(frequency); + self + } + + /// Sets the damping ratio for this joint's constraint regularization. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + #[must_use] + pub fn damping_ratio(mut self, ratio: Real) -> Self { + self.0.data.set_damping_ratio(ratio); + 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..cb308fced 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -275,13 +275,31 @@ 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 natural frequency (Hz) for this joint's constraint regularization. + /// + /// Higher values make the joint stiffer and resolve constraint violations more quickly. + #[must_use] + pub fn natural_frequency(mut self, frequency: Real) -> Self { + self.0.data.set_natural_frequency(frequency); + self + } + + /// Sets the damping ratio for this joint's constraint regularization. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + #[must_use] + pub fn damping_ratio(mut self, ratio: Real) -> Self { + self.0.data.set_damping_ratio(ratio); + 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..29a8078db 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -296,13 +296,31 @@ 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 natural frequency (Hz) for this joint's constraint regularization. + /// + /// Higher values make the joint stiffer and resolve constraint violations more quickly. + #[must_use] + pub fn natural_frequency(mut self, frequency: Real) -> Self { + self.0.data.set_natural_frequency(frequency); + self + } + + /// Sets the damping ratio for this joint's constraint regularization. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + #[must_use] + pub fn damping_ratio(mut self, ratio: Real) -> Self { + self.0.data.set_damping_ratio(ratio); + 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..c3f9d853a 100644 --- a/src/dynamics/joint/rope_joint.rs +++ b/src/dynamics/joint/rope_joint.rs @@ -245,6 +245,24 @@ impl RopeJointBuilder { self } + /// Sets the natural frequency (Hz) for this joint's constraint regularization. + /// + /// Higher values make the joint stiffer and resolve constraint violations more quickly. + #[must_use] + pub fn natural_frequency(mut self, frequency: Real) -> Self { + self.0.data.set_natural_frequency(frequency); + self + } + + /// Sets the damping ratio for this joint's constraint regularization. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + #[must_use] + pub fn damping_ratio(mut self, ratio: Real) -> Self { + self.0.data.set_damping_ratio(ratio); + 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..580f3581a 100644 --- a/src/dynamics/joint/spherical_joint.rs +++ b/src/dynamics/joint/spherical_joint.rs @@ -305,6 +305,24 @@ impl SphericalJointBuilder { self } + /// Sets the natural frequency (Hz) for this joint's constraint regularization. + /// + /// Higher values make the joint stiffer and resolve constraint violations more quickly. + #[must_use] + pub fn natural_frequency(mut self, frequency: Real) -> Self { + self.0.data.set_natural_frequency(frequency); + self + } + + /// Sets the damping ratio for this joint's constraint regularization. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + #[must_use] + pub fn damping_ratio(mut self, ratio: Real) -> Self { + self.0.data.set_damping_ratio(ratio); + self + } + /// Builds the spherical joint. #[must_use] pub fn build(self) -> SphericalJoint { diff --git a/src/dynamics/joint/spring_joint.rs b/src/dynamics/joint/spring_joint.rs index 99b3c738a..ba9384faf 100644 --- a/src/dynamics/joint/spring_joint.rs +++ b/src/dynamics/joint/spring_joint.rs @@ -170,6 +170,24 @@ impl SpringJointBuilder { // self // } + /// Sets the natural frequency (Hz) for this joint's constraint regularization. + /// + /// Higher values make the joint stiffer and resolve constraint violations more quickly. + #[must_use] + pub fn natural_frequency(mut self, frequency: Real) -> Self { + self.0.data.set_natural_frequency(frequency); + self + } + + /// Sets the damping ratio for this joint's constraint regularization. + /// + /// Larger values make the joint more compliant (allowing more drift before stabilization). + #[must_use] + pub fn damping_ratio(mut self, ratio: Real) -> Self { + self.0.data.set_damping_ratio(ratio); + self + } + /// Builds the spring joint. #[must_use] pub fn build(self) -> SpringJoint { diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs index ee48b273f..3a79fcd49 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs @@ -141,6 +141,8 @@ impl GenericJointConstraint { mb2, i - DIM, WritebackId::Dof(i), + joint.natural_frequency, + joint.damping_ratio, ); len += 1; } @@ -158,6 +160,8 @@ impl GenericJointConstraint { mb2, i, WritebackId::Dof(i), + joint.natural_frequency, + joint.damping_ratio, ); len += 1; } @@ -177,6 +181,8 @@ impl GenericJointConstraint { i - DIM, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + joint.natural_frequency, + joint.damping_ratio, ); len += 1; } @@ -195,6 +201,8 @@ impl GenericJointConstraint { i, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + joint.natural_frequency, + joint.damping_ratio, ); 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..1c7eb6090 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -407,6 +407,8 @@ impl JointConstraintHelper { mb2: LinkOrBodyRef, locked_axis: usize, writeback_id: WritebackId, + natural_frequency: Real, + damping_ratio: Real, ) -> GenericJointConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); @@ -426,7 +428,11 @@ impl JointConstraintHelper { ang_jac2, ); - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); + eprintln!( + "[joint_constraint] lock_linear_generic: joint_id={} natural_frequency={:?} damping_ratio={:?} erp_inv_dt={:?}", + joint_id, natural_frequency, damping_ratio, erp_inv_dt + ); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; c.rhs += rhs_bias; c @@ -445,6 +451,8 @@ impl JointConstraintHelper { limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, + natural_frequency: Real, + damping_ratio: Real, ) -> GenericJointConstraint { let lin_jac = self.basis.column(limited_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned(); @@ -468,7 +476,13 @@ 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 = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); + // Debug: print per-joint compliance overrides and computed ERP for limit constraints. + eprintln!( + "[joint_constraint] limit_linear_generic: joint_id={} natural_frequency={:?} damping_ratio={:?} erp_inv_dt={:?}", + joint_id, natural_frequency, damping_ratio, erp_inv_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 = [ @@ -546,6 +560,8 @@ impl JointConstraintHelper { mb2: LinkOrBodyRef, _locked_axis: usize, writeback_id: WritebackId, + natural_frequency: Real, + damping_ratio: Real, ) -> GenericJointConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); @@ -566,7 +582,7 @@ impl JointConstraintHelper { ang_jac, ); - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] @@ -588,6 +604,8 @@ impl JointConstraintHelper { _limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, + natural_frequency: Real, + damping_ratio: Real, ) -> GenericJointConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); @@ -620,7 +638,7 @@ impl JointConstraintHelper { max_enabled as u32 as Real * Real::MAX, ]; - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); 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..5680bf9a3 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -103,6 +103,8 @@ pub struct JointConstraintBuilderSimd { local_frame1: Isometry, local_frame2: Isometry, locked_axes: u8, + natural_frequency: SimdReal, + damping_ratio: SimdReal, constraint_id: usize, } @@ -149,6 +151,8 @@ impl JointConstraintBuilderSimd { local_frame1, local_frame2, locked_axes: joint[0].data.locked_axes.bits(), + natural_frequency: array![|ii| joint[ii].data.natural_frequency].into(), + damping_ratio: array![|ii| joint[ii].data.damping_ratio].into(), constraint_id: *out_constraint_id, }; @@ -191,6 +195,8 @@ impl JointConstraintBuilderSimd { &frame1, &frame2, self.locked_axes, + self.natural_frequency, + self.damping_ratio, &mut out[self.constraint_id..], ); } @@ -277,17 +283,17 @@ 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); + 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 +315,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 +353,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 +391,10 @@ impl JointConstraintHelper { writeback_id: WritebackId, ) -> JointConstraint { let inv_dt = N::splat(params.inv_dt()); + // Motors don't use joint erp/cfm; they use motor-specific parameters. + // Pass dummy values since lock_linear will override them anyway. let mut constraint = - self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id); + self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id, N::zero(), N::zero()); let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { @@ -491,12 +499,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 +519,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 +548,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 +578,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 +671,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 +686,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 +768,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 +801,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..48f3d4ca6 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -9,6 +9,8 @@ use crate::utils::{SimdDot, SimdRealCopy}; use crate::dynamics::solver::solver_body::SolverBodies; #[cfg(feature = "simd-is-enabled")] use crate::math::{SIMD_WIDTH, SimdReal}; +#[cfg(feature = "simd-is-enabled")] +use na::SimdValue; #[cfg(feature = "dim2")] use crate::num::Zero; @@ -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.joint_erp_inv_dt(params.dt); + let cfm_coeff = joint.joint_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,6 +242,8 @@ impl JointConstraint { body2, i - DIM, WritebackId::Dof(i), + erp_inv_dt, + cfm_coeff, ); len += 1; } @@ -243,7 +251,7 @@ impl JointConstraint { 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)); + builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i), erp_inv_dt, cfm_coeff); len += 1; } } @@ -258,6 +266,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 +282,8 @@ impl JointConstraint { i, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + erp_inv_dt, + cfm_coeff, ); len += 1; } @@ -290,6 +302,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 +320,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 +360,21 @@ impl JointConstraint { frame1: &Isometry, frame2: &Isometry, locked_axes: u8, + natural_frequency: SimdReal, + damping_ratio: SimdReal, out: &mut [Self], ) -> usize { + // Compute per-lane ERP and CFM coefficients using the `_with_override` methods + // Need to compute per-lane, so use the formula directly: + let ang_freq = natural_frequency * SimdReal::splat(std::f64::consts::TAU as Real); + let dt = SimdReal::splat(params.dt); + let erp_inv_dt = ang_freq / (dt * ang_freq + SimdReal::splat(2.0) * damping_ratio); + + let joint_erp = dt * erp_inv_dt; + let inv_erp_minus_one = SimdReal::splat(1.0) / joint_erp - SimdReal::splat(1.0); + let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one + / ((SimdReal::splat(1.0) + inv_erp_minus_one) * SimdReal::splat(4.0) * damping_ratio * damping_ratio); + let builder = JointConstraintHelper::new( frame1, frame2, @@ -358,7 +387,7 @@ impl JointConstraint { 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)); + builder.lock_linear(params, joint_id, body1, body2, i, WritebackId::Dof(i), erp_inv_dt, cfm_coeff); len += 1; } } @@ -372,6 +401,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..1b1340640 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -205,17 +205,8 @@ pub(crate) fn update_ui( ) .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"), - ); + // Note: joint_natural_frequency and joint_damping_ratio are now per-joint properties. + // Set them via joint.set_natural_frequency() and joint.set_damping_ratio(). } #[cfg(feature = "parallel")] From 5554ba4b042fbcd5f9d96944d89937913281d16f Mon Sep 17 00:00:00 2001 From: Dragos Daian Date: Fri, 14 Nov 2025 11:12:02 +0100 Subject: [PATCH 08/14] remove unused logs/comments --- .../joint_constraint/generic_joint_constraint_builder.rs | 9 --------- src_testbed/ui.rs | 2 -- 2 files changed, 11 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 1c7eb6090..67ccbb36f 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -429,10 +429,6 @@ impl JointConstraintHelper { ); let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); - eprintln!( - "[joint_constraint] lock_linear_generic: joint_id={} natural_frequency={:?} damping_ratio={:?} erp_inv_dt={:?}", - joint_id, natural_frequency, damping_ratio, erp_inv_dt - ); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; c.rhs += rhs_bias; c @@ -477,11 +473,6 @@ impl JointConstraintHelper { let max_enabled = limits[1] <= dist; let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); - // Debug: print per-joint compliance overrides and computed ERP for limit constraints. - eprintln!( - "[joint_constraint] limit_linear_generic: joint_id={} natural_frequency={:?} damping_ratio={:?} erp_inv_dt={:?}", - joint_id, natural_frequency, damping_ratio, erp_inv_dt - ); let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt; constraint.rhs += rhs_bias; diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 1b1340640..facf08a42 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -205,8 +205,6 @@ pub(crate) fn update_ui( ) .text(format!("damping ratio (cfm-factor = {curr_cfm_factor:.3})",)), ); - // Note: joint_natural_frequency and joint_damping_ratio are now per-joint properties. - // Set them via joint.set_natural_frequency() and joint.set_damping_ratio(). } #[cfg(feature = "parallel")] From 99d2dd6b7ce63bca6d713c21c582f045f392d0ce Mon Sep 17 00:00:00 2001 From: Dragos Daian Date: Fri, 14 Nov 2025 11:14:37 +0100 Subject: [PATCH 09/14] format after change --- examples2d/joints2.rs | 10 ++++-- examples2d/pin_slot_joint2.rs | 2 +- src/dynamics/integration_parameters.rs | 5 ++- .../multibody_joint/unit_multibody_joint.rs | 4 +-- .../joint_constraint_builder.rs | 24 ++++++++++--- .../joint_velocity_constraint.rs | 35 ++++++++++++++----- 6 files changed, 60 insertions(+), 20 deletions(-) diff --git a/examples2d/joints2.rs b/examples2d/joints2.rs index d5acd9238..ed132d2a5 100644 --- a/examples2d/joints2.rs +++ b/examples2d/joints2.rs @@ -44,7 +44,10 @@ pub fn init_world(testbed: &mut Testbed) { // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]).natural_frequency(5.0 * (i+1) as f32).damping_ratio(0.1 * (i+1) as f32); + let joint = RevoluteJointBuilder::new() + .local_anchor2(point![0.0, shift]) + .natural_frequency(5.0 * (i + 1) as f32) + .damping_ratio(0.1 * (i + 1) as f32); impulse_joints.insert(parent_handle, child_handle, joint, true); } @@ -52,7 +55,10 @@ 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]).natural_frequency(5.0 * (i+1) as f32).damping_ratio(0.1 * (i+1) as f32); + let joint = RevoluteJointBuilder::new() + .local_anchor2(point![-shift, 0.0]) + .natural_frequency(5.0 * (i + 1) as f32) + .damping_ratio(0.1 * (i + 1) as f32); 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..c650fdc43 100644 --- a/examples2d/pin_slot_joint2.rs +++ b/examples2d/pin_slot_joint2.rs @@ -1,5 +1,5 @@ -use rapier2d::prelude::*; use rapier_testbed2d::Testbed; +use rapier2d::prelude::*; pub fn init_world(testbed: &mut Testbed) { /* diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 986575a24..0d59b0eba 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -199,7 +199,6 @@ impl IntegrationParameters { pub fn contact_erp(&self) -> Real { self.dt * self.contact_erp_inv_dt() } - /// The CFM factor to be used in the constraint resolution. /// @@ -249,7 +248,6 @@ impl IntegrationParameters { /// /// This parameter is computed automatically from [`Self::joint_natural_frequency`], /// [`Self::joint_damping_ratio`] and the substep length. - /// The joint's spring angular frequency for constraint regularization, using per-joint parameter. pub fn joint_angular_frequency_with_override(&self, natural_frequency: Real) -> Real { @@ -278,7 +276,8 @@ impl IntegrationParameters { 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 * damping_ratio * damping_ratio) + inv_erp_minus_one * inv_erp_minus_one + / ((1.0 + inv_erp_minus_one) * 4.0 * damping_ratio * damping_ratio) } /// Amount of penetration the engine won't attempt to correct (default: `0.001` multiplied by diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index 8e91bf7cb..d5b62212f 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -25,11 +25,11 @@ pub fn unit_joint_limit_constraint( let ndofs = multibody.ndofs(); let min_enabled = curr_pos < limits[0]; let max_enabled = limits[1] < curr_pos; - + // Compute per-joint ERP and CFM let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); let cfm_coeff = params.joint_cfm_coeff_with_override(natural_frequency, damping_ratio); - + 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/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 5680bf9a3..0b49fb57b 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -287,8 +287,16 @@ impl JointConstraintHelper { cfm_coeff: N, ) -> JointConstraint { let zero = N::zero(); - let mut constraint = - self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id, erp_inv_dt, cfm_coeff); + 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]); @@ -393,8 +401,16 @@ impl JointConstraintHelper { let inv_dt = N::splat(params.inv_dt()); // Motors don't use joint erp/cfm; they use motor-specific parameters. // Pass dummy values since lock_linear will override them anyway. - let mut constraint = - self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id, N::zero(), N::zero()); + let mut constraint = self.lock_linear( + params, + joint_id, + body1, + body2, + motor_axis, + writeback_id, + N::zero(), + N::zero(), + ); let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index 48f3d4ca6..bf7399daf 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -9,10 +9,10 @@ use crate::utils::{SimdDot, SimdRealCopy}; use crate::dynamics::solver::solver_body::SolverBodies; #[cfg(feature = "simd-is-enabled")] use crate::math::{SIMD_WIDTH, SimdReal}; -#[cfg(feature = "simd-is-enabled")] -use na::SimdValue; #[cfg(feature = "dim2")] use crate::num::Zero; +#[cfg(feature = "simd-is-enabled")] +use na::SimdValue; #[derive(Copy, Clone, PartialEq, Debug)] pub struct MotorParameters { @@ -250,8 +250,16 @@ impl JointConstraint { } 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), erp_inv_dt, cfm_coeff); + out[len] = builder.lock_linear( + params, + [joint_id], + body1, + body2, + i, + WritebackId::Dof(i), + erp_inv_dt, + cfm_coeff, + ); len += 1; } } @@ -369,11 +377,14 @@ impl JointConstraint { let ang_freq = natural_frequency * SimdReal::splat(std::f64::consts::TAU as Real); let dt = SimdReal::splat(params.dt); let erp_inv_dt = ang_freq / (dt * ang_freq + SimdReal::splat(2.0) * damping_ratio); - + let joint_erp = dt * erp_inv_dt; let inv_erp_minus_one = SimdReal::splat(1.0) / joint_erp - SimdReal::splat(1.0); let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one - / ((SimdReal::splat(1.0) + inv_erp_minus_one) * SimdReal::splat(4.0) * damping_ratio * damping_ratio); + / ((SimdReal::splat(1.0) + inv_erp_minus_one) + * SimdReal::splat(4.0) + * damping_ratio + * damping_ratio); let builder = JointConstraintHelper::new( frame1, @@ -386,8 +397,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), erp_inv_dt, cfm_coeff); + out[len] = builder.lock_linear( + params, + joint_id, + body1, + body2, + i, + WritebackId::Dof(i), + erp_inv_dt, + cfm_coeff, + ); len += 1; } } From 5a1a2ac9ee261814add7ce99d895aa3a7d85f3d9 Mon Sep 17 00:00:00 2001 From: Dragos Daian Date: Fri, 14 Nov 2025 11:15:56 +0100 Subject: [PATCH 10/14] remove leftover comment --- src/dynamics/integration_parameters.rs | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 0d59b0eba..94aaab28f 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -244,11 +244,6 @@ impl IntegrationParameters { 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. - /// The joint's spring angular frequency for constraint regularization, using per-joint parameter. pub fn joint_angular_frequency_with_override(&self, natural_frequency: Real) -> Real { natural_frequency * Real::two_pi() From 3cf8955c84b29c87c2a27bca2353a2a687646882 Mon Sep 17 00:00:00 2001 From: Dragos Daian Date: Fri, 14 Nov 2025 11:20:08 +0100 Subject: [PATCH 11/14] clippy --- examples2d/pin_slot_joint2.rs | 1 - src/dynamics/joint/generic_joint.rs | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/examples2d/pin_slot_joint2.rs b/examples2d/pin_slot_joint2.rs index c650fdc43..589c5c30c 100644 --- a/examples2d/pin_slot_joint2.rs +++ b/examples2d/pin_slot_joint2.rs @@ -81,6 +81,5 @@ pub fn init_world(testbed: &mut Testbed) { * 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/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 60171b159..148675b7d 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,4 +1,5 @@ #![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::solver::MotorParameters; use crate::dynamics::{ From f2b5d7401c47037c0410c4f1fa1d6b52b1dc0eeb Mon Sep 17 00:00:00 2001 From: Dragos Daian Date: Fri, 14 Nov 2025 11:41:24 +0100 Subject: [PATCH 12/14] update changelog. Fix docs test --- CHANGELOG.md | 1 + src/dynamics/joint/generic_joint.rs | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e671d8d08..168d50096 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,7 @@ - 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. +- Removed `IntegrationParameters::joint_natural_frequency` and `IntegrationParameters::joint_damping_ratio` in favor of being in `GenericJoint` (#789). ## v0.30.1 (17 Oct. 2025) diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 148675b7d..d4bfc3f3a 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -467,7 +467,7 @@ impl GenericJoint { /// ``` /// # use rapier3d::prelude::*; /// let mut joint = RevoluteJoint::new(Vector::y_axis()); - /// joint.set_natural_frequency(5.0e5); // Softer than default (1.0e6) + /// joint.data.natural_frequency = 5.0e5; // Softer than default (1.0e6) /// ``` pub fn set_natural_frequency(&mut self, frequency: Real) -> &mut Self { self.natural_frequency = frequency; @@ -488,7 +488,7 @@ impl GenericJoint { /// ``` /// # use rapier3d::prelude::*; /// let mut joint = RevoluteJoint::new(Vector::y_axis()); - /// joint.set_damping_ratio(2.0); // More compliant than default (1.0) + /// joint.data.damping_ratio = 2.0; // More compliant than default (1.0) /// ``` pub fn set_damping_ratio(&mut self, ratio: Real) -> &mut Self { self.damping_ratio = ratio; From ff1f1ff0cefcaa73d1b60245450babf057ae9189 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 21 Nov 2025 15:53:37 +0100 Subject: [PATCH 13/14] feat: refactor softness coefficients behind a struct --- CHANGELOG.md | 7 +- examples2d/joints2.rs | 22 +- src/dynamics/integration_parameters.rs | 228 +++++++++--------- src/dynamics/joint/fixed_joint.rs | 31 +-- src/dynamics/joint/generic_joint.rs | 120 +-------- .../joint/multibody_joint/multibody_joint.rs | 6 +- .../multibody_joint/unit_multibody_joint.rs | 8 +- src/dynamics/joint/pin_slot_joint.rs | 31 +-- src/dynamics/joint/prismatic_joint.rs | 31 +-- src/dynamics/joint/revolute_joint.rs | 31 +-- src/dynamics/joint/rope_joint.rs | 31 +-- src/dynamics/joint/spherical_joint.rs | 31 +-- src/dynamics/joint/spring_joint.rs | 18 -- src/dynamics/mod.rs | 2 +- .../contact_with_coulomb_friction.rs | 4 +- .../contact_with_twist_friction.rs | 4 +- .../generic_contact_constraint.rs | 4 +- .../generic_joint_constraint.rs | 12 +- .../generic_joint_constraint_builder.rs | 21 +- .../joint_constraint_builder.rs | 17 +- .../joint_velocity_constraint.rs | 21 +- src_testbed/ui.rs | 10 +- 22 files changed, 299 insertions(+), 391 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 168d50096..a4ec7945a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,7 +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. -- Removed `IntegrationParameters::joint_natural_frequency` and `IntegrationParameters::joint_damping_ratio` in favor of being in `GenericJoint` (#789). +- 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 ed132d2a5..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,13 +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]) - .natural_frequency(5.0 * (i + 1) as f32) - .damping_ratio(0.1 * (i + 1) as f32); + .softness(softness); impulse_joints.insert(parent_handle, child_handle, joint, true); } @@ -57,8 +72,7 @@ pub fn init_world(testbed: &mut Testbed) { let parent_handle = body_handles[parent_index]; let joint = RevoluteJointBuilder::new() .local_anchor2(point![-shift, 0.0]) - .natural_frequency(5.0 * (i + 1) as f32) - .damping_ratio(0.1 * (i + 1) as f32); + .softness(softness); impulse_joints.insert(parent_handle, child_handle, joint, true); } diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 94aaab28f..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,21 +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, + /// 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. @@ -180,101 +274,6 @@ 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 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 joint's spring angular frequency for constraint regularization, using per-joint parameter. - pub fn joint_angular_frequency_with_override(&self, natural_frequency: Real) -> Real { - natural_frequency * Real::two_pi() - } - - /// The joint ERP coefficient (multiplied by inverse timestep), using per-joint parameters. - pub fn joint_erp_inv_dt_with_override( - &self, - natural_frequency: Real, - damping_ratio: Real, - ) -> Real { - let ang_freq = self.joint_angular_frequency_with_override(natural_frequency); - ang_freq / (self.dt * ang_freq + 2.0 * damping_ratio) - } - - /// The joint CFM coefficient for constraint regularization, using per-joint parameters. - pub fn joint_cfm_coeff_with_override( - &self, - natural_frequency: Real, - damping_ratio: Real, - ) -> Real { - let erp_inv_dt = self.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); - let joint_erp = self.dt * erp_inv_dt; - 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 * damping_ratio * damping_ratio) - } - /// 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 { @@ -305,8 +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, + 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 99c39f9ed..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 { @@ -145,21 +159,10 @@ impl FixedJointBuilder { self } - /// Sets the natural frequency (Hz) for this joint's constraint regularization. - /// - /// Higher values make the joint stiffer and resolve constraint violations more quickly. - #[must_use] - pub fn natural_frequency(mut self, frequency: Real) -> Self { - self.0.data.set_natural_frequency(frequency); - self - } - - /// Sets the damping ratio for this joint's constraint regularization. - /// - /// Larger values make the joint more compliant (allowing more drift before stabilization). + /// Sets the softness of this joint’s locked degrees of freedom. #[must_use] - pub fn damping_ratio(mut self, ratio: Real) -> Self { - self.0.data.set_damping_ratio(ratio); + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; self } diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index d4bfc3f3a..9edc65c3f 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,6 +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, @@ -283,16 +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 natural frequency (Hz) used for the joint constraint spring-like regularization. - /// - /// Higher values make the joint stiffer and resolve constraint violations more quickly. - /// (default: `1.0e6`). - pub natural_frequency: Real, - /// The damping ratio used for the joint constraint spring-like regularization. - /// - /// Larger values make the joint more compliant (allowing more drift before stabilization). - /// (default: `1.0`). - pub damping_ratio: Real, + /// 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. @@ -312,8 +305,10 @@ impl Default for GenericJoint { coupled_axes: JointAxesMask::empty(), limits: [JointLimits::default(); SPATIAL_DIM], motors: [JointMotor::default(); SPATIAL_DIM], - natural_frequency: 1.0e6, - damping_ratio: 1.0, + softness: SpringCoefficients { + natural_frequency: 1.0e6, + damping_ratio: 1.0, + }, contacts_enabled: true, enabled: JointEnabled::Enabled, user_data: 0, @@ -453,75 +448,13 @@ impl GenericJoint { self } - /// The natural frequency (Hz) for this joint's constraint regularization. - #[must_use] - pub fn natural_frequency(&self) -> Real { - self.natural_frequency - } - - /// Sets the natural frequency (Hz) for this joint's constraint regularization. - /// - /// Higher values make the joint stiffer and resolve constraint violations more quickly. - /// - /// # Example - /// ``` - /// # use rapier3d::prelude::*; - /// let mut joint = RevoluteJoint::new(Vector::y_axis()); - /// joint.data.natural_frequency = 5.0e5; // Softer than default (1.0e6) - /// ``` - pub fn set_natural_frequency(&mut self, frequency: Real) -> &mut Self { - self.natural_frequency = frequency; - self - } - - /// The damping ratio for this joint's constraint regularization. + /// Sets the spring coefficients controlling this joint constraint’s softness. #[must_use] - pub fn damping_ratio(&self) -> Real { - self.damping_ratio - } - - /// Sets the damping ratio for this joint's constraint regularization. - /// - /// Larger values make the joint more compliant (allowing more drift before stabilization). - /// - /// # Example - /// ``` - /// # use rapier3d::prelude::*; - /// let mut joint = RevoluteJoint::new(Vector::y_axis()); - /// joint.data.damping_ratio = 2.0; // More compliant than default (1.0) - /// ``` - pub fn set_damping_ratio(&mut self, ratio: Real) -> &mut Self { - self.damping_ratio = ratio; + pub fn set_softness(&mut self, softness: SpringCoefficients) -> &mut Self { + self.softness = softness; self } - /// The joint's spring angular frequency for constraint regularization. - pub fn joint_angular_frequency(&self) -> Real { - self.natural_frequency * std::f64::consts::TAU as Real - } - - /// The joint ERP coefficient (multiplied by inverse timestep). - pub fn joint_erp_inv_dt(&self, dt: Real) -> Real { - let ang_freq = self.joint_angular_frequency(); - ang_freq / (dt * ang_freq + 2.0 * self.damping_ratio) - } - - /// The effective Error Reduction Parameter for calculating regularization forces. - pub fn joint_erp(&self, dt: Real) -> Real { - dt * self.joint_erp_inv_dt(dt) - } - - /// The CFM coefficient for constraint regularization. - pub fn joint_cfm_coeff(&self, dt: Real) -> Real { - let joint_erp = self.joint_erp(dt); - 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.damping_ratio * self.damping_ratio) - } - /// The joint limits along the specified axis. #[must_use] pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits> { @@ -849,37 +782,10 @@ impl GenericJointBuilder { self } - /// Sets the natural frequency (Hz) for this joint's constraint regularization. - /// - /// Higher values make the joint stiffer and resolve constraint violations more quickly. - /// - /// # Example - /// ``` - /// # use rapier3d::prelude::*; - /// let joint = RevoluteJointBuilder::new(Vector::y_axis()) - /// .natural_frequency(5.0e5) // Softer than default - /// .build(); - /// ``` - #[must_use] - pub fn natural_frequency(mut self, frequency: Real) -> Self { - self.0.set_natural_frequency(frequency); - self - } - - /// Sets the damping ratio for this joint's constraint regularization. - /// - /// Larger values make the joint more compliant (allowing more drift before stabilization). - /// - /// # Example - /// ``` - /// # use rapier3d::prelude::*; - /// let joint = RevoluteJointBuilder::new(Vector::y_axis()) - /// .damping_ratio(2.0) // More compliant than default - /// .build(); - /// ``` + /// Sets the softness of this joint’s locked degrees of freedom. #[must_use] - pub fn damping_ratio(mut self, ratio: Real) -> Self { - self.0.set_damping_ratio(ratio); + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.softness = softness; self } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index c77f50570..a8d24c7e6 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -314,8 +314,7 @@ impl MultibodyJoint { jacobians, constraints, &mut num_constraints, - self.data.natural_frequency, - self.data.damping_ratio, + self.data.softness, ); } curr_free_dof += 1; @@ -351,8 +350,7 @@ impl MultibodyJoint { jacobians, constraints, &mut num_constraints, - self.data.natural_frequency, - self.data.damping_ratio, + 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 d5b62212f..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,16 +20,15 @@ pub fn unit_joint_limit_constraint( jacobians: &mut DVector, constraints: &mut [GenericJointConstraint], insert_at: &mut usize, - natural_frequency: Real, - damping_ratio: Real, + softness: SpringCoefficients, ) { let ndofs = multibody.ndofs(); let min_enabled = curr_pos < limits[0]; let max_enabled = limits[1] < curr_pos; // Compute per-joint ERP and CFM - let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); - let cfm_coeff = params.joint_cfm_coeff_with_override(natural_frequency, damping_ratio); + 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 abeee4d37..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 { @@ -262,21 +276,10 @@ impl PinSlotJointBuilder { self } - /// Sets the natural frequency (Hz) for this joint's constraint regularization. - /// - /// Higher values make the joint stiffer and resolve constraint violations more quickly. - #[must_use] - pub fn natural_frequency(mut self, frequency: Real) -> Self { - self.0.data.set_natural_frequency(frequency); - self - } - - /// Sets the damping ratio for this joint's constraint regularization. - /// - /// Larger values make the joint more compliant (allowing more drift before stabilization). + /// Sets the softness of this joint’s locked degrees of freedom. #[must_use] - pub fn damping_ratio(mut self, ratio: Real) -> Self { - self.0.data.set_damping_ratio(ratio); + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; self } diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index cb308fced..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 { @@ -282,21 +296,10 @@ impl PrismaticJointBuilder { self } - /// Sets the natural frequency (Hz) for this joint's constraint regularization. - /// - /// Higher values make the joint stiffer and resolve constraint violations more quickly. - #[must_use] - pub fn natural_frequency(mut self, frequency: Real) -> Self { - self.0.data.set_natural_frequency(frequency); - self - } - - /// Sets the damping ratio for this joint's constraint regularization. - /// - /// Larger values make the joint more compliant (allowing more drift before stabilization). + /// Sets the softness of this joint’s locked degrees of freedom. #[must_use] - pub fn damping_ratio(mut self, ratio: Real) -> Self { - self.0.data.set_damping_ratio(ratio); + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; self } diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index 29a8078db..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 { @@ -303,21 +317,10 @@ impl RevoluteJointBuilder { self } - /// Sets the natural frequency (Hz) for this joint's constraint regularization. - /// - /// Higher values make the joint stiffer and resolve constraint violations more quickly. - #[must_use] - pub fn natural_frequency(mut self, frequency: Real) -> Self { - self.0.data.set_natural_frequency(frequency); - self - } - - /// Sets the damping ratio for this joint's constraint regularization. - /// - /// Larger values make the joint more compliant (allowing more drift before stabilization). + /// Sets the softness of this joint’s locked degrees of freedom. #[must_use] - pub fn damping_ratio(mut self, ratio: Real) -> Self { - self.0.data.set_damping_ratio(ratio); + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; self } diff --git a/src/dynamics/joint/rope_joint.rs b/src/dynamics/joint/rope_joint.rs index c3f9d853a..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,21 +259,10 @@ impl RopeJointBuilder { self } - /// Sets the natural frequency (Hz) for this joint's constraint regularization. - /// - /// Higher values make the joint stiffer and resolve constraint violations more quickly. - #[must_use] - pub fn natural_frequency(mut self, frequency: Real) -> Self { - self.0.data.set_natural_frequency(frequency); - self - } - - /// Sets the damping ratio for this joint's constraint regularization. - /// - /// Larger values make the joint more compliant (allowing more drift before stabilization). + /// Sets the softness of this joint’s locked degrees of freedom. #[must_use] - pub fn damping_ratio(mut self, ratio: Real) -> Self { - self.0.data.set_damping_ratio(ratio); + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; self } diff --git a/src/dynamics/joint/spherical_joint.rs b/src/dynamics/joint/spherical_joint.rs index 580f3581a..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,21 +319,10 @@ impl SphericalJointBuilder { self } - /// Sets the natural frequency (Hz) for this joint's constraint regularization. - /// - /// Higher values make the joint stiffer and resolve constraint violations more quickly. - #[must_use] - pub fn natural_frequency(mut self, frequency: Real) -> Self { - self.0.data.set_natural_frequency(frequency); - self - } - - /// Sets the damping ratio for this joint's constraint regularization. - /// - /// Larger values make the joint more compliant (allowing more drift before stabilization). + /// Sets the softness of this joint’s locked degrees of freedom. #[must_use] - pub fn damping_ratio(mut self, ratio: Real) -> Self { - self.0.data.set_damping_ratio(ratio); + pub fn softness(mut self, softness: SpringCoefficients) -> Self { + self.0.data.softness = softness; self } diff --git a/src/dynamics/joint/spring_joint.rs b/src/dynamics/joint/spring_joint.rs index ba9384faf..99b3c738a 100644 --- a/src/dynamics/joint/spring_joint.rs +++ b/src/dynamics/joint/spring_joint.rs @@ -170,24 +170,6 @@ impl SpringJointBuilder { // self // } - /// Sets the natural frequency (Hz) for this joint's constraint regularization. - /// - /// Higher values make the joint stiffer and resolve constraint violations more quickly. - #[must_use] - pub fn natural_frequency(mut self, frequency: Real) -> Self { - self.0.data.set_natural_frequency(frequency); - self - } - - /// Sets the damping ratio for this joint's constraint regularization. - /// - /// Larger values make the joint more compliant (allowing more drift before stabilization). - #[must_use] - pub fn damping_ratio(mut self, ratio: Real) -> Self { - self.0.data.set_damping_ratio(ratio); - self - } - /// Builds the spring joint. #[must_use] pub fn build(self) -> SpringJoint { 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 3a79fcd49..f5f320761 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs @@ -140,9 +140,8 @@ impl GenericJointConstraint { mb1, mb2, i - DIM, + joint.softness, WritebackId::Dof(i), - joint.natural_frequency, - joint.damping_ratio, ); len += 1; } @@ -159,9 +158,8 @@ impl GenericJointConstraint { mb1, mb2, i, + joint.softness, WritebackId::Dof(i), - joint.natural_frequency, - joint.damping_ratio, ); len += 1; } @@ -180,9 +178,8 @@ impl GenericJointConstraint { mb2, i - DIM, [joint.limits[i].min, joint.limits[i].max], + joint.softness, WritebackId::Limit(i), - joint.natural_frequency, - joint.damping_ratio, ); len += 1; } @@ -200,9 +197,8 @@ impl GenericJointConstraint { mb2, i, [joint.limits[i].min, joint.limits[i].max], + joint.softness, WritebackId::Limit(i), - joint.natural_frequency, - joint.damping_ratio, ); 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 67ccbb36f..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,9 +407,8 @@ impl JointConstraintHelper { mb1: LinkOrBodyRef, mb2: LinkOrBodyRef, locked_axis: usize, + softness: SpringCoefficients, writeback_id: WritebackId, - natural_frequency: Real, - damping_ratio: Real, ) -> GenericJointConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); @@ -428,7 +428,7 @@ impl JointConstraintHelper { ang_jac2, ); - let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); + 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 @@ -446,9 +446,8 @@ impl JointConstraintHelper { mb2: LinkOrBodyRef, limited_axis: usize, limits: [Real; 2], + softness: SpringCoefficients, writeback_id: WritebackId, - natural_frequency: Real, - damping_ratio: Real, ) -> GenericJointConstraint { let lin_jac = self.basis.column(limited_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned(); @@ -472,7 +471,7 @@ impl JointConstraintHelper { let min_enabled = dist <= limits[0]; let max_enabled = limits[1] <= dist; - let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); + 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; @@ -550,9 +549,8 @@ impl JointConstraintHelper { mb1: LinkOrBodyRef, mb2: LinkOrBodyRef, _locked_axis: usize, + softness: SpringCoefficients, writeback_id: WritebackId, - natural_frequency: Real, - damping_ratio: Real, ) -> GenericJointConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); @@ -573,7 +571,7 @@ impl JointConstraintHelper { ang_jac, ); - let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); + 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")] @@ -594,9 +592,8 @@ impl JointConstraintHelper { mb2: LinkOrBodyRef, _limited_axis: usize, limits: [Real; 2], + softness: SpringCoefficients, writeback_id: WritebackId, - natural_frequency: Real, - damping_ratio: Real, ) -> GenericJointConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); @@ -629,7 +626,7 @@ impl JointConstraintHelper { max_enabled as u32 as Real * Real::MAX, ]; - let erp_inv_dt = params.joint_erp_inv_dt_with_override(natural_frequency, damping_ratio); + 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 0b49fb57b..720f429e8 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,8 +106,7 @@ pub struct JointConstraintBuilderSimd { local_frame1: Isometry, local_frame2: Isometry, locked_axes: u8, - natural_frequency: SimdReal, - damping_ratio: SimdReal, + softness: SpringCoefficients, constraint_id: usize, } @@ -151,8 +153,10 @@ impl JointConstraintBuilderSimd { local_frame1, local_frame2, locked_axes: joint[0].data.locked_axes.bits(), - natural_frequency: array![|ii| joint[ii].data.natural_frequency].into(), - damping_ratio: array![|ii| joint[ii].data.damping_ratio].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(), + }, constraint_id: *out_constraint_id, }; @@ -195,8 +199,7 @@ impl JointConstraintBuilderSimd { &frame1, &frame2, self.locked_axes, - self.natural_frequency, - self.damping_ratio, + self.softness, &mut out[self.constraint_id..], ); } diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index bf7399daf..f64a820dc 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -145,8 +145,8 @@ impl JointConstraint { let coupled_axes = joint.coupled_axes.bits(); // Compute per-joint ERP and CFM coefficients - let erp_inv_dt = joint.joint_erp_inv_dt(params.dt); - let cfm_coeff = joint.joint_cfm_coeff(params.dt); + 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; @@ -368,23 +368,12 @@ impl JointConstraint { frame1: &Isometry, frame2: &Isometry, locked_axes: u8, - natural_frequency: SimdReal, - damping_ratio: SimdReal, + softness: crate::dynamics::SpringCoefficients, out: &mut [Self], ) -> usize { - // Compute per-lane ERP and CFM coefficients using the `_with_override` methods - // Need to compute per-lane, so use the formula directly: - let ang_freq = natural_frequency * SimdReal::splat(std::f64::consts::TAU as Real); let dt = SimdReal::splat(params.dt); - let erp_inv_dt = ang_freq / (dt * ang_freq + SimdReal::splat(2.0) * damping_ratio); - - let joint_erp = dt * erp_inv_dt; - let inv_erp_minus_one = SimdReal::splat(1.0) / joint_erp - SimdReal::splat(1.0); - let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one - / ((SimdReal::splat(1.0) + inv_erp_minus_one) - * SimdReal::splat(4.0) - * damping_ratio - * damping_ratio); + let erp_inv_dt = softness.erp_inv_dt(dt); + let cfm_coeff = softness.cfm_coeff(dt); let builder = JointConstraintHelper::new( frame1, diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index facf08a42..c89a0b264 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -189,18 +189,20 @@ 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})",)), From 68a553e0839c86207ce1d26df4fbfa44e2f41791 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 21 Nov 2025 16:29:36 +0100 Subject: [PATCH 14/14] chore: misc cleanups --- examples2d/pin_slot_joint2.rs | 23 +++++++++++++++++++ src/dynamics/joint/generic_joint.rs | 5 +--- .../joint_constraint_builder.rs | 4 ++-- 3 files changed, 26 insertions(+), 6 deletions(-) diff --git a/examples2d/pin_slot_joint2.rs b/examples2d/pin_slot_joint2.rs index 589c5c30c..c5ba43faf 100644 --- a/examples2d/pin_slot_joint2.rs +++ b/examples2d/pin_slot_joint2.rs @@ -1,4 +1,7 @@ +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,6 +80,26 @@ 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. */ diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 9edc65c3f..83d77dee6 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -305,10 +305,7 @@ impl Default for GenericJoint { coupled_axes: JointAxesMask::empty(), limits: [JointLimits::default(); SPATIAL_DIM], motors: [JointMotor::default(); SPATIAL_DIM], - softness: SpringCoefficients { - natural_frequency: 1.0e6, - damping_ratio: 1.0, - }, + softness: SpringCoefficients::joint_defaults(), contacts_enabled: true, enabled: JointEnabled::Enabled, user_data: 0, diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 720f429e8..2dc702d10 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -402,8 +402,6 @@ impl JointConstraintHelper { writeback_id: WritebackId, ) -> JointConstraint { let inv_dt = N::splat(params.inv_dt()); - // Motors don't use joint erp/cfm; they use motor-specific parameters. - // Pass dummy values since lock_linear will override them anyway. let mut constraint = self.lock_linear( params, joint_id, @@ -411,6 +409,8 @@ impl JointConstraintHelper { body2, motor_axis, writeback_id, + // Set regularization factors to zero. + // The motor impl. will overwrite them after. N::zero(), N::zero(), );