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

Move the cfm factor to the velocity constraints instead of the elements.

Browse Source
This commit is contained in:
Sébastien Crozet
2022-05-30 19:14:05 +02:00
parent 0d05536ab6
commit fef8416983
11 changed files with 79 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -1,11 +1,8 @@
use crate::data::{Arena, Coarena, Index};
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::{
GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyHandle, RigidBodySet,
};
use crate::dynamics::{GenericJoint, Multibody, MultibodyJoint, RigidBodyHandle};
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
use crate::parry::partitioning::IndexedData;
use crate::prelude::RigidBody;
/// The unique handle of an multibody_joint added to a `MultibodyJointSet`.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]

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

@@ -94,6 +94,7 @@ impl GenericVelocityConstraint {
.enumerate()
{
let chunk_j_id = *jacobian_id;
let mut is_fast_contact = false;
let mut constraint = VelocityConstraint {
dir1: force_dir1,
#[cfg(feature = "dim3")]
@@ -109,6 +110,7 @@ impl GenericVelocityConstraint {
} else {
na::zero()
},
cfm_factor,
limit: 0.0,
mj_lambda1,
mj_lambda2,
@@ -199,8 +201,7 @@ impl GenericVelocityConstraint {
/* 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 };
is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5);
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
gcross1,
@@ -209,7 +210,6 @@ impl GenericVelocityConstraint {
rhs_wo_bias,
impulse: na::zero(),
r,
cfm,
};
}
@@ -290,6 +290,8 @@ impl GenericVelocityConstraint {
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0);
let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0);
// NOTE: we use the generic constraint for non-dynamic bodies because this will
@@ -317,7 +319,6 @@ impl GenericVelocityConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
@@ -339,7 +340,7 @@ impl GenericVelocityConstraint {
let elements = &mut self.velocity_constraint.elements
[..self.velocity_constraint.num_contacts as usize];
VelocityConstraintElement::generic_solve_group(
cfm_factor,
self.velocity_constraint.cfm_factor,
elements,
jacobians,
&self.velocity_constraint.dir1,
@@ -371,7 +372,7 @@ impl GenericVelocityConstraint {
self.velocity_constraint.writeback_impulses(manifolds_all);
}
pub fn remove_bias_from_rhs(&mut self) {
self.velocity_constraint.remove_bias_from_rhs();
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.velocity_constraint.remove_cfm_and_bias_from_rhs();
}
}

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

@@ -86,12 +86,14 @@ impl GenericVelocityGroundConstraint {
.enumerate()
{
let chunk_j_id = *jacobian_id;
let mut is_fast_contact = false;
let mut constraint = VelocityGroundConstraint {
dir1: force_dir1,
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: mprops2.effective_inv_mass,
cfm_factor,
limit: 0.0,
mj_lambda2,
manifold_id,
@@ -139,8 +141,8 @@ impl GenericVelocityGroundConstraint {
/* 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 };
is_fast_contact =
is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5);
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2: na::zero(), // Unused for generic constraints.
@@ -148,7 +150,6 @@ impl GenericVelocityGroundConstraint {
rhs_wo_bias,
impulse: na::zero(),
r,
cfm,
};
}
@@ -187,6 +188,8 @@ impl GenericVelocityGroundConstraint {
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
let constraint = GenericVelocityGroundConstraint {
velocity_constraint: constraint,
j_id: chunk_j_id,
@@ -204,7 +207,6 @@ impl GenericVelocityGroundConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
jacobians: &DVector<Real>,
generic_mj_lambdas: &mut DVector<Real>,
solve_restitution: bool,
@@ -215,7 +217,7 @@ impl GenericVelocityGroundConstraint {
let elements = &mut self.velocity_constraint.elements
[..self.velocity_constraint.num_contacts as usize];
VelocityGroundConstraintElement::generic_solve_group(
cfm_factor,
self.velocity_constraint.cfm_factor,
elements,
jacobians,
self.velocity_constraint.limit,
@@ -232,7 +234,7 @@ impl GenericVelocityGroundConstraint {
self.velocity_constraint.writeback_impulses(manifolds_all);
}
pub fn remove_bias_from_rhs(&mut self) {
self.velocity_constraint.remove_bias_from_rhs();
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.velocity_constraint.remove_cfm_and_bias_from_rhs();
}
}

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

@@ -47,21 +47,20 @@ impl AnyVelocityConstraint {
pub fn remove_bias_from_rhs(&mut self) {
match self {
AnyVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(),
AnyVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(),
AnyVelocityConstraint::Nongrouped(c) => c.remove_cfm_and_bias_from_rhs(),
AnyVelocityConstraint::NongroupedGround(c) => c.remove_cfm_and_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::Grouped(c) => c.remove_bias_from_rhs(),
AnyVelocityConstraint::Grouped(c) => c.remove_cfm_and_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::GroupedGround(c) => c.remove_bias_from_rhs(),
AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_bias_from_rhs(),
AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_bias_from_rhs(),
AnyVelocityConstraint::GroupedGround(c) => c.remove_cfm_and_bias_from_rhs(),
AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_cfm_and_bias_from_rhs(),
AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_cfm_and_bias_from_rhs(),
AnyVelocityConstraint::Empty => unreachable!(),
}
}
pub fn solve(
&mut self,
cfm_factor: Real,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
@@ -70,21 +69,20 @@ impl AnyVelocityConstraint {
) {
match self {
AnyVelocityConstraint::NongroupedGround(c) => {
c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
c.solve(mj_lambdas, solve_restitution, solve_friction)
}
AnyVelocityConstraint::Nongrouped(c) => {
c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
c.solve(mj_lambdas, solve_restitution, solve_friction)
}
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::GroupedGround(c) => {
c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
c.solve(mj_lambdas, solve_restitution, solve_friction)
}
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::Grouped(c) => {
c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
c.solve(mj_lambdas, solve_restitution, solve_friction)
}
AnyVelocityConstraint::NongroupedGeneric(c) => c.solve(
cfm_factor,
jacobians,
mj_lambdas,
generic_mj_lambdas,
@@ -92,7 +90,6 @@ impl AnyVelocityConstraint {
solve_friction,
),
AnyVelocityConstraint::NongroupedGenericGround(c) => c.solve(
cfm_factor,
jacobians,
generic_mj_lambdas,
solve_restitution,
@@ -124,6 +121,7 @@ pub(crate) struct VelocityConstraint {
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im1: Vector<Real>,
pub im2: Vector<Real>,
pub cfm_factor: Real,
pub limit: Real,
pub mj_lambda1: usize,
pub mj_lambda2: usize,
@@ -182,6 +180,8 @@ impl VelocityConstraint {
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let mut is_fast_contact = false;
#[cfg(not(target_arch = "wasm32"))]
let mut constraint = VelocityConstraint {
dir1: force_dir1,
@@ -190,6 +190,7 @@ impl VelocityConstraint {
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: mprops1.effective_inv_mass,
im2: mprops2.effective_inv_mass,
cfm_factor,
limit: 0.0,
mj_lambda1,
mj_lambda2,
@@ -237,6 +238,7 @@ impl VelocityConstraint {
}
constraint.im1 = mprops1.effective_inv_mass;
constraint.im2 = mprops2.effective_inv_mass;
constraint.cfm_factor = cfm_factor;
constraint.limit = 0.0;
constraint.mj_lambda1 = mj_lambda1;
constraint.mj_lambda2 = mj_lambda2;
@@ -284,8 +286,7 @@ impl VelocityConstraint {
* (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 };
is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5);
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
gcross1,
@@ -294,7 +295,6 @@ impl VelocityConstraint {
rhs_wo_bias,
impulse: na::zero(),
r: projected_mass,
cfm,
};
}
@@ -337,6 +337,8 @@ impl VelocityConstraint {
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
#[cfg(not(target_arch = "wasm32"))]
if let Some(at) = insert_at {
out_constraints[at + _l] = AnyVelocityConstraint::Nongrouped(constraint);
@@ -348,7 +350,6 @@ impl VelocityConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
@@ -357,7 +358,7 @@ impl VelocityConstraint {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
VelocityConstraintElement::solve_group(
cfm_factor,
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
@@ -394,7 +395,8 @@ impl VelocityConstraint {
}
}
pub fn remove_bias_from_rhs(&mut self) {
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = 1.0;
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
}

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

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

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

@@ -12,6 +12,7 @@ use crate::math::{
use crate::utils::WBasis;
use crate::utils::{self, WAngularInertia, WCross, WDot};
use num::Zero;
use parry::math::SimdBool;
use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
@@ -23,6 +24,7 @@ pub(crate) struct WVelocityConstraint {
pub num_contacts: u8,
pub im1: Vector<SimdReal>,
pub im2: Vector<SimdReal>,
pub cfm_factor: SimdReal,
pub limit: SimdReal,
pub mj_lambda1: [usize; SIMD_WIDTH],
pub mj_lambda2: [usize; SIMD_WIDTH],
@@ -97,6 +99,7 @@ impl WVelocityConstraint {
gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut is_fast_contact = SimdBool::splat(false);
let mut constraint = WVelocityConstraint {
dir1: force_dir1,
#[cfg(feature = "dim3")]
@@ -104,6 +107,7 @@ impl WVelocityConstraint {
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1,
im2,
cfm_factor,
limit: SimdReal::splat(0.0),
mj_lambda1,
mj_lambda2,
@@ -154,8 +158,8 @@ impl WVelocityConstraint {
* (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);
is_fast_contact =
is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
gcross1,
@@ -164,7 +168,6 @@ impl WVelocityConstraint {
rhs_wo_bias,
impulse: SimdReal::splat(0.0),
r: projected_mass,
cfm,
};
}
@@ -200,6 +203,8 @@ impl WVelocityConstraint {
}
}
constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
if let Some(at) = insert_at {
out_constraints[at + l / MAX_MANIFOLD_POINTS] =
AnyVelocityConstraint::Grouped(constraint);
@@ -211,7 +216,6 @@ impl WVelocityConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
@@ -231,7 +235,7 @@ impl WVelocityConstraint {
};
VelocityConstraintElement::solve_group(
SimdReal::splat(cfm_factor),
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
@@ -281,7 +285,8 @@ impl WVelocityConstraint {
}
}
pub fn remove_bias_from_rhs(&mut self) {
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = SimdReal::splat(1.0);
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
}

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

@@ -17,6 +17,7 @@ pub(crate) struct VelocityGroundConstraint {
#[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im2: Vector<Real>,
pub cfm_factor: Real,
pub limit: Real,
pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS],
@@ -81,6 +82,7 @@ impl VelocityGroundConstraint {
tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: mprops2.effective_inv_mass,
cfm_factor,
limit: 0.0,
mj_lambda2,
manifold_id,
@@ -133,6 +135,8 @@ impl VelocityGroundConstraint {
constraint.num_contacts = manifold_points.len() as u8;
}
let mut is_fast_contact = false;
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp2 = manifold_point.point - mprops2.world_com;
@@ -166,8 +170,8 @@ impl VelocityGroundConstraint {
* (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 };
is_fast_contact =
is_fast_contact || -rhs * params.dt > rb2.ccd.ccd_thickness * 0.5;
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2,
@@ -175,7 +179,6 @@ impl VelocityGroundConstraint {
rhs_wo_bias,
impulse: na::zero(),
r: projected_mass,
cfm,
};
}
@@ -212,6 +215,10 @@ impl VelocityGroundConstraint {
}
}
if is_fast_contact {
constraint.cfm_factor = 1.0;
}
#[cfg(not(target_arch = "wasm32"))]
if let Some(at) = insert_at {
out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGround(constraint);
@@ -223,7 +230,6 @@ impl VelocityGroundConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
@@ -231,7 +237,7 @@ impl VelocityGroundConstraint {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
VelocityGroundConstraintElement::solve_group(
cfm_factor,
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
@@ -266,7 +272,8 @@ impl VelocityGroundConstraint {
}
}
pub fn remove_bias_from_rhs(&mut self) {
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = 1.0;
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
}

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

@@ -93,7 +93,6 @@ 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> {
@@ -104,7 +103,6 @@ impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
rhs_wo_bias: na::zero(),
impulse: na::zero(),
r: na::zero(),
cfm: na::one(),
}
}
@@ -119,7 +117,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 = self.cfm * (self.impulse - self.r * dvel).simd_max(N::zero());
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;

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

@@ -9,11 +9,11 @@ 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};
use num::Zero;
use parry::math::SimdBool;
use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
@@ -24,6 +24,7 @@ pub(crate) struct WVelocityGroundConstraint {
pub elements: [VelocityGroundConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im2: Vector<SimdReal>,
pub cfm_factor: SimdReal,
pub limit: SimdReal,
pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
@@ -105,12 +106,14 @@ impl WVelocityGroundConstraint {
let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut is_fast_contact = SimdBool::splat(false);
let mut constraint = WVelocityGroundConstraint {
dir1: force_dir1,
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2,
cfm_factor,
limit: SimdReal::splat(0.0),
mj_lambda2,
manifold_id,
@@ -157,8 +160,8 @@ impl WVelocityGroundConstraint {
* (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);
is_fast_contact =
is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2,
@@ -166,7 +169,6 @@ impl WVelocityGroundConstraint {
rhs_wo_bias,
impulse: na::zero(),
r: projected_mass,
cfm,
};
}
@@ -196,6 +198,8 @@ impl WVelocityGroundConstraint {
}
}
constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
if let Some(at) = insert_at {
out_constraints[at + l / MAX_MANIFOLD_POINTS] =
AnyVelocityConstraint::GroupedGround(constraint);
@@ -207,7 +211,6 @@ impl WVelocityGroundConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
@@ -220,7 +223,7 @@ impl WVelocityGroundConstraint {
};
VelocityGroundConstraintElement::solve_group(
SimdReal::splat(cfm_factor),
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
@@ -265,7 +268,8 @@ impl WVelocityGroundConstraint {
}
}
pub fn remove_bias_from_rhs(&mut self) {
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = SimdReal::splat(1.0);
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
}

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

@@ -35,7 +35,6 @@ impl VelocitySolver {
joint_constraints: &mut [AnyJointVelocityConstraint],
generic_joint_jacobians: &DVector<Real>,
) {
let cfm_factor = params.cfm_factor();
self.mj_lambdas.clear();
self.mj_lambdas
.resize(islands.active_island(island_id).len(), DeltaVel::zero());
@@ -86,7 +85,6 @@ impl VelocitySolver {
for constraint in &mut *contact_constraints {
constraint.solve(
cfm_factor,
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
@@ -98,7 +96,6 @@ impl VelocitySolver {
if solve_friction {
for constraint in &mut *contact_constraints {
constraint.solve(
cfm_factor,
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
@@ -121,7 +118,6 @@ impl VelocitySolver {
for _ in 0..remaining_friction_iterations {
for constraint in &mut *contact_constraints {
constraint.solve(
cfm_factor,
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
@@ -191,7 +187,6 @@ impl VelocitySolver {
for constraint in &mut *contact_constraints {
constraint.solve(
1.0,
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
@@ -202,7 +197,6 @@ impl VelocitySolver {
for constraint in &mut *contact_constraints {
constraint.solve(
1.0,
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,