diff options
| author | Sébastien Crozet <developer@crozet.re> | 2020-09-01 14:02:59 +0200 |
|---|---|---|
| committer | Sébastien Crozet <developer@crozet.re> | 2020-09-01 14:02:59 +0200 |
| commit | 9622827dc6aadb391512b95381edb1efc26b1b90 (patch) | |
| tree | 3b15362d6f7736f8d30bc78b6e33ff51c893751f /src/dynamics/solver/position_constraint_wide.rs | |
| parent | 03b437f278bbcbd391acd23a4d8fa074915eb00c (diff) | |
| download | rapier-9622827dc6aadb391512b95381edb1efc26b1b90.tar.gz rapier-9622827dc6aadb391512b95381edb1efc26b1b90.tar.bz2 rapier-9622827dc6aadb391512b95381edb1efc26b1b90.zip | |
Fix constraints resolution with non-identity relative collider position.
Diffstat (limited to 'src/dynamics/solver/position_constraint_wide.rs')
| -rw-r--r-- | src/dynamics/solver/position_constraint_wide.rs | 7 |
1 files changed, 5 insertions, 2 deletions
diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 8828c3d..0633926 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -51,6 +51,9 @@ impl WPositionConstraint { let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); + let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -85,8 +88,8 @@ impl WPositionConstraint { let local_p2 = Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]); - constraint.local_p1[i] = local_p1 + shift1; - constraint.local_p2[i] = local_p2 + shift2; + constraint.local_p1[i] = delta1 * (local_p1 + shift1); + constraint.local_p2[i] = delta2 * (local_p2 + shift2); } if push { |