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_constraint.rs | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) (limited to 'src/dynamics/solver/velocity_constraint.rs') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 9212e89..eb80018 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -215,10 +215,8 @@ impl VelocityConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = (rb1.position * manifold_point.local_p1).coords - - rb1.position.translation.vector; - let dp2 = (rb2.position * manifold_point.local_p2).coords - - rb2.position.translation.vector; + let dp1 = (rb1.position * manifold_point.local_p1) - rb1.world_com; + let dp2 = (rb2.position * manifold_point.local_p2) - rb2.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); @@ -355,7 +353,7 @@ impl VelocityConstraint { } } - // Solve penetration. + // Solve non-penetration. for i in 0..self.num_contacts as usize { let elt = &mut self.elements[i].normal_part; let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular) -- 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_constraint.rs | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver/velocity_constraint.rs') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index eb80018..190076d 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -148,7 +148,9 @@ impl VelocityConstraint { let rb2 = &bodies[manifold.body_pair.body2]; let mj_lambda1 = rb1.active_set_offset; let mj_lambda2 = rb2.active_set_offset; - let force_dir1 = rb1.position * (-manifold.local_n1); + let pos_coll1 = rb1.position * manifold.delta1; + let pos_coll2 = rb2.position * manifold.delta2; + let force_dir1 = pos_coll1 * (-manifold.local_n1); let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold @@ -215,8 +217,8 @@ impl VelocityConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = (rb1.position * manifold_point.local_p1) - rb1.world_com; - let dp2 = (rb2.position * manifold_point.local_p2) - rb2.world_com; + let dp1 = (pos_coll1 * manifold_point.local_p1) - rb1.world_com; + let dp2 = (pos_coll2 * manifold_point.local_p2) - rb2.world_com; let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); -- cgit