aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/dynamics/ccd/toi_entry.rs39
-rw-r--r--src/dynamics/integration_parameters.rs3
-rw-r--r--src/dynamics/rigid_body.rs14
-rw-r--r--src/dynamics/solver/velocity_constraint.rs17
-rw-r--r--src/dynamics/solver/velocity_constraint_wide.rs27
-rw-r--r--src/dynamics/solver/velocity_ground_constraint.rs16
-rw-r--r--src/dynamics/solver/velocity_ground_constraint_wide.rs26
-rw-r--r--src/geometry/contact_pair.rs20
-rw-r--r--src/geometry/narrow_phase.rs4
9 files changed, 112 insertions, 54 deletions
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index 20f5268..6679a91 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -1,13 +1,7 @@
-use crate::data::Coarena;
-use crate::dynamics::ccd::ccd_solver::CCDContact;
-use crate::dynamics::ccd::CCDData;
use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle};
use crate::geometry::{Collider, ColliderHandle};
-use crate::math::{Isometry, Real};
-use crate::parry::query::PersistentQueryDispatcher;
-use crate::utils::WCross;
-use na::{RealField, Unit};
-use parry::query::{NonlinearRigidMotion, QueryDispatcher, TOI};
+use crate::math::Real;
+use parry::query::{NonlinearRigidMotion, QueryDispatcher};
#[derive(Copy, Clone, Debug)]
pub struct TOIEntry {
@@ -41,7 +35,7 @@ impl TOIEntry {
}
}
- pub fn try_from_colliders<QD: ?Sized + PersistentQueryDispatcher<(), ()>>(
+ pub fn try_from_colliders<QD: ?Sized + QueryDispatcher>(
params: &IntegrationParameters,
query_dispatcher: &QD,
ch1: ColliderHandle,
@@ -54,7 +48,6 @@ impl TOIEntry {
frozen2: Option<Real>,
start_time: Real,
end_time: Real,
- body_params: &Coarena<CCDData>,
) -> Option<Self> {
assert!(start_time <= end_time);
@@ -62,7 +55,7 @@ impl TOIEntry {
let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel;
let vel12 = linvel2 - linvel1;
- let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness());
+ let thickness = c1.shape().ccd_thickness() + c2.shape().ccd_thickness();
if params.dt * vel12.norm() < thickness {
return None;
@@ -70,12 +63,9 @@ impl TOIEntry {
let is_intersection_test = c1.is_sensor() || c2.is_sensor();
- let body_params1 = body_params.get(c1.parent.0)?;
- let body_params2 = body_params.get(c2.parent.0)?;
-
// Compute the TOI.
- let mut motion1 = body_params1.motion(params.dt, b1, 0.0);
- let mut motion2 = body_params2.motion(params.dt, b2, 0.0);
+ let mut motion1 = Self::body_motion(params.dt, b1);
+ let mut motion2 = Self::body_motion(params.dt, b2);
if let Some(t) = frozen1 {
motion1.freeze(t);
@@ -85,7 +75,6 @@ impl TOIEntry {
motion2.freeze(t);
}
- let mut toi;
let motion_c1 = motion1.prepend(*c1.position_wrt_parent());
let motion_c2 = motion2.prepend(*c2.position_wrt_parent());
@@ -112,7 +101,7 @@ impl TOIEntry {
)
.ok();
- toi = res_toi??;
+ let toi = res_toi??;
Some(Self::new(
toi.toi,
@@ -124,6 +113,20 @@ impl TOIEntry {
0,
))
}
+
+ fn body_motion(dt: Real, body: &RigidBody) -> NonlinearRigidMotion {
+ if body.should_resolve_ccd(dt) {
+ NonlinearRigidMotion::new(
+ 0.0,
+ body.position,
+ body.mass_properties.local_com,
+ body.linvel,
+ body.angvel,
+ )
+ } else {
+ NonlinearRigidMotion::constant_position(body.next_position)
+ }
+ }
}
impl PartialOrd for TOIEntry {
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 8c0f26c..136e345 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -27,6 +27,8 @@ 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,
+ /// Correction factor to avoid large warmstart impulse after a strong impact.
+ pub warmstart_correction_slope: Real,
/// 0-1: how much of the velocity to dampen out in the constraint solver?
/// (default `1.0`).
@@ -200,6 +202,7 @@ impl Default for IntegrationParameters {
velocity_solve_fraction: 1.0,
velocity_based_erp: 0.0,
warmstart_coeff: 1.0,
+ warmstart_correction_slope: 1.0,
allowed_linear_error: 0.005,
prediction_distance: 0.002,
allowed_angular_error: 0.001,
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 9ac4763..e557df0 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -204,8 +204,12 @@ impl RigidBody {
self.flags.contains(RigidBodyFlags::CCD_ENABLED)
}
+ pub fn is_moving_fast(&self, dt: Real) -> bool {
+ self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
+ }
+
pub(crate) fn should_resolve_ccd(&self, dt: Real) -> bool {
- self.is_ccd_enabled() && self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
+ self.is_ccd_enabled() && self.is_moving_fast(dt)
}
/// Sets the rigid-body's mass properties.
@@ -371,10 +375,6 @@ impl RigidBody {
shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
}
- pub(crate) fn position_at_time(&self, dt: Real) -> Isometry<Real> {
- self.integrate_velocity(dt) * self.position
- }
-
pub(crate) fn integrate_next_position(&mut self, dt: Real, apply_damping: bool) {
// TODO: do we want to apply damping before or after the velocity integration?
if apply_damping {
@@ -504,10 +504,6 @@ impl RigidBody {
self.linvel = dpos.translation.vector * inv_dt;
}
- pub(crate) fn update_next_position(&mut self, dt: Real) {
- self.next_position = self.integrate_velocity(dt) * self.position;
- }
-
pub(crate) fn update_world_mass_properties(&mut self) {
self.world_com = self.mass_properties.world_com(&self.position);
self.effective_inv_mass = self.mass_properties.inv_mass;
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 3e8cb61..c339ce4 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -208,6 +208,8 @@ impl VelocityConstraint {
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
+ let warmstart_correction;
+
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
@@ -234,12 +236,15 @@ impl VelocityConstraint {
rhs += manifold_point.dist.max(0.0) * inv_dt;
rhs *= is_bouncy + is_resting * params.velocity_solve_fraction;
rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0);
+ warmstart_correction = (params.warmstart_correction_slope
+ / (rhs - manifold_point.prev_rhs).abs())
+ .min(warmstart_coeff);
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
gcross1,
gcross2,
rhs,
- impulse: manifold_point.data.impulse * warmstart_coeff,
+ impulse: manifold_point.warmstart_impulse * warmstart_correction,
r,
};
}
@@ -247,10 +252,12 @@ impl VelocityConstraint {
// Tangent parts.
{
#[cfg(feature = "dim3")]
- let impulse =
- tangent_rot1 * manifold_points[k].data.tangent_impulse * warmstart_coeff;
+ let impulse = tangent_rot1
+ * manifold_points[k].warmstart_tangent_impulse
+ * warmstart_correction;
#[cfg(feature = "dim2")]
- let impulse = [manifold_points[k].data.tangent_impulse * warmstart_coeff];
+ let impulse =
+ [manifold_points[k].warmstart_tangent_impulse * warmstart_correction];
constraint.elements[k].tangent_part.impulse = impulse;
for j in 0..DIM - 1 {
@@ -332,6 +339,8 @@ impl VelocityConstraint {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
+ active_contact.data.rhs = self.elements[k].normal_part.rhs;
+
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 673af54..7d5f468 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -9,6 +9,7 @@ use crate::math::{
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{WAngularInertia, WCross, WDot};
+use na::SimdComplexField;
use num::Zero;
use simba::simd::{SimdPartialOrd, SimdValue};
@@ -44,6 +45,7 @@ impl WVelocityConstraint {
}
let inv_dt = SimdReal::splat(params.inv_dt());
+ let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope);
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
@@ -123,8 +125,11 @@ impl WVelocityConstraint {
let tangent_velocity =
Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
- let impulse =
- SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
+ let impulse = SimdReal::from(
+ array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH],
+ );
+ let prev_rhs =
+ SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]);
let dp1 = point - world_com1;
let dp2 = point - world_com2;
@@ -132,6 +137,8 @@ impl WVelocityConstraint {
let vel1 = linvel1 + angvel1.gcross(dp1);
let vel2 = linvel2 + angvel2.gcross(dp2);
+ let warmstart_correction;
+
constraint.limit = friction;
constraint.manifold_contact_id[k] =
array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
@@ -150,12 +157,15 @@ impl WVelocityConstraint {
rhs *= is_bouncy + is_resting * velocity_solve_fraction;
rhs +=
dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting);
+ warmstart_correction = (warmstart_correction_slope
+ / (rhs - prev_rhs).simd_abs())
+ .simd_min(warmstart_coeff);
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
gcross1,
gcross2,
rhs,
- impulse: impulse * warmstart_coeff,
+ impulse: impulse * warmstart_correction,
r,
};
}
@@ -163,14 +173,15 @@ impl WVelocityConstraint {
// tangent parts.
#[cfg(feature = "dim2")]
let impulse = [SimdReal::from(
- array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
- )];
+ array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
+ ) * warmstart_correction];
#[cfg(feature = "dim3")]
let impulse = tangent_rot1
* na::Vector2::from(
- array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
- );
+ array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
+ )
+ * warmstart_correction;
constraint.elements[k].tangent_part.impulse = impulse;
@@ -281,6 +292,7 @@ impl WVelocityConstraint {
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
for k in 0..self.num_contacts as usize {
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
+ let rhs: [_; SIMD_WIDTH] = self.elements[k].normal_part.rhs.into();
#[cfg(feature = "dim2")]
let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
#[cfg(feature = "dim3")]
@@ -292,6 +304,7 @@ impl WVelocityConstraint {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
+ active_contact.data.rhs = rhs[ii];
active_contact.data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index b9c5236..0e195d5 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -133,6 +133,7 @@ impl VelocityGroundConstraint {
let dp1 = manifold_point.point - rb1.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
+ let warmstart_correction;
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
@@ -153,11 +154,14 @@ impl VelocityGroundConstraint {
rhs += manifold_point.dist.max(0.0) * inv_dt;
rhs *= is_bouncy + is_resting * params.velocity_solve_fraction;
rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0);
+ warmstart_correction = (params.warmstart_correction_slope
+ / (rhs - manifold_point.prev_rhs).abs())
+ .min(warmstart_coeff);
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2,
rhs,
- impulse: manifold_point.data.impulse * warmstart_coeff,
+ impulse: manifold_point.warmstart_impulse * warmstart_correction,
r,
};
}
@@ -165,10 +169,12 @@ impl VelocityGroundConstraint {
// Tangent parts.
{
#[cfg(feature = "dim3")]
- let impulse =
- tangent_rot1 * manifold_points[k].data.tangent_impulse * warmstart_coeff;
+ let impulse = tangent_rot1
+ * manifold_points[k].warmstart_tangent_impulse
+ * warmstart_correction;
#[cfg(feature = "dim2")]
- let impulse = [manifold_points[k].data.tangent_impulse * warmstart_coeff];
+ let impulse =
+ [manifold_points[k].warmstart_tangent_impulse * warmstart_correction];
constraint.elements[k].tangent_part.impulse = impulse;
for j in 0..DIM - 1 {
@@ -237,6 +243,8 @@ impl VelocityGroundConstraint {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
+ active_contact.data.rhs = self.elements[k].normal_part.rhs;
+
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index ba1c46a..4237d99 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -10,6 +10,7 @@ use crate::math::{
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{WAngularInertia, WCross, WDot};
+use na::SimdComplexField;
use num::Zero;
use simba::simd::{SimdPartialOrd, SimdValue};
@@ -77,6 +78,7 @@ impl WVelocityGroundConstraint {
let warmstart_multiplier =
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
+ let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope);
let num_active_contacts = manifolds[0].data.num_active_contacts();
#[cfg(feature = "dim2")]
@@ -118,13 +120,17 @@ impl WVelocityGroundConstraint {
let tangent_velocity =
Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
- let impulse =
- SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
+ let impulse = SimdReal::from(
+ array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH],
+ );
+ let prev_rhs =
+ SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]);
let dp1 = point - world_com1;
let dp2 = point - world_com2;
let vel1 = linvel1 + angvel1.gcross(dp1);
let vel2 = linvel2 + angvel2.gcross(dp2);
+ let warmstart_correction;
constraint.limit = friction;
constraint.manifold_contact_id[k] =
@@ -142,11 +148,14 @@ impl WVelocityGroundConstraint {
rhs *= is_bouncy + is_resting * velocity_solve_fraction;
rhs +=
dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting);
+ warmstart_correction = (warmstart_correction_slope
+ / (rhs - prev_rhs).simd_abs())
+ .simd_min(warmstart_coeff);
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2,
rhs,
- impulse: impulse * warmstart_coeff,
+ impulse: impulse * warmstart_correction,
r,
};
}
@@ -154,13 +163,14 @@ impl WVelocityGroundConstraint {
// tangent parts.
#[cfg(feature = "dim2")]
let impulse = [SimdReal::from(
- array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
- )];
+ array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
+ ) * warmstart_correction];
#[cfg(feature = "dim3")]
let impulse = tangent_rot1
* na::Vector2::from(
- array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
- );
+ array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
+ )
+ * warmstart_correction;
constraint.elements[k].tangent_part.impulse = impulse;
for j in 0..DIM - 1 {
@@ -237,6 +247,7 @@ impl WVelocityGroundConstraint {
// FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
for k in 0..self.num_contacts as usize {
+ let rhs: [_; SIMD_WIDTH] = self.elements[k].normal_part.rhs.into();
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
#[cfg(feature = "dim2")]
let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
@@ -249,6 +260,7 @@ impl WVelocityGroundConstraint {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
+ active_contact.data.rhs = rhs[ii];
active_contact.data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index f156db5..ffd5d7f 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -38,6 +38,8 @@ pub struct ContactData {
/// collider's rigid-body.
#[cfg(feature = "dim3")]
pub tangent_impulse: na::Vector2<Real>,
+ /// The target velocity correction at the contact point.
+ pub rhs: Real,
}
impl Default for ContactData {
@@ -45,6 +47,7 @@ impl Default for ContactData {
Self {
impulse: 0.0,
tangent_impulse: na::zero(),
+ rhs: 0.0,
}
}
}
@@ -143,16 +146,25 @@ pub struct SolverContact {
/// This is set to zero by default. Set to a non-zero value to
/// simulate, e.g., conveyor belts.
pub tangent_velocity: Vector<Real>,
- /// Associated contact data used to warm-start the constraints
- /// solver.
- pub data: ContactData,
+ /// The warmstart impulse, along the contact normal, applied by this contact to the first collider's rigid-body.
+ pub warmstart_impulse: Real,
+ /// The warmstart friction impulse along the vector orthonormal to the contact normal, applied to the first
+ /// collider's rigid-body.
+ #[cfg(feature = "dim2")]
+ pub warmstart_tangent_impulse: Real,
+ /// The warmstart friction impulses along the basis orthonormal to the contact normal, applied to the first
+ /// collider's rigid-body.
+ #[cfg(feature = "dim3")]
+ pub warmstart_tangent_impulse: na::Vector2<Real>,
+ /// The last velocity correction targeted by this contact.
+ pub prev_rhs: Real,
}
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;
+ let is_new = self.warmstart_impulse == 0.0;
if is_new {
// Treat new collisions as bouncing at first, unless we have zero restitution.
self.restitution > 0.0
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 92cf57d..372d056 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -584,7 +584,9 @@ impl NarrowPhase {
friction,
restitution,
tangent_velocity: Vector::zeros(),
- data: contact.data,
+ warmstart_impulse: contact.data.impulse,
+ warmstart_tangent_impulse: contact.data.tangent_impulse,
+ prev_rhs: contact.data.rhs,
};
manifold.data.solver_contacts.push(solver_contact);
generated by cgit (git 2.34.1) at 2026-02-04 02:38:05 +0000