diff options
Diffstat (limited to 'src/dynamics/solver/contact_constraint/one_body_constraint_element.rs')
| -rw-r--r-- | src/dynamics/solver/contact_constraint/one_body_constraint_element.rs | 165 |
1 files changed, 122 insertions, 43 deletions
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs index d9ff7f4..7ec8c5d 100644 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs @@ -1,20 +1,17 @@ +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; #[derive(Copy, Clone, Debug)] 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 total_impulse: na::Vector1<N>, - #[cfg(feature = "dim3")] - pub total_impulse: na::Vector2<N>, + pub impulse: TangentImpulse<N>, + pub impulse_accumulator: TangentImpulse<N>, #[cfg(feature = "dim2")] pub r: [N; 1], #[cfg(feature = "dim3")] @@ -28,7 +25,7 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> { rhs: [na::zero(); DIM - 1], rhs_wo_bias: [na::zero(); DIM - 1], impulse: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), #[cfg(feature = "dim2")] r: [na::zero(); 1], #[cfg(feature = "dim3")] @@ -36,41 +33,31 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> { } } + /// Total impulse applied across all the solver substeps. #[inline] - pub fn apply_limit( + pub fn total_impulse(&self) -> TangentImpulse<N> { + self.impulse_accumulator + self.impulse + } + + #[inline] + 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>, - { + ) { #[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]; } } @@ -137,8 +124,9 @@ pub(crate) struct OneBodyConstraintNormalPart<N: SimdRealCopy> { pub rhs: N, pub rhs_wo_bias: N, pub impulse: N, - pub total_impulse: N, + pub impulse_accumulator: N, pub r: N, + pub r_mat_elts: [N; 2], } impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> { @@ -148,11 +136,24 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> { rhs: na::zero(), rhs_wo_bias: na::zero(), impulse: na::zero(), - total_impulse: na::zero(), + impulse_accumulator: na::zero(), r: na::zero(), + r_mat_elts: [N::zero(); 2], } } + /// Total impulse applied across all the solver substeps. + #[inline] + pub fn total_impulse(&self) -> 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, @@ -172,6 +173,44 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> { solver_vel2.linear += dir1.component_mul(im2) * -dlambda; solver_vel2.angular += self.gcross2 * dlambda; } + + #[inline] + pub fn solve_pair( + constraint_a: &mut Self, + constraint_b: &mut Self, + cfm_factor: N, + dir1: &Vector<N>, + im2: &Vector<N>, + solver_vel2: &mut SolverVel<N>, + ) where + AngVector<N>: SimdDot<AngVector<N>, Result = N>, + { + let dvel_a = -dir1.dot(&solver_vel2.linear) + + constraint_a.gcross2.gdot(solver_vel2.angular) + + constraint_a.rhs; + let dvel_b = -dir1.dot(&solver_vel2.linear) + + constraint_b.gcross2.gdot(solver_vel2.angular) + + constraint_b.rhs; + + let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse); + let new_impulse = TwoBodyConstraintNormalPart::solve_mlcp_two_constraints( + Vector2::new(dvel_a, dvel_b), + prev_impulse, + constraint_a.r, + constraint_b.r, + constraint_a.r_mat_elts, + constraint_b.r_mat_elts, + cfm_factor, + ); + + let dlambda = new_impulse - prev_impulse; + + constraint_a.impulse = new_impulse.x; + constraint_b.impulse = new_impulse.y; + + solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y); + solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y; + } } #[derive(Copy, Clone, Debug)] @@ -189,6 +228,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, elements: &mut [Self], @@ -210,13 +268,34 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> { // Solve penetration. if solve_normal { - 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); + if BLOCK_SOLVER_ENABLED { + for elements in elements.chunks_exact_mut(2) { + let [element_a, element_b] = elements else { + unreachable!() + }; + + OneBodyConstraintNormalPart::solve_pair( + &mut element_a.normal_part, + &mut element_b.normal_part, + cfm_factor, + dir1, + im2, + solver_vel2, + ); + } + + if elements.len() % 2 == 1 { + let element = elements.last_mut().unwrap(); + element + .normal_part + .solve(cfm_factor, dir1, im2, solver_vel2); + } + } else { + for element in elements.iter_mut() { + element + .normal_part + .solve(cfm_factor, dir1, im2, solver_vel2); + } } } |