From 3805943067713ce881ec479bfce2b7af5d334414 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 1 Feb 2021 12:12:45 +0100 Subject: Add a twist friction model and velocity-based error correction. --- src/dynamics/integration_parameters.rs | 13 +- src/dynamics/solver/island_solver.rs | 22 +- src/dynamics/solver/mod.rs | 12 + src/dynamics/solver/position_constraint.rs | 2 +- src/dynamics/solver/position_constraint_wide.rs | 2 +- src/dynamics/solver/position_ground_constraint.rs | 2 +- .../solver/position_ground_constraint_wide.rs | 2 +- src/dynamics/solver/solver_constraints.rs | 84 +++-- src/dynamics/solver/velocity_constraint.rs | 34 +- ...ocity_constraint_wide_with_manifold_friction.rs | 416 +++++++++++++++++++++ .../velocity_constraint_with_manifold_friction.rs | 345 +++++++++++++++++ .../solver/velocity_ground_constraint_wide.rs | 8 +- ...round_constraint_wide_with_manifold_friction.rs | 362 ++++++++++++++++++ ...ity_ground_constraint_with_manifold_friction.rs | 300 +++++++++++++++ src/geometry/contact_pair.rs | 3 + src/utils.rs | 6 + 16 files changed, 1555 insertions(+), 58 deletions(-) create mode 100644 src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs create mode 100644 src/dynamics/solver/velocity_constraint_with_manifold_friction.rs create mode 100644 src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs create mode 100644 src/dynamics/solver/velocity_ground_constraint_with_manifold_friction.rs diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 0d4d3b6..4840ec0 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -19,8 +19,11 @@ pub struct IntegrationParameters { /// This allows the user to take action during a timestep, in-between two CCD events. pub return_after_ccd_substep: bool, /// The Error Reduction Parameter in `[0, 1]` is the proportion of - /// the positional error to be corrected at each time step (default: `0.2`). - pub erp: Real, + /// the positional error to be corrected at the position level each time step (default: `0.0`). + pub positionErp: Real, + /// The Error Reduction Parameter in `[0, 1]` is the proportion of + /// the positional error to be corrected at the velocity level at each time step (default: `0.005`). + pub velocityErp: Real, /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of /// the positional error to be corrected at each time step (default: `0.2`). pub joint_erp: Real, @@ -113,7 +116,7 @@ impl IntegrationParameters { IntegrationParameters { dt, // multithreading_enabled, - erp, + positionErp: erp, joint_erp, warmstart_coeff, restitution_velocity_threshold, @@ -136,6 +139,7 @@ impl IntegrationParameters { return_after_ccd_substep, multiple_ccd_substep_sensor_events_enabled, ccd_on_penetration_enabled, + velocityErp: 0.005, } } @@ -185,7 +189,8 @@ impl Default for IntegrationParameters { dt: 1.0 / 60.0, // multithreading_enabled: true, return_after_ccd_substep: false, - erp: 0.2, + positionErp: 0.0, + velocityErp: 0.005, joint_erp: 0.2, warmstart_coeff: 1.0, restitution_velocity_threshold: 1.0, diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index deed8c2..68074e1 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -60,16 +60,18 @@ impl IslandSolver { bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt)); counters.solver.velocity_update_time.pause(); - if manifold_indices.len() != 0 || joint_indices.len() != 0 { - counters.solver.position_resolution_time.resume(); - self.position_solver.solve( - island_id, - params, - bodies, - &self.contact_constraints.position_constraints, - &self.joint_constraints.position_constraints, - ); - counters.solver.position_resolution_time.pause(); + if params.positionErp != 0.0 { + if manifold_indices.len() != 0 || joint_indices.len() != 0 { + counters.solver.position_resolution_time.resume(); + self.position_solver.solve( + island_id, + params, + bodies, + &self.contact_constraints.position_constraints, + &self.joint_constraints.position_constraints, + ); + counters.solver.position_resolution_time.pause(); + } } } } diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs index 090d0f3..2291615 100644 --- a/src/dynamics/solver/mod.rs +++ b/src/dynamics/solver/mod.rs @@ -26,9 +26,15 @@ pub(self) use position_ground_constraint_wide::*; pub(self) use velocity_constraint::*; #[cfg(feature = "simd-is-enabled")] pub(self) use velocity_constraint_wide::*; +#[cfg(feature = "simd-is-enabled")] +pub(self) use velocity_constraint_wide_with_manifold_friction::*; +pub(self) use velocity_constraint_with_manifold_friction::*; pub(self) use velocity_ground_constraint::*; #[cfg(feature = "simd-is-enabled")] pub(self) use velocity_ground_constraint_wide::*; +#[cfg(feature = "simd-is-enabled")] +pub(self) use velocity_ground_constraint_wide_with_manifold_friction::*; +pub(self) use velocity_ground_constraint_with_manifold_friction::*; mod categorization; mod delta_vel; @@ -57,8 +63,14 @@ mod solver_constraints; mod velocity_constraint; #[cfg(feature = "simd-is-enabled")] mod velocity_constraint_wide; +#[cfg(feature = "simd-is-enabled")] +mod velocity_constraint_wide_with_manifold_friction; +mod velocity_constraint_with_manifold_friction; mod velocity_ground_constraint; #[cfg(feature = "simd-is-enabled")] mod velocity_ground_constraint_wide; +#[cfg(feature = "simd-is-enabled")] +mod velocity_ground_constraint_wide_with_manifold_friction; +mod velocity_ground_constraint_with_manifold_friction; #[cfg(not(feature = "parallel"))] mod velocity_solver; diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 844b1cd..dc5b218 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -93,7 +93,7 @@ impl PositionConstraint { ii1: rb1.effective_world_inv_inertia_sqrt.squared(), ii2: rb2.effective_world_inv_inertia_sqrt.squared(), num_contacts: manifold_points.len() as u8, - erp: params.erp, + erp: params.positionErp, max_linear_correction: params.max_linear_correction, }; diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 67b5cdb..8701bfc 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -74,7 +74,7 @@ impl WPositionConstraint { im2, ii1: sqrt_ii1.squared(), ii2: sqrt_ii2.squared(), - erp: SimdReal::splat(params.erp), + erp: SimdReal::splat(params.positionErp), max_linear_correction: SimdReal::splat(params.max_linear_correction), num_contacts: num_points as u8, }; diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index 4ab07eb..e69d8ea 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -66,7 +66,7 @@ impl PositionGroundConstraint { im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), num_contacts: manifold_contacts.len() as u8, - erp: params.erp, + erp: params.positionErp, max_linear_correction: params.max_linear_correction, }; diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index f52b3f4..99a469f 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -71,7 +71,7 @@ impl WPositionGroundConstraint { dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS], im2, ii2: sqrt_ii2.squared(), - erp: SimdReal::splat(params.erp), + erp: SimdReal::splat(params.positionErp), max_linear_correction: SimdReal::splat(params.max_linear_correction), num_contacts: num_points as u8, }; diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index b9dd497..ab8c1e6 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -3,11 +3,15 @@ use super::{ }; #[cfg(feature = "simd-is-enabled")] use super::{ - WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint, + WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, + WVelocityConstraintWithManifoldFriction, WVelocityGroundConstraint, + WVelocityGroundConstraintWithManifoldFriction, }; use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; use crate::dynamics::solver::{ - AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, + AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, + PositionGroundConstraint, VelocityConstraintWithManifoldFriction, + VelocityGroundConstraintWithManifoldFriction, }; use crate::dynamics::{ solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, @@ -122,7 +126,7 @@ impl SolverConstraints { { let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH]; let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; - WVelocityConstraint::generate( + WVelocityConstraintWithManifoldFriction::generate( params, manifold_id, manifolds, @@ -130,13 +134,16 @@ impl SolverConstraints { &mut self.velocity_constraints, true, ); - WPositionConstraint::generate( - params, - manifolds, - bodies, - &mut self.position_constraints, - true, - ); + + if params.positionErp != 0.0 { + WPositionConstraint::generate( + params, + manifolds, + bodies, + &mut self.position_constraints, + true, + ); + } } } @@ -148,7 +155,7 @@ impl SolverConstraints { ) { for manifold_i in &self.interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; - VelocityConstraint::generate( + VelocityConstraintWithManifoldFriction::generate( params, *manifold_i, manifold, @@ -156,13 +163,16 @@ impl SolverConstraints { &mut self.velocity_constraints, true, ); - PositionConstraint::generate( - params, - manifold, - bodies, - &mut self.position_constraints, - true, - ); + + if params.positionErp != 0.0 { + PositionConstraint::generate( + params, + manifold, + bodies, + &mut self.position_constraints, + true, + ); + } } } @@ -180,7 +190,7 @@ impl SolverConstraints { { let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH]; let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; - WVelocityGroundConstraint::generate( + WVelocityGroundConstraintWithManifoldFriction::generate( params, manifold_id, manifolds, @@ -188,13 +198,16 @@ impl SolverConstraints { &mut self.velocity_constraints, true, ); - WPositionGroundConstraint::generate( - params, - manifolds, - bodies, - &mut self.position_constraints, - true, - ); + + if params.positionErp != 0.0 { + WPositionGroundConstraint::generate( + params, + manifolds, + bodies, + &mut self.position_constraints, + true, + ); + } } } @@ -206,7 +219,7 @@ impl SolverConstraints { ) { for manifold_i in &self.ground_interaction_groups.nongrouped_interactions { let manifold = &manifolds_all[*manifold_i]; - VelocityGroundConstraint::generate( + VelocityGroundConstraintWithManifoldFriction::generate( params, *manifold_i, manifold, @@ -214,13 +227,16 @@ impl SolverConstraints { &mut self.velocity_constraints, true, ); - PositionGroundConstraint::generate( - params, - manifold, - bodies, - &mut self.position_constraints, - true, - ) + + if params.positionErp != 0.0 { + PositionGroundConstraint::generate( + params, + manifold, + bodies, + &mut self.position_constraints, + true, + ) + } } } } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 514434b..aa3ea62 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -1,7 +1,13 @@ use super::DeltaVel; -use crate::dynamics::solver::VelocityGroundConstraint; +use crate::dynamics::solver::{ + VelocityConstraintWithManifoldFriction, VelocityGroundConstraint, + VelocityGroundConstraintWithManifoldFriction, +}; #[cfg(feature = "simd-is-enabled")] -use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; +use crate::dynamics::solver::{ + WVelocityConstraint, WVelocityConstraintWithManifoldFriction, WVelocityGroundConstraint, + WVelocityGroundConstraintWithManifoldFriction, +}; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{AngVector, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; @@ -13,10 +19,16 @@ use simba::simd::SimdPartialOrd; pub(crate) enum AnyVelocityConstraint { NongroupedGround(VelocityGroundConstraint), Nongrouped(VelocityConstraint), + Nongrouped2(VelocityConstraintWithManifoldFriction), + NongroupedGround2(VelocityGroundConstraintWithManifoldFriction), #[cfg(feature = "simd-is-enabled")] GroupedGround(WVelocityGroundConstraint), #[cfg(feature = "simd-is-enabled")] Grouped(WVelocityConstraint), + #[cfg(feature = "simd-is-enabled")] + Grouped2(WVelocityConstraintWithManifoldFriction), + #[cfg(feature = "simd-is-enabled")] + GroupedGround2(WVelocityGroundConstraintWithManifoldFriction), #[allow(dead_code)] // The Empty variant is only used with parallel code. Empty, } @@ -44,10 +56,16 @@ impl AnyVelocityConstraint { match self { AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas), AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas), + AnyVelocityConstraint::Nongrouped2(c) => c.warmstart(mj_lambdas), + AnyVelocityConstraint::NongroupedGround2(c) => c.warmstart(mj_lambdas), #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::GroupedGround(c) => c.warmstart(mj_lambdas), #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::GroupedGround2(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::Grouped(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::Grouped2(c) => c.warmstart(mj_lambdas), AnyVelocityConstraint::Empty => unreachable!(), } } @@ -56,10 +74,16 @@ impl AnyVelocityConstraint { match self { AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas), AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas), + AnyVelocityConstraint::Nongrouped2(c) => c.solve(mj_lambdas), + AnyVelocityConstraint::NongroupedGround2(c) => c.solve(mj_lambdas), #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::GroupedGround(c) => c.solve(mj_lambdas), #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::GroupedGround2(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::Grouped2(c) => c.solve(mj_lambdas), AnyVelocityConstraint::Empty => unreachable!(), } } @@ -68,10 +92,16 @@ impl AnyVelocityConstraint { match self { AnyVelocityConstraint::NongroupedGround(c) => c.writeback_impulses(manifold_all), AnyVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifold_all), + AnyVelocityConstraint::Nongrouped2(c) => c.writeback_impulses(manifold_all), + AnyVelocityConstraint::NongroupedGround2(c) => c.writeback_impulses(manifold_all), #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::GroupedGround(c) => c.writeback_impulses(manifold_all), #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::GroupedGround2(c) => c.writeback_impulses(manifold_all), + #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::Grouped(c) => c.writeback_impulses(manifold_all), + #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::Grouped2(c) => c.writeback_impulses(manifold_all), AnyVelocityConstraint::Empty => unreachable!(), } } diff --git a/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs b/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs new file mode 100644 index 0000000..d9d3e53 --- /dev/null +++ b/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs @@ -0,0 +1,416 @@ +use super::{AnyVelocityConstraint, DeltaVel}; +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}; +use num::Zero; +use simba::simd::{SimdPartialOrd, SimdValue}; + +#[derive(Copy, Clone, Debug)] +struct WVelocityConstraintPart { + gcross1: AngVector, + gcross2: AngVector, + rhs: SimdReal, + impulse: SimdReal, + r: SimdReal, +} + +impl WVelocityConstraintPart { + 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 WVelocityConstraintWithManifoldFriction { + dir1: Vector, // Non-penetration force direction for the first body. + normal_parts: [WVelocityConstraintPart; MAX_MANIFOLD_POINTS], + tangent_parts: [WVelocityConstraintPart; DIM - 1], + twist_part: WVelocityConstraintPart, + twist_weights: [SimdReal; MAX_MANIFOLD_POINTS], + num_contacts: u8, + im1: SimdReal, + im2: SimdReal, + limit: SimdReal, + mj_lambda1: [usize; SIMD_WIDTH], + mj_lambda2: [usize; SIMD_WIDTH], + manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + manifold_contact_id: usize, +} + +impl WVelocityConstraintWithManifoldFriction { + pub fn generate( + params: &IntegrationParameters, + manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + manifolds: [&ContactManifold; SIMD_WIDTH], + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let inv_dt = SimdReal::splat(params.inv_dt()); + let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; + let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; + + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); + let ii1: AngularInertia = AngularInertia::from( + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); + + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia = AngularInertia::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); + + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + + let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]); + + let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); + + let warmstart_multiplier = + SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); + let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); + let num_active_contacts = manifolds[0].data.num_active_contacts(); + + 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 + ]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + + let mut constraint = WVelocityConstraintWithManifoldFriction { + dir1: force_dir1, + normal_parts: [WVelocityConstraintPart::zero(); MAX_MANIFOLD_POINTS], + tangent_parts: [WVelocityConstraintPart::zero(); DIM - 1], + twist_part: WVelocityConstraintPart::zero(), + twist_weights: [SimdReal::splat(0.0); MAX_MANIFOLD_POINTS], + im1, + im2, + limit: SimdReal::splat(0.0), + mj_lambda1, + mj_lambda2, + manifold_id, + manifold_contact_id: l, + num_contacts: num_points as u8, + }; + + let mut manifold_center = Point::origin(); + let mut tangent_impulses = [SimdReal::splat(0.0), SimdReal::splat(0.0)]; + + for k in 0..num_points { + let friction = + SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]); + let restitution = + SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]); + let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); + + manifold_center += point.coords / SimdReal::splat(num_points as Real); + + let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + + let impulse = + SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); + tangent_impulses[0] += SimdReal::from( + array![|ii| manifold_points[ii][k].data.tangent_impulse[0]; SIMD_WIDTH], + ); + tangent_impulses[1] += SimdReal::from( + array![|ii| manifold_points[ii][k].data.tangent_impulse[1]; SIMD_WIDTH], + ); + + let dp1 = point - world_com1; + let dp2 = point - world_com2; + + let vel1 = linvel1 + angvel1.gcross(dp1); + let vel2 = linvel2 + angvel2.gcross(dp2); + + constraint.limit = friction; + + // Normal part. + { + let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1)); + let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); + + let r = SimdReal::splat(1.0) + / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); + let mut rhs = (vel1 - vel2).dot(&force_dir1); + let use_restitution = rhs.simd_le(-restitution_velocity_threshold); + let rhs_with_restitution = rhs + rhs * restitution; + rhs = rhs_with_restitution.select(use_restitution, rhs); + + rhs += ((dist.simd_min(SimdReal::splat(0.0)) + * SimdReal::splat(params.velocityErp)) + + dist.simd_max(SimdReal::splat(0.0))) + * inv_dt; + // rhs += dist.simd_max(SimdReal::splat(-0.01)) * inv_dt; + + constraint.normal_parts[k] = WVelocityConstraintPart { + gcross1, + gcross2, + rhs, + impulse: impulse * warmstart_coeff, + r, + }; + } + } + + // tangent parts. + let tangents1 = force_dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let dp1 = manifold_center - world_com1; + let dp2 = manifold_center - world_com2; + + let vel1 = linvel1 + angvel1.gcross(dp1); + let vel2 = linvel2 + angvel2.gcross(dp2); + + 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).dot(&tangents1[j]); + + constraint.tangent_parts[j] = WVelocityConstraintPart { + gcross1, + gcross2, + rhs, + impulse: tangent_impulses[j] * warmstart_coeff, + r, + }; + } + + // Twist part. + { + let twist_impulse = + SimdReal::from(array![|ii| manifolds[ii].data.twist_impulse; SIMD_WIDTH]); + + for k in 0..num_points { + let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); + constraint.twist_weights[k] = na::distance(&point, &manifold_center); + } + + let gcross1 = ii1.transform_vector(force_dir1); + let gcross2 = ii2.transform_vector(-force_dir1); + let inv_r = gcross1.norm_squared() + gcross2.norm_squared(); + let rhs = (angvel1 - angvel2).gdot(force_dir1); + constraint.twist_part = WVelocityConstraintPart { + gcross1, + gcross2, + rhs, + impulse: twist_impulse * warmstart_coeff, + r: crate::utils::simd_inv(inv_r), + } + } + + if push { + out_constraints.push(AnyVelocityConstraint::Grouped2(constraint)); + } else { + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = + AnyVelocityConstraint::Grouped2(constraint); + } + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + 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], + ), + }; + + // Normal part. + for i in 0..self.num_contacts as usize { + let elt = &self.normal_parts[i]; + 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; + } + + // Tangent part. + { + let tangents1 = self.dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let elt = &self.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; + } + } + + // Twist part. + { + mj_lambda1.angular += self.twist_part.gcross1 * self.twist_part.impulse; + mj_lambda2.angular += self.twist_part.gcross2 * self.twist_part.impulse; + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + 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], + ), + }; + + // Solve friction first. + let tangents1 = self.dir1.orthonormal_basis(); + let friction_limit = self.limit + * (self.normal_parts[0].impulse + + self.normal_parts[1].impulse + + self.normal_parts[2].impulse + + self.normal_parts[3].impulse); + + for j in 0..DIM - 1 { + let elt = &mut self.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 new_impulse = + (elt.impulse - elt.r * dimpulse).simd_clamp(-friction_limit, friction_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.normal_parts[i]; + 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; + } + + { + let twist_limit = self.limit + * (self.normal_parts[0].impulse * self.twist_weights[0] + + self.normal_parts[1].impulse * self.twist_weights[1] + + self.normal_parts[2].impulse * self.twist_weights[2] + + self.normal_parts[3].impulse * self.twist_weights[3]); + + let dimpulse = self.twist_part.gcross1.gdot(mj_lambda1.angular) + + self.twist_part.gcross2.gdot(mj_lambda2.angular) + + self.twist_part.rhs; + let new_impulse = (self.twist_part.impulse - self.twist_part.r * dimpulse) + .simd_clamp(-twist_limit, twist_limit); + let dlambda = new_impulse - self.twist_part.impulse; + self.twist_part.impulse = new_impulse; + + mj_lambda1.angular += self.twist_part.gcross1 * dlambda; + mj_lambda2.angular += self.twist_part.gcross2 * dlambda; + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + let denom = crate::utils::simd_inv( + self.normal_parts[0].impulse + + self.normal_parts[1].impulse + + self.normal_parts[2].impulse + + self.normal_parts[3].impulse, + ); + + for k in 0..self.num_contacts as usize { + let normal_factor = self.normal_parts[k].impulse * denom; + let tangent_impulses = self.tangent_parts[0].impulse * normal_factor; + let bitangent_impulses = self.tangent_parts[1].impulse * normal_factor; + + let impulses: [_; SIMD_WIDTH] = self.normal_parts[k].impulse.into(); + let tangent_impulses: [_; SIMD_WIDTH] = tangent_impulses.into(); + let bitangent_impulses: [_; SIMD_WIDTH] = bitangent_impulses.into(); + + for ii in 0..SIMD_WIDTH { + let manifold = &mut manifolds_all[self.manifold_id[ii]]; + let k_base = self.manifold_contact_id; + let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; + active_contacts[k_base + k].data.impulse = impulses[ii]; + active_contacts[k_base + k].data.tangent_impulse = + [tangent_impulses[ii], bitangent_impulses[ii]]; + } + } + + let twist_impulse: [_; SIMD_WIDTH] = self.twist_part.impulse.into(); + + for ii in 0..SIMD_WIDTH { + let manifold = &mut manifolds_all[self.manifold_id[ii]]; + manifold.data.twist_impulse = twist_impulse[ii]; + } + } +} diff --git a/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs b/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs new file mode 100644 index 0000000..66269a4 --- /dev/null +++ b/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs @@ -0,0 +1,345 @@ +use super::DeltaVel; +use crate::dynamics::solver::{AnyVelocityConstraint, VelocityGroundConstraint}; +use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{AngVector, Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; +use simba::simd::SimdPartialOrd; + +#[derive(Copy, Clone, Debug)] +struct VelocityConstraintPart { + pub gcross1: AngVector, + pub gcross2: AngVector, + pub rhs: Real, + pub impulse: Real, + pub r: Real, +} + +#[cfg(not(target_arch = "wasm32"))] +impl VelocityConstraintPart { + 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 VelocityConstraintWithManifoldFriction { + dir1: Vector, // Non-penetration force direction for the first body. + im1: Real, + im2: Real, + limit: Real, + mj_lambda1: usize, + mj_lambda2: usize, + manifold_id: ContactManifoldIndex, + manifold_contact_id: usize, + num_contacts: u8, + normal_parts: [VelocityConstraintPart; MAX_MANIFOLD_POINTS], + tangent_parts: [VelocityConstraintPart; DIM - 1], + twist_part: VelocityConstraintPart, + twist_weights: [Real; MAX_MANIFOLD_POINTS], +} + +impl VelocityConstraintWithManifoldFriction { + #[cfg(feature = "parallel")] + pub fn num_active_constraints(manifold: &ContactManifold) -> usize { + let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0; + manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize + } + + pub fn generate( + params: &IntegrationParameters, + manifold_id: ContactManifoldIndex, + manifold: &ContactManifold, + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let inv_dt = params.inv_dt(); + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; + let mj_lambda1 = rb1.active_set_offset; + let mj_lambda2 = rb2.active_set_offset; + let force_dir1 = -manifold.data.normal; + let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; + + for (l, manifold_points) in manifold + .data + .solver_contacts + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + #[cfg(not(target_arch = "wasm32"))] + let mut constraint = VelocityConstraintWithManifoldFriction { + dir1: force_dir1, + im1: rb1.effective_inv_mass, + im2: rb2.effective_inv_mass, + limit: 0.0, + mj_lambda1, + mj_lambda2, + manifold_id, + manifold_contact_id: l * MAX_MANIFOLD_POINTS, + num_contacts: manifold_points.len() as u8, + normal_parts: [VelocityConstraintPart::zero(); MAX_MANIFOLD_POINTS], + tangent_parts: [VelocityConstraintPart::zero(); DIM - 1], + twist_part: VelocityConstraintPart::zero(), + twist_weights: [0.0; MAX_MANIFOLD_POINTS], + }; + + let mut manifold_center = Point::origin(); + let mut tangent_impulses = [0.0, 0.0]; + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + manifold_center += manifold_point.point.coords / (manifold_points.len() as Real); + + let dp1 = manifold_point.point - rb1.world_com; + let dp2 = manifold_point.point - rb2.world_com; + + let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); + let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + + constraint.limit = manifold_point.friction; + + // Normal part. + let gcross1 = rb1 + .effective_world_inv_inertia_sqrt + .transform_vector(dp1.gcross(force_dir1)); + let gcross2 = rb2 + .effective_world_inv_inertia_sqrt + .transform_vector(dp2.gcross(-force_dir1)); + + let r = 1.0 + / (rb1.effective_inv_mass + + rb2.effective_inv_mass + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2)); + + let mut rhs = (vel1 - vel2).dot(&force_dir1); + + if rhs <= -params.restitution_velocity_threshold { + rhs += manifold_point.restitution * rhs + } + + rhs += manifold_point.dist.max(0.0) * inv_dt; + + let impulse = manifold_point.data.impulse * warmstart_coeff; + tangent_impulses[0] += manifold_point.data.tangent_impulse[0]; + tangent_impulses[1] += manifold_point.data.tangent_impulse[1]; + + constraint.normal_parts[k] = VelocityConstraintPart { + gcross1, + gcross2, + rhs, + impulse, + r, + }; + } + + // Tangent part. + let tangents1 = force_dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let dp1 = manifold_center - rb1.world_com; + let dp2 = manifold_center - rb2.world_com; + + let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); + let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + + let gcross1 = rb1 + .effective_world_inv_inertia_sqrt + .transform_vector(dp1.gcross(tangents1[j])); + let gcross2 = rb2 + .effective_world_inv_inertia_sqrt + .transform_vector(dp2.gcross(-tangents1[j])); + let r = 1.0 + / (rb1.effective_inv_mass + + rb2.effective_inv_mass + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2)); + let rhs = (vel1 - vel2).dot(&tangents1[j]); + let impulse = tangent_impulses[j] * warmstart_coeff; + + constraint.tangent_parts[j] = VelocityConstraintPart { + gcross1, + gcross2, + rhs, + impulse, + r, + }; + } + + // Twist part. + { + for k in 0..manifold_points.len() { + constraint.twist_weights[k] = + na::distance(&manifold_points[k].point, &manifold_center); + } + + let gcross1 = rb1 + .effective_world_inv_inertia_sqrt + .transform_vector(force_dir1); + let gcross2 = rb2 + .effective_world_inv_inertia_sqrt + .transform_vector(-force_dir1); + + let inv_r = gcross1.norm_squared() + gcross2.norm_squared(); + constraint.twist_part.r = crate::utils::inv(inv_r); + constraint.twist_part.gcross1 = gcross1; + constraint.twist_part.gcross2 = gcross2; + constraint.twist_part.rhs = (rb1.angvel - rb2.angvel).gdot(force_dir1); + constraint.twist_part.impulse = manifold.data.twist_impulse * warmstart_coeff; + } + + #[cfg(not(target_arch = "wasm32"))] + if push { + out_constraints.push(AnyVelocityConstraint::Nongrouped2(constraint)); + } else { + out_constraints[manifold.data.constraint_index + l] = + AnyVelocityConstraint::Nongrouped2(constraint); + } + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel::zero(); + let mut mj_lambda2 = DeltaVel::zero(); + + // Normal part. + for i in 0..self.num_contacts as usize { + let elt = &self.normal_parts[i]; + 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; + } + + // Tangent part. + { + // FIXME: move this out of the for loop? + let tangents1 = self.dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let elt = &self.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; + } + } + + // Twist part. + { + mj_lambda1.angular += self.twist_part.gcross1 * self.twist_part.impulse; + mj_lambda2.angular += self.twist_part.gcross2 * self.twist_part.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; + } + + 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. + let tangents1 = self.dir1.orthonormal_basis(); + let friction_limit = self.limit + * (self.normal_parts[0].impulse + + self.normal_parts[1].impulse + + self.normal_parts[2].impulse + + self.normal_parts[3].impulse); + + for j in 0..DIM - 1 { + let elt = &mut self.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 new_impulse = + (elt.impulse - elt.r * dimpulse).simd_clamp(-friction_limit, friction_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.normal_parts[i]; + 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; + } + + // Solve twist. + { + let twist_limit = self.limit + * (self.normal_parts[0].impulse * self.twist_weights[0] + + self.normal_parts[1].impulse * self.twist_weights[1] + + self.normal_parts[2].impulse * self.twist_weights[2] + + self.normal_parts[3].impulse * self.twist_weights[3]); + + let dimpulse = self.twist_part.gcross1.gdot(mj_lambda1.angular) + + self.twist_part.gcross2.gdot(mj_lambda2.angular) + + self.twist_part.rhs; + let new_impulse = (self.twist_part.impulse - self.twist_part.r * dimpulse) + .simd_clamp(-twist_limit, twist_limit); + let dlambda = new_impulse - self.twist_part.impulse; + self.twist_part.impulse = new_impulse; + + mj_lambda1.angular += self.twist_part.gcross1 * dlambda; + mj_lambda2.angular += self.twist_part.gcross2 * dlambda; + } + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + let manifold = &mut manifolds_all[self.manifold_id]; + let k_base = self.manifold_contact_id; + let denom = crate::utils::inv( + self.normal_parts[0].impulse + + self.normal_parts[1].impulse + + self.normal_parts[2].impulse + + self.normal_parts[3].impulse, + ); + + for k in 0..self.num_contacts as usize { + let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; + let normal_factor = self.normal_parts[k].impulse * denom; + active_contacts[k_base + k].data.impulse = self.normal_parts[k].impulse; + active_contacts[k_base + k].data.tangent_impulse = [ + self.tangent_parts[0].impulse * normal_factor, + self.tangent_parts[1].impulse * normal_factor, + ]; + } + + manifold.data.twist_impulse = self.twist_part.impulse; + } +} diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index c2d2c4f..efb542f 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -75,6 +75,10 @@ impl WVelocityGroundConstraint { } } + let force_dir1 = Vector::from( + array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; SIMD_WIDTH], + ); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2: AngularInertia = AngularInertia::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], @@ -89,10 +93,6 @@ impl WVelocityGroundConstraint { let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let force_dir1 = Vector::from( - array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; SIMD_WIDTH], - ); - let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); diff --git a/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs b/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs new file mode 100644 index 0000000..bf7fecc --- /dev/null +++ b/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs @@ -0,0 +1,362 @@ +use super::{AnyVelocityConstraint, DeltaVel}; +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}; +use num::Zero; +use simba::simd::{SimdPartialOrd, SimdValue}; + +#[derive(Copy, Clone, Debug)] +struct WVelocityConstraintPart { + gcross2: AngVector, + rhs: SimdReal, + impulse: SimdReal, + r: SimdReal, +} + +impl WVelocityConstraintPart { + pub fn zero() -> Self { + Self { + gcross2: AngVector::zero(), + rhs: SimdReal::zero(), + impulse: SimdReal::zero(), + r: SimdReal::zero(), + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct WVelocityGroundConstraintWithManifoldFriction { + dir1: Vector, // Non-penetration force direction for the first body. + normal_parts: [WVelocityConstraintPart; MAX_MANIFOLD_POINTS], + tangent_parts: [WVelocityConstraintPart; DIM - 1], + twist_part: WVelocityConstraintPart, + twist_weights: [SimdReal; MAX_MANIFOLD_POINTS], + num_contacts: u8, + im2: SimdReal, + limit: SimdReal, + mj_lambda2: [usize; SIMD_WIDTH], + manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + manifold_contact_id: usize, +} + +impl WVelocityGroundConstraintWithManifoldFriction { + pub fn generate( + params: &IntegrationParameters, + manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + manifolds: [&ContactManifold; SIMD_WIDTH], + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let inv_dt = SimdReal::splat(params.inv_dt()); + let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; + let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; + let mut flipped = [false; SIMD_WIDTH]; + + for ii in 0..SIMD_WIDTH { + if !rbs2[ii].is_dynamic() { + std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); + flipped[ii] = true; + } + } + + let force_dir1 = Vector::from( + array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; SIMD_WIDTH], + ); + + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia = AngularInertia::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); + + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); + + let warmstart_multiplier = + SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); + let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); + let num_active_contacts = manifolds[0].data.num_active_contacts(); + + 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 + ]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + + let mut constraint = WVelocityGroundConstraintWithManifoldFriction { + dir1: force_dir1, + normal_parts: [WVelocityConstraintPart::zero(); MAX_MANIFOLD_POINTS], + tangent_parts: [WVelocityConstraintPart::zero(); DIM - 1], + twist_part: WVelocityConstraintPart::zero(), + twist_weights: [SimdReal::splat(0.0); MAX_MANIFOLD_POINTS], + im2, + limit: SimdReal::splat(0.0), + mj_lambda2, + manifold_id, + manifold_contact_id: l, + num_contacts: num_points as u8, + }; + + let mut manifold_center = Point::origin(); + let mut tangent_impulses = [SimdReal::splat(0.0), SimdReal::splat(0.0)]; + + for k in 0..num_points { + let friction = + SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]); + let restitution = + SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]); + let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); + + manifold_center += point.coords / SimdReal::splat(num_points as Real); + + let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + + let impulse = + SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); + tangent_impulses[0] += SimdReal::from( + array![|ii| manifold_points[ii][k].data.tangent_impulse[0]; SIMD_WIDTH], + ); + tangent_impulses[1] += SimdReal::from( + array![|ii| manifold_points[ii][k].data.tangent_impulse[1]; SIMD_WIDTH], + ); + + let dp1 = point - world_com1; + let dp2 = point - world_com2; + + let vel1 = linvel1 + angvel1.gcross(dp1); + let vel2 = linvel2 + angvel2.gcross(dp2); + + constraint.limit = friction; + + // Normal part. + { + let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); + + let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); + let mut rhs = (vel1 - vel2).dot(&force_dir1); + let use_restitution = rhs.simd_le(-restitution_velocity_threshold); + let rhs_with_restitution = rhs + rhs * restitution; + rhs = rhs_with_restitution.select(use_restitution, rhs); + + rhs += ((dist.simd_min(SimdReal::splat(0.0)) + * SimdReal::splat(params.velocityErp)) + + dist.simd_max(SimdReal::splat(0.0))) + * inv_dt; + // rhs += dist.simd_max(SimdReal::splat(-0.1)) * inv_dt; + + constraint.normal_parts[k] = WVelocityConstraintPart { + gcross2, + rhs, + impulse: impulse * warmstart_coeff, + r, + }; + } + } + + // tangent parts. + let tangents1 = force_dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let dp1 = manifold_center - world_com1; + let dp2 = manifold_center - world_com2; + + let vel1 = linvel1 + angvel1.gcross(dp1); + let vel2 = linvel2 + angvel2.gcross(dp2); + + let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); + let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); + let rhs = (vel1 - vel2).dot(&tangents1[j]); + + constraint.tangent_parts[j] = WVelocityConstraintPart { + gcross2, + rhs, + impulse: tangent_impulses[j] * warmstart_coeff, + r, + }; + } + + // Twist part. + { + let twist_impulse = + SimdReal::from(array![|ii| manifolds[ii].data.twist_impulse; SIMD_WIDTH]); + + for k in 0..num_points { + let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); + constraint.twist_weights[k] = na::distance(&point, &manifold_center); + } + + let gcross2 = ii2.transform_vector(-force_dir1); + let inv_r = gcross2.norm_squared(); + let rhs = (angvel1 - angvel2).gdot(force_dir1); + constraint.twist_part = WVelocityConstraintPart { + gcross2, + rhs, + impulse: twist_impulse * warmstart_coeff, + r: crate::utils::simd_inv(inv_r), + } + } + + if push { + out_constraints.push(AnyVelocityConstraint::GroupedGround2(constraint)); + } else { + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = + AnyVelocityConstraint::GroupedGround2(constraint); + } + } + } + + pub fn warmstart(&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], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + // Normal part. + for i in 0..self.num_contacts as usize { + let elt = &self.normal_parts[i]; + mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse); + mj_lambda2.angular += elt.gcross2 * elt.impulse; + } + + // Tangent part. + { + let tangents1 = self.dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let elt = &self.tangent_parts[j]; + mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse); + mj_lambda2.angular += elt.gcross2 * elt.impulse; + } + } + + // Twist part. + { + mj_lambda2.angular += self.twist_part.gcross2 * self.twist_part.impulse; + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut m