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

CCD improvements

Browse Source 
- Fix bug where the CCD thickness wasn’t initialized properly.
- Fix bug where the contact compliance would result in unwanted tunelling, despite CCD being enabled.
This commit is contained in:
Sébastien Crozet
2022-05-30 18:21:35 +02:00
parent c630635e57
commit 6ce26f3818
14 changed files with 130 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
CHANGELOG.md
View File

@@ -1,3 +1,11 @@
## Unreleased
### Modified
- Add a `wake_up: bool` argument to the `ImpulseJointSet::insert` and `MultibodyJointSet::insert` to
automatically wake-up the rigid-bodies attached to the inserted joint.
- The methods `ImpulseJointSet::remove/remove_joints_attached_to_rigid_body`,
`MultibodyJointSet::remove/remove_joints_attached_to_rigid_body` and
`MultibodyjointSet::remove_multibody_articulations` no longer require the `bodies`
and `islands` arguments.
## v0.12.0 (30 Apr. 2022)
### Fixed

34
src/dynamics/ccd/ccd_solver.rs
View File

@@ -60,12 +60,20 @@ impl CCDSolver {
let min_toi = (rb.ccd.ccd_thickness
* 0.15
* crate::utils::inv(rb.ccd.max_point_velocity(&rb.vels)))
* crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels)))
.min(dt);
// println!("Min toi: {}, Toi: {}", min_toi, toi);
let new_pos = rb
.vels
.integrate(toi.max(min_toi), &rb.pos.position, &local_com);
// println!(
// "Min toi: {}, Toi: {}, thick: {}, max_vel: {}",
// min_toi,
// toi,
// rb.ccd.ccd_thickness,
// rb.ccd.max_point_velocity(&rb.integrated_vels)
// );
let new_pos = rb.integrated_vels.integrate(
toi.max(min_toi),
&rb.pos.position,
&local_com,
);
rb.pos.next_position = new_pos;
}
}
@@ -95,7 +103,7 @@ impl CCDSolver {
} else {
None
};
let moving_fast = rb.ccd.is_moving_fast(dt, &rb.vels, forces);
let moving_fast = rb.ccd.is_moving_fast(dt, &rb.integrated_vels, forces);
rb.ccd.ccd_active = moving_fast;
ccd_active = ccd_active || moving_fast;
}
@@ -131,7 +139,7 @@ impl CCDSolver {
let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
dt,
&rb1.forces,
&rb1.vels,
&rb1.integrated_vels,
&rb1.mprops,
);
@@ -256,7 +264,7 @@ impl CCDSolver {
let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
dt,
&rb1.forces,
&rb1.vels,
&rb1.integrated_vels,
&rb1.mprops,
);
@@ -491,7 +499,10 @@ impl CCDSolver {
let local_com1 = &rb1.mprops.local_mprops.local_com;
let frozen1 = frozen.get(&b1);
let pos1 = frozen1
.map(|t| rb1.vels.integrate(*t, &rb1.pos.position, local_com1))
.map(|t| {
rb1.integrated_vels
.integrate(*t, &rb1.pos.position, local_com1)
})
.unwrap_or(rb1.pos.next_position);
pos1 * co_parent1.pos_wrt_parent
} else {
@@ -504,7 +515,10 @@ impl CCDSolver {
let local_com2 = &rb2.mprops.local_mprops.local_com;
let frozen2 = frozen.get(&b2);
let pos2 = frozen2
.map(|t| rb2.vels.integrate(*t, &rb2.pos.position, local_com2))
.map(|t| {
rb2.integrated_vels
.integrate(*t, &rb2.pos.position, local_com2)
})
.unwrap_or(rb2.pos.next_position);
pos2 * co_parent2.pos_wrt_parent
} else {

34
src/dynamics/ccd/toi_entry.rs
View File

@@ -56,14 +56,14 @@ impl TOIEntry {
return None;
}
let linvel1 =
frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.linvel).unwrap_or(na::zero());
let linvel2 =
frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.linvel).unwrap_or(na::zero());
let angvel1 =
frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.angvel).unwrap_or(na::zero());
let angvel2 =
frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.angvel).unwrap_or(na::zero());
let linvel1 = frozen1.is_none() as u32 as Real
* rb1.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
let linvel2 = frozen2.is_none() as u32 as Real
* rb2.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
let angvel1 = frozen1.is_none() as u32 as Real
* rb1.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
let angvel2 = frozen2.is_none() as u32 as Real
* rb2.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
#[cfg(feature = "dim2")]
let vel12 = (linvel2 - linvel1).norm()
@@ -114,6 +114,20 @@ impl TOIEntry {
// because the colliders may be in a separating trajectory.
let stop_at_penetration = is_pseudo_intersection_test;
// let pos12 = motion_c1
// .position_at_time(start_time)
// .inv_mul(&motion_c2.position_at_time(start_time));
// let vel12 = linvel2 - linvel1;
// let res_toi = query_dispatcher
// .time_of_impact(
// &pos12,
// &vel12,
// co1.shape.as_ref(),
// co2.shape.as_ref(),
// end_time - start_time,
// )
// .ok();
let res_toi = query_dispatcher
.nonlinear_time_of_impact(
&motion_c1,
@@ -144,8 +158,8 @@ impl TOIEntry {
NonlinearRigidMotion::new(
rb.pos.position,
rb.mprops.local_mprops.local_com,
rb.vels.linvel,
rb.vels.angvel,
rb.integrated_vels.linvel,
rb.integrated_vels.angvel,
)
} else {
NonlinearRigidMotion::constant_position(rb.pos.next_position)

5
src/dynamics/rigid_body.rs
View File

@@ -19,6 +19,10 @@ use num::Zero;
pub struct RigidBody {
pub(crate) pos: RigidBodyPosition,
pub(crate) mprops: RigidBodyMassProps,
// NOTE: we need this so that the CCD can use the actual velocities obtained
// by the velocity solver with bias. If we switch to intepolation, we
// should remove this field.
pub(crate) integrated_vels: RigidBodyVelocity,
pub(crate) vels: RigidBodyVelocity,
pub(crate) damping: RigidBodyDamping,
pub(crate) forces: RigidBodyForces,
@@ -47,6 +51,7 @@ impl RigidBody {
Self {
pos: RigidBodyPosition::default(),
mprops: RigidBodyMassProps::default(),
integrated_vels: RigidBodyVelocity::default(),
vels: RigidBodyVelocity::default(),
damping: RigidBodyDamping::default(),
forces: RigidBodyForces::default(),

2
src/dynamics/rigid_body_components.rs
View File

@@ -752,7 +752,7 @@ pub struct RigidBodyCcd {
impl Default for RigidBodyCcd {
fn default() -> Self {
Self {
ccd_thickness: 0.0,
ccd_thickness: Real::MAX,
ccd_max_dist: 0.0,
ccd_active: false,
ccd_enabled: false,

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

@@ -34,6 +34,7 @@ impl GenericVelocityConstraint {
jacobian_id: &mut usize,
insert_at: Option<usize>,
) {
let cfm_factor = params.cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
@@ -45,6 +46,7 @@ impl GenericVelocityConstraint {
let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type);
let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type);
let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
let multibody1 = multibodies
.rigid_body_link(handle1)
@@ -196,13 +198,18 @@ impl GenericVelocityConstraint {
let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0);
let rhs = rhs_wo_bias + rhs_bias;
let is_fast_contact = -rhs * params.dt > ccd_thickness * 0.5;
let cfm = if is_fast_contact { 1.0 } else { cfm_factor };
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
gcross1,
gcross2,
rhs: rhs_wo_bias + rhs_bias,
rhs,
rhs_wo_bias,
impulse: na::zero(),
r,
cfm,
};
}

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

@@ -32,6 +32,7 @@ impl GenericVelocityGroundConstraint {
jacobian_id: &mut usize,
insert_at: Option<usize>,
) {
let cfm_factor = params.cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
@@ -130,19 +131,24 @@ impl GenericVelocityGroundConstraint {
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
let dvel = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel;
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.clamp(-params.max_penetration_correction, 0.0);
let rhs = rhs_wo_bias + rhs_bias;
let is_fast_contact = -rhs * params.dt > rb2.ccd.ccd_thickness * 0.5;
let cfm = if is_fast_contact { 1.0 } else { cfm_factor };
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2: na::zero(), // Unused for generic constraints.
rhs: rhs_wo_bias + rhs_bias,
rhs,
rhs_wo_bias,
impulse: na::zero(),
r,
cfm,
};
}

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

@@ -153,6 +153,7 @@ impl VelocityConstraint {
) {
assert_eq!(manifold.data.relative_dominance, 0);
let cfm_factor = params.cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
@@ -163,6 +164,7 @@ impl VelocityConstraint {
let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
let rb2 = &bodies[handle2];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
let mj_lambda1 = rb1.ids.active_set_offset;
let mj_lambda2 = rb2.ids.active_set_offset;
@@ -280,13 +282,19 @@ impl VelocityConstraint {
let rhs_bias = /* is_resting
* */ erp_inv_dt
* (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0);
let rhs = rhs_wo_bias + rhs_bias;
let is_fast_contact = -rhs * params.dt > ccd_thickness * 0.5;
let cfm = if is_fast_contact { 1.0 } else { cfm_factor };
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
gcross1,
gcross2,
rhs: rhs_wo_bias + rhs_bias,
rhs,
rhs_wo_bias,
impulse: na::zero(),
r: projected_mass,
cfm,
};
}

4
src/dynamics/solver/velocity_constraint_element.rs
View File

@@ -112,6 +112,7 @@ pub(crate) struct VelocityConstraintNormalPart<N: WReal> {
pub rhs_wo_bias: N,
pub impulse: N,
pub r: N,
pub cfm: N,
}
impl<N: WReal> VelocityConstraintNormalPart<N> {
@@ -123,6 +124,7 @@ impl<N: WReal> VelocityConstraintNormalPart<N> {
rhs_wo_bias: na::zero(),
impulse: na::zero(),
r: na::zero(),
cfm: na::one(),
}
}
@@ -142,7 +144,7 @@ impl<N: WReal> VelocityConstraintNormalPart<N> {
- dir1.dot(&mj_lambda2.linear)
+ self.gcross2.gdot(mj_lambda2.angular)
+ self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let new_impulse = self.cfm * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;

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

@@ -43,6 +43,8 @@ impl WVelocityConstraint {
assert_eq!(manifolds[ii].data.relative_dominance, 0);
}
let cfm_factor = SimdReal::splat(params.cfm_factor());
let dt = SimdReal::splat(params.dt);
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());
@@ -58,6 +60,10 @@ impl WVelocityConstraint {
let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops];
let ccd_thickness1 = SimdReal::from(gather![|ii| bodies[handles1[ii]].ccd.ccd_thickness]);
let ccd_thickness2 = SimdReal::from(gather![|ii| bodies[handles2[ii]].ccd.ccd_thickness]);
let ccd_thickness = ccd_thickness1 + ccd_thickness2;
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1: AngularInertia<SimdReal> =
@@ -147,6 +153,10 @@ impl WVelocityConstraint {
.simd_clamp(-max_penetration_correction, SimdReal::zero())
* (erp_inv_dt/* * is_resting */);
let rhs = rhs_wo_bias + rhs_bias;
let is_fast_contact = (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
let cfm = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
gcross1,
gcross2,
@@ -154,6 +164,7 @@ impl WVelocityConstraint {
rhs_wo_bias,
impulse: SimdReal::splat(0.0),
r: projected_mass,
cfm,
};
}

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

@@ -34,6 +34,7 @@ impl VelocityGroundConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
) {
let cfm_factor = params.cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
@@ -156,20 +157,25 @@ impl VelocityGroundConstraint {
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
let dvel = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel;
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 + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0);
let rhs = rhs_wo_bias + rhs_bias;
let is_fast_contact = -rhs * params.dt > rb2.ccd.ccd_thickness * 0.5;
let cfm = if is_fast_contact { 1.0 } else { cfm_factor };
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2,
rhs: rhs_wo_bias + rhs_bias,
rhs,
rhs_wo_bias,
impulse: na::zero(),
r: projected_mass,
cfm,
};
}

4
src/dynamics/solver/velocity_ground_constraint_element.rs
View File

@@ -93,6 +93,7 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: WReal> {
pub rhs_wo_bias: N,
pub impulse: N,
pub r: N,
pub cfm: N,
}
impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
@@ -103,6 +104,7 @@ impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
rhs_wo_bias: na::zero(),
impulse: na::zero(),
r: na::zero(),
cfm: na::one(),
}
}
@@ -117,7 +119,7 @@ impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let new_impulse = self.cfm * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;

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

@@ -9,6 +9,7 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
};
use crate::prelude::RigidBody;
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{self, WAngularInertia, WCross, WDot};
@@ -38,6 +39,8 @@ impl WVelocityGroundConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
) {
let cfm_factor = SimdReal::splat(params.cfm_factor());
let dt = SimdReal::splat(params.dt);
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());
@@ -65,11 +68,12 @@ impl WVelocityGroundConstraint {
.unwrap_or_else(Point::origin)
}]);
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] =
gather![|ii| &bodies[handles2[ii].unwrap()].vels];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii].unwrap()].ids];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
gather![|ii| &bodies[handles2[ii].unwrap()].mprops];
let bodies2 = gather![|ii| &bodies[handles2[ii].unwrap()]];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies2[ii].vels];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies2[ii].ids];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies2[ii].mprops];
let ccd_thickness = SimdReal::from(gather![|ii| bodies2[ii].ccd.ccd_thickness]);
let flipped_sign = SimdReal::from(flipped);
@@ -152,12 +156,17 @@ impl WVelocityGroundConstraint {
.simd_clamp(-max_penetration_correction, SimdReal::zero())
* (erp_inv_dt/* * is_resting */);
let rhs = rhs_wo_bias + rhs_bias;
let is_fast_contact = (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
let cfm = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2,
rhs: rhs_wo_bias + rhs_bias,
rhs,
rhs_wo_bias,
impulse: na::zero(),
r: projected_mass,
cfm,
};
}

1
src/dynamics/solver/velocity_solver.rs
View File

@@ -168,6 +168,7 @@ impl VelocitySolver {
&rb.pos.position,
&rb.mprops.local_mprops.local_com,
);
rb.integrated_vels = new_vels;
rb.pos = new_pos;
}
}