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

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

@@ -8,8 +8,8 @@ use crate::dynamics::{
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM,
MAX_MANIFOLD_POINTS, SIMD_WIDTH,
};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
@@ -104,6 +104,11 @@ impl TwoBodyConstraintBuilderSimd {
let is_bouncy = SimdReal::from(gather![
|ii| manifold_points[ii][k].is_bouncy() as u32 as Real
]);
let warmstart_impulse =
SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points
[ii][k]
.warmstart_tangent_impulse]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
@@ -140,7 +145,7 @@ impl TwoBodyConstraintBuilderSimd {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: SimdReal::splat(0.0),
impulse: warmstart_impulse,
impulse_accumulator: SimdReal::splat(0.0),
r: projected_mass,
r_mat_elts: [SimdReal::zero(); 2],
@@ -148,7 +153,7 @@ impl TwoBodyConstraintBuilderSimd {
}
// tangent parts.
constraint.elements[k].tangent_part.impulse = na::zero();
constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
@@ -253,6 +258,7 @@ impl TwoBodyConstraintBuilderSimd {
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 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]]];
@@ -297,13 +303,13 @@ impl TwoBodyConstraintBuilderSimd {
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 *= warmstart_coeff;
}
// tangent parts.
{
element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
element.tangent_part.impulse = na::zero();
element.tangent_part.impulse *= warmstart_coeff;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -334,6 +340,38 @@ pub(crate) struct TwoBodyConstraintSimd {
}
impl TwoBodyConstraintSimd {
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel1 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]),
};
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
};
TwoBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii);
solver_vels[self.solver_vel1[ii]].angular = solver_vel1.angular.extract(ii);
}
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
}
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
@@ -377,27 +415,20 @@ impl TwoBodyConstraintSimd {
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
for k in 0..self.num_contacts as usize {
let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse;
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into();
#[cfg(feature = "dim2")]
let tangent_impulses: [_; SIMD_WIDTH] =
self.elements[k].tangent_part.total_impulse()[0].into();
#[cfg(feature = "dim3")]
let tangent_impulses = self.elements[k].tangent_part.total_impulse();
for ii in 0..SIMD_WIDTH {
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.warmstart_impulse = warmstart_impulses[ii];
active_contact.data.warmstart_tangent_impulse =
warmstart_tangent_impulses.extract(ii);
active_contact.data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = tangent_impulses[ii];
}
#[cfg(feature = "dim3")]
{
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
}
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
}
}
}