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

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

@@ -202,7 +202,7 @@ impl GenericTwoBodyConstraintBuilder {
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse_accumulator: na::zero(),
impulse: na::zero(),
impulse: manifold_point.warmstart_impulse,
r,
r_mat_elts: [0.0; 2],
};
@@ -210,7 +210,8 @@ impl GenericTwoBodyConstraintBuilder {
// Tangent parts.
{
constraint.inner.elements[k].tangent_part.impulse = na::zero();
constraint.inner.elements[k].tangent_part.impulse =
manifold_point.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -374,6 +375,50 @@ impl GenericTwoBodyConstraint {
}
}
pub fn warmstart(
&mut self,
jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1])
} else {
GenericRhs::GenericId(self.inner.solver_vel1)
};
let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 {
GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2])
} else {
GenericRhs::GenericId(self.inner.solver_vel2)
};
let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
TwoBodyConstraintElement::generic_warmstart_group(
elements,
jacobians,
&self.inner.dir1,
#[cfg(feature = "dim3")]
&self.inner.tangent1,
&self.inner.im1,
&self.inner.im2,
self.ndofs1,
self.ndofs2,
self.j_id,
&mut solver_vel1,
&mut solver_vel2,
generic_solver_vels,
);
if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 {
solver_vels[self.inner.solver_vel1] = solver_vel1;
}
if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 {
solver_vels[self.inner.solver_vel2] = solver_vel2;
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,