nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

New contacts are bouncy, old are resting

Browse Source 
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.
This commit is contained in:
Emil Ernerfeldt
2021-02-15 21:18:26 +01:00
parent 9a15838ad5
commit f9e3d382d2
5 changed files with 49 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/dynamics/solver/velocity_constraint.rs
View File

@@ -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;

21
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -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,

14
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -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);
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
};
if rhs <= -params.restitution_velocity_threshold {
rhs += manifold_point.restitution * rhs
}
rhs += 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,

21
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -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,

16
src/geometry/contact_pair.rs
View File

@@ -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(