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

130
src/dynamics/integration_parameters.rs
View File

@@ -4,7 +4,6 @@ use std::num::NonZeroUsize;
// TODO: enabling the block solver in 3d introduces a lot of jitters in
// the 3D domino demo. So for now we dont enable it in 3D.
pub(crate) static BLOCK_SOLVER_ENABLED: bool = cfg!(feature = "dim2");
pub(crate) static DISABLE_FRICTION_LIMIT_REAPPLY: bool = false;
/// Parameters for a time-step of the physics engine.
#[derive(Copy, Clone, Debug)]
@@ -26,12 +25,12 @@ pub struct IntegrationParameters {
/// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
/// will be compensated for during the velocity solve.
/// (default `0.8`).
/// (default `0.1`).
pub erp: Real,
/// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization.
/// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations
/// before stabilization).
/// (default `0.25`).
/// (default `20.0`).
pub damping_ratio: Real,
/// 0-1: multiplier for how much of the joint violation
@@ -40,14 +39,21 @@ pub struct IntegrationParameters {
pub joint_erp: Real,
/// The fraction of critical damping applied to the joint for constraints regularization.
/// (default `0.25`).
/// (default `1.0`).
pub joint_damping_ratio: Real,
/// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
/// The coefficient in `[0, 1]` applied to warmstart impulses, i.e., impulses that are used as the
/// initial solution (instead of 0) at the next simulation step.
///
/// This should generally be set to 1. Can be set to 0 if using a large [`Self::erp`] value.
/// (default `1.0`).
pub warmstart_coefficient: Real,
/// Amount of penetration the engine won’t attempt to correct (default: `0.001m`).
pub allowed_linear_error: Real,
/// Maximum amount of penetration the solver will attempt to resolve in one timestep.
pub max_penetration_correction: Real,
/// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
/// The maximal distance separating two objects that will generate predictive contacts (default: `0.002m`).
pub prediction_distance: Real,
/// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
pub num_solver_iterations: NonZeroUsize,
@@ -55,8 +61,8 @@ pub struct IntegrationParameters {
pub num_additional_friction_iterations: usize,
/// Number of internal Project Gauss Seidel (PGS) iterations run at each solver iteration (default: `1`).
pub num_internal_pgs_iterations: usize,
/// The maximum number of stabilization iterations run at each solver iterations (default: `10`).
pub max_internal_stabilization_iterations: usize,
/// The number of stabilization iterations run at each solver iterations (default: `2`).
pub num_internal_stabilization_iterations: usize,
/// Minimum number of dynamic bodies in each active island (default: `128`).
pub min_island_size: usize,
/// Maximum number of substeps performed by the solver (default: `1`).
@@ -64,51 +70,6 @@ pub struct IntegrationParameters {
}
impl IntegrationParameters {
/// Configures the integration parameters to match the old PGS solver
/// from Rapier version <= 0.17.
///
/// This solver was slightly faster than the new one but resulted
/// in less stable joints and worse convergence rates.
///
/// This should only be used for comparison purpose or if you are
/// experiencing problems with the new solver.
///
/// NOTE: this does not affect any [`RigidBody::additional_solver_iterations`] that will
/// still create solver iterations based on the new "small-steps" PGS solver.
/// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`],
/// [`Self::joint_damping_ratio`] to their former default values.
pub fn switch_to_standard_pgs_solver(&mut self) {
self.num_internal_pgs_iterations *= self.num_solver_iterations.get();
self.num_solver_iterations = NonZeroUsize::new(1).unwrap();
self.erp = 0.8;
self.damping_ratio = 0.25;
self.joint_erp = 1.0;
self.joint_damping_ratio = 1.0;
}
/// Configures the integration parameters to match the new "small-steps" PGS solver
/// from Rapier version >= 0.18.
///
/// The "small-steps" PGS solver is the default one given by [`Self::default()`] so
/// calling this function is generally not needed unless
/// [`Self::switch_to_standard_pgs_solver()`] was called.
///
/// This solver results in more stable joints and significantly better convergence
/// rates but is slightly slower in its default settings.
///
/// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`],
/// [`Self::joint_damping_ratio`] to their default values.
pub fn switch_to_small_steps_pgs_solver(&mut self) {
self.num_solver_iterations = NonZeroUsize::new(self.num_internal_pgs_iterations).unwrap();
self.num_internal_pgs_iterations = 1;
let default = Self::default();
self.erp = default.erp;
self.damping_ratio = default.damping_ratio;
self.joint_erp = default.joint_erp;
self.joint_damping_ratio = default.joint_damping_ratio;
}
/// The inverse of the time-stepping length, i.e. the steps per seconds (Hz).
///
/// This is zero if `self.dt` is zero.
@@ -161,7 +122,7 @@ impl IntegrationParameters {
// let damping = 4.0 * damping_ratio * damping_ratio * projected_mass
// / (dt * inv_erp_minus_one);
// let cfm = 1.0 / (dt * dt * stiffness + dt * damping);
// NOTE: This simplies to cfm = cfm_coefff / projected_mass:
// NOTE: This simplifies to cfm = cfm_coeff / projected_mass:
let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one
/ ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.damping_ratio);
@@ -180,13 +141,14 @@ impl IntegrationParameters {
// new_impulse = cfm_factor * (old_impulse - m * delta_vel)
//
// The value returned by this function is this cfm_factor that can be used directly
// in the constraints solver.
// in the constraint solver.
1.0 / (1.0 + cfm_coeff)
}
/// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization
pub fn joint_cfm_coeff(&self) -> Real {
// Compute CFM assuming a critically damped spring multiplied by the damping ratio.
// The logic is similar to `Self::cfm_factor`.
let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0;
inv_erp_minus_one * inv_erp_minus_one
/ ((1.0 + inv_erp_minus_one)
@@ -194,23 +156,23 @@ impl IntegrationParameters {
* self.joint_damping_ratio
* self.joint_damping_ratio)
}
}
impl Default for IntegrationParameters {
fn default() -> Self {
/// Initialize the simulation paramaters with settings matching the TGS-soft solver
/// with warmstarting.
///
/// This is the default configuration, equivalent to [`IntegrationParameters::default()`].
pub fn tgs_soft() -> Self {
Self {
dt: 1.0 / 60.0,
min_ccd_dt: 1.0 / 60.0 / 100.0,
erp: 0.8,
damping_ratio: 1.0,
erp: 0.1,
damping_ratio: 20.0,
joint_erp: 1.0,
joint_damping_ratio: 1.0,
allowed_linear_error: 0.001,
max_penetration_correction: Real::MAX,
prediction_distance: 0.002,
warmstart_coefficient: 1.0,
num_internal_pgs_iterations: 1,
max_internal_stabilization_iterations: 10,
num_additional_friction_iterations: 4,
num_internal_stabilization_iterations: 2,
num_additional_friction_iterations: 0,
num_solver_iterations: NonZeroUsize::new(4).unwrap(),
// TODO: what is the optimal value for min_island_size?
// It should not be too big so that we don't end up with
@@ -218,7 +180,45 @@ impl Default for IntegrationParameters {
// However we don't want it to be too small and end up with
// tons of islands, reducing SIMD parallelism opportunities.
min_island_size: 128,
allowed_linear_error: 0.001,
max_penetration_correction: Real::MAX,
prediction_distance: 0.002,
max_ccd_substeps: 1,
}
}
/// Initialize the simulation paramaters with settings matching the TGS-soft solver
/// **without** warmstarting.
///
/// The [`IntegrationParameters::tgs_soft()`] configuration should be preferred unless
/// warmstarting proves to be undesirable for your use-case.
pub fn tgs_soft_without_warmstart() -> Self {
Self {
erp: 0.8,
damping_ratio: 1.0,
warmstart_coefficient: 0.0,
num_additional_friction_iterations: 4,
..Self::tgs_soft()
}
}
/// Initializes the integration parameters to match the legacy PGS solver from Rapier version <= 0.17.
///
/// This exists mainly for testing and comparison purpose.
pub fn pgs_legacy() -> Self {
Self {
erp: 0.8,
damping_ratio: 0.25,
warmstart_coefficient: 0.0,
num_additional_friction_iterations: 4,
num_solver_iterations: NonZeroUsize::new(1).unwrap(),
..Self::tgs_soft()
}
}
}
impl Default for IntegrationParameters {
fn default() -> Self {
Self::tgs_soft()
}
}

11
src/dynamics/solver/contact_constraint/contact_constraints_set.rs
View File

@@ -442,6 +442,17 @@ impl ContactConstraintsSet {
assert_eq!(curr_start, total_num_constraints);
}
pub fn warmstart(
&mut self,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.warmstart(jac, solver_vels, generic_solver_vels);
}
}
pub fn solve_restitution(
&mut self,
solver_vels: &mut [SolverVel<Real>],

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

@@ -259,6 +259,24 @@ impl GenericOneBodyConstraint {
}
}
pub fn warmstart(
&mut self,
jacobians: &DVector<Real>,
generic_solver_vels: &mut DVector<Real>,
) {
let solver_vel2 = self.inner.solver_vel2;
let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
OneBodyConstraintElement::generic_warmstart_group(
elements,
jacobians,
self.ndofs2,
self.j_id,
solver_vel2,
generic_solver_vels,
);
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,

86
src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
View File

@@ -7,6 +7,40 @@ use na::DVector;
use na::SimdPartialOrd;
impl OneBodyConstraintTangentPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
#[cfg(feature = "dim2")]
{
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse[0],
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
#[cfg(feature = "dim3")]
{
let j_step = ndofs2 * 2;
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse[0],
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse[1],
&jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
1.0,
);
}
}
#[inline]
pub fn generic_solve(
&mut self,
@@ -71,6 +105,22 @@ impl OneBodyConstraintTangentPart<Real> {
}
impl OneBodyConstraintNormalPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse,
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
#[inline]
pub fn generic_solve(
&mut self,
@@ -99,6 +149,42 @@ impl OneBodyConstraintNormalPart<Real> {
}
impl OneBodyConstraintElement<Real> {
#[inline]
pub fn generic_warmstart_group(
elements: &mut [Self],
jacobians: &DVector<Real>,
ndofs2: usize,
// Jacobian index of the first constraint.
j_id: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
let j_step = ndofs2 * 2 * DIM;
// Solve penetration.
let mut nrm_j_id = j_id;
for element in elements.iter_mut() {
element.normal_part.generic_warmstart(
nrm_j_id,
jacobians,
ndofs2,
solver_vel2,
solver_vels,
);
nrm_j_id += j_step;
}
// Solve friction.
let mut tng_j_id = j_id + ndofs2 * 2;
for element in elements.iter_mut() {
let part = &mut element.tangent_part;
part.generic_warmstart(tng_j_id, jacobians, ndofs2, solver_vel2, solver_vels);
tng_j_id += j_step;
}
}
#[inline]
pub fn generic_solve_group(
cfm_factor: Real,

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>,

196
src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
View File

@@ -88,6 +88,95 @@ impl GenericRhs {
}
impl TwoBodyConstraintTangentPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
j_id: usize,
jacobians: &DVector<Real>,
tangents1: [&Vector<Real>; DIM - 1],
im1: &Vector<Real>,
im2: &Vector<Real>,
ndofs1: usize,
ndofs2: usize,
solver_vel1: &mut GenericRhs,
solver_vel2: &mut GenericRhs,
solver_vels: &mut DVector<Real>,
) {
let j_id1 = j_id1(j_id, ndofs1, ndofs2);
let j_id2 = j_id2(j_id, ndofs1, ndofs2);
#[cfg(feature = "dim3")]
let j_step = j_step(ndofs1, ndofs2);
#[cfg(feature = "dim2")]
{
solver_vel1.apply_impulse(
j_id1,
ndofs1,
self.impulse[0],
jacobians,
tangents1[0],
&self.gcross1[0],
solver_vels,
im1,
);
solver_vel2.apply_impulse(
j_id2,
ndofs2,
self.impulse[0],
jacobians,
&-tangents1[0],
&self.gcross2[0],
solver_vels,
im2,
);
}
#[cfg(feature = "dim3")]
{
solver_vel1.apply_impulse(
j_id1,
ndofs1,
self.impulse[0],
jacobians,
tangents1[0],
&self.gcross1[0],
solver_vels,
im1,
);
solver_vel1.apply_impulse(
j_id1 + j_step,
ndofs1,
self.impulse[1],
jacobians,
tangents1[1],
&self.gcross1[1],
solver_vels,
im1,
);
solver_vel2.apply_impulse(
j_id2,
ndofs2,
self.impulse[0],
jacobians,
&-tangents1[0],
&self.gcross2[0],
solver_vels,
im2,
);
solver_vel2.apply_impulse(
j_id2 + j_step,
ndofs2,
self.impulse[1],
jacobians,
&-tangents1[1],
&self.gcross2[1],
solver_vels,
im2,
);
}
}
#[inline]
pub fn generic_solve(
&mut self,
@@ -240,6 +329,45 @@ impl TwoBodyConstraintTangentPart<Real> {
}
impl TwoBodyConstraintNormalPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
j_id: usize,
jacobians: &DVector<Real>,
dir1: &Vector<Real>,
im1: &Vector<Real>,
im2: &Vector<Real>,
ndofs1: usize,
ndofs2: usize,
solver_vel1: &mut GenericRhs,
solver_vel2: &mut GenericRhs,
solver_vels: &mut DVector<Real>,
) {
let j_id1 = j_id1(j_id, ndofs1, ndofs2);
let j_id2 = j_id2(j_id, ndofs1, ndofs2);
solver_vel1.apply_impulse(
j_id1,
ndofs1,
self.impulse,
jacobians,
dir1,
&self.gcross1,
solver_vels,
im1,
);
solver_vel2.apply_impulse(
j_id2,
ndofs2,
self.impulse,
jacobians,
&-dir1,
&self.gcross2,
solver_vels,
im2,
);
}
#[inline]
pub fn generic_solve(
&mut self,
@@ -290,6 +418,74 @@ impl TwoBodyConstraintNormalPart<Real> {
}
impl TwoBodyConstraintElement<Real> {
#[inline]
pub fn generic_warmstart_group(
elements: &mut [Self],
jacobians: &DVector<Real>,
dir1: &Vector<Real>,
#[cfg(feature = "dim3")] tangent1: &Vector<Real>,
im1: &Vector<Real>,
im2: &Vector<Real>,
// ndofs is 0 for a non-multibody body, or a multibody with zero
// degrees of freedom.
ndofs1: usize,
ndofs2: usize,
// Jacobian index of the first constraint.
j_id: usize,
solver_vel1: &mut GenericRhs,
solver_vel2: &mut GenericRhs,
solver_vels: &mut DVector<Real>,
) {
let j_step = j_step(ndofs1, ndofs2) * DIM;
// Solve penetration.
{
let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2);
for element in elements.iter_mut() {
element.normal_part.generic_warmstart(
nrm_j_id,
jacobians,
dir1,
im1,
im2,
ndofs1,
ndofs2,
solver_vel1,
solver_vel2,
solver_vels,
);
nrm_j_id += j_step;
}
}
// Solve friction.
{
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
for element in elements.iter_mut() {
let part = &mut element.tangent_part;
part.generic_warmstart(
tng_j_id,
jacobians,
tangents1,
im1,
im2,
ndofs1,
ndofs2,
solver_vel1,
solver_vel2,
solver_vels,
);
tng_j_id += j_step;
}
}
}
#[inline]
pub fn generic_solve_group(
cfm_factor: Real,

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();
}
}

100
src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
View File

@@ -1,9 +1,7 @@
use crate::dynamics::integration_parameters::{
BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY,
};
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart;
use crate::dynamics::solver::SolverVel;
use crate::math::{AngVector, Vector, DIM};
use crate::math::{AngVector, TangentImpulse, Vector, DIM};
use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
use na::Vector2;
@@ -12,14 +10,8 @@ pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> {
pub gcross2: [AngVector<N>; DIM - 1],
pub rhs: [N; DIM - 1],
pub rhs_wo_bias: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
#[cfg(feature = "dim2")]
pub impulse_accumulator: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse_accumulator: na::Vector2<N>,
pub impulse: TangentImpulse<N>,
pub impulse_accumulator: TangentImpulse<N>,
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
@@ -43,57 +35,29 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
/// Total impulse applied across all the solver substeps.
#[inline]
#[cfg(feature = "dim2")]
pub fn total_impulse(&self) -> na::Vector1<N> {
self.impulse_accumulator + self.impulse
}
/// Total impulse applied across all the solver substeps.
#[inline]
#[cfg(feature = "dim3")]
pub fn total_impulse(&self) -> na::Vector2<N> {
pub fn total_impulse(&self) -> TangentImpulse<N> {
self.impulse_accumulator + self.impulse
}
#[inline]
pub fn apply_limit(
pub fn warmstart(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im2: &Vector<N>,
limit: N,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
if DISABLE_FRICTION_LIMIT_REAPPLY {
return;
}
) {
#[cfg(feature = "dim2")]
{
let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2[0] * dlambda;
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0];
solver_vel2.angular += self.gcross2[0] * self.impulse[0];
}
#[cfg(feature = "dim3")]
{
let new_impulse = self.impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]
+ tangents1[1].component_mul(im2) * -self.impulse[1];
solver_vel2.angular +=
self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1];
}
}
@@ -184,6 +148,12 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
self.impulse_accumulator + self.impulse
}
#[inline]
pub fn warmstart(&mut self, dir1: &Vector<N>, im2: &Vector<N>, solver_vel2: &mut SolverVel<N>) {
solver_vel2.linear += dir1.component_mul(im2) * -self.impulse;
solver_vel2.angular += self.gcross2 * self.impulse;
}
#[inline]
pub fn solve(
&mut self,
@@ -257,6 +227,25 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
}
}
#[inline]
pub fn warmstart_group(
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im2: &Vector<N>,
solver_vel2: &mut SolverVel<N>,
) {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
element.normal_part.warmstart(dir1, im2, solver_vel2);
element.tangent_part.warmstart(tangents1, im2, solver_vel2);
}
}
#[inline]
pub fn solve_group(
cfm_factor: N,
@@ -293,13 +282,6 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
im2,
solver_vel2,
);
// There is one constraint left to solve if there isn’t an even number.
for i in 0..2 {
let limit = limit * elements[i].normal_part.impulse;
let part = &mut elements[i].tangent_part;
part.apply_limit(tangents1, im2, limit, solver_vel2);
}
}
if elements.len() % 2 == 1 {
@@ -307,18 +289,12 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
element
.normal_part
.solve(cfm_factor, dir1, im2, solver_vel2);
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.apply_limit(tangents1, im2, limit, solver_vel2);
}
} else {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, dir1, im2, solver_vel2);
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.apply_limit(tangents1, im2, limit, solver_vel2);
}
}
}

