nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Use contact ids instead of contact reordering in order to identify the impulse writeback location.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-02-22 17:51:40 +01:00
parent d31a327b45
commit ad5c10672e
6 changed files with 52 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/dynamics/solver/velocity_constraint.rs
View File

@@ -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],
}
@@ -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
@@ -382,19 +384,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_contacts.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,
];