From c286f44c4e2aa1692781e9c4c7c1ce2bacdd2415 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 31 Aug 2020 19:02:37 +0200 Subject: Constraint solver: properly take non-zero center of masses into account. --- src/dynamics/solver/velocity_ground_constraint.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/dynamics/solver/velocity_ground_constraint.rs') diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 457c3b3..65a61bd 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -155,8 +155,8 @@ impl VelocityGroundConstraint { rb2.position * manifold_point.local_p2, ) }; - let dp2 = p2.coords - rb2.position.translation.vector; - let dp1 = p1.coords - rb1.position.translation.vector; + let dp2 = p2 - rb2.world_com; + let dp1 = p1 - rb1.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); -- cgit From 9622827dc6aadb391512b95381edb1efc26b1b90 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Tue, 1 Sep 2020 14:02:59 +0200 Subject: Fix constraints resolution with non-identity relative collider position. --- src/dynamics/solver/velocity_ground_constraint.rs | 28 ++++++++++++----------- 1 file changed, 15 insertions(+), 13 deletions(-) (limited to 'src/dynamics/solver/velocity_ground_constraint.rs') diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 65a61bd..d9229ff 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -66,20 +66,22 @@ impl VelocityGroundConstraint { let mut rb1 = &bodies[manifold.body_pair.body1]; let mut rb2 = &bodies[manifold.body_pair.body2]; let flipped = !rb2.is_dynamic(); + let force_dir1; + let coll_pos1; + let coll_pos2; if flipped { + coll_pos1 = rb2.position * manifold.delta2; + coll_pos2 = rb1.position * manifold.delta1; + force_dir1 = coll_pos1 * (-manifold.local_n2); std::mem::swap(&mut rb1, &mut rb2); + } else { + coll_pos1 = rb1.position * manifold.delta1; + coll_pos2 = rb2.position * manifold.delta2; + force_dir1 = coll_pos1 * (-manifold.local_n1); } let mj_lambda2 = rb2.active_set_offset; - let force_dir1 = if flipped { - // NOTE: we already swapped rb1 and rb2 - // so we multiply by rb1.position. - rb1.position * (-manifold.local_n2) - } else { - rb1.position * (-manifold.local_n1) - }; - let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold @@ -144,15 +146,15 @@ impl VelocityGroundConstraint { let manifold_point = &manifold_points[k]; let (p1, p2) = if flipped { // NOTE: we already swapped rb1 and rb2 - // so we multiply by rb2.position. + // so we multiply by coll_pos1/coll_pos2. ( - rb1.position * manifold_point.local_p2, - rb2.position * manifold_point.local_p1, + coll_pos1 * manifold_point.local_p2, + coll_pos2 * manifold_point.local_p1, ) } else { ( - rb1.position * manifold_point.local_p1, - rb2.position * manifold_point.local_p2, + coll_pos1 * manifold_point.local_p1, + coll_pos2 * manifold_point.local_p2, ) }; let dp2 = p2 - rb2.world_com; -- cgit