59
src/dynamics/solver/contact_constraint/one_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;
@@ -120,6 +120,11 @@ impl SimdOneBodyConstraintBuilder {
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]);
@@ -155,7 +160,7 @@ impl SimdOneBodyConstraintBuilder {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
impulse: warmstart_impulse,
impulse_accumulator: na::zero(),
r: projected_mass,
r_mat_elts: [SimdReal::zero(); 2],
@@ -163,7 +168,7 @@ impl SimdOneBodyConstraintBuilder {
}
// 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 gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
@@ -263,6 +268,7 @@ impl SimdOneBodyConstraintBuilder {
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 rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
let ccd_thickness = SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
@@ -309,13 +315,13 @@ impl SimdOneBodyConstraintBuilder {
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;
@@ -344,6 +350,27 @@ pub(crate) struct OneBodyConstraintSimd {
}
impl OneBodyConstraintSimd {
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
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]),
};
OneBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im2,
&mut solver_vel2,
);
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 +404,21 @@ impl OneBodyConstraintSimd {
// FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint.
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.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.warmstart_impulse = warmstart_impulses[ii];
active_contact.data.warmstart_tangent_impulse =
warmstart_tangent_impulses.extract(ii);
active_contact.data.impulse = impulses[ii];
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
}
}
}

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

