diff options
Diffstat (limited to 'src/dynamics/solver/velocity_constraint.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_constraint.rs | 26 |
1 files changed, 14 insertions, 12 deletions
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 2cac93d..2de9807 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -124,7 +124,7 @@ pub(crate) struct VelocityConstraint { pub mj_lambda1: usize, pub mj_lambda2: usize, pub manifold_id: ContactManifoldIndex, - pub manifold_contact_id: usize, + pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], pub num_contacts: u8, pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], } @@ -152,7 +152,7 @@ impl VelocityConstraint { let force_dir1 = -manifold.data.normal; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; - for (l, manifold_points) in manifold + for (_l, manifold_points) in manifold .data .solver_contacts .chunks(MAX_MANIFOLD_POINTS) @@ -168,7 +168,7 @@ impl VelocityConstraint { mj_lambda1, mj_lambda2, manifold_id, - manifold_contact_id: l * MAX_MANIFOLD_POINTS, + manifold_contact_id: [0; MAX_MANIFOLD_POINTS], num_contacts: manifold_points.len() as u8, }; @@ -211,7 +211,7 @@ impl VelocityConstraint { constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; - constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS; + constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS]; constraint.num_contacts = manifold_points.len() as u8; } @@ -224,6 +224,8 @@ impl VelocityConstraint { let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); constraint.limit = manifold_point.friction; + constraint.manifold_contact_id[k] = manifold_point.contact_id; + // Normal part. { let gcross1 = rb1 @@ -271,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")] @@ -292,7 +295,7 @@ impl VelocityConstraint { if push { out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint)); } else { - out_constraints[manifold.data.constraint_index + l] = + out_constraints[manifold.data.constraint_index + _l] = AnyVelocityConstraint::Nongrouped(constraint); } } @@ -382,19 +385,18 @@ impl VelocityConstraint { pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { let manifold = &mut manifolds_all[self.manifold_id]; - let k_base = self.manifold_contact_id; for k in 0..self.num_contacts as usize { - let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()]; - active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; + let contact_id = self.manifold_contact_id[k]; + let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].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")] { - active_contacts[k_base + k].data.tangent_impulse = [ + active_contact.data.tangent_impulse = [ self.elements[k].tangent_part[0].impulse, self.elements[k].tangent_part[1].impulse, ]; |