diff --git a/.gitignore b/.gitignore index 055e3a4d4..db22dde05 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,4 @@ target .DS_Store package-lock.json **/*.csv -.vscode/ +.history \ No newline at end of file diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 844db4189..c1d3e26af 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -29,13 +29,11 @@ pub struct IntegrationParameters { /// A good non-zero value is around `0.2`. /// (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, + /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization. + /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations + /// before stabilization). + /// (default `0.25`). + pub damping_ratio: Real, /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). pub allowed_linear_error: Real, @@ -89,10 +87,42 @@ impl IntegrationParameters { } } - /// Convenience: `erp / dt` - #[inline] - pub(crate) fn erp_inv_dt(&self) -> Real { - self.erp * self.inv_dt() + /// The ERP coefficient, multiplied by the inverse timestep length. + pub fn erp_inv_dt(&self) -> Real { + 0.8 / self.dt + } + + /// The CFM factor to be used in the constraints resolution. + pub fn cfm_factor(&self) -> Real { + // Compute CFM assuming a critically damped spring multiplied by the dampingratio. + let inv_erp_minus_one = 1.0 / self.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 simplies to cfm = cfm_coefff / projected_mass: + let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one + / ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.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 constraints solver. + 1.0 / (1.0 + cfm_coeff) } } @@ -103,14 +133,14 @@ 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, + damping_ratio: 0.25, allowed_linear_error: 0.001, // 0.005 prediction_distance: 0.002, max_velocity_iterations: 4, max_velocity_friction_iterations: 8, max_stabilization_iterations: 1, interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability. - // FIXME: what is the optimal value for min_island_size? + // TODO: what is the optimal value for min_island_size? // It should not be too big so that we don't end up with // huge islands that don't fit in cache. // However we don't want it to be too small and end up with diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index f1ab0eadb..c1d41342e 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -23,6 +23,7 @@ pub(crate) enum AnyGenericVelocityConstraint { impl AnyGenericVelocityConstraint { pub fn solve( &mut self, + cfm_factor: Real, jacobians: &DVector, mj_lambdas: &mut [DeltaVel], generic_mj_lambdas: &mut DVector, @@ -31,6 +32,7 @@ impl AnyGenericVelocityConstraint { ) { match self { AnyGenericVelocityConstraint::Nongrouped(c) => c.solve( + cfm_factor, jacobians, mj_lambdas, generic_mj_lambdas, @@ -38,6 +40,7 @@ impl AnyGenericVelocityConstraint { solve_friction, ), AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve( + cfm_factor, jacobians, generic_mj_lambdas, solve_restitution, @@ -379,6 +382,7 @@ impl GenericVelocityConstraint { pub fn solve( &mut self, + cfm_factor: Real, jacobians: &DVector, mj_lambdas: &mut [DeltaVel], generic_mj_lambdas: &mut DVector, @@ -400,6 +404,7 @@ impl GenericVelocityConstraint { let elements = &mut self.velocity_constraint.elements [..self.velocity_constraint.num_contacts as usize]; VelocityConstraintElement::generic_solve_group( + cfm_factor, elements, jacobians, &self.velocity_constraint.dir1, diff --git a/src/dynamics/solver/generic_velocity_constraint_element.rs b/src/dynamics/solver/generic_velocity_constraint_element.rs index e75dd01f2..19fba43c3 100644 --- a/src/dynamics/solver/generic_velocity_constraint_element.rs +++ b/src/dynamics/solver/generic_velocity_constraint_element.rs @@ -243,6 +243,7 @@ impl VelocityConstraintNormalPart { #[inline] pub fn generic_solve( &mut self, + cfm_factor: Real, j_id: usize, jacobians: &DVector, dir1: &Vector, @@ -261,7 +262,7 @@ impl VelocityConstraintNormalPart { + mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas) + self.rhs; - let new_impulse = (self.impulse - self.r * dvel).max(0.0); + let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; @@ -291,6 +292,7 @@ impl VelocityConstraintNormalPart { impl VelocityConstraintElement { #[inline] pub fn generic_solve_group( + cfm_factor: Real, elements: &mut [Self], jacobians: &DVector, dir1: &Vector, @@ -318,8 +320,8 @@ impl VelocityConstraintElement { for element in elements.iter_mut() { element.normal_part.generic_solve( - nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1, mj_lambda2, - mj_lambdas, + cfm_factor, nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1, + mj_lambda2, mj_lambdas, ); nrm_j_id += j_step; } diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index c9b2c3f57..9ce824eb4 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -210,6 +210,7 @@ impl GenericVelocityGroundConstraint { pub fn solve( &mut self, + cfm_factor: Real, jacobians: &DVector, generic_mj_lambdas: &mut DVector, solve_restitution: bool, @@ -220,6 +221,7 @@ impl GenericVelocityGroundConstraint { let elements = &mut self.velocity_constraint.elements [..self.velocity_constraint.num_contacts as usize]; VelocityGroundConstraintElement::generic_solve_group( + cfm_factor, elements, jacobians, self.velocity_constraint.limit, diff --git a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs index 80c97ab1c..f645f0487 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs @@ -75,6 +75,7 @@ impl VelocityGroundConstraintNormalPart { #[inline] pub fn generic_solve( &mut self, + cfm_factor: Real, j_id2: usize, jacobians: &DVector, ndofs2: usize, @@ -86,7 +87,7 @@ impl VelocityGroundConstraintNormalPart { .dot(&mj_lambdas.rows(mj_lambda2, ndofs2)) + self.rhs; - let new_impulse = (self.impulse - self.r * dvel).max(0.0); + let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; @@ -101,6 +102,7 @@ impl VelocityGroundConstraintNormalPart { impl VelocityGroundConstraintElement { #[inline] pub fn generic_solve_group( + cfm_factor: Real, elements: &mut [Self], jacobians: &DVector, limit: Real, @@ -121,7 +123,7 @@ impl VelocityGroundConstraintElement { for element in elements.iter_mut() { element .normal_part - .generic_solve(nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas); + .generic_solve(cfm_factor, nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas); nrm_j_id += j_step; } } diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs index 2646fe812..0a7596714 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs @@ -524,6 +524,6 @@ impl JointGenericVelocityGroundConstraint { } pub fn remove_bias_from_rhs(&mut self) { - self.rhs = self.rhs_wo_bias; + self.rhs = &mut self.rhs_wo_bias; } } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 6a954928e..bb00b66a8 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], 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], 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")] diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs index b0f808706..2d2221dfb 100644 --- a/src/dynamics/solver/velocity_constraint_element.rs +++ b/src/dynamics/solver/velocity_constraint_element.rs @@ -131,6 +131,7 @@ impl VelocityConstraintNormalPart { #[inline] pub fn solve( &mut self, + cfm_factor: N, dir1: &Vector, im1: &Vector, im2: &Vector, @@ -143,7 +144,7 @@ impl VelocityConstraintNormalPart { - 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 VelocityConstraintElement { #[inline] pub fn solve_group( + cfm_factor: N, elements: &mut [Self], dir1: &Vector, #[cfg(feature = "dim3")] tangent1: &Vector, @@ -191,7 +193,7 @@ impl VelocityConstraintElement { 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); } } diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 7fcb7f482..44b91c6d3 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -48,9 +48,8 @@ impl WVelocityConstraint { let inv_dt = SimdReal::splat(params.inv_dt()); let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); - let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); - let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); + let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()]; @@ -121,7 +120,6 @@ impl WVelocityConstraint { let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]); let tangent_velocity = Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]); - let dp1 = point - world_com1; let dp2 = point - world_com2; @@ -137,10 +135,11 @@ impl WVelocityConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); let imsum = im1 + im2; - let r = delassus_inv_factor + let projected_mass = SimdReal::splat(1.0) / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); + let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; @@ -154,8 +153,8 @@ impl WVelocityConstraint { gcross2, rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, - impulse: na::zero(), - r, + impulse: SimdReal::splat(0.0), + r: projected_mass, }; } @@ -202,6 +201,7 @@ impl WVelocityConstraint { pub fn solve( &mut self, + cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -221,6 +221,7 @@ impl WVelocityConstraint { }; VelocityConstraintElement::solve_group( + SimdReal::splat(cfm_factor), &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 76de3f9a0..cf7d9eb30 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -153,7 +153,7 @@ impl VelocityGroundConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); - let r = params.delassus_inv_factor + let projected_mass = 1.0 / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + gcross2.gdot(gcross2)); @@ -172,8 +172,8 @@ impl VelocityGroundConstraint { gcross2, rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, - impulse: 0.0, - r, + impulse: na::zero(), + r: projected_mass, }; } @@ -219,6 +219,7 @@ impl VelocityGroundConstraint { pub fn solve( &mut self, + cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -226,6 +227,7 @@ impl VelocityGroundConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; VelocityGroundConstraintElement::solve_group( + cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs index a84390585..805703017 100644 --- a/src/dynamics/solver/velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/velocity_ground_constraint_element.rs @@ -109,12 +109,17 @@ impl VelocityGroundConstraintNormalPart { } #[inline] - pub fn solve(&mut self, dir1: &Vector, im2: &Vector, mj_lambda2: &mut DeltaVel) - where + pub fn solve( + &mut self, + cfm_factor: N, + dir1: &Vector, + im2: &Vector, + mj_lambda2: &mut DeltaVel, + ) where AngVector: WDot, Result = N>, { let dvel = -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; @@ -139,6 +144,7 @@ impl VelocityGroundConstraintElement { #[inline] pub fn solve_group( + cfm_factor: N, elements: &mut [Self], dir1: &Vector, #[cfg(feature = "dim3")] tangent1: &Vector, @@ -155,7 +161,9 @@ impl VelocityGroundConstraintElement { // Solve penetration. if solve_normal { for element in elements.iter_mut() { - element.normal_part.solve(&dir1, im2, mj_lambda2); + element + .normal_part + .solve(cfm_factor, &dir1, im2, mj_lambda2); } } diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 477146978..65ac46eb3 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -43,9 +43,8 @@ impl WVelocityGroundConstraint { { let inv_dt = SimdReal::splat(params.inv_dt()); let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); - let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); - let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); + let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1]; let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2]; @@ -143,8 +142,9 @@ impl WVelocityGroundConstraint { { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - let r = delassus_inv_factor + let projected_mass = SimdReal::splat(1.0) / (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2)); + let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; @@ -158,7 +158,7 @@ impl WVelocityGroundConstraint { rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, impulse: na::zero(), - r, + r: projected_mass, }; } @@ -199,6 +199,7 @@ impl WVelocityGroundConstraint { pub fn solve( &mut self, + cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -211,6 +212,7 @@ impl WVelocityGroundConstraint { }; VelocityGroundConstraintElement::solve_group( + SimdReal::splat(cfm_factor), &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 1cc43acc3..47275ed38 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -50,6 +50,7 @@ impl VelocitySolver { + ComponentSetMut + ComponentSet, { + let cfm_factor = params.cfm_factor(); self.mj_lambdas.clear(); self.mj_lambdas .resize(islands.active_island(island_id).len(), DeltaVel::zero()); @@ -93,18 +94,36 @@ impl VelocitySolver { } for constraint in &mut *contact_constraints { - constraint.solve(&mut self.mj_lambdas[..], true, solve_friction); + constraint.solve(cfm_factor, &mut self.mj_lambdas[..], true, false); } for constraint in &mut *generic_contact_constraints { constraint.solve( + cfm_factor, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, true, - solve_friction, + false, ); } + + if solve_friction { + for constraint in &mut *contact_constraints { + constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true); + } + + for constraint in &mut *generic_contact_constraints { + constraint.solve( + cfm_factor, + generic_contact_jacobians, + &mut self.mj_lambdas[..], + &mut self.generic_mj_lambdas, + false, + true, + ); + } + } } let remaining_friction_iterations = @@ -118,11 +137,12 @@ impl VelocitySolver { for _ in 0..remaining_friction_iterations { for constraint in &mut *contact_constraints { - constraint.solve(&mut self.mj_lambdas[..], false, true); + constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true); } for constraint in &mut *generic_contact_constraints { constraint.solve( + cfm_factor, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, @@ -147,10 +167,7 @@ impl VelocitySolver { multibody.velocities += mj_lambdas; multibody.integrate(params.dt); multibody.forward_kinematics(bodies, false); - - if params.max_stabilization_iterations > 0 { - multibody.velocities = prev_vels; - } + multibody.velocities = prev_vels; } } else { let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = @@ -177,88 +194,85 @@ impl VelocitySolver { new_poss.next_position = new_vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); bodies.set_internal(handle.0, new_poss); - - if params.max_stabilization_iterations == 0 { - bodies.set_internal(handle.0, new_vels); - } } } - if params.max_stabilization_iterations > 0 { - for joint in &mut *joint_constraints { - joint.remove_bias_from_rhs(); + for joint in &mut *joint_constraints { + joint.remove_bias_from_rhs(); + } + for constraint in &mut *contact_constraints { + constraint.remove_bias_from_rhs(); + } + for constraint in &mut *generic_contact_constraints { + constraint.remove_bias_from_rhs(); + } + + for _ in 0..params.max_stabilization_iterations { + for constraint in &mut *joint_constraints { + constraint.solve( + generic_joint_jacobians, + &mut self.mj_lambdas[..], + &mut self.generic_mj_lambdas, + ); } + for constraint in &mut *contact_constraints { - constraint.remove_bias_from_rhs(); + constraint.solve(1.0, &mut self.mj_lambdas[..], true, false); } + for constraint in &mut *generic_contact_constraints { - constraint.remove_bias_from_rhs(); + constraint.solve( + 1.0, + generic_contact_jacobians, + &mut self.mj_lambdas[..], + &mut self.generic_mj_lambdas, + true, + false, + ); } - for _ in 0..params.max_stabilization_iterations { - for constraint in &mut *joint_constraints { - constraint.solve( - generic_joint_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - ); - } - - for constraint in &mut *contact_constraints { - constraint.solve(&mut self.mj_lambdas[..], true, true); - } + for constraint in &mut *contact_constraints { + constraint.solve(1.0, &mut self.mj_lambdas[..], false, true); + } - for constraint in &mut *generic_contact_constraints { - constraint.solve( - generic_contact_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - true, - true, - ); - } + for constraint in &mut *generic_contact_constraints { + constraint.solve( + 1.0, + generic_contact_jacobians, + &mut self.mj_lambdas[..], + &mut self.generic_mj_lambdas, + false, + true, + ); } + } - // Update velocities. - for handle in islands.active_island(island_id) { - if let Some(link) = multibodies.rigid_body_link(*handle).copied() { - let multibody = multibodies - .get_multibody_mut_internal(link.multibody) - .unwrap(); - - if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { - let mj_lambdas = self - .generic_mj_lambdas - .rows(multibody.solver_id, multibody.ndofs()); - multibody.velocities += mj_lambdas; - } - } else { - let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = - bodies.index_bundle(handle.0); - - let dvel = self.mj_lambdas[ids.active_set_offset]; - let dangvel = mprops - .effective_world_inv_inertia_sqrt - .transform_vector(dvel.angular); - - // let mut curr_vel_pseudo_energy = 0.0; - bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| { - // curr_vel_pseudo_energy = vels.pseudo_kinetic_energy(); - vels.linvel += dvel.linear; - vels.angvel += dangvel; - }); - - // let impulse_vel_pseudo_energy = RigidBodyVelocity { - // linvel: dvel.linear, - // angvel: dangvel, - // } - // .pseudo_kinetic_energy(); - // - // bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { - // activation.energy = - // impulse_vel_pseudo_energy.max(curr_vel_pseudo_energy / 5.0); - // }); + // Update velocities. + for handle in islands.active_island(island_id) { + if let Some(link) = multibodies.rigid_body_link(*handle).copied() { + let multibody = multibodies + .get_multibody_mut_internal(link.multibody) + .unwrap(); + + if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { + let mj_lambdas = self + .generic_mj_lambdas + .rows(multibody.solver_id, multibody.ndofs()); + multibody.velocities += mj_lambdas; } + } else { + let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = + bodies.index_bundle(handle.0); + + let dvel = self.mj_lambdas[ids.active_set_offset]; + let dangvel = mprops + .effective_world_inv_inertia_sqrt + .transform_vector(dvel.angular); + + bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| { + vels.linvel += dvel.linear; + vels.angvel += dangvel; + }); } }