134f433903
* feat: solver improvements * feat: add function to get/set whether gyroscopic forces are enabled on a rigid-body * chore: switch to released versions of parry and wide instead of local patches * fix cargo doc * chore: typo fixes * chore: clippy fix * Release v0.29.0 * chore: more clippy fixes
506 lines
21 KiB
Rust
506 lines
21 KiB
Rust
use super::{ContactConstraintNormalPart, ContactConstraintTangentPart};
|
||
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
|
||
use crate::dynamics::solver::solver_body::SolverBodies;
|
||
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
|
||
use crate::geometry::{ContactManifold, ContactManifoldIndex, SimdSolverContact};
|
||
use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH, SimdReal, Vector};
|
||
#[cfg(feature = "dim2")]
|
||
use crate::utils::SimdBasis;
|
||
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
|
||
use num::Zero;
|
||
use parry::utils::SdpMatrix2;
|
||
use simba::simd::{SimdPartialOrd, SimdValue};
|
||
|
||
#[derive(Copy, Clone, Debug)]
|
||
pub struct CoulombContactPointInfos<N: SimdRealCopy> {
|
||
pub tangent_vel: Vector<N>, // PERF: could be one float less, be shared by both contact point infos?
|
||
pub normal_vel: N,
|
||
pub local_p1: Point<N>,
|
||
pub local_p2: Point<N>,
|
||
pub dist: N,
|
||
}
|
||
|
||
impl<N: SimdRealCopy> Default for CoulombContactPointInfos<N> {
|
||
fn default() -> Self {
|
||
Self {
|
||
tangent_vel: Vector::zeros(),
|
||
normal_vel: N::zero(),
|
||
local_p1: Point::origin(),
|
||
local_p2: Point::origin(),
|
||
dist: N::zero(),
|
||
}
|
||
}
|
||
}
|
||
|
||
#[derive(Copy, Clone, Debug)]
|
||
pub(crate) struct ContactWithCoulombFrictionBuilder {
|
||
infos: [CoulombContactPointInfos<SimdReal>; MAX_MANIFOLD_POINTS],
|
||
}
|
||
|
||
impl ContactWithCoulombFrictionBuilder {
|
||
pub fn generate(
|
||
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
|
||
manifolds: [&ContactManifold; SIMD_WIDTH],
|
||
bodies: &RigidBodySet,
|
||
solver_bodies: &SolverBodies,
|
||
out_builders: &mut [ContactWithCoulombFrictionBuilder],
|
||
out_constraints: &mut [ContactWithCoulombFriction],
|
||
) {
|
||
// TODO: could we avoid having to fetch the ids here? It’s the only thing we
|
||
// read from the original rigid-bodies.
|
||
let ids1: [u32; SIMD_WIDTH] = array![|ii| if manifolds[ii].data.relative_dominance <= 0
|
||
&& manifold_id[ii] != usize::MAX
|
||
{
|
||
let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check.
|
||
bodies[handle].ids.active_set_offset
|
||
} else {
|
||
u32::MAX
|
||
}];
|
||
let ids2: [u32; SIMD_WIDTH] = array![|ii| if manifolds[ii].data.relative_dominance >= 0
|
||
&& manifold_id[ii] != usize::MAX
|
||
{
|
||
let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check.
|
||
bodies[handle].ids.active_set_offset
|
||
} else {
|
||
u32::MAX
|
||
}];
|
||
|
||
let vels1 = solver_bodies.gather_vels(ids1);
|
||
let poses1 = solver_bodies.gather_poses(ids1);
|
||
let vels2 = solver_bodies.gather_vels(ids2);
|
||
let poses2 = solver_bodies.gather_poses(ids2);
|
||
|
||
let world_com1 = Point::from(poses1.pose.translation.vector);
|
||
let world_com2 = Point::from(poses2.pose.translation.vector);
|
||
|
||
// TODO PERF: implement SIMD gather
|
||
let force_dir1 = -Vector::<SimdReal>::from(gather![|ii| manifolds[ii].data.normal]);
|
||
let num_active_contacts = manifolds[0].data.num_active_contacts();
|
||
|
||
#[cfg(feature = "dim2")]
|
||
let tangents1 = force_dir1.orthonormal_basis();
|
||
#[cfg(feature = "dim3")]
|
||
let tangents1 =
|
||
super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear);
|
||
|
||
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
|
||
let manifold_points =
|
||
array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]];
|
||
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
|
||
|
||
let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
|
||
let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
|
||
|
||
constraint.dir1 = force_dir1;
|
||
constraint.im1 = poses1.im;
|
||
constraint.im2 = poses2.im;
|
||
constraint.solver_vel1 = ids1;
|
||
constraint.solver_vel2 = ids2;
|
||
constraint.manifold_id = manifold_id;
|
||
constraint.num_contacts = num_points as u8;
|
||
#[cfg(feature = "dim3")]
|
||
{
|
||
constraint.tangent1 = tangents1[0];
|
||
}
|
||
|
||
for k in 0..num_points {
|
||
// SAFETY: we already know that the `manifold_points` has `num_points` elements
|
||
// so `k` isn’t out of bounds.
|
||
let solver_contact =
|
||
unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) };
|
||
|
||
let is_bouncy = solver_contact.is_bouncy();
|
||
|
||
let dp1 = solver_contact.point - world_com1;
|
||
let dp2 = solver_contact.point - world_com2;
|
||
|
||
let vel1 = vels1.linear + vels1.angular.gcross(dp1);
|
||
let vel2 = vels2.linear + vels2.angular.gcross(dp2);
|
||
|
||
constraint.limit = solver_contact.friction;
|
||
constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8);
|
||
|
||
// Normal part.
|
||
let normal_rhs_wo_bias;
|
||
{
|
||
let torque_dir1 = dp1.gcross(force_dir1);
|
||
let torque_dir2 = dp2.gcross(-force_dir1);
|
||
let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1);
|
||
let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2);
|
||
|
||
let imsum = poses1.im + poses2.im;
|
||
let projected_mass = utils::simd_inv(
|
||
force_dir1.dot(&imsum.component_mul(&force_dir1))
|
||
+ ii_torque_dir1.gdot(torque_dir1)
|
||
+ ii_torque_dir2.gdot(torque_dir2),
|
||
);
|
||
|
||
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
|
||
normal_rhs_wo_bias =
|
||
is_bouncy * solver_contact.restitution * projected_velocity;
|
||
|
||
constraint.normal_part[k].torque_dir1 = torque_dir1;
|
||
constraint.normal_part[k].torque_dir2 = torque_dir2;
|
||
constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1;
|
||
constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2;
|
||
constraint.normal_part[k].impulse = solver_contact.warmstart_impulse;
|
||
constraint.normal_part[k].r = projected_mass;
|
||
}
|
||
|
||
// tangent parts.
|
||
constraint.tangent_part[k].impulse = solver_contact.warmstart_tangent_impulse;
|
||
|
||
for j in 0..DIM - 1 {
|
||
let torque_dir1 = dp1.gcross(tangents1[j]);
|
||
let torque_dir2 = dp2.gcross(-tangents1[j]);
|
||
let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1);
|
||
let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2);
|
||
|
||
let imsum = poses1.im + poses2.im;
|
||
|
||
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
|
||
+ ii_torque_dir1.gdot(torque_dir1)
|
||
+ ii_torque_dir2.gdot(torque_dir2);
|
||
let rhs_wo_bias = solver_contact.tangent_velocity.dot(&tangents1[j]);
|
||
|
||
constraint.tangent_part[k].torque_dir1[j] = torque_dir1;
|
||
constraint.tangent_part[k].torque_dir2[j] = torque_dir2;
|
||
constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1;
|
||
constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2;
|
||
constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias;
|
||
constraint.tangent_part[k].rhs[j] = rhs_wo_bias;
|
||
constraint.tangent_part[k].r[j] = if cfg!(feature = "dim2") {
|
||
utils::simd_inv(r)
|
||
} else {
|
||
r
|
||
};
|
||
}
|
||
|
||
#[cfg(feature = "dim3")]
|
||
{
|
||
// TODO PERF: we already applied the inverse inertia to the torque
|
||
// dire before. Could we reuse the value instead of retransforming?
|
||
constraint.tangent_part[k].r[2] = SimdReal::splat(2.0)
|
||
* (constraint.tangent_part[k].ii_torque_dir1[0]
|
||
.gdot(constraint.tangent_part[k].torque_dir1[1])
|
||
+ constraint.tangent_part[k].ii_torque_dir2[0]
|
||
.gdot(constraint.tangent_part[k].torque_dir2[1]));
|
||
}
|
||
|
||
// Builder.
|
||
builder.infos[k].local_p1 =
|
||
poses1.pose.inverse_transform_point(&solver_contact.point);
|
||
builder.infos[k].local_p2 =
|
||
poses2.pose.inverse_transform_point(&solver_contact.point);
|
||
builder.infos[k].tangent_vel = solver_contact.tangent_velocity;
|
||
builder.infos[k].dist = solver_contact.dist;
|
||
builder.infos[k].normal_vel = normal_rhs_wo_bias;
|
||
}
|
||
|
||
if BLOCK_SOLVER_ENABLED {
|
||
// Coupling between consecutive pairs.
|
||
for k in 0..num_points / 2 {
|
||
let k0 = k * 2;
|
||
let k1 = k * 2 + 1;
|
||
|
||
let imsum = poses1.im + poses2.im;
|
||
let r0 = constraint.normal_part[k0].r;
|
||
let r1 = constraint.normal_part[k1].r;
|
||
|
||
let mut r_mat = SdpMatrix2::zero();
|
||
|
||
// TODO PERF: we already applied the inverse inertia to the torque
|
||
// dire before. Could we reuse the value instead of retransforming?
|
||
r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
|
||
+ constraint.normal_part[k0]
|
||
.ii_torque_dir1
|
||
.gdot(constraint.normal_part[k1].torque_dir1)
|
||
+ constraint.normal_part[k0]
|
||
.ii_torque_dir2
|
||
.gdot(constraint.normal_part[k1].torque_dir2);
|
||
r_mat.m11 = utils::simd_inv(r0);
|
||
r_mat.m22 = utils::simd_inv(r1);
|
||
|
||
let (inv, det) = {
|
||
let _disable_fe_except =
|
||
crate::utils::DisableFloatingPointExceptionsFlags::
|
||
disable_floating_point_exceptions();
|
||
r_mat.inverse_and_get_determinant_unchecked()
|
||
};
|
||
let is_invertible = det.simd_gt(SimdReal::zero());
|
||
|
||
// If inversion failed, the contacts are redundant.
|
||
// Ignore the one with the smallest depth (it is too late to
|
||
// have the constraint removed from the constraint set, so just
|
||
// set the mass (r) matrix elements to 0.
|
||
constraint.normal_part[k0].r_mat_elts = [
|
||
inv.m11.select(is_invertible, r0),
|
||
inv.m22.select(is_invertible, SimdReal::zero()),
|
||
];
|
||
constraint.normal_part[k1].r_mat_elts = [
|
||
inv.m12.select(is_invertible, SimdReal::zero()),
|
||
r_mat.m12.select(is_invertible, SimdReal::zero()),
|
||
];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pub fn update(
|
||
&self,
|
||
params: &IntegrationParameters,
|
||
solved_dt: Real,
|
||
bodies: &SolverBodies,
|
||
_multibodies: &MultibodyJointSet,
|
||
constraint: &mut ContactWithCoulombFriction,
|
||
) {
|
||
let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
|
||
let inv_dt = SimdReal::splat(params.inv_dt());
|
||
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
|
||
let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
|
||
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
|
||
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
|
||
|
||
let poses1 = bodies.gather_poses(constraint.solver_vel1);
|
||
let poses2 = bodies.gather_poses(constraint.solver_vel2);
|
||
let all_infos = &self.infos[..constraint.num_contacts as usize];
|
||
let normal_parts = &mut constraint.normal_part[..constraint.num_contacts as usize];
|
||
let tangent_parts = &mut constraint.tangent_part[..constraint.num_contacts as usize];
|
||
|
||
#[cfg(feature = "dim2")]
|
||
let tangents1 = constraint.dir1.orthonormal_basis();
|
||
#[cfg(feature = "dim3")]
|
||
let tangents1 = [
|
||
constraint.tangent1,
|
||
constraint.dir1.cross(&constraint.tangent1),
|
||
];
|
||
|
||
let solved_dt = SimdReal::splat(solved_dt);
|
||
|
||
for ((info, normal_part), tangent_part) in all_infos
|
||
.iter()
|
||
.zip(normal_parts.iter_mut())
|
||
.zip(tangent_parts.iter_mut())
|
||
{
|
||
// NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
|
||
let p1 = poses1.pose * info.local_p1 + info.tangent_vel * solved_dt;
|
||
let p2 = poses2.pose * info.local_p2;
|
||
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
|
||
|
||
// Normal part.
|
||
{
|
||
let rhs_wo_bias = info.normal_vel + dist.simd_max(SimdReal::zero()) * inv_dt;
|
||
let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
|
||
.simd_clamp(-max_corrective_velocity, SimdReal::zero());
|
||
let new_rhs = rhs_wo_bias + rhs_bias;
|
||
|
||
normal_part.rhs_wo_bias = rhs_wo_bias;
|
||
normal_part.rhs = new_rhs;
|
||
normal_part.impulse_accumulator += normal_part.impulse;
|
||
normal_part.impulse *= warmstart_coeff;
|
||
}
|
||
|
||
// tangent parts.
|
||
{
|
||
tangent_part.impulse_accumulator += tangent_part.impulse;
|
||
tangent_part.impulse *= warmstart_coeff;
|
||
|
||
for j in 0..DIM - 1 {
|
||
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
|
||
tangent_part.rhs[j] = tangent_part.rhs_wo_bias[j] + bias;
|
||
}
|
||
}
|
||
}
|
||
|
||
constraint.cfm_factor = cfm_factor;
|
||
}
|
||
}
|
||
|
||
#[derive(Copy, Clone, Debug)]
|
||
#[repr(C)]
|
||
pub(crate) struct ContactWithCoulombFriction {
|
||
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
|
||
pub im1: Vector<SimdReal>,
|
||
pub im2: Vector<SimdReal>,
|
||
pub cfm_factor: SimdReal,
|
||
pub limit: SimdReal,
|
||
|
||
#[cfg(feature = "dim3")]
|
||
pub tangent1: Vector<SimdReal>, // One of the friction force directions.
|
||
pub normal_part: [ContactConstraintNormalPart<SimdReal>; MAX_MANIFOLD_POINTS],
|
||
pub tangent_part: [ContactConstraintTangentPart<SimdReal>; MAX_MANIFOLD_POINTS],
|
||
pub solver_vel1: [u32; SIMD_WIDTH],
|
||
pub solver_vel2: [u32; SIMD_WIDTH],
|
||
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
|
||
pub num_contacts: u8,
|
||
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
|
||
}
|
||
|
||
impl ContactWithCoulombFriction {
|
||
pub fn warmstart(&mut self, bodies: &mut SolverBodies) {
|
||
let mut solver_vel1 = bodies.gather_vels(self.solver_vel1);
|
||
let mut solver_vel2 = bodies.gather_vels(self.solver_vel2);
|
||
|
||
let normal_parts = &mut self.normal_part[..self.num_contacts as usize];
|
||
let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize];
|
||
|
||
/*
|
||
* Warmstart restitution.
|
||
*/
|
||
for normal_part in normal_parts.iter_mut() {
|
||
normal_part.warmstart(
|
||
&self.dir1,
|
||
&self.im1,
|
||
&self.im2,
|
||
&mut solver_vel1,
|
||
&mut solver_vel2,
|
||
);
|
||
}
|
||
|
||
/*
|
||
* Warmstart friction.
|
||
*/
|
||
#[cfg(feature = "dim3")]
|
||
let tangents1 = [&self.tangent1, &self.dir1.cross(&self.tangent1)];
|
||
#[cfg(feature = "dim2")]
|
||
let tangents1 = [&self.dir1.orthonormal_vector()];
|
||
|
||
for tangent_part in tangent_parts.iter_mut() {
|
||
tangent_part.warmstart(
|
||
tangents1,
|
||
&self.im1,
|
||
&self.im2,
|
||
&mut solver_vel1,
|
||
&mut solver_vel2,
|
||
);
|
||
}
|
||
|
||
bodies.scatter_vels(self.solver_vel1, solver_vel1);
|
||
bodies.scatter_vels(self.solver_vel2, solver_vel2);
|
||
}
|
||
|
||
pub fn solve(
|
||
&mut self,
|
||
bodies: &mut SolverBodies,
|
||
solve_restitution: bool,
|
||
solve_friction: bool,
|
||
) {
|
||
let mut solver_vel1 = bodies.gather_vels(self.solver_vel1);
|
||
let mut solver_vel2 = bodies.gather_vels(self.solver_vel2);
|
||
|
||
let normal_parts = &mut self.normal_part[..self.num_contacts as usize];
|
||
let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize];
|
||
|
||
/*
|
||
* Solve restitution.
|
||
*/
|
||
if solve_restitution {
|
||
if BLOCK_SOLVER_ENABLED {
|
||
for normal_part in normal_parts.chunks_exact_mut(2) {
|
||
let [normal_part_a, normal_part_b] = normal_part else {
|
||
unreachable!()
|
||
};
|
||
|
||
ContactConstraintNormalPart::solve_pair(
|
||
normal_part_a,
|
||
normal_part_b,
|
||
self.cfm_factor,
|
||
&self.dir1,
|
||
&self.im1,
|
||
&self.im2,
|
||
&mut solver_vel1,
|
||
&mut solver_vel2,
|
||
);
|
||
}
|
||
|
||
// There is one constraint left to solve if there isn’t an even number.
|
||
if normal_parts.len() % 2 == 1 {
|
||
let normal_part = normal_parts.last_mut().unwrap();
|
||
normal_part.solve(
|
||
self.cfm_factor,
|
||
&self.dir1,
|
||
&self.im1,
|
||
&self.im2,
|
||
&mut solver_vel1,
|
||
&mut solver_vel2,
|
||
);
|
||
}
|
||
} else {
|
||
for normal_part in normal_parts.iter_mut() {
|
||
normal_part.solve(
|
||
self.cfm_factor,
|
||
&self.dir1,
|
||
&self.im1,
|
||
&self.im2,
|
||
&mut solver_vel1,
|
||
&mut solver_vel2,
|
||
);
|
||
}
|
||
}
|
||
}
|
||
|
||
/*
|
||
* Solve friction.
|
||
*/
|
||
if solve_friction {
|
||
#[cfg(feature = "dim3")]
|
||
let tangents1 = [&self.tangent1, &self.dir1.cross(&self.tangent1)];
|
||
#[cfg(feature = "dim2")]
|
||
let tangents1 = [&self.dir1.orthonormal_vector()];
|
||
|
||
for (tangent_part, normal_part) in tangent_parts.iter_mut().zip(normal_parts.iter()) {
|
||
let limit = self.limit * normal_part.impulse;
|
||
tangent_part.solve(
|
||
tangents1,
|
||
&self.im1,
|
||
&self.im2,
|
||
limit,
|
||
&mut solver_vel1,
|
||
&mut solver_vel2,
|
||
);
|
||
}
|
||
}
|
||
|
||
bodies.scatter_vels(self.solver_vel1, solver_vel1);
|
||
bodies.scatter_vels(self.solver_vel2, solver_vel2);
|
||
}
|
||
|
||
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
|
||
for k in 0..self.num_contacts as usize {
|
||
#[cfg(not(feature = "simd-is-enabled"))]
|
||
let warmstart_impulses: [_; SIMD_WIDTH] = [self.normal_part[k].impulse];
|
||
#[cfg(feature = "simd-is-enabled")]
|
||
let warmstart_impulses: [_; SIMD_WIDTH] = self.normal_part[k].impulse.into();
|
||
let warmstart_tangent_impulses = self.tangent_part[k].impulse;
|
||
#[cfg(not(feature = "simd-is-enabled"))]
|
||
let impulses: [_; SIMD_WIDTH] = [self.normal_part[k].total_impulse()];
|
||
#[cfg(feature = "simd-is-enabled")]
|
||
let impulses: [_; SIMD_WIDTH] = self.normal_part[k].total_impulse().into();
|
||
let tangent_impulses = self.tangent_part[k].total_impulse();
|
||
|
||
for ii in 0..SIMD_WIDTH {
|
||
if self.manifold_id[ii] != usize::MAX {
|
||
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];
|
||
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
|
||
self.cfm_factor = SimdReal::splat(1.0);
|
||
for elt in &mut self.normal_part {
|
||
elt.rhs = elt.rhs_wo_bias;
|
||
}
|
||
for elt in &mut self.tangent_part {
|
||
elt.rhs = elt.rhs_wo_bias;
|
||
}
|
||
}
|
||
}
|