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

feat: add warmstarting to contact constraints resolution

Browse Source
This commit is contained in:
Sébastien Crozet
2024-04-21 18:55:11 +02:00
committed by Sébastien Crozet
parent da79d6fb5b
commit f58b4f7c19
32 changed files with 1540 additions and 342 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -151,7 +151,7 @@ impl OneBodyConstraintBuilder {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
impulse: manifold_point.warmstart_impulse,
impulse_accumulator: na::zero(),
r: projected_mass,
r_mat_elts: [0.0; 2],
@@ -160,7 +160,8 @@ impl OneBodyConstraintBuilder {
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse = na::zero();
constraint.elements[k].tangent_part.impulse =
manifold_point.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let gcross2 = mprops2
@@ -317,13 +318,13 @@ impl OneBodyConstraintBuilder {
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.impulse_accumulator += element.normal_part.impulse;
element.normal_part.impulse = na::zero();
element.normal_part.impulse *= params.warmstart_coefficient;
}
// Tangent part.
{
element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
element.tangent_part.impulse = na::zero();
element.tangent_part.impulse *= params.warmstart_coefficient;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -369,6 +370,21 @@ impl OneBodyConstraint {
}
}
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel2 = solver_vels[self.solver_vel2];
OneBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im2,
&mut solver_vel2,
);
solver_vels[self.solver_vel2] = solver_vel2;
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
@@ -400,17 +416,11 @@ impl OneBodyConstraint {
for k in 0..self.num_contacts as usize {
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.total_impulse();
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse =
self.elements[k].tangent_part.total_impulse()[0];
}
#[cfg(feature = "dim3")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
}
active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse;
active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse;
active_contact.data.impulse = self.elements[k].normal_part.total_impulse();
active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
}
}