aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/dynamics/solver/velocity_constraint.rs12
-rw-r--r--src/dynamics/solver/velocity_constraint_wide.rs21
-rw-r--r--src/dynamics/solver/velocity_ground_constraint.rs14
-rw-r--r--src/dynamics/solver/velocity_ground_constraint_wide.rs21
-rw-r--r--src/geometry/contact_pair.rs16
5 files changed, 49 insertions, 35 deletions
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(
generated by cgit (git 2.34.1) at 2026-02-03 18:58:18 +0000