aboutsummaryrefslogtreecommitdiff
path: root/src/dynamics/solver
diff options
context:
space:
mode:
Diffstat (limited to 'src/dynamics/solver')
-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
4 files changed, 64 insertions, 22 deletions
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")]
generated by cgit (git 2.34.1) at 2026-02-05 13:54:57 +0000