Implement dominance.

2021-02-24 13:26:51 +01:00
parent 3cc2738e5f
commit 96ecb877e2
11 changed files with 42 additions and 10 deletions
--- 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<AnyVelocityConstraint>,
        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];