diff options
Diffstat (limited to 'src/dynamics')
7 files changed, 13 insertions, 6 deletions
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index bab0fe2..e66f72d 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -36,6 +36,8 @@ pub struct IntegrationParameters { /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). pub allowed_linear_error: Real, + /// Maximum amount of penetration the solver will attempt to resolve in one timestep. + pub max_penetration_correction: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). pub prediction_distance: Real, /// Maximum number of iterations performed to solve non-penetration and joint constraints (default: `4`). @@ -150,6 +152,7 @@ impl Default for IntegrationParameters { joint_erp: 1.0, joint_damping_ratio: 1.0, allowed_linear_error: 0.001, // 0.005 + max_penetration_correction: Real::MAX, prediction_distance: 0.002, max_velocity_iterations: 4, max_velocity_friction_iterations: 8, 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 { |