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 +++----- src/dynamics/solver/velocity_constraint_wide.rs | 6 ++++-- src/dynamics/solver/velocity_ground_constraint.rs | 4 ++-- src/dynamics/solver/velocity_ground_constraint_wide.rs | 7 +++++-- 4 files changed, 14 insertions(+), 11 deletions(-) (limited to 'src') 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) diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 8f387a5..4204e68 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -80,6 +80,7 @@ impl WVelocityConstraint { let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); let ii2: AngularInertia = @@ -89,6 +90,7 @@ impl WVelocityConstraint { let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); let force_dir1 = position1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); @@ -130,8 +132,8 @@ impl WVelocityConstraint { let impulse = SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); - let dp1 = p1.coords - position1.translation.vector; - let dp2 = p2.coords - position2.translation.vector; + let dp1 = p1 - world_com1; + let dp2 = p2 - world_com2; let vel1 = linvel1 + angvel1.gcross(dp1); let vel2 = linvel2 + angvel2.gcross(dp2); 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); diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index ec9e186..af2e1e9 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -89,6 +89,9 @@ impl WVelocityGroundConstraint { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let force_dir1 = position1 * -Vector::from( array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH], @@ -130,8 +133,8 @@ impl WVelocityGroundConstraint { let impulse = SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); - let dp1 = p1.coords - position1.translation.vector; - let dp2 = p2.coords - position2.translation.vector; + let dp1 = p1 - world_com1; + let dp2 = p2 - world_com2; let vel1 = linvel1 + angvel1.gcross(dp1); let vel2 = linvel2 + angvel2.gcross(dp2); -- cgit