From 3fdb4cd6d3468733056e886c991bae551a83240d Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 23 Feb 2021 15:47:24 +0100 Subject: Properly take the tangent_velocity into account in the velocity solver. --- src/dynamics/solver/velocity_constraint.rs | 5 +++-- src/dynamics/solver/velocity_constraint_wide.rs | 4 +++- src/dynamics/solver/velocity_ground_constraint.rs | 12 +++++++----- src/dynamics/solver/velocity_ground_constraint_wide.rs | 15 +++++++++------ 4 files changed, 22 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 63139cc..3a751df 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -273,7 +273,8 @@ impl VelocityConstraint { + rb2.effective_inv_mass + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); - let rhs = (vel1 - vel2).dot(&tangents1[j]); + let rhs = + (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]); #[cfg(feature = "dim2")] let impulse = manifold_point.data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] @@ -391,7 +392,7 @@ impl VelocityConstraint { active_contact.data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts.data.tangent_impulse = self.elements[k].tangent_part[0].impulse; + active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 8838011..97fa261 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -130,6 +130,8 @@ impl WVelocityConstraint { ); let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + let tangent_velocity = + Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]); let impulse = SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); @@ -181,7 +183,7 @@ impl WVelocityConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); let r = SimdReal::splat(1.0) / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); - let rhs = (vel1 - vel2).dot(&tangents1[j]); + let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]); constraint.elements[k].tangent_parts[j] = WVelocityConstraintElementPart { gcross1, diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 6f61b17..ddd5628 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -68,11 +68,11 @@ impl VelocityGroundConstraint { let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flipped = !rb2.is_dynamic(); - let force_dir1 = if flipped { + let (force_dir1, flipped_multiplier) = if flipped { std::mem::swap(&mut rb1, &mut rb2); - manifold.data.normal + (manifold.data.normal, -1.0) } else { - -manifold.data.normal + (-manifold.data.normal, 1.0) }; let mj_lambda2 = rb2.active_set_offset; @@ -123,7 +123,7 @@ impl VelocityGroundConstraint { .as_nongrouped_ground_mut() .unwrap() } else { - unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. + unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. }; #[cfg(target_arch = "wasm32")] @@ -179,7 +179,9 @@ impl VelocityGroundConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); - let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); + let rhs = (vel1 - vel2 + + flipped_multiplier * manifold_point.tangent_velocity) + .dot(&tangents1[j]); #[cfg(feature = "dim2")] let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 6f279d8..6e7216a 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -66,15 +66,17 @@ impl WVelocityGroundConstraint { let inv_dt = SimdReal::splat(params.inv_dt()); let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; - let mut flipped = [false; SIMD_WIDTH]; + let mut flipped = [1.0; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { if !rbs2[ii].is_dynamic() { std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); - flipped[ii] = true; + flipped[ii] = -1.0; } } + let flipped_sign = SimdReal::from(flipped); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2: AngularInertia = AngularInertia::from( array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], @@ -89,9 +91,8 @@ impl WVelocityGroundConstraint { 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 = Vector::from( - array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; SIMD_WIDTH], - ); + let normal1 = Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]); + let force_dir1 = normal1 * -flipped_sign; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -125,6 +126,8 @@ impl WVelocityGroundConstraint { ); let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + let tangent_velocity = + Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]); let impulse = SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); @@ -170,7 +173,7 @@ impl WVelocityGroundConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); - let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); + let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]); constraint.elements[k].tangent_parts[j] = WVelocityGroundConstraintElementPart { -- cgit