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

feat: remove the "fast constraint" special case

Browse Source
This commit is contained in:
Sébastien Crozet
2024-05-19 15:49:42 +02:00
committed by Sébastien Crozet
parent fdd935dbf1
commit ed8645e34c
6 changed files with 16 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
View File

@@ -231,13 +231,8 @@ impl GenericOneBodyConstraintBuilder {
.unwrap()
.local_to_world;
self.inner.update_with_positions(
params,
solved_dt,
pos2,
self.ccd_thickness,
&mut constraint.inner,
);
self.inner
.update_with_positions(params, solved_dt, pos2, &mut constraint.inner);
}
}

10
src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
View File

@@ -342,14 +342,8 @@ impl GenericTwoBodyConstraintBuilder {
.map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world)
.unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position);
self.inner.update_with_positions(
params,
solved_dt,
pos1,
pos2,
self.ccd_thickness,
&mut constraint.inner,
);
self.inner
.update_with_positions(params, solved_dt, pos1, pos2, &mut constraint.inner);
}
}

17
src/dynamics/solver/contact_constraint/one_body_constraint.rs
View File

@@ -260,13 +260,7 @@ impl OneBodyConstraintBuilder {
constraint: &mut OneBodyConstraint,
) {
let rb2 = &bodies[constraint.solver_vel2];
self.update_with_positions(
params,
solved_dt,
&rb2.position,
rb2.ccd_thickness,
constraint,
)
self.update_with_positions(params, solved_dt, &rb2.position, constraint)
}
// TODO: this code is SOOOO similar to TwoBodyConstraint::update.
@@ -276,7 +270,6 @@ impl OneBodyConstraintBuilder {
params: &IntegrationParameters,
solved_dt: Real,
rb2_pos: &Isometry<Real>,
ccd_thickness: Real,
constraint: &mut OneBodyConstraint,
) {
let cfm_factor = params.cfm_factor();
@@ -290,7 +283,6 @@ impl OneBodyConstraintBuilder {
let new_pos1 = self
.vels1
.integrate(solved_dt, &rb1.position, &rb1.local_com);
let mut is_fast_contact = false;
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
@@ -309,11 +301,9 @@ impl OneBodyConstraintBuilder {
// Normal part.
{
let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
let rhs_bias = erp_inv_dt
* (dist + params.allowed_linear_error())
.clamp(-params.max_penetration_correction(), 0.0);
let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
.clamp(-params.max_corrective_velocity(), 0.0);
let new_rhs = rhs_wo_bias + rhs_bias;
is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
@@ -334,7 +324,6 @@ impl OneBodyConstraintBuilder {
}
constraint.cfm_factor = cfm_factor;
// constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
}
}

14
src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
View File

@@ -15,7 +15,6 @@ use crate::math::{
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
use parry::math::SimdBool;
use parry::utils::SdpMatrix2;
use simba::simd::{SimdPartialOrd, SimdValue};
@@ -263,15 +262,13 @@ impl SimdOneBodyConstraintBuilder {
constraint: &mut OneBodyConstraintSimd,
) {
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());
let max_penetration_correction = SimdReal::splat(params.max_penetration_correction());
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
let ccd_thickness = SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
let all_infos = &self.infos[..constraint.num_contacts as usize];
@@ -292,7 +289,6 @@ impl SimdOneBodyConstraintBuilder {
constraint.dir1.cross(&constraint.tangent1),
];
let mut is_fast_contact = SimdBool::splat(false);
let solved_dt = SimdReal::splat(solved_dt);
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
@@ -305,12 +301,9 @@ impl SimdOneBodyConstraintBuilder {
{
let rhs_wo_bias =
info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
let rhs_bias = (dist + allowed_lin_err)
.simd_clamp(-max_penetration_correction, SimdReal::zero())
* erp_inv_dt;
let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
.simd_clamp(-max_corrective_velocity, SimdReal::zero());
let new_rhs = rhs_wo_bias + rhs_bias;
is_fast_contact =
is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
@@ -331,7 +324,6 @@ impl SimdOneBodyConstraintBuilder {
}
constraint.cfm_factor = cfm_factor;
// constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
}
}

20
src/dynamics/solver/contact_constraint/two_body_constraint.rs
View File

@@ -361,15 +361,7 @@ impl TwoBodyConstraintBuilder {
) {
let rb1 = &bodies[constraint.solver_vel1];
let rb2 = &bodies[constraint.solver_vel2];
let ccd_thickness = rb1.ccd_thickness + rb2.ccd_thickness;
self.update_with_positions(
params,
solved_dt,
&rb1.position,
&rb2.position,
ccd_thickness,
constraint,
)
self.update_with_positions(params, solved_dt, &rb1.position, &rb2.position, constraint)
}
// Used by both generic and non-generic builders..
@@ -379,7 +371,6 @@ impl TwoBodyConstraintBuilder {
solved_dt: Real,
rb1_pos: &Isometry<Real>,
rb2_pos: &Isometry<Real>,
ccd_thickness: Real,
constraint: &mut TwoBodyConstraint,
) {
let cfm_factor = params.cfm_factor();
@@ -389,8 +380,6 @@ impl TwoBodyConstraintBuilder {
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
let mut is_fast_contact = false;
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
@@ -408,11 +397,9 @@ impl TwoBodyConstraintBuilder {
// Normal part.
{
let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
let rhs_bias = erp_inv_dt
* (dist + params.allowed_linear_error())
.clamp(-params.max_penetration_correction(), 0.0);
let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
.clamp(-params.max_corrective_velocity(), 0.0);
let new_rhs = rhs_wo_bias + rhs_bias;
is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
@@ -433,7 +420,6 @@ impl TwoBodyConstraintBuilder {
}
constraint.cfm_factor = cfm_factor;
// constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
}
}

16
src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
View File

@@ -15,7 +15,6 @@ use crate::math::{
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
use parry::math::SimdBool;
use parry::utils::SdpMatrix2;
use simba::simd::{SimdPartialOrd, SimdValue};
@@ -252,19 +251,15 @@ impl TwoBodyConstraintBuilderSimd {
constraint: &mut TwoBodyConstraintSimd,
) {
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());
let max_penetration_correction = SimdReal::splat(params.max_penetration_correction());
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]];
let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
let ccd_thickness = SimdReal::from(gather![|ii| rb1[ii].ccd_thickness])
+ SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
let poss1 = Isometry::from(gather![|ii| rb1[ii].position]);
let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
@@ -279,7 +274,6 @@ impl TwoBodyConstraintBuilderSimd {
constraint.dir1.cross(&constraint.tangent1),
];
let mut is_fast_contact = SimdBool::splat(false);
let solved_dt = SimdReal::splat(solved_dt);
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
@@ -292,12 +286,9 @@ impl TwoBodyConstraintBuilderSimd {
{
let rhs_wo_bias =
info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
let rhs_bias = (dist + allowed_lin_err)
.simd_clamp(-max_penetration_correction, SimdReal::zero())
* erp_inv_dt;
let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
.simd_clamp(-max_corrective_velocity, SimdReal::zero());
let new_rhs = rhs_wo_bias + rhs_bias;
is_fast_contact =
is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
@@ -318,7 +309,6 @@ impl TwoBodyConstraintBuilderSimd {
}
constraint.cfm_factor = cfm_factor;
// constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
}
}