@@ -25,6 +25,25 @@ impl<'a> AnyConstraintMut<'a, ContactConstraintTypes> {
Self::SimdTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
}
}
pub fn warmstart(
&mut self,
generic_jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::OneBody(c) => c.warmstart(solver_vels),
Self::TwoBodies(c) => c.warmstart(solver_vels),
Self::GenericOneBody(c) => c.warmstart(generic_jacobians, generic_solver_vels),
Self::GenericTwoBodies(c) => {
c.warmstart(generic_jacobians, solver_vels, generic_solver_vels)
}
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.warmstart(solver_vels),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.warmstart(solver_vels),
}
}
pub fn solve_restitution(
&mut self,
@@ -224,7 +243,7 @@ impl TwoBodyConstraintBuilder {
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],
@@ -233,7 +252,8 @@ impl TwoBodyConstraintBuilder {
// 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 gcross1 = mprops1
@@ -398,13 +418,13 @@ impl TwoBodyConstraintBuilder {
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;
@@ -418,6 +438,25 @@ impl TwoBodyConstraintBuilder {
}
impl TwoBodyConstraint {
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel1 = solver_vels[self.solver_vel1];
let mut solver_vel2 = solver_vels[self.solver_vel2];
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,
);
solver_vels[self.solver_vel1] = solver_vel1;
solver_vels[self.solver_vel2] = solver_vel2;
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
@@ -452,17 +491,10 @@ impl TwoBodyConstraint {
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.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();
#[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.tangent_impulse = self.elements[k].tangent_part.total_impulse();
}
}

129
src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
View File

@@ -1,9 +1,7 @@
use crate::dynamics::integration_parameters::{
BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY,
};
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::contact_constraint::OneBodyConstraintNormalPart;
use crate::dynamics::solver::SolverVel;
use crate::math::{AngVector, Vector, DIM};
use crate::math::{AngVector, TangentImpulse, Vector, DIM};
use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
use na::{Matrix2, Vector2};
use num::Zero;
@@ -47,67 +45,41 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
/// Total impulse applied across all the solver substeps.
#[inline]
#[cfg(feature = "dim2")]
pub fn total_impulse(&self) -> na::Vector1<N> {
self.impulse_accumulator + self.impulse
}
/// Total impulse applied across all the solver substeps.
#[inline]
#[cfg(feature = "dim3")]
pub fn total_impulse(&self) -> na::Vector2<N> {
pub fn total_impulse(&self) -> TangentImpulse<N> {
self.impulse_accumulator + self.impulse
}
#[inline]
pub fn apply_limit(
pub fn warmstart(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im1: &Vector<N>,
im2: &Vector<N>,
limit: N,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
if DISABLE_FRICTION_LIMIT_REAPPLY {
return;
}
#[cfg(feature = "dim2")]
{
let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0];
solver_vel1.angular += self.gcross1[0] * self.impulse[0];
solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda;
solver_vel1.angular += self.gcross1[0] * dlambda;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2[0] * dlambda;
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0];
solver_vel2.angular += self.gcross2[0] * self.impulse[0];
}
#[cfg(feature = "dim3")]
{
let new_impulse = self.impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0]
+ tangents1[1].component_mul(im1) * self.impulse[1];
solver_vel1.angular +=
self.gcross1[0] * self.impulse[0] + self.gcross1[1] * self.impulse[1];
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+ tangents1[1].component_mul(im1) * dlambda[1];
solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]
+ tangents1[1].component_mul(im2) * -self.impulse[1];
solver_vel2.angular +=
self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1];
}
}
@@ -220,6 +192,22 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
self.impulse_accumulator + self.impulse
}
#[inline]
pub fn warmstart(
&mut self,
dir1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) {
solver_vel1.linear += dir1.component_mul(im1) * self.impulse;
solver_vel1.angular += self.gcross1 * self.impulse;
solver_vel2.linear += dir1.component_mul(im2) * -self.impulse;
solver_vel2.angular += self.gcross2 * self.impulse;
}
#[inline]
pub fn solve(
&mut self,
@@ -339,6 +327,31 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
}
}
#[inline]
pub fn warmstart_group(
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
element
.normal_part
.warmstart(dir1, im1, im2, solver_vel1, solver_vel2);
element
.tangent_part
.warmstart(tangents1, im1, im2, solver_vel1, solver_vel2);
}
}
#[inline]
pub fn solve_group(
cfm_factor: N,
@@ -350,19 +363,13 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
limit: N,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
solve_normal: bool,
solve_restitution: bool,
solve_friction: bool,
) where
Vector<N>: SimdBasis,
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
// Solve penetration.
if solve_normal {
if solve_restitution {
if BLOCK_SOLVER_ENABLED {
for elements in elements.chunks_exact_mut(2) {
let [element_a, element_b] = elements else {
@@ -379,12 +386,6 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
solver_vel1,
solver_vel2,
);
for i in 0..2 {
let limit = limit * elements[i].normal_part.impulse;
let part = &mut elements[i].tangent_part;
part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
}
}
// There is one constraint left to solve if there isn’t an even number.
@@ -393,24 +394,22 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
element
.normal_part
.solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
}
} else {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
}
}
}
// Solve friction.
if solve_friction {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;

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);
}
}
}

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

