diff options
Diffstat (limited to 'src')
29 files changed, 760 insertions, 133 deletions
diff --git a/src/data/arena.rs b/src/data/arena.rs index c7cbf07..b14737e 100644 --- a/src/data/arena.rs +++ b/src/data/arena.rs @@ -70,11 +70,8 @@ impl Index { /// /// Providing arbitrary values will lead to malformed indices and ultimately /// panics. - pub fn from_raw_parts(a: u32, b: u32) -> Index { - Index { - index: a, - generation: b, - } + pub fn from_raw_parts(index: u32, generation: u32) -> Index { + Index { index, generation } } /// Convert this `Index` into its raw parts. diff --git a/src/data/coarena.rs b/src/data/coarena.rs index 1f01c05..c6fd122 100644 --- a/src/data/coarena.rs +++ b/src/data/coarena.rs @@ -29,6 +29,10 @@ impl<T> Coarena<T> { self.data.get(index as usize).map(|(_, t)| t) } + pub(crate) fn get_gen(&self, index: u32) -> Option<u32> { + self.data.get(index as usize).map(|(gen, _)| *gen) + } + /// Deletes an element for the coarena and returns its value. /// /// This method will reset the value to the given `removed_value`. diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index d998eec..844db41 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -30,6 +30,13 @@ pub struct IntegrationParameters { /// (default `0.0`). pub erp: Real, + /// 0-1: multiplier applied to each accumulated impulse during constraints resolution. + /// This is similar to the concept of CFN (Constraint Force Mixing) except that it is + /// a multiplicative factor instead of an additive factor. + /// Larger values lead to stiffer constraints (1.0 being completely stiff). + /// Smaller values lead to more compliant constraints. + pub delassus_inv_factor: Real, + /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). pub allowed_linear_error: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). @@ -96,6 +103,7 @@ impl Default for IntegrationParameters { min_ccd_dt: 1.0 / 60.0 / 100.0, velocity_solve_fraction: 1.0, erp: 0.8, + delassus_inv_factor: 0.75, allowed_linear_error: 0.001, // 0.005 prediction_distance: 0.002, max_velocity_iterations: 4, diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 55b47c9..53f803d 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -5,6 +5,7 @@ use crate::dynamics::{ }; use crate::geometry::{ColliderParent, NarrowPhase}; use crate::math::Real; +use crate::utils::WDot; /// Structure responsible for maintaining the set of active rigid-bodies, and /// putting non-moving rigid-bodies to sleep to save computation times. @@ -172,6 +173,7 @@ impl IslandManager { pub(crate) fn update_active_set_with_contacts<Bodies, Colliders>( &mut self, + dt: Real, bodies: &mut Bodies, colliders: &Colliders, narrow_phase: &NarrowPhase, @@ -207,12 +209,15 @@ impl IslandManager { let stack = &mut self.stack; let vels: &RigidBodyVelocity = bodies.index(h.0); - let pseudo_kinetic_energy = vels.pseudo_kinetic_energy(); + let sq_linvel = vels.linvel.norm_squared(); + let sq_angvel = vels.angvel.gdot(vels.angvel); bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| { - update_energy(activation, pseudo_kinetic_energy); + update_energy(activation, sq_linvel, sq_angvel, dt); - if activation.energy <= activation.threshold { + if activation.time_since_can_sleep + >= RigidBodyActivation::default_time_until_sleep() + { // Mark them as sleeping for now. This will // be set to false during the graph traversal // if it should not be put to sleep. @@ -346,8 +351,12 @@ impl IslandManager { } } -fn update_energy(activation: &mut RigidBodyActivation, pseudo_kinetic_energy: Real) { - let mix_factor = 0.01; - let new_energy = (1.0 - mix_factor) * activation.energy + mix_factor * pseudo_kinetic_energy; - activation.energy = new_energy.min(activation.threshold.abs() * 4.0); +fn update_energy(activation: &mut RigidBodyActivation, sq_linvel: Real, sq_angvel: Real, dt: Real) { + if sq_linvel < activation.linear_threshold * activation.linear_threshold + && sq_angvel < activation.angular_threshold * activation.angular_threshold + { + activation.time_since_can_sleep += dt; + } else { + activation.time_since_can_sleep = 0.0; + } } diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 183b668..890f021 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -100,7 +100,7 @@ impl ImpulseJointSet { /// Gets the joint with the given handle without a known generation. /// - /// This is useful when you know you want the joint at position `i` but + /// This is useful when you know you want the joint at index `i` but /// don't know what is its current generation number. Generation numbers are /// used to protect from the ABA problem because the joint position `i` /// are recycled between two insertion and a removal. diff --git a/src/dynamics/joint/joint_data.rs b/src/dynamics/joint/joint_data.rs index 35d832d..b1ad6c6 100644 --- a/src/dynamics/joint/joint_data.rs +++ b/src/dynamics/joint/joint_data.rs @@ -218,12 +218,14 @@ impl JointData { } /// 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, axis: JointAxis, model: MotorModel) -> Self { self.motors[axis as usize].model = model; self } /// Sets the target velocity this motor needs to reach. + #[must_use] pub fn motor_velocity(self, axis: JointAxis, target_vel: Real, factor: Real) -> Self { self.motor_axis( axis, @@ -235,6 +237,7 @@ impl JointData { } /// Sets the target angle this motor needs to reach. + #[must_use] pub fn motor_position( self, axis: JointAxis, @@ -246,6 +249,7 @@ impl JointData { } /// Configure both the target angle and target velocity of the motor. + #[must_use] pub fn motor_axis( mut self, axis: JointAxis, @@ -263,6 +267,7 @@ impl JointData { self } + #[must_use] pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self { self.motors[axis as usize].max_impulse = max_impulse; self diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 1b88245..e31a333 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -62,7 +62,10 @@ fn concat_rb_mass_matrix( } /// An articulated body simulated using the reduced-coordinates approach. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] pub struct Multibody { + // TODO: serialization: skip the workspace fields. links: MultibodyLinkVec, pub(crate) velocities: DVector<Real>, pub(crate) damping: DVector<Real>, diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 92749c1..1d14680 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -11,6 +11,7 @@ use na::{DVector, DVectorSliceMut}; #[cfg(feature = "dim3")] use na::{UnitQuaternion, Vector3}; +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug)] pub struct MultibodyJoint { pub data: JointData, diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 8337158..24b329f 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -1,4 +1,4 @@ -use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut}; +use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index}; use crate::dynamics::joint::MultibodyLink; use crate::dynamics::{ IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle, @@ -6,19 +6,18 @@ use crate::dynamics::{ }; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex}; use crate::parry::partitioning::IndexedData; -use std::ops::Index; /// The unique handle of an multibody_joint added to a `MultibodyJointSet`. #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[repr(transparent)] -pub struct MultibodyJointHandle(pub crate::data::arena::Index); +pub struct MultibodyJointHandle(pub Index); /// The temporary index of a multibody added to a `MultibodyJointSet`. #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[repr(transparent)] -pub struct MultibodyIndex(pub crate::data::arena::Index); +pub struct MultibodyIndex(pub Index); impl MultibodyJointHandle { /// Converts this handle into its (index, generation) components. @@ -28,12 +27,12 @@ impl MultibodyJointHandle { /// Reconstructs an handle from its (index, generation) components. pub fn from_raw_parts(id: u32, generation: u32) -> Self { - Self(crate::data::arena::Index::from_raw_parts(id, generation)) + Self(Index::from_raw_parts(id, generation)) } /// An always-invalid rigid-body handle. pub fn invalid() -> Self { - Self(crate::data::arena::Index::from_raw_parts( + Self(Index::from_raw_parts( crate::INVALID_U32, crate::INVALID_U32, )) @@ -55,6 +54,7 @@ impl IndexedData for MultibodyJointHandle { } } +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq, Eq)] pub struct MultibodyJointLink { pub graph_id: RigidBodyGraphIndex, @@ -66,7 +66,7 @@ impl Default for MultibodyJointLink { fn default() -> Self { Self { graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32), - multibody: MultibodyIndex(crate::data::arena::Index::from_raw_parts( + multibody: MultibodyIndex(Index::from_raw_parts( crate::INVALID_U32, crate::INVALID_U32, )), @@ -76,6 +76,8 @@ impl Default for MultibodyJointLink { } /// A set of rigid bodies that can be handled by a physics pipeline. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] pub struct MultibodyJointSet { pub(crate) multibodies: Arena<Multibody>, // NOTE: a Slab would be sufficient. pub(crate) rb2mb: Coarena<MultibodyJointLink>, @@ -316,6 +318,26 @@ impl MultibodyJointSet { Some((multibody, link.id)) } + /// Gets the joint with the given handle without a known generation. + /// + /// This is useful when you know you want the joint at index `i` but + /// don't know what is its current generation number. Generation numbers are + /// used to protect from the ABA problem because the joint position `i` + /// are recycled between two insertion and a removal. + /// + /// Using this is discouraged in favor of `self.get(handle)` which does not + /// suffer form the ABA problem. + pub fn get_unknown_gen(&self, i: u32) -> Option<(&Multibody, usize, MultibodyJointHandle)> { + let link = self.rb2mb.get_unknown_gen(i)?; + let gen = self.rb2mb.get_gen(i)?; + let multibody = self.multibodies.get(link.multibody.0)?; + Some(( + multibody, + link.id, + MultibodyJointHandle(Index::from_raw_parts(i, gen)), + )) + } + /// Iterate through the handles of all the rigid-bodies attached to this rigid-body /// by an multibody_joint. pub fn attached_bodies<'a>( @@ -335,7 +357,7 @@ impl MultibodyJointSet { } } -impl Index<MultibodyIndex> for MultibodyJointSet { +impl std::ops::Index<MultibodyIndex> for MultibodyJointSet { type Output = Multibody; fn index(&self, index: MultibodyIndex) -> &Multibody { diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs index 4738865..3c35446 100644 --- a/src/dynamics/joint/multibody_joint/multibody_link.rs +++ b/src/dynamics/joint/multibody_joint/multibody_link.rs @@ -5,6 +5,8 @@ use crate::math::{Isometry, Real, Vector}; use crate::prelude::RigidBodyVelocity; /// One link of a multibody. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] pub struct MultibodyLink { // FIXME: make all those private. pub(crate) internal_id: usize, @@ -96,6 +98,8 @@ impl MultibodyLink { } // FIXME: keep this even if we already have the Index2 traits? +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] pub(crate) struct MultibodyLinkVec(pub Vec<MultibodyLink>); impl MultibodyLinkVec { diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index e5dcd04..db972d4 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -1089,7 +1089,8 @@ impl RigidBodyBuilder { } if !self.can_sleep { - rb.rb_activation.threshold = -1.0; + rb.rb_activation.linear_threshold = -1.0; + rb.rb_activation.angular_threshold = -1.0; } rb diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index ff3a614..fdbd33f 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -927,11 +927,13 @@ impl RigidBodyDominance { #[derive(Copy, Clone, Debug, PartialEq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct RigidBodyActivation { - /// The threshold pseudo-kinetic energy bellow which the body can fall asleep. - pub threshold: Real, - /// The current pseudo-kinetic energy of the body. - pub energy: Real, - /// Is this body already sleeping? + /// The threshold linear velocity bellow which the body can fall asleep. + pub linear_threshold: Real, + /// The angular linear velocity bellow which the body can fall asleep. + pub angular_threshold: Real, + /// Since how much time can this body sleep? + pub time_since_can_sleep: Real, + /// Is this body sleeping? pub sleeping: bool, } @@ -942,16 +944,28 @@ impl Default for RigidBodyActivation { } impl RigidBodyActivation { - /// The default amount of energy bellow which a body can be put to sleep by rapier. - pub fn default_threshold() -> Real { - 0.01 + /// The default linear velocity bellow which a body can be put to sleep. + pub fn default_linear_threshold() -> Real { + 0.4 + } + + /// The default angular velocity bellow which a body can be put to sleep. + pub fn default_angular_threshold() -> Real { + 0.5 + } + + /// The amount of time the rigid-body must remain bellow it’s linear and angular velocity + /// threshold before falling to sleep. + pub fn default_time_until_sleep() -> Real { + 2.0 } /// Create a new rb_activation status initialised with the default rb_activation threshold and is active. pub fn active() -> Self { RigidBodyActivation { - threshold: Self::default_threshold(), - energy: Self::default_threshold() * 4.0, + linear_threshold: Self::default_linear_threshold(), + angular_threshold: Self::default_angular_threshold(), + time_since_can_sleep: 0.0, sleeping: false, } } @@ -959,16 +973,18 @@ impl RigidBodyActivation { /// Create a new rb_activation status initialised with the default rb_activation threshold and is inactive. pub fn inactive() -> Self { RigidBodyActivation { - threshold: Self::default_threshold(), - energy: 0.0, + linear_threshold: Self::default_linear_threshold(), + angular_threshold: Self::default_angular_threshold(), sleeping: true, + time_since_can_sleep: Self::default_time_until_sleep(), } } /// Create a new activation status that prevents the rigid-body from sleeping. pub fn cannot_sleep() -> Self { RigidBodyActivation { - threshold: -Real::MAX, + linear_threshold: -1.0, + angular_threshold: -1.0, ..Self::active() } } @@ -976,22 +992,22 @@ impl RigidBodyActivation { /// Returns `true` if the body is not asleep. #[inline] pub fn is_active(&self) -> bool { - self.energy != 0.0 + !self.sleeping } /// Wakes up this rigid-body. #[inline] pub fn wake_up(&mut self, strong: bool) { self.sleeping = false; - if strong || self.energy == 0.0 { - self.energy = self.threshold.abs() * 2.0; + if strong { + self.time_since_can_sleep = 0.0; } } /// Put this rigid-body to sleep. #[inline] pub fn sleep(&mut self) { - self.energy = 0.0; self.sleeping = true; + self.time_since_can_sleep = Self::default_time_until_sleep(); } } diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index c5b2601..0083388 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -9,8 +9,8 @@ pub(crate) fn categorize_contacts( manifold_indices: &[ContactManifoldIndex], out_ground: &mut Vec<ContactManifoldIndex>, out_not_ground: &mut Vec<ContactManifoldIndex>, - out_generic: &mut Vec<ContactManifoldIndex>, - _unused: &mut Vec<ContactManifoldIndex>, // Unused but useful to simplify the parallel code. + out_generic_ground: &mut Vec<ContactManifoldIndex>, + out_generic_not_ground: &mut Vec<ContactManifoldIndex>, ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; @@ -18,15 +18,19 @@ pub(crate) fn categorize_contacts( if manifold .data .rigid_body1 - .map(|rb| multibody_joints.rigid_body_link(rb)) + .and_then(|h| multibody_joints.rigid_body_link(h)) .is_some() || manifold .data - .rigid_body1 - .map(|rb| multibody_joints.rigid_body_link(rb)) + .rigid_body2 + .and_then(|h| multibody_joints.rigid_body_link(h)) .is_some() { - out_generic.push(*manifold_i); + if manifold.data.relative_dominance != 0 { + out_generic_ground.push(*manifold_i); + } else { + out_generic_not_ground.push(*manifold_i); + } } else if manifold.data.relative_dominance != 0 { out_ground.push(*manifold_i) } else { diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs index 9160f2e..697fd24 100644 --- a/src/dynamics/solver/delta_vel.rs +++ b/src/dynamics/solver/delta_vel.rs @@ -1,7 +1,7 @@ use crate::math::{AngVector, Vector, SPATIAL_DIM}; use na::{DVectorSlice, DVectorSliceMut}; use na::{Scalar, SimdRealField}; -use std::ops::AddAssign; +use std::ops::{AddAssign, Sub}; #[derive(Copy, Clone, Debug)] #[repr(C)] @@ -44,3 +44,14 @@ impl<N: SimdRealField + Copy> AddAssign for DeltaVel<N> { self.angular += rhs.angular; } } + +impl<N: SimdRealField + Copy> Sub for DeltaVel<N> { + type Output = Self; + + fn sub(self, rhs: Self) -> Self { + DeltaVel { + linear: self.linear - rhs.linear, + angular: self.angular - rhs.angular, + } + } +} diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index 8c93511..f1ab0ea 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -9,11 +9,61 @@ use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WCross, WDot}; use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart}; +use crate::dynamics::solver::GenericVelocityGroundConstraint; #[cfg(feature = "dim2")] use crate::utils::WBasis; use na::DVector; #[derive(Copy, Clone, Debug)] +pub(crate) enum AnyGenericVelocityConstraint { + NongroupedGround(GenericVelocityGroundConstraint), + Nongrouped(GenericVelocityConstraint), +} + +impl AnyGenericVelocityConstraint { + pub fn solve( + &mut self, + jacobians: &DVector<Real>, + mj_lambdas: &mut [DeltaVel<Real>], + generic_mj_lambdas: &mut DVector<Real>, + solve_restitution: bool, + solve_friction: bool, + ) { + match self { + AnyGenericVelocityConstraint::Nongrouped(c) => c.solve( + jacobians, + mj_lambdas, + generic_mj_lambdas, + solve_restitution, + solve_friction, + ), + AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve( + jacobians, + generic_mj_lambdas, + solve_restitution, + solve_friction, + ), + } + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + match self { + AnyGenericVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifolds_all), + AnyGenericVelocityConstraint::NongroupedGround(c) => { + c.writeback_impulses(manifolds_all) + } + } + } + + pub fn remove_bias_from_rhs(&mut self) { + match self { + AnyGenericVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(), + AnyGenericVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(), + } + } +} + +#[derive(Copy, Clone, Debug)] pub(crate) struct GenericVelocityConstraint { // We just build the generic constraint on top of the velocity constraint, // adding some information we can use in the generic case. @@ -31,7 +81,7 @@ impl GenericVelocityConstraint { manifold: &ContactManifold, bodies: &Bodies, multibodies: &MultibodyJointSet, - out_constraints: &mut Vec<GenericVelocityConstraint>, + out_constraints: &mut Vec<AnyGenericVelocityConstraint>, jacobians: &mut DVector<Real>, jacobian_id: &mut usize, push: bool, @@ -293,6 +343,9 @@ impl GenericVelocityConstraint { (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]); constraint.elements[k].tangent_part.rhs[j] = rhs; + // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1]) + // in lhs. See the corresponding code on the `velocity_constraint.rs` + // file. constraint.elements[k].tangent_part.r[j] = r; } } @@ -316,9 +369,10 @@ impl GenericVelocityConstraint { }; if push { - out_constraints.push(constraint); + out_constraints.push(AnyGenericVelocityConstraint::Nongrouped(constraint)); } else { - out_constraints[manifold.data.constraint_index + _l] = constraint; + out_constraints[manifold.data.constraint_index + _l] = + AnyGenericVelocityConstraint::Nongrouped(constraint); } } } diff --git a/src/dynamics/solver/generic_velocity_constraint_element.rs b/src/dynamics/solver/generic_velocity_constraint_element.rs index c31bbfb..e75dd01 100644 --- a/src/dynamics/solver/generic_velocity_constraint_element.rs +++ b/src/dynamics/solver/generic_velocity_constraint_element.rs @@ -41,7 +41,7 @@ fn tangent_j_id(j_id: usize, ndofs1: usize, ndofs2: usize) -> usize { impl GenericRhs { #[inline(always)] - fn dimpulse( + fn dvel( &self, j_id: usize, ndofs: usize, @@ -110,14 +110,14 @@ impl VelocityConstraintTangentPart<Real> { #[cfg(feature = "dim2")] { - let dimpulse_0 = mj_lambda1.dimpulse( + let dvel_0 = mj_lambda1.dvel( j_id1, ndofs1, jacobians, &tangents1[0], &self.gcross1[0], mj_lambdas, - ) + mj_lambda2.dimpulse( + ) + mj_lambda2.dvel( j_id2, ndofs2, jacobians, @@ -126,7 +126,7 @@ impl VelocityConstraintTangentPart<Real> { mj_lambdas, ) + self.rhs[0]; - let new_impulse = (self.impulse[0] - self.r[0] * dimpulse_0).simd_clamp(-limit, limit); + let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit); let dlambda = new_impulse - self.impulse[0]; self.impulse[0] = new_impulse; @@ -154,14 +154,14 @@ impl VelocityConstraintTangentPart<Real> { #[cfg(feature = "dim3")] { - let dimpulse_0 = mj_lambda1.dimpulse( + let dvel_0 = mj_lambda1.dvel( j_id1, ndofs1, jacobians, &tangents1[0], &self.gcross1[0], mj_lambdas, - ) + mj_lambda2.dimpulse( + ) + mj_lambda2.dvel( j_id2, ndofs2, jacobians, @@ -169,14 +169,14 @@ impl VelocityConstraintTangentPart<Real> { &self.gcross2[0], mj_lambdas, ) + self.rhs[0]; - let dimpulse_1 = mj_lambda1.dimpulse( + let dvel_1 = mj_lambda1.dvel( j_id1 + j_step, ndofs1, jacobians, &tangents1[1], &self.gcross1[1], mj_lambdas, - ) + mj_lambda2.dimpulse( + ) + mj_lambda2.dvel( j_id2 + j_step, ndofs2, jacobians, @@ -186,8 +186,8 @@ impl VelocityConstraintTangentPart<Real> { ) + self.rhs[1]; let new_impulse = na::Vector2::new( - self.impulse[0] - self.r[0] * dimpulse_0, - self.impulse[1] - self.r[1] * dimpulse_1, + self.impulse[0] - self.r[0] * dvel_0, + self.impulse[1] - self.r[1] * dvel_1, ); let new_impulse = new_impulse.cap_magnitude(limit); @@ -257,12 +257,11 @@ impl VelocityConstraintNormalPart<Real> { let j_id1 = j_id1(j_id, ndofs1, ndofs2); let j_id2 = j_id2(j_id, ndofs1, ndofs2); - let dimpulse = - mj_lambda1.dimpulse(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas) - + mj_lambda2.dimpulse(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas) - + self.rhs; + let dvel = mj_lambda1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas) + + mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas) + + self.rhs; - let new_impulse = (self.impulse - self.r * dimpuls |