aboutsummaryrefslogtreecommitdiff
path: root/src/dynamics/solver
diff options
context:
space:
mode:
Diffstat (limited to 'src/dynamics/solver')
-rw-r--r--src/dynamics/solver/generic_velocity_constraint.rs9
-rw-r--r--src/dynamics/solver/generic_velocity_ground_constraint.rs12
-rw-r--r--src/dynamics/solver/velocity_constraint.rs10
-rw-r--r--src/dynamics/solver/velocity_constraint_element.rs4
-rw-r--r--src/dynamics/solver/velocity_constraint_wide.rs11
-rw-r--r--src/dynamics/solver/velocity_ground_constraint.rs12
-rw-r--r--src/dynamics/solver/velocity_ground_constraint_element.rs4
-rw-r--r--src/dynamics/solver/velocity_ground_constraint_wide.rs21
-rw-r--r--src/dynamics/solver/velocity_solver.rs1
9 files changed, 68 insertions, 16 deletions
diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs
index 318727e..945590c 100644
--- a/src/dynamics/solver/generic_velocity_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_constraint.rs
@@ -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,
};
}
diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs
index 16648c4..24a9de5 100644
--- a/src/dynamics/solver/generic_velocity_ground_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs
@@ -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,
};
}
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 23d4b97..bfb1e9d 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -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,
};
}
diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs
index 30afef5..6c1006d 100644
--- a/src/dynamics/solver/velocity_constraint_element.rs
+++ b/src/dynamics/solver/velocity_constraint_element.rs
@@ -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;
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 68246da..f235ddc 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -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,
};
}
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 39a2a19..99b71e0 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -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,
};
}
diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs
index 06a727a..3b2ed86 100644
--- a/src/dynamics/solver/velocity_ground_constraint_element.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_element.rs
@@ -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;
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 37f4688..e0e7649 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -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,
};
}
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index d5dc77d..ddd7086 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -168,6 +168,7 @@ impl VelocitySolver {
&rb.pos.position,
&rb.mprops.local_mprops.local_com,
);
+ rb.integrated_vels = new_vels;
rb.pos = new_pos;
}
}
generated by cgit (git 2.34.1) at 2026-02-11 06:58:58 +0000