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

Always apply the predictive contact term, even for bouncing contacts

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-18 17:58:17 +01:00
parent 287cd46295
commit 5e7eedc3fd
4 changed files with 22 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

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

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

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

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