From 0e4393ba9e408952395cfccaef6dc192886e2839 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Sun, 7 Mar 2021 17:15:32 +0100 Subject: Reduce code duplication between the SIMD and non-SIMD contact solve and warmstart. --- src/dynamics/solver/delta_vel.rs | 8 + src/dynamics/solver/mod.rs | 4 + src/dynamics/solver/velocity_constraint.rs | 203 +++------------- src/dynamics/solver/velocity_constraint_element.rs | 257 +++++++++++++++++++++ src/dynamics/solver/velocity_constraint_wide.rs | 192 +++------------ src/dynamics/solver/velocity_ground_constraint.rs | 172 +++----------- .../solver/velocity_ground_constraint_element.rs | 200 ++++++++++++++++ .../solver/velocity_ground_constraint_wide.rs | 167 +++---------- 8 files changed, 577 insertions(+), 626 deletions(-) create mode 100644 src/dynamics/solver/velocity_constraint_element.rs create mode 100644 src/dynamics/solver/velocity_ground_constraint_element.rs (limited to 'src') diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs index 378d302..b50cb76 100644 --- a/src/dynamics/solver/delta_vel.rs +++ b/src/dynamics/solver/delta_vel.rs @@ -1,5 +1,6 @@ use crate::math::{AngVector, Vector}; use na::{Scalar, SimdRealField}; +use std::ops::AddAssign; #[derive(Copy, Clone, Debug)] //#[repr(align(64))] @@ -16,3 +17,10 @@ impl DeltaVel { } } } + +impl AddAssign for DeltaVel { + fn add_assign(&mut self, rhs: Self) { + self.linear += rhs.linear; + self.angular += rhs.angular; + } +} diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs index 090d0f3..33b6f9a 100644 --- a/src/dynamics/solver/mod.rs +++ b/src/dynamics/solver/mod.rs @@ -24,9 +24,11 @@ pub(self) use position_ground_constraint::*; #[cfg(feature = "simd-is-enabled")] pub(self) use position_ground_constraint_wide::*; pub(self) use velocity_constraint::*; +pub(self) use velocity_constraint_element::*; #[cfg(feature = "simd-is-enabled")] pub(self) use velocity_constraint_wide::*; pub(self) use velocity_ground_constraint::*; +pub(self) use velocity_ground_constraint_element::*; #[cfg(feature = "simd-is-enabled")] pub(self) use velocity_ground_constraint_wide::*; @@ -55,9 +57,11 @@ mod position_solver; #[cfg(not(feature = "parallel"))] mod solver_constraints; mod velocity_constraint; +mod velocity_constraint_element; #[cfg(feature = "simd-is-enabled")] mod velocity_constraint_wide; mod velocity_ground_constraint; +mod velocity_ground_constraint_element; #[cfg(feature = "simd-is-enabled")] mod velocity_ground_constraint_wide; #[cfg(not(feature = "parallel"))] diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 169f280..434e6fb 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -1,13 +1,12 @@ -use super::DeltaVel; use crate::dynamics::solver::VelocityGroundConstraint; #[cfg(feature = "simd-is-enabled")] use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; -#[cfg(feature = "dim2")] -use na::SimdPartialOrd; + +use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart}; //#[repr(align(64))] #[derive(Copy, Clone, Debug)] @@ -78,72 +77,6 @@ impl AnyVelocityConstraint { } } -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityConstraintTangentPart { - pub gcross1: [AngVector; DIM - 1], - pub gcross2: [AngVector; DIM - 1], - pub rhs: [Real; DIM - 1], - #[cfg(feature = "dim2")] - pub impulse: [Real; DIM - 1], - #[cfg(feature = "dim3")] - pub impulse: na::Vector2, - pub r: [Real; DIM - 1], -} - -#[cfg(not(target_arch = "wasm32"))] -impl VelocityConstraintTangentPart { - fn zero() -> Self { - Self { - gcross1: [na::zero(); DIM - 1], - gcross2: [na::zero(); DIM - 1], - rhs: [0.0; DIM - 1], - #[cfg(feature = "dim2")] - impulse: [0.0; DIM - 1], - #[cfg(feature = "dim3")] - impulse: na::zero(), - r: [0.0; DIM - 1], - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityConstraintNormalPart { - pub gcross1: AngVector, - pub gcross2: AngVector, - pub rhs: Real, - pub impulse: Real, - pub r: Real, -} - -#[cfg(not(target_arch = "wasm32"))] -impl VelocityConstraintNormalPart { - fn zero() -> Self { - Self { - gcross1: na::zero(), - gcross2: na::zero(), - rhs: 0.0, - impulse: 0.0, - r: 0.0, - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityConstraintElement { - pub normal_part: VelocityConstraintNormalPart, - pub tangent_part: VelocityConstraintTangentPart, -} - -#[cfg(not(target_arch = "wasm32"))] -impl VelocityConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: VelocityConstraintNormalPart::zero(), - tangent_part: VelocityConstraintTangentPart::zero(), - } - } -} - #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityConstraint { pub dir1: Vector, // Non-penetration force direction for the first body. @@ -159,7 +92,7 @@ pub(crate) struct VelocityConstraint { pub manifold_id: ContactManifoldIndex, pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], + pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], } impl VelocityConstraint { @@ -352,116 +285,36 @@ impl VelocityConstraint { let mut mj_lambda1 = DeltaVel::zero(); let mut mj_lambda2 = DeltaVel::zero(); - #[cfg(feature = "dim3")] - let tangents1 = [self.tangent1, self.dir1.cross(&self.tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = self.dir1.orthonormal_basis(); - - for i in 0..self.num_contacts as usize { - let elt = &self.elements[i].normal_part; - mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse); - mj_lambda1.angular += elt.gcross1 * elt.impulse; - - mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse); - mj_lambda2.angular += elt.gcross2 * elt.impulse; - - for j in 0..DIM - 1 { - let elt = &self.elements[i].tangent_part; - mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse[j]); - mj_lambda1.angular += elt.gcross1[j] * elt.impulse[j]; - - mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse[j]); - mj_lambda2.angular += elt.gcross2[j] * elt.impulse[j]; - } - } - - mj_lambdas[self.mj_lambda1 as usize].linear += mj_lambda1.linear; - mj_lambdas[self.mj_lambda1 as usize].angular += mj_lambda1.angular; - mj_lambdas[self.mj_lambda2 as usize].linear += mj_lambda2.linear; - mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular; + VelocityConstraintElement::warmstart_group( + &self.elements, + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + self.im1, + self.im2, + &mut mj_lambda1, + &mut mj_lambda2, + ); + + mj_lambdas[self.mj_lambda1 as usize] += mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] += mj_lambda2; } pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - // Solve friction. - #[cfg(feature = "dim3")] - let bitangent1 = self.dir1.cross(&self.tangent1); - #[cfg(feature = "dim2")] - let tangents1 = self.dir1.orthonormal_basis(); - - #[cfg(feature = "dim2")] - for i in 0..self.num_contacts as usize { - let normal_elt = &self.elements[i].normal_part; - let elt = &mut self.elements[i].tangent_part; - let dimpulse = tangents1[0].dot(&mj_lambda1.linear) - + elt.gcross1[0].gdot(mj_lambda1.angular) - - tangents1[0].dot(&mj_lambda2.linear) - + elt.gcross2[0].gdot(mj_lambda2.angular) - + elt.rhs[0]; - let limit = self.limit * normal_elt.impulse; - let new_impulse = (elt.impulse[0] - elt.r[0] * dimpulse).simd_clamp(-limit, limit); - let dlambda = new_impulse - elt.impulse[0]; - elt.impulse[0] = new_impulse; - - mj_lambda1.linear += tangents1[0] * (self.im1 * dlambda); - mj_lambda1.angular += elt.gcross1[0] * dlambda; - - mj_lambda2.linear += tangents1[0] * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2[0] * dlambda; - } - - #[cfg(feature = "dim3")] - for i in 0..self.num_contacts as usize { - let limit = self.limit * self.elements[i].normal_part.impulse; - let elt = &mut self.elements[i].tangent_part; - - let dimpulse_0 = self.tangent1.dot(&mj_lambda1.linear) - + elt.gcross1[0].gdot(mj_lambda1.angular) - - self.tangent1.dot(&mj_lambda2.linear) - + elt.gcross2[0].gdot(mj_lambda2.angular) - + elt.rhs[0]; - let dimpulse_1 = bitangent1.dot(&mj_lambda1.linear) - + elt.gcross1[1].gdot(mj_lambda1.angular) - - bitangent1.dot(&mj_lambda2.linear) - + elt.gcross2[1].gdot(mj_lambda2.angular) - + elt.rhs[1]; - - let new_impulse = na::Vector2::new( - elt.impulse[0] - elt.r[0] * dimpulse_0, - elt.impulse[1] - elt.r[1] * dimpulse_1, - ); - let new_impulse = new_impulse.cap_magnitude(limit); - let dlambda = new_impulse - elt.impulse; - elt.impulse = new_impulse; - - mj_lambda1.linear += - self.tangent1 * (self.im1 * dlambda[0]) + bitangent1 * (self.im1 * dlambda[1]); - mj_lambda1.angular += elt.gcross1[0] * dlambda[0] + elt.gcross1[1] * dlambda[1]; - - mj_lambda2.linear += - self.tangent1 * (-self.im2 * dlambda[0]) + bitangent1 * (-self.im2 * dlambda[1]); - mj_lambda2.angular += elt.gcross2[0] * dlambda[0] + elt.gcross2[1] * dlambda[1]; - } - - // Solve non-penetration. - for i in 0..self.num_contacts as usize { - let elt = &mut self.elements[i].normal_part; - let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular) - - self.dir1.dot(&mj_lambda2.linear) - + elt.gcross2.gdot(mj_lambda2.angular) - + elt.rhs; - let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0); - let dlambda = new_impulse - elt.impulse; - elt.impulse = new_impulse; - - mj_lambda1.linear += self.dir1 * (self.im1 * dlambda); - mj_lambda1.angular += elt.gcross1 * dlambda; - - mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2 * dlambda; - } + VelocityConstraintElement::solve_group( + &mut self.elements, + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + self.im1, + self.im2, + self.limit, + &mut mj_lambda1, + &mut mj_lambda2, + ); mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs new file mode 100644 index 0000000..e80d3dd --- /dev/null +++ b/src/dynamics/solver/velocity_constraint_element.rs @@ -0,0 +1,257 @@ +use super::DeltaVel; +use crate::math::{AngVector, Vector, DIM}; +use crate::utils::{WBasis, WDot}; +use na::SimdRealField; + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityConstraintTangentPart { + pub gcross1: [AngVector; DIM - 1], + pub gcross2: [AngVector; DIM - 1], + pub rhs: [N; DIM - 1], + #[cfg(feature = "dim2")] + pub impulse: [N; DIM - 1], + #[cfg(feature = "dim3")] + pub impulse: na::Vector2, + pub r: [N; DIM - 1], +} + +impl VelocityConstraintTangentPart { + #[cfg(not(target_arch = "wasm32"))] + fn zero() -> Self { + Self { + gcross1: [na::zero(); DIM - 1], + gcross2: [na::zero(); DIM - 1], + rhs: [na::zero(); DIM - 1], + #[cfg(feature = "dim2")] + impulse: [na::zero(); DIM - 1], + #[cfg(feature = "dim3")] + impulse: na::zero(), + r: [na::zero(); DIM - 1], + } + } + + pub fn warmstart( + &self, + tangents1: [&Vector; DIM - 1], + im1: N, + im2: N, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) where + AngVector: WDot, Result = N>, + N::Element: SimdRealField, + { + for j in 0..DIM - 1 { + mj_lambda1.linear += tangents1[j] * (im1 * self.impulse[j]); + mj_lambda1.angular += self.gcross1[j] * self.impulse[j]; + + mj_lambda2.linear += tangents1[j] * (-im2 * self.impulse[j]); + mj_lambda2.angular += self.gcross2[j] * self.impulse[j]; + } + } + + pub fn solve( + &mut self, + tangents1: [&Vector; DIM - 1], + im1: N, + im2: N, + limit: N, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) where + AngVector: WDot, Result = N>, + N::Element: SimdRealField, + { + #[cfg(feature = "dim2")] + { + let dimpulse = tangents1[0].dot(&mj_lambda1.linear) + + self.gcross1[0].gdot(mj_lambda1.angular) + - tangents1[0].dot(&mj_lambda2.linear) + + self.gcross2[0].gdot(mj_lambda2.angular) + + self.rhs[0]; + let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit); + let dlambda = new_impulse - self.impulse[0]; + self.impulse[0] = new_impulse; + + mj_lambda1.linear += tangents1[0] * (im1 * dlambda); + mj_lambda1.angular += self.gcross1[0] * dlambda; + + mj_lambda2.linear += tangents1[0] * (-im2 * dlambda); + mj_lambda2.angular += self.gcross2[0] * dlambda; + } + + #[cfg(feature = "dim3")] + { + let dimpulse_0 = tangents1[0].dot(&mj_lambda1.linear) + + self.gcross1[0].gdot(mj_lambda1.angular) + - tangents1[0].dot(&mj_lambda2.linear) + + self.gcross2[0].gdot(mj_lambda2.angular) + + self.rhs[0]; + let dimpulse_1 = tangents1[1].dot(&mj_lambda1.linear) + + self.gcross1[1].gdot(mj_lambda1.angular) + - tangents1[1].dot(&mj_lambda2.linear) + + self.gcross2[1].gdot(mj_lambda2.angular) + + self.rhs[1]; + + let new_impulse = na::Vector2::new( + self.impulse[0] - self.r[0] * dimpulse_0, + self.impulse[1] - self.r[1] * dimpulse_1, + ); + let new_impulse = new_impulse.simd_cap_magnitude(limit); + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + mj_lambda1.linear += + tangents1[0] * (im1 * dlambda[0]) + tangents1[1] * (im1 * dlambda[1]); + mj_lambda1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1]; + + mj_lambda2.linear += + tangents1[0] * (-im2 * dlambda[0]) + tangents1[1] * (-im2 * dlambda[1]); + mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityConstraintNormalPart { + pub gcross1: AngVector, + pub gcross2: AngVector, + pub rhs: N, + pub impulse: N, + pub r: N, +} + +impl VelocityConstraintNormalPart { + #[cfg(not(target_arch = "wasm32"))] + fn zero() -> Self { + Self { + gcross1: na::zero(), + gcross2: na::zero(), + rhs: na::zero(), + impulse: na::zero(), + r: na::zero(), + } + } + + #[inline] + pub fn warmstart( + &self, + dir1: &Vector, + im1: N, + im2: N, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) where + AngVector: WDot, Result = N>, + { + mj_lambda1.linear += dir1 * (im1 * self.impulse); + mj_lambda1.angular += self.gcross1 * self.impulse; + + mj_lambda2.linear += dir1 * (-im2 * self.impulse); + mj_lambda2.angular += self.gcross2 * self.impulse; + } + + #[inline] + pub fn solve( + &mut self, + dir1: &Vector, + im1: N, + im2: N, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) where + AngVector: WDot, Result = N>, + { + let dimpulse = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular) + - dir1.dot(&mj_lambda2.linear) + + self.gcross2.gdot(mj_lambda2.angular) + + self.rhs; + let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero()); + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + mj_lambda1.linear += dir1 * (im1 * dlambda); + mj_lambda1.angular += self.gcross1 * dlambda; + + mj_lambda2.linear += dir1 * (-im2 * dlambda); + mj_lambda2.angular += self.gcross2 * dlambda; + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityConstraintElement { + pub normal_part: VelocityConstraintNormalPart, + pub tangent_part: VelocityConstraintTangentPart, +} + +impl VelocityConstraintElement { + #[cfg(not(target_arch = "wasm32"))] + pub fn zero() -> Self { + Self { + normal_part: VelocityConstraintNormalPart::zero(), + tangent_part: VelocityConstraintTangentPart::zero(), + } + } + + pub fn warmstart_group( + elements: &[Self], + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im1: N, + im2: N, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) where + Vector: WBasis, + AngVector: WDot, Result = N>, + N::Element: SimdRealField, + { + #[cfg(feature = "dim3")] + let tangents1 = [tangent1, &dir1.cross(&tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&dir1.orthonormal_vector()]; + + for element in elements { + element + .tangent_part + .warmstart(tangents1, im1, im2, mj_lambda1, mj_lambda2); + element + .normal_part + .warmstart(dir1, im1, im2, mj_lambda1, mj_lambda2); + } + } + + pub fn solve_group( + elements: &mut [Self], + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im1: N, + im2: N, + limit: N, + mj_lambda1: &mut DeltaVel, + mj_lambda2: &mut DeltaVel, + ) where + Vector: WBasis, + AngVector: WDot, Result = N>, + N::Element: SimdRealField, + { + // 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; + part.solve(tangents1, im1, im2, limit, mj_lambda1, mj_lambda2); + } + + // Solve penetration. + for element in elements.iter_mut() { + element + .normal_part + .solve(&dir1, im1, im2, mj_lambda1, mj_lambda2); + } + } +} diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index ec24e56..691a983 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -1,4 +1,6 @@ -use super::{AnyVelocityConstraint, DeltaVel}; +use super::{ + AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart, +}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ @@ -10,69 +12,6 @@ use crate::utils::{WAngularInertia, WCross, WDot}; use num::Zero; use simba::simd::{SimdPartialOrd, SimdValue}; -#[derive(Copy, Clone, Debug)] -pub(crate) struct WVelocityConstraintTangentPart { - pub gcross1: [AngVector; DIM - 1], - pub gcross2: [AngVector; DIM - 1], - pub rhs: [SimdReal; DIM - 1], - #[cfg(feature = "dim2")] - pub impulse: [SimdReal; DIM - 1], - #[cfg(feature = "dim3")] - pub impulse: na::Vector2, - pub r: [SimdReal; DIM - 1], -} - -impl WVelocityConstraintTangentPart { - pub fn zero() -> Self { - Self { - gcross1: [AngVector::zero(); DIM - 1], - gcross2: [AngVector::zero(); DIM - 1], - rhs: [SimdReal::zero(); DIM - 1], - #[cfg(feature = "dim2")] - impulse: [SimdReal::zero(); DIM - 1], - #[cfg(feature = "dim3")] - impulse: na::Vector2::zeros(), - r: [SimdReal::zero(); DIM - 1], - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct WVelocityConstraintNormalPart { - pub gcross1: AngVector, - pub gcross2: AngVector, - pub rhs: SimdReal, - pub impulse: SimdReal, - pub r: SimdReal, -} - -impl WVelocityConstraintNormalPart { - pub fn zero() -> Self { - Self { - gcross1: AngVector::zero(), - gcross2: AngVector::zero(), - rhs: SimdReal::zero(), - impulse: SimdReal::zero(), - r: SimdReal::zero(), - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct WVelocityConstraintElement { - pub normal_part: WVelocityConstraintNormalPart, - pub tangent_part: WVelocityConstraintTangentPart, -} - -impl WVelocityConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: WVelocityConstraintNormalPart::zero(), - tangent_part: WVelocityConstraintTangentPart::zero(), - } - } -} - #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityConstraint { pub dir1: Vector, // Non-penetration force direction for the first body. @@ -80,7 +19,7 @@ pub(crate) struct WVelocityConstraint { pub tangent1: Vector, // One of the friction force directions. #[cfg(feature = "dim3")] pub tangent_rot1: na::UnitComplex, // Orientation of the tangent basis wrt. the reference basis. - pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS], + pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, pub im1: SimdReal, pub im2: SimdReal, @@ -159,7 +98,7 @@ impl WVelocityConstraint { tangent1: tangents1[0], #[cfg(feature = "dim3")] tangent_rot1, - elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], + elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1, im2, limit: SimdReal::splat(0.0), @@ -212,7 +151,7 @@ impl WVelocityConstraint { rhs += dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting); - constraint.elements[k].normal_part = WVelocityConstraintNormalPart { + constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, rhs, @@ -277,28 +216,16 @@ impl WVelocityConstraint { ), }; - #[cfg(feature = "dim3")] - let tangents1 = [self.tangent1, self.dir1.cross(&self.tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = self.dir1.orthonormal_basis(); - - for i in 0..self.num_contacts as usize { - let elt = &self.elements[i].normal_part; - mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse); - mj_lambda1.angular += elt.gcross1 * elt.impulse; - - mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse); - mj_lambda2.angular += elt.gcross2 * elt.impulse; - - for j in 0..DIM - 1 { - let elt = &self.elements[i].tangent_part; - mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse[j]); - mj_lambda1.angular += elt.gcross1[j] * elt.impulse[j]; - - mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse[j]); - mj_lambda2.angular += elt.gcross2[j] * elt.impulse[j]; - } - } + VelocityConstraintElement::warmstart_group( + &self.elements, + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + self.im1, + self.im2, + &mut mj_lambda1, + &mut mj_lambda2, + ); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -329,82 +256,17 @@ impl WVelocityConstraint { ), }; - // Solve friction. - #[cfg(feature = "dim3")] - let bitangent1 = self.dir1.cross(&self.tangent1); - #[cfg(feature = "dim2")] - let tangents1 = self.dir1.orthonormal_basis(); - - #[cfg(feature = "dim2")] - for i in 0..self.num_contacts as usize { - let normal_elt = &self.elements[i].normal_part; - - let elt = &mut self.elements[i].tangent_part; - let dimpulse = tangents1[0].dot(&mj_lambda1.linear) - + elt.gcross1[0].gdot(mj_lambda1.angular) - - tangents1[0].dot(&mj_lambda2.linear) - + elt.gcross2[0].gdot(mj_lambda2.angular) - + elt.rhs[0]; - let limit = self.limit * normal_elt.impulse; - let new_impulse = (elt.impulse[0] - elt.r[0] * dimpulse).simd_clamp(-limit, limit); - let dlambda = new_impulse - elt.impulse[0]; - elt.impulse[0] = new_impulse; - - mj_lambda1.linear += tangents1[0] * (self.im1 * dlambda); - mj_lambda1.angular += elt.gcross1[0] * dlambda; - mj_lambda2.linear += tangents1[0] * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2[0] * dlambda; - } - - #[cfg(feature = "dim3")] - for i in 0..self.num_contacts as usize { - let limit = self.limit * self.elements[i].normal_part.impulse; - let elts = &mut self.elements[i].tangent_part; - - let dimpulse_0 = self.tangent1.dot(&mj_lambda1.linear) - + elts.gcross1[0].gdot(mj_lambda1.angular) - - self.tangent1.dot(&mj_lambda2.linear) - + elts.gcross2[0].gdot(mj_lambda2.angular) - + elts.rhs[0]; - let dimpulse_1 = bitangent1.dot(&mj_lambda1.linear) - + elts.gcross1[1].gdot(mj_lambda1.angular) - - bitangent1.dot(&mj_lambda2.linear) - + elts.gcross2[1].gdot(mj_lambda2.angular) - + elts.rhs[1]; - - let new_impulse = na::Vector2::new( - elts.impulse[0] - elts.r[0] * dimpulse_0, - elts.impulse[1] - elts.r[1] * dimpulse_1, - ); - let new_impulse = new_impulse.simd_cap_magnitude(limit); - let dlambda = new_impulse - elts.impulse; - elts.impulse = new_impulse; - - mj_lambda1.linear += - self.tangent1 * (self.im1 * dlambda[0]) + bitangent1 * (self.im1 * dlambda[1]); - mj_lambda1.angular += elts.gcross1[0] * dlambda[0] + elts.gcross1[1] * dlambda[1]; - - mj_lambda2.linear += - self.tangent1 * (-self.im2 * dlambda[0]) + bitangent1 * (-self.im2 * dlambda[1]); - mj_lambda2.angular += elts.gcross2[0] * dlambda[0] + elts.gcross2[1] * dlambda[1]; - } - - // Solve non-penetration after friction. - for i in 0..self.num_contacts as usize { - let elt = &mut self.elements[i].normal_part; - let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular) - - self.dir1.dot(&mj_lambda2.linear) - + elt.gcross2.gdot(mj_lambda2.angular) - + elt.rhs; - let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero()); - let dlambda = new_impulse - elt.impulse; - elt.impulse = new_impulse; - - mj_lambda1.linear += self.dir1 * (self.im1 * dlambda); - mj_lambda1.angular += elt.gcross1 * dlambda; - mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2 * dlambda; - } + VelocityConstraintElement::solve_group( + &mut self.elements, + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + self.im1, + self.im2, + self.limit, + &mut mj_lambda1, + &mut mj_lambda2, + ); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 78afd39..3509dd5 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -1,90 +1,30 @@ -use super::{AnyVelocityConstraint, DeltaVel}; -use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; +use super::{ + AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, + VelocityGroundConstraintNormalPart, +}; +use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; #[cfg(feature = "dim2")] use crate::utils::WBasis; use crate::utils::{WAngularInertia, WCross, WDot}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; -#[cfg(feature = "dim2")] -use na::SimdPartialOrd; - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityGroundConstraintTangentPart { - pub gcross2: [AngVector; DIM - 1], - pub rhs: [Real; DIM - 1], - #[cfg(feature = "dim2")] - pub impulse: [Real; DIM - 1], - #[cfg(feature = "dim3")] - pub impulse: na::Vector2, - pub r: [Real; DIM - 1], -} - -#[cfg(not(target_arch = "wasm32"))] -impl VelocityGroundConstraintTangentPart { - fn zero() -> Self { - Self { - gcross2: [na::zero(); DIM - 1], - rhs: [0.0; DIM - 1], - #[cfg(feature = "dim2")] - impulse: [0.0; DIM - 1], - #[cfg(feature = "dim3")] - impulse: na::zero(), - r: [0.0; DIM - 1], - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityGroundConstraintNormalPart { - pub gcross2: AngVector, - pub rhs: Real, - pub impulse: Real, - pub r: Real, -} - -#[cfg(not(target_arch = "wasm32"))] -impl VelocityGroundConstraintNormalPart { - fn zero() -> Self { - Self { - gcross2: na::zero(), - rhs: 0.0, - impulse: 0.0, - r: 0.0, - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityGroundConstraintElement { - pub normal_part: VelocityGroundConstraintNormalPart, - pub tangent_part: VelocityGroundConstraintTangentPart, -} - -#[cfg(not(target_arch = "wasm32"))] -impl VelocityGroundConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: VelocityGroundConstraintNormalPart::zero(), - tangent_part: VelocityGroundConstraintTangentPart::zero(), - } - } -} #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityGroundConstraint { + pub mj_lambda2: usize, pub dir1: Vector, // Non-penetration force direction for the first body. #[cfg(feature = "dim3")] pub tangent1: Vector, // One of the friction force directions. - #[cfg(feature = "dim3")] - pub tangent_rot1: na::UnitComplex, // Orientation of the tangent basis wrt. the reference basis. pub im2: Real, pub limit: Real, - pub mj_lambda2: usize, + pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], + + #[cfg(feature = "dim3")] + pub tangent_rot1: na::UnitComplex, // Orientation of the tangent basis wrt. the reference basis. pub manifold_id: ContactManifoldIndex, pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], } impl VelocityGroundConstraint { @@ -254,22 +194,15 @@ impl VelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel::zero(); - #[cfg(feature = "dim3")] - let tangents1 = [self.tangent1, self.dir1.cross(&self.tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = self.dir1.orthonormal_basis(); - - for i in 0..self.num_contacts as usize { - let elt = &self.elements[i].normal_part; - mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse); - mj_lambda2.angular += elt.gcross2 * elt.impulse; - for j in 0..DIM - 1 { - let elt = &self.elements[i].tangent_part; - mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse[j]); - mj_lambda2.angular += elt.gcross2[j] * elt.impulse[j]; - } - } + VelocityGroundConstraintElement::warmstart_group( + &self.elements, + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + self.im2, + &mut mj_lambda2, + ); mj_lambdas[self.mj_lambda2 as usize].linear += mj_lambda2.linear; mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular; @@ -278,66 +211,15 @@ impl VelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; - // Solve friction. - #[cfg(feature = "dim3")] - let bitangent1 = self.dir1.cross(&self.tangent1); - #[cfg(feature = "dim2")] - let tangents1 = self.dir1.orthonormal_basis(); - - #[cfg(feature = "dim2")] - for i in 0..self.num_contacts as usize { - let normal_elt = &self.elements[i].normal_part; - let elt = &mut self.elements[i].tangent_part; - let dimpulse = -tangents1[0].dot(&mj_lambda2.linear) - + elt.gcross2[0].gdot(mj_lambda2.angular) - + elt.rhs[0]; - let limit = self.limit * normal_elt.impulse; - let new_impulse = (elt.impulse[0] - elt.r[0] * dimpulse).simd_clamp(-limit, limit); - let dlambda = new_impulse - elt.impulse[0]; - elt.impulse[0] = new_impulse; - - mj_lambda2.linear += tangents1[0] * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2[0] * dlambda; - } - - #[cfg(feature = "dim3")] - for i in 0..self.num_contacts as usize { - let limit = self.limit * self.elements[i].normal_part.impulse; - let elts = &mut self.elements[i].tangent_part; - - let dimpulse_0 = -self.tangent1.dot(&mj_lambda2.linear) - + elts.gcross2[0].gdot(mj_lambda2.angular) - + elts.rhs[0]; - let dimpulse_1 = -bitangent1.dot(&mj_lambda2.linear) - + elts.gcross2[1].gdot(mj_lambda2.angular) - + elts.rhs[1]; - - let new_impulse = na::Vector2::new( - elts.impulse[0] - elts.r[0] * dimpulse_0, - elts.impulse[1] - elts.r[1] * dimpulse_1, - ); - let new_impulse = new_impulse.cap_magnitude(limit); - let dlambda = new_impulse - elts.impulse; - - elts.impulse = new_impulse; - - mj_lambda2.linear += - self.tangent1 * (-self.im2 * dlambda[0]) + bitangent1 * (-self.im2 * dlambda[1]); - mj_lambda2.angular += elts.gcross2[0] * dlambda[0] + elts.gcross2[1] * dlambda[1]; - } - - // Solve penetration. - for i in 0..self.num_contacts as usize { - let elt = &mut self.elements[i].normal_part; - let dimpulse = - -self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs; - let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0); - let dlambda = new_impulse - elt.impulse; - elt.impulse = new_impulse; - - mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2 * dlambda; - } + VelocityGroundConstraintElement::solve_group( + &mut self.elements, + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + self.im2, + self.limit, + &mut mj_lambda2, + ); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs new file mode 100644 index 0000000..43e3e01 --- /dev/null +++ b/src/dynamics/solver/velocity_ground_constraint_element.rs @@ -0,0 +1,200 @@ +use super::DeltaVel; +use crate::math::{AngVector, Vector, DIM}; +use crate::utils::{WBasis, WDot}; +use na::SimdRealField; + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityGroundConstraintTangentPart { + pub gcross2: [AngVector; DIM - 1], + pub rhs: [N; DIM - 1], + #[cfg(feature = "dim2")] + pub impulse: [N; DIM - 1], + #[cfg(feature = "dim3")] + pub impulse: na::Vector2, + pub r: [N; DIM - 1], +} + +impl VelocityGroundConstraintTangentPart { + #[cfg(not(target_arch = "wasm32"))] + fn zero() -> Self { + Self { + gcross2: [na::zero(); DIM - 1], + rhs: [na::zero(); DIM - 1], + #[cfg(feature = "dim2")] + impulse: [na::zero(); DIM - 1], + #[cfg(feature = "dim3")] + impulse: na::zero(), + r: [na::zero(); DIM - 1], + } + } + + pub fn warmstart( + &self, + tangents1: [&Vector; DIM - 1], + im2: N, + mj_lambda2: &mut DeltaVel, + ) { + for j in 0..DIM - 1 { + mj_lambda2.linear += tangents1[j] * (-im2 * self.impulse[j]); + mj_lambda2.angular += self.gcross2[j] * self.impulse[j]; + } + } + + pub fn solve( + &mut self, + tangents1: [&Vector; DIM - 1], + im2: N, + limit: N, + mj_lambda2: &mut DeltaVel, + ) where + AngVector: WDot, Result = N>, + N::Element: SimdRealField, + { + #[cfg(feature = "dim2")] + { + let dimpulse = -tangents1[0].dot(&mj_lambda2.linear) + + self.gcross2[0].gdot(mj_lambda2.angular) + + self.rhs[0]; + let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit); + let dlambda = new_impulse - self.impulse[0]; + self.impulse[0] = new_impulse; + + mj_lambda2.linear += tangents1[0] * (-im2 * dlambda); + mj_lambda2.angular += self.gcross2[0] * dlambda; + } + + #[cfg(feature = "dim3")] + { + let dimpulse_0 = -tangents1[0].dot(&mj_lambda2.linear) + + self.gcross2[0].gdot(mj_lambda2.angular) + + self.rhs[0]; + let dimpulse_1 = -tangents1[1].dot(&mj_lambda2.linear) + + self.gcross2[1].gdot(mj_lambda2.angular) + + self.rhs[1]; + + let new_impulse = na::Vector2::new( + self.impulse[0] - self.r[0] * dimpulse_0, + self.impulse[1] - self.r[1] * dimpulse_1, + ); + let new_impulse = new_impulse.simd_cap_magnitude(limit); + let dlambda = new_impulse - self.impulse; + + self.impulse = new_impulse; + + mj_lambda2.linear += + tangents1[0] * (-im2 * dlambda[0]) + tangents1[1] * (-im2 * dlambda[1]); + mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityGroundConstraintNormalPart { + pub gcross2: AngVector, + pub rhs: N, + pub impulse: N, + pub r: N, +} + +impl VelocityGroundConstraintNormalPart { + #[cfg(not(target_arch = "wasm32"))] + fn zero() -> Self { + Self { + gcross2: na::zero(), + rhs: na::zero(), + impulse: na::zero(), + r: na::zero(), + } + } + + #[inline] + pub fn warmstart(&self, dir1: &Vector, im2: N, mj_lambda2: &mut DeltaVel) { + mj_lambda2.linear += dir1 * (-im2 * self.impulse); + mj_lambda2.angular += self.gcross2 * self.impulse; + } + + #[inline] + pub fn solve(&mut self, dir1: &Vector, im2: N, mj_lambda2: &mut DeltaVel) + where + AngVector: WDot, Result = N>, + { + let dimpulse = + -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs; + let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero()); + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + + mj_lambda2.linear += dir1 * (-im2 * dlambda); + mj_lambda2.angular += self.gcross2 * dlambda; + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityGroundConstraintElement { + pub normal_part: VelocityGroundConstraintNormalPart, + pub tangent_part: VelocityGroundConstraintTangentPart, +} + +impl VelocityGroundConstraintElement { + #[cfg(not(target_arch = "wasm32"))] + pub fn zero() -> Self { + Self { + normal_part: VelocityGroundConstraintNormalPart::zero(), + tangent_part: VelocityGroundConstraintTangentPart::zero(), + } + } + + #[inline] + pub fn warmstart_group( + elements: &[Self], + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im2: N, + mj_lambda2: &mut DeltaVel, + ) where + Vector: WBasis, + AngVector: WDot, Result = N>, + N::Element: SimdRealField, + { + #[cfg(feature = "dim3")] + let tangents1 = [tangent1, &dir1.cross(&tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&dir1.orthonormal_vector()]; + + for element in elements { + element.normal_part.warmstart(dir1, im2, mj_lambda2); + element.tangent_part.warmstart(tangents1, im2, mj_lambda2); + } + } + + #[inline] + pub fn solve_group( + elements: &mut [Self], + dir1: &Vector, + #[cfg(feature = "dim3")] tangent1: &Vector, + im2: N, + limit: N, + mj_lambda2: &mut DeltaVel, + ) where + Vector: WBasis, + AngVector: WDot, Result = N>, + N::Element: SimdRealField, + { + // 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; + part.solve(tangents1, im2, limit, mj_lambda2); + } + + // Solve penetration. + for element in elements.iter_mut() { + element.normal_part.solve(&dir1, im2, mj_lambda2); + } + } +} diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 913a458..22da895 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -1,4 +1,7 @@ -use super::{AnyVelocityConstraint, DeltaVel}; +use super::{ + AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, + VelocityGroundConstraintNormalPart, +}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ @@ -10,65 +13,6 @@ use crate::utils::{WAngularInertia, WCross, WDot}; use num::Zero; use simba::simd::{SimdPartialOrd, SimdValue}; -#[derive(Copy, Clone, Debug)] -pub(crate) struct WVelocityGroundConstraintTangentPart { - pub gcross2: [AngVector; DIM - 1], - pub rhs: [SimdReal; DIM - 1], - #[cfg(feature = "dim2")] - pub impulse: [SimdReal; DIM - 1], - #[cfg(feature = "dim3")] - pub impulse: na::Vector2, - pub r: [SimdReal; DIM - 1], -} - -impl WVelocityGroundConstraintTangentPart { - pub fn zero() -> Self { - Self { - gcross2: [AngVector::zero(); DIM - 1], - rhs: [SimdReal::zero(); DIM - 1], - #[cfg(feature = "dim2")] - impulse: [SimdReal::zero(); DIM - 1], - #[cfg(feature = "dim3")] - impulse: na::zero(), - r: [SimdReal::zero(); DIM - 1], - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct WVelocityGroundConstraintNormalPart { - pub gcross2: AngVector, - pub rhs: SimdReal, - pub impulse: SimdReal, - pub r: SimdReal, -} - -impl WVelocityGroundConstraintNormalPart { - pub fn zero() -> Self { - Self { - gcross2: AngVector::zero(), - rhs: SimdReal::zero(), - impulse: SimdReal::zero(), - r: SimdReal::zero(), - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct WVelocityGroundConstraintElement { - pub normal_part: WVelocityGroundConstraintNormalPart, - pub tangent_part: WVelocityGroundConstraintTangentPart, -} - -impl WVelocityGroundConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: WVelocityGroundConstraintNormalPart::zero(), - tangent_part: WVelocityGroundConstraintTangentPart::zero(), - } - } -} - #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityGroundConstraint { pub dir1: Vector, // Non-penetration force direction for the first body. @@ -76,7 +20,7 @@ pub(crate) struct WVelocityGroundConstraint { pub tangent1: Vector, // One of the friction force directions. #[cfg(feature = "dim3")] pub tangent_rot1: na::UnitComplex, // Orientation of the tangent basis wrt. the reference basis. - pub elements: [WVelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], + pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, pub im2: SimdReal, pub limit: SimdReal, @@ -151,7 +95,7 @@ impl WVelocityGroundConstraint { tangent1: tangents1[0], #[cfg(feature = "dim3")] tangent_rot1, - elements: [WVelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], + elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2, limit: SimdReal::splat(0.0), mj_lambda2, @@ -199,7 +143,7 @@ impl WVelocityGroundConstraint { rhs += dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting); - constraint.elements[k].normal_part = WVelocityGroundConstraintNormalPart { + constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2, rhs, impulse: impulse * warmstart_coeff, @@ -249,22 +193,14 @@ impl WVelocityGroundConstraint { ), }; - #[cfg(feature = "dim3")] - let tangents1 = [self.tangent1, self.dir1.cross(&self.tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = self.dir1.orthonormal_basis(); - - for i in 0..self.num_contacts as usize { - let elt = &self.elements[i].normal_part; - mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse); - mj_lambda2.angular += elt.gcross2 * elt.impulse; - - for j in 0..DIM - 1 { - let elt = &self.elements[i].tangent_part; - mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse[j]); - mj_lambda2.angular += elt.gcross2[j] * elt.impulse[j]; - } - } + VelocityGroundConstraintElement::warmstart_group( + &self.elements, + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + self.im2, + &mut mj_lambda2, + ); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); @@ -275,73 +211,22 @@ impl WVelocityGroundConstraint { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( - array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], ), angular: AngVector::from( - array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], ), }; - // Solve friction. - #[cfg(feature = "dim3")] - let bitangent1 = self.dir1.cross(&self.tangent1); - #[cfg(feature = "dim2")] - let tangents1 = self.dir1.orthonormal_basis(); - - #[cfg(feature = "dim2")] - for i in 0..self.num_contacts as usize { - let normal_elt = &self.elements[i].normal_part; - - let elt = &mut self.elements[i].tangent_part; - let dimpulse = -tangents1[0].dot(&mj_lambda2.linear) - + elt.gcross2[0].gdot(mj_lambda2.angular) - + elt.rhs[0]; - let limit = self.limit * normal_elt.impulse; - let new_impulse = (elt.impulse[0] - elt.r[0] * dimpulse).simd_clamp(-limit, limit); - let dlambda = new_impulse - elt.impulse[0]; - elt.impulse[0] = new_impulse; - - mj_lambda2.linear += tangents1[0] * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2[0] * dlambda; - } - - #[cfg(feature = "dim3")] - for i in 0..self.num_contacts as usize { - let limit = self.limit * self.elements[i].normal_part.impulse; - let elts = &mut self.elements[i].tangent_part; - - let dimpulse_0 = -self.tangent1.dot(&mj_lambda2.linear) - + elts.gcross2[0].gdot(mj_lambda2.angular) - + elts.rhs[0]; - let dimpulse_1 = -bitangent1.dot(&mj_lambda2.linear) - + elts.gcross2[1].gdot(mj_lambda2.angular) - + elts.rhs[1]; - - let new_impulse = na::Vector2::new( - elts.impulse[0] - elts.r[0] * dimpulse_0, - elts.impulse[1] - elts.r[1] * dimpulse_1, - ); - let new_impulse = new_impulse.simd_cap_magnitude(limit); - let dlambda = new_impulse - elts.impulse; - elts.impulse = new_impulse; - - mj_lambda2.linear += - self.tangent1 * (-self.im2 * dlambda[0]) + bitangent1 * (-self.im2 * dlambda[1]); - mj_lambda2.angular += elts.gcross2[0] * dlambda[0] + elts.gcross2[1] * dlambda[1]; - } - - // Solve non-penetration after friction. - for i in 0..self.num_contacts as usize { - let elt = &mut self.elements[i].normal_part; - let dimpulse = - -self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs; - let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero()); - let dlambda = new_impulse - elt.impulse; - elt.impulse = new_impulse; - - mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2 * dlambda; - } + VelocityGroundConstraintElement::solve_group( + &mut self.elements, + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + self.im2, + self.limit, + &mut mj_lambda2, + ); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); -- cgit