diff options
Diffstat (limited to 'src/dynamics/solver')
14 files changed, 241 insertions, 106 deletions
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 0f01798..6b8de5a 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -157,6 +157,17 @@ impl InteractionGroups { } } + #[cfg(not(feature = "parallel"))] + pub fn clear(&mut self) { + #[cfg(feature = "simd-is-enabled")] + { + self.buckets.clear(); + self.body_masks.clear(); + self.grouped_interactions.clear(); + } + self.nongrouped_interactions.clear(); + } + // FIXME: there is a lot of duplicated code with group_manifolds here. // But we don't refactor just now because we may end up with distinct // grouping strategies in the future. diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index d0866bf..c684cc5 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -24,7 +24,25 @@ impl IslandSolver { } } - pub fn solve_island( + pub fn solve_position_constraints( + &mut self, + island_id: usize, + counters: &mut Counters, + params: &IntegrationParameters, + bodies: &mut RigidBodySet, + ) { + 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(); + } + + pub fn init_constraints_and_solve_velocity_constraints( &mut self, island_id: usize, counters: &mut Counters, @@ -59,25 +77,19 @@ impl IslandSolver { counters.solver.velocity_update_time.resume(); bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.integrate(params.dt) + rb.apply_damping(params.dt); + rb.integrate_next_position(params.dt); }); counters.solver.velocity_update_time.pause(); - - 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(); } else { + self.contact_constraints.clear(); + self.joint_constraints.clear(); counters.solver.velocity_update_time.resume(); bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { // Since we didn't run the velocity solver we need to integrate the accelerations here rb.integrate_accelerations(params.dt); - rb.integrate(params.dt); + rb.apply_damping(params.dt); + rb.integrate_next_position(params.dt); }); counters.solver.velocity_update_time.pause(); } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 744c00d..93996f4 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -114,7 +114,7 @@ impl BallPositionGroundConstraint { // are the local_anchors. The rb1 and rb2 have // already been flipped by the caller. Self { - anchor1: rb1.predicted_position * cparams.local_anchor2, + anchor1: rb1.next_position * cparams.local_anchor2, im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, @@ -123,7 +123,7 @@ impl BallPositionGroundConstraint { } } else { Self { - anchor1: rb1.predicted_position * cparams.local_anchor1, + anchor1: rb1.next_position * cparams.local_anchor1, im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index 043eea7..e9162a4 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -134,7 +134,7 @@ impl WBallPositionGroundConstraint { cparams: [&BallJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); + let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]); let anchor1 = position1 * Point::from(array![|ii| if flipped[ii] { cparams[ii].local_anchor2 diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 7e8fc97..c98660f 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -100,10 +100,10 @@ impl FixedPositionGroundConstraint { let local_anchor2; if flipped { - anchor1 = rb1.predicted_position * cparams.local_anchor2; + anchor1 = rb1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; } else { - anchor1 = rb1.predicted_position * cparams.local_anchor1; + anchor1 = rb1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; }; diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs index ea7e927..ed52a52 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs @@ -119,14 +119,14 @@ impl PrismaticPositionGroundConstraint { let local_axis2; if flipped { - frame1 = rb1.predicted_position * cparams.local_frame2(); + frame1 = rb1.next_position * cparams.local_frame2(); local_frame2 = cparams.local_frame1(); - axis1 = rb1.predicted_position * cparams.local_axis2; + axis1 = rb1.next_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; } else { - frame1 = rb1.predicted_position * cparams.local_frame1(); + frame1 = rb1.next_position * cparams.local_frame1(); local_frame2 = cparams.local_frame2(); - axis1 = rb1.predicted_position * cparams.local_axis1; + axis1 = rb1.next_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; }; diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 2da49e6..96391a2 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -145,23 +145,23 @@ impl RevolutePositionGroundConstraint { let local_basis2; if flipped { - anchor1 = rb1.predicted_position * cparams.local_anchor2; + anchor1 = rb1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; - axis1 = rb1.predicted_position * cparams.local_axis2; + axis1 = rb1.next_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; basis1 = [ - rb1.predicted_position * cparams.basis2[0], - rb1.predicted_position * cparams.basis2[1], + rb1.next_position * cparams.basis2[0], + rb1.next_position * cparams.basis2[1], ]; local_basis2 = cparams.basis1; } else { - anchor1 = rb1.predicted_position * cparams.local_anchor1; + anchor1 = rb1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; - axis1 = rb1.predicted_position * cparams.local_axis1; + axis1 = rb1.next_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; basis1 = [ - rb1.predicted_position * cparams.basis1[0], - rb1.predicted_position * cparams.basis1[1], + rb1.next_position * cparams.basis1[0], + rb1.next_position * cparams.basis1[1], ]; local_basis2 = cparams.basis2; }; diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 99c1ec5..add5f2c 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -70,6 +70,8 @@ pub(crate) struct ThreadContext { pub impulse_writeback_index: AtomicUsize, pub joint_writeback_index: AtomicUsize, pub body_integration_index: AtomicUsize, + pub body_force_integration_index: AtomicUsize, + pub num_force_integrated_bodies: AtomicUsize, pub num_integrated_bodies: AtomicUsize, // Position solver. pub position_constraint_initialization_index: AtomicUsize, @@ -97,6 +99,8 @@ impl ThreadContext { num_solved_interactions: AtomicUsize::new(0), impulse_writeback_index: AtomicUsize::new(0), joint_writeback_index: AtomicUsize::new(0), + body_force_integration_index: AtomicUsize::new(0), + num_force_integrated_bodies: AtomicUsize::new(0), body_integration_index: AtomicUsize::new(0), num_integrated_bodies: AtomicUsize::new(0), position_constraint_initialization_index: AtomicUsize::new(0), @@ -146,7 +150,82 @@ impl ParallelIslandSolver { } } - pub fn solve_island<'s>( + pub fn solve_position_constraints<'s>( + &'s mut self, + scope: &Scope<'s>, + island_id: usize, + params: &'s IntegrationParameters, + bodies: &'s mut RigidBodySet, + ) { + let num_threads = rayon::current_num_threads(); + let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island? + self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here? + self.positions.clear(); + self.positions + .resize(bodies.active_island(island_id).len(), Isometry::identity()); + + for _ in 0..num_task_per_island { + // We use AtomicPtr because it is Send+Sync while *mut is not. + // See https://internals.rust-lang.org/t/shouldnt-pointers-be-send-sync-or/8818 + let thread = &self.thread; + let positions = std::sync::atomic::AtomicPtr::new(&mut self.positions as *mut _); + let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _); + let parallel_contact_constraints = + std::sync::atomic::AtomicPtr::new(&mut self.parallel_contact_constraints as *mut _); + let parallel_joint_constraints = + std::sync::atomic::AtomicPtr::new(&mut self.parallel_joint_constraints as *mut _); + + scope.spawn(move |_| { + // Transmute *mut -> &mut + let positions: &mut Vec<Isometry<Real>> = + unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) }; + let bodies: &mut RigidBodySet = + unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; + let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> = unsafe { + std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed)) + }; + let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> = unsafe { + std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed)) + }; + + enable_flush_to_zero!(); // Ensure this is enabled on each thread. + + // Write results back to rigid bodies and integrate velocities. + let island_range = bodies.active_island_range(island_id); + let active_bodies = &bodies.active_dynamic_set[island_range]; + let bodies = &mut bodies.bodies; + + concurrent_loop! { + let batch_size = thread.batch_size; + for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { + let rb = &mut bodies[handle.0]; + positions[rb.active_set_offset] = rb.next_position; + } + } + + ThreadContext::lock_until_ge(&thread.num_integrated_bodies, active_bodies.len()); + + ParallelPositionSolver::solve( + &thread, + params, + positions, + parallel_contact_constraints, + parallel_joint_constraints + ); + + // Write results back to rigid bodies. + concurrent_loop! { + let batch_size = thread.batch_size; + for handle in active_bodies[thread.position_writeback_index] { + let rb = &mut bodies[handle.0]; + rb.set_next_position(positions[rb.active_set_offset]); + } + } + }) + } + } + + pub fn init_constraints_and_solve_velocity_constraints<'s>( &'s mut self, scope: &Scope<'s>, island_id: usize, @@ -184,37 +263,11 @@ impl ParallelIslandSolver { self.positions .resize(bodies.active_island(island_id).len(), Isometry::identity()); - { - // Initialize `mj_lambdas` (per-body velocity deltas) with external accelerations (gravity etc): - - let island_range = bodies.active_island_range(island_id); - let active_bodies = &bodies.active_dynamic_set[island_range]; - let bodies = &mut bodies.bodies; - - let thread = &self.thread; - - concurrent_loop! { - let batch_size = thread.batch_size; - for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { - let rb = &mut bodies[handle.0]; - let dvel = &mut self.mj_lambdas[rb.active_set_offset]; - - dvel.linear += rb.force * (rb.effective_inv_mass * params.dt); - rb.force = na::zero(); - - // dvel.angular is actually storing angular velocity delta multiplied by the square root of the inertia tensor: - dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt; - rb.torque = na::zero(); - } - } - } - for _ in 0..num_task_per_island { // We use AtomicPtr because it is Send+Sync while *mut is not. // See https://internals.rust-lang.org/t/shouldnt-pointers-be-send-sync-or/8818 let thread = &self.thread; let mj_lambdas = std::sync::atomic::AtomicPtr::new(&mut self.mj_lambdas as *mut _); - let positions = std::sync::atomic::AtomicPtr::new(&mut self.positions as *mut _); let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _); let manifolds = std::sync::atomic::AtomicPtr::new(manifolds as *mut _); let joints = std::sync::atomic::AtomicPtr::new(joints as *mut _); @@ -227,8 +280,6 @@ impl ParallelIslandSolver { // Transmute *mut -> &mut let mj_lambdas: &mut Vec<DeltaVel<Real>> = unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) }; - let positions: &mut Vec<Isometry<Real>> = - unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) }; let bodies: &mut RigidBodySet = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; let manifolds: &mut Vec<&mut ContactManifold> = @@ -243,6 +294,32 @@ impl ParallelIslandSolver { }; enable_flush_to_zero!(); // Ensure this is enabled on each thread. + + // Initialize `mj_lambdas` (per-body velocity deltas) with external accelerations (gravity etc): + { + let island_range = bodies.active_island_range(island_id); + let active_bodies = &bodies.active_dynamic_set[island_range]; + let bodies = &mut bodies.bodies; + + concurrent_loop! { + let batch_size = thread.batch_size; + for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] { + let rb = &mut bodies[handle.0]; + let dvel = &mut mj_lambdas[rb.active_set_offset]; + + // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied + // by the square root of the inertia tensor: + dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt; + dvel.linear += rb.force * (rb.effective_inv_mass * params.dt); + } + } + + // We need to wait for every body to be force-integrated because their + // angular and linear velocities are needed by the constraints initialization. + ThreadContext::lock_until_ge(&thread.num_force_integrated_bodies, active_bodies.len()); + } + + parallel_contact_constraints.fill_constraints(&thread, params, bodies, manifolds); parallel_joint_constraints.fill_constraints(&thread, params, bodies, joints); ThreadContext::lock_until_ge( @@ -276,29 +353,8 @@ impl ParallelIslandSolver { let dvel = mj_lambdas[rb.active_set_offset]; rb.linvel += dvel.linear; rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); - rb.integrate(params.dt); - positions[rb.active_set_offset] = rb.position; - } - } - - // We need to way for every body to be integrated because it also - // initialized `positions` to the updated values. - ThreadContext::lock_until_ge(&thread.num_integrated_bodies, active_bodies.len()); - - ParallelPositionSolver::solve( - &thread, - params, - positions, - parallel_contact_constraints, - parallel_joint_constraints - ); - - // Write results back to rigid bodies. - concurrent_loop! { - let batch_size = thread.batch_size; - for handle in active_bodies[thread.position_writeback_index] { - let rb = &mut bodies[handle.0]; - rb.set_position_internal(positions[rb.active_set_offset]); + rb.apply_damping(params.dt); + rb.integrate_next_position(params.dt); } } }) diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index df0e3fc..2fa4aee 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -21,11 +21,16 @@ impl PositionSolver { contact_constraints: &[AnyPositionConstraint], joint_constraints: &[AnyJointPositionConstraint], ) { + if contact_constraints.is_empty() && joint_constraints.is_empty() { + // Nothing to do. + return; + } + self.positions.clear(); self.positions.extend( bodies .iter_active_island(island_id) - .map(|(_, b)| b.position), + .map(|(_, b)| b.next_position), ); for _ in 0..params.max_position_iterations { @@ -39,7 +44,7 @@ impl PositionSolver { } bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.set_position_internal(self.positions[rb.active_set_offset]) + rb.set_next_position(self.positions[rb.active_set_offset]) }); } } diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index b9dd497..3a4ecb7 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -38,6 +38,15 @@ impl<VelocityConstraint, PositionConstraint> position_constraints: Vec::new(), } } + + pub fn clear(&mut self) { + self.not_ground_interactions.clear(); + self.ground_interactions.clear(); + self.interaction_groups.clear(); + self.ground_interaction_groups.clear(); + self.velocity_constraints.clear(); + self.position_constraints.clear(); + } } impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> { diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 3e8cb61..c339ce4 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -208,6 +208,8 @@ impl VelocityConstraint { let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + let warmstart_correction; + constraint.limit = manifold_point.friction; constraint.manifold_contact_id[k] = manifold_point.contact_id; @@ -234,12 +236,15 @@ impl VelocityConstraint { rhs += manifold_point.dist.max(0.0) * inv_dt; rhs *= is_bouncy + is_resting * params.velocity_solve_fraction; rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); + warmstart_correction = (params.warmstart_correction_slope + / (rhs - manifold_point.prev_rhs).abs()) + .min(warmstart_coeff); constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, rhs, - impulse: manifold_point.data.impulse * warmstart_coeff, + impulse: manifold_point.warmstart_impulse * warmstart_correction, r, }; } @@ -247,10 +252,12 @@ impl VelocityConstraint { // Tangent parts. { #[cfg(feature = "dim3")] - let impulse = - tangent_rot1 * manifold_points[k].data.tangent_impulse * warmstart_coeff; + let impulse = tangent_rot1 + * manifold_points[k].warmstart_tangent_impulse + * warmstart_correction; #[cfg(feature = "dim2")] - let impulse = [manifold_points[k].data.tangent_impulse * warmstart_coeff]; + let impulse = + [manifold_points[k].warmstart_tangent_impulse * warmstart_correction]; constraint.elements[k].tangent_part.impulse = impulse; for j in 0..DIM - 1 { @@ -332,6 +339,8 @@ impl VelocityConstraint { let contact_id = self.manifold_contact_id[k]; let active_contact = &mut manifold.points[contact_id as usize]; active_contact.data.impulse = self.elements[k].normal_part.impulse; + active_contact.data.rhs = self.elements[k].normal_part.rhs; + #[cfg(feature = "dim2")] { active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 673af54..7d5f468 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -9,6 +9,7 @@ use crate::math::{ #[cfg(feature = "dim2")] use crate::utils::WBasis; use crate::utils::{WAngularInertia, WCross, WDot}; +use na::SimdComplexField; use num::Zero; use simba::simd::{SimdPartialOrd, SimdValue}; @@ -44,6 +45,7 @@ impl WVelocityConstraint { } let inv_dt = SimdReal::splat(params.inv_dt()); + let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope); let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt()); @@ -123,8 +125,11 @@ impl WVelocityConstraint { let tangent_velocity = Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]); - let impulse = - SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); + let impulse = SimdReal::from( + array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH], + ); + let prev_rhs = + SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]); let dp1 = point - world_com1; let dp2 = point - world_com2; @@ -132,6 +137,8 @@ impl WVelocityConstraint { let vel1 = linvel1 + angvel1.gcross(dp1); let vel2 = linvel2 + angvel2.gcross(dp2); + let warmstart_correction; + constraint.limit = friction; constraint.manifold_contact_id[k] = array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH]; @@ -150,12 +157,15 @@ impl WVelocityConstraint { rhs *= is_bouncy + is_resting * velocity_solve_fraction; rhs += dist.simd_min(SimdReal::zero()) * (velocity_based_erp_inv_dt * is_resting); + warmstart_correction = (warmstart_correction_slope + / (rhs - prev_rhs).simd_abs()) + .simd_min(warmstart_coeff); constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, rhs, - impulse: impulse * warmstart_coeff, + impulse: impulse * warmstart_correction, r, }; } @@ -163,14 +173,15 @@ impl WVelocityConstraint { // tangent parts. #[cfg(feature = "dim2")] let impulse = [SimdReal::from( - array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], - )]; + array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH], + ) * warmstart_correction]; #[cfg(feature = "dim3")] let impulse = tangent_rot1 * na::Vector2::from( - array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], - ); + array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH], + ) + * warmstart_correction; constraint.elements[k].tangent_part.impulse = impulse; @@ -281,6 +292,7 @@ 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 rhs: [_; SIMD_WIDTH] = self.elements[k].normal_part.rhs.into(); #[cfg(feature = "dim2")] let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into(); #[cfg(feature = "dim3")] @@ -292,6 +304,7 @@ impl WVelocityConstraint { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let contact_id = self.manifold_contact_id[k][ii]; let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.rhs = rhs[ii]; active_contact.data.impulse = impulses[ii]; #[cfg(feature = "dim2")] diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index b9c5236..0e195d5 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -133,6 +133,7 @@ impl VelocityGroundConstraint { let dp1 = manifold_point.point - rb1.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + let warmstart_correction; constraint.limit = manifold_point.friction; constraint.manifold_contact_id[k] = manifold_point.contact_id; @@ -153,11 +154,14 @@ impl VelocityGroundConstraint { rhs += manifold_point.dist.max(0.0) * inv_dt; rhs *= is_bouncy + is_resting * params.velocity_solve_fraction; rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); + warmstart_correction = (params.warmstart_correction_slope + / (rhs - manifold_point.prev_rhs).abs()) + .min(warmstart_coeff); constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2, rhs, - impulse: manifold_point.data.impulse * warmstart_coeff, + impulse: manifold_point.warmstart_impulse * warmstart_correction, r, }; } @@ -165,10 +169,12 @@ impl VelocityGroundConstraint { // Tangent parts. { #[cfg(feature = "dim3")] - let impulse = - tangent_rot1 * manifold_points[k].data.tangent_impulse * warmstart_coeff; + let impulse = tangent_rot1 + * manifold_points[k].warmstart_tangent_impulse + * warmstart_correction; #[cfg(feature = "dim2")] - let impulse = [manifold_points[k].data.tangent_impulse * warmstart_coeff]; + let impulse = + [manifold_points[k].warmstart_tangent_impulse * warmstart_correction]; constraint.elements[k].tangent_part.impulse = impulse; for j in 0..DIM - 1 { @@ -237,6 +243,8 @@ impl VelocityGroundConstraint { let contact_id = self.manifold_contact_id[k]; let active_contact = &mut manifold.points[contact_id as usize]; active_contact.data.impulse = self.elements[k].normal_part.impulse; + active_contact.data.rhs = self.elements[k].normal_part.rhs; + #[cfg(feature = "dim2")] { active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0]; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index ba1c46a..4237d99 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -10,6 +10,7 @@ use crate::math::{ #[cfg(feature = "dim2")] use crate::utils::WBasis; use crate::utils::{WAngularInertia, WCross, WDot}; +use na::SimdComplexField; use num::Zero; use simba::simd::{SimdPartialOrd, SimdValue}; @@ -77,6 +78,7 @@ impl WVelocityGroundConstraint { 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 warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope); let num_active_contacts = manifolds[0].data.num_active_contacts(); #[cfg(feature = "dim2")] @@ -118,13 +120,17 @@ impl WVelocityGroundConstraint { let tangent_velocity = Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]); - let impulse = - SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); + let impulse = SimdReal::from( + array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH], + ); + let prev_rhs = + SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]); let dp1 = point - world_com1; let dp2 = point - world_com2; let vel1 = linvel1 + angvel1.gcross(dp1); let vel2 = linvel2 + angvel2.gcross(dp2); + let warmstart_correction; |