diff options
Diffstat (limited to 'src/dynamics/solver')
6 files changed, 10 insertions, 6 deletions
diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index 261c4c8..de09a21 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -212,7 +212,7 @@ impl GenericVelocityConstraint { rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = - /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0); + /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0); constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index 044bd48..76e2e51 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -148,7 +148,7 @@ impl GenericVelocityGroundConstraint { rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = - /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0); + /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0); constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2: na::zero(), // Unused for generic constraints. diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 50c2625..dc079d4 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -283,7 +283,7 @@ impl VelocityConstraint { rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = /* is_resting * */ erp_inv_dt - * (manifold_point.dist + params.allowed_linear_error).min(0.0); + * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0); constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 1fb9397..0c85755 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -49,6 +49,7 @@ impl WVelocityConstraint { let inv_dt = SimdReal::splat(params.inv_dt()); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); + let max_penetration_correction = SimdReal::splat(params.max_penetration_correction); let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()]; @@ -145,7 +146,8 @@ impl WVelocityConstraint { (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting; - let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero()) + let rhs_bias = (dist + allowed_lin_err) + .simd_clamp(-max_penetration_correction, SimdReal::zero()) * (erp_inv_dt/* * is_resting */); constraint.elements[k].normal_part = VelocityConstraintNormalPart { diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 04acc88..93da6dc 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -167,7 +167,7 @@ impl VelocityGroundConstraint { rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = /* is_resting * */ erp_inv_dt - * (manifold_point.dist + params.allowed_linear_error).min(0.0); + * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0); constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2, diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 908e55f..1889901 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -44,6 +44,7 @@ impl WVelocityGroundConstraint { let inv_dt = SimdReal::splat(params.inv_dt()); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); + let max_penetration_correction = SimdReal::splat(params.max_penetration_correction); let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1]; let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2]; @@ -150,7 +151,8 @@ impl WVelocityGroundConstraint { (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting; - let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero()) + let rhs_bias = (dist + allowed_lin_err) + .simd_clamp(-max_penetration_correction, SimdReal::zero()) * (erp_inv_dt/* * is_resting */); constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { |