diff options
| author | Sébastien Crozet <developer@crozet.re> | 2022-01-23 08:56:27 -0800 |
|---|---|---|
| committer | GitHub <noreply@github.com> | 2022-01-23 08:56:27 -0800 |
| commit | 1608a1323ed76cdf33644cfea599cea715acf7a9 (patch) | |
| tree | 07b975a2b22b31f74a5efcbaa3d2a30aea31ae47 /src/dynamics/solver/velocity_constraint_element.rs | |
| parent | ca635674fc72071d7ff546a749ac22766579b280 (diff) | |
| parent | b3b675d2de64d4437748ad46e41cca90c691de1a (diff) | |
| download | rapier-1608a1323ed76cdf33644cfea599cea715acf7a9.tar.gz rapier-1608a1323ed76cdf33644cfea599cea715acf7a9.tar.bz2 rapier-1608a1323ed76cdf33644cfea599cea715acf7a9.zip | |
Merge pull request #282 from dimforge/critical-damping
Improve the CFM implementation
Diffstat (limited to 'src/dynamics/solver/velocity_constraint_element.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_constraint_element.rs | 6 |
1 files changed, 4 insertions, 2 deletions
diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs index b0f8087..2d2221d 100644 --- a/src/dynamics/solver/velocity_constraint_element.rs +++ b/src/dynamics/solver/velocity_constraint_element.rs @@ -131,6 +131,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> { #[inline] pub fn solve( &mut self, + cfm_factor: N, dir1: &Vector<N>, im1: &Vector<N>, im2: &Vector<N>, @@ -143,7 +144,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> { - dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs; - let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero()); + let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero()); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; @@ -171,6 +172,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> { #[inline] pub fn solve_group( + cfm_factor: N, elements: &mut [Self], dir1: &Vector<N>, #[cfg(feature = "dim3")] tangent1: &Vector<N>, @@ -191,7 +193,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> { for element in elements.iter_mut() { element .normal_part - .solve(&dir1, im1, im2, mj_lambda1, mj_lambda2); + .solve(cfm_factor, &dir1, im1, im2, mj_lambda1, mj_lambda2); } } |