feat: solver improvements + release v0.29.0 (#876)
* 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
This commit is contained in:
Sébastien Crozet
GitHub
@@ -0,0 +1,505 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user