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/velocity_constraint_element.rs | 257 +++++++++++++++++++++ 1 file changed, 257 insertions(+) create mode 100644 src/dynamics/solver/velocity_constraint_element.rs (limited to 'src/dynamics/solver/velocity_constraint_element.rs') 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); + } + } +} -- cgit From 152ada67ecd122fe38a9cae8b262542b4abf25fc Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Sun, 7 Mar 2021 17:34:49 +0100 Subject: Fix regression causing invalid contacts to be solved. --- src/dynamics/solver/velocity_constraint_element.rs | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/dynamics/solver/velocity_constraint_element.rs') diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs index e80d3dd..1324c35 100644 --- a/src/dynamics/solver/velocity_constraint_element.rs +++ b/src/dynamics/solver/velocity_constraint_element.rs @@ -30,6 +30,7 @@ impl VelocityConstraintTangentPart { } } + #[inline] pub fn warmstart( &self, tangents1: [&Vector; DIM - 1], @@ -50,6 +51,7 @@ impl VelocityConstraintTangentPart { } } + #[inline] pub fn solve( &mut self, tangents1: [&Vector; DIM - 1], @@ -193,6 +195,7 @@ impl VelocityConstraintElement { } } + #[inline] pub fn warmstart_group( elements: &[Self], dir1: &Vector, @@ -221,6 +224,7 @@ impl VelocityConstraintElement { } } + #[inline] pub fn solve_group( elements: &mut [Self], dir1: &Vector, -- cgit