diff options
Diffstat (limited to 'src/dynamics/solver/velocity_ground_constraint_element.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint_element.rs | 40 |
1 files changed, 23 insertions, 17 deletions
diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs index 4d4d3c3..a843905 100644 --- a/src/dynamics/solver/velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/velocity_ground_constraint_element.rs @@ -11,17 +11,22 @@ pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> { pub impulse: na::Vector1<N>, #[cfg(feature = "dim3")] pub impulse: na::Vector2<N>, - pub r: [N; DIM - 1], + #[cfg(feature = "dim2")] + pub r: [N; 1], + #[cfg(feature = "dim3")] + pub r: [N; DIM], } impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> { - #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))] fn zero() -> Self { Self { gcross2: [na::zero(); DIM - 1], rhs: [na::zero(); DIM - 1], impulse: na::zero(), - r: [na::zero(); DIM - 1], + #[cfg(feature = "dim2")] + r: [na::zero(); 1], + #[cfg(feature = "dim3")] + r: [na::zero(); DIM], } } @@ -38,10 +43,10 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> { { #[cfg(feature = "dim2")] { - let dimpulse = -tangents1[0].dot(&mj_lambda2.linear) + let dvel = -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 new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit); let dlambda = new_impulse - self.impulse[0]; self.impulse[0] = new_impulse; @@ -51,17 +56,22 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> { #[cfg(feature = "dim3")] { - let dimpulse_0 = -tangents1[0].dot(&mj_lambda2.linear) + let dvel_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) + let dvel_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 dvel_00 = dvel_0 * dvel_0; + let dvel_11 = dvel_1 * dvel_1; + let dvel_01 = dvel_0 * dvel_1; + let inv_lhs = (dvel_00 + dvel_11) + * crate::utils::simd_inv( + dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2], + ); + let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1]; + let new_impulse = self.impulse - delta_impulse; let new_impulse = { let _disable_fe_except = crate::utils::DisableFloatingPointExceptionsFlags:: @@ -69,7 +79,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> { new_impulse.simd_cap_magnitude(limit) }; let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0] @@ -89,7 +98,6 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> { } impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> { - #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))] fn zero() -> Self { Self { gcross2: na::zero(), @@ -105,9 +113,8 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> { where AngVector<N>: WDot<AngVector<N>, 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 dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs; + let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero()); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; @@ -123,7 +130,6 @@ pub(crate) struct VelocityGroundConstraintElement<N: SimdRealField + Copy> { } impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> { - #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))] pub fn zero() -> Self { Self { normal_part: VelocityGroundConstraintNormalPart::zero(), |