@@ -1,5 +1,4 @@
use super::{JointConstraintTypes, SolverConstraintsSet};
use crate::dynamics::integration_parameters::DISABLE_FRICTION_LIMIT_REAPPLY;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::{
solver::{ContactConstraintTypes, SolverVel},
@@ -180,6 +179,10 @@ impl VelocitySolver {
joint_constraints.update(params, multibodies, &self.solver_bodies);
contact_constraints.update(params, substep_id, multibodies, &self.solver_bodies);
if params.warmstart_coefficient != 0.0 {
contact_constraints.warmstart(&mut self.solver_vels, &mut self.generic_solver_vels);
}
for _ in 0..params.num_internal_pgs_iterations {
joint_constraints.solve(&mut self.solver_vels, &mut self.generic_solver_vels);
contact_constraints
@@ -203,60 +206,17 @@ impl VelocitySolver {
/*
* Resolution without bias.
*/
let compute_max_dlinvel = |vels: &[SolverVel<Real>]| {
vels.iter()
.map(|v| v.linear.norm())
.max_by_key(|v| OrderedFloat(*v))
.unwrap_or_default()
};
let mut prev_dlinvel = f32::MAX;
let mut prev_solver_vels = self.solver_vels.clone();
for kk in 0..params.max_internal_stabilization_iterations {
prev_solver_vels.clone_from_slice(&self.solver_vels);
for _ in 0..params.num_internal_stabilization_iterations {
joint_constraints
.solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
contact_constraints.solve_restitution_wo_bias(
&mut self.solver_vels,
&mut self.generic_solver_vels,
);
if DISABLE_FRICTION_LIMIT_REAPPLY {
contact_constraints
.solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
}
for (prev, new) in prev_solver_vels.iter_mut().zip(self.solver_vels.iter()) {
*prev -= *new;
}
let new_max_linvel = compute_max_dlinvel(&self.solver_vels);
println!(">> {} >> max_linvel: {}", kk, new_max_linvel);
if new_max_linvel > prev_dlinvel {
break;
}
prev_dlinvel = new_max_linvel;
if prev_solver_vels
.iter()
.zip(self.solver_vels.iter())
.all(|(diff, vels)| {
diff.linear.norm() < 1.0e-3
|| diff.linear.norm() <= 0.2 * vels.linear.norm()
})
{
break;
}
// if (new_max_dlinvel - max_dlinvel).abs() <= 0.2 * max_dlinvel {
// println!("Num effective stab steps: {}", kk + 1);
// break;
// }
}
contact_constraints
.solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
}
}