diff --git a/build/rapier3d-f64/Cargo.toml b/build/rapier3d-f64/Cargo.toml index 54d4892..e201811 100644 --- a/build/rapier3d-f64/Cargo.toml +++ b/build/rapier3d-f64/Cargo.toml @@ -39,7 +39,7 @@ required-features = [ "dim3", "f64" ] vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" -nalgebra = "0.25" +nalgebra = "^0.25.3" parry3d-f64 = "0.2" simba = "0.4" approx = "0.4" diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index ccd0d08..0a83872 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -39,7 +39,7 @@ required-features = [ "dim3", "f32" ] vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" -nalgebra = "0.25" +nalgebra = "^0.25.3" parry3d = "0.2" simba = "0.4" approx = "0.4" diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index 551eaf1..fc3acb1 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -40,7 +40,7 @@ impl PhysicsHooks for OneWayPlatformHook { allowed_local_n1 = -Vector2::y(); } else if context.collider_handle2 == self.platform2 { // Flip the allowed direction. - allowed_local_n1 = -Vector2::y(); + allowed_local_n1 = Vector2::y(); } // Call the helper function that simulates one-way platforms. diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 8a71665..724aa45 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -19,6 +19,7 @@ mod debug_add_remove_collider3; mod debug_boxes3; mod debug_cylinder3; mod debug_dynamic_collider_add3; +mod debug_friction3; mod debug_infinite_fall3; mod debug_rollback3; mod debug_triangle3; @@ -99,6 +100,7 @@ pub fn main() { "(Debug) dyn. coll. add", debug_dynamic_collider_add3::init_world, ), + ("(Debug) friction", debug_friction3::init_world), ("(Debug) triangle", debug_triangle3::init_world), ("(Debug) trimesh", debug_trimesh3::init_world), ("(Debug) cylinder", debug_cylinder3::init_world), diff --git a/examples3d/debug_friction3.rs b/examples3d/debug_friction3.rs new file mode 100644 index 0000000..5cf40b3 --- /dev/null +++ b/examples3d/debug_friction3.rs @@ -0,0 +1,50 @@ +use na::{Point3, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 100.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static().build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size) + .friction(1.5) + .build(); + colliders.insert(collider, handle, &mut bodies); + + // Build a dynamic box rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(0.0, 1.1, 0.0) + .rotation(Vector3::y() * 0.3) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(2.0, 1.0, 3.0).friction(1.5).build(); + colliders.insert(collider, handle, &mut bodies); + + let rigid_body = &mut bodies[handle]; + let force = rigid_body.position() * Vector3::z() * 50.0; + rigid_body.set_linvel(force, true); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index 173d03d..d117a5b 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -40,7 +40,7 @@ impl PhysicsHooks for OneWayPlatformHook { allowed_local_n1 = -Vector3::y(); } else if context.collider_handle2 == self.platform2 { // Flip the allowed direction. - allowed_local_n1 = -Vector3::y(); + allowed_local_n1 = Vector3::y(); } // Call the helper function that simulates one-way platforms. diff --git a/src/dynamics/coefficient_combine_rule.rs b/src/dynamics/coefficient_combine_rule.rs index 5e8b4a0..2c66888 100644 --- a/src/dynamics/coefficient_combine_rule.rs +++ b/src/dynamics/coefficient_combine_rule.rs @@ -25,7 +25,7 @@ impl CoefficientCombineRule { let effective_rule = rule_value1.max(rule_value2); match effective_rule { - 0 => (coeff1 + coeff1) / 2.0, + 0 => (coeff1 + coeff2) / 2.0, 1 => coeff1.min(coeff2), 2 => coeff1 * coeff2, _ => coeff1.max(coeff2), 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 243d7d7..643facb 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -1,12 +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}; -use simba::simd::SimdPartialOrd; + +use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart}; //#[repr(align(64))] #[derive(Copy, Clone, Debug)] @@ -77,47 +77,13 @@ impl AnyVelocityConstraint { } } -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityConstraintElementPart { - pub gcross1: AngVector, - pub gcross2: AngVector, - pub rhs: Real, - pub impulse: Real, - pub r: Real, -} - -#[cfg(not(target_arch = "wasm32"))] -impl VelocityConstraintElementPart { - 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: VelocityConstraintElementPart, - pub tangent_part: [VelocityConstraintElementPart; DIM - 1], -} - -#[cfg(not(target_arch = "wasm32"))] -impl VelocityConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: VelocityConstraintElementPart::zero(), - tangent_part: [VelocityConstraintElementPart::zero(); DIM - 1], - } - } -} - #[derive(Copy, Clone, Debug)] pub(crate) struct VelocityConstraint { 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 im1: Real, pub im2: Real, pub limit: Real, @@ -126,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 { @@ -156,6 +122,12 @@ impl VelocityConstraint { let force_dir1 = -manifold.data.normal; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let (tangents1, tangent_rot1) = + super::compute_tangent_contact_directions(&force_dir1, &rb1.linvel, &rb2.linvel); + for (_l, manifold_points) in manifold .data .solver_contacts @@ -165,6 +137,10 @@ impl VelocityConstraint { #[cfg(not(target_arch = "wasm32"))] let mut constraint = VelocityConstraint { dir1: force_dir1, + #[cfg(feature = "dim3")] + tangent1: tangents1[0], + #[cfg(feature = "dim3")] + tangent_rot1, elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1: rb1.effective_inv_mass, im2: rb2.effective_inv_mass, @@ -203,7 +179,7 @@ impl VelocityConstraint { .as_nongrouped_mut() .unwrap() } else { - unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. + unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. }; #[cfg(target_arch = "wasm32")] @@ -254,7 +230,7 @@ impl VelocityConstraint { rhs *= is_bouncy + is_resting * params.velocity_solve_fraction; rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); - constraint.elements[k].normal_part = VelocityConstraintElementPart { + constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, rhs, @@ -265,7 +241,12 @@ impl VelocityConstraint { // Tangent parts. { - let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let impulse = + tangent_rot1 * manifold_points[k].data.tangent_impulse * warmstart_coeff; + #[cfg(feature = "dim2")] + let impulse = [manifold_points[k].data.tangent_impulse * warmstart_coeff]; + constraint.elements[k].tangent_part.impulse = impulse; for j in 0..DIM - 1 { let gcross1 = rb1 @@ -281,18 +262,11 @@ impl VelocityConstraint { + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]); - #[cfg(feature = "dim2")] - let impulse = manifold_point.data.tangent_impulse * warmstart_coeff; - #[cfg(feature = "dim3")] - let impulse = manifold_point.data.tangent_impulse[j] * warmstart_coeff; - constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart { - gcross1, - gcross2, - rhs, - impulse, - r, - }; + constraint.elements[k].tangent_part.gcross1[j] = gcross1; + constraint.elements[k].tangent_part.gcross2[j] = gcross2; + constraint.elements[k].tangent_part.rhs[j] = rhs; + constraint.elements[k].tangent_part.r[j] = r; } } } @@ -311,79 +285,36 @@ impl VelocityConstraint { let mut mj_lambda1 = DeltaVel::zero(); let mut mj_lambda2 = DeltaVel::zero(); - 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; + VelocityConstraintElement::warmstart_group( + &self.elements[..self.num_contacts as usize], + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + self.im1, + self.im2, + &mut mj_lambda1, + &mut mj_lambda2, + ); - mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse); - mj_lambda2.angular += elt.gcross2 * elt.impulse; - - // FIXME: move this out of the for loop? - let tangents1 = self.dir1.orthonormal_basis(); - - for j in 0..DIM - 1 { - let elt = &self.elements[i].tangent_part[j]; - mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse); - mj_lambda1.angular += elt.gcross1 * elt.impulse; - - mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse); - mj_lambda2.angular += elt.gcross2 * elt.impulse; - } - } - - 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; + 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. - for i in 0..self.num_contacts as usize { - let tangents1 = self.dir1.orthonormal_basis(); - - for j in 0..DIM - 1 { - let normal_elt = &self.elements[i].normal_part; - let elt = &mut self.elements[i].tangent_part[j]; - let dimpulse = tangents1[j].dot(&mj_lambda1.linear) - + elt.gcross1.gdot(mj_lambda1.angular) - - tangents1[j].dot(&mj_lambda2.linear) - + elt.gcross2.gdot(mj_lambda2.angular) - + elt.rhs; - let limit = self.limit * normal_elt.impulse; - let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit); - let dlambda = new_impulse - elt.impulse; - elt.impulse = new_impulse; - - mj_lambda1.linear += tangents1[j] * (self.im1 * dlambda); - mj_lambda1.angular += elt.gcross1 * dlambda; - - mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2 * dlambda; - } - } - - // 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.num_contacts as usize], + &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; @@ -398,15 +329,58 @@ impl VelocityConstraint { active_contact.data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse; + active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; } #[cfg(feature = "dim3")] { - active_contact.data.tangent_impulse = [ - self.elements[k].tangent_part[0].impulse, - self.elements[k].tangent_part[1].impulse, - ]; + active_contact.data.tangent_impulse = self + .tangent_rot1 + .inverse_transform_vector(&self.elements[k].tangent_part.impulse); } } } } + +#[inline(always)] +#[cfg(feature = "dim3")] +pub(crate) fn compute_tangent_contact_directions( + force_dir1: &Vector, + linvel1: &Vector, + linvel2: &Vector, +) -> ([Vector; DIM - 1], na::UnitComplex) +where + N: na::SimdRealField, + N::Element: na::RealField, + Vector: WBasis, +{ + use na::SimdValue; + + // Compute the tangent direction. Pick the direction of + // the linear relative velocity, if it is not too small. + // Otherwise use a fallback direction. + let relative_linvel = linvel1 - linvel2; + let mut tangent_relative_linvel = + relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel)); + let tangent_linvel_norm = tangent_relative_linvel.normalize_mut(); + let threshold: N::Element = na::convert(1.0e-4); + let use_fallback = tangent_linvel_norm.simd_lt(N::splat(threshold)); + let tangent_fallback = force_dir1.orthonormal_vector(); + + let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel); + let bitangent1 = force_dir1.cross(&tangent1); + + // Rotation such that: rot * tangent_fallback = tangent1 + // (when projected in the tangent plane.) This is needed to ensure the + // warmstart impulse has the correct orientation. Indeed, at frame n + 1, + // we need to reapply the same impulse as we did in frame n. However the + // basis on which the tangent impulse is expresses may change at each frame + // (because the the relative linvel may change direction at each frame). + // So we need this rotation to: + // - Project the impulse back to the "reference" basis at after friction is resolved. + // - Project the old impulse on the new basis before the friction is resolved. + let rot = na::UnitComplex::new_unchecked(na::Complex::new( + tangent1.dot(&tangent_fallback), + bitangent1.dot(&tangent_fallback), + )); + ([tangent1, bitangent1], rot) +} diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs new file mode 100644 index 0000000..1324c35 --- /dev/null +++ b/src/dynamics/solver/velocity_constraint_element.rs @@ -0,0 +1,261 @@ +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], + } + } + + #[inline] + 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]; + } + } + + #[inline] + 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(), + } + } + + #[inline] + 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); + } + } + + #[inline] + 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 d97602c..673af54 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -1,53 +1,25 @@ -use super::{AnyVelocityConstraint, DeltaVel}; +use super::{ + AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart, +}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; -use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; +#[cfg(feature = "dim2")] +use crate::utils::WBasis; +use crate::utils::{WAngularInertia, WCross, WDot}; use num::Zero; use simba::simd::{SimdPartialOrd, SimdValue}; -#[derive(Copy, Clone, Debug)] -pub(crate) struct WVelocityConstraintElementPart { - pub gcross1: AngVector, - pub gcross2: AngVector, - pub rhs: SimdReal, - pub impulse: SimdReal, - pub r: SimdReal, -} - -impl WVelocityConstraintElementPart { - 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: WVelocityConstraintElementPart, - pub tangent_parts: [WVelocityConstraintElementPart; DIM - 1], -} - -impl WVelocityConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: WVelocityConstraintElementPart::zero(), - tangent_parts: [WVelocityConstraintElementPart::zero(); DIM - 1], - } - } -} - #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityConstraint { pub dir1: Vector, // Non-penetration force direction for the first body. - pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS], + #[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 elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, pub im1: SimdReal, pub im2: SimdReal, @@ -108,6 +80,12 @@ impl WVelocityConstraint { let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); let num_active_contacts = manifolds[0].data.num_active_contacts(); + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let (tangents1, tangent_rot1) = + super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2); + 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]; SIMD_WIDTH @@ -116,7 +94,11 @@ impl WVelocityConstraint { let mut constraint = WVelocityConstraint { dir1: force_dir1, - elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], + #[cfg(feature = "dim3")] + tangent1: tangents1[0], + #[cfg(feature = "dim3")] + tangent_rot1, + elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1, im2, limit: SimdReal::splat(0.0), @@ -169,7 +151,7 @@ impl WVelocityConstraint { rhs += dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting); - constraint.elements[k].normal_part = WVelocityConstraintElementPart { + constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, rhs, @@ -179,31 +161,30 @@ impl WVelocityConstraint { } // tangent parts. - let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim2")] + let impulse = [SimdReal::from( + array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], + )]; - for j in 0..DIM - 1 { - #[cfg(feature = "dim2")] - let impulse = SimdReal::from( + #[cfg(feature = "dim3")] + let impulse = tangent_rot1 + * na::Vector2::from( array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); - #[cfg(feature = "dim3")] - let impulse = SimdReal::from( - array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], - ); + constraint.elements[k].tangent_part.impulse = impulse; + + for j in 0..DIM - 1 { let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]); - constraint.elements[k].tangent_parts[j] = WVelocityConstraintElementPart { - gcross1, - gcross2, - rhs, - impulse: impulse * warmstart_coeff, - r, - }; + constraint.elements[k].tangent_part.gcross1[j] = gcross1; + constraint.elements[k].tangent_part.gcross2[j] = gcross2; + constraint.elements[k].tangent_part.rhs[j] = rhs; + constraint.elements[k].tangent_part.r[j] = r; } } @@ -235,26 +216,16 @@ impl WVelocityConstraint { ), }; - 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; - - // FIXME: move this out of the for loop? - let tangents1 = self.dir1.orthonormal_basis(); - - for j in 0..DIM - 1 { - let elt = &self.elements[i].tangent_parts[j]; - mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse); - mj_lambda1.angular += elt.gcross1 * elt.impulse; - - mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse); - mj_lambda2.angular += elt.gcross2 * elt.impulse; - } - } + VelocityConstraintElement::warmstart_group( + &self.elements[..self.num_contacts as usize], + &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); @@ -278,54 +249,24 @@ impl WVelocityConstraint { 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 first. - for i in 0..self.num_contacts as usize { - // FIXME: move this out of the for loop? - let tangents1 = self.dir1.orthonormal_basis(); - let normal_elt = &self.elements[i].normal_part; - - for j in 0..DIM - 1 { - let elt = &mut self.elements[i].tangent_parts[j]; - let dimpulse = tangents1[j].dot(&mj_lambda1.linear) - + elt.gcross1.gdot(mj_lambda1.angular) - - tangents1[j].dot(&mj_lambda2.linear) - + elt.gcross2.gdot(mj_lambda2.angular) - + elt.rhs; - let limit = self.limit * normal_elt.impulse; - let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit); - let dlambda = new_impulse - elt.impulse; - elt.impulse = new_impulse; - - mj_lambda1.linear += tangents1[j] * (self.im1 * dlambda); - mj_lambda1.angular += elt.gcross1 * dlambda; - mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2 * dlambda; - } - } - - // 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.num_contacts as usize], + &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); @@ -340,11 +281,12 @@ impl WVelocityConstraint { pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { for k in 0..self.num_contacts as usize { let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); - let tangent_impulses: [_; SIMD_WIDTH] = - self.elements[k].tangent_parts[0].impulse.into(); + #[cfg(feature = "dim2")] + let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into(); #[cfg(feature = "dim3")] - let bitangent_impulses: [_; SIMD_WIDTH] = - self.elements[k].tangent_parts[1].impulse.into(); + let tangent_impulses = self + .tangent_rot1 + .inverse_transform_vector(&self.elements[k].tangent_part.impulse); for ii in 0..SIMD_WIDTH { let manifold = &mut manifolds_all[self.manifold_id[ii]]; @@ -358,8 +300,7 @@ impl WVelocityConstraint { } #[cfg(feature = "dim3")] { - active_contact.data.tangent_impulse = - [tangent_impulses[ii], bitangent_impulses[ii]]; + active_contact.data.tangent_impulse = tangent_impulses.extract(ii); } } } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 4e195cd..cabce5e 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -1,57 +1,30 @@ -use super::{AnyVelocityConstraint, DeltaVel}; -use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; -use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; +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}; -use simba::simd::SimdPartialOrd; - -#[derive(Copy, Clone, Debug)] -pub(crate) struct VelocityGroundConstraintElementPart { - pub gcross2: AngVector, - pub rhs: Real, - pub impulse: Real, - pub r: Real, -} - -#[cfg(not(target_arch = "wasm32"))] -impl VelocityGroundConstraintElementPart { - 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: VelocityGroundConstraintElementPart, - pub tangent_part: [VelocityGroundConstraintElementPart; DIM - 1], -} - -#[cfg(not(target_arch = "wasm32"))] -impl VelocityGroundConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: VelocityGroundConstraintElementPart::zero(), - tangent_part: [VelocityGroundConstraintElementPart::zero(); DIM - 1], - } - } -} #[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. 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 { @@ -77,6 +50,12 @@ impl VelocityGroundConstraint { (-manifold.data.normal, 1.0) }; + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let (tangents1, tangent_rot1) = + super::compute_tangent_contact_directions(&force_dir1, &rb1.linvel, &rb2.linvel); + let mj_lambda2 = rb2.active_set_offset; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; @@ -89,6 +68,10 @@ impl VelocityGroundConstraint { #[cfg(not(target_arch = "wasm32"))] let mut constraint = VelocityGroundConstraint { dir1: force_dir1, + #[cfg(feature = "dim3")] + tangent1: tangents1[0], + #[cfg(feature = "dim3")] + tangent_rot1, elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2: rb2.effective_inv_mass, limit: 0.0, @@ -166,7 +149,7 @@ impl VelocityGroundConstraint { rhs *= is_bouncy + is_resting * params.velocity_solve_fraction; rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); - constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { + constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2, rhs, impulse: manifold_point.data.impulse * warmstart_coeff, @@ -176,7 +159,12 @@ impl VelocityGroundConstraint { // Tangent parts. { - let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let impulse = + tangent_rot1 * manifold_points[k].data.tangent_impulse * warmstart_coeff; + #[cfg(feature = "dim2")] + let impulse = [manifold_points[k].data.tangent_impulse * warmstart_coeff]; + constraint.elements[k].tangent_part.impulse = impulse; for j in 0..DIM - 1 { let gcross2 = rb2 @@ -186,18 +174,10 @@ impl VelocityGroundConstraint { let rhs = (vel1 - vel2 + flipped_multiplier * manifold_point.tangent_velocity) .dot(&tangents1[j]); - #[cfg(feature = "dim2")] - let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; - #[cfg(feature = "dim3")] - let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff; - constraint.elements[k].tangent_part[j] = - VelocityGroundConstraintElementPart { - gcross2, - rhs, - impulse, - r, - }; + constraint.elements[k].tangent_part.gcross2[j] = gcross2; + constraint.elements[k].tangent_part.rhs[j] = rhs; + constraint.elements[k].tangent_part.r[j] = r; } } } @@ -214,19 +194,15 @@ impl VelocityGroundConstraint { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { let mut mj_lambda2 = DeltaVel::zero(); - 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[j]; - mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse); - mj_lambda2.angular += elt.gcross2 * elt.impulse; - } - } + VelocityGroundConstraintElement::warmstart_group( + &self.elements[..self.num_contacts as usize], + &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; @@ -235,38 +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. - let tangents1 = self.dir1.orthonormal_basis(); - - for i in 0..self.num_contacts as usize { - for j in 0..DIM - 1 { - let normal_elt = &self.elements[i].normal_part; - let elt = &mut self.elements[i].tangent_part[j]; - let dimpulse = -tangents1[j].dot(&mj_lambda2.linear) - + elt.gcross2.gdot(mj_lambda2.angular) - + elt.rhs; - let limit = self.limit * normal_elt.impulse; - let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit); - let dlambda = new_impulse - elt.impulse; - elt.impulse = new_impulse; - - mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2 * dlambda; - } - } - - // 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.num_contacts as usize], + &self.dir1, + #[cfg(feature = "dim3")] + &self.tangent1, + self.im2, + self.limit, + &mut mj_lambda2, + ); mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; } @@ -281,14 +234,13 @@ impl VelocityGroundConstraint { active_contact.data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse; + active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; } #[cfg(feature = "dim3")] { - active_contact.data.tangent_impulse = [ - self.elements[k].tangent_part[0].impulse, - self.elements[k].tangent_part[1].impulse, - ]; + active_contact.data.tangent_impulse = self + .tangent_rot1 + .inverse_transform_vector(&self.elements[k].tangent_part.impulse); } } } 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..8bcdc82 --- /dev/null +++ b/src/dynamics/solver/velocity_ground_constraint_element.rs @@ -0,0 +1,202 @@ +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], + } + } + + #[inline] + 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]; + } + } + + #[inline] + 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 8e9f3a1..ba1c46a 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -1,51 +1,26 @@ -use super::{AnyVelocityConstraint, DeltaVel}; +use super::{ + AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, + VelocityGroundConstraintNormalPart, +}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{ AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; -use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; +#[cfg(feature = "dim2")] +use crate::utils::WBasis; +use crate::utils::{WAngularInertia, WCross, WDot}; use num::Zero; use simba::simd::{SimdPartialOrd, SimdValue}; -#[derive(Copy, Clone, Debug)] -pub(crate) struct WVelocityGroundConstraintElementPart { - pub gcross2: AngVector, - pub rhs: SimdReal, - pub impulse: SimdReal, - pub r: SimdReal, -} - -impl WVelocityGroundConstraintElementPart { - 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: WVelocityGroundConstraintElementPart, - pub tangent_parts: [WVelocityGroundConstraintElementPart; DIM - 1], -} - -impl WVelocityGroundConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: WVelocityGroundConstraintElementPart::zero(), - tangent_parts: [WVelocityGroundConstraintElementPart::zero(); DIM - 1], - } - } -} - #[derive(Copy, Clone, Debug)] pub(crate) struct WVelocityGroundConstraint { pub dir1: Vector, // Non-penetration force direction for the first body. - pub elements: [WVelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], + #[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 elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, pub im2: SimdReal, pub limit: SimdReal, @@ -104,13 +79,23 @@ impl WVelocityGroundConstraint { let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); let num_active_contacts = manifolds[0].data.num_active_contacts(); + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let (tangents1, tangent_rot1) = + super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2); + for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WVelocityGroundConstraint { dir1: force_dir1, - elements: [WVelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], + #[cfg(feature = "dim3")] + tangent1: tangents1[0], + #[cfg(feature = "dim3")] + tangent_rot1, + elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2, limit: SimdReal::splat(0.0), mj_lambda2, @@ -158,7 +143,7 @@ impl WVelocityGroundConstraint { rhs += dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting); - constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart { + constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2, rhs, impulse: impulse * warmstart_coeff, @@ -167,29 +152,25 @@ impl WVelocityGroundConstraint { } // tangent parts. - let tangents1 = force_dir1.orthonormal_basis(); - - for j in 0..DIM - 1 { - #[cfg(feature = "dim2")] - let impulse = SimdReal::from( + #[cfg(feature = "dim2")] + let impulse = [SimdReal::from( + array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], + )]; + #[cfg(feature = "dim3")] + let impulse = tangent_rot1 + * na::Vector2::from( array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); - #[cfg(feature = "dim3")] - let impulse = SimdReal::from( - array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], - ); + constraint.elements[k].tangent_part.impulse = impulse; + for j in 0..DIM - 1 { let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]); - constraint.elements[k].tangent_parts[j] = - WVelocityGroundConstraintElementPart { - gcross2, - rhs, - impulse: impulse * warmstart_coeff, - r, - }; + constraint.elements[k].tangent_part.gcross2[j] = gcross2; + constraint.elements[k].tangent_part.r[j] = r; + constraint.elements[k].tangent_part.rhs[j] = rhs; } } @@ -212,19 +193,14 @@ impl WVelocityGroundConstraint { ), }; - 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_parts[j]; - mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse); - mj_lambda2.angular += elt.gcross2 * elt.impulse; - } - } + VelocityGroundConstraintElement::warmstart_group( + &self.elements[..self.num_contacts as usize], + &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); @@ -235,46 +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 first. - let tangents1 = self.dir1.orthonormal_basis(); - - for i in 0..self.num_contacts as usize { - let normal_elt = &self.elements[i].normal_part; - - for j in 0..DIM - 1 { - let elt = &mut self.elements[i].tangent_parts[j]; - let dimpulse = -tangents1[j].dot(&mj_lambda2.linear) - + elt.gcross2.gdot(mj_lambda2.angular) - + elt.rhs; - let limit = self.limit * normal_elt.impulse; - let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit); - let dlambda = new_impulse - elt.impulse; - elt.impulse = new_impulse; - - mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda); - mj_lambda2.angular += elt.gcross2 * dlambda; - } - } - - // 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.num_contacts as usize], + &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); @@ -286,11 +238,12 @@ impl WVelocityGroundConstraint { pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { for k in 0..self.num_contacts as usize { let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); - let tangent_impulses: [_; SIMD_WIDTH] = - self.elements[k].tangent_parts[0].impulse.into(); + #[cfg(feature = "dim2")] + let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into(); #[cfg(feature = "dim3")] - let bitangent_impulses: [_; SIMD_WIDTH] = - self.elements[k].tangent_parts[1].impulse.into(); + let tangent_impulses = self + .tangent_rot1 + .inverse_transform_vector(&self.elements[k].tangent_part.impulse); for ii in 0..SIMD_WIDTH { let manifold = &mut manifolds_all[self.manifold_id[ii]]; @@ -304,8 +257,7 @@ impl WVelocityGroundConstraint { } #[cfg(feature = "dim3")] { - active_contact.data.tangent_impulse = - [tangent_impulses[ii], bitangent_impulses[ii]]; + active_contact.data.tangent_impulse = tangent_impulses.extract(ii); } } } diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 5c70e44..f156db5 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -37,26 +37,14 @@ pub struct ContactData { /// The friction impulses along the basis orthonormal to the contact normal, applied to the first /// collider's rigid-body. #[cfg(feature = "dim3")] - pub tangent_impulse: [Real; 2], -} - -impl ContactData { - #[cfg(feature = "dim2")] - pub(crate) fn zero_tangent_impulse() -> Real { - 0.0 - } - - #[cfg(feature = "dim3")] - pub(crate) fn zero_tangent_impulse() -> [Real; 2] { - [0.0, 0.0] - } + pub tangent_impulse: na::Vector2, } impl Default for ContactData { fn default() -> Self { Self { impulse: 0.0, - tangent_impulse: Self::zero_tangent_impulse(), + tangent_impulse: na::zero(), } } } diff --git a/src/pipeline/physics_hooks.rs b/src/pipeline/physics_hooks.rs index c3621ea..72b635f 100644 --- a/src/pipeline/physics_hooks.rs +++ b/src/pipeline/physics_hooks.rs @@ -81,7 +81,15 @@ impl<'a> ContactModificationContext<'a> { // normal, so remove all the contacts and mark further contacts // as forbidden. self.solver_contacts.clear(); - *self.user_data = CONTACT_CURRENTLY_FORBIDDEN; + + // NOTE: in some very rare cases `local_n1` will be + // zero if the objects are exactly touching at one point. + // So in this case we can't really conclude. + // If the norm is non-zero, then we can tell we need to forbid + // further contacts. Otherwise we have to wait for the next frame. + if self.manifold.local_n1.norm_squared() > 0.1 { + *self.user_data = CONTACT_CURRENTLY_FORBIDDEN; + } } } CONTACT_CURRENTLY_FORBIDDEN => { diff --git a/src/utils.rs b/src/utils.rs index e131b0a..4ed89f6 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -107,6 +107,8 @@ pub trait WBasis: Sized { type Basis; /// Computes the vectors which, when combined with `self`, form an orthonormal basis. fn orthonormal_basis(self) -> Self::Basis; + /// Computes a vector orthogonal to `self` with a unit length (if `self` has a unit length). + fn orthonormal_vector(self) -> Self; } impl WBasis for Vector2 { @@ -114,6 +116,9 @@ impl WBasis for Vector2 { fn orthonormal_basis(self) -> [Vector2; 1] { [Vector2::new(-self.y, self.x)] } + fn orthonormal_vector(self) -> Vector2 { + Vector2::new(-self.y, self.x) + } } impl> WBasis for Vector3 { @@ -134,6 +139,13 @@ impl> WBasis for Vector3 { Vector3::new(b, sign + self.y * self.y * a, -self.y), ] } + + fn orthonormal_vector(self) -> Vector3 { + let sign = self.z.copy_sign_to(N::one()); + let a = -N::one() / (sign + self.z); + let b = self.x * self.y * a; + Vector3::new(b, sign + self.y * self.y * a, -self.y) + } } pub(crate) trait WVec: Sized { diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index f2a4f8a..02b57c3 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -186,6 +186,10 @@ impl PhysxWorld { let pos = rb.position().into_physx(); if rb.is_dynamic() { let mut actor = physics.create_dynamic(&pos, rapier_handle).unwrap(); + let linvel = rb.linvel().into_physx(); + let angvel = rb.angvel().into_physx(); + actor.set_linear_velocity(&linvel, true); + actor.set_angular_velocity(&angvel, true); actor.set_solver_iteration_counts( integration_parameters.max_position_iterations as u32, integration_parameters.max_velocity_iterations as u32,