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.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.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_constraint.rs | 20 |
1 files changed, 12 insertions, 8 deletions
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 6a95492..bb00b66 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -55,23 +55,26 @@ impl AnyVelocityConstraint { pub fn solve( &mut self, + cfm_factor: Real, mj_lambdas: &mut [DeltaVel<Real>], solve_normal: bool, solve_friction: bool, ) { match self { AnyVelocityConstraint::NongroupedGround(c) => { - c.solve(mj_lambdas, solve_normal, solve_friction) + c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction) } AnyVelocityConstraint::Nongrouped(c) => { - c.solve(mj_lambdas, solve_normal, solve_friction) + c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction) } #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::GroupedGround(c) => { - c.solve(mj_lambdas, solve_normal, solve_friction) + c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction) } #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas, solve_normal, solve_friction), + AnyVelocityConstraint::Grouped(c) => { + c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction) + } AnyVelocityConstraint::Empty => unreachable!(), } } @@ -236,7 +239,7 @@ impl VelocityConstraint { .transform_vector(dp2.gcross(-force_dir1)); let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; - let r = params.delassus_inv_factor + let projected_mass = 1.0 / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); @@ -251,14 +254,13 @@ impl VelocityConstraint { let rhs_bias = /* is_resting * */ erp_inv_dt * (manifold_point.dist + params.allowed_linear_error).min(0.0); - constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, - impulse: 0.0, - r, + impulse: na::zero(), + r: projected_mass, }; } @@ -310,6 +312,7 @@ impl VelocityConstraint { pub fn solve( &mut self, + cfm_factor: Real, mj_lambdas: &mut [DeltaVel<Real>], solve_normal: bool, solve_friction: bool, @@ -318,6 +321,7 @@ impl VelocityConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; VelocityConstraintElement::solve_group( + cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] |