From f9e3d382d2ee097dce05997735982f5e120cca03 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Mon, 15 Feb 2021 21:18:26 +0100 Subject: New contacts are bouncy, old are resting If a contact is new (previous impluse = 0), then we treat it as bouncy (respecting restitution). If the contact is old we treat it as resting. Exceptions for restitutions <=0 and >= 1. --- src/dynamics/solver/velocity_constraint.rs | 12 +++++------- src/dynamics/solver/velocity_constraint_wide.rs | 21 +++++++++++---------- src/dynamics/solver/velocity_ground_constraint.rs | 14 ++++++-------- .../solver/velocity_ground_constraint_wide.rs | 21 +++++++++++---------- src/geometry/contact_pair.rs | 16 ++++++++++++++++ 5 files changed, 49 insertions(+), 35 deletions(-) (limited to 'src') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 514434b..dc7020f 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -239,13 +239,11 @@ impl VelocityConstraint { + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); - let mut rhs = (vel1 - vel2).dot(&force_dir1); - - if rhs <= -params.restitution_velocity_threshold { - rhs += manifold_point.restitution * rhs - } - - rhs += manifold_point.dist.max(0.0) * inv_dt; + let rhs = if manifold_point.is_bouncy() { + (1.0 + manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1) + } else { + (vel1 - vel2).dot(&force_dir1) + manifold_point.dist.max(0.0) * inv_dt + }; let impulse = manifold_point.data.impulse * warmstart_coeff; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index a8384de..bd7fdcf 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -2,7 +2,8 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, + AngVector, AngularInertia, Point, Real, SimdBool, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, + SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -96,8 +97,6 @@ impl WVelocityConstraint { let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); - let warmstart_multiplier = SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); @@ -125,8 +124,11 @@ impl WVelocityConstraint { for k in 0..num_points { let friction = SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]); - let restitution = - SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]); + let restitution_plus_1 = SimdReal::from( + array![|ii| manifold_points[ii][k].restitution + 1.0; SIMD_WIDTH], + ); + let is_bouncy = + SimdBool::from(array![|ii| manifold_points[ii][k].is_bouncy(); SIMD_WIDTH]); let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); @@ -148,11 +150,10 @@ impl WVelocityConstraint { let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); - let mut rhs = (vel1 - vel2).dot(&force_dir1); - let use_restitution = rhs.simd_le(-restitution_velocity_threshold); - let rhs_with_restitution = rhs + rhs * restitution; - rhs = rhs_with_restitution.select(use_restitution, rhs); - rhs += dist.simd_max(SimdReal::zero()) * inv_dt; + let projected_velocity = (vel1 - vel2).dot(&force_dir1); + let rhs_resting = projected_velocity + dist.simd_max(SimdReal::zero()) * inv_dt; + let rhs_bouncing = projected_velocity * restitution_plus_1; + let rhs = rhs_bouncing.select(is_bouncy, rhs_resting); constraint.elements[k].normal_part = WVelocityConstraintElementPart { gcross1, diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 20642ec..f147ccb 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -154,15 +154,13 @@ impl VelocityGroundConstraint { let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); - let mut rhs = (vel1 - vel2).dot(&force_dir1); - - if rhs <= -params.restitution_velocity_threshold { - rhs += manifold_point.restitution * rhs - } - - rhs += manifold_point.dist.max(0.0) * inv_dt; + let rhs = if manifold_point.is_bouncy() { + (1.0 + manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1) + } else { + (vel1 - vel2).dot(&force_dir1) + manifold_point.dist.max(0.0) * inv_dt + }; - let impulse = manifold_points[k].data.impulse * warmstart_coeff; + let impulse = manifold_point.data.impulse * warmstart_coeff; constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { gcross2, diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index c2d2c4f..4a58c69 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -2,7 +2,8 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, + AngVector, AngularInertia, Point, Real, SimdBool, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, + SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -95,8 +96,6 @@ impl WVelocityGroundConstraint { let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); - let warmstart_multiplier = SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); @@ -120,8 +119,11 @@ impl WVelocityGroundConstraint { for k in 0..num_points { let friction = SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]); - let restitution = - SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]); + let restitution_plus_1 = SimdReal::from( + array![|ii| manifold_points[ii][k].restitution + 1.0; SIMD_WIDTH], + ); + let is_bouncy = + SimdBool::from(array![|ii| manifold_points[ii][k].is_bouncy(); SIMD_WIDTH]); let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); @@ -140,11 +142,10 @@ impl WVelocityGroundConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); - let mut rhs = (vel1 - vel2).dot(&force_dir1); - let use_restitution = rhs.simd_le(-restitution_velocity_threshold); - let rhs_with_restitution = rhs + rhs * restitution; - rhs = rhs_with_restitution.select(use_restitution, rhs); - rhs += dist.simd_max(SimdReal::zero()) * inv_dt; + let projected_velocity = (vel1 - vel2).dot(&force_dir1); + let rhs_resting = projected_velocity + dist.simd_max(SimdReal::zero()) * inv_dt; + let rhs_bouncing = projected_velocity * restitution_plus_1; + let rhs = rhs_bouncing.select(is_bouncy, rhs_resting); constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart { gcross2, diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 47f6678..462d3ef 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -128,6 +128,22 @@ pub struct SolverContact { pub data: ContactData, } +impl SolverContact { + /// Should we treat this contact as a bouncy contact? + /// If `true`, use [`Self::restitution`]. + pub fn is_bouncy(&self) -> bool { + let is_new = self.data.impulse == 0.0; + if is_new { + // Treat new collisions as bouncing at first, unless we have zero restitution. + self.restitution > 0.0 + } else { + // If the contact is still here one step later, it is now a resting contact. + // The exception is very high restitutions, which can never rest + self.restitution >= 1.0 + } + } +} + impl Default for ContactManifoldData { fn default() -> Self { Self::new( -- cgit From 287cd4629524d180651a3953ed95a18e1be7f49b Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Mon, 15 Feb 2021 21:18:44 +0100 Subject: Remove unused restitution_velocity_threshold parameter --- src/dynamics/integration_parameters.rs | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) (limited to 'src') diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 628e07a..5d0d221 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -27,9 +27,6 @@ pub struct IntegrationParameters { /// Each cached impulse are multiplied by this coefficient in `[0, 1]` /// when they are re-used to initialize the solver (default `1.0`). pub warmstart_coeff: Real, - /// Contacts at points where the involved bodies have a relative - /// velocity smaller than this threshold wont be affected by the restitution force (default: `1.0`). - pub restitution_velocity_threshold: Real, /// Amount of penetration the engine wont attempt to correct (default: `0.005m`). pub allowed_linear_error: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). @@ -95,7 +92,7 @@ impl IntegrationParameters { erp: Real, joint_erp: Real, warmstart_coeff: Real, - restitution_velocity_threshold: Real, + _restitution_velocity_threshold: Real, allowed_linear_error: Real, allowed_angular_error: Real, max_linear_correction: Real, @@ -116,7 +113,6 @@ impl IntegrationParameters { erp, joint_erp, warmstart_coeff, - restitution_velocity_threshold, allowed_linear_error, allowed_angular_error, max_linear_correction, @@ -188,7 +184,6 @@ impl Default for IntegrationParameters { erp: 0.2, joint_erp: 0.2, warmstart_coeff: 1.0, - restitution_velocity_threshold: 1.0, allowed_linear_error: 0.005, prediction_distance: 0.002, allowed_angular_error: 0.001, -- cgit From 5e7eedc3fdf74b49ecd999b003570bf352ee3f11 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Thu, 18 Feb 2021 17:58:17 +0100 Subject: Always apply the predictive contact term, even for bouncing contacts --- src/dynamics/solver/velocity_constraint.rs | 9 ++++----- src/dynamics/solver/velocity_constraint_wide.rs | 16 +++++++--------- src/dynamics/solver/velocity_ground_constraint.rs | 9 ++++----- src/dynamics/solver/velocity_ground_constraint_wide.rs | 16 +++++++--------- 4 files changed, 22 insertions(+), 28 deletions(-) (limited to 'src') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index dc7020f..2cac93d 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -239,11 +239,10 @@ impl VelocityConstraint { + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); - let rhs = if manifold_point.is_bouncy() { - (1.0 + manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1) - } else { - (vel1 - vel2).dot(&force_dir1) + manifold_point.dist.max(0.0) * inv_dt - }; + let is_bouncy = manifold_point.is_bouncy() as u32 as Real; + let rhs = (1.0 + is_bouncy * manifold_point.restitution) + * (vel1 - vel2).dot(&force_dir1) + + manifold_point.dist.max(0.0) * inv_dt; let impulse = manifold_point.data.impulse * warmstart_coeff; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index bd7fdcf..3e068e7 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -2,8 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Point, Real, SimdBool, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -124,11 +123,11 @@ impl WVelocityConstraint { for k in 0..num_points { let friction = SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]); - let restitution_plus_1 = SimdReal::from( - array![|ii| manifold_points[ii][k].restitution + 1.0; SIMD_WIDTH], + let restitution = + SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]); + let is_bouncy = SimdReal::from( + array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH], ); - let is_bouncy = - SimdBool::from(array![|ii| manifold_points[ii][k].is_bouncy(); SIMD_WIDTH]); let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); @@ -151,9 +150,8 @@ impl WVelocityConstraint { let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let projected_velocity = (vel1 - vel2).dot(&force_dir1); - let rhs_resting = projected_velocity + dist.simd_max(SimdReal::zero()) * inv_dt; - let rhs_bouncing = projected_velocity * restitution_plus_1; - let rhs = rhs_bouncing.select(is_bouncy, rhs_resting); + let rhs = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity + + dist.simd_max(SimdReal::zero()) * inv_dt; constraint.elements[k].normal_part = WVelocityConstraintElementPart { gcross1, diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index f147ccb..88f864a 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -154,11 +154,10 @@ impl VelocityGroundConstraint { let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); - let rhs = if manifold_point.is_bouncy() { - (1.0 + manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1) - } else { - (vel1 - vel2).dot(&force_dir1) + manifold_point.dist.max(0.0) * inv_dt - }; + let is_bouncy = manifold_point.is_bouncy() as u32 as Real; + let rhs = (1.0 + is_bouncy * manifold_point.restitution) + * (vel1 - vel2).dot(&force_dir1) + + manifold_point.dist.max(0.0) * inv_dt; let impulse = manifold_point.data.impulse * warmstart_coeff; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 4a58c69..29ba5e9 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -2,8 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ - AngVector, AngularInertia, Point, Real, SimdBool, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; use num::Zero; @@ -119,11 +118,11 @@ impl WVelocityGroundConstraint { for k in 0..num_points { let friction = SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]); - let restitution_plus_1 = SimdReal::from( - array![|ii| manifold_points[ii][k].restitution + 1.0; SIMD_WIDTH], + let restitution = + SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]); + let is_bouncy = SimdReal::from( + array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH], ); - let is_bouncy = - SimdBool::from(array![|ii| manifold_points[ii][k].is_bouncy(); SIMD_WIDTH]); let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); @@ -143,9 +142,8 @@ impl WVelocityGroundConstraint { let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); let projected_velocity = (vel1 - vel2).dot(&force_dir1); - let rhs_resting = projected_velocity + dist.simd_max(SimdReal::zero()) * inv_dt; - let rhs_bouncing = projected_velocity * restitution_plus_1; - let rhs = rhs_bouncing.select(is_bouncy, rhs_resting); + let rhs = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity + + dist.simd_max(SimdReal::zero()) * inv_dt; constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart { gcross2, -- cgit