diff options
| author | Sébastien Crozet <developer@crozet.re> | 2022-01-16 07:52:19 -0800 |
|---|---|---|
| committer | GitHub <noreply@github.com> | 2022-01-16 07:52:19 -0800 |
| commit | 4454a845e98b990abf3929ca46b59d0fca5a18ec (patch) | |
| tree | e4808725e872b7178ba81c3ac5475be3a04569ac /src/dynamics/solver/velocity_ground_constraint.rs | |
| parent | 0ccd15c4b1f57d6c85a1727a55ed991c835690f5 (diff) | |
| parent | 8213e92f146fab618a406e0f8fed8a15ebd9228c (diff) | |
| download | rapier-4454a845e98b990abf3929ca46b59d0fca5a18ec.tar.gz rapier-4454a845e98b990abf3929ca46b59d0fca5a18ec.tar.bz2 rapier-4454a845e98b990abf3929ca46b59d0fca5a18ec.zip | |
Merge pull request #276 from dimforge/lock-translation-axis
Allow locking individual translational axes
Diffstat (limited to 'src/dynamics/solver/velocity_ground_constraint.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint.rs | 13 |
1 files changed, 9 insertions, 4 deletions
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 87865b3..bd60633 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -17,7 +17,7 @@ pub(crate) struct VelocityGroundConstraint { pub dir1: Vector<Real>, // Non-penetration force direction for the first body. #[cfg(feature = "dim3")] pub tangent1: Vector<Real>, // One of the friction force directions. - pub im2: Real, + pub im2: Vector<Real>, pub limit: Real, pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS], @@ -153,7 +153,9 @@ impl VelocityGroundConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); - let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2)); + let r = 1.0 + / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + + gcross2.gdot(gcross2)); let is_bouncy = manifold_point.is_bouncy() as u32 as Real; let is_resting = 1.0 - is_bouncy; @@ -184,7 +186,10 @@ impl VelocityGroundConstraint { let gcross2 = mprops2 .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); - let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2)); + let r = 1.0 + / (tangents1[j] + .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j])) + + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2 + flipped_multiplier * manifold_point.tangent_velocity) .dot(&tangents1[j]); @@ -219,7 +224,7 @@ impl VelocityGroundConstraint { &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, - self.im2, + &self.im2, self.limit, &mut mj_lambda2, solve_normal, |