From 96ecb877e290ad15459258a415aca64ca4af3a69 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 24 Feb 2021 13:26:51 +0100 Subject: Implement dominance. --- src/dynamics/solver/categorization.rs | 5 +---- src/dynamics/solver/position_ground_constraint.rs | 2 +- src/dynamics/solver/position_ground_constraint_wide.rs | 2 +- src/dynamics/solver/solver_constraints.rs | 1 - src/dynamics/solver/velocity_constraint.rs | 2 ++ src/dynamics/solver/velocity_constraint_wide.rs | 4 ++++ src/dynamics/solver/velocity_ground_constraint.rs | 2 +- src/dynamics/solver/velocity_ground_constraint_wide.rs | 2 +- 8 files changed, 11 insertions(+), 9 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index c920b69..5a00896 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -2,7 +2,6 @@ use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub(crate) fn categorize_contacts( - bodies: &RigidBodySet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], out_ground: &mut Vec, @@ -10,10 +9,8 @@ pub(crate) fn categorize_contacts( ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; - let rb1 = &bodies[manifold.data.body_pair.body1]; - let rb2 = &bodies[manifold.data.body_pair.body2]; - if !rb1.is_dynamic() || !rb2.is_dynamic() { + if manifold.data.relative_dominance != 0 { out_ground.push(*manifold_i) } else { out_not_ground.push(*manifold_i) diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index 4ab07eb..e1a4016 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -30,7 +30,7 @@ impl PositionGroundConstraint { ) { let mut rb1 = &bodies[manifold.data.body_pair.body1]; let mut rb2 = &bodies[manifold.data.body_pair.body2]; - let flip = !rb2.is_dynamic(); + let flip = manifold.data.relative_dominance < 0; let n1 = if flip { std::mem::swap(&mut rb1, &mut rb2); diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index f52b3f4..1869c9c 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -39,7 +39,7 @@ impl WPositionGroundConstraint { let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { - if !rbs2[ii].is_dynamic() { + if manifolds[ii].data.relative_dominance < 0 { flipped[ii] = true; std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); } diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index b9dd497..b3ce8c7 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -51,7 +51,6 @@ impl SolverConstraints { self.not_ground_interactions.clear(); self.ground_interactions.clear(); categorize_contacts( - bodies, manifolds, manifold_indices, &mut self.ground_interactions, diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 2de9807..abc46c9 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -144,6 +144,8 @@ impl VelocityConstraint { out_constraints: &mut Vec, push: bool, ) { + assert_eq!(manifold.data.relative_dominance, 0); + let inv_dt = params.inv_dt(); let rb1 = &bodies[manifold.data.body_pair.body1]; let rb2 = &bodies[manifold.data.body_pair.body2]; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 97fa261..82a764e 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -67,6 +67,10 @@ impl WVelocityConstraint { out_constraints: &mut Vec, push: bool, ) { + for ii in 0..SIMD_WIDTH { + assert_eq!(manifolds[ii].data.relative_dominance, 0); + } + 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]; diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index beb07ed..a3a5563 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -66,7 +66,7 @@ impl VelocityGroundConstraint { let inv_dt = params.inv_dt(); let mut rb1 = &bodies[manifold.data.body_pair.body1]; let mut rb2 = &bodies[manifold.data.body_pair.body2]; - let flipped = !rb2.is_dynamic(); + let flipped = manifold.data.relative_dominance < 0; let (force_dir1, flipped_multiplier) = if flipped { std::mem::swap(&mut rb1, &mut rb2); diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 6e7216a..5339d8a 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -69,7 +69,7 @@ impl WVelocityGroundConstraint { let mut flipped = [1.0; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { - if !rbs2[ii].is_dynamic() { + if manifolds[ii].data.relative_dominance < 0 { std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); flipped[ii] = -1.0; } -- cgit From 9cdd34c741d6e45b7dcaf797c3704d1bc9dfeaae Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 24 Feb 2021 13:40:26 +0100 Subject: Fix the parallel build. --- src/dynamics/solver/categorization.rs | 1 + src/dynamics/solver/solver_constraints.rs | 1 + 2 files changed, 2 insertions(+) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 5a00896..e3327c6 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -2,6 +2,7 @@ use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub(crate) fn categorize_contacts( + _bodies: &RigidBodySet, // Unused but useful to simplify the parallel code. manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], out_ground: &mut Vec, diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs index b3ce8c7..b9dd497 100644 --- a/src/dynamics/solver/solver_constraints.rs +++ b/src/dynamics/solver/solver_constraints.rs @@ -51,6 +51,7 @@ impl SolverConstraints { self.not_ground_interactions.clear(); self.ground_interactions.clear(); categorize_contacts( + bodies, manifolds, manifold_indices, &mut self.ground_interactions, -- cgit